mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-19 21:12:04 +08:00
Merge branch 'devel-i18n' into devel-zht
This commit is contained in:
+2
-2
@@ -14,7 +14,7 @@ Most open source development activity is coordinated through our [Discord](https
|
||||
|
||||
### Local Testing
|
||||
|
||||
You can test your changes on your machine by running `run_docker_tests.sh`. This will run some automated tests in docker against your code.
|
||||
You can test your changes on your machine by running `run_docker_tests.sh`. This will run some automated tests in docker against your code.
|
||||
|
||||
### Automated Testing
|
||||
|
||||
@@ -22,7 +22,7 @@ All PRs are automatically checked by travis. Check out `.travis.yml` for what tr
|
||||
|
||||
### Code Style and Linting
|
||||
|
||||
Code is automatically check for style by travis as part of the automated tests. You can also run these yourself by running `check_code_quality.sh`.
|
||||
Code is automatically checked for style by travis as part of the automated tests. You can also run these tests yourself by running `pylint_openpilot.sh` and `flake8_openpilot.sh`.
|
||||
|
||||
## Car Ports (openpilot)
|
||||
|
||||
|
||||
@@ -62,90 +62,92 @@ At the moment, openpilot supports the [EON DevKit](https://comma.ai/shop/product
|
||||
Supported Cars
|
||||
------
|
||||
|
||||
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
|
||||
| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------|
|
||||
| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>5</sup> | 25mph |
|
||||
| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>5</sup> | 12mph |
|
||||
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
|
||||
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
|
||||
| Chrysler | Pacifica Hybrid 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph |
|
||||
| Honda | Accord 2018-19 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Accord Hybrid 2018-19 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph |
|
||||
| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph |
|
||||
| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Stock | 0mph | 2mph |
|
||||
| Honda | CR-V 2015-16 | Touring | openpilot | 25mph<sup>5</sup> | 12mph |
|
||||
| Honda | CR-V 2017-19 | Honda Sensing | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph |
|
||||
| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph<sup>5</sup> | 12mph |
|
||||
| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph<sup>5</sup> | 0mph |
|
||||
| Honda | Passport 2019 | All | openpilot | 25mph<sup>5</sup> | 12mph |
|
||||
| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph<sup>5</sup> | 12mph |
|
||||
| Honda | Pilot 2019 | All | openpilot | 25mph<sup>5</sup> | 12mph |
|
||||
| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph<sup>5</sup> | 12mph |
|
||||
| Hyundai | Elantra 2017-19<sup>1</sup> | SCC + LKAS | Stock | 19mph | 34mph |
|
||||
| Hyundai | Genesis 2018<sup>1</sup> | All | Stock | 19mph | 34mph |
|
||||
| Hyundai | Santa Fe 2019<sup>1</sup> | All | Stock | 0mph | 0mph |
|
||||
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph |
|
||||
| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Stock | 0mph | 39mph |
|
||||
| Kia | Optima 2019<sup>1</sup> | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Kia | Sorento 2018<sup>1</sup> | All | Stock | 0mph | 0mph |
|
||||
| Kia | Stinger 2018<sup>1</sup> | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Lexus | CT Hybrid 2017-18 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Lexus | ES 2019 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph |
|
||||
| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph |
|
||||
| Lexus | NX Hybrid 2018 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Lexus | RX 2016-17 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Lexus | RX 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2016-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Toyota | Avalon 2016 | TSS-P | Stock<sup>4</sup>| 20mph<sup>5</sup> | 0mph |
|
||||
| Toyota | Avalon 2017-18 | All | Stock<sup>4</sup>| 20mph<sup>5</sup> | 0mph |
|
||||
| Toyota | Camry 2018-19 | All | Stock | 0mph<sup>2</sup> | 0mph |
|
||||
| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph<sup>2</sup> | 0mph |
|
||||
| Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph |
|
||||
| Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph |
|
||||
| Toyota | Corolla 2017-19 | All | Stock<sup>4</sup>| 20mph<sup>5</sup> | 0mph |
|
||||
| Toyota | Corolla 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Corolla Hatchback 2019-20 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Corolla Hybrid 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Highlander 2017-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2017-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Prius 2016 | TSS-P | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2017-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2017-20 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 2016 | TSS-P | Stock<sup>4</sup>| 20mph<sup>5</sup> | 0mph |
|
||||
| Toyota | Rav4 2017-18 | All | Stock<sup>4</sup>| 20mph<sup>5</sup> | 0mph |
|
||||
| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2017-18 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Sienna 2018 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Volkswagen| Golf 2016-19<sup>3</sup> | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
|
||||
| ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------|
|
||||
| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>6</sup> | 25mph |
|
||||
| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>6</sup> | 12mph |
|
||||
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
|
||||
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
|
||||
| Chrysler | Pacifica Hybrid 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph |
|
||||
| Honda | Accord 2018-19 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Accord Hybrid 2018-19 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph |
|
||||
| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph |
|
||||
| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Stock | 0mph | 2mph<sup>4</sup> |
|
||||
| Honda | CR-V 2015-16 | Touring | openpilot | 25mph<sup>6</sup> | 12mph |
|
||||
| Honda | CR-V 2017-19 | Honda Sensing | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph |
|
||||
| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph<sup>6</sup> | 12mph |
|
||||
| Honda | Insight 2019 | Honda Sensing | Stock | 0mph | 3mph |
|
||||
| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph<sup>6</sup> | 0mph |
|
||||
| Honda | Passport 2019 | All | openpilot | 25mph<sup>6</sup> | 12mph |
|
||||
| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph<sup>6</sup> | 12mph |
|
||||
| Honda | Pilot 2019 | All | openpilot | 25mph<sup>6</sup> | 12mph |
|
||||
| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph<sup>6</sup> | 12mph |
|
||||
| Hyundai | Elantra 2017-19<sup>1</sup> | SCC + LKAS | Stock | 19mph | 34mph |
|
||||
| Hyundai | Genesis 2018<sup>1</sup> | All | Stock | 19mph | 34mph |
|
||||
| Hyundai | Santa Fe 2019<sup>1</sup> | All | Stock | 0mph | 0mph |
|
||||
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph |
|
||||
| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Stock | 0mph | 39mph |
|
||||
| Kia | Optima 2019<sup>1</sup> | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Kia | Sorento 2018<sup>1</sup> | All | Stock | 0mph | 0mph |
|
||||
| Kia | Stinger 2018<sup>1</sup> | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Lexus | CT Hybrid 2017-18 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Lexus | ES 2019 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph |
|
||||
| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph |
|
||||
| Lexus | NX Hybrid 2018 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Lexus | RX 2016-17 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Lexus | RX 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2016-19 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Toyota | Avalon 2016 | TSS-P | Stock<sup>5</sup>| 20mph<sup>6</sup> | 0mph |
|
||||
| Toyota | Avalon 2017-18 | All | Stock<sup>5</sup>| 20mph<sup>6</sup> | 0mph |
|
||||
| Toyota | Camry 2018-19 | All | Stock | 0mph<sup>2</sup> | 0mph |
|
||||
| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph<sup>2</sup> | 0mph |
|
||||
| Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph |
|
||||
| Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph |
|
||||
| Toyota | Corolla 2017-19 | All | Stock<sup>5</sup>| 20mph<sup>6</sup> | 0mph |
|
||||
| Toyota | Corolla 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Corolla Hatchback 2019-20 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Corolla Hybrid 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Highlander 2017-19 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2017-19 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Prius 2016 | TSS-P | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2017-19 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2017-20 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 2016 | TSS-P | Stock<sup>5</sup>| 20mph<sup>6</sup> | 0mph |
|
||||
| Toyota | Rav4 2017-18 | All | Stock<sup>5</sup>| 20mph<sup>6</sup> | 0mph |
|
||||
| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2017-18 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Sienna 2018 | All | Stock<sup>5</sup>| 0mph | 0mph |
|
||||
| Volkswagen| Golf 2016-19<sup>3</sup> | Driver Assistance | Stock | 0mph | 0mph |
|
||||
|
||||
<sup>1</sup>Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and open sourced [Hyundai giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai), designed for the 2019 Sante Fe; pinout may differ for other Hyundai and Kia models. <br />
|
||||
<sup>2</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
|
||||
<sup>3</sup>Requires a [custom connector](https://community.comma.ai/wiki/index.php/Volkswagen#Integration_at_R242_Camera) for the [car harness](https://comma.ai/shop/products/car-harness) <br />
|
||||
<sup>4</sup>2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph. <br />
|
||||
|
||||
Community Maintained Cars and Features
|
||||
------
|
||||
|
||||
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
|
||||
| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------|
|
||||
| Buick | Regal 2018<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Cadillac | ATS 2018<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Chevrolet | Malibu 2017<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Chevrolet | Volt 2017-18<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| GMC | Acadia Denali 2018<sup>6</sup>| Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Holden | Astra 2017<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Buick | Regal 2018<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Cadillac | ATS 2018<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Chevrolet | Malibu 2017<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Chevrolet | Volt 2017-18<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| GMC | Acadia Denali 2018<sup>7</sup>| Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Holden | Astra 2017<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
|
||||
<sup>4</sup>When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).*** <br />
|
||||
<sup>5</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).*** <br />
|
||||
<sup>6</sup>Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [community built giraffe](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
|
||||
<sup>5</sup>When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).*** <br />
|
||||
<sup>6</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).*** <br />
|
||||
<sup>7</sup>Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [community built giraffe](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
|
||||
|
||||
Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`.
|
||||
|
||||
|
||||
+11
@@ -1,3 +1,14 @@
|
||||
Version 0.7.4 (2020-03-20)
|
||||
========================
|
||||
* New driving model: improved lane changes and lead car detection
|
||||
* Improved driver monitoring model: improve eye detection
|
||||
* Improved calibration stability
|
||||
* Improved lateral control on some 2019 and 2020 Toyota Prius
|
||||
* Improved lateral control on VW Golf: 20% more steering torque
|
||||
* Fixed bug where some 2017 and 2018 Toyota C-HR would use the wrong steering angle sensor
|
||||
* Support for Honda Insight thanks to theantihero!
|
||||
* Code cleanup in car abstraction layers and ui
|
||||
|
||||
Version 0.7.3 (2020-02-21)
|
||||
========================
|
||||
* Support for 2020 Highlander thanks to che220!
|
||||
|
||||
+1
-2
@@ -35,8 +35,7 @@ while 1:
|
||||
|
||||
# in publisher
|
||||
pm = messaging.PubMaster(['sensorEvents'])
|
||||
dat = messaging.new_message()
|
||||
dat.init('sensorEvents', 1)
|
||||
dat = messaging.new_message('sensorEvents', size=1)
|
||||
dat.sensorEvents[0] = {"gyro": {"v": [0.1, -0.1, 0.1]}}
|
||||
pm.send('sensorEvents', dat)
|
||||
```
|
||||
|
||||
@@ -135,6 +135,7 @@ struct CarState {
|
||||
steeringRateLimited @29 :Bool; # if the torque is limited by the rate limiter
|
||||
stockAeb @30 :Bool;
|
||||
stockFcw @31 :Bool;
|
||||
espDisabled @32 :Bool;
|
||||
|
||||
# cruise state
|
||||
cruiseState @10 :CruiseState;
|
||||
@@ -158,6 +159,10 @@ struct CarState {
|
||||
|
||||
# which packets this state came from
|
||||
canMonoTimes @12: List(UInt64);
|
||||
|
||||
# blindspot sensors
|
||||
leftBlindspot @33 :Bool; # Is there something blocking the left lane change
|
||||
rightBlindspot @34 :Bool; # Is there something blocking the right lane change
|
||||
|
||||
struct WheelSpeeds {
|
||||
# optional wheel speeds
|
||||
@@ -449,6 +454,7 @@ struct CarParams {
|
||||
noOutput @19; # like silent but without silent CAN TXs
|
||||
hondaBoschHarness @20;
|
||||
volkswagenPq @21;
|
||||
subaruLegacy @22; # pre-Global platform
|
||||
}
|
||||
|
||||
enum SteerControlType {
|
||||
@@ -476,10 +482,21 @@ struct CarParams {
|
||||
fwdCamera @3;
|
||||
engine @4;
|
||||
unknown @5;
|
||||
transmission @8; # Transmission Control Module
|
||||
srs @9; # airbag
|
||||
gateway @10; # can gateway
|
||||
hud @11; # heads up display
|
||||
combinationMeter @12; # instrument cluster
|
||||
|
||||
# Toyota only
|
||||
dsu @6;
|
||||
apgs @7;
|
||||
|
||||
# Honda only
|
||||
vsa @13; # Vehicle Stability Assist
|
||||
programmedFuelInjection @14;
|
||||
electricBrakeBooster @15;
|
||||
shiftByWire @16;
|
||||
}
|
||||
|
||||
enum FingerprintSource {
|
||||
|
||||
+73
-2
@@ -127,6 +127,8 @@ struct FrameData {
|
||||
lensTruePos @14 :Float32;
|
||||
image @6 :Data;
|
||||
gainFrac @15 :Float32;
|
||||
focusVal @16 :List(Int16);
|
||||
focusConf @17 :List(UInt8);
|
||||
|
||||
frameType @7 :FrameType;
|
||||
timestampSof @8 :UInt64;
|
||||
@@ -281,6 +283,7 @@ struct ThermalData {
|
||||
usbOnline @12 :Bool;
|
||||
networkType @22 :NetworkType;
|
||||
offroadPowerUsage @23 :UInt32; # Power usage since going offroad in uWh
|
||||
networkStrength @24 :NetworkStrength;
|
||||
|
||||
fanSpeed @10 :UInt16;
|
||||
started @11 :Bool;
|
||||
@@ -293,7 +296,7 @@ struct ThermalData {
|
||||
memUsedPercent @19 :Int8;
|
||||
cpuPerc @20 :Int8;
|
||||
|
||||
ipAddr @24 :Text; # dragonpilot
|
||||
ipAddr @25 :Text; # dragonpilot
|
||||
|
||||
enum ThermalStatus {
|
||||
green @0; # all processes run
|
||||
@@ -310,6 +313,14 @@ struct ThermalData {
|
||||
cell4G @4;
|
||||
cell5G @5;
|
||||
}
|
||||
|
||||
enum NetworkStrength {
|
||||
unknown @0;
|
||||
poor @1;
|
||||
moderate @2;
|
||||
good @3;
|
||||
great @4;
|
||||
}
|
||||
}
|
||||
|
||||
struct HealthData {
|
||||
@@ -628,9 +639,11 @@ struct ModelData {
|
||||
brakeDisengageProb @2 :Float32;
|
||||
gasDisengageProb @3 :Float32;
|
||||
steerOverrideProb @4 :Float32;
|
||||
desireState @5 :List(Float32);
|
||||
}
|
||||
|
||||
struct LongitudinalData {
|
||||
distances @2 :List(Float32);
|
||||
speeds @0 :List(Float32);
|
||||
accelerations @1 :List(Float32);
|
||||
}
|
||||
@@ -794,6 +807,52 @@ struct PathPlan {
|
||||
}
|
||||
}
|
||||
|
||||
struct LiveLocationKalman {
|
||||
|
||||
# More info on reference frames:
|
||||
# https://github.com/commaai/openpilot/tree/master/common/transformations
|
||||
|
||||
positionECEF @0 : Measurement;
|
||||
positionGeodetic @1 : Measurement;
|
||||
velocityECEF @2 : Measurement;
|
||||
velocityNED @3 : Measurement;
|
||||
velocityDevice @4 : Measurement;
|
||||
accelerationDevice @5: Measurement;
|
||||
|
||||
|
||||
# These angles are all eulers and roll, pitch, yaw
|
||||
# orientationECEF transforms to rot matrix: ecef_from_device
|
||||
orientationECEF @6 : Measurement;
|
||||
orientationNED @7 : Measurement;
|
||||
angularVelocityDevice @8 : Measurement;
|
||||
|
||||
# orientationNEDCalibrated transforms to rot matrix: NED_from_calibrated
|
||||
orientationNEDCalibrated @9 : Measurement;
|
||||
|
||||
# Calibrated frame is simply device frame
|
||||
# aligned with the vehicle
|
||||
velocityCalibrated @10 : Measurement;
|
||||
accelerationCalibrated @11 : Measurement;
|
||||
angularVelocityCalibrated @12 : Measurement;
|
||||
|
||||
gpsWeek @13 :Int32;
|
||||
gpsTimeOfWeek @14 :Float64;
|
||||
status @15 :Status;
|
||||
unixTimestampMillis @16 :Int64;
|
||||
|
||||
enum Status {
|
||||
uninitialized @0;
|
||||
uncalibrated @1;
|
||||
valid @2;
|
||||
}
|
||||
|
||||
struct Measurement {
|
||||
value @0 : List(Float64);
|
||||
std @1 : List(Float64);
|
||||
valid @2 : Bool;
|
||||
}
|
||||
}
|
||||
|
||||
struct LiveLocationData {
|
||||
status @0 :UInt8;
|
||||
|
||||
@@ -1881,6 +1940,16 @@ struct KalmanOdometry {
|
||||
rotStd @3 :List(Float32); # std rad/s in device frame
|
||||
}
|
||||
|
||||
struct Sentinel {
|
||||
enum SentinelType {
|
||||
endOfSegment @0;
|
||||
endOfRoute @1;
|
||||
startOfSegment @2;
|
||||
startOfRoute @3;
|
||||
}
|
||||
type @0 :SentinelType;
|
||||
}
|
||||
|
||||
struct Event {
|
||||
# in nanoseconds?
|
||||
logMonoTime @0 :UInt64;
|
||||
@@ -1937,7 +2006,7 @@ struct Event {
|
||||
gpsLocationExternal @48 :GpsLocationData;
|
||||
location @49 :LiveLocationData;
|
||||
uiNavigationEvent @50 :UiNavigationEvent;
|
||||
liveLocationKalman @51 :LiveLocationData;
|
||||
liveLocationKalmanDEPRECATED @51 :LiveLocationData;
|
||||
testJoystick @52 :Joystick;
|
||||
orbOdometry @53 :OrbOdometry;
|
||||
orbFeatures @54 :OrbFeatures;
|
||||
@@ -1957,5 +2026,7 @@ struct Event {
|
||||
carParams @69: Car.CarParams;
|
||||
frontFrame @70: FrameData;
|
||||
dMonitoringState @71: DMonitoringState;
|
||||
liveLocationKalman @72 :LiveLocationKalman;
|
||||
sentinel @73 :Sentinel;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -19,10 +19,15 @@ except ImportError:
|
||||
|
||||
context = Context()
|
||||
|
||||
def new_message():
|
||||
def new_message(service=None, size=None):
|
||||
dat = log.Event.new_message()
|
||||
dat.logMonoTime = int(sec_since_boot() * 1e9)
|
||||
dat.valid = True
|
||||
if service is not None:
|
||||
if size is None:
|
||||
dat.init(service)
|
||||
else:
|
||||
dat.init(service, size)
|
||||
return dat
|
||||
|
||||
def pub_sock(endpoint):
|
||||
@@ -148,12 +153,11 @@ class SubMaster():
|
||||
self.sock[s] = sub_sock(s, poller=self.poller, addr=addr, conflate=True)
|
||||
self.freq[s] = service_list[s].frequency
|
||||
|
||||
data = new_message()
|
||||
try:
|
||||
data.init(s)
|
||||
data = new_message(s)
|
||||
except capnp.lib.capnp.KjException:
|
||||
# lists
|
||||
data.init(s, 0)
|
||||
data = new_message(s, 0)
|
||||
|
||||
self.data[s] = getattr(data, s)
|
||||
self.logMonoTime[s] = 0
|
||||
|
||||
@@ -19,7 +19,7 @@ controlsState: [8007, true, 100., 100]
|
||||
model: [8009, true, 20., 5]
|
||||
features: [8010, true, 0.]
|
||||
health: [8011, true, 2., 1]
|
||||
radarState: [8012, true, 20.]
|
||||
radarState: [8012, true, 20., 5]
|
||||
#liveUI: [8014, true, 0.]
|
||||
encodeIdx: [8015, true, 20.]
|
||||
liveTracks: [8016, true, 20.]
|
||||
@@ -57,7 +57,7 @@ orbslamCorrection: [8050, true, 0.]
|
||||
liveLocationCorrected: [8051, true, 0.]
|
||||
orbObservation: [8052, true, 0.]
|
||||
applanixLocation: [8053, true, 0.]
|
||||
liveLocationKalman: [8054, true, 0.]
|
||||
liveLocationKalman: [8054, true, 0., 1]
|
||||
uiNavigationEvent: [8055, true, 0.]
|
||||
orbOdometry: [8057, true, 0.]
|
||||
orbFeatures: [8058, false, 0.]
|
||||
|
||||
@@ -8,6 +8,7 @@ import random
|
||||
from cereal import log
|
||||
|
||||
NetworkType = log.ThermalData.NetworkType
|
||||
NetworkStrength = log.ThermalData.NetworkStrength
|
||||
|
||||
ANDROID = os.path.isfile('/EON')
|
||||
|
||||
@@ -128,3 +129,156 @@ def get_network_type():
|
||||
19: NetworkType.cell4G
|
||||
}
|
||||
return cell_networks.get(cell_check, NetworkType.none)
|
||||
|
||||
def get_network_strength(network_type):
|
||||
network_strength = NetworkStrength.unknown
|
||||
# from SignalStrength.java
|
||||
def get_lte_level(rsrp, rssnr):
|
||||
INT_MAX = 2147483647
|
||||
lvl_rsrp = NetworkStrength.unknown
|
||||
lvl_rssnr = NetworkStrength.unknown
|
||||
if rsrp == INT_MAX:
|
||||
lvl_rsrp = NetworkStrength.unknown
|
||||
elif rsrp >= -95:
|
||||
lvl_rsrp = NetworkStrength.great
|
||||
elif rsrp >= -105:
|
||||
lvl_rsrp = NetworkStrength.good
|
||||
elif rsrp >= -115:
|
||||
lvl_rsrp = NetworkStrength.moderate
|
||||
else:
|
||||
lvl_rsrp = NetworkStrength.poor
|
||||
if rssnr == INT_MAX:
|
||||
lvl_rssnr = NetworkStrength.unknown
|
||||
elif rssnr >= 45:
|
||||
lvl_rssnr = NetworkStrength.great
|
||||
elif rssnr >= 10:
|
||||
lvl_rssnr = NetworkStrength.good
|
||||
elif rssnr >= -30:
|
||||
lvl_rssnr = NetworkStrength.moderate
|
||||
else:
|
||||
lvl_rssnr = NetworkStrength.poor
|
||||
return max(lvl_rsrp, lvl_rssnr)
|
||||
|
||||
def get_tdscdma_level(tdscmadbm):
|
||||
lvl = NetworkStrength.unknown
|
||||
if tdscmadbm > -25:
|
||||
lvl = NetworkStrength.unknown
|
||||
elif tdscmadbm >= -49:
|
||||
lvl = NetworkStrength.great
|
||||
elif tdscmadbm >= -73:
|
||||
lvl = NetworkStrength.good
|
||||
elif tdscmadbm >= -97:
|
||||
lvl = NetworkStrength.moderate
|
||||
elif tdscmadbm >= -110:
|
||||
lvl = NetworkStrength.poor
|
||||
return lvl
|
||||
|
||||
def get_gsm_level(asu):
|
||||
lvl = NetworkStrength.unknown
|
||||
if asu <= 2 or asu == 99:
|
||||
lvl = NetworkStrength.unknown
|
||||
elif asu >= 12:
|
||||
lvl = NetworkStrength.great
|
||||
elif asu >= 8:
|
||||
lvl = NetworkStrength.good
|
||||
elif asu >= 5:
|
||||
lvl = NetworkStrength.moderate
|
||||
else:
|
||||
lvl = NetworkStrength.poor
|
||||
return lvl
|
||||
|
||||
def get_evdo_level(evdodbm, evdosnr):
|
||||
lvl_evdodbm = NetworkStrength.unknown
|
||||
lvl_evdosnr = NetworkStrength.unknown
|
||||
if evdodbm >= -65:
|
||||
lvl_evdodbm = NetworkStrength.great
|
||||
elif evdodbm >= -75:
|
||||
lvl_evdodbm = NetworkStrength.good
|
||||
elif evdodbm >= -90:
|
||||
lvl_evdodbm = NetworkStrength.moderate
|
||||
elif evdodbm >= -105:
|
||||
lvl_evdodbm = NetworkStrength.poor
|
||||
if evdosnr >= 7:
|
||||
lvl_evdosnr = NetworkStrength.great
|
||||
elif evdosnr >= 5:
|
||||
lvl_evdosnr = NetworkStrength.good
|
||||
elif evdosnr >= 3:
|
||||
lvl_evdosnr = NetworkStrength.moderate
|
||||
elif evdosnr >= 1:
|
||||
lvl_evdosnr = NetworkStrength.poor
|
||||
return max(lvl_evdodbm, lvl_evdosnr)
|
||||
|
||||
def get_cdma_level(cdmadbm, cdmaecio):
|
||||
lvl_cdmadbm = NetworkStrength.unknown
|
||||
lvl_cdmaecio = NetworkStrength.unknown
|
||||
if cdmadbm >= -75:
|
||||
lvl_cdmadbm = NetworkStrength.great
|
||||
elif cdmadbm >= -85:
|
||||
lvl_cdmadbm = NetworkStrength.good
|
||||
elif cdmadbm >= -95:
|
||||
lvl_cdmadbm = NetworkStrength.moderate
|
||||
elif cdmadbm >= -100:
|
||||
lvl_cdmadbm = NetworkStrength.poor
|
||||
if cdmaecio >= -90:
|
||||
lvl_cdmaecio = NetworkStrength.great
|
||||
elif cdmaecio >= -110:
|
||||
lvl_cdmaecio = NetworkStrength.good
|
||||
elif cdmaecio >= -130:
|
||||
lvl_cdmaecio = NetworkStrength.moderate
|
||||
elif cdmaecio >= -150:
|
||||
lvl_cdmaecio = NetworkStrength.poor
|
||||
return max(lvl_cdmadbm, lvl_cdmaecio)
|
||||
|
||||
if network_type == NetworkType.none:
|
||||
return network_strength
|
||||
if network_type == NetworkType.wifi:
|
||||
out = subprocess.check_output('dumpsys connectivity', shell=True).decode('ascii')
|
||||
network_strength = NetworkStrength.unknown
|
||||
for line in out.split('\n'):
|
||||
signal_str = "SignalStrength: "
|
||||
if signal_str in line:
|
||||
lvl_idx_start = line.find(signal_str) + len(signal_str)
|
||||
lvl_idx_end = line.find(']', lvl_idx_start)
|
||||
lvl = int(line[lvl_idx_start : lvl_idx_end])
|
||||
if lvl >= -50:
|
||||
network_strength = NetworkStrength.great
|
||||
elif lvl >= -60:
|
||||
network_strength = NetworkStrength.good
|
||||
elif lvl >= -70:
|
||||
network_strength = NetworkStrength.moderate
|
||||
else:
|
||||
network_strength = NetworkStrength.poor
|
||||
return network_strength
|
||||
else:
|
||||
# check cell strength
|
||||
out = subprocess.check_output('dumpsys telephony.registry', shell=True).decode('ascii')
|
||||
for line in out.split('\n'):
|
||||
if "mSignalStrength" in line:
|
||||
arr = line.split(' ')
|
||||
ns = 0
|
||||
if ("gsm" in arr[14]):
|
||||
rsrp = int(arr[9])
|
||||
rssnr = int(arr[11])
|
||||
ns = get_lte_level(rsrp, rssnr)
|
||||
if ns == NetworkStrength.unknown:
|
||||
tdscmadbm = int(arr[13])
|
||||
ns = get_tdscdma_level(tdscmadbm)
|
||||
if ns == NetworkStrength.unknown:
|
||||
asu = int(arr[1])
|
||||
ns = get_gsm_level(asu)
|
||||
else:
|
||||
cdmadbm = int(arr[3])
|
||||
cdmaecio = int(arr[4])
|
||||
evdodbm = int(arr[5])
|
||||
evdosnr = int(arr[7])
|
||||
lvl_cdma = get_cdma_level(cdmadbm, cdmaecio)
|
||||
lvl_edmo = get_evdo_level(evdodbm, evdosnr)
|
||||
if lvl_edmo == NetworkStrength.unknown:
|
||||
ns = lvl_cdma
|
||||
elif lvl_cdma == NetworkStrength.unknown:
|
||||
ns = lvl_edmo
|
||||
else:
|
||||
ns = min(lvl_cdma, lvl_edmo)
|
||||
network_strength = max(network_strength, ns)
|
||||
|
||||
return network_strength
|
||||
|
||||
+3
-4
@@ -6,7 +6,7 @@ import shutil
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
android_packages = ("tw.com.ainvest.outpack", "cn.dragonpilot.gpsservice", "com.autonavi.amapauto", "com.mixplorer", "com.tomtom.speedcams.android.map", "ai.comma.plus.offroad", "ai.comma.plus.frame")
|
||||
android_packages = ("ai.comma.plus.offroad", "tw.com.ainvest.outpack", "cn.dragonpilot.gpsservice", "com.autonavi.amapauto", "com.mixplorer", "com.tomtom.speedcams.android.map")
|
||||
|
||||
def get_installed_apks():
|
||||
dat = subprocess.check_output(["pm", "list", "packages", "-f"], encoding='utf8').strip().split("\n")
|
||||
@@ -26,9 +26,9 @@ def install_apk(path):
|
||||
os.remove(install_path)
|
||||
return ret == 0
|
||||
|
||||
def start_frame():
|
||||
def start_offroad():
|
||||
set_package_permissions()
|
||||
system("am start -n ai.comma.plus.frame/.MainActivity")
|
||||
system("am start -n ai.comma.plus.offroad/.MainActivity")
|
||||
|
||||
def set_package_permissions():
|
||||
pm_grant("ai.comma.plus.offroad", "android.permission.ACCESS_FINE_LOCATION")
|
||||
@@ -95,4 +95,3 @@ def pm_apply_packages(cmd):
|
||||
|
||||
if __name__ == "__main__":
|
||||
update_apks()
|
||||
|
||||
|
||||
@@ -0,0 +1,50 @@
|
||||
def cputime_total(ct):
|
||||
return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem
|
||||
|
||||
|
||||
def print_cpu_usage(first_proc, last_proc):
|
||||
r = 0
|
||||
procs = [
|
||||
("selfdrive.controls.controlsd", 59.46),
|
||||
("./_modeld", 48.94),
|
||||
("./loggerd", 28.49),
|
||||
("selfdrive.controls.plannerd", 19.77),
|
||||
("selfdrive.controls.radard", 9.54),
|
||||
("./_ui", 9.54),
|
||||
("./camerad", 7.07),
|
||||
("selfdrive.locationd.locationd", 7.13),
|
||||
("./_sensord", 6.17),
|
||||
("selfdrive.controls.dmonitoringd", 5.48),
|
||||
("./boardd", 3.63),
|
||||
("./_dmonitoringmodeld", 2.67),
|
||||
("selfdrive.logmessaged", 2.71),
|
||||
("selfdrive.thermald", 2.41),
|
||||
("./paramsd", 2.18),
|
||||
("selfdrive.locationd.calibrationd", 1.76),
|
||||
("./proclogd", 1.54),
|
||||
("./_gpsd", 0.09),
|
||||
("./clocksd", 0.02),
|
||||
("./ubloxd", 0.02),
|
||||
("selfdrive.tombstoned", 0),
|
||||
("./logcatd", 0),
|
||||
("selfdrive.updated", 0),
|
||||
]
|
||||
|
||||
dt = (last_proc.logMonoTime - first_proc.logMonoTime) / 1e9
|
||||
print("------------------------------------------------")
|
||||
for proc_name, normal_cpu_usage in procs:
|
||||
try:
|
||||
first = [p for p in first_proc.procLog.procs if proc_name in p.cmdline][0]
|
||||
last = [p for p in last_proc.procLog.procs if proc_name in p.cmdline][0]
|
||||
cpu_time = cputime_total(last) - cputime_total(first)
|
||||
cpu_usage = cpu_time / dt * 100.
|
||||
if cpu_usage > max(normal_cpu_usage * 1.1, normal_cpu_usage + 5.0):
|
||||
print(f"Warning {proc_name} using more CPU than normal")
|
||||
r = 1
|
||||
|
||||
print(f"{proc_name.ljust(35)} {cpu_usage:.2f}%")
|
||||
except IndexError:
|
||||
print(f"{proc_name.ljust(35)} NO METRICS FOUND")
|
||||
print("------------------------------------------------")
|
||||
|
||||
return r
|
||||
+7
-2
@@ -22,6 +22,8 @@ file in place without messing with <params_dir>/d.
|
||||
"""
|
||||
import time
|
||||
import os
|
||||
import string
|
||||
import binascii
|
||||
import errno
|
||||
import sys
|
||||
import shutil
|
||||
@@ -59,6 +61,7 @@ keys = {
|
||||
"CommunityFeaturesToggle": [TxType.PERSISTENT],
|
||||
"CompletedTrainingVersion": [TxType.PERSISTENT],
|
||||
"ControlsParams": [TxType.PERSISTENT],
|
||||
"DisablePowerDown": [TxType.PERSISTENT],
|
||||
"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"DongleId": [TxType.PERSISTENT],
|
||||
"GitBranch": [TxType.PERSISTENT],
|
||||
@@ -169,6 +172,8 @@ keys = {
|
||||
"DragonBootHotspot": [TxType.PERSISTENT],
|
||||
"DragonAccelProfile": [TxType.PERSISTENT],
|
||||
"DragonLastModified": [TxType.PERSISTENT],
|
||||
"DragonEnableRegistration": [TxType.PERSISTENT],
|
||||
"DragonDynamicFollow": [TxType.PERSISTENT],
|
||||
}
|
||||
|
||||
|
||||
@@ -473,10 +478,10 @@ if __name__ == "__main__":
|
||||
pp = params.get(k)
|
||||
if pp is None:
|
||||
print("%s is None" % k)
|
||||
elif all(ord(c) < 128 and ord(c) >= 32 for c in pp):
|
||||
elif all(chr(c) in string.printable for c in pp):
|
||||
print("%s = %s" % (k, pp))
|
||||
else:
|
||||
print("%s = %s" % (k, pp.encode("hex")))
|
||||
print("%s = %s" % (k, binascii.hexlify(pp)))
|
||||
|
||||
# Test multiprocess:
|
||||
# seq 0 100000 | xargs -P20 -I{} python common/params.py DongleId {} && sleep 0.05
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -85,6 +85,9 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
|
||||
case 0x117: // ACC_10 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC}[counter];
|
||||
break;
|
||||
case 0x121: // Motor_20 Driver Throttle Inputs
|
||||
crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter];
|
||||
break;
|
||||
case 0x122: // ACC_06 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0x37,0x7D,0xF3,0xA9,0x18,0x46,0x6D,0x4D,0x3D,0x71,0x92,0x9C,0xE5,0x32,0x10,0xB9}[counter];
|
||||
break;
|
||||
|
||||
@@ -160,6 +160,82 @@ BO_ 545 XXX_16: 6 SCM
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
BO_ 576 LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 577 LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 579 RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 580 RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 582 ADJACENT_LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 583 ADJACENT_LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 585 ADJACENT_RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
@@ -284,5 +360,9 @@ BO_ 891 STALK_STATUS_2: 8 XXX
|
||||
CM_ SG_ 479 AEB_STATUS "set for the duration of AEB event";
|
||||
CM_ SG_ 479 AEB_BRAKING "set when braking is commanded during AEB event";
|
||||
CM_ SG_ 479 AEB_PREPARE "set 1s before AEB";
|
||||
|
||||
CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded";
|
||||
CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded";
|
||||
CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded";
|
||||
CM_ SG_ 577 LINE_DASHED "1 = line is dashed";
|
||||
CM_ SG_ 577 LINE_SOLID "1 = line is solid";
|
||||
VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
@@ -56,6 +56,7 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM
|
||||
|
||||
BO_ 420 VSA_STATUS: 8 VSA
|
||||
SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON
|
||||
SG_ COMPUTER_BRAKING : 23|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ BRAKE_HOLD_RELATED : 52|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" EON
|
||||
@@ -63,6 +64,13 @@ BO_ 420 VSA_STATUS: 8 VSA
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
BO_ 427 STEER_MOTOR_TORQUE: 3 EPS
|
||||
SG_ CONFIG_VALID : 7|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ MOTOR_TORQUE : 1|10@0+ (1,0) [0|256] "" EON
|
||||
SG_ OUTPUT_DISABLED : 22|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ COUNTER : 21|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 19|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
BO_ 432 STANDSTILL: 7 VSA
|
||||
SG_ CONTROLLED_STANDSTILL : 0|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ WHEELS_MOVING : 12|1@0+ (1,0) [0|1] "" EON
|
||||
@@ -93,12 +101,15 @@ BO_ 506 BRAKE_COMMAND: 8 ADAS
|
||||
SG_ CRUISE_FAULT_CMD : 18|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_CANCEL_CMD : 17|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ COMPUTER_BRAKE_REQUEST : 16|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ SET_ME_0X80 : 31|8@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ SET_ME_1 : 31|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ AEB_REQ_1 : 29|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ AEB_REQ_2 : 26|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ BRAKE_LIGHTS : 39|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_STATES : 38|7@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CHIME : 47|3@0+ (1,0) [0|7] "" EBCM
|
||||
SG_ ZEROS_BOH6 : 44|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ FCW : 43|2@0+ (1,0) [0|3] "" EBCM
|
||||
SG_ AEB_STATUS : 41|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ COMPUTER_BRAKE : 55|10@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EBCM
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EBCM
|
||||
@@ -138,7 +149,7 @@ BO_ 780 ACC_HUD: 8 ADAS
|
||||
SG_ BOH : 38|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ACC_PROBLEM : 37|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ FCM_OFF : 36|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ BOH_2 : 35|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ FCM_OFF_2 : 35|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ FCM_PROBLEM : 34|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ RADAR_OBSTRUCTED : 33|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ENABLE_MINI_CAR : 32|1@0+ (1,0) [0|1] "" BDY
|
||||
@@ -148,10 +159,12 @@ BO_ 780 ACC_HUD: 8 ADAS
|
||||
SG_ BOH_4 : 42|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ BOH_5 : 41|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CRUISE_CONTROL_LABEL : 40|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ HUD_DISTANCE_3 : 52|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ IMPERIAL_UNIT : 54|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SET_ME_X01_2 : 55|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ IMPERIAL_UNIT : 54|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ HUD_DISTANCE_3 : 52|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ CHIME : 51|3@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SET_ME_X01 : 48|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ICONS : 63|2@0+ (1,0) [0|1] "" BDY
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" BDY
|
||||
|
||||
@@ -201,6 +214,7 @@ BO_ 1029 DOORS_STATUS: 8 BDY
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
CM_ SG_ 490 LONG_ACCEL "wheel speed derivative, noisy and zero snapping";
|
||||
CM_ SG_ 506 AEB_REQ_1 "set for duration of suspected AEB event";
|
||||
CM_ SG_ 773 PASS_AIRBAG_ON "Might just be indicator light";
|
||||
CM_ SG_ 773 PASS_AIRBAG_OFF "Might just be indicator light";
|
||||
CM_ SG_ 780 CRUISE_SPEED "255 = no speed";
|
||||
@@ -210,6 +224,7 @@ CM_ SG_ 829 BEEP "beeps are pleasant, chimes are for warnngs etc...";
|
||||
|
||||
VAL_ 506 FCW 3 "fcw" 2 "fcw" 1 "fcw" 0 "no_fcw" ;
|
||||
VAL_ 506 CHIME 4 "double_chime" 3 "single_chime" 2 "continuous_chime" 1 "repeating_chime" 0 "no_chime" ;
|
||||
VAL_ 506 AEB_STATUS 3 "aeb_prepare" 2 "aeb_ready" 1 "aeb_braking" 0 "no_aeb" ;
|
||||
VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ;
|
||||
VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ;
|
||||
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
BO_ 1014 BSM: 8 XXX
|
||||
SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ R_ADJACENT : 1|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ R_APPROACHING : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ADJACENT_ENABLED : 7|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ APPROACHING_ENABLED : 15|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
CM_ SG_ 1014 L_ADJACENT "vehicle adjacent left side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 L_APPROACHING "vehicle approaching from left side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 R_ADJACENT "vehicle adjacent right side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 R_APPROACHING "vehicle approaching from right side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 ADJACENT_ENABLED "when BSM is enabled in settings, this is on along with APPROACHING_ENABLED. this controls bsm alert visibility";
|
||||
CM_ SG_ 1014 APPROACHING_ENABLED "when BSM is enabled in settings, this is on along with ADJACENT_ENABLED. this controls bsm alert visibility";
|
||||
@@ -2,9 +2,8 @@ CM_ "IMPORT _toyota_2017.dbc"
|
||||
CM_ "IMPORT _comma.dbc"
|
||||
|
||||
BO_ 548 BRAKE_MODULE: 8 XXX
|
||||
SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX
|
||||
SG_ BRAKE_POSITION : 16|9@0+ (1,0) [0|511] "" XXX
|
||||
SG_ BRAKE_PRESSED : 37|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BRAKE_PRESSURE : 43|12@0+ (1,0) [0|4047] "" XXX
|
||||
SG_ BRAKE_PRESSED : 5|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 581 GAS_PEDAL: 5 XXX
|
||||
SG_ GAS_PEDAL : 23|8@0+ (0.005,0) [0|1] "" XXX
|
||||
@@ -14,6 +13,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -14,6 +14,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -14,6 +14,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -11,6 +11,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -14,6 +14,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -15,6 +15,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -14,6 +14,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -15,6 +15,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -14,6 +14,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
CM_ "IMPORT _toyota_2017.dbc"
|
||||
CM_ "IMPORT _comma.dbc"
|
||||
CM_ "IMPORT _toyota_nodsu_bsm.dbc"
|
||||
|
||||
BO_ 295 GEAR_PACKET: 8 XXX
|
||||
SG_ CAR_MOVEMENT : 39|8@0- (1,0) [0|255] "" XXX
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
CM_ "IMPORT _toyota_2017.dbc"
|
||||
CM_ "IMPORT _comma.dbc"
|
||||
CM_ "IMPORT _toyota_nodsu_bsm.dbc"
|
||||
|
||||
BO_ 401 STEERING_LTA: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
@@ -14,6 +14,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -14,6 +14,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -15,6 +15,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -164,6 +164,82 @@ BO_ 545 XXX_16: 6 SCM
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
BO_ 576 LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 577 LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 579 RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 580 RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 582 ADJACENT_LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 583 ADJACENT_LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 585 ADJACENT_RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
@@ -288,7 +364,11 @@ BO_ 891 STALK_STATUS_2: 8 XXX
|
||||
CM_ SG_ 479 AEB_STATUS "set for the duration of AEB event";
|
||||
CM_ SG_ 479 AEB_BRAKING "set when braking is commanded during AEB event";
|
||||
CM_ SG_ 479 AEB_PREPARE "set 1s before AEB";
|
||||
|
||||
CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded";
|
||||
CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded";
|
||||
CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded";
|
||||
CM_ SG_ 577 LINE_DASHED "1 = line is dashed";
|
||||
CM_ SG_ 577 LINE_SOLID "1 = line is solid";
|
||||
VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
CM_ "honda_accord_lx15t_2018_can.dbc starts here"
|
||||
|
||||
@@ -164,6 +164,82 @@ BO_ 545 XXX_16: 6 SCM
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
BO_ 576 LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 577 LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 579 RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 580 RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 582 ADJACENT_LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 583 ADJACENT_LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 585 ADJACENT_RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
@@ -288,7 +364,11 @@ BO_ 891 STALK_STATUS_2: 8 XXX
|
||||
CM_ SG_ 479 AEB_STATUS "set for the duration of AEB event";
|
||||
CM_ SG_ 479 AEB_BRAKING "set when braking is commanded during AEB event";
|
||||
CM_ SG_ 479 AEB_PREPARE "set 1s before AEB";
|
||||
|
||||
CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded";
|
||||
CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded";
|
||||
CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded";
|
||||
CM_ SG_ 577 LINE_DASHED "1 = line is dashed";
|
||||
CM_ SG_ 577 LINE_SOLID "1 = line is solid";
|
||||
VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
CM_ "honda_accord_s2t_2018_can.dbc starts here"
|
||||
|
||||
@@ -164,6 +164,82 @@ BO_ 545 XXX_16: 6 SCM
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
BO_ 576 LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 577 LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 579 RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 580 RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 582 ADJACENT_LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 583 ADJACENT_LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 585 ADJACENT_RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
@@ -288,7 +364,11 @@ BO_ 891 STALK_STATUS_2: 8 XXX
|
||||
CM_ SG_ 479 AEB_STATUS "set for the duration of AEB event";
|
||||
CM_ SG_ 479 AEB_BRAKING "set when braking is commanded during AEB event";
|
||||
CM_ SG_ 479 AEB_PREPARE "set 1s before AEB";
|
||||
|
||||
CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded";
|
||||
CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded";
|
||||
CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded";
|
||||
CM_ SG_ 577 LINE_DASHED "1 = line is dashed";
|
||||
CM_ SG_ 577 LINE_SOLID "1 = line is solid";
|
||||
VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
CM_ "honda_civic_hatchback_ex_2017_can.dbc starts here"
|
||||
|
||||
@@ -164,6 +164,82 @@ BO_ 545 XXX_16: 6 SCM
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
BO_ 576 LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 577 LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 579 RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 580 RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 582 ADJACENT_LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 583 ADJACENT_LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 585 ADJACENT_RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
@@ -288,7 +364,11 @@ BO_ 891 STALK_STATUS_2: 8 XXX
|
||||
CM_ SG_ 479 AEB_STATUS "set for the duration of AEB event";
|
||||
CM_ SG_ 479 AEB_BRAKING "set when braking is commanded during AEB event";
|
||||
CM_ SG_ 479 AEB_PREPARE "set 1s before AEB";
|
||||
|
||||
CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded";
|
||||
CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded";
|
||||
CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded";
|
||||
CM_ SG_ 577 LINE_DASHED "1 = line is dashed";
|
||||
CM_ SG_ 577 LINE_SOLID "1 = line is solid";
|
||||
VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
CM_ "honda_civic_sedan_16_diesel_2019_can.dbc starts here"
|
||||
|
||||
@@ -164,6 +164,82 @@ BO_ 545 XXX_16: 6 SCM
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
BO_ 576 LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 577 LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 579 RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 580 RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 582 ADJACENT_LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 583 ADJACENT_LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 585 ADJACENT_RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
@@ -288,7 +364,11 @@ BO_ 891 STALK_STATUS_2: 8 XXX
|
||||
CM_ SG_ 479 AEB_STATUS "set for the duration of AEB event";
|
||||
CM_ SG_ 479 AEB_BRAKING "set when braking is commanded during AEB event";
|
||||
CM_ SG_ 479 AEB_PREPARE "set 1s before AEB";
|
||||
|
||||
CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded";
|
||||
CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded";
|
||||
CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded";
|
||||
CM_ SG_ 577 LINE_DASHED "1 = line is dashed";
|
||||
CM_ SG_ 577 LINE_SOLID "1 = line is solid";
|
||||
VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
CM_ "honda_crv_ex_2017_can.dbc starts here"
|
||||
|
||||
@@ -164,6 +164,82 @@ BO_ 545 XXX_16: 6 SCM
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
BO_ 576 LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 577 LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 579 RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 580 RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 582 ADJACENT_LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 583 ADJACENT_LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 585 ADJACENT_RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
@@ -288,7 +364,11 @@ BO_ 891 STALK_STATUS_2: 8 XXX
|
||||
CM_ SG_ 479 AEB_STATUS "set for the duration of AEB event";
|
||||
CM_ SG_ 479 AEB_BRAKING "set when braking is commanded during AEB event";
|
||||
CM_ SG_ 479 AEB_PREPARE "set 1s before AEB";
|
||||
|
||||
CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded";
|
||||
CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded";
|
||||
CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded";
|
||||
CM_ SG_ 577 LINE_DASHED "1 = line is dashed";
|
||||
CM_ SG_ 577 LINE_SOLID "1 = line is solid";
|
||||
VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
CM_ "honda_crv_hybrid_2019_can.dbc starts here"
|
||||
|
||||
@@ -78,6 +78,7 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM
|
||||
|
||||
BO_ 420 VSA_STATUS: 8 VSA
|
||||
SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON
|
||||
SG_ COMPUTER_BRAKING : 23|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ BRAKE_HOLD_RELATED : 52|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" EON
|
||||
@@ -85,6 +86,13 @@ BO_ 420 VSA_STATUS: 8 VSA
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
BO_ 427 STEER_MOTOR_TORQUE: 3 EPS
|
||||
SG_ CONFIG_VALID : 7|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ MOTOR_TORQUE : 1|10@0+ (1,0) [0|256] "" EON
|
||||
SG_ OUTPUT_DISABLED : 22|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ COUNTER : 21|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 19|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
BO_ 432 STANDSTILL: 7 VSA
|
||||
SG_ CONTROLLED_STANDSTILL : 0|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ WHEELS_MOVING : 12|1@0+ (1,0) [0|1] "" EON
|
||||
@@ -115,12 +123,15 @@ BO_ 506 BRAKE_COMMAND: 8 ADAS
|
||||
SG_ CRUISE_FAULT_CMD : 18|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_CANCEL_CMD : 17|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ COMPUTER_BRAKE_REQUEST : 16|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ SET_ME_0X80 : 31|8@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ SET_ME_1 : 31|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ AEB_REQ_1 : 29|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ AEB_REQ_2 : 26|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ BRAKE_LIGHTS : 39|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_STATES : 38|7@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CHIME : 47|3@0+ (1,0) [0|7] "" EBCM
|
||||
SG_ ZEROS_BOH6 : 44|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ FCW : 43|2@0+ (1,0) [0|3] "" EBCM
|
||||
SG_ AEB_STATUS : 41|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ COMPUTER_BRAKE : 55|10@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EBCM
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EBCM
|
||||
@@ -160,7 +171,7 @@ BO_ 780 ACC_HUD: 8 ADAS
|
||||
SG_ BOH : 38|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ACC_PROBLEM : 37|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ FCM_OFF : 36|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ BOH_2 : 35|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ FCM_OFF_2 : 35|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ FCM_PROBLEM : 34|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ RADAR_OBSTRUCTED : 33|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ENABLE_MINI_CAR : 32|1@0+ (1,0) [0|1] "" BDY
|
||||
@@ -170,10 +181,12 @@ BO_ 780 ACC_HUD: 8 ADAS
|
||||
SG_ BOH_4 : 42|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ BOH_5 : 41|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CRUISE_CONTROL_LABEL : 40|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ HUD_DISTANCE_3 : 52|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ IMPERIAL_UNIT : 54|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SET_ME_X01_2 : 55|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ IMPERIAL_UNIT : 54|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ HUD_DISTANCE_3 : 52|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ CHIME : 51|3@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SET_ME_X01 : 48|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ICONS : 63|2@0+ (1,0) [0|1] "" BDY
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" BDY
|
||||
|
||||
@@ -223,6 +236,7 @@ BO_ 1029 DOORS_STATUS: 8 BDY
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
CM_ SG_ 490 LONG_ACCEL "wheel speed derivative, noisy and zero snapping";
|
||||
CM_ SG_ 506 AEB_REQ_1 "set for duration of suspected AEB event";
|
||||
CM_ SG_ 773 PASS_AIRBAG_ON "Might just be indicator light";
|
||||
CM_ SG_ 773 PASS_AIRBAG_OFF "Might just be indicator light";
|
||||
CM_ SG_ 780 CRUISE_SPEED "255 = no speed";
|
||||
@@ -232,6 +246,7 @@ CM_ SG_ 829 BEEP "beeps are pleasant, chimes are for warnngs etc...";
|
||||
|
||||
VAL_ 506 FCW 3 "fcw" 2 "fcw" 1 "fcw" 0 "no_fcw" ;
|
||||
VAL_ 506 CHIME 4 "double_chime" 3 "single_chime" 2 "continuous_chime" 1 "repeating_chime" 0 "no_chime" ;
|
||||
VAL_ 506 AEB_STATUS 3 "aeb_prepare" 2 "aeb_ready" 1 "aeb_braking" 0 "no_aeb" ;
|
||||
VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ;
|
||||
VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ;
|
||||
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
|
||||
|
||||
@@ -164,6 +164,82 @@ BO_ 545 XXX_16: 6 SCM
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
BO_ 576 LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 577 LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 579 RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 580 RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 582 ADJACENT_LEFT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 583 ADJACENT_LEFT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 585 ADJACENT_RIGHT_LANE_LINE_1: 8 CAM
|
||||
SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX
|
||||
SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX
|
||||
SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM
|
||||
SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX
|
||||
SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX
|
||||
SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
@@ -288,7 +364,11 @@ BO_ 891 STALK_STATUS_2: 8 XXX
|
||||
CM_ SG_ 479 AEB_STATUS "set for the duration of AEB event";
|
||||
CM_ SG_ 479 AEB_BRAKING "set when braking is commanded during AEB event";
|
||||
CM_ SG_ 479 AEB_PREPARE "set 1s before AEB";
|
||||
|
||||
CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded";
|
||||
CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded";
|
||||
CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded";
|
||||
CM_ SG_ 577 LINE_DASHED "1 = line is dashed";
|
||||
CM_ SG_ 577 LINE_SOLID "1 = line is solid";
|
||||
VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
CM_ "honda_insight_ex_2019_can.dbc starts here"
|
||||
|
||||
@@ -361,9 +361,8 @@ CM_ "lexus_ct200h_2018_pt.dbc starts here"
|
||||
|
||||
|
||||
BO_ 548 BRAKE_MODULE: 8 XXX
|
||||
SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX
|
||||
SG_ BRAKE_POSITION : 16|9@0+ (1,0) [0|511] "" XXX
|
||||
SG_ BRAKE_PRESSED : 37|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BRAKE_PRESSURE : 43|12@0+ (1,0) [0|4047] "" XXX
|
||||
SG_ BRAKE_PRESSED : 5|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 581 GAS_PEDAL: 5 XXX
|
||||
SG_ GAS_PEDAL : 23|8@0+ (0.005,0) [0|1] "" XXX
|
||||
@@ -373,6 +372,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -373,6 +373,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -373,6 +373,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -370,6 +370,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -373,6 +373,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
+147
-71
@@ -45,26 +45,28 @@ BO_ 645 WheelspeedRear: 8 XXX
|
||||
SG_ RR : 7|16@0+ (0.00555,0) [0|65535] "KPH" XXX
|
||||
SG_ RL : 23|16@0+ (0.00555,0) [0|65535] "KPH" XXX
|
||||
|
||||
BO_ 768 STEER_TORQUE: 8 XXX
|
||||
SG_ STEERING_TOURQUE : 0|7@1+ (1,0) [0|127] "" XXX
|
||||
BO_ 768 STEER_TORQUE: 2 XXX
|
||||
SG_ STEERING_TORQUE : 6|7@0+ (1,0) [0|127] "" XXX
|
||||
SG_ DriverTouchingWheel : 15|1@0+ (-1,1) [0|7] "" XXX
|
||||
|
||||
BO_ 459 Maybe_RegenBraking: 8 XXX
|
||||
|
||||
BO_ 372 Maybe_Gear_Selector: 8 XXX
|
||||
SG_ Counter : 32|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Counter : 35|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
BO_ 374 Maybe_Motor_RPM_or_Speed: 8 XXX
|
||||
SG_ Counter : 48|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Counter : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
BO_ 460 Maybe_Brake_Related: 8 XXX
|
||||
|
||||
BO_ 2 SteeringWheel: 8 XXX
|
||||
SG_ Steering_RateChange : 23|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ Always_07 : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ Steering_RateChange : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ Always_07 : 24|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ Steering_Angle : 0|16@1- (-0.1,0) [0|65535] "" XXX
|
||||
SG_ Counter : 32|4@1+ (1,0) [0|15] "" XXX
|
||||
|
||||
BO_ 384 Maybe_PowerInfo: 8 XXX
|
||||
SG_ Unknown_Timer_PowerInfo : 48|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Unknown_Timer_PowerInfo : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ EnginePower : 27|12@0- (1,0) [0|1] "" XXX
|
||||
SG_ RequestedAccel : 23|12@0- (1,0) [0|4294967295] "" XXX
|
||||
|
||||
@@ -78,32 +80,13 @@ BO_ 666 WheelspeedFront: 8 XXX
|
||||
SG_ FR : 7|16@0+ (0.00555,0) [0|65535] "KPH" XXX
|
||||
SG_ FL : 23|16@0+ (0.00555,0) [0|65535] "KPH" XXX
|
||||
|
||||
BO_ 398 NEW_MSG_2: 8 XXX
|
||||
|
||||
BO_ 389 NEW_MSG_3: 8 XXX
|
||||
SG_ NEW_SIGNAL_1 : 15|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_2 : 22|6@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_4 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_5 : 55|4@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_6 : 63|8@0+ (1,0) [0|127] "" XXX
|
||||
SG_ COUNTER : 48|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ NEW_SIGNAL_3 : 30|8@0- (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 397 NEW_MSG_4: 8 XXX
|
||||
SG_ NEW_SIGNAL_1 : 7|16@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_2 : 23|16@0+ (1,0) [0|32767] "" XXX
|
||||
SG_ NEW_SIGNAL_3 : 39|16@0+ (1,0) [0|65535] "" XXX
|
||||
SG_ NEW_SIGNAL_4 : 55|16@0+ (1,0) [0|31] "" XXX
|
||||
|
||||
BO_ 658 NEW_MSG_5: 8 XXX
|
||||
SG_ NEW_SIGNAL_1 : 7|16@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 855 NEW_MSG_6: 8 XXX
|
||||
SG_ NEW_SIGNAL_1 : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 773 NEW_MSG_7: 8 XXX
|
||||
SG_ NEW_SIGNAL_1 : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_2 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
BO_ 389 Steering: 8 XXX
|
||||
SG_ COUNTER : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ LKAS_ACTIVE : 37|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ LKAS_Torque : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CRC : 63|8@0+ (1,0) [0|127] "" XXX
|
||||
SG_ Angle : 23|18@0+ (-0.01,1310) [0|262143] "" XXX
|
||||
SG_ DriverTorque : 7|12@0+ (-0.01,20.47) [0|4095] "Nm" XXX
|
||||
|
||||
BO_ 851 SPEED_RELATED: 8 XXX
|
||||
SG_ SPEED_RELATED : 7|16@0+ (0.01014,0) [0|65535] "" XXX
|
||||
@@ -111,31 +94,30 @@ BO_ 851 SPEED_RELATED: 8 XXX
|
||||
BO_ 386 Accelerator: 8 XXX
|
||||
SG_ Accelerator : 38|7@0+ (1,0) [0|127] "" XXX
|
||||
|
||||
BO_ 347 ACCELSOMETHING: 8 XXX
|
||||
SG_ PowerMaybe : 9|10@0+ (1,0) [0|1023] "" XXX
|
||||
SG_ ACCELERATOR2 : 6|11@0+ (1,-800) [0|65535] "" XXX
|
||||
|
||||
BO_ 346 ANOTHER_ACCEL: 8 XXX
|
||||
SG_ ANOTHERACCEL : 23|10@0+ (1,0) [0|1023] "" XXX
|
||||
SG_ Reverse_ACCEL : 25|10@0+ (1,0) [0|1023] "" XXX
|
||||
|
||||
BO_ 348 FULLRANGEACCEL: 8 XXX
|
||||
SG_ AccelFullRange : 47|10@0+ (1,0) [0|1023] "" XXX
|
||||
SG_ Accel : 26|11@0+ (1,0) [0|2047] "" XXX
|
||||
BO_ 348 Throttle: 8 XXX
|
||||
SG_ NEW_SIGNAL_1 : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ Throttle : 26|11@0+ (1,0) [0|2047] "" XXX
|
||||
SG_ ThrottlePedal : 47|10@0+ (1,0) [0|1023] "" XXX
|
||||
|
||||
BO_ 566 ANOTHERFULLRANGEACCEL: 8 XXX
|
||||
SG_ ANOTHERFULLRANGEACCEL : 43|8@0+ (1,0) [0|1023] "" XXX
|
||||
SG_ RPMORTORQUE : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
BO_ 523 CruiseThrottle: 6 XXX
|
||||
SG_ PROPILOT_BUTTON : 8|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CANCEL_BUTTON : 9|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ GAS_PEDAL_INVERTED : 37|10@0+ (1,0) [0|1023] "" XXX
|
||||
SG_ SET_BUTTON : 11|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RES_BUTTON : 12|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FOLLOW_DISTANCE_BUTTON : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NO_BUTTON_PRESSED : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ GAS_PEDAL : 31|10@0+ (1,0) [0|255] "" XXX
|
||||
SG_ USER_BRAKE_PRESSED : 21|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ USER_BRAKE_PRESSED_INVERTED : 22|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_2 : 23|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unsure2 : 43|4@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unsure1 : 7|16@0+ (1,0) [0|15] "" XXX
|
||||
SG_ GAS_PRESSED_INVERTED : 20|1@0+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 17|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ unsure3 : 19|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 523 Yetyetanotheraccel: 8 XXX
|
||||
SG_ ANOTHERREVERSEACCEL : 37|10@0+ (1,0) [0|1023] "" XXX
|
||||
SG_ yetyetanotheraccel : 31|10@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 779 ANOTHERRRFULLRANGEACCEL: 8 XXX
|
||||
SG_ ANOTHERRRFULLRANGEACCEL : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 1108 Doors: 8 XXX
|
||||
BO_ 1108 DoorsLights: 8 XXX
|
||||
SG_ DOOR_CLOSED_RR : 40|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_OPEN_RR : 41|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_CLOSED_RL : 42|1@0+ (1,0) [0|1] "" XXX
|
||||
@@ -145,29 +127,25 @@ BO_ 1108 Doors: 8 XXX
|
||||
SG_ DOOR_CLOSED_FR : 46|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ DOOR_OPEN_FR : 47|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ BOOT_OPEN : 55|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BRAKE_LIGHT : 54|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ USER_BRAKE_PRESSED : 23|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 403 LKAS_OLD: 8 XXX
|
||||
SG_ Checksum : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ Angle_2 : 32|13@0+ (1,-4000) [0|63] "" XXX
|
||||
SG_ Counter : 48|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Counter : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ Angle_1 : 10|13@0+ (0.12,-480) [0|65535] "" XXX
|
||||
SG_ Steering_Torque : 7|13@0+ (-1,4000) [0|65535] "" XXX
|
||||
SG_ Torque_Command : 29|13@0+ (1,-4000) [0|255] "" XXX
|
||||
|
||||
BO_ 412 NEW_MSG_9: 8 XXX
|
||||
SG_ NEW_SIGNAL_1 : 7|16@0+ (1,0) [0|65535] "" XXX
|
||||
SG_ NEW_SIGNAL_3 : 23|16@0+ (1,0) [0|65535] "" XXX
|
||||
SG_ NEW_SIGNAL_2 : 39|8@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_4 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 361 LKAS: 8 XXX
|
||||
SG_ NEW_SIGNAL_4 : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_X80 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ LKA_Active : 52|1@0+ (1,0) [0|15] "" XXX
|
||||
SG_ MAX_TORQUE : 39|8@0+ (0.01,0) [0|255] "Nm" XXX
|
||||
SG_ SET_0x80 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ LKA_ACTIVE : 52|1@0+ (1,0) [0|15] "" XXX
|
||||
SG_ CRC : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_0x80_2 : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ Counter : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ Des_Angle : 7|18@0+ (-0.01,1310) [0|255] "" XXX
|
||||
SG_ COUNTER : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ DESIRED_ANGLE : 7|18@0+ (-0.01,1310) [0|255] "" XXX
|
||||
|
||||
BO_ 438 ProPilot: 8 XXX
|
||||
SG_ NEW_SIGNAL_2 : 11|4@0+ (1,0) [0|255] "" XXX
|
||||
@@ -175,10 +153,108 @@ BO_ 438 ProPilot: 8 XXX
|
||||
SG_ NEW_SIGNAL_8 : 63|8@0+ (1,0) [0|7] "" XXX
|
||||
SG_ Counter : 55|4@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_6 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_3 : 32|2@1+ (1,0) [0|15] "" XXX
|
||||
SG_ SET_3 : 33|2@0+ (1,0) [0|15] "" XXX
|
||||
SG_ NEW_SIGNAL_1 : 7|12@0- (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_3 : 23|12@0- (-1,0) [0|255] "" XXX
|
||||
SG_ CRUISE_ON : 36|1@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CRUISE_ACTIVATED : 38|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ STEER_STATUS : 51|1@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
SG_ CRUISE_ON : 36|1@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_STATUS : 51|1@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 397 _GEAR: 8 XXX
|
||||
SG_ GEAR : 27|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
BO_ 1273 _SEATBELT: 7 XXX
|
||||
SG_ DRIVERS_SEATBELT : 25|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 665 _ESP: 8 XXX
|
||||
SG_ ESP_DISABLED : 24|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1055 GEARBOX: 2 XXX
|
||||
SG_ SPORTS_MODE : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ GEAR_SHIFTER : 5|3@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 1228 PROPILOT_HUD_INFO_MSG: 8 XXX
|
||||
SG_ NA_HIGH_ACCEL_TEMP : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_RADAR_NA_HIGH_CABIN_TEMP : 8|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_RADAR_MALFUNCTION : 11|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LKAS_MALFUNCTION : 12|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRONT_RADAR_MALFUNCTION : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_RADAR_NA_CLEAN_REAR_CAMERA : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NA_POOR_ROAD_CONDITIONS : 16|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CURRENTLY_UNAVAILABLE : 17|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SAFETY_SHIELD_OFF : 18|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION : 20|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED : 24|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_IMPACT_NA_RADAR_OBSTRUCTION : 25|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ WARNING_DO_NOT_ENTER : 33|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_IMPACT_SYSTEM_OFF : 34|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_IMPACT_MALFUNCTION : 35|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRONT_COLLISION_MALFUNCTION : 36|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_RADAR_MALFUNCTION2 : 37|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LKAS_MALFUNCTION2 : 38|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRONT_RADAR_MALFUNCTION2 : 39|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ PROPILOT_NA_MSGS : 42|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ BOTTOM_MSG : 45|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ HANDS_ON_WHEEL_WARNING : 47|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ WARNING_STEP_ON_BRAKE_NOW : 51|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED : 52|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ PROPILOT_NA_HIGH_CABIN_TEMP : 53|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ WARNING_PROPILOT_MALFUNCTION : 54|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ ACC_UNAVAILABLE_HIGH_CABIN_TEMP : 62|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ACC_NA_FRONT_CAMERA_IMPARED : 63|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unknown07 : 7|7@0+ (1,0) [0|127] "" XXX
|
||||
SG_ unknown10 : 10|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ unknown15 : 15|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unknown23 : 23|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ unknown19 : 19|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unknown31 : 31|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ unknown32 : 32|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unknown46 : 46|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unknown61 : 61|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ unknown55 : 55|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unknown50 : 50|3@0+ (1,0) [0|7] "" XXX
|
||||
|
||||
BO_ 689 PROPILOT_HUD: 8 XXX
|
||||
SG_ LARGE_WARNING_FLASHING : 9|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_RADAR_ERROR_FLASHING1 : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_RADAR_ERROR_FLASHING2 : 11|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RIGHT_LANE_YELLOW_FLASH : 12|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LEFT_LANE_YELLOW_FLASH : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LEAD_CAR : 14|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LEAD_CAR_ERROR : 15|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRONT_RADAR_ERROR : 16|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FRONT_RADAR_ERROR_FLASHING : 17|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RIGHT_LANE_GREEN : 24|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LEFT_LANE_GREEN : 25|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIDE_RADAR_ERROR_FLASHING3 : 27|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LKAS_ERROR_FLASHING : 29|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SAFETY_SHIELD_ACTIVE : 44|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LARGE_STEERING_WHEEL_ICON : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ RIGHT_LANE_GREEN_FLASH : 62|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LEFT_LANE_GREEN_FLASH : 63|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ FOLLOW_DISTANCE : 3|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ AUDIBLE_TONE : 47|3@0+ (1,0) [0|8] "" XXX
|
||||
SG_ SPEED_SET_ICON : 7|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ SMALL_STEERING_WHEEL_ICON : 42|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ unknown59 : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ unknown55 : 55|8@0+ (1,0) [0|63] "" XXX
|
||||
SG_ unknown26 : 26|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unknown28 : 28|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unknown31 : 31|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ unknown39 : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ unknown43 : 43|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ unknown8 : 8|7@0+ (1,0) [0|63] "" XXX
|
||||
SG_ unknown05 : 5|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ unknown02 : 1|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
VAL_ 1055 GEAR_SHIFTER 6 "L" 4 "D" 3 "N" 2 "R" 1 "P" ;
|
||||
VAL_ 1228 PROPILOT_NA_MSGS 0 "NO_MSG" 1 "NA_FRONT_CAMERA_IMPARED" 2 "STEERING_ASSIST_ON_STANDBY" 3 "NA_PARKING_ASSIST_ENABLED" 4 "STEER_ASSIST_CURRENTLY_NA" 5 "NA_BAD_WEATHER" 6 "NA_PARK_BRAKE_ON" 7 "NA_SEATBELT_NOT_FASTENED" ;
|
||||
VAL_ 1228 BOTTOM_MSG 0 "OK_STEER_ASSIST_SETTINGS" 1 "NO_MSG" 2 "PRESS_SET_TO_SET_SPEED" 3 "PRESS_RES_SET_TO_CHANGE_SPEED" 4 "PRESS_RES_TO_RESTART" 5 "NO_MSG" 6 "CRUISE_NOT_AVAIL" 7 "NO_MSG" ;
|
||||
VAL_ 689 FOLLOW_DISTANCE 0 "NO_FOLLOW_DISTANCE" 1 "FOLLOW_DISTANCE_1" 2 "FOLLOW_DISTANCE_2" 3 "FOLLOW_DISANCE_3" ;
|
||||
VAL_ 689 AUDIBLE_TONE 0 "NO_TONE" 1 "CONT" 2 "FAST_BEEP_CONT" 3 "TRIPLE_FAST_BEEP_CONT" 4 "SLOW_BEEP_CONT" 5 "QUAD_SLOW_BEEP_CONT" 6 "SINGLE_BEEP_ONCE" 7 "DOUBLE_BEEP_ONCE" ;
|
||||
VAL_ 689 SMALL_STEERING_WHEEL_ICON 0 "NO_ICON" 1 "GRAY_ICON" 2 "GRAY_ICON_FLASHING" 3 "GREEN_ICON" 4 "GREEN_ICON_FLASHING" 5 "RED_ICON" 6 "RED_ICON_FLASHING" 7 "YELLOW_ICON" ;
|
||||
VAL_ 689 LARGE_STEERING_WHEEL_ICON 0 "NO_STEERINGWHEEL" 1 "GRAY_STEERINGWHEEL" 2 "GREEN_STEERINGWHEEL" 3 "GREEN_STEERINGWHEEL_FLASHING" ;
|
||||
|
||||
@@ -120,6 +120,8 @@ BO_ 312 Brake_Pressure_L_R: 8 XXX
|
||||
SG_ Brake_1 : 48|8@1+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 313 Brake_Pedal: 8 XXX
|
||||
SG_ Checksum : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ Counter : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Brake_Pedal_On : 34|1@1+ (1,0) [0|7] "" XXX
|
||||
SG_ Brake_Pedal : 36|12@1+ (1,0) [0|65535] "" XXX
|
||||
|
||||
|
||||
@@ -374,6 +374,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -373,6 +373,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -374,6 +374,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -373,6 +373,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -1,6 +1,23 @@
|
||||
CM_ "AUTOGENERATED FILE, DO NOT EDIT"
|
||||
|
||||
|
||||
CM_ "Imported file _toyota_nodsu_bsm.dbc starts here"
|
||||
BO_ 1014 BSM: 8 XXX
|
||||
SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ R_ADJACENT : 1|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ R_APPROACHING : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ADJACENT_ENABLED : 7|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ APPROACHING_ENABLED : 15|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
CM_ SG_ 1014 L_ADJACENT "vehicle adjacent left side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 L_APPROACHING "vehicle approaching from left side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 R_ADJACENT "vehicle adjacent right side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 R_APPROACHING "vehicle approaching from right side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 ADJACENT_ENABLED "when BSM is enabled in settings, this is on along with APPROACHING_ENABLED. this controls bsm alert visibility";
|
||||
CM_ SG_ 1014 APPROACHING_ENABLED "when BSM is enabled in settings, this is on along with ADJACENT_ENABLED. this controls bsm alert visibility";
|
||||
|
||||
|
||||
CM_ "Imported file _comma.dbc starts here"
|
||||
BO_ 359 STEERING_IPAS_COMMA: 8 IPAS
|
||||
SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
@@ -360,6 +377,7 @@ CM_ "toyota_nodsu_hybrid_pt.dbc starts here"
|
||||
|
||||
|
||||
|
||||
|
||||
BO_ 295 GEAR_PACKET: 8 XXX
|
||||
SG_ CAR_MOVEMENT : 39|8@0- (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 55|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
@@ -1,6 +1,23 @@
|
||||
CM_ "AUTOGENERATED FILE, DO NOT EDIT"
|
||||
|
||||
|
||||
CM_ "Imported file _toyota_nodsu_bsm.dbc starts here"
|
||||
BO_ 1014 BSM: 8 XXX
|
||||
SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ R_ADJACENT : 1|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ R_APPROACHING : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ADJACENT_ENABLED : 7|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ APPROACHING_ENABLED : 15|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
CM_ SG_ 1014 L_ADJACENT "vehicle adjacent left side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 L_APPROACHING "vehicle approaching from left side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 R_ADJACENT "vehicle adjacent right side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 R_APPROACHING "vehicle approaching from right side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED";
|
||||
CM_ SG_ 1014 ADJACENT_ENABLED "when BSM is enabled in settings, this is on along with APPROACHING_ENABLED. this controls bsm alert visibility";
|
||||
CM_ SG_ 1014 APPROACHING_ENABLED "when BSM is enabled in settings, this is on along with ADJACENT_ENABLED. this controls bsm alert visibility";
|
||||
|
||||
|
||||
CM_ "Imported file _comma.dbc starts here"
|
||||
BO_ 359 STEERING_IPAS_COMMA: 8 IPAS
|
||||
SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
@@ -360,6 +377,7 @@ CM_ "toyota_nodsu_pt.dbc starts here"
|
||||
|
||||
|
||||
|
||||
|
||||
BO_ 401 STEERING_LTA: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
@@ -373,6 +373,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -373,6 +373,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -374,6 +374,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -825,6 +825,8 @@ BO_ 1648 Motor_18: 8 Motor_Diesel_MQB
|
||||
SG_ MO_obere_Drehzahlgrenze : 56|8@1+ (50,0) [50|12750] "Unit_MinutInver" Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB
|
||||
|
||||
BO_ 289 Motor_20: 8 Motor_Diesel_MQB
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|255] "" XXX
|
||||
SG_ MO_Fahrpedalrohwert_01 : 12|8@1+ (0.4,0) [0|101.6] "Unit_PerCent" Airbag_MQB,Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB
|
||||
SG_ MO_QBit_Fahrpedalwerte_01 : 20|1@1+ (1,0) [0|1] "" Airbag_MQB,Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB
|
||||
SG_ MO_Fahrpedalgradient : 21|8@1+ (25,0) [0|6350] "Unit_PerCentPerSecon" Airbag_MQB,Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB
|
||||
|
||||
@@ -36,6 +36,10 @@
|
||||
__typeof__ (b) _b = (b); \
|
||||
(_a > _b) ? _a : _b; })
|
||||
|
||||
#define ABS(a) \
|
||||
({ __typeof__ (a) _a = (a); \
|
||||
(_a > 0) ? _a : (-_a); })
|
||||
|
||||
#define MAX_RESP_LEN 0x40U
|
||||
|
||||
// Around (1Mbps / 8 bits/byte / 12 bytes per message)
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
uint8_t crc_checksum(uint8_t *dat, int len, const uint8_t poly) {
|
||||
uint8_t crc = 0xFF;
|
||||
int i, j;
|
||||
for (i = len - 1; i >= 0; i--) {
|
||||
crc ^= dat[i];
|
||||
for (j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80U) != 0U) {
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
}
|
||||
else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
@@ -52,7 +52,7 @@ bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool s
|
||||
void llcan_init(CAN_TypeDef *CAN_obj) {
|
||||
// Enter init mode
|
||||
register_set_bits(&(CAN_obj->FMR), CAN_FMR_FINIT);
|
||||
|
||||
|
||||
// Wait for INAK bit to be set
|
||||
while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {}
|
||||
|
||||
|
||||
@@ -19,6 +19,7 @@
|
||||
#include "drivers/timer.h"
|
||||
|
||||
#include "gpio.h"
|
||||
#include "crc.h"
|
||||
|
||||
#define CAN CAN1
|
||||
|
||||
@@ -105,26 +106,6 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
|
||||
|
||||
#endif
|
||||
|
||||
// ***************************** pedal can checksum *****************************
|
||||
|
||||
uint8_t pedal_checksum(uint8_t *dat, int len) {
|
||||
uint8_t crc = 0xFF;
|
||||
uint8_t poly = 0xD5; // standard crc8
|
||||
int i, j;
|
||||
for (i = len - 1; i >= 0; i--) {
|
||||
crc ^= dat[i];
|
||||
for (j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80U) != 0U) {
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
}
|
||||
else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
// ***************************** can port *****************************
|
||||
|
||||
// addresses to be used on CAN
|
||||
@@ -155,6 +136,8 @@ uint32_t current_index = 0;
|
||||
#define FAULT_INVALID 6U
|
||||
uint8_t state = FAULT_STARTUP;
|
||||
|
||||
const uint8_t crc_poly = 0xD5; // standard crc8
|
||||
|
||||
void CAN1_RX0_IRQ_Handler(void) {
|
||||
while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) {
|
||||
#ifdef DEBUG
|
||||
@@ -184,7 +167,7 @@ void CAN1_RX0_IRQ_Handler(void) {
|
||||
uint16_t value_1 = (dat[2] << 8) | dat[3];
|
||||
bool enable = ((dat[4] >> 7) & 1U) != 0U;
|
||||
uint8_t index = dat[4] & COUNTER_CYCLE;
|
||||
if (pedal_checksum(dat, CAN_GAS_SIZE - 1) == dat[5]) {
|
||||
if (crc_checksum(dat, CAN_GAS_SIZE - 1, crc_poly) == dat[5]) {
|
||||
if (((current_index + 1U) & COUNTER_CYCLE) == index) {
|
||||
#ifdef DEBUG
|
||||
puts("setting gas ");
|
||||
@@ -247,7 +230,7 @@ void TIM3_IRQ_Handler(void) {
|
||||
dat[2] = (pdl1 >> 8) & 0xFFU;
|
||||
dat[3] = (pdl1 >> 0) & 0xFFU;
|
||||
dat[4] = ((state & 0xFU) << 4) | pkt_idx;
|
||||
dat[5] = pedal_checksum(dat, CAN_GAS_SIZE - 1);
|
||||
dat[5] = crc_checksum(dat, CAN_GAS_SIZE - 1, crc_poly);
|
||||
CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
|
||||
CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5] << 8);
|
||||
CAN->sTxMailBox[0].TDTR = 6; // len of packet is 5
|
||||
|
||||
+22
-5
@@ -4,7 +4,6 @@
|
||||
#include "safety/safety_defaults.h"
|
||||
#include "safety/safety_honda.h"
|
||||
#include "safety/safety_toyota.h"
|
||||
#include "safety/safety_toyota_ipas.h"
|
||||
#include "safety/safety_tesla.h"
|
||||
#include "safety/safety_gm_ascm.h"
|
||||
#include "safety/safety_gm.h"
|
||||
@@ -14,6 +13,7 @@
|
||||
#include "safety/safety_chrysler.h"
|
||||
#include "safety/safety_subaru.h"
|
||||
#include "safety/safety_mazda.h"
|
||||
#include "safety/safety_nissan.h"
|
||||
#include "safety/safety_volkswagen.h"
|
||||
#include "safety/safety_elm327.h"
|
||||
|
||||
@@ -31,12 +31,13 @@
|
||||
#define SAFETY_TESLA 10U
|
||||
#define SAFETY_SUBARU 11U
|
||||
#define SAFETY_MAZDA 13U
|
||||
#define SAFETY_VOLKSWAGEN 15U
|
||||
#define SAFETY_TOYOTA_IPAS 16U
|
||||
#define SAFETY_NISSAN 14U
|
||||
#define SAFETY_VOLKSWAGEN_MQB 15U
|
||||
#define SAFETY_ALLOUTPUT 17U
|
||||
#define SAFETY_GM_ASCM 18U
|
||||
#define SAFETY_NOOUTPUT 19U
|
||||
#define SAFETY_HONDA_BOSCH_HARNESS 20U
|
||||
#define SAFETY_SUBARU_LEGACY 22U
|
||||
|
||||
uint16_t current_safety_mode = SAFETY_SILENT;
|
||||
const safety_hooks *current_hooks = &nooutput_hooks;
|
||||
@@ -57,6 +58,21 @@ int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return current_hooks->fwd(bus_num, to_fwd);
|
||||
}
|
||||
|
||||
// Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8
|
||||
// algorithm. Called at init time for safety modes using CRC-8.
|
||||
void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) {
|
||||
for (int i = 0; i < 256; i++) {
|
||||
uint8_t crc = i;
|
||||
for (int j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80U) != 0U)
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
else
|
||||
crc <<= 1;
|
||||
}
|
||||
crc_lut[i] = crc;
|
||||
}
|
||||
}
|
||||
|
||||
bool msg_allowed(int addr, int bus, const AddrBus addr_list[], int len) {
|
||||
bool allowed = false;
|
||||
for (int i = 0; i < len; i++) {
|
||||
@@ -184,13 +200,14 @@ const safety_hook_config safety_hook_registry[] = {
|
||||
{SAFETY_HYUNDAI, &hyundai_hooks},
|
||||
{SAFETY_CHRYSLER, &chrysler_hooks},
|
||||
{SAFETY_SUBARU, &subaru_hooks},
|
||||
{SAFETY_SUBARU_LEGACY, &subaru_legacy_hooks},
|
||||
{SAFETY_MAZDA, &mazda_hooks},
|
||||
{SAFETY_VOLKSWAGEN, &volkswagen_hooks},
|
||||
{SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks},
|
||||
{SAFETY_NOOUTPUT, &nooutput_hooks},
|
||||
#ifdef ALLOW_DEBUG
|
||||
{SAFETY_CADILLAC, &cadillac_hooks},
|
||||
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
|
||||
{SAFETY_TESLA, &tesla_hooks},
|
||||
{SAFETY_NISSAN, &nissan_hooks},
|
||||
{SAFETY_ALLOUTPUT, &alloutput_hooks},
|
||||
{SAFETY_GM_ASCM, &gm_ascm_hooks},
|
||||
{SAFETY_FORD, &ford_hooks},
|
||||
|
||||
@@ -4,28 +4,77 @@ const uint32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int CHRYSLER_MAX_RATE_UP = 3;
|
||||
const int CHRYSLER_MAX_RATE_DOWN = 3;
|
||||
const int CHRYSLER_MAX_TORQUE_ERROR = 80; // max torque cmd in excess of torque motor
|
||||
const int CHRYSLER_GAS_THRSLD = 30; // 7% more than 2m/s
|
||||
const int CHRYSLER_STANDSTILL_THRSLD = 10; // about 1m/s
|
||||
const AddrBus CHRYSLER_TX_MSGS[] = {{571, 0}, {658, 0}, {678, 0}};
|
||||
|
||||
// TODO: do checksum and counter checks
|
||||
AddrCheckStruct chrysler_rx_checks[] = {
|
||||
{.addr = {544}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {500}, .bus = 0, .expected_timestep = 20000U},
|
||||
{.addr = {544}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U},
|
||||
{.addr = {514}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U},
|
||||
{.addr = {500}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {308}, .bus = 0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {320}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
};
|
||||
const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]);
|
||||
|
||||
int chrysler_rt_torque_last = 0;
|
||||
int chrysler_desired_torque_last = 0;
|
||||
int chrysler_cruise_engaged_last = 0;
|
||||
int chrysler_speed = 0;
|
||||
uint32_t chrysler_ts_last = 0;
|
||||
struct sample_t chrysler_torque_meas; // last few torques measured
|
||||
|
||||
static uint8_t chrysler_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int checksum_byte = GET_LEN(to_push) - 1;
|
||||
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
|
||||
}
|
||||
|
||||
static uint8_t chrysler_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
/* This function does not want the checksum byte in the input data.
|
||||
jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf */
|
||||
uint8_t checksum = 0xFF;
|
||||
int len = GET_LEN(to_push);
|
||||
for (int j = 0; j < (len - 1); j++) {
|
||||
uint8_t shift = 0x80;
|
||||
uint8_t curr = (uint8_t)GET_BYTE(to_push, j);
|
||||
for (int i=0; i<8; i++) {
|
||||
uint8_t bit_sum = curr & shift;
|
||||
uint8_t temp_chk = checksum & 0x80U;
|
||||
if (bit_sum != 0U) {
|
||||
bit_sum = 0x1C;
|
||||
if (temp_chk != 0U) {
|
||||
bit_sum = 1;
|
||||
}
|
||||
checksum = checksum << 1;
|
||||
temp_chk = checksum | 1U;
|
||||
bit_sum ^= temp_chk;
|
||||
} else {
|
||||
if (temp_chk != 0U) {
|
||||
bit_sum = 0x1D;
|
||||
}
|
||||
checksum = checksum << 1;
|
||||
bit_sum ^= checksum;
|
||||
}
|
||||
checksum = bit_sum;
|
||||
shift = shift >> 1;
|
||||
}
|
||||
}
|
||||
return ~checksum;
|
||||
}
|
||||
|
||||
static uint8_t chrysler_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
// Well defined counter only for 8 bytes messages
|
||||
return (uint8_t)(GET_BYTE(to_push, 6) >> 4);
|
||||
}
|
||||
|
||||
static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
bool valid = addr_safety_check(to_push, chrysler_rx_checks, CHRYSLER_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
chrysler_get_checksum, chrysler_compute_checksum,
|
||||
chrysler_get_counter);
|
||||
|
||||
if (valid) {
|
||||
int bus = GET_BUS(to_push);
|
||||
if (valid && (GET_BUS(to_push) == 0)) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
// Measured eps torque
|
||||
@@ -37,7 +86,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
if (addr == 0x1F4) {
|
||||
if (addr == 500) {
|
||||
int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7;
|
||||
if (cruise_engaged && !chrysler_cruise_engaged_last) {
|
||||
controls_allowed = 1;
|
||||
@@ -48,10 +97,33 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
chrysler_cruise_engaged_last = cruise_engaged;
|
||||
}
|
||||
|
||||
// TODO: add gas pressed check
|
||||
// update speed
|
||||
if (addr == 514) {
|
||||
int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4);
|
||||
int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4);
|
||||
chrysler_speed = (speed_l + speed_r) / 2;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if (addr == 308) {
|
||||
bool gas_pressed = (GET_BYTE(to_push, 5) & 0x7F) != 0;
|
||||
if (gas_pressed && !gas_pressed_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
gas_pressed_prev = gas_pressed;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake press
|
||||
if (addr == 320) {
|
||||
bool brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5;
|
||||
if (brake_pressed && (!brake_pressed_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
}
|
||||
|
||||
// check if stock camera ECU is on bus 0
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) {
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x292)) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7,8 +7,6 @@
|
||||
// brake rising edge
|
||||
// brake > 0mph
|
||||
|
||||
int ford_brake_prev = 0;
|
||||
int ford_gas_prev = 0;
|
||||
bool ford_moving = false;
|
||||
|
||||
static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
@@ -39,20 +37,20 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
// exit controls on rising edge of brake press or on brake press when
|
||||
// speed > 0
|
||||
if (addr == 0x165) {
|
||||
int brake = GET_BYTE(to_push, 0) & 0x20;
|
||||
if (brake && (!(ford_brake_prev) || ford_moving)) {
|
||||
int brake_pressed = GET_BYTE(to_push, 0) & 0x20;
|
||||
if (brake_pressed && (!brake_pressed_prev || ford_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
ford_brake_prev = brake;
|
||||
brake_pressed_prev = brake_pressed;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if (addr == 0x204) {
|
||||
int gas = (GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1);
|
||||
if (gas && !(ford_gas_prev)) {
|
||||
bool gas_pressed = ((GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1)) != 0;
|
||||
if (gas_pressed && !gas_pressed_prev) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
ford_gas_prev = gas;
|
||||
gas_pressed_prev = gas_pressed;
|
||||
}
|
||||
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) {
|
||||
@@ -74,7 +72,7 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
|
||||
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
|
||||
// and the the latching controls_allowed flag is True
|
||||
int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_moving);
|
||||
int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && ford_moving);
|
||||
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
|
||||
|
||||
if (relay_malfunction) {
|
||||
|
||||
@@ -33,8 +33,6 @@ AddrCheckStruct gm_rx_checks[] = {
|
||||
};
|
||||
const int GM_RX_CHECK_LEN = sizeof(gm_rx_checks) / sizeof(gm_rx_checks[0]);
|
||||
|
||||
int gm_brake_prev = 0;
|
||||
int gm_gas_prev = 0;
|
||||
bool gm_moving = false;
|
||||
int gm_rt_torque_last = 0;
|
||||
int gm_desired_torque_last = 0;
|
||||
@@ -46,8 +44,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
bool valid = addr_safety_check(to_push, gm_rx_checks, GM_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
|
||||
if (valid) {
|
||||
int bus = GET_BUS(to_push);
|
||||
if (valid && (GET_BUS(to_push) == 0)) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
if (addr == 388) {
|
||||
@@ -82,25 +79,22 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
// exit controls on rising edge of brake press or on brake press when
|
||||
// speed > 0
|
||||
if (addr == 241) {
|
||||
int brake = GET_BYTE(to_push, 1);
|
||||
// Brake pedal's potentiometer returns near-zero reading
|
||||
// even when pedal is not pressed
|
||||
if (brake < 10) {
|
||||
brake = 0;
|
||||
}
|
||||
if (brake && (!gm_brake_prev || gm_moving)) {
|
||||
bool brake_pressed = GET_BYTE(to_push, 1) >= 10;
|
||||
if (brake_pressed && (!brake_pressed_prev || gm_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
gm_brake_prev = brake;
|
||||
brake_pressed_prev = brake_pressed;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if (addr == 417) {
|
||||
int gas = GET_BYTE(to_push, 6);
|
||||
if (gas && !gm_gas_prev) {
|
||||
bool gas_pressed = GET_BYTE(to_push, 6) != 0;
|
||||
if (gas_pressed && !gas_pressed_prev) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
gm_gas_prev = gas;
|
||||
gas_pressed_prev = gas_pressed;
|
||||
}
|
||||
|
||||
// exit controls on regen paddle
|
||||
@@ -115,7 +109,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
// on powertrain bus.
|
||||
// 384 = ASCMLKASteeringCmd
|
||||
// 715 = ASCMGasRegenCmd
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 384) || (addr == 715))) {
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 384) || (addr == 715))) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
}
|
||||
@@ -144,7 +138,7 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
|
||||
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
|
||||
// and the the latching controls_allowed flag is True
|
||||
int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_moving);
|
||||
int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && gm_moving);
|
||||
bool current_controls_allowed = controls_allowed && !pedal_pressed;
|
||||
|
||||
// BRAKE: safety check
|
||||
|
||||
@@ -28,8 +28,6 @@ AddrCheckStruct honda_bh_rx_checks[] = {
|
||||
const int HONDA_BH_RX_CHECKS_LEN = sizeof(honda_bh_rx_checks) / sizeof(honda_bh_rx_checks[0]);
|
||||
|
||||
int honda_brake = 0;
|
||||
int honda_gas_prev = 0;
|
||||
bool honda_brake_pressed_prev = false;
|
||||
bool honda_moving = false;
|
||||
bool honda_alt_brake_msg = false;
|
||||
bool honda_fwd_brake = false;
|
||||
@@ -48,7 +46,7 @@ static uint8_t honda_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
while (addr > 0U) {
|
||||
checksum += (addr & 0xFU); addr >>= 4;
|
||||
}
|
||||
for (int j = 0; (j < len); j++) {
|
||||
for (int j = 0; j < len; j++) {
|
||||
uint8_t byte = GET_BYTE(to_push, j);
|
||||
checksum += (byte & 0xFU) + (byte >> 4U);
|
||||
if (j == (len - 1)) {
|
||||
@@ -112,10 +110,10 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C);
|
||||
if (is_user_brake_msg) {
|
||||
bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
|
||||
if (brake_pressed && (!(honda_brake_pressed_prev) || honda_moving)) {
|
||||
if (brake_pressed && (!brake_pressed_prev || honda_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
honda_brake_pressed_prev = brake_pressed;
|
||||
brake_pressed_prev = brake_pressed;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
|
||||
@@ -133,11 +131,11 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
// exit controls on rising edge of gas press if no interceptor
|
||||
if (!gas_interceptor_detected) {
|
||||
if (addr == 0x17C) {
|
||||
int gas = GET_BYTE(to_push, 0);
|
||||
if (gas && !honda_gas_prev) {
|
||||
bool gas_pressed = GET_BYTE(to_push, 0) != 0;
|
||||
if (gas_pressed && !gas_pressed_prev) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
honda_gas_prev = gas;
|
||||
gas_pressed_prev = gas_pressed;
|
||||
}
|
||||
}
|
||||
if ((bus == 2) && (addr == 0x1FA)) {
|
||||
@@ -194,9 +192,9 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
|
||||
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
|
||||
// and the the latching controls_allowed flag is True
|
||||
//int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
|
||||
// (honda_brake_pressed_prev && honda_moving);
|
||||
int pedal_pressed = honda_brake_pressed_prev && honda_moving;
|
||||
//int pedal_pressed = gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
|
||||
// (brake_pressed_prev && honda_moving);
|
||||
int pedal_pressed = brake_pressed_prev && honda_moving;
|
||||
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
|
||||
|
||||
// BRAKE: safety check
|
||||
|
||||
@@ -1,15 +1,19 @@
|
||||
const int HYUNDAI_MAX_STEER = 255; // like stock
|
||||
const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
|
||||
const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int HYUNDAI_MAX_RATE_UP = 3;
|
||||
const int HYUNDAI_MAX_RATE_DOWN = 7;
|
||||
const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
|
||||
const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
|
||||
const int HYUNDAI_STANDSTILL_THRSLD = 30; // ~1kph
|
||||
const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}};
|
||||
|
||||
// TODO: do checksum and counter checks
|
||||
AddrCheckStruct hyundai_rx_checks[] = {
|
||||
{.addr = {608}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {897}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {902}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {916}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {1057}, .bus = 0, .expected_timestep = 20000U},
|
||||
};
|
||||
const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]);
|
||||
@@ -17,6 +21,7 @@ const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_c
|
||||
int hyundai_rt_torque_last = 0;
|
||||
int hyundai_desired_torque_last = 0;
|
||||
int hyundai_cruise_engaged_last = 0;
|
||||
int hyundai_speed = 0;
|
||||
uint32_t hyundai_ts_last = 0;
|
||||
struct sample_t hyundai_torque_driver; // last few driver torques measured
|
||||
|
||||
@@ -25,8 +30,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
bool valid = addr_safety_check(to_push, hyundai_rx_checks, HYUNDAI_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
|
||||
if (valid) {
|
||||
int bus = GET_BUS(to_push);
|
||||
if (valid && GET_BUS(to_push) == 0) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
if (addr == 897) {
|
||||
@@ -48,10 +52,33 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
hyundai_cruise_engaged_last = cruise_engaged;
|
||||
}
|
||||
|
||||
// TODO: check gas pressed
|
||||
// exit controls on rising edge of gas press
|
||||
if (addr == 608) {
|
||||
bool gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0;
|
||||
if (gas_pressed && !gas_pressed_prev) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
gas_pressed_prev = gas_pressed;
|
||||
}
|
||||
|
||||
// sample subaru wheel speed, averaging opposite corners
|
||||
if (addr == 902) {
|
||||
hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL
|
||||
hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF; // RL
|
||||
hyundai_speed /= 2;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake press
|
||||
if (addr == 916) {
|
||||
bool brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0;
|
||||
if (brake_pressed && (!brake_pressed_prev || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
}
|
||||
|
||||
// check if stock camera ECU is on bus 0
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 832)) {
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,213 @@
|
||||
|
||||
const uint32_t NISSAN_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
|
||||
const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_UP = {
|
||||
{2., 7., 17.},
|
||||
{5., .8, .15}};
|
||||
|
||||
const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = {
|
||||
{2., 7., 17.},
|
||||
{5., 3.5, .5}};
|
||||
|
||||
const struct lookup_t NISSAN_LOOKUP_MAX_ANGLE = {
|
||||
{3.3, 12, 32},
|
||||
{540., 120., 23.}};
|
||||
|
||||
const int NISSAN_DEG_TO_CAN = 100;
|
||||
|
||||
const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x2b1, 0}, {0x4cc, 0}, {0x20b, 2}};
|
||||
|
||||
AddrCheckStruct nissan_rx_checks[] = {
|
||||
{.addr = {0x2}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {0x29a}, .bus = 0, .expected_timestep = 20000U},
|
||||
{.addr = {0x1b6}, .bus = 1, .expected_timestep = 10000U},
|
||||
};
|
||||
const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]);
|
||||
|
||||
float nissan_speed = 0;
|
||||
//int nissan_controls_allowed_last = 0;
|
||||
uint32_t nissan_ts_angle_last = 0;
|
||||
int nissan_cruise_engaged_last = 0;
|
||||
int nissan_desired_angle_last = 0;
|
||||
|
||||
struct sample_t nissan_angle_meas; // last 3 steer angles
|
||||
|
||||
|
||||
static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
bool valid = addr_safety_check(to_push, nissan_rx_checks, NISSAN_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
|
||||
if (valid) {
|
||||
int bus = GET_BUS(to_push);
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
if (bus == 0) {
|
||||
if (addr == 0x2) {
|
||||
// Current steering angle
|
||||
// Factor -0.1, little endian
|
||||
int angle_meas_new = (GET_BYTES_04(to_push) & 0xFFFF);
|
||||
// Need to multiply by 10 here as LKAS and Steering wheel are different base unit
|
||||
angle_meas_new = to_signed(angle_meas_new, 16) * 10;
|
||||
|
||||
// update array of samples
|
||||
update_sample(&nissan_angle_meas, angle_meas_new);
|
||||
}
|
||||
|
||||
if (addr == 0x29a) {
|
||||
// Get current speed
|
||||
// Factor 0.00555
|
||||
nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.00555 / 3.6;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if (addr == 0x15c) {
|
||||
bool gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
|
||||
if (gas_pressed && !gas_pressed_prev) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
gas_pressed_prev = gas_pressed;
|
||||
}
|
||||
|
||||
// 0x169 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x169)) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (bus == 1) {
|
||||
if (addr == 0x1b6) {
|
||||
int cruise_engaged = (GET_BYTE(to_push, 4) >> 6) & 1;
|
||||
if (cruise_engaged && !nissan_cruise_engaged_last) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
if (!cruise_engaged) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
nissan_cruise_engaged_last = cruise_engaged;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake press, or if speed > 0 and brake
|
||||
if (addr == 0x454) {
|
||||
bool brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0;
|
||||
if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
}
|
||||
}
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
|
||||
static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
int tx = 1;
|
||||
int addr = GET_ADDR(to_send);
|
||||
int bus = GET_BUS(to_send);
|
||||
bool violation = 0;
|
||||
|
||||
if (!msg_allowed(addr, bus, NISSAN_TX_MSGS, sizeof(NISSAN_TX_MSGS) / sizeof(NISSAN_TX_MSGS[0]))) {
|
||||
tx = 0;
|
||||
}
|
||||
|
||||
if (relay_malfunction) {
|
||||
tx = 0;
|
||||
}
|
||||
|
||||
// steer cmd checks
|
||||
if (addr == 0x169) {
|
||||
int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3));
|
||||
bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1;
|
||||
|
||||
// offeset 1310 * NISSAN_DEG_TO_CAN
|
||||
desired_angle = desired_angle - 131000;
|
||||
|
||||
if (controls_allowed && lka_active) {
|
||||
// add 1 to not false trigger the violation
|
||||
float delta_angle_float;
|
||||
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_UP, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
|
||||
int delta_angle_up = (int)(delta_angle_float);
|
||||
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_DOWN, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
|
||||
int delta_angle_down = (int)(delta_angle_float);
|
||||
int highest_desired_angle = nissan_desired_angle_last + ((nissan_desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
|
||||
int lowest_desired_angle = nissan_desired_angle_last - ((nissan_desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);
|
||||
|
||||
// Limit maximum steering angle at current speed
|
||||
int maximum_angle = ((int)interpolate(NISSAN_LOOKUP_MAX_ANGLE, nissan_speed));
|
||||
|
||||
if (highest_desired_angle > (maximum_angle * NISSAN_DEG_TO_CAN)) {
|
||||
highest_desired_angle = (maximum_angle * NISSAN_DEG_TO_CAN);
|
||||
}
|
||||
if (lowest_desired_angle < (-maximum_angle * NISSAN_DEG_TO_CAN)) {
|
||||
lowest_desired_angle = (-maximum_angle * NISSAN_DEG_TO_CAN);
|
||||
}
|
||||
|
||||
// check for violation;
|
||||
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
|
||||
//nissan_controls_allowed_last = controls_allowed;
|
||||
}
|
||||
nissan_desired_angle_last = desired_angle;
|
||||
|
||||
// desired steer angle should be the same as steer angle measured when controls are off
|
||||
if ((!controls_allowed) &&
|
||||
((desired_angle < (nissan_angle_meas.min - 1)) ||
|
||||
(desired_angle > (nissan_angle_meas.max + 1)))) {
|
||||
violation = 1;
|
||||
}
|
||||
|
||||
// no lka_enabled bit if controls not allowed
|
||||
if (!controls_allowed && lka_active) {
|
||||
violation = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// acc button check, only allow cancel button to be sent
|
||||
if (addr == 0x20b) {
|
||||
// Violation of any button other than cancel is pressed
|
||||
violation |= ((GET_BYTE(to_send, 1) & 0x3d) > 0);
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
controls_allowed = 0;
|
||||
tx = 0;
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
|
||||
static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
int bus_fwd = -1;
|
||||
int addr = GET_ADDR(to_fwd);
|
||||
|
||||
if (bus_num == 0) {
|
||||
bus_fwd = 2; // ADAS
|
||||
}
|
||||
|
||||
if (bus_num == 2) {
|
||||
// 0x169 is LKAS, 0x2b1 LKAS_HUD, 0x4cc LKAS_HUD_INFO_MSG
|
||||
int block_msg = ((addr == 0x169) || (addr == 0x2b1) || (addr == 0x4cc));
|
||||
if (!block_msg) {
|
||||
bus_fwd = 0; // V-CAN
|
||||
}
|
||||
}
|
||||
|
||||
if (relay_malfunction) {
|
||||
bus_fwd = -1;
|
||||
}
|
||||
|
||||
// fallback to do not forward
|
||||
return bus_fwd;
|
||||
}
|
||||
|
||||
const safety_hooks nissan_hooks = {
|
||||
.init = nooutput_init,
|
||||
.rx = nissan_rx_hook,
|
||||
.tx = nissan_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
.fwd = nissan_fwd_hook,
|
||||
.addr_check = nissan_rx_checks,
|
||||
.addr_check_len = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]),
|
||||
};
|
||||
@@ -7,42 +7,84 @@ const int SUBARU_MAX_RATE_UP = 50;
|
||||
const int SUBARU_MAX_RATE_DOWN = 70;
|
||||
const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60;
|
||||
const int SUBARU_DRIVER_TORQUE_FACTOR = 10;
|
||||
const int SUBARU_STANDSTILL_THRSLD = 20; // about 1kph
|
||||
|
||||
const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x164, 0}, {0x221, 0}, {0x322, 0}};
|
||||
const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x221, 0}, {0x322, 0}};
|
||||
const AddrBus SUBARU_L_TX_MSGS[] = {{0x164, 0}, {0x221, 0}, {0x322, 0}};
|
||||
const int SUBARU_TX_MSGS_LEN = sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]);
|
||||
const int SUBARU_L_TX_MSGS_LEN = sizeof(SUBARU_L_TX_MSGS) / sizeof(SUBARU_L_TX_MSGS[0]);
|
||||
|
||||
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
|
||||
AddrCheckStruct subaru_rx_checks[] = {
|
||||
{.addr = {0x119, 0x371}, .bus = 0, .expected_timestep = 20000U},
|
||||
{.addr = {0x240, 0x144}, .bus = 0, .expected_timestep = 50000U},
|
||||
{.addr = { 0x40}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U},
|
||||
{.addr = {0x119}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {0x139}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {0x13a}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {0x240}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U},
|
||||
};
|
||||
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
|
||||
AddrCheckStruct subaru_l_rx_checks[] = {
|
||||
{.addr = {0x140}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {0x371}, .bus = 0, .expected_timestep = 20000U},
|
||||
{.addr = {0x144}, .bus = 0, .expected_timestep = 50000U},
|
||||
};
|
||||
const int SUBARU_RX_CHECK_LEN = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]);
|
||||
const int SUBARU_L_RX_CHECK_LEN = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_rx_checks[0]);
|
||||
|
||||
int subaru_cruise_engaged_last = 0;
|
||||
int subaru_rt_torque_last = 0;
|
||||
int subaru_desired_torque_last = 0;
|
||||
int subaru_speed = 0;
|
||||
uint32_t subaru_ts_last = 0;
|
||||
bool subaru_global = false;
|
||||
struct sample_t subaru_torque_driver; // last few driver torques measured
|
||||
|
||||
static uint8_t subaru_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
return (uint8_t)GET_BYTE(to_push, 0);
|
||||
}
|
||||
|
||||
static uint8_t subaru_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
return (uint8_t)(GET_BYTE(to_push, 1) & 0xF);
|
||||
}
|
||||
|
||||
static uint8_t subaru_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
int len = GET_LEN(to_push);
|
||||
uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U);
|
||||
for (int i = 1; i < len; i++) {
|
||||
checksum += (uint8_t)GET_BYTE(to_push, i);
|
||||
}
|
||||
return checksum;
|
||||
}
|
||||
|
||||
static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
bool valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
bool valid = false;
|
||||
if (subaru_global) {
|
||||
valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN,
|
||||
subaru_get_checksum, subaru_compute_checksum, subaru_get_counter);
|
||||
} else {
|
||||
valid = addr_safety_check(to_push, subaru_l_rx_checks, SUBARU_L_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
}
|
||||
|
||||
if (valid) {
|
||||
int bus = GET_BUS(to_push);
|
||||
if (valid && (GET_BUS(to_push) == 0)) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
if (((addr == 0x119) || (addr == 0x371)) && (bus == 0)){
|
||||
int bit_shift = (addr == 0x119) ? 16 : 29;
|
||||
int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF);
|
||||
if (((addr == 0x119) && subaru_global) ||
|
||||
((addr == 0x371) && !subaru_global)) {
|
||||
int torque_driver_new;
|
||||
if (subaru_global) {
|
||||
torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FF);
|
||||
} else {
|
||||
torque_driver_new = (GET_BYTE(to_push, 3) >> 5) + (GET_BYTE(to_push, 4) << 3);
|
||||
}
|
||||
torque_driver_new = to_signed(torque_driver_new, 11);
|
||||
// update array of samples
|
||||
update_sample(&subaru_torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
if (((addr == 0x240) || (addr == 0x144)) && (bus == 0)) {
|
||||
int bit_shift = (addr == 0x240) ? 9 : 17;
|
||||
if (((addr == 0x240) && subaru_global) ||
|
||||
((addr == 0x144) && !subaru_global)) {
|
||||
int bit_shift = subaru_global ? 9 : 17;
|
||||
int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1);
|
||||
if (cruise_engaged && !subaru_cruise_engaged_last) {
|
||||
controls_allowed = 1;
|
||||
@@ -53,9 +95,35 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
subaru_cruise_engaged_last = cruise_engaged;
|
||||
}
|
||||
|
||||
// TODO: enforce cancellation on gas pressed
|
||||
// sample subaru wheel speed, averaging opposite corners
|
||||
if ((addr == 0x13a) && subaru_global) {
|
||||
subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF; // FR
|
||||
subaru_speed += (GET_BYTES_48(to_push) >> 6) & 0x1FFF; // RL
|
||||
subaru_speed /= 2;
|
||||
}
|
||||
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) {
|
||||
// exit controls on rising edge of brake press (TODO: missing check for unsupported legacy models)
|
||||
if ((addr == 0x139) && subaru_global) {
|
||||
bool brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
|
||||
if (brake_pressed && (!brake_pressed_prev || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if (((addr == 0x40) && subaru_global) ||
|
||||
((addr == 0x140) && !subaru_global)) {
|
||||
int byte = subaru_global ? 4 : 0;
|
||||
bool gas_pressed = GET_BYTE(to_push, byte) != 0;
|
||||
if (gas_pressed && !gas_pressed_prev) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
gas_pressed_prev = gas_pressed;
|
||||
}
|
||||
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) &&
|
||||
(((addr == 0x122) && subaru_global) || ((addr == 0x164) && !subaru_global))) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
}
|
||||
@@ -67,7 +135,8 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
int addr = GET_ADDR(to_send);
|
||||
int bus = GET_BUS(to_send);
|
||||
|
||||
if (!msg_allowed(addr, bus, SUBARU_TX_MSGS, sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]))) {
|
||||
if ((!msg_allowed(addr, bus, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN) && subaru_global) ||
|
||||
(!msg_allowed(addr, bus, SUBARU_L_TX_MSGS, SUBARU_L_TX_MSGS_LEN) && !subaru_global)) {
|
||||
tx = 0;
|
||||
}
|
||||
|
||||
@@ -76,8 +145,9 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
}
|
||||
|
||||
// steer cmd checks
|
||||
if ((addr == 0x122) || (addr == 0x164)) {
|
||||
int bit_shift = (addr == 0x122) ? 16 : 8;
|
||||
if (((addr == 0x122) && subaru_global) ||
|
||||
((addr == 0x164) && !subaru_global)) {
|
||||
int bit_shift = subaru_global ? 16 : 8;
|
||||
int desired_torque = ((GET_BYTES_04(to_send) >> bit_shift) & 0x1FFF);
|
||||
bool violation = 0;
|
||||
uint32_t ts = TIM2->CNT;
|
||||
@@ -141,7 +211,9 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
// 545 is ES_Distance
|
||||
// 802 is ES_LKAS
|
||||
int addr = GET_ADDR(to_fwd);
|
||||
int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802);
|
||||
int block_msg = ((addr == 0x122) && subaru_global) ||
|
||||
((addr == 0x164) && !subaru_global) ||
|
||||
(addr == 0x221) || (addr == 0x322);
|
||||
if (!block_msg) {
|
||||
bus_fwd = 0; // Main CAN
|
||||
}
|
||||
@@ -151,8 +223,22 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return bus_fwd;
|
||||
}
|
||||
|
||||
static void subaru_init(int16_t param) {
|
||||
UNUSED(param);
|
||||
controls_allowed = false;
|
||||
relay_malfunction = false;
|
||||
subaru_global = true;
|
||||
}
|
||||
|
||||
static void subaru_legacy_init(int16_t param) {
|
||||
UNUSED(param);
|
||||
controls_allowed = false;
|
||||
relay_malfunction = false;
|
||||
subaru_global = false;
|
||||
}
|
||||
|
||||
const safety_hooks subaru_hooks = {
|
||||
.init = nooutput_init,
|
||||
.init = subaru_init,
|
||||
.rx = subaru_rx_hook,
|
||||
.tx = subaru_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
@@ -160,3 +246,13 @@ const safety_hooks subaru_hooks = {
|
||||
.addr_check = subaru_rx_checks,
|
||||
.addr_check_len = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]),
|
||||
};
|
||||
|
||||
const safety_hooks subaru_legacy_hooks = {
|
||||
.init = subaru_legacy_init,
|
||||
.rx = subaru_rx_hook,
|
||||
.tx = subaru_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
.fwd = subaru_fwd_hook,
|
||||
.addr_check = subaru_l_rx_checks,
|
||||
.addr_check_len = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_rx_checks[0]),
|
||||
};
|
||||
|
||||
@@ -34,8 +34,6 @@ float tesla_ts_angle_last = 0;
|
||||
|
||||
int tesla_controls_allowed_last = 0;
|
||||
|
||||
int tesla_brake_prev = 0;
|
||||
int tesla_gas_prev = 0;
|
||||
int tesla_speed = 0;
|
||||
int eac_status = 0;
|
||||
|
||||
|
||||
@@ -16,7 +16,8 @@ const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int TOYOTA_MAX_ACCEL = 4000; // 1.5 m/s2
|
||||
const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2
|
||||
|
||||
const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475; // ratio between offset and gain from dbc file
|
||||
const int TOYOTA_STANDSTILL_THRSLD = 100; // 1kph
|
||||
const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 475; // ratio between offset and gain from dbc file
|
||||
|
||||
const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0}, {0x344, 0}, {0x365, 0}, {0x366, 0}, {0x4CB, 0}, // DSU bus 0
|
||||
{0x128, 1}, {0x141, 1}, {0x160, 1}, {0x161, 1}, {0x470, 1}, // DSU bus 1
|
||||
@@ -24,8 +25,10 @@ const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0}
|
||||
{0x200, 0}, {0x750, 0}}; // interceptor + Blindspot monitor
|
||||
|
||||
AddrCheckStruct toyota_rx_checks[] = {
|
||||
{.addr = {0x260}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 20000U},
|
||||
{.addr = {0x1D2}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 30000U},
|
||||
{.addr = { 0xaa}, .bus = 0, .check_checksum = false, .expected_timestep = 12000U},
|
||||
{.addr = {0x260}, .bus = 0, .check_checksum = true, .expected_timestep = 20000U},
|
||||
{.addr = {0x1D2}, .bus = 0, .check_checksum = true, .expected_timestep = 30000U},
|
||||
{.addr = {0x224, 0x226}, .bus = 0, .check_checksum = false, .expected_timestep = 25000U},
|
||||
};
|
||||
const int TOYOTA_RX_CHECKS_LEN = sizeof(toyota_rx_checks) / sizeof(toyota_rx_checks[0]);
|
||||
|
||||
@@ -37,7 +40,7 @@ int toyota_desired_torque_last = 0; // last desired steer torque
|
||||
int toyota_rt_torque_last = 0; // last desired torque for real time check
|
||||
uint32_t toyota_ts_last = 0;
|
||||
int toyota_cruise_engaged_last = 0; // cruise state
|
||||
int toyota_gas_prev = 0;
|
||||
bool toyota_moving = false;
|
||||
struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps
|
||||
|
||||
|
||||
@@ -60,8 +63,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
bool valid = addr_safety_check(to_push, toyota_rx_checks, TOYOTA_RX_CHECKS_LEN,
|
||||
toyota_get_checksum, toyota_compute_checksum, NULL);
|
||||
if (valid) {
|
||||
int bus = GET_BUS(to_push);
|
||||
if (valid && (GET_BUS(to_push) == 0)) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
// get eps motor torque (0.66 factor in dbc)
|
||||
@@ -93,12 +95,34 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
toyota_cruise_engaged_last = cruise_engaged;
|
||||
}
|
||||
|
||||
// sample speed
|
||||
if (addr == 0xaa) {
|
||||
int speed = 0;
|
||||
// sum 4 wheel speeds
|
||||
for (int i=0; i<8; i+=2) {
|
||||
int next_byte = i + 1; // hack to deal with misra 10.8
|
||||
speed += (GET_BYTE(to_push, i) << 8) + GET_BYTE(to_push, next_byte) - 0x1a6f;
|
||||
}
|
||||
toyota_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake pedal
|
||||
// most cars have brake_pressed on 0x226, corolla and rav4 on 0x224
|
||||
if ((addr == 0x224) || (addr == 0x226)) {
|
||||
int byte = (addr == 0x224) ? 0 : 4;
|
||||
bool brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
|
||||
if (brake_pressed && (!brake_pressed_prev || toyota_moving)) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of interceptor gas press
|
||||
if (addr == 0x201) {
|
||||
gas_interceptor_detected = 1;
|
||||
int gas_interceptor = GET_INTERCEPTOR(to_push);
|
||||
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
|
||||
(gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD)) {
|
||||
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) &&
|
||||
(gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRSLD)) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
gas_interceptor_prev = gas_interceptor;
|
||||
@@ -106,15 +130,15 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if (addr == 0x2C1) {
|
||||
int gas = GET_BYTE(to_push, 6) & 0xFF;
|
||||
if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) {
|
||||
bool gas_pressed = GET_BYTE(to_push, 6) != 0;
|
||||
if (gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
toyota_gas_prev = gas;
|
||||
gas_pressed_prev = gas_pressed;
|
||||
}
|
||||
|
||||
// 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4) && (bus == 0)) {
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,169 +0,0 @@
|
||||
// uses tons from safety_toyota
|
||||
// TODO: refactor to repeat less code
|
||||
|
||||
// IPAS override
|
||||
const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
|
||||
|
||||
// 2m/s are added to be less restrictive
|
||||
const struct lookup_t LOOKUP_ANGLE_RATE_UP = {
|
||||
{2., 7., 17.},
|
||||
{5., .8, .15}};
|
||||
|
||||
const struct lookup_t LOOKUP_ANGLE_RATE_DOWN = {
|
||||
{2., 7., 17.},
|
||||
{5., 3.5, .4}};
|
||||
|
||||
const float RT_ANGLE_FUDGE = 1.5; // for RT checks allow 50% more angle change
|
||||
const float CAN_TO_DEG = 2. / 3.; // convert angles from CAN unit to degrees
|
||||
|
||||
int ipas_state = 1; // 1 disabled, 3 executing angle control, 5 override
|
||||
int angle_control = 0; // 1 if direct angle control packets are seen
|
||||
float speed = 0.;
|
||||
|
||||
struct sample_t angle_meas; // last 3 steer angles
|
||||
struct sample_t torque_driver; // last 3 driver steering torque
|
||||
|
||||
// state of angle limits
|
||||
int16_t desired_angle_last = 0; // last desired steer angle
|
||||
int16_t rt_angle_last = 0; // last desired torque for real time check
|
||||
uint32_t ts_angle_last = 0;
|
||||
|
||||
int controls_allowed_last = 0;
|
||||
|
||||
|
||||
static int toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
// check standard toyota stuff as well
|
||||
bool valid = toyota_rx_hook(to_push);
|
||||
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
if (addr == 0x260) {
|
||||
// get driver steering torque
|
||||
int16_t torque_driver_new = (GET_BYTE(to_push, 1) << 8) | GET_BYTE(to_push, 2);
|
||||
|
||||
// update array of samples
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// get steer angle
|
||||
if (addr == 0x25) {
|
||||
int angle_meas_new = ((GET_BYTE(to_push, 0) & 0xF) << 8) | GET_BYTE(to_push, 1);
|
||||
uint32_t ts = TIM2->CNT;
|
||||
|
||||
angle_meas_new = to_signed(angle_meas_new, 12);
|
||||
|
||||
// update array of samples
|
||||
update_sample(&angle_meas, angle_meas_new);
|
||||
|
||||
// *** angle real time check
|
||||
// add 1 to not false trigger the violation and multiply by 20 since the check is done every 250ms and steer angle is updated at 80Hz
|
||||
int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG) + 1.)));
|
||||
int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG) + 1.)));
|
||||
int highest_rt_angle = rt_angle_last + ((rt_angle_last > 0) ? rt_delta_angle_up : rt_delta_angle_down);
|
||||
int lowest_rt_angle = rt_angle_last - ((rt_angle_last > 0) ? rt_delta_angle_down : rt_delta_angle_up);
|
||||
|
||||
// every RT_INTERVAL or when controls are turned on, set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last);
|
||||
if ((ts_elapsed > TOYOTA_RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) {
|
||||
rt_angle_last = angle_meas_new;
|
||||
ts_angle_last = ts;
|
||||
}
|
||||
|
||||
// check for violation
|
||||
if (angle_control &&
|
||||
((angle_meas_new < lowest_rt_angle) ||
|
||||
(angle_meas_new > highest_rt_angle))) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
|
||||
controls_allowed_last = controls_allowed;
|
||||
}
|
||||
|
||||
// get speed
|
||||
if (addr == 0xb4) {
|
||||
speed = ((float)((GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6))) * 0.01 / 3.6;
|
||||
}
|
||||
|
||||
// get ipas state
|
||||
if (addr == 0x262) {
|
||||
ipas_state = GET_BYTE(to_push, 0) & 0xf;
|
||||
}
|
||||
|
||||
// exit controls on high steering override
|
||||
if (angle_control && ((torque_driver.min > TOYOTA_IPAS_OVERRIDE_THRESHOLD) ||
|
||||
(torque_driver.max < -TOYOTA_IPAS_OVERRIDE_THRESHOLD) ||
|
||||
(ipas_state==5))) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
|
||||
int tx = 1;
|
||||
int bypass_standard_tx_hook = 0;
|
||||
int bus = GET_BUS(to_send);
|
||||
int addr = GET_ADDR(to_send);
|
||||
|
||||
// Check if msg is sent on BUS 0
|
||||
if (bus == 0) {
|
||||
|
||||
// STEER ANGLE
|
||||
if ((addr == 0x266) || (addr == 0x167)) {
|
||||
|
||||
angle_control = 1; // we are in angle control mode
|
||||
int desired_angle = ((GET_BYTE(to_send, 0) & 0xF) << 8) | GET_BYTE(to_send, 1);
|
||||
int ipas_state_cmd = GET_BYTE(to_send, 0) >> 4;
|
||||
bool violation = 0;
|
||||
|
||||
desired_angle = to_signed(desired_angle, 12);
|
||||
|
||||
if (controls_allowed) {
|
||||
// add 1 to not false trigger the violation
|
||||
float delta_angle_float;
|
||||
delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG) + 1.;
|
||||
int delta_angle_up = (int) (delta_angle_float);
|
||||
delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG) + 1.;
|
||||
int delta_angle_down = (int) (delta_angle_float);
|
||||
|
||||
int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
|
||||
int lowest_desired_angle = desired_angle_last - ((desired_angle_last > 0) ? delta_angle_down : delta_angle_up);
|
||||
if ((desired_angle > highest_desired_angle) ||
|
||||
(desired_angle < lowest_desired_angle)){
|
||||
violation = 1;
|
||||
controls_allowed = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// desired steer angle should be the same as steer angle measured when controls are off
|
||||
if ((!controls_allowed) &&
|
||||
((desired_angle < (angle_meas.min - 1)) ||
|
||||
(desired_angle > (angle_meas.max + 1)) ||
|
||||
(ipas_state_cmd != 1))) {
|
||||
violation = 1;
|
||||
}
|
||||
|
||||
desired_angle_last = desired_angle;
|
||||
|
||||
if (violation) {
|
||||
tx = 0;
|
||||
}
|
||||
bypass_standard_tx_hook = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// check standard toyota stuff as well if addr isn't IPAS related
|
||||
if (!bypass_standard_tx_hook) {
|
||||
tx &= toyota_tx_hook(to_send);
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
const safety_hooks toyota_ipas_hooks = {
|
||||
.init = toyota_init,
|
||||
.rx = toyota_ipas_rx_hook,
|
||||
.tx = toyota_ipas_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
.fwd = toyota_fwd_hook,
|
||||
};
|
||||
@@ -1,142 +1,225 @@
|
||||
// Safety-relevant CAN messages for the Volkswagen MQB platform.
|
||||
#define MSG_EPS_01 0x09F
|
||||
#define MSG_MOTOR_20 0x121
|
||||
#define MSG_ACC_06 0x122
|
||||
#define MSG_HCA_01 0x126
|
||||
#define MSG_GRA_ACC_01 0x12B
|
||||
#define MSG_LDW_02 0x397
|
||||
|
||||
const int VOLKSWAGEN_MAX_STEER = 250; // 2.5 Nm (EPS side max of 3.0Nm with fault if violated)
|
||||
// Safety-relevant steering constants for Volkswagen
|
||||
const int VOLKSWAGEN_MAX_STEER = 300; // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
|
||||
const int VOLKSWAGEN_MAX_RT_DELTA = 75; // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75
|
||||
const uint32_t VOLKSWAGEN_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int VOLKSWAGEN_MAX_RATE_UP = 4; // 2.0 Nm/s available rate of change from the steering rack (EPS side delta-limit of 5.0 Nm/s)
|
||||
const int VOLKSWAGEN_MAX_RATE_DOWN = 10; // 5.0 Nm/s available rate of change from the steering rack (EPS side delta-limit of 5.0 Nm/s)
|
||||
const int VOLKSWAGEN_MAX_RATE_UP = 4; // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
|
||||
const int VOLKSWAGEN_MAX_RATE_DOWN = 10; // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
|
||||
const int VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE = 80;
|
||||
const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;
|
||||
|
||||
// MSG_GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
|
||||
const AddrBus VOLKSWAGEN_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}};
|
||||
// Safety-relevant CAN messages for the Volkswagen MQB platform
|
||||
#define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds
|
||||
#define MSG_EPS_01 0x09F // RX from EPS, for driver steering torque
|
||||
#define MSG_ESP_05 0x106 // RX from ABS, for brake switch state
|
||||
#define MSG_TSK_06 0x120 // RX from ECU, for ACC status from drivetrain coordinator
|
||||
#define MSG_MOTOR_20 0x121 // RX from ECU, for driver throttle input
|
||||
#define MSG_HCA_01 0x126 // TX by OP, Heading Control Assist steering torque
|
||||
#define MSG_GRA_ACC_01 0x12B // TX by OP, ACC control buttons for cancel/resume
|
||||
#define MSG_LDW_02 0x397 // TX by OP, Lane line recognition and text alerts
|
||||
|
||||
// TODO: do checksum and counter checks
|
||||
AddrCheckStruct volkswagen_rx_checks[] = {
|
||||
{.addr = {MSG_EPS_01}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {MSG_ACC_06}, .bus = 0, .expected_timestep = 20000U},
|
||||
{.addr = {MSG_MOTOR_20}, .bus = 0, .expected_timestep = 20000U},
|
||||
// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
|
||||
const AddrBus VOLKSWAGEN_MQB_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}};
|
||||
const int VOLKSWAGEN_MQB_TX_MSGS_LEN = sizeof(VOLKSWAGEN_MQB_TX_MSGS) / sizeof(VOLKSWAGEN_MQB_TX_MSGS[0]);
|
||||
|
||||
AddrCheckStruct volkswagen_mqb_rx_checks[] = {
|
||||
{.addr = {MSG_ESP_19}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U},
|
||||
{.addr = {MSG_EPS_01}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U},
|
||||
{.addr = {MSG_ESP_05}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {MSG_TSK_06}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {MSG_MOTOR_20}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
};
|
||||
const int VOLKSWAGEN_MQB_RX_CHECKS_LEN = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]);
|
||||
|
||||
const int VOLKSWAGEN_RX_CHECK_LEN = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]);
|
||||
|
||||
struct sample_t volkswagen_torque_driver; // last few driver torques measured
|
||||
struct sample_t volkswagen_torque_driver; // Last few driver torques measured
|
||||
int volkswagen_rt_torque_last = 0;
|
||||
int volkswagen_desired_torque_last = 0;
|
||||
uint32_t volkswagen_ts_last = 0;
|
||||
int volkswagen_gas_prev = 0;
|
||||
bool volkswagen_moving = false;
|
||||
int volkswagen_torque_msg = 0;
|
||||
int volkswagen_lane_msg = 0;
|
||||
uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
|
||||
|
||||
static int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
bool valid = addr_safety_check(to_push, volkswagen_rx_checks, VOLKSWAGEN_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
static uint8_t volkswagen_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
return (uint8_t)GET_BYTE(to_push, 0);
|
||||
}
|
||||
|
||||
if (valid) {
|
||||
int bus = GET_BUS(to_push);
|
||||
int addr = GET_ADDR(to_push);
|
||||
static uint8_t volkswagen_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
return (uint8_t)GET_BYTE(to_push, 1) & 0xFU;
|
||||
}
|
||||
|
||||
// Update driver input torque samples from EPS_01.Driver_Strain for absolute torque, and EPS_01.Driver_Strain_VZ
|
||||
// for the direction.
|
||||
if ((bus == 0) && (addr == MSG_EPS_01)) {
|
||||
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
|
||||
int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
|
||||
if (sign == 1) {
|
||||
torque_driver_new *= -1;
|
||||
}
|
||||
static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
int len = GET_LEN(to_push);
|
||||
|
||||
update_sample(&volkswagen_torque_driver, torque_driver_new);
|
||||
}
|
||||
// This is CRC-8H2F/AUTOSAR with a twist. See the OpenDBC implementation
|
||||
// of this algorithm for a version with explanatory comments.
|
||||
|
||||
// Monitor ACC_06.ACC_Status_ACC for stock ACC status. Because the current MQB port is lateral-only, OP's control
|
||||
// allowed state is directly driven by stock ACC engagement. Permit the ACC message to come from either bus, in
|
||||
// order to accommodate future camera-side integrations if needed.
|
||||
if (addr == MSG_ACC_06) {
|
||||
int acc_status = (GET_BYTE(to_push, 7) & 0x70) >> 4;
|
||||
controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0;
|
||||
}
|
||||
uint8_t crc = 0xFFU;
|
||||
for (int i = 1; i < len; i++) {
|
||||
crc ^= (uint8_t)GET_BYTE(to_push, i);
|
||||
crc = volkswagen_crc8_lut_8h2f[crc];
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press. Bits [12-20)
|
||||
if (addr == MSG_MOTOR_20) {
|
||||
int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
|
||||
if ((gas > 0) && (volkswagen_gas_prev == 0)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
volkswagen_gas_prev = gas;
|
||||
}
|
||||
uint8_t counter = volkswagen_get_counter(to_push);
|
||||
switch(addr) {
|
||||
case MSG_EPS_01:
|
||||
crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
|
||||
break;
|
||||
case MSG_ESP_05:
|
||||
crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
|
||||
break;
|
||||
case MSG_TSK_06:
|
||||
crc ^= (uint8_t[]){0xC4,0xE2,0x4F,0xE4,0xF8,0x2F,0x56,0x81,0x9F,0xE5,0x83,0x44,0x05,0x3F,0x97,0xDF}[counter];
|
||||
break;
|
||||
case MSG_MOTOR_20:
|
||||
crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter];
|
||||
break;
|
||||
default: // Undefined CAN message, CRC check expected to fail
|
||||
break;
|
||||
}
|
||||
crc = volkswagen_crc8_lut_8h2f[crc];
|
||||
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
return crc ^ 0xFFU;
|
||||
}
|
||||
|
||||
static void volkswagen_mqb_init(int16_t param) {
|
||||
UNUSED(param);
|
||||
|
||||
controls_allowed = false;
|
||||
relay_malfunction = false;
|
||||
volkswagen_torque_msg = MSG_HCA_01;
|
||||
volkswagen_lane_msg = MSG_LDW_02;
|
||||
gen_crc_lookup_table(0x2F, volkswagen_crc8_lut_8h2f);
|
||||
}
|
||||
|
||||
static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
bool valid = addr_safety_check(to_push, volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_RX_CHECKS_LEN,
|
||||
volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_get_counter);
|
||||
|
||||
if (valid && (GET_BUS(to_push) == 0)) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
// Update in-motion state by sampling front wheel speeds
|
||||
// Signal: ESP_19.ESP_VL_Radgeschw_02 (front left) in scaled km/h
|
||||
// Signal: ESP_19.ESP_VR_Radgeschw_02 (front right) in scaled km/h
|
||||
if (addr == MSG_ESP_19) {
|
||||
int wheel_speed_fl = GET_BYTE(to_push, 4) | (GET_BYTE(to_push, 5) << 8);
|
||||
int wheel_speed_fr = GET_BYTE(to_push, 6) | (GET_BYTE(to_push, 7) << 8);
|
||||
// Check for average front speed in excess of 0.3m/s, 1.08km/h
|
||||
// DBC speed scale 0.0075: 0.3m/s = 144, sum both wheels to compare
|
||||
volkswagen_moving = (wheel_speed_fl + wheel_speed_fr) > 288;
|
||||
}
|
||||
|
||||
// Update driver input torque samples
|
||||
// Signal: EPS_01.Driver_Strain (absolute torque)
|
||||
// Signal: EPS_01.Driver_Strain_VZ (direction)
|
||||
if (addr == MSG_EPS_01) {
|
||||
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
|
||||
int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
|
||||
if (sign == 1) {
|
||||
torque_driver_new *= -1;
|
||||
}
|
||||
update_sample(&volkswagen_torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// Update ACC status from drivetrain coordinator for controls-allowed state
|
||||
// Signal: TSK_06.TSK_Status
|
||||
if (addr == MSG_TSK_06) {
|
||||
int acc_status = (GET_BYTE(to_push, 3) & 0x7);
|
||||
controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0;
|
||||
}
|
||||
|
||||
// Exit controls on rising edge of gas press
|
||||
// Signal: Motor_20.MO_Fahrpedalrohwert_01
|
||||
if (addr == MSG_MOTOR_20) {
|
||||
bool gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFF) != 0;
|
||||
if (gas_pressed && !gas_pressed_prev) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
gas_pressed_prev = gas_pressed;
|
||||
}
|
||||
|
||||
// Exit controls on rising edge of brake press
|
||||
// Signal: ESP_05.ESP_Fahrer_bremst
|
||||
if (addr == MSG_ESP_05) {
|
||||
bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2;
|
||||
if (brake_pressed && (!brake_pressed_prev || volkswagen_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
}
|
||||
|
||||
// If there are HCA messages on bus 0 not sent by OP, there's a relay problem
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == MSG_HCA_01)) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
static bool volkswagen_steering_check(int desired_torque) {
|
||||
bool violation = false;
|
||||
uint32_t ts = TIM2->CNT;
|
||||
|
||||
if (controls_allowed) {
|
||||
// *** global torque limit check ***
|
||||
violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
violation |= driver_limit_check(desired_torque, volkswagen_desired_torque_last, &volkswagen_torque_driver,
|
||||
VOLKSWAGEN_MAX_STEER, VOLKSWAGEN_MAX_RATE_UP, VOLKSWAGEN_MAX_RATE_DOWN,
|
||||
VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE, VOLKSWAGEN_DRIVER_TORQUE_FACTOR);
|
||||
volkswagen_desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, volkswagen_ts_last);
|
||||
if (ts_elapsed > VOLKSWAGEN_RT_INTERVAL) {
|
||||
volkswagen_rt_torque_last = desired_torque;
|
||||
volkswagen_ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
// no torque if controls is not allowed
|
||||
if (!controls_allowed && (desired_torque != 0)) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
volkswagen_desired_torque_last = 0;
|
||||
volkswagen_rt_torque_last = 0;
|
||||
volkswagen_ts_last = ts;
|
||||
}
|
||||
|
||||
return violation;
|
||||
}
|
||||
|
||||
static int volkswagen_mqb_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
int addr = GET_ADDR(to_send);
|
||||
int bus = GET_BUS(to_send);
|
||||
int tx = 1;
|
||||
|
||||
if (!msg_allowed(addr, bus, VOLKSWAGEN_TX_MSGS, sizeof(VOLKSWAGEN_TX_MSGS)/sizeof(VOLKSWAGEN_TX_MSGS[0]))) {
|
||||
if (!msg_allowed(addr, bus, VOLKSWAGEN_MQB_TX_MSGS, VOLKSWAGEN_MQB_TX_MSGS_LEN) || relay_malfunction) {
|
||||
tx = 0;
|
||||
}
|
||||
|
||||
if (relay_malfunction) {
|
||||
tx = 0;
|
||||
}
|
||||
|
||||
// Safety check for HCA_01 Heading Control Assist torque.
|
||||
// Safety check for HCA_01 Heading Control Assist torque
|
||||
// Signal: HCA_01.Assist_Torque (absolute torque)
|
||||
// Signal: HCA_01.Assist_VZ (direction)
|
||||
if (addr == MSG_HCA_01) {
|
||||
bool violation = false;
|
||||
|
||||
int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x3F) << 8);
|
||||
int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7;
|
||||
if (sign == 1) {
|
||||
desired_torque *= -1;
|
||||
}
|
||||
|
||||
uint32_t ts = TIM2->CNT;
|
||||
|
||||
if (controls_allowed) {
|
||||
|
||||
// *** global torque limit check ***
|
||||
violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
violation |= driver_limit_check(desired_torque, volkswagen_desired_torque_last, &volkswagen_torque_driver,
|
||||
VOLKSWAGEN_MAX_STEER, VOLKSWAGEN_MAX_RATE_UP, VOLKSWAGEN_MAX_RATE_DOWN,
|
||||
VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE, VOLKSWAGEN_DRIVER_TORQUE_FACTOR);
|
||||
volkswagen_desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, volkswagen_ts_last);
|
||||
if (ts_elapsed > VOLKSWAGEN_RT_INTERVAL) {
|
||||
volkswagen_rt_torque_last = desired_torque;
|
||||
volkswagen_ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
// no torque if controls is not allowed
|
||||
if (!controls_allowed && (desired_torque != 0)) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
volkswagen_desired_torque_last = 0;
|
||||
volkswagen_rt_torque_last = 0;
|
||||
volkswagen_ts_last = ts;
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
if (volkswagen_steering_check(desired_torque)) {
|
||||
tx = 0;
|
||||
}
|
||||
}
|
||||
@@ -158,25 +241,23 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
int addr = GET_ADDR(to_fwd);
|
||||
int bus_fwd = -1;
|
||||
|
||||
// NOTE: Will need refactoring for other bus layouts, such as no-forwarding at camera or J533 running-gear CAN
|
||||
|
||||
if (!relay_malfunction) {
|
||||
switch (bus_num) {
|
||||
case 0:
|
||||
// Forward all traffic from J533 gateway to Extended CAN devices.
|
||||
// Forward all traffic from the Extended CAN onward
|
||||
bus_fwd = 2;
|
||||
break;
|
||||
case 2:
|
||||
if ((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) {
|
||||
// OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera.
|
||||
if ((addr == volkswagen_torque_msg) || (addr == volkswagen_lane_msg)) {
|
||||
// OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera
|
||||
bus_fwd = -1;
|
||||
} else {
|
||||
// Forward all remaining traffic from Extended CAN devices to J533 gateway.
|
||||
// Forward all remaining traffic from Extended CAN devices to J533 gateway
|
||||
bus_fwd = 0;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
// No other buses should be in use; fallback to do-not-forward.
|
||||
// No other buses should be in use; fallback to do-not-forward
|
||||
bus_fwd = -1;
|
||||
break;
|
||||
}
|
||||
@@ -184,12 +265,13 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return bus_fwd;
|
||||
}
|
||||
|
||||
const safety_hooks volkswagen_hooks = {
|
||||
.init = nooutput_init,
|
||||
.rx = volkswagen_rx_hook,
|
||||
.tx = volkswagen_tx_hook,
|
||||
// Volkswagen MQB platform
|
||||
const safety_hooks volkswagen_mqb_hooks = {
|
||||
.init = volkswagen_mqb_init,
|
||||
.rx = volkswagen_mqb_rx_hook,
|
||||
.tx = volkswagen_mqb_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
.fwd = volkswagen_fwd_hook,
|
||||
.addr_check = volkswagen_rx_checks,
|
||||
.addr_check_len = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]),
|
||||
.addr_check = volkswagen_mqb_rx_checks,
|
||||
.addr_check_len = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]),
|
||||
};
|
||||
|
||||
@@ -49,6 +49,7 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
|
||||
const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
|
||||
bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
|
||||
float interpolate(struct lookup_t xy, float x);
|
||||
void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]);
|
||||
bool msg_allowed(int addr, int bus, const AddrBus addr_list[], int len);
|
||||
int get_addr_check_index(CAN_FIFOMailBox_TypeDef *to_push, AddrCheckStruct addr_list[], const int len);
|
||||
void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter);
|
||||
@@ -84,6 +85,8 @@ bool controls_allowed = false;
|
||||
bool relay_malfunction = false;
|
||||
bool gas_interceptor_detected = false;
|
||||
int gas_interceptor_prev = 0;
|
||||
bool gas_pressed_prev = false;
|
||||
bool brake_pressed_prev = false;
|
||||
|
||||
// time since safety mode has been changed
|
||||
uint32_t safety_mode_cnt = 0U;
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
# Connecting to White Panda via Wi-Fi
|
||||
|
||||
1. First connect to your White Panda's Wi-Fi pairing network (this should be the Wi-Fi network WITH the "-pair" at the end)
|
||||
|
||||
2. Now in your favorite web browser go to this address **192.168.0.10** (this should open a web interface to interact with the White Panda)
|
||||
|
||||
3. Inside the web interface enable secured mode by clinking the **secure it** link/button (this should make the White Panda's Wi-Fi network visible)
|
||||
|
||||
### If you need your White Panda's Wi-Fi Password
|
||||
|
||||
* Run the **get_panda_password.py** script in found in **examples/** (Must have panda paw for this step because you need to connect White Panda via USB to retrive the Wi-Fi password)
|
||||
* Also ensure that you are connected to your White Panda's Wi-Fi pairing network
|
||||
|
||||
4. Connect to your White Panda's default Wi-Fi network (this should be the Wi-Fi network WITHOUT the "-pair" at the end)
|
||||
|
||||
5. Your White Panda is now connected to Wi-Fi you can test this by running this line of code `python -c 'from panda import Panda; panda = Panda("WIFI")'` in your terminal of choice.
|
||||
@@ -123,12 +123,13 @@ class Panda(object):
|
||||
SAFETY_TESLA = 10
|
||||
SAFETY_SUBARU = 11
|
||||
SAFETY_MAZDA = 13
|
||||
SAFETY_VOLKSWAGEN = 15
|
||||
SAFETY_TOYOTA_IPAS = 16
|
||||
SAFETY_NISSAN = 14
|
||||
SAFETY_VOLKSWAGEN_MQB = 15
|
||||
SAFETY_ALLOUTPUT = 17
|
||||
SAFETY_GM_ASCM = 18
|
||||
SAFETY_NOOUTPUT = 19
|
||||
SAFETY_HONDA_BOSCH_HARNESS = 20
|
||||
SAFETY_SUBARU_LEGACY = 22
|
||||
|
||||
SERIAL_DEBUG = 0
|
||||
SERIAL_ESP = 1
|
||||
|
||||
@@ -9,3 +9,4 @@ requests
|
||||
flake8==3.7.9
|
||||
pylint==2.4.3
|
||||
cffi==1.11.4
|
||||
crcmod
|
||||
|
||||
@@ -13,26 +13,58 @@ def make_msg(bus, addr, length=8):
|
||||
|
||||
return to_send
|
||||
|
||||
def test_relay_malfunction(test, addr):
|
||||
# input is a test class and the address that, if seen on bus 0, triggers
|
||||
# the relay_malfunction protection logic: both tx_hook and fwd_hook are
|
||||
# expected to return failure
|
||||
test.assertFalse(test.safety.get_relay_malfunction())
|
||||
test.safety.safety_rx_hook(make_msg(0, addr, 8))
|
||||
test.assertTrue(test.safety.get_relay_malfunction())
|
||||
for a in range(1, 0x800):
|
||||
for b in range(0, 3):
|
||||
test.assertFalse(test.safety.safety_tx_hook(make_msg(b, a, 8)))
|
||||
test.assertEqual(-1, test.safety.safety_fwd_hook(b, make_msg(b, a, 8)))
|
||||
class StdTest:
|
||||
@staticmethod
|
||||
def test_relay_malfunction(test, addr, bus=0):
|
||||
# input is a test class and the address that, if seen on specified bus, triggers
|
||||
# the relay_malfunction protection logic: both tx_hook and fwd_hook are
|
||||
# expected to return failure
|
||||
test.assertFalse(test.safety.get_relay_malfunction())
|
||||
test.safety.safety_rx_hook(make_msg(bus, addr, 8))
|
||||
test.assertTrue(test.safety.get_relay_malfunction())
|
||||
for a in range(1, 0x800):
|
||||
for b in range(0, 3):
|
||||
test.assertFalse(test.safety.safety_tx_hook(make_msg(b, a, 8)))
|
||||
test.assertEqual(-1, test.safety.safety_fwd_hook(b, make_msg(b, a, 8)))
|
||||
|
||||
def test_manually_enable_controls_allowed(test):
|
||||
test.safety.set_controls_allowed(1)
|
||||
test.assertTrue(test.safety.get_controls_allowed())
|
||||
test.safety.set_controls_allowed(0)
|
||||
test.assertFalse(test.safety.get_controls_allowed())
|
||||
@staticmethod
|
||||
def test_manually_enable_controls_allowed(test):
|
||||
test.safety.set_controls_allowed(1)
|
||||
test.assertTrue(test.safety.get_controls_allowed())
|
||||
test.safety.set_controls_allowed(0)
|
||||
test.assertFalse(test.safety.get_controls_allowed())
|
||||
|
||||
def test_spam_can_buses(test, TX_MSGS):
|
||||
for addr in range(1, 0x800):
|
||||
for bus in range(0, 4):
|
||||
if all(addr != m[0] or bus != m[1] for m in TX_MSGS):
|
||||
test.assertFalse(test.safety.safety_tx_hook(make_msg(bus, addr, 8)))
|
||||
@staticmethod
|
||||
def test_spam_can_buses(test, TX_MSGS):
|
||||
for addr in range(1, 0x800):
|
||||
for bus in range(0, 4):
|
||||
if all(addr != m[0] or bus != m[1] for m in TX_MSGS):
|
||||
test.assertFalse(test.safety.safety_tx_hook(make_msg(bus, addr, 8)))
|
||||
|
||||
@staticmethod
|
||||
def test_allow_brake_at_zero_speed(test):
|
||||
# Brake was already pressed
|
||||
test.safety.safety_rx_hook(test._speed_msg(0))
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.safety.set_controls_allowed(1)
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.assertTrue(test.safety.get_controls_allowed())
|
||||
test.safety.safety_rx_hook(test._brake_msg(0))
|
||||
test.assertTrue(test.safety.get_controls_allowed())
|
||||
# rising edge of brake should disengage
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.assertFalse(test.safety.get_controls_allowed())
|
||||
test.safety.safety_rx_hook(test._brake_msg(0)) # reset no brakes
|
||||
|
||||
@staticmethod
|
||||
def test_not_allow_brake_when_moving(test, standstill_threshold):
|
||||
# Brake was already pressed
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.safety.set_controls_allowed(1)
|
||||
test.safety.safety_rx_hook(test._speed_msg(standstill_threshold))
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.assertTrue(test.safety.get_controls_allowed())
|
||||
test.safety.safety_rx_hook(test._speed_msg(standstill_threshold + 1))
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.assertFalse(test.safety.get_controls_allowed())
|
||||
test.safety.safety_rx_hook(test._speed_msg(0))
|
||||
|
||||
@@ -37,9 +37,10 @@ bool get_relay_malfunction(void);
|
||||
void set_gas_interceptor_detected(bool c);
|
||||
bool get_gas_interceptor_detetcted(void);
|
||||
int get_gas_interceptor_prev(void);
|
||||
bool get_gas_pressed_prev(void);
|
||||
bool get_brake_pressed_prev(void);
|
||||
int get_hw_type(void);
|
||||
void set_timer(uint32_t t);
|
||||
void reset_angle_control(void);
|
||||
|
||||
int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send);
|
||||
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push);
|
||||
@@ -49,18 +50,14 @@ int set_safety_hooks(uint16_t mode, int16_t param);
|
||||
void init_tests_toyota(void);
|
||||
int get_toyota_torque_meas_min(void);
|
||||
int get_toyota_torque_meas_max(void);
|
||||
int get_toyota_gas_prev(void);
|
||||
void set_toyota_torque_meas(int min, int max);
|
||||
void set_toyota_desired_torque_last(int t);
|
||||
void set_toyota_rt_torque_last(int t);
|
||||
|
||||
void init_tests_honda(void);
|
||||
bool get_honda_moving(void);
|
||||
bool get_honda_brake_pressed_prev(void);
|
||||
int get_honda_gas_prev(void);
|
||||
void set_honda_fwd_brake(bool);
|
||||
void set_honda_alt_brake_msg(bool);
|
||||
void set_honda_hw(int);
|
||||
int get_honda_hw(void);
|
||||
|
||||
void init_tests_cadillac(void);
|
||||
@@ -88,13 +85,18 @@ void set_chrysler_torque_meas(int min, int max);
|
||||
void init_tests_subaru(void);
|
||||
void set_subaru_desired_torque_last(int t);
|
||||
void set_subaru_rt_torque_last(int t);
|
||||
void set_subaru_torque_driver(int min, int max);
|
||||
bool get_subaru_global(void);
|
||||
|
||||
void init_tests_volkswagen(void);
|
||||
int get_volkswagen_torque_driver_min(void);
|
||||
int get_volkswagen_torque_driver_max(void);
|
||||
bool get_volkswagen_moving(void);
|
||||
void set_volkswagen_desired_torque_last(int t);
|
||||
void set_volkswagen_rt_torque_last(int t);
|
||||
void set_volkswagen_torque_driver(int min, int max);
|
||||
int get_volkswagen_gas_prev(void);
|
||||
|
||||
void init_tests_nissan(void);
|
||||
void set_nissan_desired_angle_last(int t);
|
||||
|
||||
""")
|
||||
|
||||
|
||||
+42
-29
@@ -58,6 +58,10 @@ uint8_t hw_type = HW_TYPE_UNKNOWN;
|
||||
__typeof__ (b) _b = (b); \
|
||||
_a > _b ? _a : _b; })
|
||||
|
||||
#define ABS(a) \
|
||||
({ __typeof__ (a) _a = (a); \
|
||||
(_a > 0) ? _a : (-_a); })
|
||||
|
||||
// from llcan.h
|
||||
#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
|
||||
#define GET_LEN(msg) ((msg)->RDTR & 0xf)
|
||||
@@ -85,10 +89,6 @@ void set_gas_interceptor_detected(bool c){
|
||||
gas_interceptor_detected = c;
|
||||
}
|
||||
|
||||
void reset_angle_control(void){
|
||||
angle_control = 0;
|
||||
}
|
||||
|
||||
bool get_controls_allowed(void){
|
||||
return controls_allowed;
|
||||
}
|
||||
@@ -105,10 +105,22 @@ int get_gas_interceptor_prev(void){
|
||||
return gas_interceptor_prev;
|
||||
}
|
||||
|
||||
bool get_gas_pressed_prev(void){
|
||||
return gas_pressed_prev;
|
||||
}
|
||||
|
||||
bool get_brake_pressed_prev(void){
|
||||
return brake_pressed_prev;
|
||||
}
|
||||
|
||||
int get_hw_type(void){
|
||||
return hw_type;
|
||||
}
|
||||
|
||||
bool get_subaru_global(void){
|
||||
return subaru_global;
|
||||
}
|
||||
|
||||
void set_timer(uint32_t t){
|
||||
timer.CNT = t;
|
||||
}
|
||||
@@ -138,16 +150,19 @@ void set_chrysler_torque_meas(int min, int max){
|
||||
chrysler_torque_meas.max = max;
|
||||
}
|
||||
|
||||
void set_subaru_torque_driver(int min, int max){
|
||||
subaru_torque_driver.min = min;
|
||||
subaru_torque_driver.max = max;
|
||||
}
|
||||
|
||||
void set_volkswagen_torque_driver(int min, int max){
|
||||
volkswagen_torque_driver.min = min;
|
||||
volkswagen_torque_driver.max = max;
|
||||
}
|
||||
|
||||
int get_volkswagen_torque_driver_min(void){
|
||||
return volkswagen_torque_driver.min;
|
||||
}
|
||||
|
||||
int get_volkswagen_torque_driver_max(void){
|
||||
return volkswagen_torque_driver.max;
|
||||
}
|
||||
|
||||
int get_chrysler_torque_meas_min(void){
|
||||
return chrysler_torque_meas.min;
|
||||
}
|
||||
@@ -156,10 +171,6 @@ int get_chrysler_torque_meas_max(void){
|
||||
return chrysler_torque_meas.max;
|
||||
}
|
||||
|
||||
int get_toyota_gas_prev(void){
|
||||
return toyota_gas_prev;
|
||||
}
|
||||
|
||||
int get_toyota_torque_meas_min(void){
|
||||
return toyota_torque_meas.min;
|
||||
}
|
||||
@@ -224,30 +235,18 @@ void set_volkswagen_desired_torque_last(int t){
|
||||
volkswagen_desired_torque_last = t;
|
||||
}
|
||||
|
||||
int get_volkswagen_gas_prev(void){
|
||||
return volkswagen_gas_prev;
|
||||
int get_volkswagen_moving(void){
|
||||
return volkswagen_moving;
|
||||
}
|
||||
|
||||
bool get_honda_moving(void){
|
||||
return honda_moving;
|
||||
}
|
||||
|
||||
bool get_honda_brake_pressed_prev(void){
|
||||
return honda_brake_pressed_prev;
|
||||
}
|
||||
|
||||
int get_honda_gas_prev(void){
|
||||
return honda_gas_prev;
|
||||
}
|
||||
|
||||
void set_honda_alt_brake_msg(bool c){
|
||||
honda_alt_brake_msg = c;
|
||||
}
|
||||
|
||||
void set_honda_hw(int c){
|
||||
honda_hw = c;
|
||||
}
|
||||
|
||||
int get_honda_hw(void) {
|
||||
return honda_hw;
|
||||
}
|
||||
@@ -256,10 +255,16 @@ void set_honda_fwd_brake(bool c){
|
||||
honda_fwd_brake = c;
|
||||
}
|
||||
|
||||
void set_nissan_desired_angle_last(int t){
|
||||
nissan_desired_angle_last = t;
|
||||
}
|
||||
|
||||
void init_tests(void){
|
||||
// get HW_TYPE from env variable set in test.sh
|
||||
hw_type = atoi(getenv("HW_TYPE"));
|
||||
safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic
|
||||
gas_pressed_prev = false;
|
||||
brake_pressed_prev = false;
|
||||
}
|
||||
|
||||
void init_tests_toyota(void){
|
||||
@@ -304,6 +309,7 @@ void init_tests_hyundai(void){
|
||||
|
||||
void init_tests_chrysler(void){
|
||||
init_tests();
|
||||
chrysler_speed = 0;
|
||||
chrysler_torque_meas.min = 0;
|
||||
chrysler_torque_meas.max = 0;
|
||||
chrysler_desired_torque_last = 0;
|
||||
@@ -324,6 +330,7 @@ void init_tests_subaru(void){
|
||||
|
||||
void init_tests_volkswagen(void){
|
||||
init_tests();
|
||||
volkswagen_moving = false;
|
||||
volkswagen_torque_driver.min = 0;
|
||||
volkswagen_torque_driver.max = 0;
|
||||
volkswagen_desired_torque_last = 0;
|
||||
@@ -335,11 +342,17 @@ void init_tests_volkswagen(void){
|
||||
void init_tests_honda(void){
|
||||
init_tests();
|
||||
honda_moving = false;
|
||||
honda_brake_pressed_prev = false;
|
||||
honda_gas_prev = 0;
|
||||
honda_fwd_brake = false;
|
||||
}
|
||||
|
||||
void init_tests_nissan(void){
|
||||
init_tests();
|
||||
nissan_angle_meas.min = 0;
|
||||
nissan_angle_meas.max = 0;
|
||||
nissan_desired_angle_last = 0;
|
||||
set_timer(0);
|
||||
}
|
||||
|
||||
void set_gmlan_digital_output(int to_set){
|
||||
}
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@ import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
|
||||
from panda.tests.safety.common import make_msg, StdTest
|
||||
|
||||
|
||||
MAX_RATE_UP = 2
|
||||
@@ -56,13 +56,13 @@ class TestCadillacSafety(unittest.TestCase):
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
test_spam_can_buses(self, TX_MSGS)
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
test_manually_enable_controls_allowed(self)
|
||||
StdTest.test_manually_enable_controls_allowed(self)
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 0x370)
|
||||
|
||||
@@ -3,7 +3,7 @@ import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
|
||||
from panda.tests.safety.common import StdTest, make_msg
|
||||
|
||||
MAX_RATE_UP = 3
|
||||
MAX_RATE_DOWN = 3
|
||||
@@ -16,7 +16,37 @@ MAX_TORQUE_ERROR = 80
|
||||
|
||||
TX_MSGS = [[571, 0], [658, 0], [678, 0]]
|
||||
|
||||
def chrysler_checksum(msg, len_msg):
|
||||
checksum = 0xFF
|
||||
for idx in range(0, len_msg-1):
|
||||
curr = (msg.RDLR >> (8*idx)) if idx < 4 else (msg.RDHR >> (8*(idx - 4)))
|
||||
curr &= 0xFF
|
||||
shift = 0x80
|
||||
for i in range(0, 8):
|
||||
bit_sum = curr & shift
|
||||
temp_chk = checksum & 0x80
|
||||
if (bit_sum != 0):
|
||||
bit_sum = 0x1C
|
||||
if (temp_chk != 0):
|
||||
bit_sum = 1
|
||||
checksum = checksum << 1
|
||||
temp_chk = checksum | 1
|
||||
bit_sum ^= temp_chk
|
||||
else:
|
||||
if (temp_chk != 0):
|
||||
bit_sum = 0x1D
|
||||
checksum = checksum << 1
|
||||
bit_sum ^= checksum
|
||||
checksum = bit_sum
|
||||
shift = shift >> 1
|
||||
return ~checksum & 0xFF
|
||||
|
||||
class TestChryslerSafety(unittest.TestCase):
|
||||
cnt_torque_meas = 0
|
||||
cnt_gas = 0
|
||||
cnt_cruise = 0
|
||||
cnt_brake = 0
|
||||
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
@@ -28,6 +58,36 @@ class TestChryslerSafety(unittest.TestCase):
|
||||
to_send[0].RDLR = buttons
|
||||
return to_send
|
||||
|
||||
def _cruise_msg(self, active):
|
||||
to_send = make_msg(0, 500)
|
||||
to_send[0].RDLR = 0x380000 if active else 0
|
||||
to_send[0].RDHR |= (self.cnt_cruise % 16) << 20
|
||||
to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24
|
||||
self.__class__.cnt_cruise += 1
|
||||
return to_send
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
speed = int(speed / 0.071028)
|
||||
to_send = make_msg(0, 514, 4)
|
||||
to_send[0].RDLR = ((speed & 0xFF0) >> 4) + ((speed & 0xF) << 12) + \
|
||||
((speed & 0xFF0) << 12) + ((speed & 0xF) << 28)
|
||||
return to_send
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
to_send = make_msg(0, 308)
|
||||
to_send[0].RDHR = (gas & 0x7F) << 8
|
||||
to_send[0].RDHR |= (self.cnt_gas % 16) << 20
|
||||
self.__class__.cnt_gas += 1
|
||||
return to_send
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(0, 320)
|
||||
to_send[0].RDLR = 5 if brake else 0
|
||||
to_send[0].RDHR |= (self.cnt_brake % 16) << 20
|
||||
to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24
|
||||
self.__class__.cnt_brake += 1
|
||||
return to_send
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_chrysler_desired_torque_last(t)
|
||||
self.safety.set_chrysler_rt_torque_last(t)
|
||||
@@ -36,6 +96,9 @@ class TestChryslerSafety(unittest.TestCase):
|
||||
def _torque_meas_msg(self, torque):
|
||||
to_send = make_msg(0, 544)
|
||||
to_send[0].RDHR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8)
|
||||
to_send[0].RDHR |= (self.cnt_torque_meas % 16) << 20
|
||||
to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24
|
||||
self.__class__.cnt_torque_meas += 1
|
||||
return to_send
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
@@ -44,10 +107,10 @@ class TestChryslerSafety(unittest.TestCase):
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
test_spam_can_buses(self, TX_MSGS)
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
test_relay_malfunction(self, 0x292)
|
||||
StdTest.test_relay_malfunction(self, 0x292)
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
@@ -63,23 +126,33 @@ class TestChryslerSafety(unittest.TestCase):
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
test_manually_enable_controls_allowed(self)
|
||||
StdTest.test_manually_enable_controls_allowed(self)
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 0x1F4)
|
||||
to_push[0].RDLR = 0x380000
|
||||
|
||||
to_push = self._cruise_msg(True)
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 0x1F4)
|
||||
to_push[0].RDLR = 0
|
||||
|
||||
to_push = self._cruise_msg(False)
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_gas_disable(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._speed_msg(2.2))
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.safety.safety_rx_hook(self._speed_msg(2.3))
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_brake_disengage(self):
|
||||
StdTest.test_allow_brake_at_zero_speed(self)
|
||||
StdTest.test_not_allow_brake_when_moving(self, 0)
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@ import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
|
||||
from panda.tests.safety.common import StdTest, make_msg
|
||||
|
||||
MAX_RATE_UP = 7
|
||||
MAX_RATE_DOWN = 17
|
||||
@@ -90,10 +90,10 @@ class TestGmSafety(unittest.TestCase):
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
test_spam_can_buses(self, TX_MSGS)
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
test_relay_malfunction(self, 384)
|
||||
StdTest.test_relay_malfunction(self, 384)
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
@@ -116,29 +116,9 @@ class TestGmSafety(unittest.TestCase):
|
||||
self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disengage_on_brake(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_allow_brake_at_zero_speed(self):
|
||||
# Brake was already pressed
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.safety.set_controls_allowed(1)
|
||||
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._brake_msg(False))
|
||||
|
||||
def test_not_allow_brake_when_moving(self):
|
||||
# Brake was already pressed
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.safety.safety_rx_hook(self._speed_msg(100))
|
||||
self.safety.set_controls_allowed(1)
|
||||
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._brake_msg(False))
|
||||
def test_brake_disengage(self):
|
||||
StdTest.test_allow_brake_at_zero_speed(self)
|
||||
StdTest.test_not_allow_brake_when_moving(self, 0)
|
||||
|
||||
def test_disengage_on_gas(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
@@ -182,7 +162,7 @@ class TestGmSafety(unittest.TestCase):
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
test_manually_enable_controls_allowed(self)
|
||||
StdTest.test_manually_enable_controls_allowed(self)
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_gm_torque_driver(0, 0)
|
||||
|
||||
@@ -3,14 +3,15 @@ import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import test_relay_malfunction, make_msg, \
|
||||
test_manually_enable_controls_allowed, \
|
||||
test_spam_can_buses, MAX_WRONG_COUNTERS
|
||||
from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS
|
||||
|
||||
MAX_BRAKE = 255
|
||||
|
||||
INTERCEPTOR_THRESHOLD = 328
|
||||
TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]]
|
||||
N_TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]]
|
||||
BH_TX_MSGS = [[0xE4, 0], [0x296, 1], [0x33D, 0]] # Bosch Harness
|
||||
BG_TX_MSGS = [[0xE4, 2], [0x296, 0], [0x33D, 2]] # Bosch Giraffe
|
||||
|
||||
|
||||
HONDA_N_HW = 0
|
||||
HONDA_BG_HW = 1
|
||||
@@ -30,39 +31,41 @@ def honda_checksum(msg, addr, len_msg):
|
||||
|
||||
|
||||
class TestHondaSafety(unittest.TestCase):
|
||||
cnt_speed = 0
|
||||
cnt_gas = 0
|
||||
cnt_button = 0
|
||||
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_NIDEC, 0)
|
||||
cls.safety.init_tests_honda()
|
||||
cls.cnt_speed = 0
|
||||
cls.cnt_gas = 0
|
||||
cls.cnt_button = 0
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
to_send = make_msg(0, 0x158)
|
||||
bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0
|
||||
to_send = make_msg(bus, 0x158)
|
||||
to_send[0].RDLR = speed
|
||||
to_send[0].RDHR |= (self.cnt_speed % 4) << 28
|
||||
to_send[0].RDHR |= honda_checksum(to_send[0], 0x158, 8) << 24
|
||||
self.cnt_speed += 1
|
||||
self.__class__.cnt_speed += 1
|
||||
return to_send
|
||||
|
||||
def _button_msg(self, buttons, addr):
|
||||
honda_hw = self.safety.get_honda_hw()
|
||||
bus = 1 if honda_hw == HONDA_BH_HW else 0
|
||||
bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0
|
||||
to_send = make_msg(bus, addr)
|
||||
to_send[0].RDLR = buttons << 5
|
||||
to_send[0].RDHR |= (self.cnt_button % 4) << 28
|
||||
to_send[0].RDHR |= honda_checksum(to_send[0], addr, 8) << 24
|
||||
self.cnt_button += 1
|
||||
self.__class__.cnt_button += 1
|
||||
return to_send
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(0, 0x17C)
|
||||
bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0
|
||||
to_send = make_msg(bus, 0x17C)
|
||||
to_send[0].RDHR = 0x200000 if brake else 0
|
||||
to_send[0].RDHR |= (self.cnt_gas % 4) << 28
|
||||
to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24
|
||||
self.cnt_gas += 1
|
||||
self.__class__.cnt_gas += 1
|
||||
return to_send
|
||||
|
||||
def _alt_brake_msg(self, brake):
|
||||
@@ -71,11 +74,12 @@ class TestHondaSafety(unittest.TestCase):
|
||||
return to_send
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
to_send = make_msg(0, 0x17C)
|
||||
bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0
|
||||
to_send = make_msg(bus, 0x17C)
|
||||
to_send[0].RDLR = 1 if gas else 0
|
||||
to_send[0].RDHR |= (self.cnt_gas % 4) << 28
|
||||
to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24
|
||||
self.cnt_gas += 1
|
||||
self.__class__.cnt_gas += 1
|
||||
return to_send
|
||||
|
||||
def _send_brake_msg(self, brake):
|
||||
@@ -91,39 +95,48 @@ class TestHondaSafety(unittest.TestCase):
|
||||
return to_send
|
||||
|
||||
def _send_steer_msg(self, steer):
|
||||
to_send = make_msg(0, 0xE4, 6)
|
||||
bus = 2 if self.safety.get_honda_hw() == HONDA_BG_HW else 0
|
||||
to_send = make_msg(bus, 0xE4, 6)
|
||||
to_send[0].RDLR = steer
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
self.safety.set_honda_hw(HONDA_N_HW)
|
||||
test_spam_can_buses(self, TX_MSGS)
|
||||
hw_type = self.safety.get_honda_hw()
|
||||
if hw_type == HONDA_N_HW:
|
||||
tx_msgs = N_TX_MSGS
|
||||
elif hw_type == HONDA_BH_HW:
|
||||
tx_msgs = BH_TX_MSGS
|
||||
elif hw_type == HONDA_BG_HW:
|
||||
tx_msgs = BG_TX_MSGS
|
||||
StdTest.test_spam_can_buses(self, tx_msgs)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
test_relay_malfunction(self, 0xE4)
|
||||
hw = self.safety.get_honda_hw()
|
||||
bus = 2 if hw == HONDA_BG_HW else 0
|
||||
StdTest.test_relay_malfunction(self, 0xE4, bus=bus)
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
test_manually_enable_controls_allowed(self)
|
||||
StdTest.test_manually_enable_controls_allowed(self)
|
||||
|
||||
def test_resume_button(self):
|
||||
RESUME_BTN = 4
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._button_msg(RESUME_BTN, 0x1A6))
|
||||
self.safety.safety_rx_hook(self._button_msg(RESUME_BTN, 0x296))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_set_button(self):
|
||||
SET_BTN = 3
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6))
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_cancel_button(self):
|
||||
CANCEL_BTN = 2
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN, 0x1A6))
|
||||
self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN, 0x296))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_sample_speed(self):
|
||||
@@ -132,9 +145,9 @@ class TestHondaSafety(unittest.TestCase):
|
||||
self.assertEqual(1, self.safety.get_honda_moving())
|
||||
|
||||
def test_prev_brake(self):
|
||||
self.assertFalse(self.safety.get_honda_brake_pressed_prev())
|
||||
self.assertFalse(self.safety.get_brake_pressed_prev())
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.assertTrue(self.safety.get_honda_brake_pressed_prev())
|
||||
self.assertTrue(self.safety.get_brake_pressed_prev())
|
||||
|
||||
def test_disengage_on_brake(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
@@ -152,29 +165,15 @@ class TestHondaSafety(unittest.TestCase):
|
||||
self.safety.safety_rx_hook(self._alt_brake_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_allow_brake_at_zero_speed(self):
|
||||
# Brake was already pressed
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.safety.set_controls_allowed(1)
|
||||
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._brake_msg(False)) # reset no brakes
|
||||
|
||||
def test_not_allow_brake_when_moving(self):
|
||||
# Brake was already pressed
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.safety.safety_rx_hook(self._speed_msg(100))
|
||||
self.safety.set_controls_allowed(1)
|
||||
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
def test_brake_disengage(self):
|
||||
StdTest.test_allow_brake_at_zero_speed(self)
|
||||
StdTest.test_not_allow_brake_when_moving(self, 0)
|
||||
|
||||
def test_prev_gas(self):
|
||||
self.safety.safety_rx_hook(self._gas_msg(False))
|
||||
self.assertFalse(self.safety.get_honda_gas_prev())
|
||||
self.assertFalse(self.safety.get_gas_pressed_prev())
|
||||
self.safety.safety_rx_hook(self._gas_msg(True))
|
||||
self.assertTrue(self.safety.get_honda_gas_prev())
|
||||
self.assertTrue(self.safety.get_gas_pressed_prev())
|
||||
|
||||
def test_prev_gas_interceptor(self):
|
||||
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
|
||||
@@ -215,29 +214,32 @@ class TestHondaSafety(unittest.TestCase):
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
|
||||
def test_brake_safety_check(self):
|
||||
for fwd_brake in [False, True]:
|
||||
self.safety.set_honda_fwd_brake(fwd_brake)
|
||||
for brake in np.arange(0, MAX_BRAKE + 10, 1):
|
||||
for controls_allowed in [True, False]:
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
if fwd_brake:
|
||||
send = False # block openpilot brake msg when fwd'ing stock msg
|
||||
elif controls_allowed:
|
||||
send = MAX_BRAKE >= brake >= 0
|
||||
else:
|
||||
send = brake == 0
|
||||
self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake)))
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
hw = self.safety.get_honda_hw()
|
||||
if hw == HONDA_N_HW:
|
||||
for fwd_brake in [False, True]:
|
||||
self.safety.set_honda_fwd_brake(fwd_brake)
|
||||
for brake in np.arange(0, MAX_BRAKE + 10, 1):
|
||||
for controls_allowed in [True, False]:
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
if fwd_brake:
|
||||
send = False # block openpilot brake msg when fwd'ing stock msg
|
||||
elif controls_allowed:
|
||||
send = MAX_BRAKE >= brake >= 0
|
||||
else:
|
||||
send = brake == 0
|
||||
self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake)))
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
|
||||
def test_gas_interceptor_safety_check(self):
|
||||
for gas in np.arange(0, 4000, 100):
|
||||
for controls_allowed in [True, False]:
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
if controls_allowed:
|
||||
send = True
|
||||
else:
|
||||
send = gas == 0
|
||||
self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200)))
|
||||
if self.safety.get_honda_hw() == HONDA_N_HW:
|
||||
for gas in np.arange(0, 4000, 100):
|
||||
for controls_allowed in [True, False]:
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
if controls_allowed:
|
||||
send = True
|
||||
else:
|
||||
send = gas == 0
|
||||
self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200)))
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
self.safety.set_controls_allowed(0)
|
||||
@@ -245,12 +247,12 @@ class TestHondaSafety(unittest.TestCase):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._send_steer_msg(0x1000)))
|
||||
|
||||
def test_spam_cancel_safety_check(self):
|
||||
RESUME_BTN = 4
|
||||
SET_BTN = 3
|
||||
CANCEL_BTN = 2
|
||||
BUTTON_MSG = 0x296
|
||||
for hw in [HONDA_BG_HW, HONDA_BH_HW]:
|
||||
self.safety.set_honda_hw(hw)
|
||||
hw = self.safety.get_honda_hw()
|
||||
if hw != HONDA_N_HW:
|
||||
RESUME_BTN = 4
|
||||
SET_BTN = 3
|
||||
CANCEL_BTN = 2
|
||||
BUTTON_MSG = 0x296
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN, BUTTON_MSG)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
|
||||
@@ -260,12 +262,16 @@ class TestHondaSafety(unittest.TestCase):
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
|
||||
|
||||
def test_rx_hook(self):
|
||||
|
||||
# checksum checks
|
||||
SET_BTN = 3
|
||||
for msg in ["btn1", "btn2", "gas", "speed"]:
|
||||
self.safety.set_controls_allowed(1)
|
||||
if msg == "btn1":
|
||||
to_push = self._button_msg(SET_BTN, 0x1A6)
|
||||
if self.safety.get_honda_hw() == HONDA_N_HW:
|
||||
to_push = self._button_msg(SET_BTN, 0x1A6) # only in Honda_NIDEC
|
||||
else:
|
||||
continue
|
||||
if msg == "btn2":
|
||||
to_push = self._button_msg(SET_BTN, 0x296)
|
||||
if msg == "gas":
|
||||
@@ -273,23 +279,23 @@ class TestHondaSafety(unittest.TestCase):
|
||||
if msg == "speed":
|
||||
to_push = self._speed_msg(0)
|
||||
self.assertTrue(self.safety.safety_rx_hook(to_push))
|
||||
to_push[0].RDHR = 0
|
||||
to_push[0].RDHR = 0 # invalidate checksum
|
||||
self.assertFalse(self.safety.safety_rx_hook(to_push))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# counter
|
||||
# reset wrong_counters to zero by sending valid messages
|
||||
for i in range(MAX_WRONG_COUNTERS + 1):
|
||||
self.cnt_speed = 0
|
||||
self.cnt_gas = 0
|
||||
self.cnt_button = 0
|
||||
self.__class__.cnt_speed += 1
|
||||
self.__class__.cnt_gas += 1
|
||||
self.__class__.cnt_button += 1
|
||||
if i < MAX_WRONG_COUNTERS:
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6))
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296))
|
||||
self.safety.safety_rx_hook(self._speed_msg(0))
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
else:
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6)))
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)))
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._speed_msg(0)))
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._gas_msg(0)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
@@ -297,10 +303,10 @@ class TestHondaSafety(unittest.TestCase):
|
||||
# restore counters for future tests with a couple of good messages
|
||||
for i in range(2):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6))
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296))
|
||||
self.safety.safety_rx_hook(self._speed_msg(0))
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6))
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
|
||||
@@ -309,8 +315,6 @@ class TestHondaSafety(unittest.TestCase):
|
||||
msgs = list(range(0x1, 0x800))
|
||||
fwd_brake = [False, True]
|
||||
|
||||
self.safety.set_honda_hw(HONDA_N_HW)
|
||||
|
||||
for f in fwd_brake:
|
||||
self.safety.set_honda_fwd_brake(f)
|
||||
blocked_msgs = [0xE4, 0x194, 0x33D]
|
||||
@@ -332,5 +336,43 @@ class TestHondaSafety(unittest.TestCase):
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
|
||||
|
||||
class TestHondaBoschGiraffeSafety(TestHondaSafety):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
TestHondaSafety.setUp()
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 0)
|
||||
cls.safety.init_tests_honda()
|
||||
|
||||
def test_fwd_hook(self):
|
||||
buss = range(0x0, 0x3)
|
||||
msgs = range(0x1, 0x800)
|
||||
hw = self.safety.get_honda_hw()
|
||||
bus_rdr_cam = 2 if hw == HONDA_BH_HW else 1
|
||||
bus_rdr_car = 0 if hw == HONDA_BH_HW else 2
|
||||
bus_pt = 1 if hw == HONDA_BH_HW else 0
|
||||
|
||||
blocked_msgs = [0xE4, 0x33D]
|
||||
for b in buss:
|
||||
for m in msgs:
|
||||
if b == bus_pt:
|
||||
fwd_bus = -1
|
||||
elif b == bus_rdr_cam:
|
||||
fwd_bus = -1 if m in blocked_msgs else bus_rdr_car
|
||||
elif b == bus_rdr_car:
|
||||
fwd_bus = bus_rdr_cam
|
||||
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
|
||||
class TestHondaBoschHarnessSafety(TestHondaBoschGiraffeSafety):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
TestHondaBoschGiraffeSafety.setUp()
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_HARNESS, 0)
|
||||
cls.safety.init_tests_honda()
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
||||
@@ -1,50 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import make_msg, test_spam_can_buses
|
||||
from panda.tests.safety.test_honda import HONDA_BG_HW, HONDA_BH_HW
|
||||
|
||||
MAX_BRAKE = 255
|
||||
|
||||
H_TX_MSGS = [[0xE4, 0], [0x296, 1], [0x33D, 0]] # Bosch Harness
|
||||
G_TX_MSGS = [[0xE4, 2], [0x296, 0], [0x33D, 2]] # Bosch Giraffe
|
||||
|
||||
|
||||
class TestHondaSafety(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 0)
|
||||
cls.safety.init_tests_honda()
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
for hw in [HONDA_BG_HW, HONDA_BH_HW]:
|
||||
self.safety.set_honda_hw(hw)
|
||||
test_spam_can_buses(self, H_TX_MSGS if hw == HONDA_BH_HW else G_TX_MSGS)
|
||||
|
||||
def test_fwd_hook(self):
|
||||
buss = range(0x0, 0x3)
|
||||
msgs = range(0x1, 0x800)
|
||||
for hw in [HONDA_BG_HW, HONDA_BH_HW]:
|
||||
self.safety.set_honda_hw(hw)
|
||||
bus_rdr_cam = 2 if hw == HONDA_BH_HW else 1
|
||||
bus_rdr_car = 0 if hw == HONDA_BH_HW else 2
|
||||
bus_pt = 1 if hw == HONDA_BH_HW else 0
|
||||
|
||||
blocked_msgs = [0xE4, 0x33D]
|
||||
for b in buss:
|
||||
for m in msgs:
|
||||
if b == bus_pt:
|
||||
fwd_bus = -1
|
||||
elif b == bus_rdr_cam:
|
||||
fwd_bus = -1 if m in blocked_msgs else bus_rdr_car
|
||||
elif b == bus_rdr_car:
|
||||
fwd_bus = bus_rdr_cam
|
||||
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -3,7 +3,7 @@ import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
|
||||
from panda.tests.safety.common import StdTest, make_msg
|
||||
|
||||
MAX_RATE_UP = 3
|
||||
MAX_RATE_DOWN = 7
|
||||
@@ -15,6 +15,8 @@ RT_INTERVAL = 250000
|
||||
DRIVER_TORQUE_ALLOWANCE = 50;
|
||||
DRIVER_TORQUE_FACTOR = 2;
|
||||
|
||||
SPEED_THRESHOLD = 30 # ~1kph
|
||||
|
||||
TX_MSGS = [[832, 0], [1265, 0]]
|
||||
|
||||
def twos_comp(val, bits):
|
||||
@@ -41,6 +43,22 @@ class TestHyundaiSafety(unittest.TestCase):
|
||||
to_send[0].RDLR = buttons
|
||||
return to_send
|
||||
|
||||
def _gas_msg(self, val):
|
||||
to_send = make_msg(0, 608)
|
||||
to_send[0].RDHR = (val & 0x3) << 30;
|
||||
return to_send
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(0, 916)
|
||||
to_send[0].RDHR = brake << 23;
|
||||
return to_send
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
to_send = make_msg(0, 902)
|
||||
to_send[0].RDLR = speed & 0x3FFF;
|
||||
to_send[0].RDHR = (speed & 0x3FFF) << 16;
|
||||
return to_send
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_hyundai_desired_torque_last(t)
|
||||
self.safety.set_hyundai_rt_torque_last(t)
|
||||
@@ -56,10 +74,10 @@ class TestHyundaiSafety(unittest.TestCase):
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
test_spam_can_buses(self, TX_MSGS)
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
test_relay_malfunction(self, 832)
|
||||
StdTest.test_relay_malfunction(self, 832)
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
@@ -75,7 +93,7 @@ class TestHyundaiSafety(unittest.TestCase):
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
test_manually_enable_controls_allowed(self)
|
||||
StdTest.test_manually_enable_controls_allowed(self)
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 1057)
|
||||
@@ -89,6 +107,17 @@ class TestHyundaiSafety(unittest.TestCase):
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disengage_on_gas(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_brake_disengage(self):
|
||||
StdTest.test_allow_brake_at_zero_speed(self)
|
||||
StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD)
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_hyundai_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
@@ -0,0 +1,198 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import StdTest, make_msg
|
||||
|
||||
ANGLE_MAX_BP = [1.3, 10., 30.]
|
||||
ANGLE_MAX_V = [540., 120., 23.]
|
||||
ANGLE_DELTA_BP = [0., 5., 15.]
|
||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit
|
||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
|
||||
|
||||
TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2]]
|
||||
|
||||
def twos_comp(val, bits):
|
||||
if val >= 0:
|
||||
return val
|
||||
else:
|
||||
return (2**bits) + val
|
||||
|
||||
def sign(a):
|
||||
if a > 0:
|
||||
return 1
|
||||
else:
|
||||
return -1
|
||||
|
||||
|
||||
class TestNissanSafety(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0)
|
||||
cls.safety.init_tests_nissan()
|
||||
|
||||
def _angle_meas_msg(self, angle):
|
||||
to_send = make_msg(0, 0x2)
|
||||
angle = int(angle * -10)
|
||||
t = twos_comp(angle, 16)
|
||||
to_send[0].RDLR = t & 0xFFFF
|
||||
|
||||
return to_send
|
||||
|
||||
def _set_prev_angle(self, t):
|
||||
t = int(t * -100)
|
||||
self.safety.set_nissan_desired_angle_last(t)
|
||||
|
||||
def _angle_meas_msg_array(self, angle):
|
||||
for i in range(6):
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(angle))
|
||||
|
||||
def _lkas_state_msg(self, state):
|
||||
to_send = make_msg(0, 0x1b6)
|
||||
to_send[0].RDHR = (state & 0x1) << 6
|
||||
|
||||
return to_send
|
||||
|
||||
def _lkas_control_msg(self, angle, state):
|
||||
to_send = make_msg(0, 0x169)
|
||||
angle = int((angle - 1310) * -100)
|
||||
to_send[0].RDLR = ((angle & 0x3FC00) >> 10) | ((angle & 0x3FC) << 6) | ((angle & 0x3) << 16)
|
||||
to_send[0].RDHR = ((state & 0x1) << 20)
|
||||
|
||||
return to_send
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
to_send = make_msg(0, 0x29a)
|
||||
speed = int(speed / 0.00555 * 3.6)
|
||||
to_send[0].RDLR = ((speed & 0xFF) << 24) | ((speed & 0xFF00) << 8)
|
||||
|
||||
return to_send
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(1, 0x454)
|
||||
to_send[0].RDLR = ((brake & 0x1) << 23)
|
||||
|
||||
return to_send
|
||||
|
||||
def _send_gas_cmd(self, gas):
|
||||
to_send = make_msg(0, 0x15c)
|
||||
to_send[0].RDHR = ((gas & 0x3fc) << 6) | ((gas & 0x3) << 22)
|
||||
|
||||
return to_send
|
||||
|
||||
def _acc_button_cmd(self, buttons):
|
||||
to_send = make_msg(2, 0x20b)
|
||||
to_send[0].RDLR = (buttons << 8)
|
||||
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_angle_cmd_when_enabled(self):
|
||||
|
||||
# when controls are allowed, angle cmd rate limit is enforced
|
||||
# test 1: no limitations if we stay within limits
|
||||
speeds = [0., 1., 5., 10., 15., 100.]
|
||||
angles = [-300, -100, -10, 0, 10, 100, 300]
|
||||
for a in angles:
|
||||
for s in speeds:
|
||||
max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V)
|
||||
max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
|
||||
angle_lim = np.interp(s, ANGLE_MAX_BP, ANGLE_MAX_V)
|
||||
|
||||
# first test against false positives
|
||||
self._angle_meas_msg_array(a)
|
||||
self.safety.safety_rx_hook(self._speed_msg(s))
|
||||
|
||||
self._set_prev_angle(np.clip(a, -angle_lim, angle_lim))
|
||||
self.safety.set_controls_allowed(1)
|
||||
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(
|
||||
np.clip(a + sign(a) * max_delta_up, -angle_lim, angle_lim), 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(
|
||||
self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(
|
||||
np.clip(a - sign(a) * max_delta_down, -angle_lim, angle_lim), 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
# now inject too high rates
|
||||
self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) *
|
||||
(max_delta_up + 1), 1)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._set_prev_angle(np.clip(a, -angle_lim, angle_lim))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(
|
||||
self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a - sign(a) *
|
||||
(max_delta_down + 1), 1)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# Check desired steer should be the same as steer angle when controls are off
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 0)))
|
||||
|
||||
def test_angle_cmd_when_disabled(self):
|
||||
self.safety.set_controls_allowed(0)
|
||||
|
||||
self._set_prev_angle(0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._lkas_control_msg(0, 1)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_brake_disengage(self):
|
||||
StdTest.test_allow_brake_at_zero_speed(self)
|
||||
StdTest.test_not_allow_brake_when_moving(self, 0)
|
||||
|
||||
def test_gas_rising_edge(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._send_gas_cmd(100))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_acc_buttons(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x2)) # Cancel button
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x1)) # ProPilot button
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x4)) # Follow Distance button
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x8)) # Set button
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x10)) # Res button
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x20)) # No button pressed
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
StdTest.test_relay_malfunction(self, 0x169)
|
||||
|
||||
def test_fwd_hook(self):
|
||||
|
||||
buss = list(range(0x0, 0x3))
|
||||
msgs = list(range(0x1, 0x800))
|
||||
|
||||
blocked_msgs = [0x169,0x2b1,0x4cc]
|
||||
for b in buss:
|
||||
for m in msgs:
|
||||
if b == 0:
|
||||
fwd_bus = 2
|
||||
elif b == 1:
|
||||
fwd_bus = -1
|
||||
elif b == 2:
|
||||
fwd_bus = -1 if m in blocked_msgs else 0
|
||||
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -3,7 +3,7 @@ import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
|
||||
from panda.tests.safety.common import StdTest, make_msg
|
||||
|
||||
MAX_RATE_UP = 50
|
||||
MAX_RATE_DOWN = 70
|
||||
@@ -15,7 +15,10 @@ RT_INTERVAL = 250000
|
||||
DRIVER_TORQUE_ALLOWANCE = 60;
|
||||
DRIVER_TORQUE_FACTOR = 10;
|
||||
|
||||
TX_MSGS = [[0x122, 0], [0x164, 0], [0x221, 0], [0x322, 0]]
|
||||
SPEED_THRESHOLD = 20 # 1kph (see dbc file)
|
||||
|
||||
TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]]
|
||||
TX_L_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]]
|
||||
|
||||
def twos_comp(val, bits):
|
||||
if val >= 0:
|
||||
@@ -29,7 +32,23 @@ def sign(a):
|
||||
else:
|
||||
return -1
|
||||
|
||||
def subaru_checksum(msg, addr, len_msg):
|
||||
checksum = addr + (addr >> 8)
|
||||
for i in range(len_msg):
|
||||
if i < 4:
|
||||
checksum += (msg.RDLR >> (8 * i))
|
||||
else:
|
||||
checksum += (msg.RDHR >> (8 * (i - 4)))
|
||||
return checksum & 0xff
|
||||
|
||||
|
||||
class TestSubaruSafety(unittest.TestCase):
|
||||
cnt_gas = 0
|
||||
cnt_torque_driver = 0
|
||||
cnt_cruise = 0
|
||||
cnt_speed = 0
|
||||
cnt_brake = 0
|
||||
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
@@ -42,38 +61,105 @@ class TestSubaruSafety(unittest.TestCase):
|
||||
|
||||
def _torque_driver_msg(self, torque):
|
||||
t = twos_comp(torque, 11)
|
||||
to_send = make_msg(0, 0x119)
|
||||
to_send[0].RDLR = ((t & 0x7FF) << 16)
|
||||
if self.safety.get_subaru_global():
|
||||
to_send = make_msg(0, 0x119)
|
||||
to_send[0].RDLR = ((t & 0x7FF) << 16)
|
||||
to_send[0].RDLR |= (self.cnt_torque_driver & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x119, 8)
|
||||
self.__class__.cnt_torque_driver += 1
|
||||
else:
|
||||
to_send = make_msg(0, 0x371)
|
||||
to_send[0].RDLR = (t & 0x7) << 29
|
||||
to_send[0].RDHR = (t >> 3) & 0xFF
|
||||
return to_send
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
speed &= 0x1FFF
|
||||
to_send = make_msg(0, 0x13a)
|
||||
to_send[0].RDLR = speed << 12
|
||||
to_send[0].RDHR = speed << 6
|
||||
to_send[0].RDLR |= (self.cnt_speed & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x13a, 8)
|
||||
self.__class__.cnt_speed += 1
|
||||
return to_send
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(0, 0x139)
|
||||
to_send[0].RDHR = (brake << 4) & 0xFFF
|
||||
to_send[0].RDLR |= (self.cnt_brake & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x139, 8)
|
||||
self.__class__.cnt_brake += 1
|
||||
return to_send
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
to_send = make_msg(0, 0x122)
|
||||
t = twos_comp(torque, 13)
|
||||
to_send[0].RDLR = (t << 16)
|
||||
if self.safety.get_subaru_global():
|
||||
to_send = make_msg(0, 0x122)
|
||||
to_send[0].RDLR = (t << 16)
|
||||
else:
|
||||
to_send = make_msg(0, 0x164)
|
||||
to_send[0].RDLR = (t << 8)
|
||||
return to_send
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
if self.safety.get_subaru_global():
|
||||
to_send = make_msg(0, 0x40)
|
||||
to_send[0].RDHR = gas & 0xFF
|
||||
to_send[0].RDLR |= (self.cnt_gas & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x40, 8)
|
||||
self.__class__.cnt_gas += 1
|
||||
else:
|
||||
to_send = make_msg(0, 0x140)
|
||||
to_send[0].RDLR = gas & 0xFF
|
||||
return to_send
|
||||
|
||||
def _cruise_msg(self, cruise):
|
||||
if self.safety.get_subaru_global():
|
||||
to_send = make_msg(0, 0x240)
|
||||
to_send[0].RDHR = cruise << 9
|
||||
to_send[0].RDLR |= (self.cnt_cruise & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x240, 8)
|
||||
self.__class__.cnt_cruise += 1
|
||||
else:
|
||||
to_send = make_msg(0, 0x144)
|
||||
to_send[0].RDHR = cruise << 17
|
||||
return to_send
|
||||
|
||||
def _set_torque_driver(self, min_t, max_t):
|
||||
for i in range(0, 5):
|
||||
self.safety.safety_rx_hook(self._torque_driver_msg(min_t))
|
||||
self.safety.safety_rx_hook(self._torque_driver_msg(max_t))
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
test_spam_can_buses(self, TX_MSGS)
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS if self.safety.get_subaru_global() else TX_L_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
test_relay_malfunction(self, 0x122)
|
||||
StdTest.test_relay_malfunction(self, 0x122 if self.safety.get_subaru_global() else 0x164)
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 0x240)
|
||||
to_push[0].RDHR = 1 << 9
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.safety.safety_rx_hook(self._cruise_msg(True))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 0x240)
|
||||
to_push[0].RDHR = 0
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.safety.safety_rx_hook(self._cruise_msg(False))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disengage_on_gas(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_brake_disengage(self):
|
||||
if (self.safety.get_subaru_global()):
|
||||
StdTest.test_allow_brake_at_zero_speed(self)
|
||||
StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD)
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
for t in range(-3000, 3000):
|
||||
@@ -85,10 +171,10 @@ class TestSubaruSafety(unittest.TestCase):
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
test_manually_enable_controls_allowed(self)
|
||||
StdTest.test_manually_enable_controls_allowed(self)
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_subaru_torque_driver(0, 0)
|
||||
self._set_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
self._set_prev_torque(0)
|
||||
@@ -103,7 +189,7 @@ class TestSubaruSafety(unittest.TestCase):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_subaru_torque_driver(0, 0)
|
||||
self._set_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
def test_against_torque_driver(self):
|
||||
@@ -112,33 +198,36 @@ class TestSubaruSafety(unittest.TestCase):
|
||||
for sign in [-1, 1]:
|
||||
for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1):
|
||||
t *= -sign
|
||||
self.safety.set_subaru_torque_driver(t, t)
|
||||
self._set_torque_driver(t, t)
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign)))
|
||||
|
||||
self.safety.set_subaru_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self._set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER)))
|
||||
|
||||
# arbitrary high driver torque to ensure max steer torque is allowed
|
||||
max_driver_torque = int(MAX_STEER / DRIVER_TORQUE_FACTOR + DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
|
||||
# spot check some individual cases
|
||||
for sign in [-1, 1]:
|
||||
driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign
|
||||
torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign
|
||||
delta = 1 * sign
|
||||
self._set_prev_torque(torque_desired)
|
||||
self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque)
|
||||
self._set_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
|
||||
self._set_prev_torque(torque_desired + delta)
|
||||
self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque)
|
||||
self._set_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
|
||||
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||
|
||||
|
||||
@@ -148,7 +237,7 @@ class TestSubaruSafety(unittest.TestCase):
|
||||
for sign in [-1, 1]:
|
||||
self.safety.init_tests_subaru()
|
||||
self._set_prev_torque(0)
|
||||
self.safety.set_subaru_torque_driver(0, 0)
|
||||
self._set_torque_driver(0, 0)
|
||||
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||
t *= sign
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
@@ -168,7 +257,7 @@ class TestSubaruSafety(unittest.TestCase):
|
||||
def test_fwd_hook(self):
|
||||
buss = list(range(0x0, 0x3))
|
||||
msgs = list(range(0x1, 0x800))
|
||||
blocked_msgs = [290, 356, 545, 802]
|
||||
blocked_msgs = [290, 545, 802] if self.safety.get_subaru_global() else [356, 545, 802]
|
||||
for b in buss:
|
||||
for m in msgs:
|
||||
if b == 0:
|
||||
@@ -181,6 +270,12 @@ class TestSubaruSafety(unittest.TestCase):
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
class TestSubaruLegacySafety(TestSubaruSafety):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU_LEGACY, 0)
|
||||
cls.safety.init_tests_subaru()
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
||||
@@ -3,7 +3,7 @@ import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
|
||||
from panda.tests.safety.common import StdTest, make_msg
|
||||
|
||||
MAX_RATE_UP = 10
|
||||
MAX_RATE_DOWN = 25
|
||||
@@ -15,6 +15,8 @@ MIN_ACCEL = -3000
|
||||
MAX_RT_DELTA = 375
|
||||
RT_INTERVAL = 250000
|
||||
|
||||
STANDSTILL_THRESHOLD = 100 # 1kph
|
||||
|
||||
MAX_TORQUE_ERROR = 350
|
||||
INTERCEPTOR_THRESHOLD = 475
|
||||
|
||||
@@ -62,7 +64,7 @@ class TestToyotaSafety(unittest.TestCase):
|
||||
t = twos_comp(torque, 16)
|
||||
to_send = make_msg(0, 0x260)
|
||||
to_send[0].RDHR = (t & 0xff00) | ((t & 0xFF) << 16)
|
||||
to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24)
|
||||
to_send[0].RDHR |= toyota_checksum(to_send[0], 0x260, 8) << 24
|
||||
return to_send
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
@@ -77,6 +79,21 @@ class TestToyotaSafety(unittest.TestCase):
|
||||
to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8)
|
||||
return to_send
|
||||
|
||||
def _speed_msg(self, s):
|
||||
offset = (0x6f << 8) + 0x1a # there is a 0x1a6f offset in the signal
|
||||
to_send = make_msg(0, 0xaa)
|
||||
to_send[0].RDLR = ((s & 0xFF) << 8 | (s >> 8)) + offset
|
||||
to_send[0].RDLR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16)
|
||||
to_send[0].RDHR = ((s & 0xFF) << 8 | (s >> 8)) + offset
|
||||
to_send[0].RDHR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16)
|
||||
return to_send
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(0, 0x226)
|
||||
to_send[0].RDHR = brake << 5
|
||||
to_send[0].RDHR |= toyota_checksum(to_send[0], 0x226, 8) << 24
|
||||
return to_send
|
||||
|
||||
def _send_gas_msg(self, gas):
|
||||
to_send = make_msg(0, 0x2C1)
|
||||
to_send[0].RDHR = (gas & 0xFF) << 16
|
||||
@@ -96,16 +113,16 @@ class TestToyotaSafety(unittest.TestCase):
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
test_spam_can_buses(self, TX_MSGS)
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
test_relay_malfunction(self, 0x2E4)
|
||||
StdTest.test_relay_malfunction(self, 0x2E4)
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
test_manually_enable_controls_allowed(self)
|
||||
StdTest.test_manually_enable_controls_allowed(self)
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
self.safety.safety_rx_hook(self._pcm_cruise_msg(False))
|
||||
@@ -121,7 +138,7 @@ class TestToyotaSafety(unittest.TestCase):
|
||||
def test_prev_gas(self):
|
||||
for g in range(0, 256):
|
||||
self.safety.safety_rx_hook(self._send_gas_msg(g))
|
||||
self.assertEqual(g, self.safety.get_toyota_gas_prev())
|
||||
self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev())
|
||||
|
||||
def test_prev_gas_interceptor(self):
|
||||
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
|
||||
@@ -155,6 +172,10 @@ class TestToyotaSafety(unittest.TestCase):
|
||||
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
|
||||
def test_brake_disengage(self):
|
||||
StdTest.test_allow_brake_at_zero_speed(self)
|
||||
StdTest.test_not_allow_brake_when_moving(self, STANDSTILL_THRESHOLD)
|
||||
|
||||
def test_allow_engage_with_gas_interceptor_pressed(self):
|
||||
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
|
||||
self.safety.set_controls_allowed(1)
|
||||
@@ -304,6 +325,5 @@ class TestToyotaSafety(unittest.TestCase):
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
||||
@@ -1,243 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import make_msg
|
||||
from panda.tests.safety.test_toyota import toyota_checksum
|
||||
|
||||
IPAS_OVERRIDE_THRESHOLD = 200
|
||||
|
||||
ANGLE_DELTA_BP = [0., 5., 15.]
|
||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit
|
||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
|
||||
|
||||
def twos_comp(val, bits):
|
||||
if val >= 0:
|
||||
return val
|
||||
else:
|
||||
return (2**bits) + val
|
||||
|
||||
def sign(a):
|
||||
if a > 0:
|
||||
return 1
|
||||
else:
|
||||
return -1
|
||||
|
||||
|
||||
class TestToyotaSafety(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA_IPAS, 66)
|
||||
cls.safety.init_tests_toyota()
|
||||
|
||||
def _torque_driver_msg(self, torque):
|
||||
to_send = make_msg(0, 0x260)
|
||||
t = twos_comp(torque, 16)
|
||||
to_send[0].RDLR = t | ((t & 0xFF) << 16)
|
||||
to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24)
|
||||
return to_send
|
||||
|
||||
def _torque_driver_msg_array(self, torque):
|
||||
for i in range(6):
|
||||
self.safety.safety_rx_hook(self._torque_driver_msg(torque))
|
||||
|
||||
def _angle_meas_msg(self, angle):
|
||||
to_send = make_msg(0, 0x25)
|
||||
t = twos_comp(angle, 12)
|
||||
to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
|
||||
return to_send
|
||||
|
||||
def _angle_meas_msg_array(self, angle):
|
||||
for i in range(6):
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(angle))
|
||||
|
||||
def _ipas_state_msg(self, state):
|
||||
to_send = make_msg(0, 0x262)
|
||||
to_send[0].RDLR = state & 0xF
|
||||
return to_send
|
||||
|
||||
def _ipas_control_msg(self, angle, state):
|
||||
# note: we command 2/3 of the angle due to CAN conversion
|
||||
to_send = make_msg(0, 0x266)
|
||||
t = twos_comp(angle, 12)
|
||||
to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
|
||||
to_send[0].RDLR |= ((state & 0xf) << 4)
|
||||
|
||||
return to_send
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
to_send = make_msg(0, 0xB4)
|
||||
speed = int(speed * 100 * 3.6)
|
||||
|
||||
to_send[0].RDHR = ((speed & 0xFF) << 16) | (speed & 0xFF00)
|
||||
return to_send
|
||||
|
||||
def test_ipas_override(self):
|
||||
|
||||
## angle control is not active
|
||||
self.safety.set_controls_allowed(1)
|
||||
|
||||
# 3 consecutive msgs where driver exceeds threshold but angle_control isn't active
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
# ipas state is override
|
||||
self.safety.safety_rx_hook(self._ipas_state_msg(5))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
## now angle control is active
|
||||
self.safety.safety_tx_hook(self._ipas_control_msg(0, 0))
|
||||
self.safety.safety_rx_hook(self._ipas_state_msg(0))
|
||||
|
||||
# 3 consecutive msgs where driver does exceed threshold
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# ipas state is override and torque isn't overriding any more
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._torque_driver_msg_array(0)
|
||||
self.safety.safety_rx_hook(self._ipas_state_msg(5))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# 3 consecutive msgs where driver does not exceed threshold and
|
||||
# ipas state is not override
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._ipas_state_msg(0))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_cmd_when_disabled(self):
|
||||
|
||||
self.safety.set_controls_allowed(0)
|
||||
|
||||
# test angle cmd too far from actual
|
||||
angle_refs = [-10, 10]
|
||||
deltas = list(range(-2, 3))
|
||||
expected_results = [False, True, True, True, False]
|
||||
|
||||
for a in angle_refs:
|
||||
self._angle_meas_msg_array(a)
|
||||
for i, d in enumerate(deltas):
|
||||
self.assertEqual(expected_results[i], self.safety.safety_tx_hook(self._ipas_control_msg(a + d, 1)))
|
||||
|
||||
# test ipas state cmd enabled
|
||||
self._angle_meas_msg_array(0)
|
||||
self.assertEqual(0, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3)))
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_cmd_when_enabled(self):
|
||||
|
||||
# ipas angle cmd should pass through when controls are enabled
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._angle_meas_msg_array(0)
|
||||
self.safety.safety_rx_hook(self._speed_msg(0.1))
|
||||
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1)))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(4, 1)))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3)))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-4, 3)))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-8, 3)))
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_cmd_rate_when_disabled(self):
|
||||
|
||||
# as long as the command is close to the measured, no rate limit is enforced when
|
||||
# controls are disabled
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(0))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1)))
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(100))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(100, 1)))
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(-100))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-100, 1)))
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_cmd_rate_when_enabled(self):
|
||||
|
||||
# when controls are allowed, angle cmd rate limit is enforced
|
||||
# test 1: no limitations if we stay within limits
|
||||
speeds = [0., 1., 5., 10., 15., 100.]
|
||||
angles = [-300, -100, -10, 0, 10, 100, 300]
|
||||
for a in angles:
|
||||
for s in speeds:
|
||||
|
||||
# first test against false positives
|
||||
self._angle_meas_msg_array(a)
|
||||
self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._speed_msg(s))
|
||||
max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.)
|
||||
max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.)
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) * max_delta_down, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
# now inject too high rates
|
||||
self.assertEqual(False, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) *
|
||||
(max_delta_up + 1), 1)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(False, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) *
|
||||
(max_delta_down + 1), 1)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_measured_rate(self):
|
||||
|
||||
speeds = [0., 1., 5., 10., 15., 100.]
|
||||
angles = [-300, -100, -10, 0, 10, 100, 300]
|
||||
angles = [10]
|
||||
for a in angles:
|
||||
for s in speeds:
|
||||
self._angle_meas_msg_array(a)
|
||||
self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._speed_msg(s))
|
||||
#max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.)
|
||||
#max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.)
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(a))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(a + 150))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -1,226 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
|
||||
|
||||
MAX_RATE_UP = 4
|
||||
MAX_RATE_DOWN = 10
|
||||
MAX_STEER = 250
|
||||
|
||||
MAX_RT_DELTA = 75
|
||||
RT_INTERVAL = 250000
|
||||
|
||||
DRIVER_TORQUE_ALLOWANCE = 80
|
||||
DRIVER_TORQUE_FACTOR = 3
|
||||
|
||||
TX_MSGS = [[0x126, 0], [0x12B, 0], [0x12B, 2], [0x397, 0]]
|
||||
|
||||
def sign(a):
|
||||
if a > 0:
|
||||
return 1
|
||||
else:
|
||||
return -1
|
||||
|
||||
class TestVolkswagenSafety(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN, 0)
|
||||
cls.safety.init_tests_volkswagen()
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_volkswagen_desired_torque_last(t)
|
||||
self.safety.set_volkswagen_rt_torque_last(t)
|
||||
|
||||
def _torque_driver_msg(self, torque):
|
||||
to_send = make_msg(0, 0x9F)
|
||||
t = abs(torque)
|
||||
to_send[0].RDHR = ((t & 0x1FFF) << 8)
|
||||
if torque < 0:
|
||||
to_send[0].RDHR |= 0x1 << 23
|
||||
return to_send
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
to_send = make_msg(0, 0x126)
|
||||
t = abs(torque)
|
||||
to_send[0].RDLR = (t & 0xFFF) << 16
|
||||
if torque < 0:
|
||||
to_send[0].RDLR |= 0x1 << 31
|
||||
return to_send
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
to_send = make_msg(0, 0x121)
|
||||
to_send[0].RDLR = (gas & 0xFF) << 12
|
||||
return to_send
|
||||
|
||||
def _button_msg(self, bit):
|
||||
to_send = make_msg(2, 0x12B)
|
||||
to_send[0].RDLR = 1 << bit
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
test_relay_malfunction(self, 0x126)
|
||||
|
||||
def test_prev_gas(self):
|
||||
for g in range(0, 256):
|
||||
self.safety.safety_rx_hook(self._gas_msg(g))
|
||||
self.assertEqual(g, self.safety.get_volkswagen_gas_prev())
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 0x122)
|
||||
to_push[0].RDHR = 0x30000000
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 0x122)
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disengage_on_gas(self):
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_allow_engage_with_gas_pressed(self):
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
for t in range(-500, 500):
|
||||
self.safety.set_controls_allowed(enabled)
|
||||
self._set_prev_torque(t)
|
||||
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
else:
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
test_manually_enable_controls_allowed(self)
|
||||
|
||||
def test_spam_cancel_safety_check(self):
|
||||
BIT_CANCEL = 13
|
||||
BIT_RESUME = 19
|
||||
BIT_SET = 16
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(BIT_CANCEL)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(BIT_RESUME)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(BIT_SET)))
|
||||
# do not block resume if we are engaged already
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(BIT_RESUME)))
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP)))
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP)))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
def test_against_torque_driver(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1):
|
||||
t *= -sign
|
||||
self.safety.set_volkswagen_torque_driver(t, t)
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign)))
|
||||
|
||||
self.safety.set_volkswagen_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER)))
|
||||
|
||||
# spot check some individual cases
|
||||
for sign in [-1, 1]:
|
||||
driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign
|
||||
torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign
|
||||
delta = 1 * sign
|
||||
self._set_prev_torque(torque_desired)
|
||||
self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
|
||||
self._set_prev_torque(torque_desired + delta)
|
||||
self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
|
||||
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||
|
||||
|
||||
def test_realtime_limits(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
self.safety.init_tests_volkswagen()
|
||||
self._set_prev_torque(0)
|
||||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||
t *= sign
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||
t *= sign
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
|
||||
# Increase timer to update rt_torque_last
|
||||
self.safety.set_timer(RT_INTERVAL + 1)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
|
||||
def test_fwd_hook(self):
|
||||
buss = list(range(0x0, 0x3))
|
||||
msgs = list(range(0x1, 0x800))
|
||||
blocked_msgs_0to2 = []
|
||||
blocked_msgs_2to0 = [0x126, 0x397]
|
||||
for b in buss:
|
||||
for m in msgs:
|
||||
if b == 0:
|
||||
fwd_bus = -1 if m in blocked_msgs_0to2 else 2
|
||||
elif b == 1:
|
||||
fwd_bus = -1
|
||||
elif b == 2:
|
||||
fwd_bus = -1 if m in blocked_msgs_2to0 else 0
|
||||
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,397 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
import numpy as np
|
||||
import crcmod
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS
|
||||
|
||||
MAX_RATE_UP = 4
|
||||
MAX_RATE_DOWN = 10
|
||||
MAX_STEER = 300
|
||||
MAX_RT_DELTA = 75
|
||||
RT_INTERVAL = 250000
|
||||
|
||||
DRIVER_TORQUE_ALLOWANCE = 80
|
||||
DRIVER_TORQUE_FACTOR = 3
|
||||
|
||||
MSG_ESP_19 = 0xB2 # RX from ABS, for wheel speeds
|
||||
MSG_EPS_01 = 0x9F # RX from EPS, for driver steering torque
|
||||
MSG_ESP_05 = 0x106 # RX from ABS, for brake light state
|
||||
MSG_TSK_06 = 0x120 # RX from ECU, for ACC status from drivetrain coordinator
|
||||
MSG_MOTOR_20 = 0x121 # RX from ECU, for driver throttle input
|
||||
MSG_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque
|
||||
MSG_GRA_ACC_01 = 0x12B # TX by OP, ACC control buttons for cancel/resume
|
||||
MSG_LDW_02 = 0x397 # TX by OP, Lane line recognition and text alerts
|
||||
|
||||
# Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
|
||||
TX_MSGS = [[MSG_HCA_01, 0], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2], [MSG_LDW_02, 0]]
|
||||
|
||||
def sign(a):
|
||||
if a > 0:
|
||||
return 1
|
||||
else:
|
||||
return -1
|
||||
|
||||
# Python crcmod works differently somehow from every other CRC calculator. The
|
||||
# implied leading 1 on the polynomial isn't a problem, but to get the right
|
||||
# result for CRC-8H2F/AUTOSAR, we have to feed it initCrc 0x00 instead of 0xFF.
|
||||
volkswagen_crc_8h2f = crcmod.mkCrcFun(0x12F, initCrc=0x00, rev=False, xorOut=0xFF)
|
||||
|
||||
def volkswagen_mqb_crc(msg, addr, len_msg):
|
||||
# This is CRC-8H2F/AUTOSAR with a twist. See the OpenDBC implementation of
|
||||
# this algorithm for a version with explanatory comments.
|
||||
msg_bytes = msg.RDLR.to_bytes(4, 'little') + msg.RDHR.to_bytes(4, 'little')
|
||||
counter = (msg.RDLR & 0xF00) >> 8
|
||||
if addr == MSG_EPS_01:
|
||||
magic_pad = b'\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5'[counter]
|
||||
elif addr == MSG_ESP_05:
|
||||
magic_pad = b'\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07'[counter]
|
||||
elif addr == MSG_TSK_06:
|
||||
magic_pad = b'\xC4\xE2\x4F\xE4\xF8\x2F\x56\x81\x9F\xE5\x83\x44\x05\x3F\x97\xDF'[counter]
|
||||
elif addr == MSG_MOTOR_20:
|
||||
magic_pad = b'\xE9\x65\xAE\x6B\x7B\x35\xE5\x5F\x4E\xC7\x86\xA2\xBB\xDD\xEB\xB4'[counter]
|
||||
elif addr == MSG_HCA_01:
|
||||
magic_pad = b'\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA'[counter]
|
||||
elif addr == MSG_GRA_ACC_01:
|
||||
magic_pad = b'\x6A\x38\xB4\x27\x22\xEF\xE1\xBB\xF8\x80\x84\x49\xC7\x9E\x1E\x2B'[counter]
|
||||
else:
|
||||
magic_pad = None
|
||||
return volkswagen_crc_8h2f(msg_bytes[1:len_msg] + magic_pad.to_bytes(1, 'little'))
|
||||
|
||||
class TestVolkswagenMqbSafety(unittest.TestCase):
|
||||
cnt_eps_01 = 0
|
||||
cnt_esp_05 = 0
|
||||
cnt_tsk_06 = 0
|
||||
cnt_motor_20 = 0
|
||||
cnt_hca_01 = 0
|
||||
cnt_gra_acc_01 = 0
|
||||
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_MQB, 0)
|
||||
cls.safety.init_tests_volkswagen()
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_volkswagen_desired_torque_last(t)
|
||||
self.safety.set_volkswagen_rt_torque_last(t)
|
||||
|
||||
# Wheel speeds _esp_19_msg
|
||||
def _speed_msg(self, speed):
|
||||
wheel_speed_scaled = int(speed / 0.0075)
|
||||
to_send = make_msg(0, MSG_ESP_19)
|
||||
to_send[0].RDLR = wheel_speed_scaled | (wheel_speed_scaled << 16)
|
||||
to_send[0].RDHR = wheel_speed_scaled | (wheel_speed_scaled << 16)
|
||||
return to_send
|
||||
|
||||
# Brake light switch _esp_05_msg
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(0, MSG_ESP_05)
|
||||
to_send[0].RDLR = (0x1 << 26) if brake else 0
|
||||
to_send[0].RDLR |= (self.cnt_esp_05 % 16) << 8
|
||||
to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_ESP_05, 8)
|
||||
self.__class__.cnt_esp_05 += 1
|
||||
return to_send
|
||||
|
||||
# Driver steering input torque
|
||||
def _eps_01_msg(self, torque):
|
||||
to_send = make_msg(0, MSG_EPS_01)
|
||||
t = abs(torque)
|
||||
to_send[0].RDHR = ((t & 0x1FFF) << 8)
|
||||
if torque < 0:
|
||||
to_send[0].RDHR |= 0x1 << 23
|
||||
to_send[0].RDLR |= (self.cnt_eps_01 % 16) << 8
|
||||
to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_EPS_01, 8)
|
||||
self.__class__.cnt_eps_01 += 1
|
||||
return to_send
|
||||
|
||||
# openpilot steering output torque
|
||||
def _hca_01_msg(self, torque):
|
||||
to_send = make_msg(0, MSG_HCA_01)
|
||||
t = abs(torque)
|
||||
to_send[0].RDLR = (t & 0xFFF) << 16
|
||||
if torque < 0:
|
||||
to_send[0].RDLR |= 0x1 << 31
|
||||
to_send[0].RDLR |= (self.cnt_hca_01 % 16) << 8
|
||||
to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_HCA_01, 8)
|
||||
self.__class__.cnt_hca_01 += 1
|
||||
return to_send
|
||||
|
||||
# ACC engagement status
|
||||
def _tsk_06_msg(self, status):
|
||||
to_send = make_msg(0, MSG_TSK_06)
|
||||
to_send[0].RDLR = (status & 0x7) << 24
|
||||
to_send[0].RDLR |= (self.cnt_tsk_06 % 16) << 8
|
||||
to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_TSK_06, 8)
|
||||
self.__class__.cnt_tsk_06 += 1
|
||||
return to_send
|
||||
|
||||
# Driver throttle input
|
||||
def _motor_20_msg(self, gas):
|
||||
to_send = make_msg(0, MSG_MOTOR_20)
|
||||
to_send[0].RDLR = (gas & 0xFF) << 12
|
||||
to_send[0].RDLR |= (self.cnt_motor_20 % 16) << 8
|
||||
to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_MOTOR_20, 8)
|
||||
self.__class__.cnt_motor_20 += 1
|
||||
return to_send
|
||||
|
||||
# Cruise control buttons
|
||||
def _gra_acc_01_msg(self, bit):
|
||||
to_send = make_msg(2, MSG_GRA_ACC_01)
|
||||
to_send[0].RDLR = 1 << bit
|
||||
to_send[0].RDLR |= (self.cnt_gra_acc_01 % 16) << 8
|
||||
to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_GRA_ACC_01, 8)
|
||||
self.__class__.cnt_gra_acc_01 += 1
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
StdTest.test_relay_malfunction(self, MSG_HCA_01)
|
||||
|
||||
def test_prev_gas(self):
|
||||
for g in range(0, 256):
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(g))
|
||||
self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev())
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._tsk_06_msg(3))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disable_control_allowed_from_cruise(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._tsk_06_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_sample_speed(self):
|
||||
# Stationary
|
||||
self.safety.safety_rx_hook(self._speed_msg(0))
|
||||
self.assertEqual(0, self.safety.get_volkswagen_moving())
|
||||
# 1 km/h, just under 0.3 m/s safety grace threshold
|
||||
self.safety.safety_rx_hook(self._speed_msg(1))
|
||||
self.assertEqual(0, self.safety.get_volkswagen_moving())
|
||||
# 2 km/h, just over 0.3 m/s safety grace threshold
|
||||
self.safety.safety_rx_hook(self._speed_msg(2))
|
||||
self.assertEqual(1, self.safety.get_volkswagen_moving())
|
||||
# 144 km/h, openpilot V_CRUISE_MAX
|
||||
self.safety.safety_rx_hook(self._speed_msg(144))
|
||||
self.assertEqual(1, self.safety.get_volkswagen_moving())
|
||||
|
||||
def test_prev_brake(self):
|
||||
self.assertFalse(self.safety.get_brake_pressed_prev())
|
||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||
self.assertTrue(self.safety.get_brake_pressed_prev())
|
||||
|
||||
def test_brake_disengage(self):
|
||||
StdTest.test_allow_brake_at_zero_speed(self)
|
||||
StdTest.test_not_allow_brake_when_moving(self, 1)
|
||||
|
||||
def test_disengage_on_gas(self):
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(0))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_allow_engage_with_gas_pressed(self):
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(1))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
for t in range(-500, 500):
|
||||
self.safety.set_controls_allowed(enabled)
|
||||
self._set_prev_torque(t)
|
||||
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(t)))
|
||||
else:
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t)))
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
StdTest.test_manually_enable_controls_allowed(self)
|
||||
|
||||
def test_spam_cancel_safety_check(self):
|
||||
BIT_CANCEL = 13
|
||||
BIT_RESUME = 19
|
||||
BIT_SET = 16
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_CANCEL)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_SET)))
|
||||
# do not block resume if we are engaged already
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME)))
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(MAX_RATE_UP)))
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_RATE_UP)))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(MAX_RATE_UP + 1)))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_RATE_UP - 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
def test_against_torque_driver(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1):
|
||||
t *= -sign
|
||||
self.safety.set_volkswagen_torque_driver(t, t)
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(MAX_STEER * sign)))
|
||||
|
||||
self.safety.set_volkswagen_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_STEER)))
|
||||
|
||||
# spot check some individual cases
|
||||
for sign in [-1, 1]:
|
||||
driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign
|
||||
torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign
|
||||
delta = 1 * sign
|
||||
self._set_prev_torque(torque_desired)
|
||||
self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(torque_desired)))
|
||||
self._set_prev_torque(torque_desired + delta)
|
||||
self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(torque_desired + delta)))
|
||||
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(0)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||
|
||||
def test_realtime_limits(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
self.safety.init_tests_volkswagen()
|
||||
self._set_prev_torque(0)
|
||||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||
t *= sign
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||
t *= sign
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t)))
|
||||
|
||||
# Increase timer to update rt_torque_last
|
||||
self.safety.set_timer(RT_INTERVAL + 1)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
def test_torque_measurements(self):
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(50))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(-50))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
|
||||
self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min())
|
||||
self.assertEqual(50, self.safety.get_volkswagen_torque_driver_max())
|
||||
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max())
|
||||
self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min())
|
||||
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max())
|
||||
self.assertEqual(0, self.safety.get_volkswagen_torque_driver_min())
|
||||
|
||||
def test_rx_hook(self):
|
||||
# checksum checks
|
||||
# TODO: Would be ideal to check ESP_19 as well, but it has no checksum
|
||||
# or counter, and I'm not sure if we can easily validate Panda's simple
|
||||
# temporal reception-rate check here.
|
||||
for msg in [MSG_EPS_01, MSG_ESP_05, MSG_TSK_06, MSG_MOTOR_20]:
|
||||
self.safety.set_controls_allowed(1)
|
||||
if msg == MSG_EPS_01:
|
||||
to_push = self._eps_01_msg(0)
|
||||
if msg == MSG_ESP_05:
|
||||
to_push = self._brake_msg(False)
|
||||
if msg == MSG_TSK_06:
|
||||
to_push = self._tsk_06_msg(3)
|
||||
if msg == MSG_MOTOR_20:
|
||||
to_push = self._motor_20_msg(0)
|
||||
self.assertTrue(self.safety.safety_rx_hook(to_push))
|
||||
to_push[0].RDHR ^= 0xFF
|
||||
self.assertFalse(self.safety.safety_rx_hook(to_push))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# counter
|
||||
# reset wrong_counters to zero by sending valid messages
|
||||
for i in range(MAX_WRONG_COUNTERS + 1):
|
||||
self.__class__.cnt_eps_01 += 1
|
||||
self.__class__.cnt_esp_05 += 1
|
||||
self.__class__.cnt_tsk_06 += 1
|
||||
self.__class__.cnt_motor_20 += 1
|
||||
if i < MAX_WRONG_COUNTERS:
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.safety.safety_rx_hook(self._brake_msg(False))
|
||||
self.safety.safety_rx_hook(self._tsk_06_msg(3))
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(0))
|
||||
else:
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._eps_01_msg(0)))
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._brake_msg(False)))
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._tsk_06_msg(3)))
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._motor_20_msg(0)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# restore counters for future tests with a couple of good messages
|
||||
for i in range(2):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.safety.safety_rx_hook(self._brake_msg(False))
|
||||
self.safety.safety_rx_hook(self._tsk_06_msg(3))
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(0))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_fwd_hook(self):
|
||||
buss = list(range(0x0, 0x3))
|
||||
msgs = list(range(0x1, 0x800))
|
||||
blocked_msgs_0to2 = []
|
||||
blocked_msgs_2to0 = [MSG_HCA_01, MSG_LDW_02]
|
||||
for b in buss:
|
||||
for m in msgs:
|
||||
if b == 0:
|
||||
fwd_bus = -1 if m in blocked_msgs_0to2 else 2
|
||||
elif b == 1:
|
||||
fwd_bus = -1
|
||||
elif b == 2:
|
||||
fwd_bus = -1 if m in blocked_msgs_2to0 else 0
|
||||
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -25,7 +25,8 @@ RUN mkdir /openpilot
|
||||
WORKDIR /openpilot
|
||||
RUN git clone https://github.com/commaai/cereal.git || true
|
||||
WORKDIR /openpilot/cereal
|
||||
RUN git checkout f7043fde062cbfd49ec90af669901a9caba52de9
|
||||
RUN git pull
|
||||
RUN git checkout bb2cc7572de99becce1bfbae63f3b38d5464e162
|
||||
COPY . /openpilot/panda
|
||||
|
||||
WORKDIR /openpilot/panda/tests/safety_replay
|
||||
|
||||
@@ -18,7 +18,8 @@ logs = [
|
||||
("02ec6bea180a4d36|2019-04-17--11-21-35.bz2", Panda.SAFETY_HYUNDAI, 0), # HYUNDAI.SANTA_FE
|
||||
("6fb4948a7ebe670e|2019-11-12--00-35-53.bz2", Panda.SAFETY_CHRYSLER, 0), # CHRYSLER.PACIFICA_2018_HYBRID
|
||||
("791340bc01ed993d|2019-04-08--10-26-00.bz2", Panda.SAFETY_SUBARU, 0), # SUBARU.IMPREZA
|
||||
("b0c9d2329ad1606b|2019-11-17--17-06-13.bz2", Panda.SAFETY_VOLKSWAGEN, 0), # VOLKSWAGEN.GOLF
|
||||
("76b83eb0245de90e|2020-03-05--19-16-05.bz2", Panda.SAFETY_VOLKSWAGEN_MQB, 0), # VOLKSWAGEN.GOLF
|
||||
("fbbfa6af821552b9|2020-03-03--08-09-43.bz2", Panda.SAFETY_NISSAN, 0), # NISSAN.XTRAIL
|
||||
]
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
+1
-1
@@ -3,4 +3,4 @@
|
||||
export LD_LIBRARY_PATH=/data/data/com.termux/files/usr/lib
|
||||
export HOME=/data/data/com.termux/files/home
|
||||
export PATH=/usr/local/bin:/data/data/com.termux/files/usr/bin:/data/data/com.termux/files/usr/sbin:/data/data/com.termux/files/usr/bin/applets:/bin:/sbin:/vendor/bin:/system/sbin:/system/bin:/system/xbin:/data/data/com.termux/files/usr/bin/git
|
||||
cd /data/openpilot && git reset --hard @{u} && git clean -xdf && git pull && reboot
|
||||
cd /data/openpilot && git reset --hard @{u} && git clean -xdf && git pull && scons --clean && reboot
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 2.1 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 29 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 957 B |
Binary file not shown.
|
After Width: | Height: | Size: 416 B |
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user