diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index b4cccc2da..2ccc6b25f 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -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) diff --git a/README.md b/README.md index 42770fe80..1ed8ca60c 100644 --- a/README.md +++ b/README.md @@ -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 | 25mph5 | 25mph | -| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph5 | 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 | 25mph5 | 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 | 25mph5 | 12mph | -| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph5 | 0mph | -| Honda | Passport 2019 | All | openpilot | 25mph5 | 12mph | -| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph5 | 12mph | -| Honda | Pilot 2019 | All | openpilot | 25mph5 | 12mph | -| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph5 | 12mph | -| Hyundai | Elantra 2017-191 | SCC + LKAS | Stock | 19mph | 34mph | -| Hyundai | Genesis 20181 | All | Stock | 19mph | 34mph | -| Hyundai | Santa Fe 20191 | 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 20191 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Sorento 20181 | All | Stock | 0mph | 0mph | -| Kia | Stinger 20181 | SCC + LKAS | Stock | 0mph | 0mph | -| Lexus | CT Hybrid 2017-18 | All | Stock4| 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 | Stock4| 0mph | 0mph | -| Lexus | RX 2016-17 | All | Stock4| 0mph | 0mph | -| Lexus | RX 2020 | All | openpilot | 0mph | 0mph | -| Lexus | RX Hybrid 2016-19 | All | Stock4| 0mph | 0mph | -| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph | -| Toyota | Avalon 2016 | TSS-P | Stock4| 20mph5 | 0mph | -| Toyota | Avalon 2017-18 | All | Stock4| 20mph5 | 0mph | -| Toyota | Camry 2018-19 | All | Stock | 0mph2 | 0mph | -| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph2 | 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 | Stock4| 20mph5 | 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 | Stock4| 0mph | 0mph | -| Toyota | Highlander Hybrid 2017-19 | All | Stock4| 0mph | 0mph | -| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Prius 2016 | TSS-P | Stock4| 0mph | 0mph | -| Toyota | Prius 2017-19 | All | Stock4| 0mph | 0mph | -| Toyota | Prius Prime 2017-20 | All | Stock4| 0mph | 0mph | -| Toyota | Rav4 2016 | TSS-P | Stock4| 20mph5 | 0mph | -| Toyota | Rav4 2017-18 | All | Stock4| 20mph5 | 0mph | -| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph | -| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock4| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2017-18 | All | Stock4| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph | -| Toyota | Sienna 2018 | All | Stock4| 0mph | 0mph | -| Volkswagen| Golf 2016-193 | 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 | 25mph6 | 25mph | +| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph6 | 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 | 2mph4 | +| Honda | CR-V 2015-16 | Touring | openpilot | 25mph6 | 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 | 25mph6 | 12mph | +| Honda | Insight 2019 | Honda Sensing | Stock | 0mph | 3mph | +| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph6 | 0mph | +| Honda | Passport 2019 | All | openpilot | 25mph6 | 12mph | +| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph6 | 12mph | +| Honda | Pilot 2019 | All | openpilot | 25mph6 | 12mph | +| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph6 | 12mph | +| Hyundai | Elantra 2017-191 | SCC + LKAS | Stock | 19mph | 34mph | +| Hyundai | Genesis 20181 | All | Stock | 19mph | 34mph | +| Hyundai | Santa Fe 20191 | 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 20191 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Sorento 20181 | All | Stock | 0mph | 0mph | +| Kia | Stinger 20181 | SCC + LKAS | Stock | 0mph | 0mph | +| Lexus | CT Hybrid 2017-18 | All | Stock5| 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 | Stock5| 0mph | 0mph | +| Lexus | RX 2016-17 | All | Stock5| 0mph | 0mph | +| Lexus | RX 2020 | All | openpilot | 0mph | 0mph | +| Lexus | RX Hybrid 2016-19 | All | Stock5| 0mph | 0mph | +| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph | +| Toyota | Avalon 2016 | TSS-P | Stock5| 20mph6 | 0mph | +| Toyota | Avalon 2017-18 | All | Stock5| 20mph6 | 0mph | +| Toyota | Camry 2018-19 | All | Stock | 0mph2 | 0mph | +| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph2 | 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 | Stock5| 20mph6 | 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 | Stock5| 0mph | 0mph | +| Toyota | Highlander Hybrid 2017-19 | All | Stock5| 0mph | 0mph | +| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Prius 2016 | TSS-P | Stock5| 0mph | 0mph | +| Toyota | Prius 2017-19 | All | Stock5| 0mph | 0mph | +| Toyota | Prius Prime 2017-20 | All | Stock5| 0mph | 0mph | +| Toyota | Rav4 2016 | TSS-P | Stock5| 20mph6 | 0mph | +| Toyota | Rav4 2017-18 | All | Stock5| 20mph6 | 0mph | +| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph | +| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock5| 0mph | 0mph | +| Toyota | Rav4 Hybrid 2017-18 | All | Stock5| 0mph | 0mph | +| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph | +| Toyota | Sienna 2018 | All | Stock5| 0mph | 0mph | +| Volkswagen| Golf 2016-193 | Driver Assistance | Stock | 0mph | 0mph | 1Requires 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.
228mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
3Requires 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)
+42019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
Community Maintained Cars and Features ------ | Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | | ----------| ------------------------------| ------------------| -----------------| -------------------| -------------| -| Buick | Regal 20186 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Cadillac | ATS 20186 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chevrolet | Malibu 20176 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chevrolet | Volt 2017-186 | Adaptive Cruise | openpilot | 0mph | 7mph | -| GMC | Acadia Denali 20186| Adaptive Cruise | openpilot | 0mph | 7mph | -| Holden | Astra 20176 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Buick | Regal 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Cadillac | ATS 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chevrolet | Malibu 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chevrolet | Volt 2017-187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| GMC | Acadia Denali 20187| Adaptive Cruise | openpilot | 0mph | 7mph | +| Holden | Astra 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | -4When 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).***
-5[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).***
-6Requires 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).***
+5When 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).***
+6[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).***
+7Requires 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).***
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`. diff --git a/RELEASES.md b/RELEASES.md index afce0cd5e..542c7a27b 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -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! diff --git a/cereal/README.md b/cereal/README.md index ec0efc676..d03e29439 100644 --- a/cereal/README.md +++ b/cereal/README.md @@ -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) ``` diff --git a/cereal/car.capnp b/cereal/car.capnp index d5e9e8fef..efbf542ef 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -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 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 00e063b65..522a9da5c 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -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; } } diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py index e5a004740..b53d6fb7a 100644 --- a/cereal/messaging/__init__.py +++ b/cereal/messaging/__init__.py @@ -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 diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index 5747293ab..960cd44f5 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -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.] diff --git a/common/android.py b/common/android.py index e0933e822..5e26924f2 100644 --- a/common/android.py +++ b/common/android.py @@ -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 diff --git a/common/apk.py b/common/apk.py index ad2eb9ed1..9d53ff906 100644 --- a/common/apk.py +++ b/common/apk.py @@ -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() - diff --git a/common/manager_helpers.py b/common/manager_helpers.py new file mode 100644 index 000000000..a8cfb3df0 --- /dev/null +++ b/common/manager_helpers.py @@ -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 diff --git a/common/params.py b/common/params.py index fa93520ee..57d2e56ba 100755 --- a/common/params.py +++ b/common/params.py @@ -22,6 +22,8 @@ file in place without messing with /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 diff --git a/models/dmonitoring_model_q.dlc b/models/dmonitoring_model_q.dlc index 59104e18e..9dbe09274 100644 Binary files a/models/dmonitoring_model_q.dlc and b/models/dmonitoring_model_q.dlc differ diff --git a/models/supercombo.dlc b/models/supercombo.dlc index d7942d6df..ea6d0a068 100644 Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ diff --git a/opendbc/can/common.cc b/opendbc/can/common.cc index 5a131eb6c..de19514b6 100644 --- a/opendbc/can/common.cc +++ b/opendbc/can/common.cc @@ -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; diff --git a/opendbc/generator/honda/_bosch_2018.dbc b/opendbc/generator/honda/_bosch_2018.dbc index bfbfe9b8b..18a1ff182 100644 --- a/opendbc/generator/honda/_bosch_2018.dbc +++ b/opendbc/generator/honda/_bosch_2018.dbc @@ -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" ; diff --git a/opendbc/generator/honda/_dual_can_nidec_2018.dbc b/opendbc/generator/honda/_dual_can_nidec_2018.dbc index 67c019c92..a5d72d67f 100644 --- a/opendbc/generator/honda/_dual_can_nidec_2018.dbc +++ b/opendbc/generator/honda/_dual_can_nidec_2018.dbc @@ -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" ; diff --git a/opendbc/generator/toyota/_toyota_nodsu_bsm.dbc b/opendbc/generator/toyota/_toyota_nodsu_bsm.dbc new file mode 100644 index 000000000..2f80e58e8 --- /dev/null +++ b/opendbc/generator/toyota/_toyota_nodsu_bsm.dbc @@ -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"; diff --git a/opendbc/generator/toyota/lexus_ct200h_2018_pt.dbc b/opendbc/generator/toyota/lexus_ct200h_2018_pt.dbc index c6ec5e11d..1024849bc 100644 --- a/opendbc/generator/toyota/lexus_ct200h_2018_pt.dbc +++ b/opendbc/generator/toyota/lexus_ct200h_2018_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/lexus_gs300h_2017_pt.dbc b/opendbc/generator/toyota/lexus_gs300h_2017_pt.dbc index 070b4e6e7..35ad1c8f7 100644 --- a/opendbc/generator/toyota/lexus_gs300h_2017_pt.dbc +++ b/opendbc/generator/toyota/lexus_gs300h_2017_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/lexus_nx300h_2018_pt.dbc b/opendbc/generator/toyota/lexus_nx300h_2018_pt.dbc index fb42de9f6..96526849a 100644 --- a/opendbc/generator/toyota/lexus_nx300h_2018_pt.dbc +++ b/opendbc/generator/toyota/lexus_nx300h_2018_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/lexus_rx_350_2016_pt.dbc b/opendbc/generator/toyota/lexus_rx_350_2016_pt.dbc index 4a6fe48fd..03f314b1a 100644 --- a/opendbc/generator/toyota/lexus_rx_350_2016_pt.dbc +++ b/opendbc/generator/toyota/lexus_rx_350_2016_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/lexus_rx_hybrid_2017_pt.dbc b/opendbc/generator/toyota/lexus_rx_hybrid_2017_pt.dbc index fb42de9f6..96526849a 100644 --- a/opendbc/generator/toyota/lexus_rx_hybrid_2017_pt.dbc +++ b/opendbc/generator/toyota/lexus_rx_hybrid_2017_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/toyota_avalon_2017_pt.dbc b/opendbc/generator/toyota/toyota_avalon_2017_pt.dbc index b867e9370..219cfa37c 100644 --- a/opendbc/generator/toyota/toyota_avalon_2017_pt.dbc +++ b/opendbc/generator/toyota/toyota_avalon_2017_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/toyota_corolla_2017_pt.dbc b/opendbc/generator/toyota/toyota_corolla_2017_pt.dbc index 724b6e834..627634f77 100644 --- a/opendbc/generator/toyota/toyota_corolla_2017_pt.dbc +++ b/opendbc/generator/toyota/toyota_corolla_2017_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/toyota_highlander_2017_pt.dbc b/opendbc/generator/toyota/toyota_highlander_2017_pt.dbc index b867e9370..219cfa37c 100644 --- a/opendbc/generator/toyota/toyota_highlander_2017_pt.dbc +++ b/opendbc/generator/toyota/toyota_highlander_2017_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/toyota_highlander_hybrid_2018_pt.dbc b/opendbc/generator/toyota/toyota_highlander_hybrid_2018_pt.dbc index 8fe5ec199..9d6484d52 100644 --- a/opendbc/generator/toyota/toyota_highlander_hybrid_2018_pt.dbc +++ b/opendbc/generator/toyota/toyota_highlander_hybrid_2018_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/toyota_nodsu_hybrid_pt.dbc b/opendbc/generator/toyota/toyota_nodsu_hybrid_pt.dbc index ac91c726f..ef5f0f1e2 100644 --- a/opendbc/generator/toyota/toyota_nodsu_hybrid_pt.dbc +++ b/opendbc/generator/toyota/toyota_nodsu_hybrid_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/toyota_nodsu_pt.dbc b/opendbc/generator/toyota/toyota_nodsu_pt.dbc index 9bc1b1e67..5290b9404 100644 --- a/opendbc/generator/toyota/toyota_nodsu_pt.dbc +++ b/opendbc/generator/toyota/toyota_nodsu_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/toyota_rav4_2017_pt.dbc b/opendbc/generator/toyota/toyota_rav4_2017_pt.dbc index e9d748b6c..79f99be28 100644 --- a/opendbc/generator/toyota/toyota_rav4_2017_pt.dbc +++ b/opendbc/generator/toyota/toyota_rav4_2017_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/toyota_rav4_hybrid_2017_pt.dbc b/opendbc/generator/toyota/toyota_rav4_hybrid_2017_pt.dbc index d5a265fbb..bf7146a09 100644 --- a/opendbc/generator/toyota/toyota_rav4_hybrid_2017_pt.dbc +++ b/opendbc/generator/toyota/toyota_rav4_hybrid_2017_pt.dbc @@ -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 diff --git a/opendbc/generator/toyota/toyota_sienna_xle_2018_pt.dbc b/opendbc/generator/toyota/toyota_sienna_xle_2018_pt.dbc index 0c6ba26bc..394cc04fb 100644 --- a/opendbc/generator/toyota/toyota_sienna_xle_2018_pt.dbc +++ b/opendbc/generator/toyota/toyota_sienna_xle_2018_pt.dbc @@ -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 diff --git a/opendbc/honda_accord_lx15t_2018_can_generated.dbc b/opendbc/honda_accord_lx15t_2018_can_generated.dbc index 00a01f6ed..aff997394 100644 --- a/opendbc/honda_accord_lx15t_2018_can_generated.dbc +++ b/opendbc/honda_accord_lx15t_2018_can_generated.dbc @@ -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" diff --git a/opendbc/honda_accord_s2t_2018_can_generated.dbc b/opendbc/honda_accord_s2t_2018_can_generated.dbc index a89d4504f..4ee645eb3 100644 --- a/opendbc/honda_accord_s2t_2018_can_generated.dbc +++ b/opendbc/honda_accord_s2t_2018_can_generated.dbc @@ -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" diff --git a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc index 9b5091c2f..da4d0013c 100644 --- a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc +++ b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc @@ -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" diff --git a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc index c9042135d..f0bcc5ed8 100644 --- a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc +++ b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc @@ -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" diff --git a/opendbc/honda_crv_ex_2017_can_generated.dbc b/opendbc/honda_crv_ex_2017_can_generated.dbc index 6cdb0c02d..3af7967fc 100644 --- a/opendbc/honda_crv_ex_2017_can_generated.dbc +++ b/opendbc/honda_crv_ex_2017_can_generated.dbc @@ -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" diff --git a/opendbc/honda_crv_hybrid_2019_can_generated.dbc b/opendbc/honda_crv_hybrid_2019_can_generated.dbc index f744c490b..ca7372992 100644 --- a/opendbc/honda_crv_hybrid_2019_can_generated.dbc +++ b/opendbc/honda_crv_hybrid_2019_can_generated.dbc @@ -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" diff --git a/opendbc/honda_fit_hybrid_2018_can_generated.dbc b/opendbc/honda_fit_hybrid_2018_can_generated.dbc index fe76cbf57..af7e137b8 100644 --- a/opendbc/honda_fit_hybrid_2018_can_generated.dbc +++ b/opendbc/honda_fit_hybrid_2018_can_generated.dbc @@ -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" ; diff --git a/opendbc/honda_insight_ex_2019_can_generated.dbc b/opendbc/honda_insight_ex_2019_can_generated.dbc index f5cc66dcc..123a45fab 100644 --- a/opendbc/honda_insight_ex_2019_can_generated.dbc +++ b/opendbc/honda_insight_ex_2019_can_generated.dbc @@ -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" diff --git a/opendbc/lexus_ct200h_2018_pt_generated.dbc b/opendbc/lexus_ct200h_2018_pt_generated.dbc index 0f322dd16..8c90eecb1 100644 --- a/opendbc/lexus_ct200h_2018_pt_generated.dbc +++ b/opendbc/lexus_ct200h_2018_pt_generated.dbc @@ -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 diff --git a/opendbc/lexus_gs300h_2017_pt_generated.dbc b/opendbc/lexus_gs300h_2017_pt_generated.dbc index 5bd0bbf5d..ddb29c425 100644 --- a/opendbc/lexus_gs300h_2017_pt_generated.dbc +++ b/opendbc/lexus_gs300h_2017_pt_generated.dbc @@ -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 diff --git a/opendbc/lexus_nx300h_2018_pt_generated.dbc b/opendbc/lexus_nx300h_2018_pt_generated.dbc index c5f0e7ab4..d9826a8f9 100644 --- a/opendbc/lexus_nx300h_2018_pt_generated.dbc +++ b/opendbc/lexus_nx300h_2018_pt_generated.dbc @@ -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 diff --git a/opendbc/lexus_rx_350_2016_pt_generated.dbc b/opendbc/lexus_rx_350_2016_pt_generated.dbc index 2370835ae..455cb41f3 100644 --- a/opendbc/lexus_rx_350_2016_pt_generated.dbc +++ b/opendbc/lexus_rx_350_2016_pt_generated.dbc @@ -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 diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc index ef9052b28..76c43fed9 100644 --- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc +++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc @@ -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 diff --git a/opendbc/nissan_2017.dbc b/opendbc/nissan_2017.dbc index 1d03b7ab5..2fd0f0a0f 100644 --- a/opendbc/nissan_2017.dbc +++ b/opendbc/nissan_2017.dbc @@ -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" ; diff --git a/opendbc/subaru_global_2017.dbc b/opendbc/subaru_global_2017.dbc index 38b2774ab..6da81f281 100644 --- a/opendbc/subaru_global_2017.dbc +++ b/opendbc/subaru_global_2017.dbc @@ -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 diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc index 026191e9b..4adc94870 100644 --- a/opendbc/toyota_avalon_2017_pt_generated.dbc +++ b/opendbc/toyota_avalon_2017_pt_generated.dbc @@ -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 diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc index f02e50195..3ea7fe4de 100644 --- a/opendbc/toyota_corolla_2017_pt_generated.dbc +++ b/opendbc/toyota_corolla_2017_pt_generated.dbc @@ -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 diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc index a5306ac8f..e7aa6b668 100644 --- a/opendbc/toyota_highlander_2017_pt_generated.dbc +++ b/opendbc/toyota_highlander_2017_pt_generated.dbc @@ -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 diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc index d19ac45ff..0920a2bf0 100644 --- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc @@ -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 diff --git a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc index 28ad1d61a..3b7679743 100644 --- a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc +++ b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc @@ -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 diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index ab54939b5..75e4115e8 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -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 diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc index a9e8a03e5..89cf5b698 100644 --- a/opendbc/toyota_rav4_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_2017_pt_generated.dbc @@ -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 diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc index 27ced0f9c..9a50d42c2 100644 --- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc @@ -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 diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc index 2cbd918ce..8cd737426 100644 --- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc +++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc @@ -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 diff --git a/opendbc/vw_mqb_2010.dbc b/opendbc/vw_mqb_2010.dbc index 063cd1b79..21f974cce 100644 --- a/opendbc/vw_mqb_2010.dbc +++ b/opendbc/vw_mqb_2010.dbc @@ -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 diff --git a/panda/board/config.h b/panda/board/config.h index 15096b69f..335d2e1ec 100644 --- a/panda/board/config.h +++ b/panda/board/config.h @@ -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) diff --git a/panda/board/crc.h b/panda/board/crc.h new file mode 100644 index 000000000..ab969e517 --- /dev/null +++ b/panda/board/crc.h @@ -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; +} diff --git a/panda/board/drivers/llcan.h b/panda/board/drivers/llcan.h index 4cd9b4b5a..5e9f276e5 100644 --- a/panda/board/drivers/llcan.h +++ b/panda/board/drivers/llcan.h @@ -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)) {} diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c index 667d27bf0..3192f4b2b 100644 --- a/panda/board/pedal/main.c +++ b/panda/board/pedal/main.c @@ -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 diff --git a/panda/board/safety.h b/panda/board/safety.h index 7fd057b52..407f33f66 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -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}, diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index 293839b7f..0bcffd636 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -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; } } diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index 97e0f57a2..067ed1cad 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -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) { diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 815129084..8672cc267 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -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 diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index 7ca25dc29..75ecae214 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -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 diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index 33a670f29..1b5b656ff 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -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; } } diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h new file mode 100644 index 000000000..baa82b8d4 --- /dev/null +++ b/panda/board/safety/safety_nissan.h @@ -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]), +}; diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index 885e2731d..9b7665f2b 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -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]), +}; diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index fbb464dcd..78b80e3bc 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -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; diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index fa0848a04..5aedb3764 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -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; } } diff --git a/panda/board/safety/safety_toyota_ipas.h b/panda/board/safety/safety_toyota_ipas.h deleted file mode 100644 index e9515658c..000000000 --- a/panda/board/safety/safety_toyota_ipas.h +++ /dev/null @@ -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, -}; diff --git a/panda/board/safety/safety_volkswagen.h b/panda/board/safety/safety_volkswagen.h index 102cb22b5..132c10d11 100644 --- a/panda/board/safety/safety_volkswagen.h +++ b/panda/board/safety/safety_volkswagen.h @@ -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]), }; diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 1a6a8dd21..aa9f7fdfe 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -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; diff --git a/panda/docs/panda_wifi_setup.md b/panda/docs/panda_wifi_setup.md new file mode 100644 index 000000000..a6dfaf1ab --- /dev/null +++ b/panda/docs/panda_wifi_setup.md @@ -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. \ No newline at end of file diff --git a/panda/python/__init__.py b/panda/python/__init__.py index ea8dea13d..c0e27e8d6 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -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 diff --git a/panda/requirements.txt b/panda/requirements.txt index 5ceb3cf6e..fb01edced 100644 --- a/panda/requirements.txt +++ b/panda/requirements.txt @@ -9,3 +9,4 @@ requests flake8==3.7.9 pylint==2.4.3 cffi==1.11.4 +crcmod diff --git a/panda/tests/safety/common.py b/panda/tests/safety/common.py index 63408ce43..e0296d3ac 100644 --- a/panda/tests/safety/common.py +++ b/panda/tests/safety/common.py @@ -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)) diff --git a/panda/tests/safety/libpandasafety_py.py b/panda/tests/safety/libpandasafety_py.py index 77131c1a3..3b552d0f8 100644 --- a/panda/tests/safety/libpandasafety_py.py +++ b/panda/tests/safety/libpandasafety_py.py @@ -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); """) diff --git a/panda/tests/safety/test.c b/panda/tests/safety/test.c index 59611d313..9c7955b28 100644 --- a/panda/tests/safety/test.c +++ b/panda/tests/safety/test.c @@ -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){ } diff --git a/panda/tests/safety/test_cadillac.py b/panda/tests/safety/test_cadillac.py index 7806fb7b2..e4e75f15b 100644 --- a/panda/tests/safety/test_cadillac.py +++ b/panda/tests/safety/test_cadillac.py @@ -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) diff --git a/panda/tests/safety/test_chrysler.py b/panda/tests/safety/test_chrysler.py index 2c472706f..c66b90b6b 100755 --- a/panda/tests/safety/test_chrysler.py +++ b/panda/tests/safety/test_chrysler.py @@ -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) diff --git a/panda/tests/safety/test_gm.py b/panda/tests/safety/test_gm.py index 06872dede..34d92357c 100644 --- a/panda/tests/safety/test_gm.py +++ b/panda/tests/safety/test_gm.py @@ -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) diff --git a/panda/tests/safety/test_honda.py b/panda/tests/safety/test_honda.py index 87f708930..adf02fd5c 100755 --- a/panda/tests/safety/test_honda.py +++ b/panda/tests/safety/test_honda.py @@ -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() diff --git a/panda/tests/safety/test_honda_bosch.py b/panda/tests/safety/test_honda_bosch.py deleted file mode 100755 index 99c8d3546..000000000 --- a/panda/tests/safety/test_honda_bosch.py +++ /dev/null @@ -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() diff --git a/panda/tests/safety/test_hyundai.py b/panda/tests/safety/test_hyundai.py index 9067ec984..2d022759e 100644 --- a/panda/tests/safety/test_hyundai.py +++ b/panda/tests/safety/test_hyundai.py @@ -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) diff --git a/panda/tests/safety/test_nissan.py b/panda/tests/safety/test_nissan.py new file mode 100644 index 000000000..ab826fffb --- /dev/null +++ b/panda/tests/safety/test_nissan.py @@ -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() diff --git a/panda/tests/safety/test_subaru.py b/panda/tests/safety/test_subaru.py index e18d7515c..d68090d19 100644 --- a/panda/tests/safety/test_subaru.py +++ b/panda/tests/safety/test_subaru.py @@ -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() diff --git a/panda/tests/safety/test_toyota.py b/panda/tests/safety/test_toyota.py index 493fb1f62..bc0795595 100644 --- a/panda/tests/safety/test_toyota.py +++ b/panda/tests/safety/test_toyota.py @@ -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() diff --git a/panda/tests/safety/test_toyota_ipas.py b/panda/tests/safety/test_toyota_ipas.py deleted file mode 100644 index 3fcca95f1..000000000 --- a/panda/tests/safety/test_toyota_ipas.py +++ /dev/null @@ -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() diff --git a/panda/tests/safety/test_volkswagen.py b/panda/tests/safety/test_volkswagen.py deleted file mode 100644 index db58cdc58..000000000 --- a/panda/tests/safety/test_volkswagen.py +++ /dev/null @@ -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() diff --git a/panda/tests/safety/test_volkswagen_mqb.py b/panda/tests/safety/test_volkswagen_mqb.py new file mode 100644 index 000000000..a413fd445 --- /dev/null +++ b/panda/tests/safety/test_volkswagen_mqb.py @@ -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() diff --git a/panda/tests/safety_replay/Dockerfile b/panda/tests/safety_replay/Dockerfile index f5175c745..b0b5f0c27 100644 --- a/panda/tests/safety_replay/Dockerfile +++ b/panda/tests/safety_replay/Dockerfile @@ -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 diff --git a/panda/tests/safety_replay/test_safety_replay.py b/panda/tests/safety_replay/test_safety_replay.py index fd36b248e..81b9f5b13 100755 --- a/panda/tests/safety_replay/test_safety_replay.py +++ b/panda/tests/safety_replay/test_safety_replay.py @@ -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__": diff --git a/reset_update.sh b/reset_update.sh index 6bfe86d88..18d77f9d3 100755 --- a/reset_update.sh +++ b/reset_update.sh @@ -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 \ No newline at end of file +cd /data/openpilot && git reset --hard @{u} && git clean -xdf && git pull && scons --clean && reboot \ No newline at end of file diff --git a/selfdrive/assets/images/battery.png b/selfdrive/assets/images/battery.png new file mode 100644 index 000000000..cac3b9096 Binary files /dev/null and b/selfdrive/assets/images/battery.png differ diff --git a/selfdrive/assets/images/battery_charging.png b/selfdrive/assets/images/battery_charging.png new file mode 100644 index 000000000..4c25566d0 Binary files /dev/null and b/selfdrive/assets/images/battery_charging.png differ diff --git a/selfdrive/assets/images/button_home.png b/selfdrive/assets/images/button_home.png new file mode 100644 index 000000000..07f47ab5d Binary files /dev/null and b/selfdrive/assets/images/button_home.png differ diff --git a/selfdrive/assets/images/button_settings.png b/selfdrive/assets/images/button_settings.png new file mode 100644 index 000000000..e04262b88 Binary files /dev/null and b/selfdrive/assets/images/button_settings.png differ diff --git a/selfdrive/assets/images/network_0.png b/selfdrive/assets/images/network_0.png new file mode 100644 index 000000000..2ce959ca5 Binary files /dev/null and b/selfdrive/assets/images/network_0.png differ diff --git a/selfdrive/assets/images/network_1.png b/selfdrive/assets/images/network_1.png new file mode 100644 index 000000000..d7ae713f9 Binary files /dev/null and b/selfdrive/assets/images/network_1.png differ diff --git a/selfdrive/assets/images/network_2.png b/selfdrive/assets/images/network_2.png new file mode 100644 index 000000000..17ecd977f Binary files /dev/null and b/selfdrive/assets/images/network_2.png differ diff --git a/selfdrive/assets/images/network_3.png b/selfdrive/assets/images/network_3.png new file mode 100644 index 000000000..1e854e678 Binary files /dev/null and b/selfdrive/assets/images/network_3.png differ diff --git a/selfdrive/assets/images/network_4.png b/selfdrive/assets/images/network_4.png new file mode 100644 index 000000000..08c9ab91f Binary files /dev/null and b/selfdrive/assets/images/network_4.png differ diff --git a/selfdrive/assets/images/network_5.png b/selfdrive/assets/images/network_5.png new file mode 100644 index 000000000..fba67a95a Binary files /dev/null and b/selfdrive/assets/images/network_5.png differ diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index e3a1bf2f1..90043bfed 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -294,7 +294,7 @@ def ws_send(ws, end_event): def backoff(retries): return random.randrange(0, min(128, int(2 ** retries))) -def main(gctx=None): +def main(): params = Params() dongle_id = params.get("DongleId").decode('utf-8') ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id diff --git a/selfdrive/athena/test.py b/selfdrive/athena/test.py index 0bedfdeb7..b3e1d6921 100755 --- a/selfdrive/athena/test.py +++ b/selfdrive/athena/test.py @@ -41,8 +41,7 @@ class TestAthenadMethods(unittest.TestCase): start = time.time() while time.time() - start < 1: - msg = messaging.new_message() - msg.init('thermal') + msg = messaging.new_message('thermal') pub_sock.send(msg.to_bytes()) time.sleep(0.01) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 1ef45fda1..49fdfea05 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -395,12 +395,18 @@ void can_health(PubSocket *publisher) { bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP); bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX; if ((no_ignition_exp || (voltage_f < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) { - printf("TURN OFF CHARGING!\n"); - pthread_mutex_lock(&usb_lock); - libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT); - pthread_mutex_unlock(&usb_lock); - printf("POWER DOWN DEVICE\n"); - system("service call power 17 i32 0 i32 1"); + char *disable_power_down = NULL; + size_t disable_power_down_sz = 0; + const int result = read_db_value(NULL, "DisablePowerDown", &disable_power_down, &disable_power_down_sz); + if (disable_power_down_sz != 1 || disable_power_down[0] != '1') { + printf("TURN OFF CHARGING!\n"); + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + printf("POWER DOWN DEVICE\n"); + system("service call power 17 i32 0 i32 1"); + } + if (disable_power_down) free(disable_power_down); } if (!no_ignition_exp && (voltage_f > VBATT_START_CHARGING) && !cdp_mode) { printf("TURN ON CHARGING!\n"); diff --git a/selfdrive/boardd/tests/boardd_old.py b/selfdrive/boardd/tests/boardd_old.py index 48c7a9459..f6ca7eb87 100755 --- a/selfdrive/boardd/tests/boardd_old.py +++ b/selfdrive/boardd/tests/boardd_old.py @@ -27,8 +27,7 @@ except Exception: # *** serialization functions *** def can_list_to_can_capnp(can_msgs, msgtype='can'): - dat = messaging.new_message() - dat.init(msgtype, len(can_msgs)) + dat = messaging.new_message(msgtype, len(can_msgs)) for i, can_msg in enumerate(can_msgs): if msgtype == 'sendcan': cc = dat.sendcan[i] @@ -168,8 +167,7 @@ def boardd_loop(rate=100): # health packet @ 2hz if (rk.frame % (rate // 2)) == 0: health = can_health() - msg = messaging.new_message() - msg.init('health') + msg = messaging.new_message('health') # store the health to be logged msg.health.voltage = health['voltage'] @@ -232,7 +230,7 @@ def boardd_proxy_loop(rate=100, address="192.168.2.251"): rk.keep_time() -def main(gctx=None): +def main(): if os.getenv("MOCK") is not None: boardd_mock_loop() elif os.getenv("PROXY") is not None: diff --git a/selfdrive/camerad/cameras/camera_qcom.c b/selfdrive/camerad/cameras/camera_qcom.c index a483a6ba0..a45f0c0b8 100644 --- a/selfdrive/camerad/cameras/camera_qcom.c +++ b/selfdrive/camerad/cameras/camera_qcom.c @@ -2210,4 +2210,4 @@ void cameras_run(DualCameraState *s) { void cameras_close(DualCameraState *s) { camera_close(&s->rear); camera_close(&s->front); -} +} \ No newline at end of file diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 7c65f7555..9594fb49d 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -437,6 +437,12 @@ void* processing_thread(void *arg) { framed.setLensErr(frame_data.lens_err); framed.setLensTruePos(frame_data.lens_true_pos); framed.setGainFrac(frame_data.gain_frac); +#ifdef QCOM + kj::ArrayPtr focus_vals(&s->cameras.rear.focus[0], NUM_FOCUS); + kj::ArrayPtr focus_confs(&s->cameras.rear.confidence[0], NUM_FOCUS); + framed.setFocusVal(focus_vals); + framed.setFocusConf(focus_confs); +#endif #ifndef QCOM framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index a223b5cea..8ef75a4ce 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -33,10 +33,12 @@ def load_interfaces(brand_names): CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carcontroller.py'): CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController + CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState else: CarController = None + CarState = None for model_name in brand_names[brand_name]: - ret[model_name] = (CarInterface, CarController) + ret[model_name] = (CarInterface, CarController, CarState) return ret @@ -58,7 +60,8 @@ def _get_interface_names(): # imports from directory selfdrive/car// -interfaces = load_interfaces(_get_interface_names()) +interface_names = _get_interface_names() +interfaces = load_interfaces(interface_names) def only_toyota_left(candidate_cars): @@ -112,11 +115,16 @@ def fingerprint(logcan, sendcan, has_relay): cached_params = Params().get("CarParamsCache") if cached_params is not None: + cached_params = car.CarParams.from_bytes(cached_params) + if cached_params.carName == "mock": + cached_params = None + + if cached_params is not None and len(cached_params.carFw) > 0 and cached_params.carVin is not VIN_UNKNOWN: cloudlog.warning("Using cached CarParams") - CP = car.CarParams.from_bytes(cached_params) - vin = CP.carVin - car_fw = list(CP.carFw) + vin = cached_params.carVin + car_fw = list(cached_params.carFw) else: + cloudlog.warning("Getting VIN & FW versions") _, vin = get_vin(logcan, sendcan, bus) car_fw = get_fw_versions(logcan, sendcan, bus) @@ -173,16 +181,15 @@ def fingerprint(logcan, sendcan, has_relay): frame += 1 + source = car.CarParams.FingerprintSource.can + if dragon_has_cache: car_fingerprint = dragon_car_fingerprint finger = dragon_finger vin = dragon_vin car_fw = dragon_car_fw source = dragon_source - else: - source = car.CarParams.FingerprintSource.can - # If FW query returns exactly 1 candidate, use it if len(fw_candidates) == 1: car_fingerprint = list(fw_candidates)[0] @@ -202,11 +209,6 @@ def fingerprint(logcan, sendcan, has_relay): car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed - fixed_fingerprint = os.environ.get('FINGERPRINT', "") - if len(fixed_fingerprint): - car_fingerprint = fixed_fingerprint - source = car.CarParams.FingerprintSource.fixed - cloudlog.warning("fingerprinted %s", car_fingerprint) return car_fingerprint, finger, vin, car_fw, source @@ -244,10 +246,10 @@ def get_car(logcan, sendcan, has_relay=False): x = threading.Thread(target=log_fingerprinted, args=(candidate,)) x.start() - CarInterface, CarController = interfaces[candidate] + CarInterface, CarController, CarState = interfaces[candidate] car_params = CarInterface.get_params(candidate, fingerprints, has_relay, car_fw) car_params.carVin = vin car_params.carFw = car_fw car_params.fingerprintSource = source - return CarInterface(car_params, CarController), car_params + return CarInterface(car_params, CarController, CarState), car_params diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index f6b55b9e1..8c58d3db0 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,27 +1,21 @@ from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \ create_wheel_buttons -from selfdrive.car.chrysler.values import Ecu, CAR, SteerLimitParams +from selfdrive.car.chrysler.values import CAR, SteerLimitParams from opendbc.can.packer import CANPacker class CarController(): - def __init__(self, dbc_name, car_fingerprint, enable_camera): + def __init__(self, dbc_name, CP, VM): self.braking = False - # redundant safety check with the board - self.controls_allowed = True self.apply_steer_last = 0 self.ccframe = 0 self.prev_frame = -1 self.hud_count = 0 - self.car_fingerprint = car_fingerprint + self.car_fingerprint = CP.carFingerprint self.alert_active = False self.gone_fast_yet = False self.steer_rate_limited = False - self.fake_ecus = set() - if enable_camera: - self.fake_ecus.add(Ecu.fwdCamera) - self.packer = CANPacker(dbc_name) @@ -41,7 +35,7 @@ class CarController(): moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit self.gone_fast_yet = True - elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020_HYBRID, CAR.JEEP_CHEROKEE_2019): + elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0): self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5 lkas_active = moving_fast and enabled diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index bab58b963..99e09f00f 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -5,63 +5,6 @@ from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import CarStateBase from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD -def get_can_parser(CP): - - signals = [ - # sig_name, sig_address, default - ("PRNDL", "GEAR", 0), - ("DOOR_OPEN_FL", "DOORS", 0), - ("DOOR_OPEN_FR", "DOORS", 0), - ("DOOR_OPEN_RL", "DOORS", 0), - ("DOOR_OPEN_RR", "DOORS", 0), - ("BRAKE_PRESSED_2", "BRAKE_2", 0), - ("ACCEL_134", "ACCEL_GAS_134", 0), - ("SPEED_LEFT", "SPEED_1", 0), - ("SPEED_RIGHT", "SPEED_1", 0), - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), - ("STEER_ANGLE", "STEERING", 0), - ("STEERING_RATE", "STEERING", 0), - ("TURN_SIGNALS", "STEERING_LEVERS", 0), - ("ACC_STATUS_2", "ACC_2", 0), - ("HIGH_BEAM_FLASH", "STEERING_LEVERS", 0), - ("ACC_SPEED_CONFIG_KPH", "DASHBOARD", 0), - ("TORQUE_DRIVER", "EPS_STATUS", 0), - ("TORQUE_MOTOR", "EPS_STATUS", 0), - ("LKAS_STATE", "EPS_STATUS", 1), - ("COUNTER", "EPS_STATUS", -1), - ("TRACTION_OFF", "TRACTION_BUTTON", 0), - ("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS", 0), - ("COUNTER", "WHEEL_BUTTONS", -1), # incrementing counter for 23b - ] - - # It's considered invalid if it is not received for 10x the expected period (1/f). - checks = [ - # sig_address, frequency - ("BRAKE_2", 50), - ("EPS_STATUS", 100), - ("SPEED_1", 100), - ("WHEEL_SPEEDS", 50), - ("STEERING", 100), - ("ACC_2", 50), - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - -def get_camera_parser(CP): - signals = [ - # sig_name, sig_address, default - # TODO read in all the other values - ("COUNTER", "LKAS_COMMAND", -1), - ("CAR_MODEL", "LKAS_HUD", -1), - ("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1) - ] - checks = [] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) - class CarState(CarStateBase): def __init__(self, CP): @@ -73,7 +16,6 @@ class CarState(CarStateBase): ret = car.CarState.new_message() - self.frame_23b = int(cp.vl["WHEEL_BUTTONS"]['COUNTER']) self.frame = int(cp.vl["EPS_STATUS"]['COUNTER']) ret.doorOpen = any([cp.vl["DOORS"]['DOOR_OPEN_FL'], @@ -88,7 +30,7 @@ class CarState(CarStateBase): ret.gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134'] ret.gasPressed = ret.gas > 1e-5 - self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1) + ret.espDisabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1) ret.wheelSpeeds.fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL'] ret.wheelSpeeds.rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR'] @@ -121,3 +63,58 @@ class CarState(CarStateBase): self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]['LKAS_STATUS_OK'] return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address, default + ("PRNDL", "GEAR", 0), + ("DOOR_OPEN_FL", "DOORS", 0), + ("DOOR_OPEN_FR", "DOORS", 0), + ("DOOR_OPEN_RL", "DOORS", 0), + ("DOOR_OPEN_RR", "DOORS", 0), + ("BRAKE_PRESSED_2", "BRAKE_2", 0), + ("ACCEL_134", "ACCEL_GAS_134", 0), + ("SPEED_LEFT", "SPEED_1", 0), + ("SPEED_RIGHT", "SPEED_1", 0), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), + ("STEER_ANGLE", "STEERING", 0), + ("STEERING_RATE", "STEERING", 0), + ("TURN_SIGNALS", "STEERING_LEVERS", 0), + ("ACC_STATUS_2", "ACC_2", 0), + ("HIGH_BEAM_FLASH", "STEERING_LEVERS", 0), + ("ACC_SPEED_CONFIG_KPH", "DASHBOARD", 0), + ("TORQUE_DRIVER", "EPS_STATUS", 0), + ("TORQUE_MOTOR", "EPS_STATUS", 0), + ("LKAS_STATE", "EPS_STATUS", 1), + ("COUNTER", "EPS_STATUS", -1), + ("TRACTION_OFF", "TRACTION_BUTTON", 0), + ("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS", 0), + ] + + checks = [ + # sig_address, frequency + ("BRAKE_2", 50), + ("EPS_STATUS", 100), + ("SPEED_1", 100), + ("WHEEL_SPEEDS", 50), + ("STEERING", 100), + ("ACC_2", 50), + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + signals = [ + # sig_name, sig_address, default + ("COUNTER", "LKAS_COMMAND", -1), + ("CAR_MODEL", "LKAS_HUD", -1), + ("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1) + ] + checks = [] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index 3c5a913fc..c06c27e96 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -10,19 +10,10 @@ def calc_checksum(data): jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf """ - end_index = len(data) - index = 0 checksum = 0xFF - temp_chk = 0 - bit_sum = 0 - if(end_index <= index): - return False - for index in range(0, end_index): + for curr in data[:-1]: shift = 0x80 - curr = data[index] - iterate = 8 - while(iterate > 0): - iterate -= 1 + for i in range(0, 8): bit_sum = curr & shift temp_chk = checksum & 0x80 if (bit_sum != 0): @@ -84,7 +75,6 @@ def create_lkas_command(packer, apply_steer, moving_fast, frame): } dat = packer.make_can_msg("LKAS_COMMAND", 0, values)[2] - dat = dat[:-1] checksum = calc_checksum(dat) values["CHECKSUM"] = checksum @@ -95,6 +85,6 @@ def create_wheel_buttons(frame): # WHEEL_BUTTONS (571) Message sent to cancel ACC. start = b"\x01" # acc cancel set counter = (frame % 10) << 4 - dat = start + counter.to_bytes(1, 'little') - dat = dat + calc_checksum(dat).to_bytes(1, 'little') + dat = start + counter.to_bytes(1, 'little') + b"\x00" + dat = dat[:-1] + calc_checksum(dat).to_bytes(1, 'little') return make_can_msg(0x23b, dat, 0) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 93220fd12..428b7f214 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -2,35 +2,11 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.chrysler.values import Ecu, ECU_FINGERPRINT, CAR, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -GearShifter = car.CarState.GearShifter -ButtonType = car.CarState.ButtonEvent.Type - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.VM = VehicleModel(CP) - - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False - self.low_speed_alert = False - self.left_blinker_prev = False - self.right_blinker_prev = False - - # *** init the major players *** - self.CS = CarState(CP) - self.cp = get_can_parser(CP) - self.cp_cam = get_camera_parser(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera) @staticmethod def compute_gb(accel, speed): @@ -38,18 +14,10 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() - + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "chrysler" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay - ret.safetyModel = car.CarParams.SafetyModel.chrysler - # pedal - ret.enableCruise = True - # Speed conversion: 20, 45 mph ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 @@ -69,43 +37,19 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 ret.minSteerSpeed = 3.8 # m/s - ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this - if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020_HYBRID, CAR.JEEP_CHEROKEE_2019): - ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. + if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): # TODO allow 2019 cars to steer down to 13 m/s if already engaged. + ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase + # starting with reasonable value for civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. - - # steer, gas, brake limitations VS speed - ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph - ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.5] - ret.brakeMaxBP = [5., 20.] - ret.brakeMaxV = [1., 0.8] - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay print("ECU Camera Simulated: {0}".format(ret.enableCamera)) - ret.openpilotLongitudinalControl = False - - ret.stoppingControl = False - ret.startAccel = 0.0 - - ret.longitudinalTuning.deadzoneBP = [0., 9.] - ret.longitudinalTuning.deadzoneV = [0., .15] - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] - ret.longitudinalTuning.kiBP = [0., 35.] - ret.longitudinalTuning.kiV = [0.54, 0.36] return ret @@ -123,53 +67,17 @@ class CarInterface(CarInterfaceBase): ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - # TODO: button presses - buttonEvents = [] - - if ret.leftBlinker != self.left_blinker_prev: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = ret.leftBlinker != 0 - buttonEvents.append(be) - - if ret.rightBlinker != self.right_blinker_prev: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = ret.rightBlinker != 0 - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents - - self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed) + ret.buttonEvents = [] # events - events = [] - if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)): - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not ret.cruiseState.available: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gearShifter == GearShifter.reverse: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], gas_resume_speed = 2.) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # disable on gas pedal and speed isn't zero. Gas pedal is used to resume ACC - # from a 3+ second stop. - if (ret.gasPressed and (not self.gas_pressed_prev) and ret.vEgo > 2.0): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if self.low_speed_alert: + if ret.vEgo < self.CP.minSteerSpeed: events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events @@ -177,8 +85,6 @@ class CarInterface(CarInterfaceBase): self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - self.left_blinker_prev = ret.leftBlinker - self.right_blinker_prev = ret.rightBlinker # copy back carState packet to CS self.CS.out = ret.as_reader() diff --git a/selfdrive/car/chrysler/test_chryslercan.py b/selfdrive/car/chrysler/test_chryslercan.py index 637cdc9ff..d4d410576 100644 --- a/selfdrive/car/chrysler/test_chryslercan.py +++ b/selfdrive/car/chrysler/test_chryslercan.py @@ -12,8 +12,8 @@ GearShifter = car.CarState.GearShifter class TestChryslerCan(unittest.TestCase): def test_checksum(self): - self.assertEqual(0x75, chryslercan.calc_checksum(b"\x01\x20")) - self.assertEqual(0xcc, chryslercan.calc_checksum(b"\x14\x00\x00\x00\x20")) + self.assertEqual(0x75, chryslercan.calc_checksum(b"\x01\x20\x00")) + self.assertEqual(0xcc, chryslercan.calc_checksum(b"\x14\x00\x00\x00\x20\x00")) def test_hud(self): packer = CANPacker('chrysler_pacifica_2017_hybrid') diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 1e9cdcc5f..efbcc67b9 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -15,7 +15,6 @@ class CAR: PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018" PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" - PACIFICA_2020_HYBRID = "CHRYSLER PACIFICA HYBRID 2020" PACIFICA_2018 = "CHRYSLER PACIFICA 2018" # Also covers Pacifica 2017. JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # Also covers Tailhawk 2017. JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" @@ -54,12 +53,13 @@ FINGERPRINTS = { { 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8 }, - # Based on "d26bf42deb1910e7|2020-02-13--16-22-31" - {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1262: 8, 1284: 8, 1568: 8, 1902: 8, 2015: 8, 2016: 8, 2018: 8, 2023: 8, 2024: 8, 2026: 8, 2031: 8 + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 2015: 8, 2016: 8, 2024: 8 }, - ], - CAR.PACIFICA_2020_HYBRID: [ - {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 2015: 8, 2016: 8, 2024: 8}, + # Based on "8190c7275a24557b|2020-02-24--09-57-23" + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 + } ], CAR.JEEP_CHEROKEE: [ # JEEP GRAND CHEROKEE V6 2018 @@ -88,9 +88,6 @@ DBC = { CAR.PACIFICA_2019_HYBRID: dbc_dict( # Same DBC file works. 'chrysler_pacifica_2017_hybrid', # 'pt' 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' - CAR.PACIFICA_2020_HYBRID: dbc_dict( # Same DBC file works. - 'chrysler_pacifica_2017_hybrid', # 'pt' - 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' CAR.JEEP_CHEROKEE: dbc_dict( # Same DBC file works. 'chrysler_pacifica_2017_hybrid', # 'pt' 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index d708c1358..4c0a37ab0 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -8,12 +8,12 @@ MAX_STEER_DELTA = 1 TOGGLE_DEBUG = False class CarController(): - def __init__(self, dbc_name, enable_camera, vehicle_model): + def __init__(self, dbc_name, CP, VM): self.packer = CANPacker(dbc_name) - self.enable_camera = enable_camera + self.enable_camera = CP.enableCamera self.enabled_last = False self.main_on_last = False - self.vehicle_model = vehicle_model + self.vehicle_model = VM self.generic_toggle_last = 0 self.steer_alert_last = False self.lkas_action = 0 diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index ec461ab16..20341ac9a 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -7,32 +7,6 @@ from selfdrive.car.ford.values import DBC WHEEL_RADIUS = 0.33 -def get_can_parser(CP): - - signals = [ - # sig_name, sig_address, default - ("WhlRr_W_Meas", "WheelSpeed_CG1", 0.), - ("WhlRl_W_Meas", "WheelSpeed_CG1", 0.), - ("WhlFr_W_Meas", "WheelSpeed_CG1", 0.), - ("WhlFl_W_Meas", "WheelSpeed_CG1", 0.), - ("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1", 0.), - ("Cruise_State", "Cruise_Status", 0.), - ("Set_Speed", "Cruise_Status", 0.), - ("LaActAvail_D_Actl", "Lane_Keep_Assist_Status", 0), - ("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status", 0), - ("LaActDeny_B_Actl", "Lane_Keep_Assist_Status", 0), - ("ApedPosScal_Pc_Actl", "EngineData_14", 0.), - ("Dist_Incr", "Steering_Buttons", 0.), - ("Brake_Drv_Appl", "Cruise_Status", 0.), - ("Brake_Lights", "BCM_to_HS_Body", 0.), - ] - - checks = [ - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - - class CarState(CarStateBase): def update(self, cp): ret = car.CarState.new_message() @@ -58,3 +32,25 @@ class CarState(CarStateBase): self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address, default + ("WhlRr_W_Meas", "WheelSpeed_CG1", 0.), + ("WhlRl_W_Meas", "WheelSpeed_CG1", 0.), + ("WhlFr_W_Meas", "WheelSpeed_CG1", 0.), + ("WhlFl_W_Meas", "WheelSpeed_CG1", 0.), + ("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1", 0.), + ("Cruise_State", "Cruise_Status", 0.), + ("Set_Speed", "Cruise_Status", 0.), + ("LaActAvail_D_Actl", "Lane_Keep_Assist_Status", 0), + ("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status", 0), + ("LaActDeny_B_Actl", "Lane_Keep_Assist_Status", 0), + ("ApedPosScal_Pc_Actl", "EngineData_14", 0.), + ("Dist_Incr", "Steering_Buttons", 0.), + ("Brake_Drv_Appl", "Cruise_Status", 0.), + ("Brake_Lights", "BCM_to_HS_Body", 0.), + ] + checks = [] + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 6b1f9340f..6a5992406 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -3,31 +3,12 @@ from cereal import car from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.ford.carstate import CarState, get_can_parser from selfdrive.car.ford.values import MAX_ANGLE, Ecu, ECU_FINGERPRINT, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.VM = VehicleModel(CP) - - self.frame = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False - - # *** init the major players *** - self.CS = CarState(CP) - - self.cp = get_can_parser(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP.enableCamera, self.VM) @staticmethod def compute_gb(accel, speed): @@ -35,19 +16,11 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() - + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "ford" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay - ret.safetyModel = car.CarParams.SafetyModel.ford ret.dashcamOnly = True - # pedal - ret.enableCruise = True - ret.wheelbase = 2.85 ret.steerRatio = 14.8 ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG @@ -60,10 +33,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 tire_stiffness_factor = 0.5328 - # min speed to enable ACC. if car can do stop and go, then set enabling speed - # to a negative value, so it won't matter. - ret.minEnableSpeed = -1. - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) @@ -73,32 +42,11 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. ret.steerControlType = car.CarParams.SteerControlType.angle - # steer, gas, brake limitations VS speed - ret.steerMaxBP = [0.] # breakpoints at 1 and 40 kph - ret.steerMaxV = [1.0] # 2/3rd torque allowed above 45 kph - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.5] - ret.brakeMaxBP = [5., 20.] - ret.brakeMaxV = [1., 0.8] - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay - ret.openpilotLongitudinalControl = False cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) - ret.stoppingControl = False - ret.startAccel = 0.0 - - ret.longitudinalTuning.deadzoneBP = [0., 9.] - ret.longitudinalTuning.deadzoneV = [0., .15] - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] - ret.longitudinalTuning.kiBP = [0., 35.] - ret.longitudinalTuning.kiV = [0.54, 0.36] - return ret # returns a car.CarState @@ -111,10 +59,7 @@ class CarInterface(CarInterfaceBase): ret.canValid = self.cp.can_valid # events - events = [] - - if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + events = self.create_common_events(ret) # enable request in prius is simple, as we activate when Toyota is active (rising edge) if ret.cruiseState.enabled and not self.cruise_enabled_prev: @@ -122,14 +67,6 @@ class CarInterface(CarInterfaceBase): elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13.* CV.MPH_TO_MS and ret.cruiseState.enabled: events.append(create_event('steerTempUnavailableMute', [ET.WARNING])) diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 96b7756c2..95b970f12 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -85,16 +85,16 @@ def match_fw_to_car(fw_versions): ecu_type = ecu[0] addr = ecu[1:] found_version = fw_versions_dict.get(addr, None) - + ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa, Ecu.electricBrakeBooster] if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER] and found_version is None: continue # TODO: COROLLA_TSS2 engine can show on two different addresses - if ecu_type == Ecu.engine and candidate == TOYOTA.COROLLA_TSS2 and found_version is None: + if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR] and found_version is None: continue - # Allow DSU not being present - if ecu_type in [Ecu.unknown, Ecu.dsu] and found_version is None: + # ignore non essential ecus + if ecu_type not in ESSENTIAL_ECUS and found_version is None: continue if found_version not in expected_versions: diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 53eccf1f3..4fe0e5a15 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -4,7 +4,7 @@ from common.numpy_fast import interp from selfdrive.config import Conversions as CV from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan -from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS +from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS, CanBus from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -69,22 +69,18 @@ def process_hud_alert(hud_alert): return steer class CarController(): - def __init__(self, canbus, car_fingerprint): + def __init__(self, dbc_name, CP, VM): self.pedal_steady = 0. self.start_time = 0. - self.steer_idx = 0 self.apply_steer_last = 0 - self.car_fingerprint = car_fingerprint + self.car_fingerprint = CP.carFingerprint self.lka_icon_status_last = (False, False) self.steer_rate_limited = False - # Setup detection helper. Routes commands to - # an appropriate CAN bus number. - self.canbus = canbus - self.params = CarControllerParams(car_fingerprint) + self.params = CarControllerParams(CP.carFingerprint) - self.packer_pt = CANPacker(DBC[car_fingerprint]['pt']) - self.packer_ch = CANPacker(DBC[car_fingerprint]['chassis']) + self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt']) + self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) def update(self, enabled, CS, frame, actuators, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): @@ -93,7 +89,6 @@ class CarController(): # Send CAN commands. can_sends = [] - canbus = self.canbus alert_out = process_hud_alert(hud_alert) steer = alert_out @@ -101,10 +96,10 @@ class CarController(): ### STEER ### if (frame % P.STEER_STEP) == 0: - lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED + lkas_enabled = enabled and not CS.steer_warning and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = actuators.steer * P.STEER_MAX - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer else: apply_steer = 0 @@ -114,10 +109,10 @@ class CarController(): if self.car_fingerprint in SUPERCRUISE_CARS: can_sends += gmcan.create_steering_control_ct6(self.packer_pt, - canbus, apply_steer, CS.v_ego, idx, lkas_enabled) + CanBus, apply_steer, CS.out.vEgo, idx, lkas_enabled) else: can_sends.append(gmcan.create_steering_control(self.packer_pt, - canbus.powertrain, apply_steer, idx, lkas_enabled)) + CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) ### GAS/BRAKE ### @@ -142,16 +137,16 @@ class CarController(): if (frame % 4) == 0: idx = (frame // 4) % 4 - at_full_stop = enabled and CS.standstill - near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) - can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop)) + at_full_stop = enabled and CS.out.standstill + near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) + can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) - at_full_stop = enabled and CS.standstill - can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop)) + at_full_stop = enabled and CS.out.standstill + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: - can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, canbus.powertrain, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car)) + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) @@ -160,17 +155,17 @@ class CarController(): if frame % time_and_headlights_step == 0: idx = (frame // time_and_headlights_step) % 4 - can_sends.append(gmcan.create_adas_time_status(canbus.obstacle, int((tt - self.start_time) * 60), idx)) - can_sends.append(gmcan.create_adas_headlights_status(canbus.obstacle)) + can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) + can_sends.append(gmcan.create_adas_headlights_status(CanBus.OBSTACLE)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: idx = (frame // speed_and_accelerometer_step) % 4 - can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx)) - can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx)) + can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) + can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx)) if frame % P.ADAS_KEEPALIVE_STEP == 0: - can_sends += gmcan.create_adas_keepalive(canbus.powertrain) + can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. @@ -181,7 +176,7 @@ class CarController(): lka_icon_status = (lka_active, lka_critical) if frame % P.CAMERA_KEEPALIVE_STEP == 0 \ or lka_icon_status != self.lka_icon_status_last: - can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical, steer)) + can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer)) self.lka_icon_status_last = lka_icon_status return can_sends diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index f924c66f1..cb849c0d2 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -4,52 +4,10 @@ from selfdrive.config import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.gm.values import DBC, CAR, \ +from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, \ CruiseButtons, is_eps_status_ok, \ STEER_THRESHOLD, SUPERCRUISE_CARS -def get_powertrain_can_parser(CP, canbus): - # this function generates lists for signal, messages and initial values - signals = [ - # sig_name, sig_address, default - ("BrakePedalPosition", "EBCMBrakePedalPosition", 0), - ("FrontLeftDoor", "BCMDoorBeltStatus", 0), - ("FrontRightDoor", "BCMDoorBeltStatus", 0), - ("RearLeftDoor", "BCMDoorBeltStatus", 0), - ("RearRightDoor", "BCMDoorBeltStatus", 0), - ("LeftSeatBelt", "BCMDoorBeltStatus", 0), - ("RightSeatBelt", "BCMDoorBeltStatus", 0), - ("TurnSignals", "BCMTurnSignals", 0), - ("AcceleratorPedal", "AcceleratorPedal", 0), - ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS), - ("SteeringWheelAngle", "PSCMSteeringAngle", 0), - ("FLWheelSpd", "EBCMWheelSpdFront", 0), - ("FRWheelSpd", "EBCMWheelSpdFront", 0), - ("RLWheelSpd", "EBCMWheelSpdRear", 0), - ("RRWheelSpd", "EBCMWheelSpdRear", 0), - ("PRNDL", "ECMPRDNL", 0), - ("LKADriverAppldTrq", "PSCMStatus", 0), - ("LKATorqueDeliveredStatus", "PSCMStatus", 0), - ] - - if CP.carFingerprint == CAR.VOLT: - signals += [ - ("RegenPaddle", "EBCMRegenPaddle", 0), - ] - if CP.carFingerprint in SUPERCRUISE_CARS: - signals += [ - ("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0) - ] - else: - signals += [ - ("TractionControlOn", "ESPStatus", 0), - ("EPBClosed", "EPBStatus", 0), - ("CruiseMainOn", "ECMEngineStatus", 0), - ("CruiseState", "AcceleratorPedal2", 0), - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain) - class CarState(CarStateBase): def __init__(self, CP): @@ -58,73 +16,109 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"] def update(self, pt_cp): + ret = car.CarState.new_message() + self.prev_cruise_buttons = self.cruise_buttons self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons'] - self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS - self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS - self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS - self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS - self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) - self.standstill = self.v_ego_raw < 0.01 + ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS + ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.01 - self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle'] - self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None)) - self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] + ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle'] + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None)) + ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0 + # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. + if ret.brake < 10/0xd0: + ret.brake = 0. - self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] - self.user_gas_pressed = self.pedal_gas > 0 + ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254. + ret.gasPressed = ret.gas > 1e-5 - self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq'] - self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD - - # 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed - self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus'] - self.steer_not_allowed = not is_eps_status_ok(self.lkas_status, self.car_fingerprint) + ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq'] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD # 1 - open, 0 - closed - self.door_all_closed = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0 and - pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0 and - pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0 and - pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0) + ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 1) # 1 - latched - self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1 - - self.steer_error = False - - self.brake_error = False - - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on - self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1 - self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2 + ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 0 + ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1 + ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2 if self.car_fingerprint in SUPERCRUISE_CARS: self.park_brake = False - self.main_on = False - self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive'] - self.esp_disabled = False - self.regen_pressed = False - self.pcm_acc_status = int(self.acc_active) + ret.cruiseState.available = False + ret.espDisabled = False + regen_pressed = False + self.pcm_acc_status = int(pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']) else: self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed'] - self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'] - self.acc_active = False - self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 + ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']) + ret.espDisabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState'] if self.car_fingerprint == CAR.VOLT: - self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) + regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) else: - self.regen_pressed = False - - # Brake pedal's potentiometer returns near-zero reading - # even when pedal is not pressed. - if self.user_brake < 10: - self.user_brake = 0 + regen_pressed = False # Regen braking is braking - self.brake_pressed = self.user_brake > 10 or self.regen_pressed + ret.brakePressed = ret.brake > 1e-5 or regen_pressed + ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF + ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL - self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive + # 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed + self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus'] + self.steer_warning = not is_eps_status_ok(self.lkas_status, self.car_fingerprint) + + return ret + + @staticmethod + def get_can_parser(CP): + # this function generates lists for signal, messages and initial values + signals = [ + # sig_name, sig_address, default + ("BrakePedalPosition", "EBCMBrakePedalPosition", 0), + ("FrontLeftDoor", "BCMDoorBeltStatus", 0), + ("FrontRightDoor", "BCMDoorBeltStatus", 0), + ("RearLeftDoor", "BCMDoorBeltStatus", 0), + ("RearRightDoor", "BCMDoorBeltStatus", 0), + ("LeftSeatBelt", "BCMDoorBeltStatus", 0), + ("RightSeatBelt", "BCMDoorBeltStatus", 0), + ("TurnSignals", "BCMTurnSignals", 0), + ("AcceleratorPedal", "AcceleratorPedal", 0), + ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS), + ("SteeringWheelAngle", "PSCMSteeringAngle", 0), + ("FLWheelSpd", "EBCMWheelSpdFront", 0), + ("FRWheelSpd", "EBCMWheelSpdFront", 0), + ("RLWheelSpd", "EBCMWheelSpdRear", 0), + ("RRWheelSpd", "EBCMWheelSpdRear", 0), + ("PRNDL", "ECMPRDNL", 0), + ("LKADriverAppldTrq", "PSCMStatus", 0), + ("LKATorqueDeliveredStatus", "PSCMStatus", 0), + ] + + if CP.carFingerprint == CAR.VOLT: + signals += [ + ("RegenPaddle", "EBCMRegenPaddle", 0), + ] + if CP.carFingerprint in SUPERCRUISE_CARS: + signals += [ + ("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0) + ] + else: + signals += [ + ("TractionControlOn", "ESPStatus", 0), + ("EPBClosed", "EPBStatus", 0), + ("CruiseMainOn", "ECMEngineStatus", 0), + ("CruiseState", "AcceleratorPedal2", 0), + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], CanBus.POWERTRAIN) diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 3db0f5b02..d1e479352 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -11,7 +11,7 @@ def create_steering_control(packer, bus, apply_steer, idx, lkas_active): return packer.make_can_msg("ASCMLKASteeringCmd", bus, values) -def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled): +def create_steering_control_ct6(packer, CanBus, apply_steer, v_ego, idx, enabled): values = { "LKASteeringCmdActive": 1 if enabled else 0, @@ -31,10 +31,10 @@ def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled # pack again with checksum dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2] - return [0x152, 0, dat, canbus.powertrain], \ - [0x154, 0, dat, canbus.powertrain], \ - [0x151, 0, dat, canbus.chassis], \ - [0x153, 0, dat, canbus.chassis] + return [0x152, 0, dat, CanBus.POWERTRAIN], \ + [0x154, 0, dat, CanBus.POWERTRAIN], \ + [0x151, 0, dat, CanBus.CHASSIS], \ + [0x153, 0, dat, CanBus.CHASSIS] def create_adas_keepalive(bus): diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 450fce853..48b744a92 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -2,41 +2,14 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.gm.values import DBC, CAR, Ecu, ECU_FINGERPRINT, \ +from selfdrive.car.gm.values import CAR, Ecu, ECU_FINGERPRINT, CruiseButtons, \ SUPERCRUISE_CARS, AccState, FINGERPRINTS -from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type -class CanBus(CarInterfaceBase): - def __init__(self): - self.powertrain = 0 - self.obstacle = 1 - self.chassis = 2 - self.sw_gmlan = 3 - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - - self.frame = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.acc_active_prev = 0 - - # *** init the major players *** - canbus = CanBus() - self.CS = CarState(CP) - self.VM = VehicleModel(CP) - self.pt_cp = get_powertrain_can_parser(CP, canbus) - self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis'] - - self.CC = None - if CarController is not None: - self.CC = CarController(canbus, CP.carFingerprint) @staticmethod def compute_gb(accel, speed): @@ -44,13 +17,11 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - ret = car.CarParams.new_message() - + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "gm" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay + ret.safetyModel = car.CarParams.SafetyModel.gm # default to gm + ret.enableCruise = False # stock cruise control is kept off - ret.enableCruise = False # GM port is considered a community feature, since it disables AEB; # TODO: make a port that uses a car harness and it only intercepts the camera ret.communityFeature = True @@ -75,7 +46,6 @@ class CarInterface(CarInterfaceBase): # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1607. + STD_CARGO_KG - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.69 ret.steerRatio = 15.7 ret.steerRatioRear = 0. @@ -85,7 +55,6 @@ class CarInterface(CarInterfaceBase): # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1496. + STD_CARGO_KG - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 ret.steerRatio = 15.8 ret.steerRatioRear = 0. @@ -97,14 +66,12 @@ class CarInterface(CarInterfaceBase): # Remaining parameters copied from Volt for now ret.centerToFront = ret.wheelbase * 0.4 ret.minEnableSpeed = 18 * CV.MPH_TO_MS - ret.safetyModel = car.CarParams.SafetyModel.gm ret.steerRatio = 15.7 ret.steerRatioRear = 0. elif candidate == CAR.ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.86 ret.steerRatio = 14.4 #end to end is 13.46 ret.steerRatioRear = 0. @@ -113,7 +80,6 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.BUICK_REGAL: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 #111.4 inches in meters ret.steerRatio = 14.4 # guess for tourx ret.steerRatioRear = 0. @@ -122,7 +88,6 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.CADILLAC_ATS: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1601. + STD_CARGO_KG - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.78 ret.steerRatio = 15.3 ret.steerRatioRear = 0. @@ -138,7 +103,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatioRear = 0. # TODO: there is RAS on this car! ret.centerToFront = ret.wheelbase * 0.465 - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) @@ -148,95 +112,31 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - ret.steerMaxBP = [0.] # m/s - ret.steerMaxV = [1.] - ret.gasMaxBP = [0.] - ret.gasMaxV = [.5] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [1.] - ret.longitudinalTuning.kpBP = [5., 35.] ret.longitudinalTuning.kpV = [2.4, 1.5] ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.36] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] ret.stoppingControl = True ret.startAccel = 0.8 ret.steerLimitTimer = 0.4 ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz - ret.steerControlType = car.CarParams.SteerControlType.torque return ret # returns a car.CarState def update(self, c, can_strings): - self.pt_cp.update_strings(can_strings) + self.cp.update_strings(can_strings) - self.CS.update(self.pt_cp) + ret = self.CS.update(self.cp) - # create message - ret = car.CarState.new_message() - - ret.canValid = self.pt_cp.can_valid - - # speeds - ret.vEgo = self.CS.v_ego - ret.aEgo = self.CS.a_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # gas pedal information. - ret.gas = self.CS.pedal_gas / 254.0 - ret.gasPressed = self.CS.user_gas_pressed - - # brake pedal - ret.brake = self.CS.user_brake / 0xd0 - ret.brakePressed = self.CS.brake_pressed - - # steering wheel - ret.steeringAngle = self.CS.angle_steers - - # torque and user override. Driver awareness - # timer resets when the user uses the steering wheel. - ret.steeringPressed = self.CS.steer_override - ret.steeringTorque = self.CS.steer_torque_driver + ret.canValid = self.cp.can_valid + ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - # cruise state - ret.cruiseState.available = bool(self.CS.main_on) - cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF - ret.cruiseState.enabled = cruiseEnabled - ret.cruiseState.standstill = self.CS.pcm_acc_status == 4 - - ret.leftBlinker = self.CS.left_blinker_on - ret.rightBlinker = self.CS.right_blinker_on - ret.doorOpen = not self.CS.door_all_closed - ret.seatbeltUnlatched = not self.CS.seatbelt - ret.gearShifter = self.CS.gear_shifter - buttonEvents = [] - # blinkers - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on - buttonEvents.append(be) - if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown @@ -247,7 +147,7 @@ class CarInterface(CarInterfaceBase): be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: - if not (cruiseEnabled and self.CS.standstill): + if not (ret.cruiseState.enabled and ret.standstill): be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed. elif but == CruiseButtons.DECEL_SET: be.type = ButtonType.decelCruise @@ -259,43 +159,20 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = buttonEvents - events = [] - if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - if self.CS.steer_not_allowed: - events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + events = self.create_common_events(ret) if self.CS.car_fingerprint in SUPERCRUISE_CARS: - if self.CS.acc_active and not self.acc_active_prev: + if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) - if not self.CS.acc_active: + if not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) else: - if self.CS.brake_error: - events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - if not self.CS.gear_shifter_valid: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not self.CS.main_on: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if self.CS.gear_shifter == 3: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + # TODO: why is this only not supercruise? ignore supercruise? if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: @@ -313,15 +190,15 @@ class CarInterface(CarInterfaceBase): ret.events = events # update previous brake/gas pressed - self.acc_active_prev = self.CS.acc_active + self.cruise_enabled_prev = ret.cruiseState.enabled self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed - # cast to reader so it can't be modified - return ret.as_reader() + # copy back carState packet to CS + self.CS.out = ret.as_reader() + + return self.CS.out - # pass in a car.CarControl - # to be called @ 100hz def apply(self, c): hud_v_cruise = c.hudControl.setSpeed if hud_v_cruise > 70: @@ -329,7 +206,7 @@ class CarInterface(CarInterfaceBase): # For Openpilot, "enabled" includes pre-enable. # In GM, PCM faults out if ACC command overlaps user gas. - enabled = c.enabled and not self.CS.user_gas_pressed + enabled = c.enabled and not self.CS.out.gasPressed can_sends = self.CC.update(enabled, self.CS, self.frame, \ c.actuators, diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index d36db9422..b20706a05 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -4,8 +4,7 @@ import math import time from cereal import car from opendbc.can.parser import CANParser -from selfdrive.car.gm.interface import CanBus -from selfdrive.car.gm.values import DBC, CAR +from selfdrive.car.gm.values import DBC, CAR, CanBus from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import RadarInterfaceBase @@ -17,7 +16,7 @@ NUM_SLOTS = 20 # messages that are present in DBC LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS -def create_radar_can_parser(canbus, car_fingerprint): +def create_radar_can_parser(car_fingerprint): dbc_f = DBC[car_fingerprint]['radar'] if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): @@ -38,7 +37,7 @@ def create_radar_can_parser(canbus, car_fingerprint): checks = [] - return CANParser(dbc_f, signals, checks, canbus.obstacle) + return CANParser(dbc_f, signals, checks, CanBus.OBSTACLE) else: return None @@ -49,9 +48,7 @@ class RadarInterface(RadarInterfaceBase): self.delay = 0 # Delay of radar - canbus = CanBus() - print("Using %d as obstacle CAN bus ID" % canbus.obstacle) - self.rcp = create_radar_can_parser(canbus, CP.carFingerprint) + self.rcp = create_radar_can_parser(CP.carFingerprint) self.trigger_msg = LAST_RADAR_MSG self.updated_messages = set() diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index ccde62ea3..c48ed4e1f 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -27,6 +27,12 @@ class AccState: FAULTED = 3 STANDSTILL = 4 +class CanBus: + POWERTRAIN = 0 + OBSTACLE = 1 + CHASSIS = 2 + SW_GMLAN = 3 + def is_eps_status_ok(eps_status, car_fingerprint): valid_eps_status = [] if car_fingerprint in SUPERCRUISE_CARS: diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index f8bf0f749..956f73cdd 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -2,7 +2,7 @@ from collections import namedtuple from cereal import car from common.realtime import DT_CTRL from selfdrive.controls.lib.drive_helpers import rate_limit -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from selfdrive.car import create_gas_command from selfdrive.car.honda import hondacan from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD @@ -33,7 +33,7 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): brake_steady = brake + brake_hyst_gap brake = brake_steady - if (car_fingerprint in (CAR.ACURA_ILX, CAR.CRV)) and brake > 0.0: + if (car_fingerprint in (CAR.ACURA_ILX, CAR.CRV, CAR.CRV_EU)) and brake > 0.0: brake += 0.15 return brake, braking, brake_steady @@ -77,9 +77,18 @@ HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "car", "lanes", "fcw", "acc_alert", "steer_required", "dashed_lanes"]) +class CarControllerParams(): + def __init__(self, CP): + self.BRAKE_MAX = 1024//4 + self.STEER_MAX = CP.lateralParams.torqueBP[-1] + # mirror of list (assuming first item is zero) for interp of signed request values + assert(CP.lateralParams.torqueBP[0] == 0) + assert(CP.lateralParams.torqueBP[0] == 0) + self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP) + self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV) class CarController(): - def __init__(self, dbc_name, CP): + def __init__(self, dbc_name, CP, VM): self.braking = False self.brake_steady = 0. self.brake_last = 0. @@ -87,17 +96,15 @@ class CarController(): self.last_pump_ts = 0. self.packer = CANPacker(dbc_name) self.new_radar_config = False - self.eps_modified = False - for fw in CP.carFw: - if fw.ecu == "eps" and b"," in fw.fwVersion: - print("EPS FW MODIFIED!") - self.eps_modified = True + + self.params = CarControllerParams(CP) # dragonpilot self.turning_signal_timer = 0 self.dragon_enable_steering_on_signal = False self.dragon_lat_ctrl = True self.dp_last_modified = None + self.lane_change_enabled = True def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ @@ -106,15 +113,24 @@ class CarController(): if frame % 500 == 0: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.lane_change_enabled = False if params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if not self.lane_change_enabled: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = False + else: + self.dragon_enable_steering_on_signal = False self.dp_last_modified = modified + P = self.params + # *** apply brake hysteresis *** - brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) + brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** - if not enabled and CS.pcm_acc_status: + if not enabled and CS.out.cruiseState.enabled: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True @@ -142,29 +158,10 @@ class CarController(): # **** process the car messages **** - # *** compute control surfaces *** - BRAKE_MAX = 1024//4 - if CS.CP.carFingerprint in (CAR.ACURA_ILX): - STEER_MAX = 0xF00 - elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): - STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value - elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): - STEER_MAX = 0x7FFF - elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: - STEER_MAX = 0x1400 - else: - STEER_MAX = 0x1000 - # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) - apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) - apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) - - if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: - if apply_steer > 0xA00: - apply_steer = (apply_steer - 0xA00) / 2 + 0xA00 - elif apply_steer < -0xA00: - apply_steer = (apply_steer + 0xA00) / 2 - 0xA00 + apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) + apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode @@ -174,7 +171,7 @@ class CarController(): # dragonpilot if enabled: if self.dragon_enable_steering_on_signal: - if CS.left_blinker_on == 0 and CS.right_blinker_on == 0: + if not CS.out.leftBlinker and not CS.out.rightBlinker: self.turning_signal_timer = 0 else: self.turning_signal_timer = 100 @@ -202,7 +199,7 @@ class CarController(): # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) - elif CS.stopped: + elif CS.out.cruiseState.standstill: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 077263ca0..f78e4ef6a 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,3 +1,4 @@ +from cereal import car from collections import defaultdict from common.numpy_fast import interp from opendbc.can.can_define import CANDefine @@ -69,7 +70,7 @@ def get_can_signals(CP): ("SCM_BUTTONS", 25), ] - if CP.carFingerprint == CAR.CRV_HYBRID: + if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL): checks += [ ("GEARBOX", 50), ] @@ -80,7 +81,7 @@ def get_can_signals(CP): if CP.radarOffCan: # Civic is only bosch to use the same brake message as other hondas. - if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): + if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)] checks += [("BRAKE_MODULE", 50)] signals += [("CAR_GAS", "GAS_PEDAL_2", 0), @@ -102,7 +103,7 @@ def get_can_signals(CP): else: checks += [("CRUISE_PARAMS", 50)] - if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): + if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)] elif CP.carFingerprint == CAR.ODYSSEY_CHN: signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)] @@ -122,7 +123,7 @@ def get_can_signals(CP): elif CP.carFingerprint == CAR.ACURA_ILX: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_BUTTONS", 0)] - elif CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX, CAR.PILOT_2019, CAR.RIDGELINE): + elif CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.PILOT_2019, CAR.RIDGELINE): signals += [("MAIN_ON", "SCM_BUTTONS", 0)] elif CP.carFingerprint == CAR.FIT: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), @@ -151,37 +152,6 @@ def get_can_signals(CP): return signals, checks -def get_can_parser(CP): - signals, checks = get_can_signals(CP) - bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0 - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt) - - -def get_cam_can_parser(CP): - signals = [] - - if CP.carFingerprint in HONDA_BOSCH: - signals += [("ACCEL_COMMAND", "ACC_CONTROL", 0), - ("AEB_STATUS", "ACC_CONTROL", 0)] - else: - signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND", 0), - ("AEB_REQ_1", "BRAKE_COMMAND", 0), - ("FCW", "BRAKE_COMMAND", 0), - ("CHIME", "BRAKE_COMMAND", 0), - ("FCM_OFF", "ACC_HUD", 0), - ("FCM_OFF_2", "ACC_HUD", 0), - ("FCM_PROBLEM", "ACC_HUD", 0), - ("ICONS", "ACC_HUD", 0)] - - - # all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering - checks = [(0xe4, 100)] - if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: - checks = [(0x194, 100)] - - bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2 - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam) - class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) @@ -195,12 +165,12 @@ class CarState(CarStateBase): self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 self.cruise_mode = 0 - self.stopped = 0 # dragonpilot self.lkMode = True def update(self, cp, cp_cam): + ret = car.CarState.new_message() # car params v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping @@ -209,21 +179,19 @@ class CarState(CarStateBase): # update prevs, update must run once per loop self.prev_cruise_buttons = self.cruise_buttons self.prev_cruise_setting = self.cruise_setting - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on # ******************* parse out can ******************* - if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): # TODO: find wheels moving bit in dbc - self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 - self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'] + if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): # TODO: find wheels moving bit in dbc + ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 + ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']) elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: - self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 - self.door_all_closed = not cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'] + ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 + ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN']) else: - self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] - self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], - cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) - self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'] + ret.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] + ret.doorOpen = any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], + cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) + ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] or not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED']) steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]['STEER_STATUS']] self.steer_error = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT'] @@ -236,31 +204,22 @@ class CarState(CarStateBase): self.brake_error = 0 else: self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2'] - self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED'] + ret.espDisabled = cp.vl["VSA_STATUS"]['ESP_DISABLED'] != 0 speed_factor = SPEED_FACTOR[self.CP.carFingerprint] - self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor - self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor - self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor - self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor - v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr)/4. + ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor + ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor + ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor + ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor + v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr)/4. # blend in transmission speed at low speed, since it has more low speed accuracy - self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v) - self.v_ego_raw = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \ - self.v_weight * v_wheel + v_weight = interp(v_wheel, v_weight_bp, v_weight_v) + ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) - - # this is a hack for the interceptor. This is now only used in the simulation - # TODO: Replace tests by toyota so this can go away - if self.CP.enableGasInterceptor: - self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. - self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change - - self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR'] - self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] - self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] + ret.steeringAngle = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + ret.steeringRate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] # when user presses LKAS button on steering wheel if self.cruise_setting == 1: @@ -272,85 +231,126 @@ class CarState(CarStateBase): self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] - self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] - self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] - self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] + ret.leftBlinker = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] != 0 + ret.rightBlinker = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] != 0 self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] - if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): + if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 - self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] + main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 - self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] + main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] else: self.park_brake = 0 # TODO - self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] + main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] - can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER']) - self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None)) + gear = int(cp.vl["GEARBOX"]['GEAR_SHIFTER']) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None)) self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS'] # crv doesn't include cruise control - if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN): - self.car_gas = self.pedal_gas + if self.CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN): + ret.gas = self.pedal_gas / 256. else: - self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] + ret.gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] / 256. - self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] - self.steer_torque_motor = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE'] - self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint] + # this is a hack for the interceptor. This is now only used in the simulation + # TODO: Replace tests by toyota so this can go away + if self.CP.enableGasInterceptor: + self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. + self.user_gas_pressed = self.user_gas > 1e-5 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change + ret.gasPressed = self.user_gas_pressed + else: + ret.gasPressed = self.pedal_gas > 1e-5 - self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + ret.steeringTorque = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] + ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE'] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.CP.carFingerprint] + + self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != 0 if self.CP.radarOffCan: self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL'] - self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. - self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego) - if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID): - self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] - self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ + ret.cruiseState.standstill = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. + ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo) + if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.ACCORDH, CAR.CRV_HYBRID, CAR.INSIGHT): + ret.brakePressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] != 0 or \ (self.brake_switch and self.brake_switch_prev and \ cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) self.brake_switch_prev = self.brake_switch self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] else: - self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] + ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0 # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this. - self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] - self.v_cruise_pcm_prev = self.v_cruise_pcm + ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] * CV.KPH_TO_MS + self.v_cruise_pcm_prev = ret.cruiseState.speed else: - self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] - self.cruise_speed_offset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego) - self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] + ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], ret.vEgo) + ret.cruiseState.speed = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] * CV.KPH_TO_MS # brake switch has shown some single time step noise, so only considered when # switch is on for at least 2 consecutive CAN samples - self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ - (self.brake_switch and self.brake_switch_prev and \ - cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) + ret.brakePressed = bool(cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or + (self.brake_switch and self.brake_switch_prev and + cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)) self.brake_switch_prev = self.brake_switch self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] - self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] - self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] + ret.brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] + ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] != 0 + ret.cruiseState.available = bool(main_on) and self.cruise_mode == 0 # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE): - if self.user_brake > 0.05: - self.brake_pressed = 1 + if ret.brake > 0.05: + ret.brakePressed = True # TODO: discover the CAN msg that has the imperial unit bit for all other cars self.is_metric = not cp.vl["HUD_SETTING"]['IMPERIAL_UNIT'] if self.CP.carFingerprint in (CAR.CIVIC) else False if self.CP.carFingerprint in HONDA_BOSCH: - self.stock_aeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5) + ret.stockAeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5) else: - self.stock_aeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5) + ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5) if self.CP.carFingerprint in HONDA_BOSCH: self.stock_hud = False - self.stock_fcw = False + ret.stockFcw = False else: - self.stock_fcw = bool(cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0) + ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0 self.stock_hud = cp_cam.vl["ACC_HUD"] self.stock_brake = cp_cam.vl["BRAKE_COMMAND"] + + return ret + + @staticmethod + def get_can_parser(CP): + signals, checks = get_can_signals(CP) + bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0 + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt) + + @staticmethod + def get_cam_can_parser(CP): + signals = [] + + if CP.carFingerprint in HONDA_BOSCH: + signals += [("ACCEL_COMMAND", "ACC_CONTROL", 0), + ("AEB_STATUS", "ACC_CONTROL", 0)] + else: + signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND", 0), + ("AEB_REQ_1", "BRAKE_COMMAND", 0), + ("FCW", "BRAKE_COMMAND", 0), + ("CHIME", "BRAKE_COMMAND", 0), + ("FCM_OFF", "ACC_HUD", 0), + ("FCM_OFF_2", "ACC_HUD", 0), + ("FCM_PROBLEM", "ACC_HUD", 0), + ("ICONS", "ACC_HUD", 0)] + + + # all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering + checks = [(0xe4, 100)] + if CP.carFingerprint in [CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: + checks = [(0x194, 100)] + + bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2 + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index e005c6c40..ef7da044e 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -2,24 +2,18 @@ import numpy as np from cereal import car from common.numpy_fast import clip, interp -from common.realtime import DT_CTRL, sec_since_boot +from common.realtime import DT_CTRL from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, Ecu, ECU_FINGERPRINT, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING from selfdrive.car.interfaces import CarInterfaceBase -from common.params import Params -params = Params() -from selfdrive.dragonpilot.dragonconf import dp_get_last_modified A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) ButtonType = car.CarState.ButtonEvent.Type -GearShifter = car.CarState.GearShifter def compute_gb_honda(accel, speed): creep_brake = 0.0 @@ -76,38 +70,17 @@ def get_compute_gb_acura(): class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) - self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - - self.cp = get_can_parser(CP) - self.cp_cam = get_cam_can_parser(CP) - - # *** init the major players *** - self.CS = CarState(CP) - self.VM = VehicleModel(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP) if self.CS.CP.carFingerprint == CAR.ACURA_ILX: self.compute_gb = get_compute_gb_acura() else: self.compute_gb = compute_gb_honda - # dragonpilot - self.dragon_enable_steering_on_signal = False - self.dragon_allow_gas = False - self.ts_last_check = 0. - self.dragon_lat_ctrl = True - self.dp_last_modified = None - @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): @@ -143,10 +116,8 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - ret = car.CarParams.new_message() + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "honda" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay if candidate in HONDA_BOSCH: ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness if has_relay else car.CarParams.SafetyModel.hondaBoschGiraffe @@ -170,7 +141,7 @@ class CarInterface(CarInterfaceBase): # which improves controls quality as it removes the steering column torsion from feedback. # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" - + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]] ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward @@ -179,16 +150,40 @@ class CarInterface(CarInterfaceBase): if fw.ecu == "eps" and b"," in fw.fwVersion: eps_modified = True - if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]: + if candidate == CAR.CIVIC: stop_and_go = True ret.mass = CivicParams.MASS ret.wheelbase = CivicParams.WHEELBASE ret.centerToFront = CivicParams.CENTER_TO_FRONT ret.steerRatio = 15.38 # 10.93 is end-to-end spec + if eps_modified: + # stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE + # stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680 + # modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180 + # stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108 + # modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480 + # note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] + else: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] tire_stiffness_factor = 1. - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if eps_modified else [[0.8], [0.24]] - ret.lateralTuning.pid.kf = 0.00006 + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.54, 0.36] + + elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL): + stop_and_go = True + ret.mass = CivicParams.MASS + ret.wheelbase = CivicParams.WHEELBASE + ret.centerToFront = CivicParams.CENTER_TO_FRONT + ret.steerRatio = 15.38 # 10.93 is end-to-end spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 1. + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiBP = [0., 35.] @@ -202,6 +197,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 16.33 # 11.82 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.8467 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -215,6 +211,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 18.61 # 15.3 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.72 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -222,12 +219,13 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] - elif candidate == CAR.CRV: + elif candidate in (CAR.CRV, CAR.CRV_EU): stop_and_go = False ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.89 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -242,8 +240,16 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end + if eps_modified: + # stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F + # stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400 + # modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] + else: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]] tire_stiffness_factor = 0.677 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] @@ -256,6 +262,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.677 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -269,6 +276,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.53 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 13.06 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.75 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.06]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -282,6 +290,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -295,6 +304,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -308,6 +318,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.90 ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY ret.steerRatio = 14.35 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -321,6 +332,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.82 ret.centerToFront = ret.wheelbase * 0.428 ret.steerRatio = 17.25 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -334,6 +346,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.59 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -341,6 +354,21 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] + elif candidate == CAR.INSIGHT: + stop_and_go = True + ret.mass = 2987. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.centerToFront = ret.wheelbase * 0.39 + ret.steerRatio = 15.0 # 12.58 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.82 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] + + else: raise ValueError("unsupported car %s" % candidate) @@ -360,21 +388,11 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. - - # no max steer limit VS speed - ret.steerMaxBP = [0.] # m/s - ret.steerMaxV = [1.] # max steer allowed - ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.stoppingControl = True ret.startAccel = 0.5 @@ -386,94 +404,21 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): - # dragonpilot, don't check for param too often as it's a kernel call - ts = sec_since_boot() - if ts - self.ts_last_check >= 5.: - modified = dp_get_last_modified() - if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else True - self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False - self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True - self.dp_last_modified = modified - self.ts_last_check = ts - + self.dp_load_params('honda') # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - self.CS.update(self.cp, self.cp_cam) - - # create message - ret = car.CarState.new_message() + ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid - - # speeds - ret.vEgo = self.CS.v_ego - ret.aEgo = self.CS.a_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # gas pedal - ret.gas = self.CS.car_gas / 256.0 - if not self.CP.enableGasInterceptor: - ret.gasPressed = self.CS.pedal_gas > 0 - else: - ret.gasPressed = self.CS.user_gas_pressed - - # brake pedal - ret.brake = self.CS.user_brake - ret.brakePressed = self.CS.brake_pressed != 0 + ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) - # steering wheel - ret.steeringAngle = self.CS.angle_steers - ret.steeringRate = self.CS.angle_steers_rate - - # gear shifter lever - ret.gearShifter = self.CS.gear_shifter - - ret.steeringTorque = self.CS.steer_torque_driver - ret.steeringTorqueEps = self.CS.steer_torque_motor - ret.steeringPressed = self.CS.steer_override - - # cruise state - ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 - ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS - ret.cruiseState.available = bool(self.CS.main_on) and not bool(self.CS.cruise_mode) - ret.cruiseState.speedOffset = self.CS.cruise_speed_offset - ret.cruiseState.standstill = False - - # TODO: button presses buttonEvents = [] - ret.leftBlinker = bool(self.CS.left_blinker_on) - ret.rightBlinker = bool(self.CS.right_blinker_on) - - ret.doorOpen = not self.CS.door_all_closed - ret.seatbeltUnlatched = not self.CS.seatbelt - - ret.stockAeb = self.CS.stock_aeb - ret.stockFcw = self.CS.stock_fcw - - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on != 0 - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on != 0 - buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() @@ -510,29 +455,9 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = buttonEvents # events - events = [] - if not self.CS.lkMode or not self.dragon_lat_ctrl: - events.append(create_event('manualSteeringRequired', [ET.WARNING])) - elif self.CS.lkMode and (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal: - events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) - elif self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - elif self.CS.steer_warning: - events.append(create_event('steerTempUnavailable', [ET.WARNING])) + events = self.create_common_events(ret) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - if not ret.gearShifter == GearShifter.drive: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not self.CS.main_on or self.CS.cruise_mode: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gearShifter == GearShifter.reverse: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: @@ -541,19 +466,6 @@ class CarInterface(CarInterfaceBase): if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) - # DragonAllowGas - if not self.dragon_allow_gas: - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - else: - if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl): @@ -599,8 +511,8 @@ class CarInterface(CarInterfaceBase): self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed - # cast to reader so it can't be modified - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 5c7562267..665aca808 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -27,9 +27,11 @@ class CAR: ACCORDH = "HONDA ACCORD 2018 HYBRID TOURING" CIVIC = "HONDA CIVIC 2016 TOURING" CIVIC_BOSCH = "HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019" + CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL" ACURA_ILX = "ACURA ILX 2016 ACURAWATCH PLUS" CRV = "HONDA CR-V 2016 TOURING" CRV_5G = "HONDA CR-V 2017 EX" + CRV_EU = "HONDA CR-V 2016 EXECUTIVE" CRV_HYBRID = "HONDA CR-V 2019 HYBRID" FIT = "HONDA FIT 2018 EX" ODYSSEY = "HONDA ODYSSEY 2018 EX-L" @@ -38,6 +40,7 @@ class CAR: PILOT = "HONDA PILOT 2017 TOURING" PILOT_2019 = "HONDA PILOT 2019 ELITE" RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION" + INSIGHT = "HONDA INSIGHT 2019 TOURING" # diag message that in some Nidec cars only appear with 1s freq if VIN query is performed DIAG_MSGS = {1600: 5, 1601: 8} @@ -50,13 +53,6 @@ FINGERPRINTS = { 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8 }], CAR.ACCORDH: [{ - 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8 - }, - { - 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1416: 5, 1600: 5, 1601: 8, 1652: 8 - }, - { - # Honda Inspire Hybrid 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 408: 6, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 475: 8, 477: 8, 479: 8, 481: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1416: 5, 1600: 5, 1601: 8, 1652: 8 }], CAR.ACURA_ILX: [{ @@ -73,17 +69,17 @@ FINGERPRINTS = { # 2017 Civic Hatchback EX, 2019 Civic Sedan Touring Canadian, and 2018 Civic Hatchback Executive Premium 1.0L CVT European 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1629: 5, 1633: 8, }, - # Manual CIVIC from AlexNoop - { - 57: 3, 148: 8, 228: 5, 274: 3, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1029: 8, 1036: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1633: 8, - }, + # # Manual CIVIC from AlexNoop + # { + # 57: 3, 148: 8, 228: 5, 274: 3, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1029: 8, 1036: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1633: 8, + # }, # 2017 Civic Hatchback LX { 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 815: 8, 825: 4, 829: 5, 846: 8, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 892: 8, 918: 7, 927: 8, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1092: 1, 1108: 8, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8 - }, - # 2019 Civic Hatchback EX - { - 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8 + }], + CAR.CIVIC_BOSCH_DIESEL: [{ + # 2019 Civic Sedan 1.6 i-dtec Diesel European + 57: 3, 148: 8, 228: 5, 308: 5, 316: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 426: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 507: 1, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 801: 3, 804: 8, 806: 8, 808: 8, 815: 8, 824: 8, 825: 4, 829: 5, 837: 5, 862: 8, 881: 8, 882: 4, 884: 8, 887: 8, 888: 8, 891: 8, 902: 8, 918: 7, 927: 8, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1092: 1, 1108: 8, 1115: 2, 1125: 8, 1296: 8, 1302: 8, 1322: 5, 1337: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8 }], CAR.CRV: [{ 57: 3, 145: 8, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 401: 8, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8, @@ -91,6 +87,10 @@ FINGERPRINTS = { CAR.CRV_5G: [{ 57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 2, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5 }], + # 1057: 5 1024: 5 are also on the OBD2 bus. their lengths differ from the camera's f-can bus. re-fingerprint after obd2 connection is split in panda firmware from bus 1. + CAR.CRV_EU: [{ + 57: 3, 145: 8, 308: 5, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 404: 4, 419: 8, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 510: 3, 538: 3, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 768: 8, 769: 8, 773: 7, 777: 8, 780: 8, 800: 8, 801: 3, 803: 8, 804: 8, 808: 8, 824: 8, 829: 5, 837: 5, 862: 8, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 930: 8, 931: 8, 983: 8, 1024: 8, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1040: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1045: 8, 1046: 8, 1047: 8, 1056: 8, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1064: 7, 1072: 8, 1073: 8, 1074: 8, 1075: 8, 1076: 8, 1077: 8, 1078: 8, 1079: 8, 1080: 8, 1081: 8, 1088: 8, 1089: 8, 1090: 8, 1091: 8, 1092: 8, 1093: 8, 1108: 8, 1125: 8, 1279: 8, 1280: 8, 1296: 8, 1297: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8, + }], CAR.CRV_HYBRID: [{ 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 408: 6, 415: 6, 419: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 490: 8, 495: 8, 525: 8, 531: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 814: 4, 829: 5, 833: 6, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 930: 8, 931: 8, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1626: 5, 1627: 5 }], @@ -131,9 +131,16 @@ FINGERPRINTS = { # 2019 Ridgeline { 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422:8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 424: 5, 1613: 5, 1616: 5, 1618: 5, 1623: 5, 1668: 5 + }], + # 2019 Insight + CAR.INSIGHT: [{ + 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 476: 8, 477: 8, 479: 8, 490: 8, 495: 8, 507: 1, 525: 8, 531: 8, 545: 6, 547: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 829: 5, 832: 3, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 954: 2, 985: 3, 1029: 8, 1093: 4, 1115: 2, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1652: 8, 2015: 3 }] } +# Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection +IGNORED_FINGERPRINTS = [CAR.INSIGHT, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_EU] + # add DIAG_MSGS to fingerprints for c in FINGERPRINTS: for f, _ in enumerate(FINGERPRINTS[c]): @@ -143,74 +150,105 @@ for c in FINGERPRINTS: # TODO: Figure out what is relevant FW_VERSIONS = { CAR.ACCORD: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-6A0-A640\x00\x00', b'37805-6B2-A550\x00\x00', b'37805-6B2-A650\x00\x00', b'37805-6B2-A660\x00\x00', b'37805-6B2-M520\x00\x00', ], - (Ecu.unknown, 0x18da0bf1, None): [b'54008-TVC-A910\x00\x00'], - (Ecu.unknown, 0x18da1ef1, None): [b'28102-6B8-A560\x00\x00', b'28102-6B8-M520\x00\x00'], - (Ecu.unknown, 0x18da2bf1, None): [b'46114-TVA-A060\x00\x00', b'46114-TVA-A080\x00\x00'], - (Ecu.unknown, 0x18da28f1, None): [b'57114-TVA-C050\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TVC-A910\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28102-6B8-A560\x00\x00', + b'28102-6B8-M520\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'46114-TVA-A060\x00\x00', + b'46114-TVA-A080\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TVA-C050\x00\x00', + ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TVA-A150\x00\x00', b'39990-TVA-A160\x00\x00', b'39990-TVA-X030\x00\x00', ], - (Ecu.unknown, 0x18da3af1, None): [b'39390-TVA-A020\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TVA-A460\x00\x00', b'77959-TVA-X330\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.unknown, 0x18da3af1, None): [ + b'39390-TVA-A020\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TVA-A460\x00\x00', + b'77959-TVA-X330\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TVA-A210\x00\x00', b'78109-TVC-A010\x00\x00', b'78109-TVC-A110\x00\x00', b'78109-TVC-A210\x00\x00', b'78109-TVC-M510\x00\x00', ], - (Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TVA-A010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TVA-A160\x00\x00', b'36802-TVA-A160\x00\x00', b'36802-TVA-A170\x00\x00', b'36802-TWA-A070\x00\x00', ], - (Ecu.unknown, 0x18dab5f1, None): [b'36161-TVA-A060\x00\x00', b'36161-TWA-A070\x00\x00'], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TVA-A010\x00\x00'], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TVA-A060\x00\x00', + b'36161-TWA-A070\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TVA-A010\x00\x00', + ], }, CAR.ACCORD_15: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-6A0-9620\x00\x00', b'37805-6A0-A640\x00\x00', b'37805-6A0-A740\x00\x00', b'37805-6A0-A840\x00\x00', b'37805-6A0-A850\x00\x00', ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-6A7-A220\x00\x00', b'28101-6A7-A320\x00\x00', b'28101-6A7-A510\x00\x00', ], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TVA-A230\x00\x00'], - (Ecu.unknown, 0x18da2bf1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TVA-A230\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ b'46114-TVA-A050\x00\x00', b'46114-TVA-A060\x00\x00', b'46114-TVA-A120\x00\x00', ], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TVA-A010\x00\x00', b'78109-TVA-A210\x00\x00', b'78109-TVA-A220\x00\x00', b'78109-TVA-A310\x00\x00', b'78109-TWA-A210\x00\x00', ], - (Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'], - (Ecu.unknown, 0x18dab5f1, None): [b'36161-TVA-A060\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TVA-A460\x00\x00'], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TVA-A010\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TVA-A060\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TVA-A460\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TVA-B050\x00\x00', b'57114-TVA-B040\x00\x00', ], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TVA-A150\x00\x00', b'36802-TVA-A160\x00\x00', b'36802-TVA-A170\x00\x00', @@ -222,23 +260,41 @@ FW_VERSIONS = { ], }, CAR.ACCORDH: { - (Ecu.unknown, 0x18daeff1, None): [b'38897-TWA-A120\x00\x00'], - (Ecu.unknown, 0x18da28f1, None): [b'57114-TWA-A040\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TWA-A440\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TWA-A120\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TWA-A040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TWA-A440\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TWA-A010\x00\x00', b'78109-TWA-A120\x00\x00', b'78109-TWA-A210\x00\x00', b'78109-TWA-A110\x00\x00', ], - (Ecu.unknown, 0x18da0bf1, None): [b'54008-TWA-A910\x00\x00'], - (Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'], - (Ecu.unknown, 0x18dab5f1, None): [b'36161-TWA-A070\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [b'36802-TWA-A080\x00\x00', b'36802-TWA-A070\x00\x00'], - (Ecu.eps, 0x18da30f1, None): [b'39990-TVA-A160\x00\x00', b'39990-TVA-A150\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TWA-A910\x00\x00', + ], + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TVA-A010\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TWA-A070\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TWA-A080\x00\x00', + b'36802-TWA-A070\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TVA-A160\x00\x00', + b'39990-TVA-A150\x00\x00', + ], }, CAR.CIVIC: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5AA-A640\x00\x00', b'37805-5AA-A650\x00\x00', b'37805-5AA-A670\x00\x00', @@ -251,7 +307,7 @@ FW_VERSIONS = { b'37805-5BA-L940\x00\x00', b'37805-5BA-L960\x00\x00', ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A040\x00\x00', b'28101-5CG-A050\x00\x00', b'28101-5CG-A070\x00\x00', @@ -262,10 +318,10 @@ FW_VERSIONS = { b'28101-5DJ-A060\x00\x00', b'28101-5DJ-A510\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TBA-A550\x00\x00', b'57114-TBA-A560\x00\x00', - b'57114-TBA-A570\x00\x00' + b'57114-TBA-A570\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TBA,A030\x00\x00', @@ -273,12 +329,12 @@ FW_VERSIONS = { b'39990-TBG-A030\x00\x00', b'39990-TEG-A010\x00\x00', ], - (Ecu.unknown, 0x18da53f1, None): [ + (Ecu.srs, 0x18da53f1, None): [ b'77959-TBA-A030\x00\x00', b'77959-TBA-A040\x00\x00', b'77959-TBG-A030\x00\x00', ], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TBC-A310\x00\x00', b'78109-TBC-A320\x00\x00', b'78109-TBC-A510\x00\x00', @@ -288,32 +344,34 @@ FW_VERSIONS = { b'78109-TBH-A530\x00\x00', b'78109-TEG-A310\x00\x00', ], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-TBA-A030\x00\x00', b'36161-TBC-A020\x00\x00', b'36161-TBC-A030\x00\x00', b'36161-TEG-A010\x00\x00', ], - (Ecu.unknown, 0x18daeff1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ b'38897-TBA-A010\x00\x00', b'38897-TBA-A020\x00\x00', ], }, CAR.CIVIC_BOSCH: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5AA-A950\x00\x00', b'37805-5AA-L950\x00\x00', b'37805-5AN-A750\x00\x00', b'37805-5AN-A830\x00\x00', b'37805-5AN-A930\x00\x00', + b'37805-5AN-A950\x00\x00', b'37805-5AN-L940\x00\x00', b'37805-5AN-LH20\x00\x00', b'37805-5AN-LJ20\x00\x00', b'37805-5AZ-E850\x00\x00', b'37805-5BB-L640\x00\x00', + b'37805-5AN-AH20\x00\x00', b'37805-5AN-E410\x00\x00', # AlexNoop's Manual CIVIC_BOSCH ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A920\x00\x00', b'28101-5CG-C220\x00\x00', b'28101-5CG-C320\x00\x00', @@ -324,11 +382,12 @@ FW_VERSIONS = { b'28101-5DJ-A710\x00\x00', b'28101-5DV-E330\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TBG-A340\x00\x00', b'57114-TGG-A340\x00\x00', b'57114-TGL-G330\x00\x00', b'57114-TGG-C320\x00\x00', + b'57114-TGL-G130\x00\x00', # AlexNoop's Manual CIVIC_BOSCH ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TBA-C020\x00\x00', @@ -337,14 +396,15 @@ FW_VERSIONS = { b'39990-TGG-A120\x00\x00', b'39990-TGL-E130\x00\x00', b'39990-TGG-A020\x00\x00', + b'39990-TGN-E120\x00\x00', # AlexNoop's Manual CIVIC_BOSCH ], - (Ecu.unknown, 0x18da53f1, None): [ + (Ecu.srs, 0x18da53f1, None): [ b'77959-TBA-A060\x00\x00', b'77959-TGG-A020\x00\x00', b'77959-TGG-G010\x00\x00', b'77959-TGG-A020\x00\x00', ], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TBA-A910\x00\x00', b'78109-TBC-A740\x00\x00', b'78109-TGG-A210\x00\x00', @@ -353,27 +413,44 @@ FW_VERSIONS = { b'78109-TGG-A810\x00\x00', b'78109-TGG-A820\x00\x00', b'78109-TGL-G120\x00\x00', + b'78109-TGG-BA10\x00\x00', # AlexNoop's Manual CIVIC_BOSCH ], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TBA-A150\x00\x00', b'36802-TGG-A050\x00\x00', b'36802-TGL-G040\x00\x00', b'36802-TGG-A060\x00\x00', + b'36802-TGG-G040\x00\x00', # AlexNoop's Manual CIVIC_BOSCH ], - (Ecu.unknown, 0x18dab5f1, None): [ + (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TBA-A130\x00\x00', b'36161-TGG-A060\x00\x00', b'36161-TGL-G050\x00\x00', b'36161-TGG-A080\x00\x00', + b'36161-TGG-G070\x00\x00', # AlexNoop's Manual CIVIC_BOSCH ], (Ecu.unknown, 0x18daeff1, None): [ b'38897-TBA-A110\x00\x00', b'38897-TBA-A020\x00\x00', b'38897-TBA-A020\x00\x00', ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TBA-A110\x00\x00', + b'38897-TBA-A020\x00\x00', + ], + }, + CAR.CIVIC_BOSCH_DIESEL: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [b'37805-59N-G830\x00\x00'], + (Ecu.transmission, 0x18da1ef1, None): [b'28101-59Y-G620\x00\x00'], + (Ecu.vsa, 0x18da28f1, None): [b'57114-TGN-E320\x00\x00'], + (Ecu.eps, 0x18da30f1, None): [b'39990-TFK-G020\x00\x00'], + (Ecu.srs, 0x18da53f1, None): [b'77959-TFK-G210\x00\x00'], + (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-TFK-G020\x00\x00'], + (Ecu.fwdRadar, 0x18dab0f1, None): [b'36802-TFK-G130\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [b'54008-TGN-E010\x00\x00'], }, CAR.CRV_5G: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5PA-3080\x00\x00', b'37805-5PA-4050\x00\x00', b'37805-5PA-6530\x00\x00', @@ -385,7 +462,7 @@ FW_VERSIONS = { b'37805-5PA-A880\x00\x00', b'37805-5PA-A890\x00\x00', ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5RG-A020\x00\x00', b'28101-5RG-A030\x00\x00', b'28101-5RG-A040\x00\x00', @@ -394,78 +471,127 @@ FW_VERSIONS = { b'28101-5RH-A040\x00\x00', b'28101-5RH-A120\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TLA-A040\x00\x00', b'57114-TLA-A050\x00\x00', b'57114-TLA-A060\x00\x00', ], - (Ecu.eps, 0x18da30f1, None): [b'39990-TLA-A040\x00\x00', b'39990-TLA,A040\x00\x00'], - (Ecu.unknown, 0x18da2bf1, None): [b'46114-TLA-A040\x00\x00', b'46114-TLA-A050\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TLA-A040\x00\x00', + b'39990-TLA,A040\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'46114-TLA-A040\x00\x00', + b'46114-TLA-A050\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TLA-A110\x00\x00', b'78109-TLA-A210\x00\x00', b'78109-TLA-C210\x00\x00', b'78109-TLB-A110\x00\x00', b'78109-TLB-A210\x00\x00', ], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TLA-A010\x00\x00', b'38897-TNY-G010\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TLA-A010\x00\x00', + b'38897-TNY-G010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TLA-A040\x00\x00', b'36802-TLA-A050\x00\x00', b'36802-TLA-A060\x00\x00', ], - (Ecu.unknown, 0x18dab5f1, None): [ + (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TLA-A060\x00\x00', b'36161-TLA-A070\x00\x00', b'36161-TLA-A080\x00\x00', ], - (Ecu.unknown, 0x18da53f1, None): [ + (Ecu.srs, 0x18da53f1, None): [ b'77959-TLA-A240\x00\x00', b'77959-TLA-A250\x00\x00', b'77959-TLA-A320\x00\x00', ], }, + CAR.CRV_EU: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [b'37805-R5Z-G740\x00\x00'], + (Ecu.vsa, 0x18da28f1, None): [b'57114-T1V-G920\x00\x00'], + (Ecu.fwdRadar, 0x18dab0f1, None): [b'36161-T1V-G520\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [b'54008-T1V-G010\x00\x00'], + (Ecu.transmission, 0x18da1ef1, None): [b'28101-5LH-E120\x00\x00'], + (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-T1V-G020\x00\x00'], + (Ecu.srs, 0x18da53f1, None): [b'77959-T1G-G940\x00\x00'], + }, CAR.CRV_HYBRID: { - (Ecu.unknown, 0x18da28f1, None): [b'57114-TPA-G020\x00\x00'], - (Ecu.eps, 0x18da30f1, None): [b'39990-TPA-G030\x00\x00'], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TMA-H110\x00\x00'], - (Ecu.unknown, 0x18da0bf1, None): [b'54008-TMB-H510\x00\x00'], - (Ecu.unknown, 0x18dab5f1, None): [b'36161-TPA-E050\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [b'78109-TPA-G520\x00\x00'], - (Ecu.unknown, 0x18da61f1, None): [b'78209-TLA-X010\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [b'36802-TPA-E040\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TLA-G220\x00\x00'], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TPA-G020\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TPA-G030\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TMA-H110\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TMB-H510\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TPA-E050\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TPA-G520\x00\x00', + ], + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TLA-X010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TPA-E040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TLA-G220\x00\x00', + ], }, CAR.ODYSSEY: { - (Ecu.unknown, 0x18daeff1, None): [b'38897-THR-A010\x00\x00', b'38897-THR-A020\x00\x00'], - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-THR-A010\x00\x00', + b'38897-THR-A020\x00\x00', + ], + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5MR-A250\x00\x00', b'37805-5MR-A310\x00\x00', b'37805-5MR-A750\x00\x00', b'37805-5MR-A840\x00\x00', b'37805-5MR-C620\x00\x00', ], - (Ecu.eps, 0x18da30f1, None): [b'39990-THR-A020\x00\x00', b'39990-THR-A030\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-THR-A010\x00\x00', b'77959-THR-A110\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.eps, 0x18da30f1, None): [ + b'39990-THR-A020\x00\x00', + b'39990-THR-A030\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-THR-A010\x00\x00', + b'77959-THR-A110\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-THR-A030\x00\x00', b'36161-THR-A110\x00\x00', b'36161-THR-A720\x00\x00', b'36161-THR-A810\x00\x00', b'36161-THR-C010\x00\x00', ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5NZ-A310\x00\x00', b'28101-5NZ-C310\x00\x00', b'28102-5MX-A001\x00\x00', b'28102-5MX-A610\x00\x00', b'28102-5MX-A710\x00\x00', + b'28102-5MX-A900\x00\x00', b'28102-5MX-A910\x00\x00', b'28102-5MX-C001\x00\x00', b'28103-5NZ-A300\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [b'57114-THR-A040\x00\x00', b'57114-THR-A110\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-THR-A040\x00\x00', + b'57114-THR-A110\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-THR-A230\x00\x00', b'78109-THR-A430\x00\x00', b'78109-THR-A820\x00\x00', @@ -478,20 +604,31 @@ FW_VERSIONS = { b'78109-THR-C330\x00\x00', b'78109-THR-CE20\x00\x00', ], - (Ecu.unknown, 0x18da0bf1, None): [b'54008-THR-A020\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-THR-A020\x00\x00', + ], }, CAR.PILOT_2019: { - (Ecu.eps, 0x18da30f1, None): [b'39990-TG7-A060\x00\x00', b'39990-TGS-A230\x00\x00'], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TG7-A110\x00\x00', b'38897-TG7-A030\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TG7-A060\x00\x00', + b'39990-TGS-A230\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TG7-A110\x00\x00', + b'38897-TG7-A030\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-TG7-A630\x00\x00', b'36161-TG7-A930\x00\x00', b'36161-TG8-A630\x00\x00', b'36161-TGS-A130\x00\x00', b'36161-TGT-A030\x00\x00', ], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TG7-A210\x00\x00', b'77959-TGS-A010\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TG7-A210\x00\x00', + b'77959-TGS-A010\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TG7-AJ20\x00\x00', b'78109-TG7-AP10\x00\x00', b'78109-TG7-AP20\x00\x00', @@ -500,7 +637,7 @@ FW_VERSIONS = { b'78109-TGS-AP20\x00\x00', b'78109-TGT-AJ20\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TG7-A630\x00\x00', b'57114-TG7-A730\x00\x00', b'57114-TG8-A630\x00\x00', @@ -509,12 +646,38 @@ FW_VERSIONS = { ], }, CAR.RIDGELINE: { - (Ecu.eps, 0x18da30f1, None): [b'39990-T6Z-A020\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [b'36161-T6Z-A310\x00\x00'], - (Ecu.unknown, 0x18daeff1, None): [b'38897-T6Z-A010\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [b'78109-T6Z-A420\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-T6Z-A020\x00\x00'], - (Ecu.unknown, 0x18da28f1, None): [b'57114-T6Z-A130\x00\x00'], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-T6Z-A020\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-T6Z-A310\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-T6Z-A010\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T6Z-A420\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T6Z-A020\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-T6Z-A130\x00\x00', + ], + }, + CAR.INSIGHT: { + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TXM-A040\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TXM-A070\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TXM-A050\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TXM-A230\x00\x00', + ], }, } @@ -526,8 +689,10 @@ DBC = { CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), + CAR.CIVIC_BOSCH_DIESEL: dbc_dict('honda_civic_sedan_16_diesel_2019_can_generated', None), CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None), + CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None), CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), @@ -535,6 +700,7 @@ DBC = { CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'), + CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None), } STEER_THRESHOLD = { @@ -545,8 +711,10 @@ STEER_THRESHOLD = { CAR.ACURA_RDX: 400, CAR.CIVIC: 1200, CAR.CIVIC_BOSCH: 1200, + CAR.CIVIC_BOSCH_DIESEL: 1200, CAR.CRV: 1200, CAR.CRV_5G: 1200, + CAR.CRV_EU: 400, CAR.CRV_HYBRID: 1200, CAR.FIT: 1200, CAR.ODYSSEY: 1200, @@ -554,6 +722,7 @@ STEER_THRESHOLD = { CAR.PILOT: 1200, CAR.PILOT_2019: 1200, CAR.RIDGELINE: 1200, + CAR.INSIGHT: 1200, } SPEED_FACTOR = { @@ -564,8 +733,10 @@ SPEED_FACTOR = { CAR.ACURA_RDX: 1., CAR.CIVIC: 1., CAR.CIVIC_BOSCH: 1., + CAR.CIVIC_BOSCH_DIESEL: 1., CAR.CRV: 1.025, CAR.CRV_5G: 1.025, + CAR.CRV_EU: 1.025, CAR.CRV_HYBRID: 1.025, CAR.FIT: 1., CAR.ODYSSEY: 1., @@ -573,6 +744,7 @@ SPEED_FACTOR = { CAR.PILOT: 1., CAR.PILOT_2019: 1., CAR.RIDGELINE: 1., + CAR.INSIGHT: 1., } # msgs sent for steering controller by camera module on can 0. @@ -581,4 +753,4 @@ ECU_FINGERPRINT = { Ecu.fwdCamera: [0xE4, 0x194], # steer torque cmd } -HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_5G, CAR.CRV_HYBRID] +HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT] diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index fc0e49b3d..285e835ae 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -7,9 +7,9 @@ params = Params() from selfdrive.dragonpilot.dragonconf import dp_get_last_modified class CarController(): - def __init__(self, dbc_name, car_fingerprint): + def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 - self.car_fingerprint = car_fingerprint + self.car_fingerprint = CP.carFingerprint self.lkas11_cnt = 0 self.cnt = 0 self.last_resume_cnt = 0 @@ -21,6 +21,7 @@ class CarController(): self.dragon_enable_steering_on_signal = False self.dragon_lat_ctrl = True self.dp_last_modified = None + self.lane_change_enabled = True def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert): @@ -28,13 +29,20 @@ class CarController(): if frame % 500 == 0: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.lane_change_enabled = False if params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if not self.lane_change_enabled: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = False + else: + self.dragon_enable_steering_on_signal = False self.dp_last_modified = modified ### Steering Torque new_steer = actuators.steer * SteerLimitParams.STEER_MAX - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer if not enabled: @@ -49,7 +57,7 @@ class CarController(): # dragonpilot if enabled: if self.dragon_enable_steering_on_signal: - if CS.left_blinker_on == 0 and CS.right_blinker_on == 0: + if not CS.out.leftBlinker and not CS.out.rightBlinker: self.turning_signal_timer = 0 else: self.turning_signal_timer = 100 @@ -71,7 +79,7 @@ class CarController(): if pcm_cancel_cmd: can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) - elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5: + elif CS.out.cruiseState.standstill and (self.cnt - self.last_resume_cnt) > 5: self.last_resume_cnt = self.cnt can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 5305396f3..59c825f44 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -1,221 +1,222 @@ from cereal import car -from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD +from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV GearShifter = car.CarState.GearShifter -def get_can_parser(CP): - - signals = [ - # sig_name, sig_address, default - ("WHL_SPD_FL", "WHL_SPD11", 0), - ("WHL_SPD_FR", "WHL_SPD11", 0), - ("WHL_SPD_RL", "WHL_SPD11", 0), - ("WHL_SPD_RR", "WHL_SPD11", 0), - - ("YAW_RATE", "ESP12", 0), - - ("CF_Gway_DrvSeatBeltInd", "CGW4", 1), - - ("CF_Gway_DrvSeatBeltSw", "CGW1", 0), - ("CF_Gway_TSigLHSw", "CGW1", 0), - ("CF_Gway_TurnSigLh", "CGW1", 0), - ("CF_Gway_TSigRHSw", "CGW1", 0), - ("CF_Gway_TurnSigRh", "CGW1", 0), - ("CF_Gway_ParkBrakeSw", "CGW1", 0), - - ("BRAKE_ACT", "EMS12", 0), - ("PV_AV_CAN", "EMS12", 0), - ("TPS", "EMS12", 0), - - ("CYL_PRES", "ESP12", 0), - - ("CF_Clu_CruiseSwState", "CLU11", 0), - ("CF_Clu_CruiseSwMain", "CLU11", 0), - ("CF_Clu_SldMainSW", "CLU11", 0), - ("CF_Clu_ParityBit1", "CLU11", 0), - ("CF_Clu_VanzDecimal" , "CLU11", 0), - ("CF_Clu_Vanz", "CLU11", 0), - ("CF_Clu_SPEED_UNIT", "CLU11", 0), - ("CF_Clu_DetentOut", "CLU11", 0), - ("CF_Clu_RheostatLevel", "CLU11", 0), - ("CF_Clu_CluInfo", "CLU11", 0), - ("CF_Clu_AmpInfo", "CLU11", 0), - ("CF_Clu_AliveCnt1", "CLU11", 0), - - ("CF_Clu_InhibitD", "CLU15", 0), - ("CF_Clu_InhibitP", "CLU15", 0), - ("CF_Clu_InhibitN", "CLU15", 0), - ("CF_Clu_InhibitR", "CLU15", 0), - - ("CF_Lvr_Gear", "LVR12",0), - ("CUR_GR", "TCU12",0), - - ("ACCEnable", "TCS13", 0), - ("ACC_REQ", "TCS13", 0), - ("DriverBraking", "TCS13", 0), - ("DriverOverride", "TCS13", 0), - - ("ESC_Off_Step", "TCS15", 0), - - ("CF_Lvr_GearInf", "LVR11", 0), #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev) - - ("CR_Mdps_DrvTq", "MDPS11", 0), - - ("CR_Mdps_StrColTq", "MDPS12", 0), - ("CF_Mdps_ToiActive", "MDPS12", 0), - ("CF_Mdps_ToiUnavail", "MDPS12", 0), - ("CF_Mdps_FailStat", "MDPS12", 0), - ("CR_Mdps_OutTq", "MDPS12", 0), - - ("VSetDis", "SCC11", 0), - ("SCCInfoDisplay", "SCC11", 0), - ("ACCMode", "SCC12", 1), - - ("SAS_Angle", "SAS11", 0), - ("SAS_Speed", "SAS11", 0), - ] - - checks = [ - # address, frequency - ("MDPS12", 50), - ("MDPS11", 100), - ("TCS15", 10), - ("TCS13", 50), - ("CLU11", 50), - ("ESP12", 100), - ("EMS12", 100), - ("CGW1", 10), - ("CGW4", 5), - ("WHL_SPD11", 50), - ("SCC11", 50), - ("SCC12", 50), - ("SAS11", 100) - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - - -def get_camera_parser(CP): - - signals = [ - # sig_name, sig_address, default - ("CF_Lkas_LdwsSysState", "LKAS11", 0), - ("CF_Lkas_SysWarning", "LKAS11", 0), - ("CF_Lkas_LdwsLHWarning", "LKAS11", 0), - ("CF_Lkas_LdwsRHWarning", "LKAS11", 0), - ("CF_Lkas_HbaLamp", "LKAS11", 0), - ("CF_Lkas_FcwBasReq", "LKAS11", 0), - ("CF_Lkas_ToiFlt", "LKAS11", 0), - ("CF_Lkas_HbaSysState", "LKAS11", 0), - ("CF_Lkas_FcwOpt", "LKAS11", 0), - ("CF_Lkas_HbaOpt", "LKAS11", 0), - ("CF_Lkas_FcwSysState", "LKAS11", 0), - ("CF_Lkas_FcwCollisionWarning", "LKAS11", 0), - ("CF_Lkas_FusionState", "LKAS11", 0), - ("CF_Lkas_FcwOpt_USM", "LKAS11", 0), - ("CF_Lkas_LdwsOpt_USM", "LKAS11", 0) - ] - - checks = [] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) - - class CarState(CarStateBase): def update(self, cp, cp_cam): - # update prevs, update must run once per Loop - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on + ret = car.CarState.new_message() - self.door_all_closed = True - self.seatbelt = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] + ret.doorOpen = False # FIXME + ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0 - self.brake_pressed = cp.vl["TCS13"]['DriverBraking'] - self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step'] + ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw'] - self.main_on = True - self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0 - self.pcm_acc_status = int(self.acc_active) + ret.standstill = ret.vEgoRaw < 0.1 - self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS - self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS - self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS - self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS - self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) + ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle'] + ret.steeringRate = cp.vl["SAS11"]['SAS_Speed'] + ret.yawRate = cp.vl["ESP12"]['YAW_RATE'] + ret.leftBlinker = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] != 0 + ret.rightBlinker = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] != 0 + ret.steeringTorque = cp.vl["MDPS11"]['CR_Mdps_DrvTq'] + ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq'] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - self.low_speed_lockout = self.v_ego_raw < 1.0 - - is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]) - speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS - self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv - self.standstill = not self.v_ego_raw > 0.1 - - self.angle_steers = cp.vl["SAS11"]['SAS_Angle'] - self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed'] - self.yaw_rate = cp.vl["ESP12"]['YAW_RATE'] - self.main_on = True - self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] - self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] - self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > STEER_THRESHOLD - self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE - self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] - self.brake_error = 0 - self.steer_torque_driver = cp.vl["MDPS11"]['CR_Mdps_DrvTq'] - self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq'] - self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. - - self.user_brake = 0 - - self.brake_pressed = cp.vl["TCS13"]['DriverBraking'] - self.brake_lights = bool(self.brake_pressed) - if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1): - self.pedal_gas = 0 + # cruise state + ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0 + ret.cruiseState.available = True + ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. + if ret.cruiseState.enabled: + is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]) + speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS + ret.cruiseState.speed = cp.vl["SCC11"]['VSetDis'] * speed_conv else: - self.pedal_gas = cp.vl["EMS12"]['TPS'] - self.car_gas = cp.vl["EMS12"]['TPS'] + ret.cruiseState.speed = 0 + + ret.brake = 0 # FIXME + ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0 + ret.brakeLights = ret.brakePressed + ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100 + ret.gasPressed = cp.vl["EMS16"]["CF_Ems_AclAct"] != 0 + ret.espDisabled = cp.vl["TCS15"]['ESC_Off_Step'] != 0 # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with gear = cp.vl["LVR12"]["CF_Lvr_Gear"] if gear == 5: - self.gear_shifter = GearShifter.drive + gear_shifter = GearShifter.drive elif gear == 6: - self.gear_shifter = GearShifter.neutral + gear_shifter = GearShifter.neutral elif gear == 0: - self.gear_shifter = GearShifter.park + gear_shifter = GearShifter.park elif gear == 7: - self.gear_shifter = GearShifter.reverse + gear_shifter = GearShifter.reverse else: - self.gear_shifter = GearShifter.unknown + gear_shifter = GearShifter.unknown # Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method. if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1: - self.gear_shifter_cluster = GearShifter.drive + gear_shifter_cluster = GearShifter.drive elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1: - self.gear_shifter_cluster = GearShifter.neutral + gear_shifter_cluster = GearShifter.neutral elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1: - self.gear_shifter_cluster = GearShifter.park + gear_shifter_cluster = GearShifter.park elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1: - self.gear_shifter_cluster = GearShifter.reverse + gear_shifter_cluster = GearShifter.reverse else: - self.gear_shifter_cluster = GearShifter.unknown + gear_shifter_cluster = GearShifter.unknown # Gear Selecton via TCU12 gear2 = cp.vl["TCU12"]["CUR_GR"] if gear2 == 0: - self.gear_tcu = GearShifter.park + gear_tcu = GearShifter.park elif gear2 == 14: - self.gear_tcu = GearShifter.reverse + gear_tcu = GearShifter.reverse elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently - self.gear_tcu = GearShifter.drive + gear_tcu = GearShifter.drive else: - self.gear_tcu = GearShifter.unknown + gear_tcu = GearShifter.unknown + + # gear shifter + if self.CP.carFingerprint in FEATURES["use_cluster_gears"]: + ret.gearShifter = gear_shifter_cluster + elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: + ret.gearShifter = gear_tcu + else: + ret.gearShifter = gear_shifter # save the entire LKAS11 and CLU11 self.lkas11 = cp_cam.vl["LKAS11"] self.clu11 = cp.vl["CLU11"] + self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw'] + self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE + self.steer_warning = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] + self.brake_error = 0 + + return ret + + @staticmethod + def get_can_parser(CP): + + signals = [ + # sig_name, sig_address, default + ("WHL_SPD_FL", "WHL_SPD11", 0), + ("WHL_SPD_FR", "WHL_SPD11", 0), + ("WHL_SPD_RL", "WHL_SPD11", 0), + ("WHL_SPD_RR", "WHL_SPD11", 0), + + ("YAW_RATE", "ESP12", 0), + + ("CF_Gway_DrvSeatBeltInd", "CGW4", 1), + + ("CF_Gway_DrvSeatBeltSw", "CGW1", 0), + ("CF_Gway_TSigLHSw", "CGW1", 0), + ("CF_Gway_TurnSigLh", "CGW1", 0), + ("CF_Gway_TSigRHSw", "CGW1", 0), + ("CF_Gway_TurnSigRh", "CGW1", 0), + ("CF_Gway_ParkBrakeSw", "CGW1", 0), + + ("BRAKE_ACT", "EMS12", 0), + ("PV_AV_CAN", "EMS12", 0), + ("CF_Ems_AclAct", "EMS16", 0), + + ("CYL_PRES", "ESP12", 0), + + ("CF_Clu_CruiseSwState", "CLU11", 0), + ("CF_Clu_CruiseSwMain", "CLU11", 0), + ("CF_Clu_SldMainSW", "CLU11", 0), + ("CF_Clu_ParityBit1", "CLU11", 0), + ("CF_Clu_VanzDecimal" , "CLU11", 0), + ("CF_Clu_Vanz", "CLU11", 0), + ("CF_Clu_SPEED_UNIT", "CLU11", 0), + ("CF_Clu_DetentOut", "CLU11", 0), + ("CF_Clu_RheostatLevel", "CLU11", 0), + ("CF_Clu_CluInfo", "CLU11", 0), + ("CF_Clu_AmpInfo", "CLU11", 0), + ("CF_Clu_AliveCnt1", "CLU11", 0), + + ("CF_Clu_InhibitD", "CLU15", 0), + ("CF_Clu_InhibitP", "CLU15", 0), + ("CF_Clu_InhibitN", "CLU15", 0), + ("CF_Clu_InhibitR", "CLU15", 0), + + ("CF_Lvr_Gear", "LVR12",0), + ("CUR_GR", "TCU12",0), + + ("ACCEnable", "TCS13", 0), + ("DriverBraking", "TCS13", 0), + + ("ESC_Off_Step", "TCS15", 0), + + ("CF_Lvr_GearInf", "LVR11", 0), #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev) + + ("CR_Mdps_DrvTq", "MDPS11", 0), + + ("CR_Mdps_StrColTq", "MDPS12", 0), + ("CF_Mdps_ToiActive", "MDPS12", 0), + ("CF_Mdps_ToiUnavail", "MDPS12", 0), + ("CF_Mdps_FailStat", "MDPS12", 0), + ("CR_Mdps_OutTq", "MDPS12", 0), + + ("VSetDis", "SCC11", 0), + ("SCCInfoDisplay", "SCC11", 0), + ("ACCMode", "SCC12", 1), + + ("SAS_Angle", "SAS11", 0), + ("SAS_Speed", "SAS11", 0), + ] + + checks = [ + # address, frequency + ("MDPS12", 50), + ("MDPS11", 100), + ("TCS15", 10), + ("TCS13", 50), + ("CLU11", 50), + ("ESP12", 100), + ("EMS12", 100), + ("EMS16", 100), + ("CGW1", 10), + ("CGW4", 5), + ("WHL_SPD11", 50), + ("SCC11", 50), + ("SCC12", 50), + ("SAS11", 100) + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + + signals = [ + # sig_name, sig_address, default + ("CF_Lkas_LdwsSysState", "LKAS11", 0), + ("CF_Lkas_SysWarning", "LKAS11", 0), + ("CF_Lkas_LdwsLHWarning", "LKAS11", 0), + ("CF_Lkas_LdwsRHWarning", "LKAS11", 0), + ("CF_Lkas_HbaLamp", "LKAS11", 0), + ("CF_Lkas_FcwBasReq", "LKAS11", 0), + ("CF_Lkas_ToiFlt", "LKAS11", 0), + ("CF_Lkas_HbaSysState", "LKAS11", 0), + ("CF_Lkas_FcwOpt", "LKAS11", 0), + ("CF_Lkas_HbaOpt", "LKAS11", 0), + ("CF_Lkas_FcwSysState", "LKAS11", 0), + ("CF_Lkas_FcwCollisionWarning", "LKAS11", 0), + ("CF_Lkas_FusionState", "LKAS11", 0), + ("CF_Lkas_FcwOpt_USM", "LKAS11", 0), + ("CF_Lkas_LdwsOpt_USM", "LKAS11", 0) + ] + + checks = [] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 5a8e58475..a7e0d68be 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,49 +1,12 @@ #!/usr/bin/env python3 from cereal import car -from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser -from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, get_hud_alerts, FEATURES, FINGERPRINTS +from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, get_hud_alerts, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -from common.params import Params -params = Params() -from selfdrive.dragonpilot.dragonconf import dp_get_last_modified - -GearShifter = car.CarState.GearShifter -ButtonType = car.CarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.VM = VehicleModel(CP) - self.idx = 0 - self.lanes = 0 - self.lkas_request = 0 - - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False - self.low_speed_alert = False - - # *** init the major players *** - self.CS = CarState(CP) - self.cp = get_can_parser(CP) - self.cp_cam = get_camera_parser(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP.carFingerprint) - - # dragonpilot - self.frame = 0 - self.dragon_enable_steering_on_signal = False - self.dragon_allow_gas = False - self.ts_last_check = 0. - self.dragon_lat_ctrl = True - self.dp_last_modified = None @staticmethod def compute_gb(accel, speed): @@ -51,15 +14,11 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "hyundai" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay - ret.radarOffCan = True ret.safetyModel = car.CarParams.SafetyModel.hyundai - ret.enableCruise = True # stock acc + ret.radarOffCan = True ret.steerActuatorDelay = 0.1 # Default delay ret.steerRateCost = 0.5 @@ -120,14 +79,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 0. - ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this - ret.longitudinalTuning.kpBP = [0.] - ret.longitudinalTuning.kpV = [0.] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.centerToFront = ret.wheelbase * 0.4 # TODO: get actual value, for now starting with reasonable value for @@ -139,159 +90,32 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. - ret.steerControlType = car.CarParams.SteerControlType.torque - - # steer, gas, brake limitations VS speed - ret.steerMaxBP = [0.] - ret.steerMaxV = [1.0] - ret.gasMaxBP = [0.] - ret.gasMaxV = [1.] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [1.] - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay - ret.openpilotLongitudinalControl = False - - ret.stoppingControl = False - ret.startAccel = 0.0 return ret - # returns a car.CarState def update(self, c, can_strings): - # dragonpilot, don't check for param too often as it's a kernel call - ts = sec_since_boot() - if ts - self.ts_last_check >= 5.: - modified = dp_get_last_modified() - if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else True - self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False - self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True - self.dp_last_modified = modified - self.ts_last_check = ts - - # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - self.CS.update(self.cp, self.cp_cam) - # create message - ret = car.CarState.new_message() - + ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid - # speeds - ret.vEgo = self.CS.v_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.aEgo = self.CS.a_ego - ret.yawRate = self.CS.yaw_rate - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # gear shifter - if self.CP.carFingerprint in FEATURES["use_cluster_gears"]: - ret.gearShifter = self.CS.gear_shifter_cluster - elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: - ret.gearShifter = self.CS.gear_tcu - else: - ret.gearShifter = self.CS.gear_shifter - - # gas pedal - ret.gas = self.CS.car_gas - ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading - - # brake pedal - ret.brake = self.CS.user_brake - ret.brakePressed = self.CS.brake_pressed != 0 - ret.brakeLights = self.CS.brake_lights - - # steering wheel - ret.steeringAngle = self.CS.angle_steers - ret.steeringRate = self.CS.angle_steers_rate # it's unsigned - - ret.steeringTorque = self.CS.steer_torque_driver - ret.steeringPressed = self.CS.steer_override - - # cruise state - ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 - if self.CS.pcm_acc_status != 0: - ret.cruiseState.speed = self.CS.cruise_set_speed - else: - ret.cruiseState.speed = 0 - ret.cruiseState.available = bool(self.CS.main_on) - ret.cruiseState.standstill = False - # TODO: button presses - buttonEvents = [] + ret.buttonEvents = [] - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on != 0 - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on != 0 - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents - ret.leftBlinker = bool(self.CS.left_blinker_on) - ret.rightBlinker = bool(self.CS.right_blinker_on) - - ret.doorOpen = not self.CS.door_all_closed - ret.seatbeltUnlatched = not self.CS.seatbelt - - # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) - if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: - self.low_speed_alert = True - if ret.vEgo > (self.CP.minSteerSpeed + 4.): - self.low_speed_alert = False - - events = [] - if not ret.gearShifter == GearShifter.drive: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not self.CS.main_on: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gearShifter == GearShifter.reverse: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if not self.dragon_lat_ctrl: - events.append(create_event('manualSteeringRequired', [ET.WARNING])) - elif (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal: - events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) - elif self.CS.steer_error: - events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) + events = self.create_common_events(ret) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # DragonAllowGas - if not self.dragon_allow_gas: - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - else: - if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.1): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - + # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) + if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: + self.low_speed_alert = True + if ret.vEgo > (self.CP.minSteerSpeed + 4.): + self.low_speed_alert = False if self.low_speed_alert: events.append(create_event('belowSteerSpeed', [ET.WARNING])) @@ -301,7 +125,8 @@ class CarInterface(CarInterfaceBase): self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out def apply(self, c): diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index d29aba44e..7fe689c62 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -44,9 +44,14 @@ FINGERPRINTS = { { 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 }], - CAR.KIA_OPTIMA: [{ - 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1952: 8, 1960: 8, 1988: 8, 1996: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 - }], + CAR.KIA_OPTIMA: [ + { + 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1952: 8, 1960: 8, 1988: 8, 1996: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }, + { + 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 897: 8, 899: 8, 902: 8, 903: 6, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1491: 8, 1492: 8 + }, + ], CAR.KIA_SORENTO: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1 }], diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index e98b2681b..cb3883673 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -4,14 +4,43 @@ from cereal import car from common.kalman.simple_kalman import KF1D from common.realtime import DT_CTRL from selfdrive.car import gen_empty_fingerprint +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event +from selfdrive.controls.lib.vehicle_model import VehicleModel +from common.realtime import sec_since_boot +from common.params import Params +params = Params() +from selfdrive.dragonpilot.dragonconf import dp_get_last_modified GearShifter = car.CarState.GearShifter # generic car and radar interfaces class CarInterfaceBase(): - def __init__(self, CP, CarController): - pass + def __init__(self, CP, CarController, CarState): + self.CP = CP + self.VM = VehicleModel(CP) + + self.frame = 0 + self.gas_pressed_prev = False + self.brake_pressed_prev = False + self.cruise_enabled_prev = False + self.low_speed_alert = False + + self.CS = CarState(CP) + self.cp = self.CS.get_can_parser(CP) + self.cp_cam = self.CS.get_cam_can_parser(CP) + + self.CC = None + if CarController is not None: + self.CC = CarController(self.cp.dbc_name, CP, self.VM) + + # dragonpilot + self.dragon_toyota_stock_dsu = False + self.dragon_enable_steering_on_signal = False + self.dragon_allow_gas = False + self.ts_last_check = 0. + self.dragon_lat_ctrl = True + self.dp_last_modified = None @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): @@ -25,6 +54,37 @@ class CarInterfaceBase(): def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): raise NotImplementedError + # returns a set of default params to avoid repetition in car specific params + @staticmethod + def get_std_params(candidate, fingerprint, has_relay): + ret = car.CarParams.new_message() + ret.carFingerprint = candidate + ret.isPandaBlack = has_relay + + # standard ALC params + ret.steerControlType = car.CarParams.SteerControlType.torque + ret.steerMaxBP = [0.] + ret.steerMaxV = [1.] + + # stock ACC by default + ret.enableCruise = True + ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this + ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA + ret.gasMaxBP = [0.] + ret.gasMaxV = [.5] # half max brake + ret.brakeMaxBP = [0.] + ret.brakeMaxV = [1.] + ret.openpilotLongitudinalControl = False + ret.startAccel = 0.0 + ret.stoppingControl = False + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] + ret.longitudinalTuning.kpBP = [0.] + ret.longitudinalTuning.kpV = [1.] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [1.] + return ret + # returns a car.CarState, pass in car.CarControl def update(self, c, can_strings): raise NotImplementedError @@ -33,6 +93,71 @@ class CarInterfaceBase(): def apply(self, c): raise NotImplementedError + def dp_load_params(self, car_name): + # dragonpilot, don't check for param too often as it's a kernel call + ts = sec_since_boot() + if ts - self.ts_last_check >= 5.: + modified = dp_get_last_modified() + if self.dp_last_modified != modified: + self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = True + if car_name == 'toyota': + self.dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False + else: + self.dragon_toyota_stock_dsu = False + if not self.dragon_toyota_stock_dsu: + self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False + self.dp_last_modified = modified + self.ts_last_check = ts + + def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1): + events = [] + + if cs_out.doorOpen: + events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if cs_out.seatbeltUnlatched: + events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if cs_out.gearShifter != GearShifter.drive and cs_out.gearShifter not in extra_gears: + events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if cs_out.gearShifter == GearShifter.reverse: + events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if not cs_out.cruiseState.available: + events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) + if cs_out.espDisabled: + events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not self.dragon_toyota_stock_dsu: + if not self.dragon_allow_gas: + if cs_out.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + + # TODO: move this stuff to the capnp strut + if not self.dragon_lat_ctrl: + events.append(create_event('manualSteeringRequired', [ET.WARNING])) + elif self.dragon_enable_steering_on_signal and (cs_out.leftBlinker or cs_out.rightBlinker): + events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) + elif getattr(self.CS, "steer_error", False): + events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + elif getattr(self.CS, "steer_warning", False): + events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) + + # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. + # Optionally allow to press gas at zero speed to resume. + # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME! + if not self.dragon_toyota_stock_dsu: + # DragonAllowGas + if not self.dragon_allow_gas: + if (cs_out.gasPressed and (not self.gas_pressed_prev) and cs_out.vEgo > gas_resume_speed) or \ + (cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill)): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + else: + if cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + + return events + class RadarInterfaceBase(): def __init__(self, CP): self.pts = {} @@ -51,8 +176,6 @@ class CarStateBase: def __init__(self, CP): self.CP = CP self.car_fingerprint = CP.carFingerprint - self.left_blinker_on = 0 - self.right_blinker_on = 0 self.cruise_buttons = 0 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) @@ -74,3 +197,7 @@ class CarStateBase: return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral, 'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive, 'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake}.get(gear, GearShifter.unknown) + + @staticmethod + def get_cam_can_parser(CP): + return None diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 09c5b5c61..dc9724403 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -14,7 +14,7 @@ LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS) class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): + def __init__(self, CP, CarController, CarState): self.CP = CP self.CC = CarController @@ -35,17 +35,9 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() - + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "mock" - ret.carFingerprint = candidate - ret.safetyModel = car.CarParams.SafetyModel.noOutput - ret.openpilotLongitudinalControl = False - - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars ret.mass = 1700. ret.rotationalInertia = 2500. ret.wheelbase = 2.70 @@ -53,22 +45,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13. # reasonable ret.tireStiffnessFront = 1e6 # very stiff to neglect slip ret.tireStiffnessRear = 1e6 # very stiff to neglect slip - ret.steerRatioRear = 0. - - ret.steerMaxBP = [0.] - ret.steerMaxV = [0.] # 2/3rd torque allowed above 45 kph - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [0.] - - ret.longitudinalTuning.kpBP = [0.] - ret.longitudinalTuning.kpV = [0.] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.steerActuatorDelay = 0. return ret diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index fe1f309b0..23cca1df5 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -20,25 +20,24 @@ class CarControllerParams(): class CarController(): - def __init__(self, car_fingerprint): + def __init__(self, dbc_name, CP, VM): self.lkas_active = False - self.steer_idx = 0 self.apply_steer_last = 0 - self.car_fingerprint = car_fingerprint self.es_distance_cnt = -1 self.es_lkas_cnt = -1 self.steer_rate_limited = False # Setup detection helper. Routes commands to # an appropriate CAN bus number. - self.params = CarControllerParams(car_fingerprint) - self.packer = CANPacker(DBC[car_fingerprint]['pt']) + self.params = CarControllerParams(CP.carFingerprint) + self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) # dragonpilot self.turning_signal_timer = 0 self.dragon_enable_steering_on_signal = False self.dragon_lat_ctrl = True self.dp_last_modified = None + self.lane_change_enabled = True def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line): """ Controls thread """ @@ -47,8 +46,15 @@ class CarController(): if frame % 500 == 0: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.lane_change_enabled = False if params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if not self.lane_change_enabled: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = False + else: + self.dragon_enable_steering_on_signal = False self.dp_last_modified = modified P = self.params @@ -66,7 +72,7 @@ class CarController(): # limits due to driver torque new_steer = int(round(apply_steer)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer lkas_enabled = enabled @@ -77,7 +83,7 @@ class CarController(): # dragonpilot if enabled: if self.dragon_enable_steering_on_signal: - if CS.left_blinker_on == 0 and CS.right_blinker_on == 0: + if not CS.out.leftBlinker and not CS.out.rightBlinker: self.turning_signal_timer = 0 else: self.turning_signal_timer = 100 diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 10500ce6f..a6cd6e9f3 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -1,138 +1,136 @@ import copy +from cereal import car from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD -def get_powertrain_can_parser(CP): - # this function generates lists for signal, messages and initial values - signals = [ - # sig_name, sig_address, default - ("Steer_Torque_Sensor", "Steering_Torque", 0), - ("Steering_Angle", "Steering_Torque", 0), - ("Cruise_On", "CruiseControl", 0), - ("Cruise_Activated", "CruiseControl", 0), - ("Brake_Pedal", "Brake_Pedal", 0), - ("Throttle_Pedal", "Throttle", 0), - ("LEFT_BLINKER", "Dashlights", 0), - ("RIGHT_BLINKER", "Dashlights", 0), - ("SEATBELT_FL", "Dashlights", 0), - ("FL", "Wheel_Speeds", 0), - ("FR", "Wheel_Speeds", 0), - ("RL", "Wheel_Speeds", 0), - ("RR", "Wheel_Speeds", 0), - ("DOOR_OPEN_FR", "BodyInfo", 1), - ("DOOR_OPEN_FL", "BodyInfo", 1), - ("DOOR_OPEN_RR", "BodyInfo", 1), - ("DOOR_OPEN_RL", "BodyInfo", 1), - ("Units", "Dash_State", 1), - ] - - checks = [ - # sig_address, frequency - ("Dashlights", 10), - ("CruiseControl", 20), - ("Wheel_Speeds", 50), - ("Steering_Torque", 50), - ("BodyInfo", 10), - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - - -def get_camera_can_parser(CP): - signals = [ - ("Cruise_Set_Speed", "ES_DashStatus", 0), - - ("Counter", "ES_Distance", 0), - ("Signal1", "ES_Distance", 0), - ("Signal2", "ES_Distance", 0), - ("Main", "ES_Distance", 0), - ("Signal3", "ES_Distance", 0), - - ("Checksum", "ES_LKAS_State", 0), - ("Counter", "ES_LKAS_State", 0), - ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0), - ("Empty_Box", "ES_LKAS_State", 0), - ("Signal1", "ES_LKAS_State", 0), - ("LKAS_ACTIVE", "ES_LKAS_State", 0), - ("Signal2", "ES_LKAS_State", 0), - ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0), - ("LKAS_ENABLE_3", "ES_LKAS_State", 0), - ("Signal3", "ES_LKAS_State", 0), - ("LKAS_ENABLE_2", "ES_LKAS_State", 0), - ("Signal4", "ES_LKAS_State", 0), - ("LKAS_Left_Line_Visible", "ES_LKAS_State", 0), - ("Signal6", "ES_LKAS_State", 0), - ("LKAS_Right_Line_Visible", "ES_LKAS_State", 0), - ("Signal7", "ES_LKAS_State", 0), - ("FCW_Cont_Beep", "ES_LKAS_State", 0), - ("FCW_Repeated_Beep", "ES_LKAS_State", 0), - ("Throttle_Management_Activated", "ES_LKAS_State", 0), - ("Traffic_light_Ahead", "ES_LKAS_State", 0), - ("Right_Depart", "ES_LKAS_State", 0), - ("Signal5", "ES_LKAS_State", 0), - - ] - - checks = [ - ("ES_DashStatus", 10), - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) - class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) - # initialize can parser self.left_blinker_cnt = 0 self.right_blinker_cnt = 0 def update(self, cp, cp_cam): + ret = car.CarState.new_message() - self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal'] - self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal'] - self.user_gas_pressed = self.pedal_gas > 0 - self.brake_pressed = self.brake_pressure > 0 - self.brake_lights = bool(self.brake_pressed) + ret.gas = cp.vl["Throttle"]['Throttle_Pedal'] / 255. + ret.gasPressed = ret.gas > 1e-5 + ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 1e-5 + ret.brakeLights = ret.brakePressed - self.v_wheel_fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS - self.v_wheel_fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS - self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS - self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS - - self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed'] - # 1 = imperial, 6 = metric - if cp.vl["Dash_State"]['Units'] == 1: - self.v_cruise_pcm *= CV.MPH_TO_KPH - - self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. + ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. # Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) - - self.standstill = self.v_ego_raw < 0.01 - - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.01 # continuous blinker signals for assisted lane change self.left_blinker_cnt = 50 if cp.vl["Dashlights"]['LEFT_BLINKER'] else max(self.left_blinker_cnt - 1, 0) - self.left_blinker_on = self.left_blinker_cnt > 0 - + ret.leftBlinker = self.left_blinker_cnt > 0 self.right_blinker_cnt = 50 if cp.vl["Dashlights"]['RIGHT_BLINKER'] else max(self.right_blinker_cnt - 1, 0) - self.right_blinker_on = self.right_blinker_cnt > 0 + ret.rightBlinker = self.right_blinker_cnt > 0 - self.seatbelt_unlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1 - self.steer_torque_driver = cp.vl["Steering_Torque"]['Steer_Torque_Sensor'] - self.acc_active = cp.vl["CruiseControl"]['Cruise_Activated'] - self.main_on = cp.vl["CruiseControl"]['Cruise_On'] - self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.car_fingerprint] - self.angle_steers = cp.vl["Steering_Torque"]['Steering_Angle'] - self.door_open = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'], + ret.steeringAngle = cp.vl["Steering_Torque"]['Steering_Angle'] + ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor'] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint] + + ret.cruiseState.enabled = cp.vl["CruiseControl"]['Cruise_Activated'] != 0 + ret.cruiseState.available = cp.vl["CruiseControl"]['Cruise_On'] != 0 + ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed'] * CV.KPH_TO_MS + # 1 = imperial, 6 = metric + if cp.vl["Dash_State"]['Units'] == 1: + ret.cruiseState.speed *= CV.MPH_TO_KPH + + ret.seatbeltUnlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1 + ret.doorOpen = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'], cp.vl["BodyInfo"]['DOOR_OPEN_RL'], cp.vl["BodyInfo"]['DOOR_OPEN_FR'], cp.vl["BodyInfo"]['DOOR_OPEN_FL']]) self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"]) self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) + + return ret + + @staticmethod + def get_can_parser(CP): + # this function generates lists for signal, messages and initial values + signals = [ + # sig_name, sig_address, default + ("Steer_Torque_Sensor", "Steering_Torque", 0), + ("Steering_Angle", "Steering_Torque", 0), + ("Cruise_On", "CruiseControl", 0), + ("Cruise_Activated", "CruiseControl", 0), + ("Brake_Pedal", "Brake_Pedal", 0), + ("Throttle_Pedal", "Throttle", 0), + ("LEFT_BLINKER", "Dashlights", 0), + ("RIGHT_BLINKER", "Dashlights", 0), + ("SEATBELT_FL", "Dashlights", 0), + ("FL", "Wheel_Speeds", 0), + ("FR", "Wheel_Speeds", 0), + ("RL", "Wheel_Speeds", 0), + ("RR", "Wheel_Speeds", 0), + ("DOOR_OPEN_FR", "BodyInfo", 1), + ("DOOR_OPEN_FL", "BodyInfo", 1), + ("DOOR_OPEN_RR", "BodyInfo", 1), + ("DOOR_OPEN_RL", "BodyInfo", 1), + ("Units", "Dash_State", 1), + ] + + checks = [ + # sig_address, frequency + ("Dashlights", 10), + ("CruiseControl", 20), + ("Wheel_Speeds", 50), + ("Steering_Torque", 50), + ("BodyInfo", 10), + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + signals = [ + ("Cruise_Set_Speed", "ES_DashStatus", 0), + + ("Counter", "ES_Distance", 0), + ("Signal1", "ES_Distance", 0), + ("Signal2", "ES_Distance", 0), + ("Main", "ES_Distance", 0), + ("Signal3", "ES_Distance", 0), + + ("Checksum", "ES_LKAS_State", 0), + ("Counter", "ES_LKAS_State", 0), + ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0), + ("Empty_Box", "ES_LKAS_State", 0), + ("Signal1", "ES_LKAS_State", 0), + ("LKAS_ACTIVE", "ES_LKAS_State", 0), + ("Signal2", "ES_LKAS_State", 0), + ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0), + ("LKAS_ENABLE_3", "ES_LKAS_State", 0), + ("Signal3", "ES_LKAS_State", 0), + ("LKAS_ENABLE_2", "ES_LKAS_State", 0), + ("Signal4", "ES_LKAS_State", 0), + ("LKAS_Left_Line_Visible", "ES_LKAS_State", 0), + ("Signal6", "ES_LKAS_State", 0), + ("LKAS_Right_Line_Visible", "ES_LKAS_State", 0), + ("Signal7", "ES_LKAS_State", 0), + ("FCW_Cont_Beep", "ES_LKAS_State", 0), + ("FCW_Repeated_Beep", "ES_LKAS_State", 0), + ("Throttle_Management_Activated", "ES_LKAS_State", 0), + ("Traffic_light_Ahead", "ES_LKAS_State", 0), + ("Right_Depart", "ES_LKAS_State", 0), + ("Signal5", "ES_LKAS_State", 0), + + ] + + checks = [ + ("ES_DashStatus", 10), + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 021adb1a9..0c71cb13e 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -2,45 +2,11 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET -from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.subaru.values import CAR -from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -from common.realtime import sec_since_boot -from common.params import Params -params = Params() -from selfdrive.dragonpilot.dragonconf import dp_get_last_modified - -ButtonType = car.CarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - - self.frame = 0 - self.acc_active_prev = 0 - self.gas_pressed_prev = False - - # *** init the major players *** - self.CS = CarState(CP) - self.VM = VehicleModel(CP) - self.pt_cp = get_powertrain_can_parser(CP) - self.cam_cp = get_camera_can_parser(CP) - - self.gas_pressed_prev = False - - self.CC = None - if CarController is not None: - self.CC = CarController(CP.carFingerprint) - - # dragonpilot - self.frame = 0 - self.dragon_enable_steering_on_signal = False - self.dragon_allow_gas = False - self.ts_last_check = 0. - self.dragon_lat_ctrl = True - self.dp_last_modified = None @staticmethod def compute_gb(accel, speed): @@ -48,16 +14,12 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - ret = car.CarParams.new_message() + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "subaru" ret.radarOffCan = True - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.subaru - ret.enableCruise = True - # force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches) # was never released ret.enableCamera = True @@ -74,26 +36,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]] - ret.steerMaxBP = [0.] # m/s - ret.steerMaxV = [1.] - - ret.steerControlType = car.CarParams.SteerControlType.torque - ret.steerRatioRear = 0. - # testing tuning - - # No long control in subaru - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [0.] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.longitudinalTuning.kpBP = [0.] - ret.longitudinalTuning.kpV = [0.] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.] - - # end from gm # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase @@ -107,115 +49,36 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): - # dragonpilot, don't check for param too often as it's a kernel call - ts = sec_since_boot() - if ts - self.ts_last_check >= 5.: - modified = dp_get_last_modified() - if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else True - self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False - self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True - self.dp_last_modified = modified - self.ts_last_check = ts + self.dp_load_params('subaru') + self.cp.update_strings(can_strings) + self.cp_cam.update_strings(can_strings) - self.pt_cp.update_strings(can_strings) - self.cam_cp.update_strings(can_strings) + ret = self.CS.update(self.cp, self.cp_cam) - self.CS.update(self.pt_cp, self.cam_cp) - - # create message - ret = car.CarState.new_message() - - ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid - - # speeds - ret.vEgo = self.CS.v_ego - ret.aEgo = self.CS.a_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # steering wheel - ret.steeringAngle = self.CS.angle_steers - - # torque and user override. Driver awareness - # timer resets when the user uses the steering wheel. - ret.steeringPressed = self.CS.steer_override - ret.steeringTorque = self.CS.steer_torque_driver + ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - - ret.gas = self.CS.pedal_gas / 255. - ret.gasPressed = self.CS.user_gas_pressed - - # cruise state - ret.cruiseState.enabled = bool(self.CS.acc_active) - ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS - ret.cruiseState.available = bool(self.CS.main_on) - ret.cruiseState.speedOffset = 0. - - ret.leftBlinker = self.CS.left_blinker_on - ret.rightBlinker = self.CS.right_blinker_on - ret.seatbeltUnlatched = self.CS.seatbelt_unlatched - ret.doorOpen = self.CS.door_open + ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) buttonEvents = [] - - # blinkers - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on - buttonEvents.append(be) - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.accelCruise + be.type = car.CarState.ButtonEvent.Type.accelCruise buttonEvents.append(be) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.unknown]) - events = [] - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - - if not self.dragon_lat_ctrl: - events.append(create_event('manualSteeringRequired', [ET.WARNING])) - elif (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal: - events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) - - if self.CS.acc_active and not self.acc_active_prev: + if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) - if not self.CS.acc_active: + if not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # DragonAllowGas - if not self.dragon_allow_gas: - # disable on gas pedal rising edge - if (ret.gasPressed and not self.gas_pressed_prev): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - ret.events = events - # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed - self.acc_active_prev = self.CS.acc_active + self.brake_pressed_prev = ret.brakePressed + self.cruise_enabled_prev = ret.cruiseState.enabled - # cast to reader so it can't be modified - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out def apply(self, c): can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index e65ade1cc..29fbcb339 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,9 +1,8 @@ from cereal import car -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ - create_ipas_steer_command, create_accel_command, \ - create_acc_cancel_command, create_fcw_command + create_accel_command, create_acc_cancel_command, create_fcw_command from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, SteerLimitParams from opendbc.can.packer import CANPacker from common.params import Params @@ -18,14 +17,6 @@ ACCEL_MAX = 1.5 # 1.5 m/s2 ACCEL_MIN = -3.0 # 3 m/s2 ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) - -# Steer angle limits (tested at the Crows Landing track and considered ok) -ANGLE_MAX_BP = [0., 5.] -ANGLE_MAX_V = [510., 300.] -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 accel_hysteresis(accel, accel_steady, enabled): # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command @@ -54,53 +45,22 @@ def process_hud_alert(hud_alert): return steer, fcw -def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_counter): - - if enabled and not steer_angle_enabled: - #ipas_reset_counter = max(0, ipas_reset_counter - 1) - #if ipas_reset_counter == 0: - # steer_angle_enabled = True - #else: - # steer_angle_enabled = False - #return steer_angle_enabled, ipas_reset_counter - return True, 0 - - elif enabled and steer_angle_enabled: - if steer_angle_enabled and not ipas_active: - ipas_reset_counter += 1 - else: - ipas_reset_counter = 0 - if ipas_reset_counter > 10: # try every 0.1s - steer_angle_enabled = False - return steer_angle_enabled, ipas_reset_counter - - else: - return False, 0 - - class CarController(): - def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_apg): + def __init__(self, dbc_name, CP, VM): self.braking = False - # redundant safety check with the board - self.controls_allowed = True self.last_steer = 0 - self.last_angle = 0 self.accel_steady = 0. - self.car_fingerprint = car_fingerprint + self.car_fingerprint = CP.carFingerprint self.alert_active = False self.last_standstill = False self.standstill_req = False - self.angle_control = False - self.steer_angle_enabled = False - self.ipas_reset_counter = 0 self.last_fault_frame = -200 self.steer_rate_limited = False self.fake_ecus = set() - if enable_camera: self.fake_ecus.add(Ecu.fwdCamera) - if enable_dsu: self.fake_ecus.add(Ecu.dsu) - if enable_apg: self.fake_ecus.add(Ecu.apgs) + if CP.enableCamera: self.fake_ecus.add(Ecu.fwdCamera) + if CP.enableDsu: self.fake_ecus.add(Ecu.dsu) self.packer = CANPacker(dbc_name) @@ -111,6 +71,7 @@ class CarController(): self.dragon_lane_departure_warning = True self.dragon_toyota_sng_mod = False self.dp_last_modified = None + self.lane_change_enabled = True def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): @@ -119,10 +80,17 @@ class CarController(): if frame % 500 == 0: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False - self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True self.dragon_toyota_sng_mod = True if params.get("DragonToyotaSnGMod", encoding='utf8') == "1" else False + self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.lane_change_enabled = False if params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if not self.lane_change_enabled: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = False + else: + self.dragon_enable_steering_on_signal = False self.dp_last_modified = modified # *** compute control surfaces *** @@ -143,7 +111,7 @@ class CarController(): # steer torque new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer # only cut torque when steer state is a known fault @@ -157,71 +125,39 @@ class CarController(): else: apply_steer_req = 1 - self.steer_angle_enabled, self.ipas_reset_counter = \ - ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) - #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active)) - - # steer angle - if self.steer_angle_enabled and CS.ipas_active: - apply_angle = actuators.steerAngle - angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) - apply_angle = clip(apply_angle, -angle_lim, angle_lim) - - # windup slower - if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): - angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) - else: - angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) - - apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) - else: - apply_angle = CS.angle_steers - if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = 1 # on entering standstill, send standstill request - if not self.dragon_toyota_sng_mod and CS.standstill and not self.last_standstill: + if not self.dragon_toyota_sng_mod and CS.out.standstill and not self.last_standstill: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False self.last_steer = apply_steer - self.last_angle = apply_angle self.last_accel = apply_accel - self.last_standstill = CS.standstill + self.last_standstill = CS.out.standstill can_sends = [] # dragonpilot if enabled: if self.dragon_enable_steering_on_signal: - if CS.left_blinker_on == 0 and CS.right_blinker_on == 0: + if not CS.out.leftBlinker and not CS.out.rightBlinker: self.turning_signal_timer = 0 else: self.turning_signal_timer = 100 - + if self.turning_signal_timer > 0: self.turning_signal_timer -= 1 apply_steer_req = 0 else: self.turning_signal_timer = 0 - + if not self.dragon_lat_ctrl: apply_steer_req = 0 - else: - if CS.v_ego > 12.5: - if right_lane_depart and not CS.right_blinker_on: - apply_steer = self.last_steer + 3 - apply_steer = min(apply_steer , 800) - apply_steer_req = 1 - - if left_lane_depart and not CS.left_blinker_on: - apply_steer = self.last_steer - 3 - apply_steer = max(apply_steer , -800) - apply_steer_req = 1 #*** control msgs *** #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) @@ -230,20 +166,11 @@ class CarController(): # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages if Ecu.fwdCamera in self.fake_ecus: - if self.angle_control: - can_sends.append(create_steer_command(self.packer, 0., 0, frame)) - else: - can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) - - if self.angle_control: - can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, - Ecu.apgs in self.fake_ecus)) - elif Ecu.apgs in self.fake_ecus: - can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) + can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus): - lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged + lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged # Lexus IS uses a different cancellation message if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_ISH, CAR.LEXUS_GSH]: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index c137a3427..e8e1343be 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,220 +1,231 @@ +from cereal import car from common.numpy_fast import mean from opendbc.can.can_define import CANDefine from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR - +from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_STOP_TIMER_CAR from common.realtime import sec_since_boot from common.params import Params +from selfdrive.dragonpilot.dragonconf import dp_get_last_modified params = Params() - -def get_can_parser(CP): - - signals = [ - # sig_name, sig_address, default - ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), - ("GEAR", "GEAR_PACKET", 0), - ("BRAKE_PRESSED", "BRAKE_MODULE", 0), - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), - ("DOOR_OPEN_FL", "SEATS_DOORS", 1), - ("DOOR_OPEN_FR", "SEATS_DOORS", 1), - ("DOOR_OPEN_RL", "SEATS_DOORS", 1), - ("DOOR_OPEN_RR", "SEATS_DOORS", 1), - ("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1), - ("TC_DISABLED", "ESP_CONTROL", 1), - ("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0), - ("STEER_RATE", "STEER_ANGLE_SENSOR", 0), - ("CRUISE_ACTIVE", "PCM_CRUISE", 0), - ("CRUISE_STATE", "PCM_CRUISE", 0), - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), - ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), - ("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers - ("LKA_STATE", "EPS_STATUS", 0), - ("IPAS_STATE", "EPS_STATUS", 1), - ("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0), - ] - - checks = [ - ("WHEEL_SPEEDS", 80), - ("STEER_ANGLE_SENSOR", 80), - ("PCM_CRUISE", 33), - ("STEER_TORQUE_SENSOR", 50), - ("EPS_STATUS", 25), - ] - - if CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: - signals.append(("GAS_PEDAL", "GAS_PEDAL_ALT", 0)) - signals.append(("MAIN_ON", "PCM_CRUISE_ALT", 0)) - signals.append(("SET_SPEED", "PCM_CRUISE_ALT", 0)) - signals.append(("AUTO_HIGH_BEAM", "LIGHT_STALK_ISH", 0)) - checks += [ - ("BRAKE_MODULE", 50), - ("GAS_PEDAL_ALT", 50), - ("PCM_CRUISE_ALT", 1), - ] - else: - signals += [ - ("AUTO_HIGH_BEAM", "LIGHT_STALK", 0), - ("GAS_PEDAL", "GAS_PEDAL", 0), - ] - checks += [ - ("BRAKE_MODULE", 40), - ("GAS_PEDAL", 33), - ] - - if CP.carFingerprint == CAR.LEXUS_IS: - signals.append(("MAIN_ON", "DSU_CRUISE", 0)) - signals.append(("SET_SPEED", "DSU_CRUISE", 0)) - checks.append(("DSU_CRUISE", 5)) - else: - signals.append(("MAIN_ON", "PCM_CRUISE_2", 0)) - signals.append(("SET_SPEED", "PCM_CRUISE_2", 0)) - signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0)) - checks.append(("PCM_CRUISE_2", 33)) - - if CP.carFingerprint in NO_DSU_CAR or CP.carFingerprint == CAR.LEXUS_ISH: - signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)] - - if CP.carFingerprint == CAR.PRIUS: - signals += [("STATE", "AUTOPARK_STATUS", 0)] - - # add gas interceptor reading if we are using it - if CP.enableGasInterceptor: - signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) - signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) - checks.append(("GAS_SENSOR", 50)) - - checks = [] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - - -def get_cam_can_parser(CP): - - signals = [("FORCE", "PRE_COLLISION", 0), ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)] - - # use steering message to check if panda is connected to frc - checks = [("STEERING_LKA", 42)] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) - - class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR'] - self.angle_offset = 0. - self.init_angle_offset = False + # All TSS2 car have the accurate sensor + self.accurate_steer_angle_seen = CP.carFingerprint in TSS2_CAR + + # On NO_DSU cars but not TSS2 cars the cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] + # is zeroed to where the steering angle is at start. + # Need to apply an offset as soon as the steering angle measurements are both received + self.needs_angle_offset = CP.carFingerprint not in TSS2_CAR or CP.carFingerprint == CAR.LEXUS_ISH + self.angle_offset = 0. # dragonpilot self.dragon_toyota_stock_dsu = False self.ts_last_check = 0. + self.last_modifed = None def update(self, cp, cp_cam): # dragonpilot, don't check for param too often as it's a kernel call ts = sec_since_boot() if ts - self.ts_last_check >= 5.: - self.dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False + modified = dp_get_last_modified() + if self.last_modifed != modified: + self.dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False + self.last_modifed = modified self.ts_last_check = ts - # update prevs, update must run once per loop - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on + ret = car.CarState.new_message() - self.door_all_closed = not any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'], - cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']]) - self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] + ret.doorOpen = any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'], + cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']]) + ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] != 0 - self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] + ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0 + ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or ret.brakePressed) if self.CP.enableGasInterceptor: - self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. + ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. + ret.gasPressed = ret.gas > 15 elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: - self.pedal_gas = cp.vl["GAS_PEDAL_ALT"]['GAS_PEDAL'] + ret.gas = cp.vl["GAS_PEDAL_ALT"]['GAS_PEDAL'] + ret.gasPressed = ret.gas > 1e-5 else: - self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] - self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] + ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] + ret.gasPressed = ret.gas > 1e-5 - self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS - self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS - self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS - self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS - self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) + ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS + ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - self.standstill = not self.v_ego_raw > 0.001 + ret.standstill = ret.vEgoRaw < 0.001 - if self.CP.carFingerprint in TSS2_CAR: - self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - elif self.CP.carFingerprint in NO_DSU_CAR or self.CP.carFingerprint == CAR.LEXUS_ISH: - # cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start. - # need to apply an offset as soon as the steering angle measurements are both received - self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset - angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] - if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset: - self.init_angle_offset = True - self.angle_offset = self.angle_steers - angle_wheel + # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero + if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3: + self.accurate_steer_angle_seen = True + + if self.accurate_steer_angle_seen: + ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset + + if self.needs_angle_offset: + angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] + if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3: + self.needs_angle_offset = False + self.angle_offset = ret.steeringAngle - angle_wheel else: - self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] - self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] + ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] + + ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] can_gear = int(cp.vl["GEAR_PACKET"]['GEAR']) - self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) - if self.CP.carFingerprint == CAR.LEXUS_IS: - self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON'] - elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: - self.main_on = cp.vl["PCM_CRUISE_ALT"]['MAIN_ON'] - else: - self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] - self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 - self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2 + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 + ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2 - # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state - self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] - self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5] - self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3 - self.brake_error = 0 - self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER'] - self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS'] + ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER'] + ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS'] # we could use the override bit from dbc, but it's triggered at too high torque values - self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - self.user_brake = 0 if self.CP.carFingerprint == CAR.LEXUS_IS: - self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED'] + ret.cruiseState.available = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0 + ret.cruiseState.speed = cp.vl["DSU_CRUISE"]['SET_SPEED'] * CV.KPH_TO_MS self.low_speed_lockout = False elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: - self.v_cruise_pcm = cp.vl["PCM_CRUISE_ALT"]['SET_SPEED'] + ret.cruiseState.available = cp.vl["PCM_CRUISE_ALT"]['MAIN_ON'] != 0 + ret.cruiseState.speed = cp.vl["PCM_CRUISE_ALT"]['SET_SPEED'] * CV.KPH_TO_MS self.low_speed_lockout = False else: - self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] + ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0 + ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] * CV.KPH_TO_MS self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2 if self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: - # Lexus ISH does not have curise status value (always 0), so we use curise_active value instead + # Lexus ISH does not have CRUISE_STATUS value (always 0), so we use CRUISE_ACTIVE value instead self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'] else: self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] - self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) - self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed) - if self.CP.carFingerprint == CAR.PRIUS: - self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 - elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: - self.generic_toggle = bool(cp.vl["LIGHT_STALK_ISH"]['AUTO_HIGH_BEAM']) + if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: + # ignore standstill in hybrid vehicles, since pcm allows to restart without + # receiving any special command. Also if interceptor is detected + ret.cruiseState.standstill = False else: - self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) + ret.cruiseState.standstill = self.pcm_acc_status == 7 + ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) - self.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) + if self.CP.carFingerprint == CAR.PRIUS: + ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 + elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: + ret.genericToggle = bool(cp.vl["LIGHT_STALK_ISH"]['AUTO_HIGH_BEAM']) + else: + ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) + ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) - if self.dragon_toyota_stock_dsu and self.generic_toggle and self.main_on: + ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0 + # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state + self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] + self.steer_warning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5] + + if self.dragon_toyota_stock_dsu and ret.genericToggle and ret.cruiseState.available: enable_acc = True - if not self.seatbelt or not self.door_all_closed: - enable_acc = False - self.pcm_acc_active = enable_acc - if self.standstill: - self.pcm_acc_status = 7 - else: - self.pcm_acc_status = 1 + if ret.seatbeltUnlatched or ret.doorOpen: + enable_acc = False + ret.cruiseState.enabled = enable_acc + + return ret + + @staticmethod + def get_can_parser(CP): + + signals = [ + # sig_name, sig_address, default + ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), + ("GEAR", "GEAR_PACKET", 0), + ("BRAKE_PRESSED", "BRAKE_MODULE", 0), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), + ("DOOR_OPEN_FL", "SEATS_DOORS", 1), + ("DOOR_OPEN_FR", "SEATS_DOORS", 1), + ("DOOR_OPEN_RL", "SEATS_DOORS", 1), + ("DOOR_OPEN_RR", "SEATS_DOORS", 1), + ("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1), + ("TC_DISABLED", "ESP_CONTROL", 1), + ("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0), + ("STEER_RATE", "STEER_ANGLE_SENSOR", 0), + ("CRUISE_ACTIVE", "PCM_CRUISE", 0), + ("CRUISE_STATE", "PCM_CRUISE", 0), + ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), + ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), + ("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0), + ("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers + ("LKA_STATE", "EPS_STATUS", 0), + ("IPAS_STATE", "EPS_STATUS", 1), + ("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0), + ] + + checks = [ + ("WHEEL_SPEEDS", 80), + ("STEER_ANGLE_SENSOR", 80), + ("PCM_CRUISE", 33), + ("STEER_TORQUE_SENSOR", 50), + ("EPS_STATUS", 25), + ] + + if CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: + signals.append(("GAS_PEDAL", "GAS_PEDAL_ALT", 0)) + signals.append(("MAIN_ON", "PCM_CRUISE_ALT", 0)) + signals.append(("SET_SPEED", "PCM_CRUISE_ALT", 0)) + signals.append(("AUTO_HIGH_BEAM", "LIGHT_STALK_ISH", 0)) + checks += [ + ("BRAKE_MODULE", 50), + ("GAS_PEDAL_ALT", 50), + ("PCM_CRUISE_ALT", 1), + ] + else: + signals += [ + ("AUTO_HIGH_BEAM", "LIGHT_STALK", 0), + ("GAS_PEDAL", "GAS_PEDAL", 0), + ] + checks += [ + ("BRAKE_MODULE", 40), + ("GAS_PEDAL", 33), + ] + + if CP.carFingerprint == CAR.LEXUS_IS: + signals.append(("MAIN_ON", "DSU_CRUISE", 0)) + signals.append(("SET_SPEED", "DSU_CRUISE", 0)) + checks.append(("DSU_CRUISE", 5)) + else: + signals.append(("MAIN_ON", "PCM_CRUISE_2", 0)) + signals.append(("SET_SPEED", "PCM_CRUISE_2", 0)) + signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0)) + checks.append(("PCM_CRUISE_2", 33)) + + if CP.carFingerprint == CAR.PRIUS: + signals += [("STATE", "AUTOPARK_STATUS", 0)] + + # add gas interceptor reading if we are using it + if CP.enableGasInterceptor: + signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) + signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) + checks.append(("GAS_SENSOR", 50)) + + checks = [] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + + signals = [("FORCE", "PRE_COLLISION", 0), ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)] + + # use steering message to check if panda is connected to frc + checks = [("STEERING_LKA", 42)] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index ebb26394f..152cae2bb 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,47 +2,12 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser -from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, NO_STOP_TIMER_CAR, TSS2_CAR, FINGERPRINTS +from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase -from common.realtime import sec_since_boot -from common.params import Params -params = Params() -from selfdrive.dragonpilot.dragonconf import dp_get_last_modified - -ButtonType = car.CarState.ButtonEvent.Type -GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.VM = VehicleModel(CP) - - self.frame = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False - - # *** init the major players *** - self.CS = CarState(CP) - - self.cp = get_can_parser(CP) - self.cp_cam = get_cam_can_parser(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) - - # dragonpilot - self.dragon_toyota_stock_dsu = False - self.dragon_enable_steering_on_signal = False - self.dragon_allow_gas = False - self.ts_last_check = 0. - self.dragon_lat_ctrl = True - self.dp_last_modified = None @staticmethod def compute_gb(accel, speed): @@ -50,17 +15,11 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "toyota" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay - ret.safetyModel = car.CarParams.SafetyModel.toyota - ret.enableCruise = True - ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 @@ -81,19 +40,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.indi.outerLoopGain = 3.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.0 - - # TODO: Determine if this is better than INDI - # ret.lateralTuning.init('lqr') - # ret.lateralTuning.lqr.scale = 1500.0 - # ret.lateralTuning.lqr.ki = 0.01 - - # ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] - # ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] - # ret.lateralTuning.lqr.c = [1., 0.] - # ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255] - # ret.lateralTuning.lqr.l = [0.03233671, 0.03185757] - # ret.lateralTuning.lqr.dcGain = 0.002237852961363602 - ret.steerActuatorDelay = 0.5 elif candidate in [CAR.RAV4, CAR.RAV4H]: @@ -317,28 +263,16 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. - ret.steerControlType = car.CarParams.SteerControlType.torque - - # steer, gas, brake limitations VS speed - ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph - ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [1.] - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it smartDsu = 0x2FF in fingerprint[0] # In TSS2 cars the camera does long control ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in TSS2_CAR - ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.apgs) ret.enableGasInterceptor = 0x201 in fingerprint[0] # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR) cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu) - cloudlog.warning("ECU APGS Simulated: %r", ret.enableApgs) cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) # min speed to enable ACC. if car can do stop and go, then set enabling speed @@ -353,8 +287,6 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.deadzoneV = [0., .15] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kiBP = [0., 35.] - ret.stoppingControl = False - ret.startAccel = 0.0 if ret.enableGasInterceptor: ret.gasMaxBP = [0., 9., 35] @@ -371,125 +303,23 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): - # dragonpilot, don't check for param too often as it's a kernel call - ts = sec_since_boot() - if ts - self.ts_last_check >= 5.: - modified = dp_get_last_modified() - if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False - self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False - self.dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False - self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True - self.dp_last_modified = modified - self.ts_last_check = ts - + self.dp_load_params('toyota') # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - self.CS.update(self.cp, self.cp_cam) - - # create message - ret = car.CarState.new_message() + ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid - - # speeds - ret.vEgo = self.CS.v_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.aEgo = self.CS.a_ego - ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # gear shifter - ret.gearShifter = self.CS.gear_shifter - - # gas pedal - ret.gas = self.CS.pedal_gas - if self.CP.enableGasInterceptor: - # use interceptor values to disengage on pedal press - ret.gasPressed = self.CS.pedal_gas > 15 - else: - ret.gasPressed = self.CS.pedal_gas > 0 - - # brake pedal - ret.brake = self.CS.user_brake - ret.brakePressed = self.CS.brake_pressed != 0 - ret.brakeLights = self.CS.brake_lights - - # steering wheel - ret.steeringAngle = self.CS.angle_steers - ret.steeringRate = self.CS.angle_steers_rate - - ret.steeringTorque = self.CS.steer_torque_driver - ret.steeringTorqueEps = self.CS.steer_torque_motor - ret.steeringPressed = self.CS.steer_override + ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - - # cruise state - ret.cruiseState.enabled = self.CS.pcm_acc_active - ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS - ret.cruiseState.available = bool(self.CS.main_on) - ret.cruiseState.speedOffset = 0. - - if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: - # ignore standstill in hybrid vehicles, since pcm allows to restart without - # receiving any special command - # also if interceptor is detected - ret.cruiseState.standstill = False - else: - ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 - - buttonEvents = [] - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on != 0 - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on != 0 - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents - ret.leftBlinker = bool(self.CS.left_blinker_on) - ret.rightBlinker = bool(self.CS.right_blinker_on) - - ret.doorOpen = not self.CS.door_all_closed - ret.seatbeltUnlatched = not self.CS.seatbelt - - ret.genericToggle = self.CS.generic_toggle - ret.stockAeb = self.CS.stock_aeb + ret.buttonEvents = [] # events - events = [] + events = self.create_common_events(ret) if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera: events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT])) - if not ret.gearShifter == GearShifter.drive and self.CP.openpilotLongitudinalControl: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not self.CS.main_on and self.CP.openpilotLongitudinalControl: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if not self.dragon_lat_ctrl: - events.append(create_event('manualSteeringRequired', [ET.WARNING])) - elif (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal: - events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) - elif self.CS.steer_error: - events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl: events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl: @@ -507,27 +337,14 @@ class CarInterface(CarInterfaceBase): elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - if not self.dragon_toyota_stock_dsu: - # DragonAllowGas - if not self.dragon_allow_gas: - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - else: - if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - ret.events = events self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 48d6229c9..a5d149729 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -1,27 +1,3 @@ -def create_ipas_steer_command(packer, steer, enabled, apgs_enabled): - """Creates a CAN message for the Toyota Steer Command.""" - if steer < 0: - direction = 3 - elif steer > 0: - direction = 1 - else: - direction = 2 - - mode = 3 if enabled else 1 - - values = { - "STATE": mode, - "DIRECTION_CMD": direction, - "ANGLE": steer, - "SET_ME_X10": 0x10, - "SET_ME_X40": 0x40 - } - if apgs_enabled: - return packer.make_can_msg("STEERING_IPAS", 0, values) - else: - return packer.make_can_msg("STEERING_IPAS_COMMA", 0, values) - - def create_steer_command(packer, steer, steer_req, raw_cnt): """Creates a CAN message for the Toyota Steer Command.""" diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 9c8e222b2..9c900c090 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -59,20 +59,11 @@ STATIC_MSGS = [ (0x470, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'), (0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'), (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), - - (0x292, Ecu.apgs, (CAR.PRIUS), 0, 3, b'\x00\x00\x00\x00\x00\x00\x00\x9e'), - (0x32E, Ecu.apgs, (CAR.PRIUS), 0, 20, b'\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x396, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\xBD\x00\x00\x00\x60\x0F\x02\x00'), - (0x43A, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x84\x00\x00\x00\x00\x00\x00\x00'), - (0x43B, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x497, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x4CC, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x0D\x00\x00\x00\x00\x00\x00\x00'), ] ECU_FINGERPRINT = { Ecu.fwdCamera: [0x2e4], # steer torque cmd Ecu.dsu: [0x283], # accel cmd - Ecu.apgs: [0x835], # angle cmd } @@ -81,14 +72,13 @@ FINGERPRINTS = { 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8 }], CAR.RAV4H: [{ - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, # Chinese RAV4 { 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8 }], CAR.PRIUS: [{ - # with ipas 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, { @@ -190,26 +180,14 @@ FINGERPRINTS = { 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], CAR.HIGHLANDER: [{ - 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1992: 8, 1996: 8, 1990: 8, 1998: 8 - }, - # 2019 Highlander XLE - { - 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 - }, - # 2017 Highlander Limited - { - 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 - }, - # 2018 Highlander Limited Platinum - { - 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1585: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1988: 8, 1990: 8, 1996: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8 + 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1585: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8 }], CAR.HIGHLANDER_TSS2: [{ # 2020 highlander limited 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1952: 8, 1960: 8, 1990: 8, 1998: 8 }], CAR.HIGHLANDERH: [{ - 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, { # 2019 Highlander Hybrid Limited Platinum @@ -250,11 +228,7 @@ FINGERPRINTS = { }, { # 2019 Chinese Levin (Petrol) from Shell - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1984: 8, 1992: 8, 2002: 8 - }, - { - # 2019 Chinese Corolla (Petrol) from Shell - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8 + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1984: 8, 1992: 8, 2002: 8 }], CAR.COROLLAH_TSS2: [ # 2019 Taiwan Altis Hybrid @@ -338,6 +312,7 @@ FW_VERSIONS = { b'\x018966333P4400\x00\x00\x00\x00', b'\x018966333P4500\x00\x00\x00\x00', b'\x018966333P4700\x00\x00\x00\x00', + b'\x018966333Q6200\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'8821F0607200 ', @@ -350,7 +325,10 @@ FW_VERSIONS = { b'F152606290\x00\x00\x00\x00\x00\x00', b'F152633540\x00\x00\x00\x00\x00\x00', ], - (Ecu.eps, 0x7a1, None): [b'8965B33540\x00\x00\x00\x00\x00\x00'], + (Ecu.eps, 0x7a1, None): [ + b'8965B33540\x00\x00\x00\x00\x00\x00', + b'8965B33542\x00\x00\x00\x00\x00\x00', + ], (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 b'8821F0601300 ', b'8821F0603300 ', @@ -360,6 +338,7 @@ FW_VERSIONS = { b'8646F0601200 ', b'8646F0601300 ', b'8646F0603400 ', + b'8646F0605000 ', ], }, CAR.CAMRYH: { @@ -392,12 +371,40 @@ FW_VERSIONS = { ], }, CAR.CHR: { - (Ecu.dsu, 0x791, None): [b'8821FF404100 '], - (Ecu.esp, 0x7b0, None): [b'F1526F4122\x00\x00\x00\x00\x00\x00'], - (Ecu.eps, 0x7a1, None): [b'8965B10040\x00\x00\x00\x00\x00\x00'], - (Ecu.engine, 0x7e0, None): [b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00'], - (Ecu.fwdRadar, 0x750, 0xf): [b'8821FF404100 '], - (Ecu.fwdCamera, 0x750, 0x6d): [b'8646FF404000 '], + (Ecu.engine, 0x700, None): [ + b'\x0189663F413100\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'8821FF401600 ', + b'8821FF404100 ', + ], + (Ecu.esp, 0x7b0, None): [ + b'F1526F4073\x00\x00\x00\x00\x00\x00', + b'F1526F4122\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B10011\x00\x00\x00\x00\x00\x00', + b'8965B10040\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821FF401600 ', + b'8821FF404100 ', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646FF401800 ', + b'8646FF404000 ', + ], + }, + CAR.CHRH: { + (Ecu.engine, 0x700, None): [b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00'], + (Ecu.esp, 0x7b0, None): [b'F152610040\x00\x00\x00\x00\x00\x00'], + (Ecu.dsu, 0x791, None): [b'8821FF407100 '], + (Ecu.eps, 0x7a1, None): [b'8965B10050\x00\x00\x00\x00\x00\x00'], + (Ecu.fwdRadar, 0x750, 0xf): [b'8821FF407100 '], + (Ecu.fwdCamera, 0x750, 0x6d): [b'8646FF407000 '], }, CAR.COROLLA: { (Ecu.engine, 0x7e0, None): [ @@ -501,9 +508,11 @@ FW_VERSIONS = { (Ecu.esp, 0x7b0, None): [ b'F152612590\x00\x00\x00\x00\x00\x00', b'F152612691\x00\x00\x00\x00\x00\x00', + b'F152612692\x00\x00\x00\x00\x00\x00', b'F152612700\x00\x00\x00\x00\x00\x00', b'F152612800\x00\x00\x00\x00\x00\x00', b'F152612840\x00\x00\x00\x00\x00\x00', + b'F152612A10\x00\x00\x00\x00\x00\x00', b'F152642540\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ @@ -583,6 +592,7 @@ FW_VERSIONS = { b'\x03896634786000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', b'\x03896634789000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703002\x00\x00\x00\x00', b'\x038966347A3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707001\x00\x00\x00\x00', + b'\x038966347B7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B47021\x00\x00\x00\x00\x00\x00', @@ -690,7 +700,6 @@ FW_VERSIONS = { }, CAR.RAV4_TSS2: { (Ecu.engine, 0x700, None): [ - b'\x018966333Q6200\x00\x00\x00\x00', b'\x018966342E2000\x00\x00\x00\x00', b'\x018966342M8000\x00\x00\x00\x00', b'\x018966342T1000\x00\x00\x00\x00', @@ -705,7 +714,6 @@ FW_VERSIONS = { b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ - b'F152606230\x00\x00\x00\x00\x00\x00', b'F152642520\x00\x00\x00\x00\x00\x00', b'\x01F15260R210\x00\x00\x00\x00\x00\x00', b'\x01F15260R220\x00\x00\x00\x00\x00\x00', @@ -714,20 +722,17 @@ FW_VERSIONS = { b'\x01F152642710\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ - b'8965B33540\x00\x00\x00\x00\x00\x00', b'8965B42170\x00\x00\x00\x00\x00\x00', b'8965B42171\x00\x00\x00\x00\x00\x00', b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ - b'8821F0607200 ', b'\x018821F3301100\x00\x00\x00\x00', b'\x018821F3301200\x00\x00\x00\x00', b'\x018821F3301300\x00\x00\x00\x00', b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ - b'8646F0605000 ', b'\x028646F4203200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F4203300\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', @@ -736,12 +741,14 @@ FW_VERSIONS = { }, CAR.RAV4H_TSS2: { (Ecu.engine, 0x700, None): [ + b'\x018966342M5000\x00\x00\x00\x00', b'\x018966342X6000\x00\x00\x00\x00', b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152642291\x00\x00\x00\x00\x00\x00', + b'F152642330\x00\x00\x00\x00\x00\x00', b'F152642531\x00\x00\x00\x00\x00\x00', b'F152642532\x00\x00\x00\x00\x00\x00', ], @@ -789,6 +796,28 @@ FW_VERSIONS = { ], (Ecu.fwdCamera, 0x750, 0x6d): [b'8646F0801100\x00\x00\x00\x00'], }, + CAR.LEXUS_ESH_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x028966333S8000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'F152633423\x00\x00\x00\x00\x00\x00', + b'F152633680\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33252\x00\x00\x00\x00\x00\x00', + b'8965B33590\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + ], + }, CAR.LEXUS_RXH: { (Ecu.engine, 0x7e0, None): [ b'\x02348Q4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 610e1d5a0..6c90d69e7 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,20 +1,17 @@ from cereal import car from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import volkswagencan -from selfdrive.car.volkswagen.values import DBC, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams +from selfdrive.car.volkswagen.values import DBC, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert class CarController(): - def __init__(self, canbus, car_fingerprint): + def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 - self.car_fingerprint = car_fingerprint - # Setup detection helper. Routes commands to an appropriate CAN bus number. - self.canbus = canbus - self.packer_pt = CANPacker(DBC[car_fingerprint]['pt']) + self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt']) self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 @@ -32,7 +29,6 @@ class CarController(): # Send CAN commands. can_sends = [] - canbus = self.canbus #-------------------------------------------------------------------------- # # @@ -49,14 +45,14 @@ class CarController(): # FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop # commanding HCA if there's a fault, so the steering rack recovers. - if enabled and not (CS.standstill or CS.steeringFault): + if enabled and not (CS.out.standstill or CS.steeringFault): # FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This # is inherently handled by scaling to STEER_MAX. The rack doesn't seem # to care about up/down rate, but we have some evidence it may do its # own rate limiting, and matching OP helps for accurate tuning. new_steer = int(round(actuators.steer * P.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steeringTorque, P) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer # FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending @@ -102,7 +98,7 @@ class CarController(): self.apply_steer_last = apply_steer idx = (frame / P.HCA_STEP) % 16 - can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, canbus.pt, apply_steer, + can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) #-------------------------------------------------------------------------- @@ -116,15 +112,15 @@ class CarController(): # filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz. if frame % P.LDW_STEP == 0: - hcaEnabled = True if enabled and not CS.standstill else False + hcaEnabled = True if enabled and not CS.out.standstill else False if visual_alert == VisualAlert.steerRequired: hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] - can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, canbus.pt, hcaEnabled, - CS.steeringPressed, hud_alert, leftLaneVisible, + can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, hcaEnabled, + CS.out.steeringPressed, hud_alert, leftLaneVisible, rightLaneVisible)) #-------------------------------------------------------------------------- @@ -140,11 +136,11 @@ class CarController(): # stock ACC with OP disengagement, or to auto-resume from stop. if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: - if not enabled and CS.accEnabled: + if not enabled and CS.out.cruiseState.enabled: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True - elif enabled and CS.standstill: + elif enabled and CS.out.standstill: # Blip the Resume button if we're engaged at standstill. # FIXME: This is a naive implementation, improve with visiond or radar input. # A subset of MQBs like to "creep" too aggressively with this implementation. @@ -182,7 +178,7 @@ class CarController(): if self.graMsgSentCount == 0: self.graMsgStartFramePrev = frame idx = (CS.graMsgBusCounter + 1) % 16 - can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, canbus.pt, self.graButtonStatesToSend, CS, idx)) + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, CANBUS.pt, self.graButtonStatesToSend, CS, idx)) self.graMsgSentCount += 1 if self.graMsgSentCount >= P.GRA_VBP_COUNT: self.graButtonStatesToSend = None diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 79cd06310..91b24dc4f 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -1,149 +1,59 @@ import numpy as np +from cereal import car from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES, CarControllerParams - -def get_mqb_pt_can_parser(CP, canbus): - # this function generates lists for signal, messages and initial values - signals = [ - # sig_name, sig_address, default - ("LWI_Lenkradwinkel", "LWI_01", 0), # Absolute steering angle - ("LWI_VZ_Lenkradwinkel", "LWI_01", 0), # Steering angle sign - ("LWI_Lenkradw_Geschw", "LWI_01", 0), # Absolute steering rate - ("LWI_VZ_Lenkradw_Geschw", "LWI_01", 0), # Steering rate sign - ("ESP_VL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front left - ("ESP_VR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front right - ("ESP_HL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear left - ("ESP_HR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear right - ("ESP_Gierrate", "ESP_02", 0), # Absolute yaw rate - ("ESP_VZ_Gierrate", "ESP_02", 0), # Yaw rate sign - ("ZV_FT_offen", "Gateway_72", 0), # Door open, driver - ("ZV_BT_offen", "Gateway_72", 0), # Door open, passenger - ("ZV_HFS_offen", "Gateway_72", 0), # Door open, rear left - ("ZV_HBFS_offen", "Gateway_72", 0), # Door open, rear right - ("ZV_HD_offen", "Gateway_72", 0), # Trunk or hatch open - ("BH_Blinker_li", "Gateway_72", 0), # Left turn signal on - ("BH_Blinker_re", "Gateway_72", 0), # Right turn signal on - ("GE_Fahrstufe", "Getriebe_11", 0), # Auto trans gear selector position - ("AB_Gurtschloss_FA", "Airbag_02", 0), # Seatbelt status, driver - ("AB_Gurtschloss_BF", "Airbag_02", 0), # Seatbelt status, passenger - ("ESP_Fahrer_bremst", "ESP_05", 0), # Brake pedal pressed - ("ESP_Status_Bremsdruck", "ESP_05", 0), # Brakes applied - ("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied - ("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value - ("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch - ("Driver_Strain", "EPS_01", 0), # Absolute driver torque input - ("Driver_Strain_VZ", "EPS_01", 0), # Driver torque input sign - ("HCA_Ready", "EPS_01", 0), # Steering rack HCA support configured - ("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled - ("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display - ("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied - ("TSK_Fahrzeugmasse_02", "Motor_16", 0), # Estimated vehicle mass from drivetrain coordinator - ("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status - ("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go) - ("SetSpeed", "ACC_02", 0), # ACC set speed - ("GRA_Hauptschalter", "GRA_ACC_01", 0), # ACC button, on/off - ("GRA_Abbrechen", "GRA_ACC_01", 0), # ACC button, cancel - ("GRA_Tip_Setzen", "GRA_ACC_01", 0), # ACC button, set - ("GRA_Tip_Hoch", "GRA_ACC_01", 0), # ACC button, increase or accel - ("GRA_Tip_Runter", "GRA_ACC_01", 0), # ACC button, decrease or decel - ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01", 0), # ACC button, resume - ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01", 0), # ACC button, time gap adj - ("GRA_Typ_Hauptschalter", "GRA_ACC_01", 0), # ACC main button type - ("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type - ("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type - ("COUNTER", "GRA_ACC_01", 0), # GRA_ACC_01 CAN message counter - ] - - checks = [ - # sig_address, frequency - ("LWI_01", 100), # From J500 Steering Assist with integrated sensors - ("EPS_01", 100), # From J500 Steering Assist with integrated sensors - ("ESP_19", 100), # From J104 ABS/ESP controller - ("ESP_05", 50), # From J104 ABS/ESP controller - ("ESP_21", 50), # From J104 ABS/ESP controller - ("ACC_06", 50), # From J428 ACC radar control module - ("Motor_20", 50), # From J623 Engine control module - ("GRA_ACC_01", 33), # From J??? steering wheel control buttons - ("ACC_02", 17), # From J428 ACC radar control module - ("Getriebe_11", 20), # From J743 Auto transmission control module - ("Gateway_72", 10), # From J533 CAN gateway (aggregated data) - ("Motor_14", 10), # From J623 Engine control module - ("Airbag_02", 5), # From J234 Airbag control module - ("Kombi_01", 2), # From J285 Instrument cluster - ("Motor_16", 2), # From J623 Engine control module - ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.pt) - -# A single signal is monitored from the camera CAN bus, and then ignored, -# so the presence of CAN traffic can be verified with cam_cp.valid. - -def get_mqb_cam_can_parser(CP, canbus): - - signals = [ - # sig_name, sig_address, default - ("Kombi_Lamp_Green", "LDW_02", 0), # Lane Assist status LED - ] - - checks = [ - # sig_address, frequency - ("LDW_02", 10) # From R242 Driver assistance camera - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.cam) - +from selfdrive.car.volkswagen.values import DBC, CANBUS, BUTTON_STATES, CarControllerParams class CarState(CarStateBase): - def __init__(self, CP, canbus): + def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe'] self.buttonStates = BUTTON_STATES.copy() def update(self, pt_cp): + ret = car.CarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. - self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS - self.wheelSpeedFR = pt_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS - self.wheelSpeedRL = pt_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS - self.wheelSpeedRR = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS + ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = pt_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = pt_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS - self.vEgoRaw = float(np.mean([self.wheelSpeedFL, self.wheelSpeedFR, self.wheelSpeedRL, self.wheelSpeedRR])) - self.vEgo, self.aEgo = self.update_speed_kf(self.vEgoRaw) + ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - self.standstill = self.vEgoRaw < 0.1 + ret.standstill = ret.vEgoRaw < 0.1 # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. - self.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] - self.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] - self.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])] - self.steeringPressed = abs(self.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE - self.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD + ret.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] + ret.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] + ret.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])] + ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE + ret.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD # Update gas, brakes, and gearshift. - self.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0 - self.gasPressed = self.gas > 0 - self.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects - self.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst']) - self.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) + ret.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0 + ret.gasPressed = ret.gas > 0 + ret.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects + ret.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst']) + ret.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) # Update gear and/or clutch position data. can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe']) - self.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None)) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None)) # Update door and trunk/hatch lid open status. - self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'], - pt_cp.vl["Gateway_72"]['ZV_BT_offen'], - pt_cp.vl["Gateway_72"]['ZV_HFS_offen'], - pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'], - pt_cp.vl["Gateway_72"]['ZV_HD_offen']]) + ret.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'], + pt_cp.vl["Gateway_72"]['ZV_BT_offen'], + pt_cp.vl["Gateway_72"]['ZV_HFS_offen'], + pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'], + pt_cp.vl["Gateway_72"]['ZV_HD_offen']]) # Update seatbelt fastened status. - self.seatbeltUnlatched = False if pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True + ret.seatbeltUnlatched = False if pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True # Update driver preference for metric. VW stores many different unit # preferences, including separate units for for distance vs. speed. @@ -151,42 +61,35 @@ class CarState(CarStateBase): self.displayMetricUnits = not pt_cp.vl["Einheiten_01"]["KBI_MFA_v_Einheit_02"] # Update ACC radar status. - accStatus = pt_cp.vl["ACC_06"]['ACC_Status_ACC'] - if accStatus == 1: - # ACC okay but disabled - self.accFault = False - self.accAvailable = False - self.accEnabled = False - elif accStatus == 2: + accStatus = pt_cp.vl["TSK_06"]['TSK_Status'] + if accStatus == 2: # ACC okay and enabled, but not currently engaged - self.accFault = False - self.accAvailable = True - self.accEnabled = False + ret.cruiseState.available = True + ret.cruiseState.enabled = False elif accStatus in [3, 4, 5]: # ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5) - self.accFault = False - self.accAvailable = True - self.accEnabled = True + ret.cruiseState.available = True + ret.cruiseState.enabled = True else: - # ACC fault of some sort. Seen statuses 6 or 7 for CAN comms disruptions, visibility issues, etc. - self.accFault = True - self.accAvailable = False - self.accEnabled = False + # ACC okay but disabled (1), or a radar visibility or other fault/disruption (6 or 7) + ret.cruiseState.available = False + ret.cruiseState.enabled = False # Update ACC setpoint. When the setpoint is zero or there's an error, the # radar sends a set-speed of ~90.69 m/s / 203mph. - self.accSetSpeed = pt_cp.vl["ACC_02"]['SetSpeed'] - if self.accSetSpeed > 90: self.accSetSpeed = 0 + ret.cruiseState.speed = pt_cp.vl["ACC_02"]['SetSpeed'] + if ret.cruiseState.speed > 90: + ret.cruiseState.speed = 0 # Update control button states for turn signals and ACC controls. - self.buttonStates["leftBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_li']) - self.buttonStates["rightBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_re']) self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Hoch']) self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Runter']) self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Abbrechen']) self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Setzen']) self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme']) self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke']) + ret.leftBlinker = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_li']) + ret.rightBlinker = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_re']) # Read ACC hardware button type configuration info that has to pass thru # to the radar. Ends up being different for steering wheel buttons vs @@ -205,5 +108,101 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well - self.stabilityControlDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv'] + ret.espDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv'] != 0 + return ret + + @staticmethod + def get_can_parser(CP): + # this function generates lists for signal, messages and initial values + signals = [ + # sig_name, sig_address, default + ("LWI_Lenkradwinkel", "LWI_01", 0), # Absolute steering angle + ("LWI_VZ_Lenkradwinkel", "LWI_01", 0), # Steering angle sign + ("LWI_Lenkradw_Geschw", "LWI_01", 0), # Absolute steering rate + ("LWI_VZ_Lenkradw_Geschw", "LWI_01", 0), # Steering rate sign + ("ESP_VL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front left + ("ESP_VR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front right + ("ESP_HL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear left + ("ESP_HR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear right + ("ESP_Gierrate", "ESP_02", 0), # Absolute yaw rate + ("ESP_VZ_Gierrate", "ESP_02", 0), # Yaw rate sign + ("ZV_FT_offen", "Gateway_72", 0), # Door open, driver + ("ZV_BT_offen", "Gateway_72", 0), # Door open, passenger + ("ZV_HFS_offen", "Gateway_72", 0), # Door open, rear left + ("ZV_HBFS_offen", "Gateway_72", 0), # Door open, rear right + ("ZV_HD_offen", "Gateway_72", 0), # Trunk or hatch open + ("BH_Blinker_li", "Gateway_72", 0), # Left turn signal on + ("BH_Blinker_re", "Gateway_72", 0), # Right turn signal on + ("GE_Fahrstufe", "Getriebe_11", 0), # Auto trans gear selector position + ("AB_Gurtschloss_FA", "Airbag_02", 0), # Seatbelt status, driver + ("AB_Gurtschloss_BF", "Airbag_02", 0), # Seatbelt status, passenger + ("ESP_Fahrer_bremst", "ESP_05", 0), # Brake pedal pressed + ("ESP_Status_Bremsdruck", "ESP_05", 0), # Brakes applied + ("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied + ("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value + ("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch + ("Driver_Strain", "EPS_01", 0), # Absolute driver torque input + ("Driver_Strain_VZ", "EPS_01", 0), # Driver torque input sign + ("HCA_Ready", "EPS_01", 0), # Steering rack HCA support configured + ("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled + ("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display + ("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied + ("TSK_Status", "TSK_06", 0), # ACC engagement status from drivetrain coordinator + ("TSK_Fahrzeugmasse_02", "Motor_16", 0), # Estimated vehicle mass from drivetrain coordinator + ("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status + ("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go) + ("SetSpeed", "ACC_02", 0), # ACC set speed + ("GRA_Hauptschalter", "GRA_ACC_01", 0), # ACC button, on/off + ("GRA_Abbrechen", "GRA_ACC_01", 0), # ACC button, cancel + ("GRA_Tip_Setzen", "GRA_ACC_01", 0), # ACC button, set + ("GRA_Tip_Hoch", "GRA_ACC_01", 0), # ACC button, increase or accel + ("GRA_Tip_Runter", "GRA_ACC_01", 0), # ACC button, decrease or decel + ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01", 0), # ACC button, resume + ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01", 0), # ACC button, time gap adj + ("GRA_Typ_Hauptschalter", "GRA_ACC_01", 0), # ACC main button type + ("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type + ("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type + ("COUNTER", "GRA_ACC_01", 0), # GRA_ACC_01 CAN message counter + ] + + checks = [ + # sig_address, frequency + ("LWI_01", 100), # From J500 Steering Assist with integrated sensors + ("EPS_01", 100), # From J500 Steering Assist with integrated sensors + ("ESP_19", 100), # From J104 ABS/ESP controller + ("ESP_05", 50), # From J104 ABS/ESP controller + ("ESP_21", 50), # From J104 ABS/ESP controller + ("ACC_06", 50), # From J428 ACC radar control module + ("Motor_20", 50), # From J623 Engine control module + ("TSK_06", 50), # From J623 Engine control module + ("GRA_ACC_01", 33), # From J??? steering wheel control buttons + ("ACC_02", 17), # From J428 ACC radar control module + ("Getriebe_11", 20), # From J743 Auto transmission control module + ("Gateway_72", 10), # From J533 CAN gateway (aggregated data) + ("Motor_14", 10), # From J623 Engine control module + ("Airbag_02", 5), # From J234 Airbag control module + ("Kombi_01", 2), # From J285 Instrument cluster + ("Motor_16", 2), # From J623 Engine control module + ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.pt) + + # A single signal is monitored from the camera CAN bus, and then ignored, + # so the presence of CAN traffic can be verified with cam_cp.valid. + + @staticmethod + def get_cam_can_parser(CP): + + signals = [ + # sig_name, sig_address, default + ("Kombi_Lamp_Green", "LDW_02", 0), # Lane Assist status LED + ] + + checks = [ + # sig_address, frequency + ("LDW_02", 10) # From R242 Driver assistance camera + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.cam) diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 0e8b5206d..4a2f7cb7a 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,67 +1,38 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET -from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES -from selfdrive.car.volkswagen.carstate import CarState, get_mqb_pt_can_parser, get_mqb_cam_can_parser from common.params import Params from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase GEAR = car.CarState.GearShifter -class CANBUS: - pt = 0 - cam = 2 - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.CC = None + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) - self.frame = 0 - - self.gasPressedPrev = False - self.brakePressedPrev = False - self.cruiseStateEnabledPrev = False self.displayMetricUnitsPrev = None self.buttonStatesPrev = BUTTON_STATES.copy() - # *** init the major players *** - self.CS = CarState(CP, CANBUS) - self.VM = VehicleModel(CP) - self.pt_cp = get_mqb_pt_can_parser(CP, CANBUS) - self.cam_cp = get_mqb_cam_can_parser(CP, CANBUS) - - # sending if read only is False - if CarController is not None: - self.CC = CarController(CANBUS, CP.carFingerprint) - @staticmethod def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - ret = car.CarParams.new_message() - - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) if candidate == CAR.GOLF: # Set common MQB parameters that will apply globally ret.carName = "volkswagen" + ret.radarOffCan = True ret.safetyModel = car.CarParams.SafetyModel.volkswagen - ret.enableCruise = True # Stock ACC still controls acceleration and braking - ret.openpilotLongitudinalControl = False - ret.steerControlType = car.CarParams.SteerControlType.torque # Additional common MQB parameters that may be overridden per-vehicle ret.steerRateCost = 0.5 ret.steerActuatorDelay = 0.05 # Hopefully all MQB racks are similar here ret.steerLimitTimer = 0.4 - ret.steerMaxBP = [0.] # m/s - ret.steerMaxV = [1.] # As a starting point for speed-adjusted lateral tuning, use the example # map speed breakpoints from a VW Tiguan (SSP 399 page 9). It's unclear @@ -89,19 +60,6 @@ class CarInterface(CarInterfaceBase): ret.enableCamera = True # Stock camera detection doesn't apply to VW ret.transmissionType = car.CarParams.TransmissionType.automatic - ret.steerRatioRear = 0. - - # No support for OP longitudinal control on Volkswagen at this time. - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [0.] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.longitudinalTuning.kpBP = [0.] - ret.longitudinalTuning.kpV = [0.] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.] # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase @@ -117,63 +75,24 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): canMonoTimes = [] - events = [] buttonEvents = [] params = Params() - ret = car.CarState.new_message() # Process the most recent CAN message traffic, and check for validity # The camera CAN has no signals we use at this time, but we process it # anyway so we can test connectivity with can_valid - self.pt_cp.update_strings(can_strings) - self.cam_cp.update_strings(can_strings) - self.CS.update(self.pt_cp) - ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid + self.cp.update_strings(can_strings) + self.cp_cam.update_strings(can_strings) - # Wheel and vehicle speed, yaw rate - ret.wheelSpeeds.fl = self.CS.wheelSpeedFL - ret.wheelSpeeds.fr = self.CS.wheelSpeedFR - ret.wheelSpeeds.rl = self.CS.wheelSpeedRL - ret.wheelSpeeds.rr = self.CS.wheelSpeedRR - ret.vEgoRaw = self.CS.vEgoRaw - ret.vEgo = self.CS.vEgo - ret.aEgo = self.CS.aEgo - ret.standstill = self.CS.standstill - - # Steering wheel position, movement, yaw rate, and driver input - ret.steeringAngle = self.CS.steeringAngle - ret.steeringRate = self.CS.steeringRate - ret.steeringTorque = self.CS.steeringTorque - ret.steeringPressed = self.CS.steeringPressed + ret = self.CS.update(self.cp) + ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - ret.yawRate = self.CS.yawRate - - # Gas, brakes and shifting - ret.gas = self.CS.gas - ret.gasPressed = self.CS.gasPressed - ret.brake = self.CS.brake - ret.brakePressed = self.CS.brakePressed - ret.brakeLights = self.CS.brakeLights - ret.gearShifter = self.CS.gearShifter - - # Doors open, seatbelt unfastened - ret.doorOpen = self.CS.doorOpen - ret.seatbeltUnlatched = self.CS.seatbeltUnlatched # Update the EON metric configuration to match the car at first startup, # or if there's been a change. if self.CS.displayMetricUnits != self.displayMetricUnitsPrev: params.put("IsMetric", "1" if self.CS.displayMetricUnits else "0") - # Blinker switch updates - ret.leftBlinker = self.CS.buttonStates["leftBlinker"] - ret.rightBlinker = self.CS.buttonStates["rightBlinker"] - - # ACC cruise state - ret.cruiseState.available = self.CS.accAvailable - ret.cruiseState.enabled = self.CS.accEnabled - ret.cruiseState.speed = self.CS.accSetSpeed - # Check for and process state-change events (button press or release) from # the turn stalk switch or ACC steering wheel/control stalk buttons. for button in self.CS.buttonStates: @@ -183,40 +102,20 @@ class CarInterface(CarInterfaceBase): be.pressed = self.CS.buttonStates[button] buttonEvents.append(be) - # Vehicle operation safety checks and events - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.gearShifter == GEAR.reverse: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if not ret.gearShifter in [GEAR.drive, GEAR.eco, GEAR.sport]: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.stabilityControlDisabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + events = self.create_common_events(ret, extra_gears=[GEAR.eco, GEAR.sport]) + + # Vehicle health and operation safety checks if self.CS.parkingBrakeSet: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) - - # Vehicle health safety checks and events - if self.CS.accFault: - events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.steeringFault: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) - # Per the Comma safety model, disable on pedals rising edge or when brake - # is pressed and speed isn't zero. - if (ret.gasPressed and not self.gasPressedPrev) or \ - (ret.brakePressed and (not self.brakePressedPrev or not ret.standstill)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - # Engagement and longitudinal control using stock ACC. Make sure OP is # disengaged if stock ACC is disengaged. if not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # Attempt OP engagement only on rising edge of stock ACC engagement. - elif not self.cruiseStateEnabledPrev: + elif not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) ret.events = events @@ -224,14 +123,14 @@ class CarInterface(CarInterfaceBase): ret.canMonoTimes = canMonoTimes # update previous car states - self.gasPressedPrev = ret.gasPressed - self.brakePressedPrev = ret.brakePressed - self.cruiseStateEnabledPrev = ret.cruiseState.enabled + self.gas_pressed_prev = ret.gasPressed + self.brake_pressed_prev = ret.brakePressed + self.cruise_enabled_prev = ret.cruiseState.enabled self.displayMetricUnitsPrev = self.CS.displayMetricUnits self.buttonStatesPrev = self.CS.buttonStates.copy() - # cast to reader so it can't be modified - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out def apply(self, c): can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 045e03ae3..30c32b608 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -9,18 +9,20 @@ class CarControllerParams: GRA_VBP_COUNT = 16 # Send VBP messages for ~0.5s (GRA_ACC_STEP * 16) # Observed documented MQB limits: 3.00 Nm max, rate of change 5.00 Nm/sec. - # Limiting both torque and rate-of-change based on real-world testing and - # Comma's safety requirements for minimum time to lane departure. - STEER_MAX = 250 # Max heading control assist torque 2.50 Nm - STEER_DELTA_UP = 4 # Max HCA reached in 1.25s (STEER_MAX / (50Hz * 1.25)) + # Limiting rate-of-change based on real-world testing and Comma's safety + # requirements for minimum time to lane departure. + STEER_MAX = 300 # Max heading control assist torque 3.00 Nm + STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50)) STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) STEER_DRIVER_ALLOWANCE = 80 STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily STEER_DRIVER_FACTOR = 1 # from dbc +class CANBUS: + pt = 0 + cam = 2 + BUTTON_STATES = { - "leftBlinker": False, - "rightBlinker": False, "accelCruise": False, "decelCruise": False, "cancel": False, diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 74d3ae1ba..714b71d8e 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.7.3-release" +#define COMMA_VERSION "0.7.4-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4bef4b8a5..4c521ae35 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -71,6 +71,8 @@ def events_to_bytes(events): for e in events: if isinstance(e, capnp.lib.capnp._DynamicStructReader): e = e.as_builder() + if not e.is_root: + e = e.copy() ret.append(e.to_bytes()) return ret @@ -361,8 +363,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk force_decel = sm['dMonitoringState'].awarenessStatus < 0. # controlsState - dat = messaging.new_message() - dat.init('controlsState') + dat = messaging.new_message('controlsState') dat.valid = CS.canValid dat.controlsState = { "alertText1": AM.alert_text_1, @@ -414,8 +415,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk pm.send('controlsState', dat) # carState - cs_send = messaging.new_message() - cs_send.init('carState') + cs_send = messaging.new_message('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = events @@ -424,21 +424,18 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk # carEvents - logged every second or on change events_bytes = events_to_bytes(events) if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev): - ce_send = messaging.new_message() - ce_send.init('carEvents', len(events)) + ce_send = messaging.new_message('carEvents', len(events)) ce_send.carEvents = events pm.send('carEvents', ce_send) # carParams - logged every 50 seconds (> 1 per segment) if (sm.frame % int(50. / DT_CTRL) == 0): - cp_send = messaging.new_message() - cp_send.init('carParams') + cp_send = messaging.new_message('carParams') cp_send.carParams = CP pm.send('carParams', cp_send) # carControl - cc_send = messaging.new_message() - cc_send.init('carControl') + cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC pm.send('carControl', cc_send) @@ -557,7 +554,10 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if dp_last_modified != modified: dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False dragon_lat_control = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True - dragon_display_steering_limit_alert = False if params.get("DragonDisplaySteeringLimitAlert", encoding='utf8') == "0" else True + if dragon_lat_control: + dragon_display_steering_limit_alert = False if params.get("DragonDisplaySteeringLimitAlert", encoding='utf8') == "0" else True + else: + dragon_display_steering_limit_alert = False dragon_lead_car_moving_alert = True if params.get("DragonEnableLeadCarMovingAlert", encoding='utf8') == "1" else False dp_last_modified = modified ts_last_check = ts @@ -612,9 +612,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): else: dragon_stopped_has_lead_count = 0 - # when we detect lead car over a sec and the lead car is started moving, we are ready to send alerts + # when we detect lead car over 3 secs and the lead car is started moving, we are ready to send alerts # once the condition is triggered, we want to keep the trigger - if dragon_stopped_has_lead_count >= 100: + if dragon_stopped_has_lead_count >= 300: if abs(sm['plan'].vTargetFuture) >= 0.1: events.append(create_event('leadCarMoving', [ET.WARNING])) else: diff --git a/selfdrive/controls/dmonitoringd.py b/selfdrive/controls/dmonitoringd.py index a1639b522..0a6a3a792 100755 --- a/selfdrive/controls/dmonitoringd.py +++ b/selfdrive/controls/dmonitoringd.py @@ -59,20 +59,17 @@ def dmonitoringd_thread(sm=None, pm=None): # load driver monitor val only when safety is on if dp_enable_driver_safety_check: dp_enable_driver_monitoring = False if params.get("DragonEnableDriverMonitoring", encoding='utf8') == "0" else True - # load steering monitor timer val only when driver monitor is on - if dp_enable_driver_safety_check: try: dp_awareness_time = int(params.get("DragonSteeringMonitorTimer", encoding='utf8')) except TypeError: dp_awareness_time = 0. driver_status.awareness_time = 86400 if dp_awareness_time <= 0. else dp_awareness_time * 60. + else: + dp_enable_driver_monitoring = False + driver_status.awareness_time = 86400 dp_last_modified = modified last_ts = cur_time - if not dp_enable_driver_safety_check: - dp_enable_driver_monitoring = False - driver_status.awareness_time = 86400 - # reset all awareness val and set to rhd region, this will enforce steering monitor. if not dp_enable_driver_monitoring: driver_status.is_rhd_region = True @@ -123,8 +120,7 @@ def dmonitoringd_thread(sm=None, pm=None): events = driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill) # dMonitoringState packet - dat = messaging.new_message() - dat.init('dMonitoringState') + dat = messaging.new_message('dMonitoringState') dat.dMonitoringState = { "events": events, "faceDetected": driver_status.face_detected, diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index a5f14a58f..8635b947d 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -5,7 +5,7 @@ from cereal import car, log # Priority class Priority: LOWEST = 0 - LOW_LOWEST = 1 + LOWER = 1 LOW = 2 MID = 3 HIGH = 4 @@ -171,28 +171,28 @@ ALERTS = [ "隨時準備好接管", "請您將手放在方向盤上並持續注意路況", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), Alert( "startupMaster", "WARNING: This branch is not tested", "Always keep hands on wheel and eyes on road", AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), Alert( "startupNoControl", "行車記錄模式", "請您將手放在方向盤上並持續注意路況", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), Alert( "startupNoCar", "行車記錄模式 (尚未支援車種)", "請您將手放在方向盤上並持續注意路況", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), Alert( "ethicalDilemma", @@ -695,21 +695,21 @@ ALERTS = [ "LKAS 錯誤：請重新發動車子", "", AlertStatus.normal, AlertSize.small, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "brakeUnavailablePermanent", "巡航系統錯誤：請重新發動車子", "", AlertStatus.normal, AlertSize.small, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "lowSpeedLockoutPermanent", "巡航系統錯誤：請重新發動車子", "", AlertStatus.normal, AlertSize.small, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "calibrationIncompletePermanent", @@ -723,14 +723,14 @@ ALERTS = [ "未支援的 Giraffe 設置", "請查閱 comma.ai/tg", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "internetConnectivityNeededPermanent", "請連接網路", "需檢查更新後才能啟用", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "communityFeatureDisallowedPermanent", @@ -744,28 +744,28 @@ ALERTS = [ "沒有收到任何來自 EON 傳感器的資料", "請重啟您的裝置", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "soundsUnavailablePermanent", "找不到音效裝置", "請重啟您的裝置", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "lowMemoryPermanent", "記憶體嚴重不足", "請重啟您的裝置", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "carUnrecognizedPermanent", "行車記錄模式", "無法辨識您的車款", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "vehicleModelInvalid", diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 3e53ec3d0..029c30153 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -71,9 +71,9 @@ class LanePlanner(): self.l_prob = md.leftLane.prob # left line prob self.r_prob = md.rightLane.prob # right line prob - if len(md.meta.desirePrediction): - self.l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] - self.r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] + if len(md.meta.desireState): + self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1] + self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1] def update_d_poly(self, v_ego): ts = sec_since_boot() diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index f694af377..35cfde668 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -1,5 +1,6 @@ import os import math +import time import cereal.messaging as messaging from selfdrive.swaglog import cloudlog @@ -7,9 +8,19 @@ from common.realtime import sec_since_boot from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG +# df + dp +from common.numpy_fast import interp, clip +from selfdrive.config import Conversions as CV +from common.params import Params +from selfdrive.dragonpilot.dragonconf import dp_get_last_modified LOG_MPC = os.environ.get('LOG_MPC', False) +# df profile +DF_PROFILE_OFF = -2 +DF_PROFILE_LONG = -1 +DF_PROFILE_NORMAL = 0 +DF_PROFILE_SHORT = 1 class LongitudinalMpc(): def __init__(self, mpc_id): @@ -23,13 +34,24 @@ class LongitudinalMpc(): self.prev_lead_status = False self.prev_lead_x = 0.0 self.new_lead = False - self.last_cloudlog_t = 0.0 + # df from Shane Smiskol + self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} + self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} + self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data + self.last_cost = 0.0 + self.df_profile = DF_PROFILE_OFF + self.sng = False + + # dragonpilot + self.params = Params() + self.last_ts = 0 + self.last_modified = 0 + def send_mpc_solution(self, pm, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) - dat = messaging.new_message() - dat.init('liveLongitudinalMpc') + dat = messaging.new_message('liveLongitudinalMpc') dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego) dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego) dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) @@ -57,8 +79,133 @@ class LongitudinalMpc(): self.cur_state[0].v_ego = v self.cur_state[0].a_ego = a + def get_TR(self, CS): + # load profile + ts = sec_since_boot() + if ts - self.last_ts >= 10.: + modified = dp_get_last_modified() + if self.last_modified != modified: + try: + self.df_profile = int(self.params.get('DragonDynamicFollow', encoding='utf8')) + self.df_profile = self.df_profile if self.df_profile in [DF_PROFILE_OFF, DF_PROFILE_LONG, DF_PROFILE_NORMAL, DF_PROFILE_SHORT] else DF_PROFILE_OFF + except TypeError: + self.df_profile = DF_PROFILE_OFF + self.last_modified = modified + self.last_ts = ts + + if not self.lead_data['status'] or self.df_profile == DF_PROFILE_OFF: + TR = 1.8 + else: + self.store_df_data() + TR = self.dynamic_follow(CS) + + # only change cost when profile is not off + if not self.df_profile == DF_PROFILE_OFF: + self.change_cost(TR) + return TR + + def change_cost(self, TR): + TRs = [0.9, 1.8, 2.7] + costs = [1.0, 0.1, 0.05] + cost = interp(TR, TRs, costs) + if self.last_cost != cost: + self.libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.last_cost = cost + + def store_df_data(self): + v_lead_retention = 1.9 # keep only last x seconds + v_ego_retention = 2.5 + + cur_time = time.time() + if self.lead_data['status']: + self.df_data['v_leads'] = [sample for sample in self.df_data['v_leads'] if + cur_time - sample['time'] <= v_lead_retention + and not self.new_lead] # reset when new lead + self.df_data['v_leads'].append({'v_lead': self.lead_data['v_lead'], 'time': cur_time}) + + self.df_data['v_egos'] = [sample for sample in self.df_data['v_egos'] if cur_time - sample['time'] <= v_ego_retention] + self.df_data['v_egos'].append({'v_ego': self.car_data['v_ego'], 'time': cur_time}) + + def calculate_lead_accel(self): + min_consider_time = 1.0 # minimum amount of time required to consider calculation + a_lead = self.lead_data['a_lead'] + if len(self.df_data['v_leads']): # if not empty + elapsed = self.df_data['v_leads'][-1]['time'] - self.df_data['v_leads'][0]['time'] + if elapsed > min_consider_time: # if greater than min time (not 0) + a_calculated = (self.df_data['v_leads'][-1]['v_lead'] - self.df_data['v_leads'][0]['v_lead']) / elapsed # delta speed / delta time + # old version: # if abs(a_calculated) > abs(a_lead) and a_lead < 0.33528: # if a_lead is greater than calculated accel (over last 1.5s, use that) and if lead accel is not above 0.75 mph/s + # a_lead = a_calculated + + # long version of below: if (a_calculated < 0 and a_lead >= 0 and a_lead < -a_calculated * 0.5) or (a_calculated > 0 and a_lead <= 0 and -a_lead > a_calculated * 0.5) or (a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)): + if (a_calculated < 0 <= a_lead < -a_calculated * 0.55) or (a_calculated > 0 >= a_lead and -a_lead < a_calculated * 0.45) or ( + a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)): # this is a mess, fix + a_lead = a_calculated + return a_lead # if above doesn't execute, we'll return a_lead from radar + + def dynamic_follow(self, CS): + x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities + profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632] # profile mod speeds, mph: [5., 30., 55., 80.] + if self.df_profile == DF_PROFILE_LONG: + y_dist = [1.3847, 1.3946, 1.4078, 1.4243, 1.4507, 1.4837, 1.5327, 1.553, 1.581, 1.617, 1.653, 1.687, 1.74] # TRs + profile_mod_pos = [0.99, 0.9025, 0.815, 0.55] + profile_mod_neg = [1.0, 1.18, 1.382, 1.787] + elif self.df_profile == DF_PROFILE_SHORT: # for in congested traffic + x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] + y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] + profile_mod_pos = [1.05, 1.375, 2.99, 3.8] + profile_mod_neg = [0.79, .1, 0.0, 0.0] + else: # default to relaxed/stock + y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] + profile_mod_pos = [1.0] * 4 + profile_mod_neg = [1.0] * 4 + + sng_TR = 1.7 # reacceleration stop and go TR + sng_speed = 15.0 * CV.MPH_TO_MS + + if self.car_data['v_ego'] > sng_speed: # keep sng distance until we're above sng speed again + self.sng = False + + if (self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][0]['v_ego'] >= self.car_data[ + 'v_ego']) and not self.sng: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease + TR = interp(self.car_data['v_ego'], x_vel, y_dist) + else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating + self.sng = True + x = [sng_speed / 3.0, sng_speed] # decrease TR between 5 and 15 mph from 1.8s to defined TR above at 15mph while accelerating + y = [sng_TR, interp(sng_speed, x_vel, y_dist)] + TR = interp(self.car_data['v_ego'], x, y) + + TR_mod = [] + # Dynamic follow modifications (the secret sauce) + x = [-20.0383, -15.6978, -11.2053, -7.8781, -5.0407, -3.2167, -1.6122, 0.0, 0.6847, 1.3772, 1.9007, 2.7452] # relative velocity values + y = [0.641, 0.506, 0.418, 0.334, 0.24, 0.115, 0.065, 0.0, -0.049, -0.068, -0.142, -0.221] # modification values + TR_mod.append(interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y)) + + x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values + y = [0.265, 0.187, 0.096, 0.057, 0.033, 0.024, 0.0, -0.009, -0.042, -0.053, -0.059] # modification values + TR_mod.append(interp(self.calculate_lead_accel(), x, y)) + + profile_mod_pos = interp(self.car_data['v_ego'], profile_mod_x, profile_mod_pos) + profile_mod_neg = interp(self.car_data['v_ego'], profile_mod_x, profile_mod_neg) + + TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mod]) # alter TR modification according to profile + TR += TR_mod + + if CS.leftBlinker or CS.rightBlinker and self.df_profile != DF_PROFILE_SHORT: + x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph + y = [1.0, .75, .65] # reduce TR when changing lanes + TR *= interp(self.car_data['v_ego'], x, y) + + return clip(TR, 0.9, 2.7) + + def process_lead(self, v_lead, a_lead, x_lead, status): + self.lead_data['v_lead'] = v_lead + self.lead_data['a_lead'] = a_lead + self.lead_data['x_lead'] = x_lead + self.lead_data['status'] = status + def update(self, pm, CS, lead, v_cruise_setpoint): v_ego = CS.vEgo + self.car_data = {'v_ego': CS.vEgo, 'a_ego': CS.aEgo} # Setup current mpc state self.cur_state[0].x_ego = 0.0 @@ -71,6 +218,7 @@ class LongitudinalMpc(): if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): v_lead = 0.0 a_lead = 0.0 + self.process_lead(v_lead, a_lead, x_lead, lead.status) self.a_lead_tau = lead.aLeadTau self.new_lead = False @@ -83,6 +231,7 @@ class LongitudinalMpc(): self.cur_state[0].x_l = x_lead self.cur_state[0].v_l = v_lead else: + self.process_lead(None, None, None, False) self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 @@ -92,7 +241,7 @@ class LongitudinalMpc(): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS)) duration = int((sec_since_boot() - t) * 1e9) if LOG_MPC: diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c index 8cfc06f3b..2adeb4397 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c @@ -68,7 +68,7 @@ acadoWorkspace.evGu[lRun1 * 3 + 2] = acadoWorkspace.state[14]; return ret; } -void acado_evaluateLSQ(const real_t* in, real_t* out) +void acado_evaluateLSQ(const real_t* in, real_t* out, double TR) { const real_t* xd = in; const real_t* u = in + 3; @@ -78,29 +78,29 @@ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); a[8] = (a[2]*a[2]); -a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[12] = (a[10]*a[10]); /* Compute outputs: */ out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); -out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[4] = a[4]; out[5] = a[9]; out[6] = (real_t)(0.0000000000000000e+00); out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); -out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (real_t)(0.0000000000000000e+00); out[11] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -114,7 +114,7 @@ out[18] = (real_t)(0.0000000000000000e+00); out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)); } -void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) +void acado_evaluateLSQEndTerm(const real_t* in, real_t* out, double TR) { const real_t* xd = in; const real_t* od = in + 3; @@ -123,28 +123,28 @@ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); a[8] = (a[2]*a[2]); -a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[12] = (a[10]*a[10]); /* Compute outputs: */ out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); -out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = a[4]; out[4] = a[9]; out[5] = (real_t)(0.0000000000000000e+00); out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); -out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); out[8] = (real_t)(0.0000000000000000e+00); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -207,7 +207,7 @@ tmpQN1[7] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[4] + tmpQN2[8]*tmpFx[7]; tmpQN1[8] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[5] + tmpQN2[8]*tmpFx[8]; } -void acado_evaluateObjective( ) +void acado_evaluateObjective( double TR ) { int runObj; for (runObj = 0; runObj < 20; ++runObj) @@ -219,7 +219,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[runObj]; acadoWorkspace.objValueIn[4] = acadoVariables.od[runObj * 2]; acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2 + 1]; -acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0]; acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1]; acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2]; @@ -235,7 +235,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; -acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; @@ -412,7 +412,7 @@ int lRun3; int lRun4; int lRun5; /** Row vector of size: 20 */ -static const int xBoundIndices[ 20 ] = +static const int xBoundIndices[ 20 ] = { 4, 7, 10, 13, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, 52, 55, 58, 61 }; acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); acado_moveGxT( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.T ); @@ -4589,12 +4589,12 @@ acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVar acado_multEDu( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 60 ]) ); } -int acado_preparationStep( ) +int acado_preparationStep( double TR ) { int ret; ret = acado_modelSimulation(); -acado_evaluateObjective( ); +acado_evaluateObjective( TR ); acado_condensePrep( ); return ret; } @@ -4660,7 +4660,7 @@ acadoVariables.x[60] = xEnd[0]; acadoVariables.x[61] = xEnd[1]; acadoVariables.x[62] = xEnd[2]; } -else if (strategy == 2) +else if (strategy == 2) { acadoWorkspace.state[0] = acadoVariables.x[60]; acadoWorkspace.state[1] = acadoVariables.x[61]; @@ -4726,7 +4726,7 @@ kkt += fabs(acadoWorkspace.ubA[index] * prd); return kkt; } -real_t acado_getObjective( ) +real_t acado_getObjective( TR ) { real_t objVal; @@ -4746,7 +4746,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[lRun1]; acadoWorkspace.objValueIn[4] = acadoVariables.od[lRun1 * 2]; acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2 + 1]; -acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4]; acadoWorkspace.Dy[lRun1 * 4 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 4 + 1]; acadoWorkspace.Dy[lRun1 * 4 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 4 + 2]; @@ -4757,7 +4757,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; -acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2]; @@ -4779,4 +4779,3 @@ objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + objVal *= 0.5; return objVal; } - diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index c40b4e071..fd8947ff0 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -29,8 +29,9 @@ def _get_libmpc(mpc_id): void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); + void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); int run_mpc(state_t * x0, log_t * solution, - double l, double a_l_0); + double l, double a_l_0, double TR); """) return (ffi, ffi.dlopen(libmpc_fn)) diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c index d4bfee8c8..f0c6cca85 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c @@ -68,6 +68,25 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j } +void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){ + int i; + const int STEP_MULTIPLIER = 3; + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = STEP_MULTIPLIER; + } + acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc) + acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance + acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration + acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk + } + acadoVariables.WN[0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone + acadoVariables.WN[4] = distanceCost * STEP_MULTIPLIER; // desired distance + acadoVariables.WN[8] = accelerationCost * STEP_MULTIPLIER; // acceleration +} + void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){ int i; @@ -112,7 +131,7 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0 for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; } -int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ +int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0, double TR){ // Calculate lead vehicle predictions int i; double t = 0.; @@ -152,7 +171,7 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego; acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego; - acado_preparationStep(); + acado_preparationStep(TR); acado_feedbackStep(); for (i = 0; i <= N; i++){ @@ -164,10 +183,10 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ solution->j_ego[i] = acadoVariables.u[i]; } } - solution->cost = acado_getObjective(); + solution->cost = acado_getObjective(TR); // Dont shift states here. Current solution is closer to next timestep than if // we shift by 0.2 seconds. return acado_getNWSR(); -} +} \ No newline at end of file diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 9e784c4de..03bb2b8a0 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -25,19 +25,16 @@ DESIRES = { LaneChangeDirection.none: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none, }, LaneChangeDirection.left: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft, }, LaneChangeDirection.right: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight, }, } @@ -65,7 +62,6 @@ class PathPlanner(): # dragonpilot self.params = Params() - self.dragon_assisted_lc_enabled = False self.dragon_auto_lc_enabled = False self.dragon_auto_lc_allowed = False self.dragon_auto_lc_timer = None @@ -97,8 +93,8 @@ class PathPlanner(): if cur_time - self.last_ts >= 5.: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_assisted_lc_enabled = self.lane_change_enabled - if self.dragon_assisted_lc_enabled: + self.lane_change_enabled = True if self.params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if self.lane_change_enabled: self.dragon_auto_lc_enabled = True if self.params.get("DragonEnableAutoLC", encoding='utf8') == "1" else False # adjustable assisted lc min speed self.dragon_assisted_lc_min_mph = int(self.params.get("DragonAssistedLCMinMPH", encoding='utf8')) * CV.MPH_TO_MS @@ -116,6 +112,8 @@ class PathPlanner(): self.dragon_auto_lc_delay = int(self.params.get("DragonAutoLCDelay", encoding='utf8')) if self.dragon_auto_lc_delay < 0: self.dragon_auto_lc_delay = 0 + else: + self.dragon_auto_lc_enabled = False self.dp_last_modified = modified self.last_ts = cur_time @@ -182,14 +180,10 @@ class PathPlanner(): if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied: - self.lane_change_state = LaneChangeState.laneChangeStarting - - # starting - elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5: - self.lane_change_state = LaneChangeState.laneChangeFinishing + self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing - elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2: + elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.5: if one_blinker: self.lane_change_state = LaneChangeState.preLaneChange else: @@ -254,8 +248,7 @@ class PathPlanner(): self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 - plan_send = messaging.new_message() - plan_send.init('pathPlan') + plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] @@ -280,8 +273,7 @@ class PathPlanner(): pm.send('pathPlan', plan_send) if LOG_MPC: - dat = messaging.new_message() - dat.init('liveMpc') + dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 8fd37832c..841349367 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -253,8 +253,7 @@ class Planner(): radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** - plan_send = messaging.new_message() - plan_send.init('plan') + plan_send = messaging.new_message('plan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index a99a0bdfb..0b8523313 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -163,8 +163,7 @@ class RadarD(): self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) # *** publish radarState *** - dat = messaging.new_message() - dat.init('radarState') + dat = messaging.new_message('radarState') dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model']) dat.radarState.mdMonoTime = self.last_md_ts dat.radarState.canMonoTimes = list(rr.canMonoTimes) @@ -223,8 +222,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks - dat = messaging.new_message() - dat.init('liveTracks', len(tracks)) + dat = messaging.new_message('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 63545e6ee..11995c6c7 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -46,8 +46,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0): dt) # Setup CarState - CS = messaging.new_message() - CS.init('carState') + CS = messaging.new_message('carState') CS.carState.vEgo = v_ego CS.carState.aEgo = a_ego diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 2905ac061..1baf32a60 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -8,8 +8,9 @@ from selfdrive.version import version, dirty, origin, branch uniqueID = Params().get('DongleId', None) from selfdrive.swaglog import cloudlog +from common.android import ANDROID -if os.getenv("NOLOG") or os.getenv("NOCRASH"): +if os.getenv("NOLOG") or os.getenv("NOCRASH") or not ANDROID: def capture_exception(*exc_info): pass def bind_user(**kwargs): diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index 4f324711d..6e1029620 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -14,9 +14,24 @@ def cputime_busy(ct): return ct.user + ct.nice + ct.system + ct.irq + ct.softirq +def proc_cputime_total(ct): + return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem + + +def proc_name(proc): + name = proc.name + if len(proc.cmdline): + name = proc.cmdline[0] + if len(proc.exe): + name = proc.exe + " - " + name + + return name + + if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument('--mem', action='store_true') + parser.add_argument('--cpu', action='store_true') args = parser.parse_args() sm = SubMaster(['thermal', 'procLog']) @@ -26,6 +41,9 @@ if __name__ == "__main__": total_times = [0., 0., 0., 0.] busy_times = [0., 0., 0.0, 0.] + prev_proclog = None + prev_proclog_t = None + while True: sm.update() @@ -37,7 +55,6 @@ if __name__ == "__main__": if sm.updated['procLog']: m = sm['procLog'] - cores = [0., 0., 0., 0.] total_times_new = [0., 0., 0., 0.] busy_times_new = [0., 0., 0.0, 0.] @@ -57,16 +74,33 @@ if __name__ == "__main__": print("CPU %.2f%% - RAM: %.2f - Temp %.2f" % (100. * np.mean(cores), last_mem, last_temp)) + if args.cpu and prev_proclog is not None: + procs = {} + dt = (sm.logMonoTime['procLog'] - prev_proclog_t) / 1e9 + for proc in m.procs: + try: + name = proc_name(proc) + prev_proc = [p for p in prev_proclog.procs if proc.pid == p.pid][0] + cpu_time = proc_cputime_total(proc) - proc_cputime_total(prev_proc) + cpu_usage = cpu_time / dt * 100. + procs[name] = cpu_usage + except IndexError: + pass + + print("Top CPU usage:") + for k, v in sorted(procs.items(), key=lambda item: item[1], reverse=True)[:10]: + print(f"{k.rjust(70)} {v:.2f} %") + print() + if args.mem: mems = {} for proc in m.procs: - name = proc.name - if len(proc.cmdline): - name = proc.cmdline[0] - if len(proc.exe): - name = proc.exe + " - " + name + name = proc_name(proc) mems[name] = float(proc.memRss) / 1e6 print("Top memory usage:") for k, v in sorted(mems.items(), key=lambda item: item[1], reverse=True)[:10]: print(f"{k.rjust(70)} {v:.2f} MB") print() + + prev_proclog = m + prev_proclog_t = sm.logMonoTime['procLog'] diff --git a/selfdrive/debug/show_matching_cars.py b/selfdrive/debug/show_matching_cars.py index df2b70363..273ae1469 100755 --- a/selfdrive/debug/show_matching_cars.py +++ b/selfdrive/debug/show_matching_cars.py @@ -16,8 +16,7 @@ candidate_cars = all_known_cars() for addr, l in fingerprint.items(): - dat = messaging.new_message() - dat.init('can', 1) + dat = messaging.new_message('can', 1) msg = dat.can[0] msg.address = addr diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index 5bf8c4a76..2e668046e 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 +import argparse import os import traceback -import sys from tqdm import tqdm from tools.lib.logreader import LogReader from selfdrive.car.fw_versions import match_fw_to_car @@ -13,14 +13,15 @@ from selfdrive.car.honda.values import FINGERPRINTS as HONDA_FINGERPRINTS if __name__ == "__main__": - if len(sys.argv) < 2: - print("Usage: ./test_fw_query_on_routes.py /") - sys.exit(1) + parser = argparse.ArgumentParser(description='Run FW fingerprint on Qlog of route or list of routes') + parser.add_argument('route', help='Route or file with list of routes') + parser.add_argument('--car', help='Force comparison fingerprint to known car') + args = parser.parse_args() - if os.path.exists(sys.argv[1]): - routes = list(open(sys.argv[1])) + if os.path.exists(args.route): + routes = list(open(args.route)) else: - routes = [sys.argv[1]] + routes = [args.route] wrong = 0 good = 0 @@ -51,6 +52,9 @@ if __name__ == "__main__": dongles.append(dongle_id) live_fingerprint = msg.carParams.carFingerprint + if args.car is not None: + live_fingerprint = args.car + if live_fingerprint not in list(TOYOTA_FINGERPRINTS.keys()) + list(HONDA_FINGERPRINTS.keys()): continue diff --git a/selfdrive/dragonpilot/appd/appd.py b/selfdrive/dragonpilot/appd/appd.py index 86c97e282..3e9900c95 100644 --- a/selfdrive/dragonpilot/appd/appd.py +++ b/selfdrive/dragonpilot/appd/appd.py @@ -20,14 +20,6 @@ class App(): TYPE_FULLSCREEN = 3 TYPE_UTIL = 4 - # frame app - FRAME = "ai.comma.plus.frame" - FRAME_MAIN = ".MainActivity" - - # offroad app - OFFROAD = "ai.comma.plus.offroad" - OFFROAD_MAIN = ".MainActivity" - # manual switch stats MANUAL_OFF = "-1" MANUAL_IDLE = "0" @@ -58,7 +50,6 @@ class App(): # read manual run param self.manual_ctrl_param = manual_ctrl_param # if it's a service app, we do not kill if device is too hot - # if it's a full screen app, we need to do extra process on frame/offroad self.app_type = app_type # app permissions self.permissions = permissions @@ -117,11 +108,6 @@ class App(): # only run app if it's not running if force or not self.is_running: - # if it's a full screen app, we need to stop frame and offroad to get keyboard access - if self.app_type == self.TYPE_FULLSCREEN: - self.system("pm disable %s" % self.FRAME) - self.system("am start -n %s/%s" % (self.OFFROAD, self.OFFROAD_MAIN)) - self.system("pm enable %s" % self.app) if self.app_type == self.TYPE_GPS_SERVICE: @@ -143,13 +129,6 @@ class App(): # only kill app if it's running if force or self.is_running: - # if it's a full screen app, we need to restart offroad and frame - if self.app_type == self.TYPE_FULLSCREEN: - self.system("pm disable %s" % self.OFFROAD) - self.system("pm enable %s" % self.OFFROAD) - self.system("pm enable %s" % self.FRAME) - self.system("am start -n %s/%s" % (self.FRAME, self.FRAME_MAIN)) - if self.app_type == self.TYPE_GPS_SERVICE: self.appops_set(self.app, "android:mock_location", "deny") diff --git a/selfdrive/dragonpilot/dashcamd/dashcamd.py b/selfdrive/dragonpilot/dashcamd/dashcamd.py index 78aa49af1..7694a665b 100644 --- a/selfdrive/dragonpilot/dashcamd/dashcamd.py +++ b/selfdrive/dragonpilot/dashcamd/dashcamd.py @@ -56,10 +56,12 @@ def main(gctx=None): break except os.error as e: pass - time_diff = int(time.time()-start_time) + time_diff = time.time()-start_time # we start the process 1 second before screenrecord ended # to make sure there are no missing footage - time.sleep(duration-1-time_diff) + sleep_time = duration-1-time_diff + if sleep_time >= 0.: + time.sleep(sleep_time) else: time.sleep(5) diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 9437308ba..9bd27b33d 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -69,7 +69,9 @@ default_conf = { 'DragonBTG': 0, 'DragonBootHotspot': 0, 'DragonAccelProfile': '0', - 'DragonLastModified': str(floor(time.time())) + 'DragonLastModified': str(floor(time.time())), + 'DragonEnableRegistration': '1', + 'DragonDynamicFollow': '-2', # OFF = -2, LONG = -1, NORMAL = 0, SHORT = 1 } deprecated_conf = { diff --git a/selfdrive/dragonpilot/shutdownd/shutdownd.py b/selfdrive/dragonpilot/shutdownd/shutdownd.py index 702bda718..ac92ec713 100644 --- a/selfdrive/dragonpilot/shutdownd/shutdownd.py +++ b/selfdrive/dragonpilot/shutdownd/shutdownd.py @@ -9,7 +9,6 @@ from selfdrive.dragonpilot.dragonconf import dp_get_last_modified def main(): thermal_sock = messaging.sub_sock('thermal') - last_ts = 0 secs = 0 last_secs = None shutdown_at = 0 @@ -20,29 +19,28 @@ def main(): dp_last_modified = None while 1: cur_time = sec_since_boot() - if cur_time - last_ts >= 10.: - modified = dp_get_last_modified() - if dp_last_modified != modified: - enabled = True if params.get("DragonEnableAutoShutdown", encoding='utf8') == '1' else False - if enabled: - secs = int(params.get("DragonAutoShutdownAt", encoding='utf8')) * 60 - - dp_last_modified = modified + modified = dp_get_last_modified() + if dp_last_modified != modified: + enabled = True if params.get("DragonEnableAutoShutdown", encoding='utf8') == '1' else False if enabled: - msg = messaging.recv_sock(thermal_sock, wait=True) - started = msg.thermal.started - usb_online = msg.thermal.usbOnline + secs = int(params.get("DragonAutoShutdownAt", encoding='utf8')) * 60 + dp_last_modified = modified - if last_enabled != enabled or last_secs != secs or started or usb_online: - shutdown_at = cur_time + secs + if last_enabled != enabled or last_secs != secs or started or usb_online: + shutdown_at = cur_time + secs - last_enabled = enabled - last_secs = secs - last_ts = cur_time + if enabled: + msg = messaging.recv_sock(thermal_sock, wait=True) + started = msg.thermal.started + usb_online = msg.thermal.usbOnline + + if not started and not usb_online and secs > 0 and cur_time >= shutdown_at: + os.system('LD_LIBRARY_PATH="" svc power shutdown') + + last_enabled = enabled + last_secs = secs - if enabled and not started and not usb_online and secs > 0 and cur_time >= shutdown_at: - os.system('LD_LIBRARY_PATH="" svc power shutdown') time.sleep(10) if __name__ == "__main__": diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index b2b5c18a8..bc0f0a039 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -14,13 +14,13 @@ from common.transformations.camera import view_frame_from_device_frame, get_view MPH_TO_MS = 0.44704 MIN_SPEED_FILTER = 15 * MPH_TO_MS -MAX_SPEED_STD = 1.5 +MAX_VEL_ANGLE_STD = np.radians(0.25) MAX_YAW_RATE_FILTER = np.radians(2) # per second # This is all 20Hz, blocks needed for efficiency BLOCK_SIZE = 100 INPUTS_NEEDED = 5 # allow to update VP every so many frames -INPUTS_WANTED = 20 # We want a little bit more than we need for stability +INPUTS_WANTED = 50 # We want a little bit more than we need for stability WRITE_CYCLES = 10 # write every 1000 cycles VP_INIT = np.array([W/2., H/2.]) @@ -89,9 +89,10 @@ class Calibrator(): self.just_calibrated = True def handle_cam_odom(self, trans, rot, trans_std, rot_std): - if ((trans[0] > MIN_SPEED_FILTER) and - (trans_std[0] < MAX_SPEED_STD) and - (abs(rot[2]) < MAX_YAW_RATE_FILTER)): + straight_and_fast = ((trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) + certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or + (self.valid_blocks < INPUTS_NEEDED)) + if straight_and_fast and certain_if_calib: # intrinsics are not eon intrinsics, since this is calibrated frame intrinsics = intrinsics_from_vp(self.vp) new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans)) @@ -104,7 +105,8 @@ class Calibrator(): self.block_idx += 1 self.valid_blocks = max(self.block_idx, self.valid_blocks) self.block_idx = self.block_idx % INPUTS_WANTED - self.vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0) + if self.valid_blocks > 0: + self.vp = np.mean(self.vps[:self.valid_blocks], axis=0) self.update_status() if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated): @@ -119,8 +121,7 @@ class Calibrator(): calib = get_calib_from_vp(self.vp) extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height) - cal_send = messaging.new_message() - cal_send.init('liveCalibration') + cal_send = messaging.new_message('liveCalibration') cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] diff --git a/selfdrive/locationd/kalman/SConscript b/selfdrive/locationd/kalman/SConscript index 2a67ad64f..dcacb97c3 100644 --- a/selfdrive/locationd/kalman/SConscript +++ b/selfdrive/locationd/kalman/SConscript @@ -1,19 +1,25 @@ -Import('env') +Import('env', 'arch') templates = Glob('templates/*') sympy_helpers = "helpers/sympy_helpers.py" ekf_sym = "helpers/ekf_sym.py" to_build = { - 'pos_computer_4': 'helpers/lst_sq_computer.py', - 'pos_computer_5': 'helpers/lst_sq_computer.py', - 'feature_handler_5': 'helpers/feature_handler.py', - 'gnss': 'models/gnss_kf.py', - 'loc_4': 'models/loc_kf.py', + 'car': 'models/car_kf.py', 'live': 'models/live_kf.py', - 'lane': '#xx/pipeline/lib/ekf/lane_kf.py', } + +if arch != "aarch64": + to_build.update({ + 'lane': '#xx/pipeline/lib/ekf/lane_kf.py', + 'pos_computer_4': 'helpers/lst_sq_computer.py', + 'pos_computer_5': 'helpers/lst_sq_computer.py', + 'feature_handler_5': 'helpers/feature_handler.py', + 'loc_4': 'models/loc_kf.py', + 'gnss': 'models/gnss_kf.py', + }) + found = {} for target, command in to_build.items(): diff --git a/selfdrive/locationd/kalman/helpers/__init__.py b/selfdrive/locationd/kalman/helpers/__init__.py index 5ac7fb3f2..2ec5ba0dd 100644 --- a/selfdrive/locationd/kalman/helpers/__init__.py +++ b/selfdrive/locationd/kalman/helpers/__init__.py @@ -56,28 +56,44 @@ class ObservationKind(): PSEUDORANGE = 22 PSEUDORANGE_RATE = 23 - names = ['Unknown', - 'No observation', - 'GPS NED', - 'Odometric speed', - 'Phone gyro', - 'GPS velocity', - 'GPS pseudorange', - 'GPS pseudorange rate', - 'Speed', - 'No rotation', - 'Phone acceleration', - 'ORB point', - 'ECEF pos', - 'camera odometric translation', - 'camera odometric rotation', - 'ORB features', - 'MSCKF test', - 'Feature track test', - 'Lane ecef point', - 'imu frame eulers', - 'GLONASS pseudorange', - 'GLONASS pseudorange rate'] + ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s] + ROAD_FRAME_YAW_RATE = 25 # [rad/s] + STEER_ANGLE = 26 # [rad] + ANGLE_OFFSET_FAST = 27 # [rad] + STIFFNESS = 28 # [-] + STEER_RATIO = 29 # [-] + + names = [ + 'Unknown', + 'No observation', + 'GPS NED', + 'Odometric speed', + 'Phone gyro', + 'GPS velocity', + 'GPS pseudorange', + 'GPS pseudorange rate', + 'Speed', + 'No rotation', + 'Phone acceleration', + 'ORB point', + 'ECEF pos', + 'camera odometric translation', + 'camera odometric rotation', + 'ORB features', + 'MSCKF test', + 'Feature track test', + 'Lane ecef point', + 'imu frame eulers', + 'GLONASS pseudorange', + 'GLONASS pseudorange rate', + + 'Road Frame x,y speed', + 'Road Frame yaw rate', + 'Steer Angle', + 'Fast Angle Offset', + 'Stiffness', + 'Steer Ratio', + ] @classmethod def to_string(cls, kind): diff --git a/selfdrive/locationd/kalman/helpers/ekf_sym.py b/selfdrive/locationd/kalman/helpers/ekf_sym.py index 98d4a87b1..c9eb093a4 100644 --- a/selfdrive/locationd/kalman/helpers/ekf_sym.py +++ b/selfdrive/locationd/kalman/helpers/ekf_sym.py @@ -27,7 +27,7 @@ def null(H, eps=1e-12): return np.transpose(null_space) -def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]): +def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[], global_vars=None): # optional state transition matrix, H modifier # and err_function if an error-state kalman filter (ESKF) # is desired. Best described in "Quaternion kinematics @@ -74,8 +74,13 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No # linearize with jacobians F_sym = f_err_sym.jacobian(x_err_sym) - for sym in x_err_sym: - F_sym = F_sym.subs(sym, 0) + + if eskf_params: + for sym in x_err_sym: + F_sym = F_sym.subs(sym, 0) + + assert dt_sym in F_sym.free_symbols + for i in range(len(obs_eqs)): obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym)) if msckf and obs_eqs[i][1] in feature_track_kinds: @@ -105,7 +110,7 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym])) # Generate and wrap all th c code - header, code = sympy_into_c(sympy_functions) + header, code = sympy_into_c(sympy_functions, global_vars) extra_header = "#define DIM %d\n" % dim_x extra_header += "#define EDIM %d\n" % dim_err extra_header += "#define MEDIM %d\n" % dim_main_err @@ -135,6 +140,17 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No code += '\nextern "C"{\n' + extra_header + "\n}\n" code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read() code += '\nextern "C"{\n' + extra_post + "\n}\n" + + if global_vars is not None: + global_code = '\nextern "C"{\n' + for var in global_vars: + global_code += f"\ndouble {var.name};\n" + global_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n" + extra_header += f"\nvoid set_{var.name}(double x);\n" + + global_code += '\n}\n' + code = global_code + code + header += "\n" + extra_header write_code(name, code, header) @@ -142,7 +158,7 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No class EKF_sym(): def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err, - N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]): + N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], global_vars=None): """Generates process function and all observation functions for the kalman filter.""" self.msckf = N > 0 self.N = N @@ -163,6 +179,8 @@ class EKF_sym(): # tested for outlier rejection self.maha_test_kinds = maha_test_kinds + self.global_vars = global_vars + # process noise self.Q = Q @@ -221,6 +239,11 @@ class EKF_sym(): if self.msckf and kind in self.feature_track_kinds: self.Hes[kind] = wrap_2lists("He_%d" % kind) + if self.global_vars is not None: + for var in self.global_vars: + fun_name = f"set_{var.name}" + setattr(self, fun_name, getattr(lib, fun_name)) + # wrap the C++ predict function def _predict_blas(x, P, dt): lib.predict(ffi.cast("double *", x.ctypes.data), @@ -336,6 +359,17 @@ class EKF_sym(): self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:] self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:] + def predict(self, t): + # initialize time + if self.filter_time is None: + self.filter_time = t + + # predict + dt = t - self.filter_time + assert dt >= 0 + self.x, self.P = self._predict(self.x, self.P, dt) + self.filter_time = t + def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False): # TODO handle rewinding at this level" diff --git a/selfdrive/locationd/kalman/helpers/sympy_helpers.py b/selfdrive/locationd/kalman/helpers/sympy_helpers.py index f8c4f1c5f..dee9fb373 100644 --- a/selfdrive/locationd/kalman/helpers/sympy_helpers.py +++ b/selfdrive/locationd/kalman/helpers/sympy_helpers.py @@ -46,11 +46,11 @@ def quat_matrix_r(p): [p[3], p[2], -p[1], p[0]]]) -def sympy_into_c(sympy_functions): +def sympy_into_c(sympy_functions, global_vars=None): from sympy.utilities import codegen routines = [] for name, expr, args in sympy_functions: - r = codegen.make_routine(name, expr, language="C99") + r = codegen.make_routine(name, expr, language="C99", global_vars=global_vars) # argument ordering input to sympy is broken with function with output arguments nargs = [] diff --git a/selfdrive/locationd/kalman/models/car_kf.py b/selfdrive/locationd/kalman/models/car_kf.py new file mode 100755 index 000000000..aa729fc10 --- /dev/null +++ b/selfdrive/locationd/kalman/models/car_kf.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python3 + +import math +import numpy as np +import sympy as sp + +from selfdrive.locationd.kalman.helpers import ObservationKind +from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code + +i = 0 + + +def _slice(n): + global i + s = slice(i, i + n) + i += n + + return s + + +class States(): + # Vehicle model params + STIFFNESS = _slice(1) # [-] + STEER_RATIO = _slice(1) # [-] + ANGLE_OFFSET = _slice(1) # [rad] + ANGLE_OFFSET_FAST = _slice(1) # [rad] + + VELOCITY = _slice(2) # (x, y) [m/s] + YAW_RATE = _slice(1) # [rad/s] + STEER_ANGLE = _slice(1) # [rad] + + +class CarKalman(): + name = 'car' + + x_initial = np.array([ + 1.0, + 15.0, + 0.0, + 0.0, + + 10.0, 0.0, + 0.0, + 0.0, + ]) + + # state covariance + P_initial = np.diag([ + .1**2, + .1**2, + math.radians(0.1)**2, + math.radians(0.1)**2, + + 10**2, 10**2, + 1**2, + 1**2, + ]) + + # process noise + Q = np.diag([ + (.05/10)**2, + .0001**2, + math.radians(0.01)**2, + math.radians(0.2)**2, + + .1**2, .1**2, + math.radians(0.1)**2, + math.radians(0.1)**2, + ]) + + obs_noise = { + ObservationKind.ROAD_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]), + ObservationKind.ROAD_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2), + ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**2), + ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2), + ObservationKind.STEER_RATIO: np.atleast_2d(50.0**2), + ObservationKind.STIFFNESS: np.atleast_2d(50.0**2), + } + + maha_test_kinds = [] # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED] + global_vars = [ + sp.Symbol('mass'), + sp.Symbol('rotational_inertia'), + sp.Symbol('center_to_front'), + sp.Symbol('center_to_rear'), + sp.Symbol('stiffness_front'), + sp.Symbol('stiffness_rear'), + ] + + @staticmethod + def generate_code(): + dim_state = CarKalman.x_initial.shape[0] + name = CarKalman.name + maha_test_kinds = CarKalman.maha_test_kinds + + # globals + m, j, aF, aR, cF_orig, cR_orig = CarKalman.global_vars + + # make functions and jacobians with sympy + # state variables + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + + # Vehicle model constants + x = state[States.STIFFNESS, :][0, 0] + + cF, cR = x * cF_orig, x * cR_orig + angle_offset = state[States.ANGLE_OFFSET, :][0, 0] + angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0] + sa = state[States.STEER_ANGLE, :][0, 0] + + sR = state[States.STEER_RATIO, :][0, 0] + u, v = state[States.VELOCITY, :] + r = state[States.YAW_RATE, :][0, 0] + + A = sp.Matrix(np.zeros((2, 2))) + A[0, 0] = -(cF + cR) / (m * u) + A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u + A[1, 0] = -(cF * aF - cR * aR) / (j * u) + A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u) + + B = sp.Matrix(np.zeros((2, 1))) + B[0, 0] = cF / m / sR + B[1, 0] = (cF * aF) / j / sR + + x = sp.Matrix([v, r]) # lateral velocity, yaw rate + x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) + + dt = sp.Symbol('dt') + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.VELOCITY.start + 1, 0] = x_dot[0] + state_dot[States.YAW_RATE.start, 0] = x_dot[1] + + # Basic descretization, 1st order integrator + # Can be pretty bad if dt is big + f_sym = state + dt * state_dot + + # + # Observation functions + # + obs_eqs = [ + [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None], + [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None], + [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None], + [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None], + [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None], + [sp.Matrix([x]), ObservationKind.STIFFNESS, None], + ] + + gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds, global_vars=CarKalman.global_vars) + + def __init__(self): + self.dim_state = self.x_initial.shape[0] + + # init filter + self.filter = EKF_sym(self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, self.dim_state, maha_test_kinds=self.maha_test_kinds, global_vars=self.global_vars) + + @property + def x(self): + return self.filter.state() + + @property + def P(self): + return self.filter.covs() + + def predict(self, t): + return self.filter.predict(t) + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=False) + + def get_R(self, kind, n): + obs_noise = self.obs_noise[kind] + dim = obs_noise.shape[0] + R = np.zeros((n, dim, dim)) + for i in range(n): + R[i, :, :] = obs_noise + return R + + def init_state(self, state, covs_diag=None, covs=None, filter_time=None): + if covs_diag is not None: + P = np.diag(covs_diag) + elif covs is not None: + P = covs + else: + P = self.filter.covs() + self.filter.init_state(state, P, filter_time) + + def predict_and_observe(self, t, kind, data): + if len(data) > 0: + data = np.atleast_2d(data) + self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) + + +if __name__ == "__main__": + CarKalman.generate_code() diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py old mode 100755 new mode 100644 index 3cf9560d6..001f6d2e9 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -6,56 +6,113 @@ import numpy as np import cereal.messaging as messaging import common.transformations.coordinates as coord from common.transformations.orientation import (ecef_euler_from_ned, - euler2quat, + euler_from_quat, ned_euler_from_ecef, - quat2euler, - rotations_from_quats) + quat_from_euler, + rot_from_quat, rot_from_euler) from selfdrive.locationd.kalman.helpers import ObservationKind, KalmanError from selfdrive.locationd.kalman.models.live_kf import LiveKalman, States from selfdrive.swaglog import cloudlog +#from datetime import datetime +#from laika.gps_time import GPSTime + VISION_DECIMATION = 2 SENSOR_DECIMATION = 10 +def to_float(arr): + return [float(arr[0]), float(arr[1]), float(arr[2])] + + class Localizer(): def __init__(self, disabled_logs=[], dog=None): self.kf = LiveKalman() self.reset_kalman() self.max_age = .2 # seconds self.disabled_logs = disabled_logs + self.calib = np.zeros(3) + self.device_from_calib = np.eye(3) + self.calib_from_device = np.eye(3) + self.calibrated = 0 def liveLocationMsg(self, time): - fix = messaging.log.LiveLocationData.new_message() - predicted_state = self.kf.x + predicted_std = np.diagonal(self.kf.P) fix_ecef = predicted_state[States.ECEF_POS] + fix_ecef_std = predicted_std[States.ECEF_POS_ERR] + vel_ecef = predicted_state[States.ECEF_VELOCITY] + vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR] fix_pos_geo = coord.ecef2geodetic(fix_ecef) - fix.lat = float(fix_pos_geo[0]) - fix.lon = float(fix_pos_geo[1]) - fix.alt = float(fix_pos_geo[2]) + fix_pos_geo_std = coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo + ned_vel = self.converter.ecef2ned(fix_ecef + vel_ecef) - self.converter.ecef2ned(fix_ecef) + ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef) + device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T + vel_device = device_from_ecef.dot(vel_ecef) + vel_device_std = device_from_ecef.dot(vel_ecef_std) + orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]) + orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR] + orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef) + orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned + vel_calib = self.calib_from_device.dot(vel_device) + vel_calib_std = self.calib_from_device.dot(vel_device_std) + acc_calib = self.calib_from_device.dot(predicted_state[States.ACCELERATION]) + acc_calib_std = self.calib_from_device.dot(predicted_std[States.ACCELERATION_ERR]) + ang_vel_calib = self.calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY]) + ang_vel_calib_std = self.calib_from_device.dot(predicted_std[States.ANGULAR_VELOCITY_ERR]) - fix.speed = float(np.linalg.norm(predicted_state[States.ECEF_VELOCITY])) - orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION])) - fix.roll = math.degrees(orientation_ned_euler[0]) - fix.pitch = math.degrees(orientation_ned_euler[1]) - fix.heading = math.degrees(orientation_ned_euler[2]) + fix = messaging.log.LiveLocationKalman.new_message() + fix.positionGeodetic.value = to_float(fix_pos_geo) + fix.positionGeodetic.std = to_float(fix_pos_geo_std) + fix.positionGeodetic.valid = True + fix.positionECEF.value = to_float(fix_ecef) + fix.positionECEF.std = to_float(fix_ecef_std) + fix.positionECEF.valid = True + fix.velocityECEF.value = to_float(vel_ecef) + fix.velocityECEF.std = to_float(vel_ecef_std) + fix.velocityECEF.valid = True + fix.velocityNED.value = to_float(ned_vel) + fix.velocityNED.std = to_float(ned_vel_std) + fix.velocityNED.valid = True + fix.velocityDevice.value = to_float(vel_device) + fix.velocityDevice.std = to_float(vel_device_std) + fix.velocityDevice.valid = True + fix.accelerationDevice.value = to_float(predicted_state[States.ACCELERATION]) + fix.accelerationDevice.std = to_float(predicted_std[States.ACCELERATION_ERR]) + fix.accelerationDevice.valid = True - fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])] - fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])] + fix.orientationECEF.value = to_float(orientation_ecef) + fix.orientationECEF.std = to_float(orientation_ecef_std) + fix.orientationECEF.valid = True + fix.orientationNED.value = to_float(orientation_ned) + fix.orientationNED.std = to_float(orientation_ned_std) + fix.orientationNED.valid = True + fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY]) + fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR]) + fix.angularVelocityDevice.valid = True - ned_vel = self.converter.ecef2ned(predicted_state[States.ECEF_POS] + predicted_state[States.ECEF_VELOCITY]) - self.converter.ecef2ned(predicted_state[States.ECEF_POS]) - fix.vNED = [float(ned_vel[0]), float(ned_vel[1]), float(ned_vel[2])] - fix.source = 'kalman' + fix.velocityCalibrated.value = to_float(vel_calib) + fix.velocityCalibrated.std = to_float(vel_calib_std) + fix.velocityCalibrated.valid = True + fix.angularVelocityCalibrated.value = to_float(ang_vel_calib) + fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std) + fix.angularVelocityCalibrated.valid = True + fix.accelerationCalibrated.value = to_float(acc_calib) + fix.accelerationCalibrated.std = to_float(acc_calib_std) + fix.accelerationCalibrated.valid = True - #local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY]) - #fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0])) - #fix.yawCalibration = math.degrees(math.atan2(local_vel[1], local_vel[0])) + #fix.gpsWeek = self.time.week + #fix.gpsTimeOfWeek = self.time.tow + fix.unixTimestampMillis = self.unix_timestamp_millis - imu_frame = predicted_state[States.IMU_OFFSET] - fix.imuFrame = [math.degrees(imu_frame[0]), math.degrees(imu_frame[1]), math.degrees(imu_frame[2])] + if self.filter_ready and self.calibrated: + fix.status = 'valid' + elif self.filter_ready: + fix.status = 'uncalibrated' + else: + fix.status = 'uninitialized' return fix def update_kalman(self, time, kind, meas): @@ -75,23 +132,26 @@ class Localizer(): self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude]) fix_ecef = self.converter.ned2ecef([0, 0, 0]) + #self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3)) + self.unix_timestamp_millis = log.timestamp + # TODO initing with bad bearing not allowed, maybe not bad? if not self.filter_ready and log.speed > 5: self.filter_ready = True initial_ecef = fix_ecef gps_bearing = math.radians(log.bearing) initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing]) - initial_pose_ecef_quat = euler2quat(initial_pose_ecef) + initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef) gps_speed = log.speed quat_uncertainty = 0.2**2 - initial_pose_ecef_quat = euler2quat(initial_pose_ecef) + initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef) initial_state = LiveKalman.initial_x initial_covs_diag = LiveKalman.initial_P_diag initial_state[States.ECEF_POS] = initial_ecef initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat - initial_state[States.ECEF_VELOCITY] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) + initial_state[States.ECEF_VELOCITY] = rot_from_quat(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) initial_covs_diag[States.ECEF_POS_ERR] = 10**2 initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty @@ -119,12 +179,16 @@ class Localizer(): self.cam_counter += 1 if self.cam_counter % VISION_DECIMATION == 0: + rot_device = self.device_from_calib.dot(log.rot) + rot_device_std = self.device_from_calib.dot(log.rotStd) self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, - np.concatenate([log.rot, log.rotStd])) + np.concatenate([rot_device, rot_device_std])) + trans_device = self.device_from_calib.dot(log.trans) + trans_device_std = self.device_from_calib.dot(log.transStd) self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, - np.concatenate([log.trans, log.transStd])) + np.concatenate([trans_device, trans_device_std])) def handle_sensors(self, current_time, log): # TODO does not yet account for double sensor readings in the log @@ -147,6 +211,12 @@ class Localizer(): v = sensor_reading.acceleration.v self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) + def handle_live_calib(self, current_time, log): + self.calib = log.rpyCalib + self.device_from_calib = rot_from_euler(self.calib) + self.calib_from_device = self.device_from_calib.T + self.calibrated = log.calStatus == 1 + def reset_kalman(self): self.filter_time = None self.filter_ready = False @@ -160,9 +230,9 @@ class Localizer(): def locationd_thread(sm, pm, disabled_logs=[]): if sm is None: - sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry']) + sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration']) if pm is None: - pm = messaging.PubMaster(['liveLocation']) + pm = messaging.PubMaster(['liveLocationKalman']) localizer = Localizer(disabled_logs=disabled_logs) @@ -180,16 +250,16 @@ def locationd_thread(sm, pm, disabled_logs=[]): localizer.handle_car_state(t, sm[sock]) elif sock == "cameraOdometry": localizer.handle_cam_odo(t, sm[sock]) + elif sock == "liveCalibration": + localizer.handle_live_calib(t, sm[sock]) if localizer.filter_ready and sm.updated['gpsLocationExternal']: t = sm.logMonoTime['gpsLocationExternal'] - msg = messaging.new_message() + msg = messaging.new_message('liveLocationKalman') msg.logMonoTime = t - msg.init('liveLocation') - msg.liveLocation = localizer.liveLocationMsg(t * 1e-9) - - pm.send('liveLocation', msg) + msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9) + pm.send('liveLocationKalman', msg) def main(sm=None, pm=None): diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py new file mode 100755 index 000000000..93523502c --- /dev/null +++ b/selfdrive/locationd/paramsd.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 +import math + +import cereal.messaging as messaging +from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States + +CARSTATE_DECIMATION = 5 + + +class ParamsLearner: + def __init__(self, CP): + self.kf = CarKalman() + + self.kf.filter.set_mass(CP.mass) # pylint: disable=no-member + self.kf.filter.set_rotational_inertia(CP.rotationalInertia) # pylint: disable=no-member + self.kf.filter.set_center_to_front(CP.centerToFront) # pylint: disable=no-member + self.kf.filter.set_center_to_rear(CP.wheelbase - CP.centerToFront) # pylint: disable=no-member + self.kf.filter.set_stiffness_front(CP.tireStiffnessFront) # pylint: disable=no-member + self.kf.filter.set_stiffness_rear(CP.tireStiffnessRear) # pylint: disable=no-member + + self.active = False + + self.speed = 0 + self.steering_pressed = False + self.steering_angle = 0 + self.carstate_counter = 0 + + def update_active(self): + self.active = (abs(self.steering_angle) < 45 or not self.steering_pressed) and self.speed > 5 + + def handle_log(self, t, which, msg): + if which == 'liveLocationKalman': + + v_calibrated = msg.velocityCalibrated.value + # v_calibrated_std = msg.velocityCalibrated.std + self.speed = v_calibrated[0] + + yaw_rate = msg.angularVelocityCalibrated.value[2] + # yaw_rate_std = msg.angularVelocityCalibrated.std[2] + + self.update_active() + if self.active: + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, [-yaw_rate]) + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_XY_SPEED, [[v_calibrated[0], -v_calibrated[1]]]) + + # Clamp values + x = self.kf.x + if not (10 < x[States.STEER_RATIO] < 25): + self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, [15.0]) + + if not (0.5 < x[States.STIFFNESS] < 3.0): + self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, [1.0]) + + else: + self.kf.filter.filter_time = t - 0.1 + + elif which == 'carState': + self.carstate_counter += 1 + if self.carstate_counter % CARSTATE_DECIMATION == 0: + self.steering_angle = msg.steeringAngle + self.steering_pressed = msg.steeringPressed + + self.update_active() + if self.active: + self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, [math.radians(msg.steeringAngle)]) + self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, [0]) + else: + self.kf.filter.filter_time = t - 0.1 + + +def main(sm=None, pm=None): + if sm is None: + sm = messaging.SubMaster(['liveLocationKalman', 'carState']) + if pm is None: + pm = messaging.PubMaster(['liveParameters']) + + # TODO: Read from car params at runtime + from selfdrive.car.toyota.interface import CarInterface + from selfdrive.car.toyota.values import CAR + + CP = CarInterface.get_params(CAR.COROLLA_TSS2) + learner = ParamsLearner(CP) + + while True: + sm.update() + + for which, updated in sm.updated.items(): + if not updated: + continue + t = sm.logMonoTime[which] * 1e-9 + learner.handle_log(t, which, sm[which]) + + # TODO: set valid to false when locationd stops sending + # TODO: make sure controlsd knows when there is no gyro + # TODO: move posenetValid somewhere else to show the model uncertainty alert + # TODO: Save and resume values from param + # TODO: Change KF to allow mass, etc to be inputs in predict step + + if sm.updated['carState']: + msg = messaging.new_message('liveParameters') + msg.logMonoTime = sm.logMonoTime['carState'] + + msg.liveParameters.valid = True # TODO: Check if learned values are sane + msg.liveParameters.posenetValid = True + msg.liveParameters.sensorValid = True + + x = learner.kf.x + msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) + msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) + msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET]) + msg.liveParameters.angleOffset = math.degrees(x[States.ANGLE_OFFSET_FAST]) + + # P = learner.kf.P + # print() + # print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5) + # print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5) + # print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5) + # print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5) + + pm.send('liveParameters', msg) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index e29595810..65fdfce9b 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -276,7 +276,7 @@ def handle_msg(dev, msg, nav_frame_buffer): #if dev is not None and dev.dev is not None: # dev.close() -def main(gctx=None): +def main(): global gpsLocationExternal, ubloxGnss nav_frame_buffer = {} nav_frame_buffer[0] = {} diff --git a/selfdrive/locationd/test/ubloxd_easy.py b/selfdrive/locationd/test/ubloxd_easy.py index a2fa753e7..8f226604e 100755 --- a/selfdrive/locationd/test/ubloxd_easy.py +++ b/selfdrive/locationd/test/ubloxd_easy.py @@ -10,7 +10,7 @@ import cereal.messaging as messaging unlogger = os.getenv("UNLOGGER") is not None # debug prints -def main(gctx=None): +def main(): poller = zmq.Poller() gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index dfc32fc83..1feb899c2 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -1,6 +1,6 @@ Import('env', 'arch', 'messaging', 'common', 'visionipc') -src = ['loggerd.cc', 'logger.c'] +src = ['loggerd.cc', 'logger.cc'] libs = ['zmq', 'czmq', 'capnp', 'kj', 'z', 'avformat', 'avcodec', 'swscale', 'avutil', 'yuv', 'bz2', common, 'json', messaging, visionipc] diff --git a/selfdrive/loggerd/deleter.py b/selfdrive/loggerd/deleter.py index b9ae0e8d4..27c40e835 100644 --- a/selfdrive/loggerd/deleter.py +++ b/selfdrive/loggerd/deleter.py @@ -35,7 +35,7 @@ def deleter_thread(exit_event): exit_event.wait(30) -def main(gctx=None): +def main(): deleter_thread(threading.Event()) diff --git a/selfdrive/loggerd/logger.c b/selfdrive/loggerd/logger.cc similarity index 86% rename from selfdrive/loggerd/logger.c rename to selfdrive/loggerd/logger.cc index 7dfb610ba..0bb6c1b74 100644 --- a/selfdrive/loggerd/logger.c +++ b/selfdrive/loggerd/logger.cc @@ -17,6 +17,21 @@ #include "logger.h" +#include +#include "cereal/gen/cpp/log.capnp.h" + +static void log_sentinel(LoggerState *s, cereal::Sentinel::SentinelType type) { + capnp::MallocMessageBuilder msg; + auto event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto sen = event.initSentinel(); + sen.setType(type); + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + + logger_log(s, bytes.begin(), bytes.size(), true); +} + static int mkpath(char* file_path) { assert(file_path && *file_path); char* p; @@ -125,6 +140,9 @@ fail: int logger_next(LoggerState *s, const char* root_path, char* out_segment_path, size_t out_segment_path_len, int* out_part) { + bool is_start_of_route = !s->cur_handle; + if (!is_start_of_route) log_sentinel(s, cereal::Sentinel::SentinelType::END_OF_SEGMENT); + pthread_mutex_lock(&s->lock); s->part++; @@ -147,6 +165,8 @@ int logger_next(LoggerState *s, const char* root_path, } pthread_mutex_unlock(&s->lock); + + log_sentinel(s, is_start_of_route ? cereal::Sentinel::SentinelType::START_OF_ROUTE : cereal::Sentinel::SentinelType::START_OF_SEGMENT); return 0; } @@ -171,6 +191,8 @@ void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog) { } void logger_close(LoggerState *s) { + log_sentinel(s, cereal::Sentinel::SentinelType::END_OF_ROUTE); + pthread_mutex_lock(&s->lock); free(s->init_data); if (s->cur_handle) { diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 6067f01ed..dba2287b8 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -72,7 +72,7 @@ def is_on_wifi(): if result is None: return True return 'WIFI' in result - except (AttributeError, subprocess.CalledProcessError): + except Exception: cloudlog.exception("is_on_wifi failed") return False @@ -266,7 +266,7 @@ def uploader_fn(exit_event): backoff = min(backoff*2, 120) cloudlog.info("upload done, success=%r", success) -def main(gctx=None): +def main(): uploader_fn(threading.Event()) if __name__ == "__main__": diff --git a/selfdrive/logmessaged.py b/selfdrive/logmessaged.py index 3362b5baa..693d259a7 100755 --- a/selfdrive/logmessaged.py +++ b/selfdrive/logmessaged.py @@ -3,7 +3,7 @@ import zmq from logentries import LogentriesHandler import cereal.messaging as messaging -def main(gctx=None): +def main(): # setup logentries. we forward log messages to it le_token = "e8549616-0798-4d7e-a2ca-2513ae81fa17" le_handler = LogentriesHandler(le_token, use_tls=False, verbose=False) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 39ca4871c..7c4fbc0b5 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -57,7 +57,10 @@ def unblock_stdout(): except (OSError, IOError, UnicodeDecodeError): pass - os._exit(os.wait()[1]) + # os.wait() returns a tuple with the pid and a 16 bit value + # whose low byte is the signal number and whose high byte is the exit satus + exit_status = os.wait()[1] >> 8 + os._exit(exit_status) if __name__ == "__main__": @@ -127,13 +130,14 @@ from selfdrive.version import version, dirty from selfdrive.loggerd.config import ROOT from selfdrive.launcher import launcher from common import android -from common.apk import update_apks, pm_apply_packages, start_frame +from common.apk import update_apks, pm_apply_packages, start_offroad +from common.manager_helpers import print_cpu_usage ThermalStatus = cereal.log.ThermalData.ThermalStatus # comment out anything you don't want to run managed_processes = { - "thermald": "selfdrive.thermald", + "thermald": "selfdrive.thermald.thermald", "uploader": "selfdrive.loggerd.uploader", "deleter": "selfdrive.loggerd.deleter", "controlsd": "selfdrive.controls.controlsd", @@ -390,6 +394,9 @@ def manager_thread(): # now loop thermal_sock = messaging.sub_sock('thermal') + if os.getenv("GET_CPU_USAGE"): + proc_sock = messaging.sub_sock('procLog', conflate=True) + cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) @@ -406,16 +413,23 @@ def manager_thread(): for p in persistent_processes: start_managed_process(p) - # start frame + # start offroad if ANDROID: pm_apply_packages('enable') - start_frame() + start_offroad() if os.getenv("NOBOARD") is None: start_managed_process("pandad") + if os.getenv("BLOCK") is not None: + for k in os.getenv("BLOCK").split(","): + del managed_processes[k] + logger_dead = False + start_t = time.time() + first_proc = None + while 1: msg = messaging.recv_sock(thermal_sock, wait=True) @@ -451,6 +465,21 @@ def manager_thread(): if params.get("DoUninstall", encoding='utf8') == "1": break + if os.getenv("GET_CPU_USAGE"): + dt = time.time() - start_t + + # Get first sample + if dt > 30 and first_proc is None: + first_proc = messaging.recv_sock(proc_sock) + + # Get last sample and exit + if dt > 90: + first_proc = first_proc + last_proc = messaging.recv_sock(proc_sock, wait=True) + + cleanup_all_processes(None, None) + sys.exit(print_cpu_usage(first_proc, last_proc)) + def manager_prepare(spinner=None): # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) @@ -482,40 +511,29 @@ def main(): params = Params() params.manager_start() + default_params = [ + ("CommunityFeaturesToggle", "0"), + ("CompletedTrainingVersion", "0"), + ("IsMetric", "0"), + ("RecordFront", "0"), + ("HasAcceptedTerms", "0"), + ("HasCompletedSetup", "0"), + ("IsUploadRawEnabled", "1"), + ("IsLdwEnabled", "1"), + ("IsGeofenceEnabled", "-1"), + ("SpeedLimitOffset", "0"), + ("LongitudinalControl", "0"), + ("LimitSetSpeed", "0"), + ("LimitSetSpeedNeural", "0"), + ("LastUpdateTime", datetime.datetime.now().isoformat().encode('utf8')), + ("OpenpilotEnabledToggle", "1"), + ("LaneChangeEnabled", "1"), + ] + # set unset params - if params.get("CommunityFeaturesToggle") is None: - params.put("CommunityFeaturesToggle", "0") - if params.get("CompletedTrainingVersion") is None: - params.put("CompletedTrainingVersion", "0") - if params.get("IsMetric") is None: - params.put("IsMetric", "0") - if params.get("RecordFront") is None: - params.put("RecordFront", "0") - if params.get("HasAcceptedTerms") is None: - params.put("HasAcceptedTerms", "0") - if params.get("HasCompletedSetup") is None: - params.put("HasCompletedSetup", "0") - if params.get("IsUploadRawEnabled") is None: - params.put("IsUploadRawEnabled", "1") - if params.get("IsLdwEnabled") is None: - params.put("IsLdwEnabled", "1") - if params.get("IsGeofenceEnabled") is None: - params.put("IsGeofenceEnabled", "-1") - if params.get("SpeedLimitOffset") is None: - params.put("SpeedLimitOffset", "0") - if params.get("LongitudinalControl") is None: - params.put("LongitudinalControl", "0") - if params.get("LimitSetSpeed") is None: - params.put("LimitSetSpeed", "0") - if params.get("LimitSetSpeedNeural") is None: - params.put("LimitSetSpeedNeural", "0") - if params.get("LastUpdateTime") is None: - t = datetime.datetime.now().isoformat() - params.put("LastUpdateTime", t.encode('utf8')) - if params.get("OpenpilotEnabledToggle") is None: - params.put("OpenpilotEnabledToggle", "1") - if params.get("LaneChangeEnabled") is None: - params.put("LaneChangeEnabled", "1") + for k, v in default_params: + if params.get(k) is None: + params.put(k, v) dragonpilot_set_params(params) @@ -526,9 +544,11 @@ def main(): if params.get("Passive") is None: raise Exception("Passive must be set to continue") + reg = False if params.get("DragonEnableRegistration", encoding='utf8') == "0" else True + if ANDROID: update_apks() - manager_init() + manager_init(reg) manager_prepare(spinner) spinner.close() @@ -547,6 +567,8 @@ def main(): try: manager_thread() + except SystemExit: + raise except Exception: traceback.print_exc() crash.capture_exception() diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index b7d1c1ced..b544a3e59 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -10,10 +10,11 @@ #define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1 #define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2 #define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 2 -#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION +#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION #define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2 #define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2 -#define META_IDX LONG_A_IDX + TIME_DISTANCE*2 +#define DESIRE_STATE_IDX LONG_A_IDX + TIME_DISTANCE*2 +#define META_IDX DESIRE_STATE_IDX + DESIRE_LEN #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE #define OUTPUT_SIZE POSE_IDX + POSE_SIZE #ifdef TEMPORAL @@ -43,7 +44,9 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t #ifdef DESIRE s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0; - s->m->addDesire(s->desire, DESIRE_SIZE); + s->pulse_desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); + for (int i = 0; i < DESIRE_SIZE; i++) s->pulse_desire[i] = 0.0; + s->m->addDesire(s->pulse_desire, DESIRE_SIZE); #endif // Build Vandermonde matrix @@ -61,7 +64,16 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, mat3 transform, void* sock, float *desire_in) { #ifdef DESIRE if (desire_in != NULL) { - for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i]; + for (int i = 0; i < DESIRE_SIZE; i++) { + // Model decides when action is completed + // so desire input is just a pulse triggered on rising edge + if (desire_in[i] - s->desire[i] == 1) { + s->pulse_desire[i] = desire_in[i]; + } else { + s->pulse_desire[i] = 0.0; + } + s->desire[i] = desire_in[i]; + } } #endif @@ -88,7 +100,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, net_outputs.long_x = &s->output[LONG_X_IDX]; net_outputs.long_v = &s->output[LONG_V_IDX]; net_outputs.long_a = &s->output[LONG_A_IDX]; - net_outputs.meta = &s->output[META_IDX]; + net_outputs.meta = &s->output[DESIRE_STATE_IDX]; net_outputs.pose = &s->output[POSE_IDX]; return net_outputs; } @@ -167,11 +179,11 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo path.setStd(std); } -void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx) { +void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx, int t_offset) { const double x_scale = 10.0; const double y_scale = 10.0; - lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE])); + lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE + t_offset])); lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]); lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS])); lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]); @@ -183,11 +195,13 @@ void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, in } void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) { - meta.setEngagedProb(meta_data[0]); - meta.setGasDisengageProb(meta_data[1]); - meta.setBrakeDisengageProb(meta_data[2]); - meta.setSteerOverrideProb(meta_data[3]); - kj::ArrayPtr desire_pred(&meta_data[OTHER_META_SIZE], DESIRE_PRED_SIZE); + kj::ArrayPtr desire_state(&meta_data[0], DESIRE_LEN); + meta.setDesireState(desire_state); + meta.setEngagedProb(meta_data[DESIRE_LEN]); + meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]); + meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]); + meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]); + kj::ArrayPtr desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE); meta.setDesirePrediction(desire_pred); } @@ -228,22 +242,24 @@ void model_publish(PubSocket *sock, uint32_t frame_id, // Find the distribution that corresponds to the current lead int mdn_max_idx = 0; + int t_offset = 0; for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) { + if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { mdn_max_idx = i; } } auto lead = framed.initLead(); - fill_lead(lead, net_outputs.lead, mdn_max_idx); + fill_lead(lead, net_outputs.lead, mdn_max_idx, t_offset); // Find the distribution that corresponds to the lead in 2s mdn_max_idx = 0; + t_offset = 1; for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) { + if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { mdn_max_idx = i; } } auto lead_future = framed.initLeadFuture(); - fill_lead(lead_future, net_outputs.lead, mdn_max_idx); + fill_lead(lead_future, net_outputs.lead, mdn_max_idx, t_offset); auto meta = framed.initMeta(); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index bf46cb9d4..742eb6ced 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -34,6 +34,7 @@ #define MODEL_PATH_DISTANCE 192 #define POLYFIT_DEGREE 4 #define SPEED_PERCENTILES 10 +#define DESIRE_LEN 8 #define DESIRE_PRED_SIZE 32 #define OTHER_META_SIZE 4 #define LEAD_MDN_N 5 // probs for 5 groups @@ -51,6 +52,7 @@ struct ModelDataRaw { float *long_x; float *long_v; float *long_a; + float *desire_state; float *meta; float *pose; }; @@ -63,6 +65,7 @@ typedef struct ModelState { RunModel *m; #ifdef DESIRE float *desire; + float *pulse_desire; #endif } ModelState; diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index 2c21bdf72..bbd0664dc 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -86,7 +86,7 @@ def update_panda(): raise AssertionError -def main(gctx=None): +def main(): update_panda() os.chdir("boardd") diff --git a/selfdrive/registration.py b/selfdrive/registration.py index bebe05492..5b01d1446 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -15,9 +15,10 @@ def register(): params.put("Version", version) params.put("TermsVersion", terms_version) params.put("TrainingVersion", training_version) - params.put("GitCommit", get_git_commit()) - params.put("GitBranch", get_git_branch()) - params.put("GitRemote", get_git_remote()) + + params.put("GitCommit", get_git_commit(default="")) + params.put("GitBranch", get_git_branch(default="")) + params.put("GitRemote", get_git_remote(default="")) params.put("SubscriberInfo", get_subscriber_info()) # create a key for auth diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 2170cb71f..7d6b24d43 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -357,8 +357,7 @@ class Plant(): # Fake sockets that controlsd subscribes to - live_parameters = messaging.new_message() - live_parameters.init('liveParameters') + live_parameters = messaging.new_message('liveParameters') live_parameters.liveParameters.valid = True live_parameters.liveParameters.sensorValid = True live_parameters.liveParameters.posenetValid = True @@ -366,19 +365,16 @@ class Plant(): live_parameters.liveParameters.stiffnessFactor = 1.0 Plant.live_params.send(live_parameters.to_bytes()) - driver_state = messaging.new_message() - driver_state.init('driverState') + driver_state = messaging.new_message('driverState') driver_state.driverState.faceOrientation = [0.] * 3 driver_state.driverState.facePosition = [0.] * 2 Plant.driverState.send(driver_state.to_bytes()) - health = messaging.new_message() - health.init('health') + health = messaging.new_message('health') health.health.controlsAllowed = True Plant.health.send(health.to_bytes()) - thermal = messaging.new_message() - thermal.init('thermal') + thermal = messaging.new_message('thermal') thermal.thermal.freeSpace = 1. thermal.thermal.batteryPercent = 100 Plant.thermal.send(thermal.to_bytes()) @@ -386,10 +382,8 @@ class Plant(): # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step if publish_model and self.frame % 5 == 0: - md = messaging.new_message() - cal = messaging.new_message() - md.init('model') - cal.init('liveCalibration') + md = messaging.new_message('model') + cal = messaging.new_message('liveCalibration') md.model.frameId = 0 for x in [md.model.path, md.model.leftLane, md.model.rightLane]: x.points = [0.0]*50 diff --git a/selfdrive/test/longitudinal_maneuvers/plant_ui.py b/selfdrive/test/longitudinal_maneuvers/plant_ui.py index f4b7715e3..3c73f8329 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant_ui.py +++ b/selfdrive/test/longitudinal_maneuvers/plant_ui.py @@ -65,8 +65,7 @@ if __name__ == "__main__": else: cruise_buttons = 0 - md = messaging.new_message() - md.init('model') + md = messaging.new_message('model') md.model.frameId = 0 for x in [md.model.path, md.model.leftLane, md.model.rightLane]: x.points = [0.0]*50 diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 6cd689f17..02e4e63ca 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -333,7 +333,6 @@ class LongitudinalControl(unittest.TestCase): params.put("OpenpilotEnabledToggle", "1") params.put("CommunityFeaturesToggle", "1") - manager.gctx = {} manager.prepare_managed_process('radard') manager.prepare_managed_process('controlsd') manager.prepare_managed_process('plannerd') diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index 82e701a75..731e71672 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -2,6 +2,7 @@ import bz2 import os import sys +import numbers import dictdiffer if "CI" in os.environ: @@ -13,7 +14,7 @@ from tools.lib.logreader import LogReader def save_log(dest, log_msgs): dat = b"" - for msg in log_msgs: + for msg in tqdm(log_msgs): dat += msg.as_builder().to_bytes() dat = bz2.compress(dat) @@ -22,7 +23,7 @@ def save_log(dest, log_msgs): def remove_ignored_fields(msg, ignore): msg = msg.as_builder() - for key, val in ignore: + for key in ignore: attr = msg keys = key.split(".") if msg.which() not in key and len(keys) > 1: @@ -34,21 +35,29 @@ def remove_ignored_fields(msg, ignore): except: break else: + v = getattr(attr, keys[-1]) + if isinstance(v, bool): + val = False + elif isinstance(v, numbers.Number): + val = 0 + else: + raise NotImplementedError setattr(attr, keys[-1], val) return msg.as_reader() -def compare_logs(log1, log2, ignore=[]): +def compare_logs(log1, log2, ignore_fields=[], ignore_msgs=[]): + filter_msgs = lambda m: m.which() not in ignore_msgs + log1, log2 = [list(filter(filter_msgs, log)) for log in (log1, log2)] assert len(log1) == len(log2), "logs are not same length: " + str(len(log1)) + " VS " + str(len(log2)) - ignore_fields = [k for k, v in ignore] diff = [] for msg1, msg2 in tqdm(zip(log1, log2)): if msg1.which() != msg2.which(): print(msg1, msg2) - assert False, "msgs not aligned between logs" + raise Exception("msgs not aligned between logs") - msg1_bytes = remove_ignored_fields(msg1, ignore).as_builder().to_bytes() - msg2_bytes = remove_ignored_fields(msg2, ignore).as_builder().to_bytes() + msg1_bytes = remove_ignored_fields(msg1, ignore_fields).as_builder().to_bytes() + msg2_bytes = remove_ignored_fields(msg2, ignore_fields).as_builder().to_bytes() if msg1_bytes != msg2_bytes: msg1_dict = msg1.to_dict(verbose=True) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 9b27308b2..b8288987b 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -61,8 +61,7 @@ class FakeSocket: class DumbSocket: def __init__(self, s=None): if s is not None: - dat = messaging.new_message() - dat.init(s) + dat = messaging.new_message(s) self.data = dat.to_bytes() def receive(self, non_blocking=False): @@ -108,11 +107,10 @@ class FakePubMaster(messaging.PubMaster): self.sock = {} self.last_updated = None for s in services: - data = messaging.new_message() try: - data.init(s) + data = messaging.new_message(s) except: - data.init(s, 0) + data = messaging.new_message(s, 0) self.data[s] = data.as_reader() self.sock[s] = DumbSocket() self.send_called = threading.Event() @@ -200,7 +198,7 @@ CONFIGS = [ "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "model": [], }, - ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)], + ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, should_recv_callback=None, ), @@ -210,7 +208,7 @@ CONFIGS = [ "can": ["radarState", "liveTracks"], "liveParameters": [], "controlsState": [], "model": [], }, - ignore=[("logMonoTime", 0), ("valid", True), ("radarState.cumLagMs", 0)], + ignore=["logMonoTime", "valid", "radarState.cumLagMs"], init_callback=get_car_params, should_recv_callback=radar_rcv_callback, ), @@ -220,7 +218,7 @@ CONFIGS = [ "model": ["pathPlan"], "radarState": ["plan"], "carState": [], "controlsState": [], "liveParameters": [], }, - ignore=[("logMonoTime", 0), ("valid", True), ("plan.processingDelay", 0)], + ignore=["logMonoTime", "valid", "plan.processingDelay"], init_callback=get_car_params, should_recv_callback=None, ), @@ -229,7 +227,7 @@ CONFIGS = [ pub_sub={ "cameraOdometry": ["liveCalibration"] }, - ignore=[("logMonoTime", 0), ("valid", True)], + ignore=["logMonoTime", "valid"], init_callback=get_car_params, should_recv_callback=calibration_rcv_callback, ), @@ -239,7 +237,7 @@ CONFIGS = [ "driverState": ["dMonitoringState"], "liveCalibration": [], "carState": [], "model": [], "gpsLocation": [], }, - ignore=[("logMonoTime", 0), ("valid", True)], + ignore=["logMonoTime", "valid"], init_callback=get_car_params, should_recv_callback=None, ), diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1414556bf..150649e67 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -917e6889be1691fb96e7566a92e0c6bbefc861a4 \ No newline at end of file +b98f4b6858a21fd6602fb41c6a21f69b1ca81921 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 6832aba33..f57fc83c3 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -1,116 +1,179 @@ #!/usr/bin/env python3 +import argparse import os import requests import sys import tempfile -from selfdrive.test.process_replay.compare_logs import compare_logs +from selfdrive.car.car_helpers import interface_names from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS +from selfdrive.test.process_replay.compare_logs import compare_logs from tools.lib.logreader import LogReader + +INJECT_MODEL = 0 + segments = [ - "0375fdf7b1ce594d|2019-06-13--08-32-25--3", # HONDA.ACCORD - "99c94dc769b5d96e|2019-08-03--14-19-59--2", # HONDA.CIVIC - "cce908f7eb8db67d|2019-08-02--15-09-51--3", # TOYOTA.COROLLA_TSS2 - "7ad88f53d406b787|2019-07-09--10-18-56--8", # GM.VOLT - "704b2230eb5190d6|2019-07-06--19-29-10--0", # HYUNDAI.KIA_SORENTO - "b6e1317e1bfbefa6|2019-07-06--04-05-26--5", # CHRYSLER.JEEP_CHEROKEE - "7873afaf022d36e2|2019-07-03--18-46-44--0", # SUBARU.IMPREZA + ("HONDA", "0375fdf7b1ce594d|2019-06-13--08-32-25--3"), # HONDA.ACCORD + ("HONDA", "99c94dc769b5d96e|2019-08-03--14-19-59--2"), # HONDA.CIVIC + ("TOYOTA", "77611a1fac303767|2020-02-29--13-29-33--3"), # TOYOTA.COROLLA_TSS2 + ("GM", "7cc2a8365b4dd8a9|2018-12-02--12-10-44--2"), # GM.ACADIA + ("CHRYSLER", "b6849f5cf2c926b1|2020-02-28--07-29-48--13"), # CHRYSLER.PACIFICA + ("HYUNDAI", "38bfd238edecbcd7|2018-08-29--22-02-15--4"), # HYUNDAI.SANTA_FE + #("CHRYSLER", "b6e1317e1bfbefa6|2020-03-04--13-11-40"), # CHRYSLER.JEEP_CHEROKEE + ("SUBARU", "7873afaf022d36e2|2019-07-03--18-46-44--0"), # SUBARU.IMPREZA + ("VOLKSWAGEN", "76b83eb0245de90e|2020-03-05--19-16-05--3"), # VW.GOLF + + # Enable when port is tested and dascamOnly is no longer set + # ("NISSAN", "fbbfa6af821552b9|2020-03-03--08-09-43--0"), # NISSAN.XTRAIL ] -def get_segment(segment_name): +# ford doesn't need to be tested until a full port is done +excluded_interfaces = ["mock", "ford", "nissan"] + +BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" + +# run the full test (including checks) when no args given +FULL_TEST = len(sys.argv) <= 1 + +def get_segment(segment_name, original=True): route_name, segment_num = segment_name.rsplit("--", 1) - rlog_url = "https://commadataci.blob.core.windows.net/openpilotci/%s/%s/rlog.bz2" \ - % (route_name.replace("|", "/"), segment_num) - r = requests.get(rlog_url) - if r.status_code != 200: - return None + if original: + rlog_url = BASE_URL + "%s/%s/rlog.bz2" % (route_name.replace("|", "/"), segment_num) + else: + process_replay_dir = os.path.dirname(os.path.abspath(__file__)) + model_ref_commit = open(os.path.join(process_replay_dir, "model_ref_commit")).read().strip() + rlog_url = BASE_URL + "%s/%s/rlog_%s.bz2" % (route_name.replace("|", "/"), segment_num, model_ref_commit) + req = requests.get(rlog_url) + assert req.status_code == 200, ("Failed to download log for %s" % segment_name) with tempfile.NamedTemporaryFile(delete=False, suffix=".bz2") as f: - f.write(r.content) + f.write(req.content) return f.name +def test_process(cfg, lr, cmp_log_fn, ignore_fields=[], ignore_msgs=[]): + if not os.path.isfile(cmp_log_fn): + req = requests.get(BASE_URL + os.path.basename(cmp_log_fn)) + assert req.status_code == 200, ("Failed to download %s" % cmp_log_fn) + + with tempfile.NamedTemporaryFile(suffix=".bz2") as f: + f.write(req.content) + f.flush() + f.seek(0) + cmp_log_msgs = list(LogReader(f.name)) + else: + cmp_log_msgs = list(LogReader(cmp_log_fn)) + + log_msgs = replay_process(cfg, lr) + + # check to make sure openpilot is engaged in the route + # TODO: update routes so enable check can run + # failed enable check: honda bosch, hyundai, chrysler, and subaru + if cfg.proc_name == "controlsd" and FULL_TEST and False: + for msg in log_msgs: + if msg.which() == "controlsState": + if msg.controlsState.active: + break + else: + segment = cmp_log_fn.split("/")[-1].split("_")[0] + raise Exception("Route never enabled: %s" % segment) + + return compare_logs(cmp_log_msgs, log_msgs, ignore_fields+cfg.ignore, ignore_msgs) + +def format_diff(results, ref_commit): + diff1, diff2 = "", "" + diff2 += "***** tested against commit %s *****\n" % ref_commit + + failed = False + for segment, result in list(results.items()): + diff1 += "***** results for segment %s *****\n" % segment + diff2 += "***** differences for segment %s *****\n" % segment + + for proc, diff in list(result.items()): + diff1 += "\t%s\n" % proc + diff2 += "*** process: %s ***\n" % proc + + if isinstance(diff, str): + diff1 += "\t\t%s\n" % diff + failed = True + elif len(diff): + cnt = {} + for d in diff: + diff2 += "\t%s\n" % str(d) + + k = str(d[1]) + cnt[k] = 1 if k not in cnt else cnt[k] + 1 + + for k, v in sorted(cnt.items()): + diff1 += "\t\t%s: %s\n" % (k, v) + failed = True + return diff1, diff2, failed + if __name__ == "__main__": - process_replay_dir = os.path.dirname(os.path.abspath(__file__)) - ref_commit_fn = os.path.join(process_replay_dir, "ref_commit") + parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output") - if not os.path.isfile(ref_commit_fn): + # whitelist has precedence over blacklist in case both are defined + parser.add_argument("--whitelist-procs", type=str, nargs="*", default=[], + help="Whitelist given processes from the test (e.g. controlsd)") + parser.add_argument("--whitelist-cars", type=str, nargs="*", default=[], + help="Whitelist given cars from the test (e.g. HONDA)") + parser.add_argument("--blacklist-procs", type=str, nargs="*", default=[], + help="Blacklist given processes from the test (e.g. controlsd)") + parser.add_argument("--blacklist-cars", type=str, nargs="*", default=[], + help="Blacklist given cars from the test (e.g. HONDA)") + parser.add_argument("--ignore-fields", type=str, nargs="*", default=[], + help="Extra fields or msgs to ignore (e.g. carState.events)") + parser.add_argument("--ignore-msgs", type=str, nargs="*", default=[], + help="Msgs to ignore (e.g. carEvents)") + args = parser.parse_args() + + cars_whitelisted = len(args.whitelist_cars) > 0 + procs_whitelisted = len(args.whitelist_procs) > 0 + + process_replay_dir = os.path.dirname(os.path.abspath(__file__)) + try: + ref_commit = open(os.path.join(process_replay_dir, "ref_commit")).read().strip() + except: print("couldn't find reference commit") sys.exit(1) - ref_commit = open(ref_commit_fn).read().strip() print("***** testing against commit %s *****" % ref_commit) + # check to make sure all car brands are tested + if FULL_TEST: + tested_cars = set(c.lower() for c, _ in segments) + untested = (set(interface_names) - set(excluded_interfaces)) - tested_cars + assert len(untested) == 0, "Cars missing routes: %s" % (str(untested)) + results = {} - for segment in segments: + for car_brand, segment in segments: + if (cars_whitelisted and car_brand.upper() not in args.whitelist_cars) or \ + (not cars_whitelisted and car_brand.upper() in args.blacklist_cars): + continue + print("***** testing route segment %s *****\n" % segment) results[segment] = {} rlog_fn = get_segment(segment) - - if rlog_fn is None: - print("failed to get segment %s" % segment) - sys.exit(1) - lr = LogReader(rlog_fn) for cfg in CONFIGS: - log_msgs = replay_process(cfg, lr) + if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \ + (not procs_whitelisted and cfg.proc_name in args.blacklist_procs): + continue - log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit)) - - if not os.path.isfile(log_fn): - url = "https://commadataci.blob.core.windows.net/openpilotci/" - req = requests.get(url + os.path.basename(log_fn)) - if req.status_code != 200: - results[segment][cfg.proc_name] = "failed to download comparison log" - continue - - with tempfile.NamedTemporaryFile(suffix=".bz2") as f: - f.write(req.content) - f.flush() - f.seek(0) - cmp_log_msgs = list(LogReader(f.name)) - else: - cmp_log_msgs = list(LogReader(log_fn)) - - diff = compare_logs(cmp_log_msgs, log_msgs, cfg.ignore) - results[segment][cfg.proc_name] = diff + cmp_log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit)) + results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs) os.remove(rlog_fn) - failed = False + diff1, diff2, failed = format_diff(results, ref_commit) with open(os.path.join(process_replay_dir, "diff.txt"), "w") as f: - f.write("***** tested against commit %s *****\n" % ref_commit) + f.write(diff2) + print(diff1) - for segment, result in list(results.items()): - f.write("***** differences for segment %s *****\n" % segment) - print("***** results for segment %s *****" % segment) - - for proc, diff in list(result.items()): - f.write("*** process: %s ***\n" % proc) - print("\t%s" % proc) - - if isinstance(diff, str): - print("\t\t%s" % diff) - failed = True - elif len(diff): - cnt = {} - for d in diff: - f.write("\t%s\n" % str(d)) - - k = str(d[1]) - cnt[k] = 1 if k not in cnt else cnt[k] + 1 - - for k, v in sorted(cnt.items()): - print("\t\t%s: %s" % (k, v)) - failed = True - - if failed: - print("TEST FAILED") - else: - print("TEST SUCCEEDED") + print("TEST", "FAILED" if failed else "SUCCEEDED") print("\n\nTo update the reference logs for this test run:") print("./update_refs.py") diff --git a/selfdrive/test/process_replay/update_refs.py b/selfdrive/test/process_replay/update_refs.py index e0da25133..4a769d865 100755 --- a/selfdrive/test/process_replay/update_refs.py +++ b/selfdrive/test/process_replay/update_refs.py @@ -20,7 +20,7 @@ if __name__ == "__main__": with open(ref_commit_fn, "w") as f: f.write(ref_commit) - for segment in segments: + for car_brand, segment in segments: rlog_fn = get_segment(segment) if rlog_fn is None: diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index 1436f404e..fd7a8e60b 100755 --- a/selfdrive/test/test_car_models.py +++ b/selfdrive/test/test_car_models.py @@ -20,7 +20,7 @@ from selfdrive.car.hyundai.values import CAR as HYUNDAI from selfdrive.car.chrysler.values import CAR as CHRYSLER from selfdrive.car.subaru.values import CAR as SUBARU from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN -from selfdrive.car.mock.values import CAR as MOCK +from selfdrive.car.nissan.values import CAR as NISSAN os.environ['NOCRASH'] = '1' @@ -58,35 +58,15 @@ def get_route_log(route_name): sys.exit(-1) routes = { - "975b26878285314d|2018-12-25--14-42-13": { - 'carFingerprint': CHRYSLER.PACIFICA_2018_HYBRID, - 'enableCamera': True, - }, - "b0c9d2329ad1606b|2019-01-06--10-11-23": { + "420a8e183f1aed48|2020-03-05--07-15-29": { 'carFingerprint': CHRYSLER.PACIFICA_2017_HYBRID, 'enableCamera': True, }, - "0607d2516fc2148f|2019-02-13--23-03-16": { + "8190c7275a24557b|2020-01-29--08-33-58": { # 2020 model year 'carFingerprint': CHRYSLER.PACIFICA_2019_HYBRID, 'enableCamera': True, }, - "8190c7275a24557b|2020-01-29--08-33-58": { - 'carFingerprint': CHRYSLER.PACIFICA_2020_HYBRID, - 'enableCamera': True, - }, # This pacifica was removed because the fingerprint seemed from a Volt - #"9f7a7e50a51fb9db|2019-01-03--14-05-01": { - # 'carFingerprint': CHRYSLER.PACIFICA_2018, - # 'enableCamera': True, - #}, - "9f7a7e50a51fb9db|2019-01-17--18-34-21": { - 'carFingerprint': CHRYSLER.JEEP_CHEROKEE, - 'enableCamera': True, - }, - "192a598e34926b1e|2019-04-04--13-27-39": { - 'carFingerprint': CHRYSLER.JEEP_CHEROKEE_2019, - 'enableCamera': True, - }, "f1b4c567731f4a1b|2018-04-18--11-29-37": { 'carFingerprint': FORD.FUSION, 'enableCamera': False, @@ -99,31 +79,15 @@ routes = { 'carFingerprint': GM.CADILLAC_CT6, 'enableCamera': True, }, - "265007255e794bce|2018-11-24--22-08-31": { - 'carFingerprint': GM.CADILLAC_ATS, - 'enableCamera': True, - }, "c950e28c26b5b168|2018-05-30--22-03-41": { 'carFingerprint': GM.VOLT, 'enableCamera': True, }, # TODO: use another route that has radar data at start - "aadda448b49c99ad|2018-10-25--17-16-22": { - 'carFingerprint': GM.MALIBU, - 'enableCamera': True, - }, - "49c73650e65ff465|2018-11-19--16-58-04": { - 'carFingerprint': GM.HOLDEN_ASTRA, - 'enableCamera': True, - }, "7cc2a8365b4dd8a9|2018-12-02--12-10-44": { 'carFingerprint': GM.ACADIA, 'enableCamera': True, }, - "aa20e335f61ba898|2018-12-17--21-10-37": { - 'carFingerprint': GM.BUICK_REGAL, - 'enableCamera': False, - }, "aa20e335f61ba898|2019-02-05--16-59-04": { 'carFingerprint': GM.BUICK_REGAL, 'enableCamera': True, @@ -132,13 +96,10 @@ routes = { 'carFingerprint': HONDA.CIVIC, 'enableCamera': True, }, - "c9d60e5e02c04c5c|2018-01-08--16-01-49": { - 'carFingerprint': HONDA.CRV, - 'enableCamera': True, - }, - "1851183c395ef471|2018-05-31--18-07-21": { - 'carFingerprint': HONDA.CRV_5G, + "a859a044a447c2b0|2020-03-03--18-42-45": { + 'carFingerprint': HONDA.CRV_EU, 'enableCamera': True, + 'fingerprintSource': 'fixed', }, "232585b7784c1af4|2019-04-08--14-12-14": { 'carFingerprint': HONDA.CRV_HYBRID, @@ -152,10 +113,6 @@ routes = { 'carFingerprint': HONDA.ACURA_ILX, 'enableCamera': True, }, - "21aa231dee2a68bd|2018-01-30--04-54-41": { - 'carFingerprint': HONDA.ODYSSEY, - 'enableCamera': True, - }, "81722949a62ea724|2019-03-29--15-51-26": { 'carFingerprint': HONDA.ODYSSEY_CHN, 'enableCamera': False, @@ -164,43 +121,48 @@ routes = { 'carFingerprint': HONDA.ODYSSEY_CHN, 'enableCamera': True, }, - "5a2cfe4bb362af9e|2018-02-02--23-41-07": { - 'carFingerprint': HONDA.ACURA_RDX, - 'enableCamera': True, - }, - "3e9592a1c78a3d63|2018-02-08--20-28-24": { - 'carFingerprint': HONDA.PILOT, - 'enableCamera': True, - }, - "34a84d2b765df688|2018-08-28--12-41-00": { - 'carFingerprint': HONDA.PILOT_2019, - 'enableCamera': True, - }, - "900ad17e536c3dc7|2018-04-12--22-02-36": { - 'carFingerprint': HONDA.RIDGELINE, + "08a3deb07573f157|2020-03-06--16-11-19": { + 'carFingerprint': HONDA.ACCORD_15, 'enableCamera': True, }, "f1b4c567731f4a1b|2018-06-06--14-43-46": { 'carFingerprint': HONDA.ACCORD, 'enableCamera': True, }, - "1582e1dc57175194|2018-08-15--07-46-07": { - 'carFingerprint': HONDA.ACCORD_15, + "690c4c9f9f2354c7|2018-09-15--17-36-05": { + 'carFingerprint': HONDA.ACCORDH, 'enableCamera': True, }, - # TODO: This doesnt fingerprint because the fingerprint overlaps with the Insight - # "690c4c9f9f2354c7|2018-09-15--17-36-05": { - # 'carFingerprint': HONDA.ACCORDH, - # 'enableCamera': True, - # }, - "1632088eda5e6c4d|2018-06-07--08-03-18": { + "1ad763dd22ef1a0e|2020-02-29--18-37-03": { + 'carFingerprint': HONDA.CRV_5G, + 'enableCamera': True, + }, + "0a96f86fcfe35964|2020-02-05--07-25-51": { + 'carFingerprint': HONDA.ODYSSEY, + 'enableCamera': True, + }, + "d83f36766f8012a5|2020-02-05--18-42-21": { + 'carFingerprint': HONDA.CIVIC_BOSCH_DIESEL, + 'enableCamera': True, + 'fingerprintSource': 'fixed', + }, + "fb51d190ddfd8a90|2020-02-25--14-43-43": { + 'carFingerprint': HONDA.INSIGHT, + 'enableCamera': True, + 'fingerprintSource': 'fixed', + }, + "07d37d27996096b6|2020-03-04--21-57-27": { + 'carFingerprint': HONDA.PILOT, + 'enableCamera': True, + }, + "22affd6c545d985e|2020-03-08--01-08-09": { + 'carFingerprint': HONDA.PILOT_2019, + 'enableCamera': True, + }, + "0a78dfbacc8504ef|2020-03-04--13-29-55": { 'carFingerprint': HONDA.CIVIC_BOSCH, 'enableCamera': True, }, - #"18971a99f3f2b385|2018-11-14--19-09-31": { - # 'carFingerprint': HONDA.INSIGHT, - # 'enableCamera': True, - #}, "38bfd238edecbcd7|2018-08-22--09-45-44": { 'carFingerprint': HYUNDAI.SANTA_FE, 'enableCamera': False, @@ -209,31 +171,20 @@ routes = { 'carFingerprint': HYUNDAI.SANTA_FE, 'enableCamera': True, }, - "a893a80e5c5f72c8|2019-01-14--20-02-59": { - 'carFingerprint': HYUNDAI.GENESIS, - 'enableCamera': True, - }, - "9d5fb4f0baa1b3e1|2019-01-14--17-45-59": { - 'carFingerprint': HYUNDAI.KIA_SORENTO, - 'enableCamera': True, - }, - "215cd70e9c349266|2018-11-25--22-22-12": { - 'carFingerprint': HYUNDAI.KIA_STINGER, - 'enableCamera': True, - }, - "31390e3eb6f7c29a|2019-01-23--08-56-00": { + "7653b2bce7bcfdaa|2020-03-04--15-34-32": { 'carFingerprint': HYUNDAI.KIA_OPTIMA, 'enableCamera': True, }, - "53ac3251e03f95d7|2019-01-10--13-43-32": { - 'carFingerprint': HYUNDAI.ELANTRA, - 'enableCamera': True, - }, "f7b6be73e3dfd36c|2019-05-12--18-07-16": { 'carFingerprint': TOYOTA.AVALON, 'enableCamera': False, 'enableDsu': False, }, + "6cdecc4728d4af37|2020-02-23--15-44-18": { + 'carFingerprint': TOYOTA.CAMRY, + 'enableCamera': True, + 'enableDsu': False, + }, "f7b6be73e3dfd36c|2019-05-11--22-34-20": { 'carFingerprint': TOYOTA.AVALON, 'enableCamera': True, @@ -281,6 +232,12 @@ routes = { 'enableDsu': True, 'enableGasInterceptor': True, }, + "32a7df20486b0f70|2020-02-06--16-06-50": { + 'carFingerprint': TOYOTA.RAV4H, + 'enableCamera': True, + 'enableDsu': True, + 'enableGasInterceptor': False, + }, "cdf2f7de565d40ae|2019-04-25--03-53-41": { 'carFingerprint': TOYOTA.RAV4_TSS2, 'enableCamera': True, @@ -296,6 +253,11 @@ routes = { 'enableCamera': True, 'enableDsu': False, }, + "25057fa6a5a63dfb|2020-03-04--08-44-23": { + 'carFingerprint': TOYOTA.LEXUS_CTH, + 'enableCamera': True, + 'enableDsu': True, + }, "f49e8041283f2939|2019-05-29--13-48-33": { 'carFingerprint': TOYOTA.LEXUS_ESH_TSS2, 'enableCamera': False, @@ -330,49 +292,7 @@ routes = { 'enableCamera': True, 'enableDsu': False, }, - #FIXME: This works sometimes locally, but never in CI. Timing issue? - #"b0f5a01cf604185c|2018-01-31--20-11-39": { - # 'carFingerprint': TOYOTA.LEXUS_RXH, - # 'enableCamera': False, - # 'enableDsu': False, - #}, - "8ae193ceb56a0efe|2018-06-18--20-07-32": { - 'carFingerprint': TOYOTA.RAV4H, - 'enableCamera': True, - 'enableDsu': True, - }, - "fd10b9a107bb2e49|2018-07-24--16-32-42": { - 'carFingerprint': TOYOTA.CHR, - 'enableCamera': True, - 'enableDsu': False, - }, - "fd10b9a107bb2e49|2018-07-24--20-32-08": { - 'carFingerprint': TOYOTA.CHR, - 'enableCamera': False, - 'enableDsu': False, - }, - "b4c18bf13d5955da|2018-07-29--13-39-46": { - 'carFingerprint': TOYOTA.CHRH, - 'enableCamera': True, - 'enableDsu': False, - }, - "d2d8152227f7cb82|2018-07-25--13-40-56": { - 'carFingerprint': TOYOTA.CAMRY, - 'enableCamera': True, - 'enableDsu': False, - }, - "fbd011384db5e669|2018-07-26--20-51-48": { - 'carFingerprint': TOYOTA.CAMRYH, - 'enableCamera': True, - 'enableDsu': False, - }, - # TODO: This replay has no good model/video - # "c9fa2dd0f76caf23|2018-02-10--13-40-28": { - # 'carFingerprint': TOYOTA.CAMRYH, - # 'enableCamera': False, - # 'enableDsu': False, - # }, - # TODO: missingsome combos for highlander + # TODO: missing some combos for highlander "0a302ffddbb3e3d3|2020-02-08--16-19-08": { 'carFingerprint': TOYOTA.HIGHLANDER_TSS2, 'enableCamera': True, @@ -383,21 +303,11 @@ routes = { 'enableCamera': False, 'enableDsu': False, }, - "362d23d4d5bea2fa|2018-09-02--17-03-55": { - 'carFingerprint': TOYOTA.HIGHLANDERH, - 'enableCamera': True, - 'enableDsu': True, - }, "eb6acd681135480d|2019-06-20--20-00-00": { 'carFingerprint': TOYOTA.SIENNA, 'enableCamera': True, 'enableDsu': False, }, - "362d23d4d5bea2fa|2018-08-10--13-31-40": { - 'carFingerprint': TOYOTA.HIGHLANDERH, - 'enableCamera': False, - 'enableDsu': False, - }, "2e07163a1ba9a780|2019-08-25--13-15-13": { 'carFingerprint': TOYOTA.LEXUS_IS, 'enableCamera': True, @@ -421,66 +331,45 @@ routes = { 'carFingerprint': SUBARU.IMPREZA, 'enableCamera': True, }, - # Tesla route, should result in mock car - "07cb8a788c31f645|2018-06-17--18-50-29": { - 'carFingerprint': MOCK.MOCK, + "fbbfa6af821552b9|2020-03-03--08-09-43": { + 'carFingerprint': NISSAN.XTRAIL, + 'enableCamera': True, }, - ## Route with no can data, should result in mock car. This is not supported anymore - #"bfa17080b080f3ec|2018-06-28--23-27-47": { - # 'carFingerprint': MOCK.MOCK, - #}, } passive_routes = [ - "07cb8a788c31f645|2018-06-17--18-50-29", - #"bfa17080b080f3ec|2018-06-28--23-27-47", ] forced_dashcam_routes = [ # Ford fusion "f1b4c567731f4a1b|2018-04-18--11-29-37", "f1b4c567731f4a1b|2018-04-30--10-15-35", -] -# TODO: replace all these with public routes -non_public_routes = [ - "0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019 - "3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING - "aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018 - "1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX - "9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 2018 - "b4c18bf13d5955da|2018-07-29--13-39-46", # TOYOTA C-HR HYBRID 2018 - "5a2cfe4bb362af9e|2018-02-02--23-41-07", # ACURA RDX 2018 ACURAWATCH PLUS - "362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018 - "aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018 - "215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018 - "192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019 - "34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE - "b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017 - "31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019 - "fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018 - "9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018 - "aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017 - "362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018 - "1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T - "fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 2018 - "265007255e794bce|2018-11-24--22-08-31", # CADILLAC ATS Premium Performance 2018 - "53ac3251e03f95d7|2019-01-10--13-43-32", # HYUNDAI ELANTRA LIMITED ULTIMATE 2017 - "21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L - "900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION - "975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018 - "8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017 - "a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018 - "49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017 - "d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018 - "07cb8a788c31f645|2018-06-17--18-50-29", # mock - "c9d60e5e02c04c5c|2018-01-08--16-01-49", # HONDA CR-V 2016 TOURING - "1632088eda5e6c4d|2018-06-07--08-03-18", # HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019 - "fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018 + # Nissan + "fbbfa6af821552b9|2020-03-03--08-09-43", ] # TODO: add routes for these cars -non_tested_cars = [TOYOTA.LEXUS_CTH, CHRYSLER.PACIFICA_2018, HONDA.ACCORDH] +non_tested_cars = [ + CHRYSLER.JEEP_CHEROKEE, + CHRYSLER.JEEP_CHEROKEE_2019, + CHRYSLER.PACIFICA_2018, + CHRYSLER.PACIFICA_2018_HYBRID, + GM.CADILLAC_ATS, + GM.HOLDEN_ASTRA, + GM.MALIBU, + HONDA.ACURA_RDX, + HONDA.CRV, + HONDA.RIDGELINE, + HYUNDAI.ELANTRA, + HYUNDAI.GENESIS, + HYUNDAI.KIA_SORENTO, + HYUNDAI.KIA_STINGER, + TOYOTA.CAMRYH, + TOYOTA.CHR, + TOYOTA.CHRH, + TOYOTA.HIGHLANDERH, +] if __name__ == "__main__": @@ -503,12 +392,9 @@ if __name__ == "__main__": results = {} for route, checks in routes.items(): - if route not in non_public_routes: - print("GETTING ROUTE LOGS") - get_route_log(route) - print("DONE GETTING ROUTE LOGS") - elif "UNLOGGER_PATH" not in os.environ: - continue + print("GETTING ROUTE LOGS") + get_route_log(route) + print("DONE GETTING ROUTE LOGS") params = Params() params.clear_all() @@ -529,10 +415,7 @@ if __name__ == "__main__": # Start unlogger print("Start unlogger") - if route in non_public_routes: - unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), route] - else: - unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), route, '/tmp'] + unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), route, '/tmp'] unlogger = subprocess.Popen(unlogger_cmd + ['--disable', 'frame,encodeIdx,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams', '--no-interactive'], preexec_fn=os.setsid) print("Check sockets") diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py index aeb75896d..b0c4d4510 100755 --- a/selfdrive/test/test_fingerprints.py +++ b/selfdrive/test/test_fingerprints.py @@ -2,6 +2,7 @@ import os import sys from common.basedir import BASEDIR +from selfdrive.car.fingerprints import IGNORED_FINGERPRINTS # messages reserved for CAN based ignition (see can_ignition_hook function in panda/board/drivers/can) # (addr, len) @@ -65,6 +66,9 @@ car_names = [] brand_names = [] for brand in fingerprints: for car in fingerprints[brand]: + if car in IGNORED_FINGERPRINTS: + continue + fingerprints_flat += fingerprints[brand][car] for i in range(len(fingerprints[brand][car])): car_names.append(car) @@ -77,10 +81,6 @@ valid = True for idx1, f1 in enumerate(fingerprints_flat): for idx2, f2 in enumerate(fingerprints_flat): if idx1 < idx2 and not check_fingerprint_consistency(f1, f2): - if car_names[idx1] == car_names[idx2]: - print(f"Warning, overlap in {car_names[idx1]}") - continue - valid = False print("Those two fingerprints are inconsistent {0} {1}".format(car_names[idx1], car_names[idx2])) print("") diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index e5ea32736..3e9a31758 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -2,7 +2,7 @@ import os os.environ['FAKEUPLOAD'] = "1" -from common.apk import update_apks, start_frame, pm_apply_packages, android_packages +from common.apk import update_apks, start_offroad, pm_apply_packages, android_packages from common.params import Params from common.testing import phone_only from selfdrive.manager import manager_init, manager_prepare @@ -57,7 +57,7 @@ def with_apks(): update_apks() pm_apply_packages('enable') - start_frame() + start_offroad() func() diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py new file mode 100644 index 000000000..e03ef6985 --- /dev/null +++ b/selfdrive/thermald/power_monitoring.py @@ -0,0 +1,153 @@ +import datetime +import random +import threading +import time +from statistics import mean + +from cereal import log +from selfdrive.swaglog import cloudlog + +PANDA_OUTPUT_VOLTAGE = 5.28 + + +# Parameters +def get_battery_capacity(): + return _read_param("/sys/class/power_supply/battery/capacity", int) + + +def get_battery_status(): + # This does not correspond with actual charging or not. + # If a USB cable is plugged in, it responds with 'Charging', even when charging is disabled + return _read_param("/sys/class/power_supply/battery/status", lambda x: x.strip(), '') + + +def get_battery_current(): + return _read_param("/sys/class/power_supply/battery/current_now", int) + + +def get_battery_voltage(): + return _read_param("/sys/class/power_supply/battery/voltage_now", int) + + +def get_usb_present(): + return _read_param("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False) + + +def get_battery_charging(): + # This does correspond with actually charging + return _read_param("/sys/class/power_supply/battery/charge_type", lambda x: x.strip() != "N/A", False) + + +def set_battery_charging(on): + with open('/sys/class/power_supply/battery/charging_enabled', 'w') as f: + f.write(f"{1 if on else 0}\n") + + +# Helpers +def _read_param(path, parser, default=0): + try: + with open(path) as f: + return parser(f.read()) + except Exception: + return default + + +def panda_current_to_actual_current(panda_current): + # From white/grey panda schematic + return (3.3 - (panda_current * 3.3 / 4096)) / 8.25 + + +class PowerMonitoring: + def __init__(self): + self.last_measurement_time = None # Used for integration delta + self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad + self.next_pulsed_measurement_time = None + self.integration_lock = threading.Lock() + + # Calculation tick + def calculate(self, health): + try: + now = time.time() + + # Check that time is valid + if datetime.datetime.fromtimestamp(now).year < 2019: + return + + # Only integrate when there is no ignition + # If health is None, we're probably not in a car, so we don't care + if health is None or (health.health.ignitionLine or health.health.ignitionCan): + self.last_measurement_time = None + self.power_used_uWh = 0 + return + + # First measurement, set integration time + if self.last_measurement_time is None: + self.last_measurement_time = now + return + + # Get current power draw somehow + current_power = 0 + if get_battery_status() == 'Discharging': + # If the battery is discharging, we can use this measurement + # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in + current_power = ((get_battery_voltage() / 1000000) * (get_battery_current() / 1000000)) + elif (health.health.hwType in [log.HealthData.HwType.whitePanda, log.HealthData.HwType.greyPanda]) and (health.health.current > 1): + # If white/grey panda, use the integrated current measurements if the measurement is not 0 + # If the measurement is 0, the current is 400mA or greater, and out of the measurement range of the panda + # This seems to be accurate to about 5% + current_power = (PANDA_OUTPUT_VOLTAGE * panda_current_to_actual_current(health.health.current)) + elif (self.next_pulsed_measurement_time is not None) and (self.next_pulsed_measurement_time <= now): + # TODO: Figure out why this is off by a factor of 3/4??? + FUDGE_FACTOR = 1.33 + + # Turn off charging for about 10 sec in a thread that does not get killed on SIGINT, and perform measurement here to avoid blocking thermal + def perform_pulse_measurement(now): + try: + set_battery_charging(False) + time.sleep(5) + + # Measure for a few sec to get a good average + voltages = [] + currents = [] + for i in range(6): + voltages.append(get_battery_voltage()) + currents.append(get_battery_current()) + time.sleep(1) + current_power = ((mean(voltages) / 1000000) * (mean(currents) / 1000000)) + self._perform_integration(now, current_power * FUDGE_FACTOR) + + # Enable charging again + set_battery_charging(True) + except Exception: + cloudlog.exception("Pulsed power measurement failed") + + # Start pulsed measurement and return + threading.Thread(target=perform_pulse_measurement, args=(now,)).start() + self.next_pulsed_measurement_time = None + return + + elif self.next_pulsed_measurement_time is None: + # On a charging EON with black panda, or drawing more than 400mA out of a white/grey one + # Only way to get the power draw is to turn off charging for a few sec and check what the discharging rate is + # We shouldn't do this very often, so make sure it has been some long-ish random time interval + self.next_pulsed_measurement_time = now + random.randint(120, 180) + return + else: + # Do nothing + return + + # Do the integration + self._perform_integration(now, current_power) + except Exception: + cloudlog.exception("Power monitoring calculation failed:") + + def _perform_integration(self, t, current_power): + self.integration_lock.acquire() + integration_time_h = (t - self.last_measurement_time) / 3600 + self.power_used_uWh += (current_power * 1000000) * integration_time_h + self.last_measurement_time = t + self.integration_lock.release() + + # Get the power usage + def get_power_used(self): + return int(self.power_used_uWh) diff --git a/selfdrive/thermald.py b/selfdrive/thermald/thermald.py old mode 100755 new mode 100644 similarity index 87% rename from selfdrive/thermald.py rename to selfdrive/thermald/thermald.py index 495df408b..3c760072b --- a/selfdrive/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -4,10 +4,9 @@ import json import copy import datetime import psutil -import subprocess from smbus2 import SMBus from cereal import log -from common.android import ANDROID, get_network_type +from common.android import ANDROID, get_network_type, get_network_strength from common.basedir import BASEDIR from common.params import Params, put_nonblocking from common.realtime import sec_since_boot, DT_TRML @@ -18,25 +17,34 @@ from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.loggerd.config import get_available_percent from selfdrive.pandad import get_expected_signature +from selfdrive.thermald.power_monitoring import PowerMonitoring, get_battery_capacity, get_battery_status, get_battery_current, get_battery_voltage, get_usb_present, set_battery_charging, get_battery_charging +params = Params() FW_SIGNATURE = get_expected_signature() -params = Params() import subprocess import re from selfdrive.dragonpilot.dragonconf import dp_get_last_modified ThermalStatus = log.ThermalData.ThermalStatus NetworkType = log.ThermalData.NetworkType +NetworkStrength = log.ThermalData.NetworkStrength CURRENT_TAU = 15. # 15s time constant DAYS_NO_CONNECTIVITY_MAX = 7 # do not allow to engage after a week without internet DAYS_NO_CONNECTIVITY_PROMPT = 4 # send an offroad prompt after 4 days with no internet +LEON = False +last_eon_fan_val = None + with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file: OFFROAD_ALERTS = json.load(json_file) + def read_tz(x, clip=True): + if not ANDROID: + # we don't monitor thermal on PC + return 0 try: with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: ret = int(f.read()) @@ -47,9 +55,9 @@ def read_tz(x, clip=True): return ret + def read_thermal(): - dat = messaging.new_message() - dat.init('thermal') + dat = messaging.new_message('thermal') dat.thermal.cpu0 = read_tz(5) dat.thermal.cpu1 = read_tz(7) dat.thermal.cpu2 = read_tz(10) @@ -60,7 +68,7 @@ def read_thermal(): dat.thermal.pa0 = read_tz(25) return dat -LEON = False + def setup_eon_fan(): global LEON @@ -74,11 +82,10 @@ def setup_eon_fan(): bus.write_byte_data(0x21, 0x04, 0x4) # manual override source except IOError: print("LEON detected") - #os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg") LEON = True bus.close() -last_eon_fan_val = None + def set_eon_fan(val): global LEON, last_eon_fan_val @@ -104,17 +111,19 @@ def set_eon_fan(val): bus.close() last_eon_fan_val = val + # temp thresholds to control fan speed - high hysteresis _TEMP_THRS_H = [50., 65., 80., 10000] # temp thresholds to control fan speed - low hysteresis _TEMP_THRS_L = [42.5, 57.5, 72.5, 10000] -# fan speed options -_FAN_SPEEDS = [0, 16384, 32768, 65535] -# max fan speed only allowed if battery is hot -_BAT_TEMP_THERSHOLD = 45. if params.get('DragonNoctuaMode', encoding='utf8') == "1": _FAN_SPEEDS = [65535, 65535, 65535, 65535] _BAT_TEMP_THERSHOLD = 20. +else: + # fan speed options + _FAN_SPEEDS = [0, 16384, 32768, 65535] + # max fan speed only allowed if battery is hot + _BAT_TEMP_THERSHOLD = 45. def handle_fan_eon(max_cpu_temp, bat_temp, fan_speed, ignition): @@ -132,7 +141,7 @@ def handle_fan_eon(max_cpu_temp, bat_temp, fan_speed, ignition): # no max fan speed unless battery is hot fan_speed = min(fan_speed, _FAN_SPEEDS[-2]) - set_eon_fan(fan_speed//16384) + set_eon_fan(fan_speed // 16384) return fan_speed @@ -145,6 +154,7 @@ def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition): return new_speed + def thermald_thread(): # prevent LEECO from undervoltage BATT_PERC_OFF = 10 if LEON else 3 @@ -169,6 +179,7 @@ def thermald_thread(): usb_power_prev = True network_type = NetworkType.none + network_strength = NetworkStrength.unknown current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) health_prev = None @@ -184,6 +195,7 @@ def thermald_thread(): setup_eon_fan() handle_fan = handle_fan_eon + pm = PowerMonitoring() # dragonpilot ts_last_ip = None ts_last_update_vars = 0 @@ -192,6 +204,7 @@ def thermald_thread(): ip_addr = '255.255.255.255' dragon_charging_ctrl = False + dragon_charging_ctrl_prev = False dragon_to_discharge = 70 dragon_to_charge = 60 @@ -213,27 +226,20 @@ def thermald_thread(): if (count % int(10. / DT_TRML)) == 0: try: network_type = get_network_type() - except subprocess.CalledProcessError: - pass + network_strength = get_network_strength(network_type) + except Exception: + cloudlog.exception("Error getting network status") msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0 msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent)) msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) msg.thermal.networkType = network_type - - try: - with open("/sys/class/power_supply/battery/capacity") as f: - msg.thermal.batteryPercent = int(f.read()) - with open("/sys/class/power_supply/battery/status") as f: - msg.thermal.batteryStatus = f.read().strip() - with open("/sys/class/power_supply/battery/current_now") as f: - msg.thermal.batteryCurrent = int(f.read()) - with open("/sys/class/power_supply/battery/voltage_now") as f: - msg.thermal.batteryVoltage = int(f.read()) - with open("/sys/class/power_supply/usb/present") as f: - msg.thermal.usbOnline = bool(int(f.read())) - except FileNotFoundError: - pass + msg.thermal.networkStrength = network_strength + msg.thermal.batteryPercent = get_battery_capacity() + msg.thermal.batteryStatus = get_battery_status() + msg.thermal.batteryCurrent = get_battery_current() + msg.thermal.batteryVoltage = get_battery_voltage() + msg.thermal.usbOnline = get_usb_present() # Fake battery levels on uno for frame if is_uno: @@ -258,7 +264,7 @@ def thermald_thread(): max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.) - bat_temp = msg.thermal.bat/1000. + bat_temp = msg.thermal.bat / 1000. fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition) msg.thermal.fanSpeed = fan_speed @@ -267,7 +273,7 @@ def thermald_thread(): if max_cpu_temp > 107. or bat_temp >= 63.: # onroad not allowed thermal_status = ThermalStatus.danger - elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C + elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) elif max_cpu_temp > 87.5: @@ -393,6 +399,10 @@ def thermald_thread(): started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') + # Offroad power monitoring + pm.calculate(health) + msg.thermal.offroadPowerUsage = pm.get_power_used() + msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9*(started_ts or 0)) @@ -431,30 +441,33 @@ def thermald_thread(): dp_last_modified = modified ts_last_update_vars = ts - # we update charging status once every min - if ts_last_charging_ctrl is None or ts - ts_last_charging_ctrl >= 60.: - if dragon_charging_ctrl: - if msg.thermal.batteryPercent >= dragon_to_discharge: - os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled') - if msg.thermal.batteryPercent <= dragon_to_charge: - os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') - else: - os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') - ts_last_charging_ctrl = ts + if dragon_charging_ctrl != dragon_charging_ctrl_prev: + set_battery_charging(True) + + if dragon_charging_ctrl: + if ts_last_charging_ctrl is None or ts - ts_last_charging_ctrl >= 60.: + if msg.thermal.batteryPercent >= dragon_to_discharge and get_battery_charging(): + set_battery_charging(False) + elif msg.thermal.batteryPercent <= dragon_to_charge and not get_battery_charging(): + set_battery_charging(True) + ts_last_charging_ctrl = ts + + dragon_charging_ctrl_prev = dragon_charging_ctrl # report to server once per minute if (count % int(60. / DT_TRML)) == 0: cloudlog.event("STATUS_PACKET", - count=count, - health=(health.to_dict() if health else None), - location=(location.to_dict() if location else None), - thermal=msg.to_dict()) + count=count, + health=(health.to_dict() if health else None), + location=(location.to_dict() if location else None), + thermal=msg.to_dict()) count += 1 -def main(gctx=None): +def main(): thermald_thread() + if __name__ == "__main__": main() diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 95a3c6683..187414724 100644 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -101,7 +101,7 @@ def report_tombstone(fn, client): cloudlog.error({'tombstone': message}) -def main(gctx=None): +def main(): initial_tombstones = set(get_tombstones()) client = Client('https://980a0cba712a4c3593c33c78a12446e1:fecab286bcaf4dba8b04f7cff0188e2d@sentry.io/1488600', diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 5dbf8016d..e0c353b43 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,7 +1,7 @@ Import('env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal') -src = ['ui.cc', 'paint.cc', '#phonelibs/nanovg/nanovg.c'] -libs = [common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', cereal, 'json', messaging, gpucommon, visionipc] +src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] +libs = [common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', cereal, messaging, gpucommon, visionipc] if arch == "aarch64": src += ['sound.cc', 'slplay.c'] @@ -11,7 +11,7 @@ else: src += ['linux.cc'] libs += ['pthread', 'glfw'] linkflags = [] - + env.Program('_ui', src, LINKFLAGS=linkflags, LIBS=libs) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 780dc02a9..28fe11f97 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -108,9 +108,23 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz, nvgRestore(s->vg); } -static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { - const UIScene *scene = &s->scene; +static void draw_lead(UIState *s, float d_rel, float v_rel, float y_rel){ + // Draw lead car indicator + float fillAlpha = 0; + float speedBuff = 10.; + float leadBuff = 40.; + if (d_rel < leadBuff) { + fillAlpha = 255*(1.0-(d_rel/leadBuff)); + if (v_rel < 0) { + fillAlpha += 255*(-1*(v_rel/speedBuff)); + } + fillAlpha = (int)(fmin(fillAlpha, 255)); + } + draw_chevron(s, d_rel, y_rel, 25, + nvgRGBA(201, 34, 49, fillAlpha), nvgRGBA(218, 202, 37, 255)); +} +static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { nvgSave(s->vg); nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x @@ -204,7 +218,7 @@ static void update_all_track_data(UIState *s) { static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { -const UIScene *scene = &s->scene; + const UIScene *scene = &s->scene; const PathData path = scene->model.path; const float *mpc_x_coords = &scene->mpc_x[0]; const float *mpc_y_coords = &scene->mpc_y[0]; @@ -416,20 +430,13 @@ static void ui_draw_world(UIState *s) { // Draw lane edges and vision/mpc tracks ui_draw_vision_lanes(s); - if (s->dragon_ui_lead && scene->lead_status) { - // Draw lead car indicator - float fillAlpha = 0; - float speedBuff = 10.; - float leadBuff = 40.; - if (scene->lead_d_rel < leadBuff) { - fillAlpha = 255*(1.0-(scene->lead_d_rel/leadBuff)); - if (scene->lead_v_rel < 0) { - fillAlpha += 255*(-1*(scene->lead_v_rel/speedBuff)); - } - fillAlpha = (int)(fmin(fillAlpha, 255)); + if (s->dragon_ui_lead) { + if (scene->lead_status) { + draw_lead(s, scene->lead_d_rel, scene->lead_v_rel, scene->lead_y_rel); + } + if ((scene->lead_status2) && (fabs(scene->lead_d_rel - scene->lead_d_rel2) > 3.0)) { + draw_lead(s, scene->lead_d_rel2, scene->lead_v_rel2, scene->lead_y_rel2); } - draw_chevron(s, scene->lead_d_rel+2.7, scene->lead_y_rel, 25, - nvgRGBA(201, 34, 49, fillAlpha), nvgRGBA(218, 202, 37, 255)); } } @@ -659,7 +666,7 @@ static void ui_draw_vision_speed(UIState *s) { } nvgFontFace(s->vg, "sans-bold"); nvgFontSize(s->vg, 96*2.5); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFillColor(s->vg, s->scene.brakeLights? COLOR_RED : COLOR_WHITE); nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL); nvgFontFace(s->vg, "sans-regular"); @@ -831,11 +838,19 @@ static void ui_draw_infobar(UIState *s) { char infobar[100]; // create time string - char date_time[20]; + char date_time[17]; time_t rawtime = time(NULL); struct tm timeinfo; localtime_r(&rawtime, &timeinfo); - strftime(date_time, sizeof(date_time),"%F %T", &timeinfo); + strftime(date_time, sizeof(date_time),"%D %T", &timeinfo); + + // Create temp string + char temp[6]; + snprintf(temp, sizeof(temp), "%02d°C", s->scene.paTemp); + + // create battery percentage string + char battery[4]; + snprintf(battery, sizeof(battery), "%02d%%", s->scene.batteryPercent); if (s->dragon_ui_dev_mini) { char rel_steer[9]; @@ -858,8 +873,10 @@ static void ui_draw_infobar(UIState *s) { snprintf( infobar, sizeof(infobar), - "%s /REL: %s /DES: %s /DIS: %s", + "%s /TMP: %s /BAT: %s /REL: %s /DES: %s /DIS: %s", date_time, + temp, + battery, rel_steer, des_steer, lead_dist @@ -868,17 +885,19 @@ static void ui_draw_infobar(UIState *s) { snprintf( infobar, sizeof(infobar), - "%s", - date_time + "%s /TMP: %s /BAT: %s", + date_time, + temp, + battery ); } nvgBeginPath(s->vg); - nvgRoundedRect(s->vg, rect_x, rect_y, rect_w, rect_h, 15); + nvgRect(s->vg, rect_x, rect_y, rect_w, rect_h); nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 180)); nvgFill(s->vg); - nvgFontSize(s->vg, hasSidebar? 43:50); + nvgFontSize(s->vg, hasSidebar? 35:42); nvgFontFace(s->vg, "courbd"); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 180)); nvgTextAlign(s->vg, NVG_ALIGN_CENTER); @@ -1230,6 +1249,7 @@ static void ui_draw_blank(UIState *s) { } void ui_draw(UIState *s) { + ui_draw_sidebar(s); if (s->vision_connected && s->active_app == cereal_UiLayoutState_App_home && s->status != STATUS_STOPPED) { ui_draw_vision(s); } else { @@ -1338,6 +1358,25 @@ void ui_nvg_init(UIState *s) { assert(s->img_map >= 0); s->img_map = nvgCreateImage(s->vg, "../assets/img_map.png", 1); + assert(s->img_button_settings >= 0); + s->img_button_settings = nvgCreateImage(s->vg, "../assets/images/button_settings.png", 1); + + assert(s->img_button_home >= 0); + s->img_button_home = nvgCreateImage(s->vg, "../assets/images/button_home.png", 1); + + assert(s->img_battery >= 0); + s->img_battery = nvgCreateImage(s->vg, "../assets/images/battery.png", 1); + + assert(s->img_battery_charging >= 0); + s->img_battery_charging = nvgCreateImage(s->vg, "../assets/images/battery_charging.png", 1); + + for(int i=0;i<=5;++i) { + assert(s->img_network[i] >= 0); + char network_asset[32]; + snprintf(network_asset, sizeof(network_asset), "../assets/images/network_%d.png", i); + s->img_network[i] = nvgCreateImage(s->vg, network_asset, 1); + } + // init gl s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader); assert(s->frame_program); diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc new file mode 100644 index 000000000..cd5ce49b6 --- /dev/null +++ b/selfdrive/ui/sidebar.cc @@ -0,0 +1,265 @@ +#include +#include +#include +#include "ui.hpp" + +static void ui_draw_sidebar_background(UIState *s, bool hasSidebar) { + int sbr_x = hasSidebar ? 0 : -(sbr_w) + bdr_s * 2; + + nvgBeginPath(s->vg); + nvgRect(s->vg, sbr_x, 0, sbr_w, vwp_h); + nvgFillColor(s->vg, COLOR_BLACK_ALPHA); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_settings_button(UIState *s, bool hasSidebar) { + bool settingsActive = s->active_app == cereal_UiLayoutState_App_settings; + const int settings_btn_xr = hasSidebar ? settings_btn_x : -(sbr_w); + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, settings_btn_xr, settings_btn_y, + settings_btn_w, settings_btn_h, 0, s->img_button_settings, settingsActive ? 1.0f : 0.65f); + nvgRect(s->vg, settings_btn_xr, settings_btn_y, settings_btn_w, settings_btn_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_home_button(UIState *s, bool hasSidebar) { + bool homeActive = s->active_app == cereal_UiLayoutState_App_home; + const int home_btn_xr = hasSidebar ? home_btn_x : -(sbr_w); + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, home_btn_xr, home_btn_y, + home_btn_w, home_btn_h, 0, s->img_button_home, homeActive ? 1.0f : 0.65f); + nvgRect(s->vg, home_btn_xr, home_btn_y, home_btn_w, home_btn_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_network_strength(UIState *s, bool hasSidebar) { + const int network_img_h = 27; + const int network_img_w = 176; + const int network_img_x = hasSidebar ? 58 : -(sbr_w); + const int network_img_y = 196; + const int network_img = s->scene.networkType == cereal_ThermalData_NetworkType_none ? + s->img_network[0] : s->img_network[s->scene.networkStrength + 1]; + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, network_img_x, network_img_y, + network_img_w, network_img_h, 0, network_img, 1.0f); + nvgRect(s->vg, network_img_x, network_img_y, network_img_w, network_img_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_ip_addr(UIState *s, bool hasSidebar) { + const int network_ip_w = 176; + const int network_ip_x = hasSidebar ? 58 : -(sbr_w); + const int network_ip_y = 255; + + char network_ip_str[20]; + snprintf(network_ip_str, sizeof(network_ip_str), "%s", s->scene.ipAddr); + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 32); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, network_ip_x, network_ip_y, network_ip_w, network_ip_str, NULL); +} + +static void ui_draw_sidebar_battery_icon(UIState *s, bool hasSidebar) { + const int battery_img_h = 36; + const int battery_img_w = 76; + const int battery_img_x = hasSidebar ? 160 : -(sbr_w); + const int battery_img_y = 255; + + int battery_img = strcmp(s->scene.batteryStatus, "Charging") == 0 ? + s->img_battery_charging : s->img_battery; + + nvgBeginPath(s->vg); + nvgRect(s->vg, battery_img_x + 6, battery_img_y + 5, + ((battery_img_w - 19) * (s->scene.batteryPercent * 0.01)), battery_img_h - 11); + nvgFillColor(s->vg, COLOR_WHITE); + nvgFill(s->vg); + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, battery_img_x, battery_img_y, + battery_img_w, battery_img_h, 0, battery_img, 1.0f); + nvgRect(s->vg, battery_img_x, battery_img_y, battery_img_w, battery_img_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_battery_text(UIState *s, bool hasSidebar) { + const int battery_img_h = 36; + const int battery_img_w = 76; + const int battery_img_x = hasSidebar ? 150 : -(sbr_w); + const int battery_img_y = 305; + + char battery_str[6]; + snprintf(battery_str, sizeof(battery_str), "%d%%", s->scene.batteryPercent); + nvgFillColor(s->vg, strcmp(s->scene.batteryStatus, "Charging") == 0? COLOR_GREEN : s->scene.batteryPercent <= 50? COLOR_YELLOW : s->scene.batteryPercent <= 15? COLOR_RED : COLOR_WHITE); + nvgFontSize(s->vg, 40); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, battery_img_x, battery_img_y, battery_img_w, battery_str, NULL); +} + +static void ui_draw_sidebar_network_type(UIState *s, bool hasSidebar) { + const int network_x = hasSidebar ? 50 : -(sbr_w); + const int network_y = 303; + const int network_w = 100; + const int network_h = 100; + const char *network_types[6] = {"--", "WiFi", "2G", "3G", "4G", "5G"}; + char network_type_str[32]; + + if (s->scene.networkType <= 5) { + snprintf(network_type_str, sizeof(network_type_str), "%s", network_types[s->scene.networkType]); + } + + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 48); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, network_x, network_y, network_w, network_type_str, NULL); +} + +static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char* value_str, const int severity, const int y_offset, const char* message_str, bool hasSidebar) { + const int metric_x = hasSidebar ? 30 : -(sbr_w); + const int metric_y = 338 + y_offset; + const int metric_w = 240; + const int metric_h = message_str ? strlen(message_str) > 8 ? 124 : 100 : 148; + NVGcolor status_color; + + if (severity == 0) { + status_color = COLOR_WHITE; + } else if (severity == 1) { + status_color = COLOR_YELLOW; + } else if (severity > 1) { + status_color = COLOR_RED; + } + + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, metric_x, metric_y, metric_w, metric_h, 20); + nvgStrokeColor(s->vg, severity > 0 ? COLOR_WHITE : COLOR_WHITE_ALPHA); + nvgStrokeWidth(s->vg, 2); + nvgStroke(s->vg); + + nvgBeginPath(s->vg); + nvgRoundedRectVarying(s->vg, metric_x + 6, metric_y + 6, 18, metric_h - 12, 25, 0, 0, 25); + nvgFillColor(s->vg, status_color); + nvgFill(s->vg); + + if (!message_str) { + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 78); + nvgFontFace(s->vg, "sans-bold"); + nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, metric_x + 50, metric_y + 50, metric_w - 60, value_str, NULL); + + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 48); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, metric_x + 50, metric_y + 50 + 66, metric_w - 60, label_str, NULL); + } else { + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 48); + nvgFontFace(s->vg, "sans-bold"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, metric_x + 35, metric_y + (strlen(message_str) > 8 ? 40 : 50), metric_w - 50, message_str, NULL); + } +} + +static void ui_draw_sidebar_storage_metric(UIState *s, bool hasSidebar) { + int storage_severity; + char storage_label_str[32]; + char storage_value_str[32]; + char storage_value_unit[32]; + const int storage_y_offset = 0; + const float storage_pct = ceilf((1.0 - s->scene.freeSpace) * 100); + + if (storage_pct < 75.0) { + storage_severity = 0; + } else if (storage_pct >= 75.0 && storage_pct < 87.0) { + storage_severity = 1; + } else if (storage_pct >= 87.0) { + storage_severity = 2; + } + + snprintf(storage_value_str, sizeof(storage_value_str), "%d", (int)storage_pct); + snprintf(storage_value_unit, sizeof(storage_value_unit), "%s", "%"); + snprintf(storage_label_str, sizeof(storage_label_str), "%s", "STORAGE"); + strcat(storage_value_str, storage_value_unit); + + ui_draw_sidebar_metric(s, storage_label_str, storage_value_str, storage_severity, storage_y_offset, NULL, hasSidebar); +} + +static void ui_draw_sidebar_temp_metric(UIState *s, bool hasSidebar) { + int temp_severity; + char temp_label_str[32]; + char temp_value_str[32]; + char temp_value_unit[32]; + const int temp_y_offset = 148 + 32; + + if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_green) { + temp_severity = 0; + } else if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_yellow) { + temp_severity = 1; + } else if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_red) { + temp_severity = 2; + } else if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_danger) { + temp_severity = 3; + } + + snprintf(temp_value_str, sizeof(temp_value_str), "%d", s->scene.paTemp); + snprintf(temp_value_unit, sizeof(temp_value_unit), "%s", "°C"); + snprintf(temp_label_str, sizeof(temp_label_str), "%s", "TEMP"); + strcat(temp_value_str, temp_value_unit); + + ui_draw_sidebar_metric(s, temp_label_str, temp_value_str, temp_severity, temp_y_offset, NULL, hasSidebar); +} + +static void ui_draw_sidebar_panda_metric(UIState *s, bool hasSidebar) { + int panda_severity; + char panda_message_str[32]; + const int panda_y_offset = (148 + 32) * 2; + + if (s->scene.hwType == cereal_HealthData_HwType_unknown) { + panda_severity = 2; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "NO PANDA"); + } else if (s->scene.hwType == cereal_HealthData_HwType_whitePanda) { + panda_severity = 0; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA\nACTIVE"); + } else if ( + (s->scene.hwType == cereal_HealthData_HwType_greyPanda) || + (s->scene.hwType == cereal_HealthData_HwType_blackPanda) || + (s->scene.hwType == cereal_HealthData_HwType_uno)) { + if (s->scene.satelliteCount == -1) { + panda_severity = 0; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA\nACTIVE"); + } else { + if (s->scene.satelliteCount < 6) { + panda_severity = 1; + } else if (s->scene.satelliteCount >= 6) { + panda_severity = 0; + } + snprintf(panda_message_str, sizeof(panda_message_str), "%s %d", "PANDA\nGPS:", s->scene.satelliteCount); + } + } + + ui_draw_sidebar_metric(s, NULL, NULL, panda_severity, panda_y_offset, panda_message_str, hasSidebar); +} + +void ui_draw_sidebar(UIState *s) { + bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; + ui_draw_sidebar_background(s, hasSidebar); + ui_draw_sidebar_settings_button(s, hasSidebar); + ui_draw_sidebar_home_button(s, hasSidebar); + ui_draw_sidebar_network_strength(s, hasSidebar); + ui_draw_sidebar_ip_addr(s, hasSidebar); + ui_draw_sidebar_battery_text(s, hasSidebar); + ui_draw_sidebar_network_type(s, hasSidebar); + ui_draw_sidebar_storage_metric(s, hasSidebar); + ui_draw_sidebar_temp_metric(s, hasSidebar); + ui_draw_sidebar_panda_metric(s, hasSidebar); +} diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 90b879daa..ca6bfe13c 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -6,7 +6,6 @@ #include #include -#include #include #include "common/util.h" @@ -58,6 +57,45 @@ static void set_awake(UIState *s, bool awake) { #endif } +static void navigate_to_settings(UIState *s) { +#ifdef QCOM + system("am broadcast -a 'ai.comma.plus.SidebarSettingsTouchUpInside'"); +#else + // computer UI doesn't have offroad settings +#endif +} + +static void navigate_to_home(UIState *s) { +#ifdef QCOM + system("am broadcast -a 'ai.comma.plus.HomeButtonTouchUpInside'"); +#else + // computer UI doesn't have offroad home +#endif +} + +static void handle_sidebar_touch(UIState *s, int touch_x, int touch_y) { + if (!s->scene.uilayout_sidebarcollapsed && touch_x <= sbr_w) { + if (touch_x >= settings_btn_x && touch_x < (settings_btn_x + settings_btn_w) + && touch_y >= settings_btn_y && touch_y < (settings_btn_y + settings_btn_h)) { + navigate_to_settings(s); + } + if (touch_x >= home_btn_x && touch_x < (home_btn_x + home_btn_w) + && touch_y >= home_btn_y && touch_y < (home_btn_y + home_btn_h)) { + navigate_to_home(s); + if (s->vision_connected) { + s->scene.uilayout_sidebarcollapsed = true; + } + } + } +} + +static void handle_vision_touch(UIState *s, int touch_x, int touch_y) { + if (s->vision_connected && (touch_x >= s->scene.ui_viz_rx - bdr_s) + && (s->active_app != cereal_UiLayoutState_App_settings)) { + s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed; + } +} + volatile sig_atomic_t do_exit = 0; static void set_do_exit(int sig) { do_exit = 1; @@ -111,6 +149,9 @@ static void ui_init(UIState *s) { s->uilayout_sock = SubSocket::create(s->ctx, "uiLayoutState"); s->livecalibration_sock = SubSocket::create(s->ctx, "liveCalibration"); s->radarstate_sock = SubSocket::create(s->ctx, "radarState"); + s->thermal_sock = SubSocket::create(s->ctx, "thermal"); + s->health_sock = SubSocket::create(s->ctx, "health"); + s->ubloxgnss_sock = SubSocket::create(s->ctx, "ubloxGnss"); s->carstate_sock = SubSocket::create(s->ctx, "carState"); assert(s->model_sock != NULL); @@ -118,6 +159,9 @@ static void ui_init(UIState *s) { assert(s->uilayout_sock != NULL); assert(s->livecalibration_sock != NULL); assert(s->radarstate_sock != NULL); + assert(s->thermal_sock != NULL); + assert(s->health_sock != NULL); + assert(s->ubloxgnss_sock != NULL); assert(s->carstate_sock != NULL); s->poller = Poller::create({ @@ -126,6 +170,9 @@ static void ui_init(UIState *s) { s->uilayout_sock, s->livecalibration_sock, s->radarstate_sock, + s->thermal_sock, + s->health_sock, + s->ubloxgnss_sock, s->carstate_sock }); @@ -419,6 +466,11 @@ void handle_message(UIState *s, Message * msg) { s->scene.lead_d_rel = leaddatad.dRel; s->scene.lead_y_rel = leaddatad.yRel; s->scene.lead_v_rel = leaddatad.vRel; + cereal_read_RadarState_LeadData(&leaddatad, datad.leadTwo); + s->scene.lead_status2 = leaddatad.status; + s->scene.lead_d_rel2 = leaddatad.dRel; + s->scene.lead_y_rel2 = leaddatad.yRel; + s->scene.lead_v_rel2 = leaddatad.vRel; s->livempc_or_radarstate_changed = true; } else if (eventd.which == cereal_Event_liveCalibration) { s->scene.world_objects_visible = true; @@ -457,23 +509,36 @@ void handle_message(UIState *s, Message * msg) { cereal_read_UiLayoutState(&datad, eventd.uiLayoutState); s->active_app = datad.activeApp; s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed; - s->scene.uilayout_mapenabled = datad.mapEnabled; - - bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; - bool mapEnabled = s->scene.uilayout_mapenabled; - if (mapEnabled) { - s->scene.ui_viz_rx = hasSidebar ? (box_x+nav_w) : (box_x+nav_w-(bdr_s*4)); - s->scene.ui_viz_rw = hasSidebar ? (box_w-nav_w) : (box_w-nav_w+(bdr_s*4)); - s->scene.ui_viz_ro = -(sbr_w + 4*bdr_s); - } else { - s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2); - s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2)); - s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6*bdr_s) : 0; - } } else if (eventd.which == cereal_Event_liveMapData) { struct cereal_LiveMapData datad; cereal_read_LiveMapData(&datad, eventd.liveMapData); s->scene.map_valid = datad.mapValid; + } else if (eventd.which == cereal_Event_thermal) { + struct cereal_ThermalData datad; + cereal_read_ThermalData(&datad, eventd.thermal); + + s->scene.networkType = datad.networkType; + s->scene.networkStrength = datad.networkStrength; + s->scene.batteryPercent = datad.batteryPercent; + snprintf(s->scene.batteryStatus, sizeof(s->scene.batteryStatus), "%s", datad.batteryStatus.str); + s->scene.freeSpace = datad.freeSpace; + s->scene.thermalStatus = datad.thermalStatus; + s->scene.paTemp = datad.pa0; + snprintf(s->scene.ipAddr, sizeof(s->scene.ipAddr), "%s", datad.ipAddr.str); + } else if (eventd.which == cereal_Event_ubloxGnss) { + struct cereal_UbloxGnss datad; + cereal_read_UbloxGnss(&datad, eventd.ubloxGnss); + if (datad.which == cereal_UbloxGnss_measurementReport) { + struct cereal_UbloxGnss_MeasurementReport reportdatad; + cereal_read_UbloxGnss_MeasurementReport(&reportdatad, datad.measurementReport); + s->scene.satelliteCount = reportdatad.numMeas; + } + } else if (eventd.which == cereal_Event_health) { + struct cereal_HealthData datad; + cereal_read_HealthData(&datad, eventd.health); + + s->scene.hwType = datad.hwType; + s->hardware_timeout = 5*30; // 5 seconds at 30 fps } else if (eventd.which == cereal_Event_carState) { struct cereal_CarState datad; cereal_read_CarState(&datad, eventd.carState); @@ -483,10 +548,29 @@ void handle_message(UIState *s, Message * msg) { } s->scene.leftBlinker = datad.leftBlinker; s->scene.rightBlinker = datad.rightBlinker; + s->scene.brakeLights = datad.brakeLights; } capn_free(&ctx); } +static void check_messages(UIState *s) { + while(true) { + auto polls = s->poller->poll(0); + + if (polls.size() == 0) + break; + + for (auto sock : polls){ + Message * msg = sock->receive(); + if (msg == NULL) continue; + + handle_message(s, msg); + + delete msg; + } + } +} + static void ui_update(UIState *s) { int err; @@ -556,7 +640,7 @@ static void ui_update(UIState *s) { assert(glGetError() == GL_NO_ERROR); - // Default UI Measurements (Assumes sidebar collapsed) + s->scene.uilayout_sidebarcollapsed = true; s->scene.ui_viz_rx = (box_x-sbr_w+bdr_s*2); s->scene.ui_viz_rw = (box_w+sbr_w-(bdr_s*2)); s->scene.ui_viz_ro = 0; @@ -637,23 +721,7 @@ static void ui_update(UIState *s) { break; } // peek and consume all events in the zmq queue, then return. - while(true) { - auto polls = s->poller->poll(0); - - if (polls.size() == 0) - return; - - for (auto sock : polls){ - Message * msg = sock->receive(); - if (msg == NULL) continue; - - set_awake(s, true); - - handle_message(s, msg); - - delete msg; - } - } + check_messages(s); } static int vision_subscribe(int fd, VisionPacket *rp, VisionStreamType type) { @@ -794,7 +862,6 @@ fail: return NULL; } - static void* bg_thread(void* args) { UIState *s = (UIState*)args; set_thread_name("bg"); @@ -875,7 +942,6 @@ int main(int argc, char* argv[]) { TouchState touch = {0}; touch_init(&touch); s->touch_fd = touch.fd; - ui_sound_init(); // light sensor scaling params @@ -892,6 +958,9 @@ int main(int argc, char* argv[]) { set_volume(MIN_VOLUME); s->volume_timeout = 5 * UI_FREQ; int draws = 0; + + s->scene.satelliteCount = -1; + while (!do_exit) { bool should_swap = false; if (!s->vision_connected) { @@ -909,34 +978,42 @@ int main(int argc, char* argv[]) { if (smooth_brightness > 255) smooth_brightness = 255; set_brightness(s, (int)smooth_brightness); - if (!s->vision_connected) { - // Car is not started, keep in idle state and awake on touch events - zmq_pollitem_t polls[1] = {{0}}; - polls[0].fd = s->touch_fd; - polls[0].events = ZMQ_POLLIN; - int ret = zmq_poll(polls, 1, 0); - if (ret < 0){ - if (errno == EINTR) continue; - LOGW("poll failed (%d)", ret); - } else if (ret > 0) { - // awake on any touch - int touch_x = -1, touch_y = -1; - int touched = touch_read(&touch, &touch_x, &touch_y); - if (touched == 1) { - set_awake(s, true); - } + // resize vision for collapsing sidebar + const bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; + s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x - sbr_w + (bdr_s * 2)); + s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w + sbr_w - (bdr_s * 2)); + s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6 * bdr_s) : 0; + + if (s->vision_connected && s->dragon_waze_mode) { + // always collapsed sidebar when vision is connect and in waze mode + s->scene.uilayout_sidebarcollapsed = true; + } else { + // poll for touch events + int touch_x = -1, touch_y = -1; + int touched = touch_poll(&touch, &touch_x, &touch_y, 0); + if (touched == 1) { + set_awake(s, true); + handle_sidebar_touch(s, touch_x, touch_y); + handle_vision_touch(s, touch_x, touch_y); } + } + + if (!s->vision_connected) { if (s->status != STATUS_STOPPED) { update_status(s, STATUS_STOPPED); } + check_messages(s); } else { + set_awake(s, true); if (s->status == STATUS_STOPPED) { update_status(s, STATUS_DISENGAGED); } // Car started, fetch a new rgb image from ipc and peek for zmq events. ui_update(s); - if(!s->vision_connected) { + if (!s->vision_connected) { // Visiond process is just stopped, force a redraw to make screen blank again. + s->scene.satelliteCount = -1; + s->scene.uilayout_sidebarcollapsed = false; ui_draw(s); glFinish(); should_swap = true; @@ -950,8 +1027,15 @@ int main(int argc, char* argv[]) { set_awake(s, false); } - // Don't waste resources on drawing in case screen is off or car is not started. - if (s->awake && s->vision_connected) { + // manage hardware disconnect + if (s->hardware_timeout > 0) { + s->hardware_timeout--; + } else { + s->scene.hwType = cereal_HealthData_HwType_unknown; + } + + // Don't waste resources on drawing in case screen is off + if (s->awake) { ui_draw(s); glFinish(); should_swap = true; diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 7d885cf60..c4986b687 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -38,6 +38,13 @@ #define ALERTSIZE_MID 2 #define ALERTSIZE_FULL 3 +#define COLOR_BLACK_ALPHA nvgRGBA(0, 0, 0, 85) +#define COLOR_WHITE nvgRGBA(255, 255, 255, 255) +#define COLOR_WHITE_ALPHA nvgRGBA(255, 255, 255, 85) +#define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) +#define COLOR_RED nvgRGBA(201, 34, 49, 255) +#define COLOR_GREEN nvgRGBA(34, 201, 49, 255) + #ifndef QCOM #define UI_60FPS #endif @@ -60,6 +67,14 @@ const int viz_w = vwp_w-(bdr_s*2); const int header_h = 420; const int footer_h = 280; const int footer_y = vwp_h-bdr_s-footer_h; +const int settings_btn_h = 117; +const int settings_btn_w = 200; +const int settings_btn_x = 50; +const int settings_btn_y = 35; +const int home_btn_h = 180; +const int home_btn_w = 180; +const int home_btn_x = 60; +const int home_btn_y = vwp_h - home_btn_h - 40; const int UI_FREQ = 30; // Hz @@ -116,6 +131,9 @@ typedef struct UIScene { int lead_status; float lead_d_rel, lead_y_rel, lead_v_rel; + int lead_status2; + float lead_d_rel2, lead_y_rel2, lead_v_rel2; + int front_box_x, front_box_y, front_box_width, front_box_height; uint64_t alert_ts; @@ -129,14 +147,26 @@ typedef struct UIScene { // Used to show gps planner status bool gps_planner_active; + uint8_t networkType; + uint8_t networkStrength; + int batteryPercent; + char batteryStatus[64]; + float freeSpace; + uint8_t thermalStatus; + int paTemp; + int hwType; + int satelliteCount; + // dragonpilot // for minimal UI float angleSteersDes; float angleSteers; + char ipAddr[20]; // for blinker, from kegman bool leftBlinker; bool rightBlinker; + bool brakeLights; int blinker_blinkingrate; } UIScene; @@ -175,6 +205,11 @@ typedef struct UIState { int img_turn; int img_face; int img_map; + int img_button_settings; + int img_button_home; + int img_battery; + int img_battery_charging; + int img_network[6]; // sockets Context *ctx; @@ -184,7 +219,11 @@ typedef struct UIState { SubSocket *radarstate_sock; SubSocket *map_data_sock; SubSocket *uilayout_sock; + SubSocket *thermal_sock; + SubSocket *health_sock; + SubSocket *ubloxgnss_sock; Poller * poller; + Poller * ublox_poller; int active_app; @@ -228,6 +267,7 @@ typedef struct UIState { int is_metric_timeout; int longitudinal_control_timeout; int limit_set_speed_timeout; + int hardware_timeout; bool controls_seen; @@ -257,7 +297,7 @@ typedef struct UIState { model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2]; track_vertices_data track_vertices[2]; - + // dragonpilot SubSocket *carstate_sock; int dragon_ui_speed_timeout; @@ -295,8 +335,9 @@ typedef struct UIState { // API void ui_draw_vision_alert(UIState *s, int va_size, int va_color, - const char* va_text1, const char* va_text2); + const char* va_text1, const char* va_text2); void ui_draw(UIState *s); +void ui_draw_sidebar(UIState *s); void ui_nvg_init(UIState *s); #endif diff --git a/selfdrive/updated.py b/selfdrive/updated.py index b25f53f15..1a3aad100 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -292,7 +292,7 @@ def attempt_update(): set_update_available_params(new_version=new_version) -def main_bak(gctx=None): +def main(): update_failed_count = 0 overlay_init_done = False wait_helper = WaitTimeHelper() diff --git a/selfdrive/version.py b/selfdrive/version.py index 77dc26668..b45fe384b 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -4,28 +4,28 @@ import subprocess from selfdrive.swaglog import cloudlog -def get_git_commit(): +def get_git_commit(default=None): try: return subprocess.check_output(["git", "rev-parse", "HEAD"], encoding='utf8').strip() except subprocess.CalledProcessError: - return None + return default -def get_git_branch(): +def get_git_branch(default=None): try: return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], encoding='utf8').strip() except subprocess.CalledProcessError: - return None + return default -def get_git_full_branchname(): +def get_git_full_branchname(default=None): try: return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], encoding='utf8').strip() except subprocess.CalledProcessError: - return None + return default -def get_git_remote(): +def get_git_remote(default=None): try: local_branch = subprocess.check_output(["git", "name-rev", "--name-only", "HEAD"], encoding='utf8').strip() tracking_remote = subprocess.check_output(["git", "config", "branch." + local_branch + ".remote"], encoding='utf8').strip() @@ -36,7 +36,7 @@ def get_git_remote(): # Not on a branch, fallback return subprocess.check_output(["git", "config", "--get", "remote.origin.url"], encoding='utf8').strip() except subprocess.CalledProcessError: - return None + return default with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")) as _versionf: