diff --git a/README.md b/README.md index 18b8a9402..6faf4b5af 100644 --- a/README.md +++ b/README.md @@ -62,6 +62,7 @@ Supported Cars | ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------| | Acura | ILX 2016-17 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | Nidec | | Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | Nidec | +| Buick3 | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Chevrolet3| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Cadillac3 | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| @@ -81,19 +82,20 @@ Supported Cars | Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom6| | Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6| | Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom6| +| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| | Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6| | Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| | Lexus | RX Hybrid 2016-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Camry 20184 | All | Yes | Stock | 0mph5 | 0mph | Toyota | | Toyota | C-HR 2017-184 | All | Yes | Stock | 0mph | 0mph | Toyota | -| Toyota | Corolla 2017-18 | All | Yes | Yes2| 20mph | 0mph | Toyota | +| Toyota | Corolla 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Highlander 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Highlander Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Prius 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Prius Prime 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph | 0mph | Toyota | -| Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph | 0mph | Toyota | +| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | +| Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | 1[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.ai](https://comma.ai)*** @@ -166,7 +168,7 @@ Directory structure ├── sensord # IMU / GPS interface code ├── test # Car simulator running code through virtual maneuvers ├── ui # The UI - └── visiond # Embedded vision pipeline + └── visiond # Vision pipeline To understand how the services interact, see `selfdrive/service_list.yaml` diff --git a/RELEASES.md b/RELEASES.md index 482f20091..a41dffa06 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,10 +1,21 @@ +Version 0.5.9 (2019-02-10) +======================== + * Improve calibration using a dedicated neural network + * Abstract planner in its own process to remove lags in controls process + * Improve speed limits with country/region defaults by road type + * Reduce mapd data usage with gzip thanks to eFiniLan + * Zip log files in the background to reduce disk usage + * Kia Optima support thanks to emmertex! + * Buick Regal 2018 support thanks to HOYS! + * Comma pedal support for Toyota thanks to wocsor! Note: tuning needed and not maintained by comma + Version 0.5.8 (2019-01-17) ======================== * Open sourced visiond * Auto-slowdown for upcoming turns * Chrysler/Jeep/Fiat support thanks to adhintz! * Honda Civic 2019 support thanks to csouers! - * Improved use of car display in Toyota thanks to arne182! + * Improve use of car display in Toyota thanks to arne182! * No data upload when connected to Android or iOS hotspots and "Enable Upload Over Cellular" setting is off * EON stops charging when 12V battery drops below 11.8V diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk index 02c4f181f..6df468220 100644 Binary files a/apk/ai.comma.plus.frame.apk and b/apk/ai.comma.plus.frame.apk differ diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index ac34dca2b..0890624be 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/cereal/log.capnp b/cereal/log.capnp index 0ebf6a730..eb6ea7632 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -374,6 +374,7 @@ struct Live100Data { l20MonoTimeDEPRECATED @17 :UInt64; mdMonoTimeDEPRECATED @18 :UInt64; planMonoTime @28 :UInt64; + pathPlanMonoTime @50 :UInt64; state @31 :ControlState; vEgo @0 :Float32; @@ -401,6 +402,7 @@ struct Live100Data { cumLagMs @15 :Float32; startMonoTime @48 :UInt64; mapValid @49 :Bool; + forceDecel @51 :Bool; enabled @19 :Bool; active @36 :Bool; @@ -546,12 +548,12 @@ struct Plan { events @13 :List(Car.CarEvent); # lateral, 3rd order polynomial - lateralValid @0 :Bool; - dPoly @1 :List(Float32); - laneWidth @11 :Float32; + lateralValidDEPRECATED @0 :Bool; + dPolyDEPRECATED @1 :List(Float32); + laneWidthDEPRECATED @11 :Float32; # longitudinal - longitudinalValid @2 :Bool; + longitudinalValidDEPRECATED @2 :Bool; vCruise @16 :Float32; aCruise @17 :Float32; vTarget @3 :Float32; @@ -560,10 +562,14 @@ struct Plan { aTargetMinDEPRECATED @4 :Float32; aTargetMaxDEPRECATED @5 :Float32; aTarget @18 :Float32; + + vStart @26 :Float32; + aStart @27 :Float32; + jerkFactor @6 :Float32; hasLead @7 :Bool; - hasLeftLane @23 :Bool; - hasRightLane @24 :Bool; + hasLeftLaneDEPRECATED @23 :Bool; + hasRightLaneDEPRECATED @24 :Bool; fcw @8 :Bool; longitudinalPlanSource @15 :LongitudinalPlanSource; @@ -577,6 +583,7 @@ struct Plan { decelForTurn @22 :Bool; mapValid @25 :Bool; + struct GpsTrajectory { x @0 :List(Float32); y @1 :List(Float32); @@ -590,6 +597,21 @@ struct Plan { } } +struct PathPlan { + laneWidth @0 :Float32; + + dPoly @1 :List(Float32); + cPoly @2 :List(Float32); + cProb @3 :Float32; + lPoly @4 :List(Float32); + lProb @5 :Float32; + rPoly @6 :List(Float32); + rProb @7 :Float32; + + angleSteers @8 :Float32; + valid @9 :Bool; +} + struct LiveLocationData { status @0 :UInt8; @@ -1276,6 +1298,7 @@ struct UbloxGnss { carrierPhaseStdev @10 :Float32; # doppler standard deviation in Hz dopplerStdev @11 :Float32; + sigId @12 :UInt8; struct TrackingStatus { # pseudorange valid @@ -1584,6 +1607,11 @@ struct LiveParametersData { struct LiveMapData { speedLimitValid @0 :Bool; speedLimit @1 :Float32; + speedAdvisoryValid @12 :Bool; + speedAdvisory @13 :Float32; + speedLimitAheadValid @14 :Bool; + speedLimitAhead @15 :Float32; + speedLimitAheadDistance @16 :Float32; curvatureValid @2 :Bool; curvature @3 :Float32; wayId @4 :UInt64; @@ -1603,6 +1631,13 @@ struct CameraOdometry { rotStd @3 :List(Float32); # std rad/s in device frame } +struct KalmanOdometry { + trans @0 :List(Float32); # m/s in device frame + rot @1 :List(Float32); # rad/s in device frame + transStd @2 :List(Float32); # std m/s in device frame + rotStd @3 :List(Float32); # std rad/s in device frame +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -1671,5 +1706,7 @@ struct Event { liveParameters @61 :LiveParametersData; liveMapData @62 :LiveMapData; cameraOdometry @63 :CameraOdometry; + pathPlan @64 :PathPlan; + kalmanOdometry @65 :KalmanOdometry; } } diff --git a/common/params.py b/common/params.py index cc137d5a7..de0e7a141 100755 --- a/common/params.py +++ b/common/params.py @@ -67,6 +67,7 @@ keys = { # written: visiond # read: visiond, controlsd "CalibrationParams": TxType.PERSISTENT, + "ControlsParams": TxType.PERSISTENT, # written: controlsd # read: radard "CarParams": TxType.CLEAR_ON_CAR_START, diff --git a/common/transformations/camera.py b/common/transformations/camera.py index 6cf517cf5..348152d79 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -29,21 +29,13 @@ view_frame_from_device_frame = device_frame_from_view_frame.T def get_calib_from_vp(vp): vp_norm = normalize(vp) yaw_calib = np.arctan(vp_norm[0]) - pitch_calib = np.arctan(vp_norm[1]*np.cos(yaw_calib)) - # TODO should be, this but written - # to be compatible with meshcalib and - # get_view_frame_from_road_fram - #pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib)) + pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib)) roll_calib = 0 return roll_calib, pitch_calib, yaw_calib # aka 'extrinsic_matrix' # road : x->forward, y -> left, z->up def get_view_frame_from_road_frame(roll, pitch, yaw, height): - # TODO - # calibration pitch is currently defined - # opposite to pitch in device frame - pitch = -pitch device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1])) view_from_road = view_frame_from_device_frame.dot(device_from_road) return np.hstack((view_from_road, [[0], [height], [0]])) diff --git a/models/posenet.dlc b/models/posenet.dlc new file mode 100644 index 000000000..58dee250f Binary files /dev/null and b/models/posenet.dlc differ diff --git a/opendbc/acura_ilx_2016_can_generated.dbc b/opendbc/acura_ilx_2016_can_generated.dbc index b0c16c814..592b6a5ad 100644 --- a/opendbc/acura_ilx_2016_can_generated.dbc +++ b/opendbc/acura_ilx_2016_can_generated.dbc @@ -76,6 +76,15 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 420 VSA_STATUS: 8 VSA + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|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 @@ -268,12 +277,6 @@ BO_ 419 GEARBOX: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/acura_rdx_2018_can_generated.dbc b/opendbc/acura_rdx_2018_can_generated.dbc index d60b90d75..4cbcfe182 100644 --- a/opendbc/acura_rdx_2018_can_generated.dbc +++ b/opendbc/acura_rdx_2018_can_generated.dbc @@ -76,6 +76,15 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 420 VSA_STATUS: 8 VSA + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|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 @@ -257,12 +266,6 @@ BO_ 404 STEERING_CONTROL: 4 EON SG_ COUNTER : 29|2@0+ (1,0) [0|15] "" EPS SG_ CHECKSUM : 27|4@0+ (1,0) [0|3] "" EPS -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/chrysler_pacifica_2017_hybrid.dbc b/opendbc/chrysler_pacifica_2017_hybrid.dbc index 02c0c6fc5..a1aa04145 100644 --- a/opendbc/chrysler_pacifica_2017_hybrid.dbc +++ b/opendbc/chrysler_pacifica_2017_hybrid.dbc @@ -372,7 +372,7 @@ CM_ SG_ 514 SPEED_LEFT "TODO find upper limit"; CM_ SG_ 653 BRAKE_PRESSURE "max seems to be 148"; CM_ SG_ 820 TURN_LIGHT_LEFT "oscillates with the light blinking"; CM_ SG_ 820 TURN_LIGHT_RIGHT "hazard blinks both right and left lights"; -CM_ SG_ 746 PRNDL "4=D, 3=N, 2=R, 1=P"; +CM_ SG_ 746 PRNDL "5=L, 4=D, 3=N, 2=R, 1=P"; CM_ SG_ 746 GEAR_CHECKSUM "different than the LKAS checksum. unknown non-simple algorithm. just build a lookup table for it."; CM_ SG_ 284 SPEED_RELATED_1 "Another Speed Signal, Maybe RPMs?"; CM_ SG_ 284 BRAKE_RELATED_1_1 "Correlates with braking"; @@ -429,5 +429,5 @@ CM_ SG_ 625 SPEED "zero on non-acc drives"; CM_ SG_ 625 ACCEL_PERHAPS "set to 7767 on non-ACC drives. ACC drive 40k is constant speed, 42k is accelerating"; CM_ SG_ 268 BRAKE_PERHAPS "triggers only on ACC braking"; CM_ SG_ 384 NEW_SIGNAL_1 "set in ACC gas driving. not set in electric human. not sure about gas human driving."; -VAL_ 746 PRNDL 4 "Drive" 3 "Neutral" 2 "Reverse" 1 "Park" ; +VAL_ 746 PRNDL 5 "Low" 4 "Drive" 3 "Neutral" 2 "Reverse" 1 "Park" ; VAL_ 792 TURN_SIGNALS 2 "Right" 1 "Left" ; diff --git a/opendbc/generator/honda/_honda_2017.dbc b/opendbc/generator/honda/_honda_2017.dbc index 82818f142..c79f2587b 100644 --- a/opendbc/generator/honda/_honda_2017.dbc +++ b/opendbc/generator/honda/_honda_2017.dbc @@ -54,6 +54,15 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 420 VSA_STATUS: 8 VSA + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|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 diff --git a/opendbc/generator/honda/acura_ilx_2016_can.dbc b/opendbc/generator/honda/acura_ilx_2016_can.dbc index 50677c4b0..e45426b3a 100644 --- a/opendbc/generator/honda/acura_ilx_2016_can.dbc +++ b/opendbc/generator/honda/acura_ilx_2016_can.dbc @@ -39,12 +39,6 @@ BO_ 419 GEARBOX: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/generator/honda/acura_rdx_2018_can.dbc b/opendbc/generator/honda/acura_rdx_2018_can.dbc index 30d09c7db..87389f8b2 100644 --- a/opendbc/generator/honda/acura_rdx_2018_can.dbc +++ b/opendbc/generator/honda/acura_rdx_2018_can.dbc @@ -28,12 +28,6 @@ BO_ 404 STEERING_CONTROL: 4 EON SG_ COUNTER : 29|2@0+ (1,0) [0|15] "" EPS SG_ CHECKSUM : 27|4@0+ (1,0) [0|3] "" EPS -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/generator/honda/honda_civic_touring_2016_can.dbc b/opendbc/generator/honda/honda_civic_touring_2016_can.dbc index 21327ec39..8af78ca05 100644 --- a/opendbc/generator/honda/honda_civic_touring_2016_can.dbc +++ b/opendbc/generator/honda/honda_civic_touring_2016_can.dbc @@ -44,14 +44,6 @@ BO_ 401 GEARBOX: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" EON - SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 450 EPB_STATUS: 8 EPB SG_ EPB_ACTIVE : 3|1@0+ (1,0) [0|1] "" EON SG_ EPB_STATE : 29|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/generator/honda/honda_crv_touring_2016_can.dbc b/opendbc/generator/honda/honda_crv_touring_2016_can.dbc index f75586a4c..3d9b48ab7 100644 --- a/opendbc/generator/honda/honda_crv_touring_2016_can.dbc +++ b/opendbc/generator/honda/honda_crv_touring_2016_can.dbc @@ -29,12 +29,6 @@ BO_ 404 STEERING_CONTROL: 4 EON SG_ COUNTER : 29|2@0+ (1,0) [0|15] "" EPS SG_ CHECKSUM : 27|4@0+ (1,0) [0|3] "" EPS -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/generator/honda/honda_fit_ex_2018_can.dbc b/opendbc/generator/honda/honda_fit_ex_2018_can.dbc index bc911ffcb..ce02d4cf8 100644 --- a/opendbc/generator/honda/honda_fit_ex_2018_can.dbc +++ b/opendbc/generator/honda/honda_fit_ex_2018_can.dbc @@ -39,14 +39,6 @@ BO_ 401 GEARBOX: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" EON - SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/generator/honda/honda_odyssey_exl_2018.dbc b/opendbc/generator/honda/honda_odyssey_exl_2018.dbc index 6c181d877..fb54a29ab 100644 --- a/opendbc/generator/honda/honda_odyssey_exl_2018.dbc +++ b/opendbc/generator/honda/honda_odyssey_exl_2018.dbc @@ -31,15 +31,6 @@ BO_ 419 GEARBOX: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" XXX - SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" XXX - SG_ BRAKE_HOLD_RELATED : 52|1@0+ (1,0) [0|1] "" XXX - BO_ 450 EPB_STATUS: 8 XXX SG_ EPB_BRAKE_AND_PULL : 6|1@0+ (1,0) [0|1] "" XXX SG_ EPB_ACTIVE : 3|1@0+ (1,0) [0|1] "" XXX diff --git a/opendbc/generator/honda/honda_pilot_touring_2017_can.dbc b/opendbc/generator/honda/honda_pilot_touring_2017_can.dbc index a499b6bc9..779cd66f5 100644 --- a/opendbc/generator/honda/honda_pilot_touring_2017_can.dbc +++ b/opendbc/generator/honda/honda_pilot_touring_2017_can.dbc @@ -39,12 +39,6 @@ BO_ 419 GEARBOX: 8 PCM SG_ GEAR_SHIFTER : 29|6@0+ (1,0) [0|63] "" EON SG_ GEAR : 7|8@0+ (1,0) [0|255] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/generator/honda/honda_ridgeline_black_edition_2017_can.dbc b/opendbc/generator/honda/honda_ridgeline_black_edition_2017_can.dbc index 7eee6a6c0..d070d314d 100644 --- a/opendbc/generator/honda/honda_ridgeline_black_edition_2017_can.dbc +++ b/opendbc/generator/honda/honda_ridgeline_black_edition_2017_can.dbc @@ -34,12 +34,6 @@ BO_ 419 GEARBOX: 8 PCM SG_ GEAR_SHIFTER : 29|6@0+ (1,0) [0|63] "" EON SG_ GEAR : 7|8@0+ (1,0) [0|255] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/generator/toyota/_comma.dbc b/opendbc/generator/toyota/_comma.dbc index 4e4c82d80..64d5ebc8d 100644 --- a/opendbc/generator/toyota/_comma.dbc +++ b/opendbc/generator/toyota/_comma.dbc @@ -11,14 +11,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/generator/toyota/lexus_rx_hybrid_2017_pt.dbc b/opendbc/generator/toyota/lexus_rx_hybrid_2017_pt.dbc index 8fe5ec199..fb42de9f6 100644 --- a/opendbc/generator/toyota/lexus_rx_hybrid_2017_pt.dbc +++ b/opendbc/generator/toyota/lexus_rx_hybrid_2017_pt.dbc @@ -22,7 +22,9 @@ BO_ 610 EPS_STATUS: 5 EPS SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX BO_ 956 GEAR_PACKET: 8 XXX + SG_ SPORT_ON : 2|1@0+ (1,0) [0|1] "" XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX + SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX CM_ SG_ 550 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 550 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; @@ -31,3 +33,5 @@ CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; +VAL_ 956 SPORT_ON 0 "off" 1 "on"; +VAL_ 956 ECON_ON 0 "off" 1 "on"; diff --git a/opendbc/generator/toyota/toyota_chr_2018_pt.dbc b/opendbc/generator/toyota/toyota_chr_2018_pt.dbc index 60865fb46..7306ed2c0 100644 --- a/opendbc/generator/toyota/toyota_chr_2018_pt.dbc +++ b/opendbc/generator/toyota/toyota_chr_2018_pt.dbc @@ -23,11 +23,15 @@ BO_ 610 EPS_STATUS: 8 EPS SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX BO_ 956 GEAR_PACKET: 8 XXX + SG_ SPORT_ON : 2|1@0+ (1,0) [0|1] "" XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX + SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 548 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; -VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; +VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; +VAL_ 956 SPORT_ON 0 "off" 1 "on"; +VAL_ 956 ECON_ON 0 "off" 1 "on"; diff --git a/opendbc/generator/toyota/toyota_rav4_hybrid_2017_pt.dbc b/opendbc/generator/toyota/toyota_rav4_hybrid_2017_pt.dbc index 8fe5ec199..d5a265fbb 100644 --- a/opendbc/generator/toyota/toyota_rav4_hybrid_2017_pt.dbc +++ b/opendbc/generator/toyota/toyota_rav4_hybrid_2017_pt.dbc @@ -22,7 +22,14 @@ BO_ 610 EPS_STATUS: 5 EPS SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX BO_ 956 GEAR_PACKET: 8 XXX + SG_ SPORT_ON : 2|1@0+ (1,0) [0|1] "" XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX + SG_ SPORT_GEAR_ON : 33|1@0+ (1,0) [0|1] "" XXX + SG_ SPORT_GEAR : 38|3@0+ (1,0) [0|7] "" XXX + SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX + SG_ DRIVE_ENGAGED : 47|1@0+ (1,0) [0|1] "" XXX + + CM_ SG_ 550 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 550 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; @@ -30,4 +37,9 @@ CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered"; CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; +VAL_ 956 SPORT_ON 0 "off" 1 "on"; VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; +VAL_ 956 SPORT_GEAR_ON 0 "off" 1 "on"; +VAL_ 956 SPORT_GEAR 1 "S1" 2 "S2" 3 "S3" 4 "S4" 5 "S5" 6 "S6"; +VAL_ 956 ECON_ON 0 "off" 1 "on"; +VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on"; diff --git a/opendbc/honda_accord_touring_2016_can.dbc b/opendbc/honda_accord_touring_2016_can.dbc index d26ccd6a1..e8f388de0 100644 --- a/opendbc/honda_accord_touring_2016_can.dbc +++ b/opendbc/honda_accord_touring_2016_can.dbc @@ -86,10 +86,13 @@ BO_ 401 GEARBOX: 8 PCM SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" NEO BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" NEO - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" NEO - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" NEO - SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" NEO + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" NEO diff --git a/opendbc/honda_civic_touring_2016_can_generated.dbc b/opendbc/honda_civic_touring_2016_can_generated.dbc index 841631bf6..8a281e265 100644 --- a/opendbc/honda_civic_touring_2016_can_generated.dbc +++ b/opendbc/honda_civic_touring_2016_can_generated.dbc @@ -76,6 +76,15 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 420 VSA_STATUS: 8 VSA + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|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 @@ -273,14 +282,6 @@ BO_ 401 GEARBOX: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" EON - SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 450 EPB_STATUS: 8 EPB SG_ EPB_ACTIVE : 3|1@0+ (1,0) [0|1] "" EON SG_ EPB_STATE : 29|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/honda_crv_touring_2016_can_generated.dbc b/opendbc/honda_crv_touring_2016_can_generated.dbc index 3fa85d899..dd924bf6a 100644 --- a/opendbc/honda_crv_touring_2016_can_generated.dbc +++ b/opendbc/honda_crv_touring_2016_can_generated.dbc @@ -76,6 +76,15 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 420 VSA_STATUS: 8 VSA + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|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 @@ -258,12 +267,6 @@ BO_ 404 STEERING_CONTROL: 4 EON SG_ COUNTER : 29|2@0+ (1,0) [0|15] "" EPS SG_ CHECKSUM : 27|4@0+ (1,0) [0|3] "" EPS -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/honda_fit_ex_2018_can_generated.dbc b/opendbc/honda_fit_ex_2018_can_generated.dbc index 2e7c7fb55..2cd5a1ea3 100644 --- a/opendbc/honda_fit_ex_2018_can_generated.dbc +++ b/opendbc/honda_fit_ex_2018_can_generated.dbc @@ -76,6 +76,15 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 420 VSA_STATUS: 8 VSA + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|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 @@ -268,14 +277,6 @@ BO_ 401 GEARBOX: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" EON - SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/honda_odyssey_exl_2018_generated.dbc b/opendbc/honda_odyssey_exl_2018_generated.dbc index 05dd1a436..090c9688e 100644 --- a/opendbc/honda_odyssey_exl_2018_generated.dbc +++ b/opendbc/honda_odyssey_exl_2018_generated.dbc @@ -76,6 +76,15 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 420 VSA_STATUS: 8 VSA + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|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 @@ -260,15 +269,6 @@ BO_ 419 GEARBOX: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" XXX - SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" XXX - SG_ BRAKE_HOLD_RELATED : 52|1@0+ (1,0) [0|1] "" XXX - BO_ 450 EPB_STATUS: 8 XXX SG_ EPB_BRAKE_AND_PULL : 6|1@0+ (1,0) [0|1] "" XXX SG_ EPB_ACTIVE : 3|1@0+ (1,0) [0|1] "" XXX diff --git a/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc b/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc index ad66716f3..91859aa24 100644 --- a/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc +++ b/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc @@ -231,6 +231,9 @@ BO_ 404 STEERING_CONTROL: 4 EON BO_ 420 VSA_STATUS: 8 VSA SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON diff --git a/opendbc/honda_pilot_touring_2017_can_generated.dbc b/opendbc/honda_pilot_touring_2017_can_generated.dbc index 0c43f98f3..a14ad60e5 100644 --- a/opendbc/honda_pilot_touring_2017_can_generated.dbc +++ b/opendbc/honda_pilot_touring_2017_can_generated.dbc @@ -76,6 +76,15 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 420 VSA_STATUS: 8 VSA + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|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 @@ -268,12 +277,6 @@ BO_ 419 GEARBOX: 8 PCM SG_ GEAR_SHIFTER : 29|6@0+ (1,0) [0|63] "" EON SG_ GEAR : 7|8@0+ (1,0) [0|255] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc b/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc index a8a97d7f1..a5ce2d23d 100644 --- a/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc +++ b/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc @@ -76,6 +76,15 @@ BO_ 380 POWERTRAIN_DATA: 8 PCM SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 420 VSA_STATUS: 8 VSA + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" 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 + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|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 @@ -263,12 +272,6 @@ BO_ 419 GEARBOX: 8 PCM SG_ GEAR_SHIFTER : 29|6@0+ (1,0) [0|63] "" EON SG_ GEAR : 7|8@0+ (1,0) [0|255] "" EON -BO_ 420 VSA_STATUS: 8 VSA - SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON - SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON - SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON - SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON - BO_ 422 SCM_BUTTONS: 8 SCM SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON diff --git a/opendbc/hyundai_kia_generic.dbc b/opendbc/hyundai_kia_generic.dbc index b9e654fa6..e3e6a1eb6 100644 --- a/opendbc/hyundai_kia_generic.dbc +++ b/opendbc/hyundai_kia_generic.dbc @@ -692,6 +692,7 @@ BO_ 1365 FPCM11: 8 FPCM SG_ CF_Fpcm_LPCtrCirFlt : 15|1@1+ (1.0,0.0) [0.0|1.0] "" EMS BO_ 871 LVR12: 8 LVR + SG_ CF_Lvr_CruiseSet : 0|8@1+ (1.0,0.0) [0.0|255.0] "" CLU,TCU SG_ CF_Lvr_Gear : 32|4@1+ (1.0,0.0) [0.0|15.0] "" CLU,TCU BO_ 872 LVR11: 8 LVR @@ -896,11 +897,11 @@ BO_ 67 DATC13: 8 DATC BO_ 66 DATC12: 8 DATC SG_ CR_Datc_DrTempDispC : 0|8@1+ (0.5,14.0) [15.0|32.0] "deg" CLU,IBOX - SG_ CR_Datc_DrTempDispF : 8|8@1+ (1.0,56.0) [58.0|90.0] "¢µ" CLU,IBOX + SG_ CR_Datc_DrTempDispF : 8|8@1+ (1.0,56.0) [58.0|90.0] "deg" CLU,IBOX SG_ CR_Datc_PsTempDispC : 16|8@1+ (0.5,14.0) [15.0|32.0] "deg" CLU,IBOX - SG_ CR_Datc_PsTempDispF : 24|8@1+ (1.0,56.0) [58.0|90.0] "¢µ" CLU,IBOX + SG_ CR_Datc_PsTempDispF : 24|8@1+ (1.0,56.0) [58.0|90.0] "deg" CLU,IBOX SG_ CR_Datc_RearDrTempDispC : 40|8@1+ (0.5,14.0) [15.0|32.0] "deg" CLU - SG_ CR_Datc_RearDrTempDispF : 48|8@1+ (1.0,58.0) [58.0|90.0] "¢µ" CLU + SG_ CR_Datc_RearDrTempDispF : 48|8@1+ (1.0,58.0) [58.0|90.0] "deg" CLU SG_ CF_Datc_CO2_Warning : 56|8@1+ (1.0,0.0) [0.0|3.0] "" CLU BO_ 1345 CGW1: 8 BCM @@ -980,6 +981,8 @@ BO_ 832 LKAS11: 8 LDWS_LKAS SG_ CF_Lkas_Chksum : 48|8@1+ (1.0,0.0) [0.0|255.0] "" MDPS SG_ CF_Lkas_FcwOpt_USM : 56|3@1+ (1.0,0.0) [0.0|7.0] "" CLU SG_ CF_Lkas_LdwsOpt_USM : 59|3@1+ (1.0,0.0) [0.0|7.0] "" CLU,MDPS + SG_ CF_Lkas_Unknown1 : 47|1@1+ (1,0) [0|3] "" XXX + SG_ CF_Lkas_Unknown2 : 63|2@0+ (1,0) [0|3] "" XXX BO_ 1342 LKAS12: 6 LDWS_LKAS SG_ CF_Lkas_TsrSlifOpt : 10|2@1+ (1.0,0.0) [0.0|3.0] "" CLU diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc index 870f0d496..f5f52c02a 100644 --- a/opendbc/lexus_is_2018_pt_generated.dbc +++ b/opendbc/lexus_is_2018_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc index 43c5d353e..de73a6130 100644 --- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc +++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON @@ -318,7 +318,9 @@ BO_ 610 EPS_STATUS: 5 EPS SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX BO_ 956 GEAR_PACKET: 8 XXX + SG_ SPORT_ON : 2|1@0+ (1,0) [0|1] "" XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX + SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX CM_ SG_ 550 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 550 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; @@ -327,3 +329,5 @@ CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; +VAL_ 956 SPORT_ON 0 "off" 1 "on"; +VAL_ 956 ECON_ON 0 "off" 1 "on"; diff --git a/opendbc/subaru_outback_2015_eyesight.dbc b/opendbc/subaru_outback_2015_eyesight.dbc new file mode 100644 index 000000000..9f592b828 --- /dev/null +++ b/opendbc/subaru_outback_2015_eyesight.dbc @@ -0,0 +1,370 @@ +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: XXX 0 + + +BO_ 2 Steering: 8 XXX + SG_ NEW_SIGNAL_1 : 31|4@0- (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_6 : 24|1@1+ (1,0) [0|3] "" XXX + SG_ Counter : 25|3@1+ (1,0) [0|15] "" XXX + SG_ Checksum : 32|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 22|3@0+ (1,0) [0|7] "" XXX + SG_ Steering_Angle : 7|16@0- (-0.1,-9) [-497|500] "degree" XXX + +BO_ 208 G_Sensor: 8 XXX + SG_ NEW_SIGNAL_3 : 32|8@1+ (1,0) [0|255] "" XXX + SG_ Steering_Angle : 0|16@1- (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_1 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ _Latitudinal : 16|16@1- (0.0035,1) [0|65535] "" XXX + SG_ _Longitudinal : 48|16@1- (0.000035,0) [0|255] "" XXX + +BO_ 209 Brake_Pedal: 8 XXX + SG_ NEW_SIGNAL_2 : 31|1@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_3 : 56|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_1 : 26|1@1+ (1,0) [0|3] "" XXX + SG_ Brake_Pedal : 23|8@0+ (1,0) [0|255] "" XXX + SG_ Speed : 0|16@1+ (0.05625,0) [0|255] "KPH" XXX + +BO_ 210 Brake_2: 8 XXX + SG_ NEW_SIGNAL_1 : 46|1@0+ (1,0) [0|4294967295] "" XXX + SG_ NEW_SIGNAL_2 : 39|3@0+ (1,0) [0|7] "" XXX + SG_ Right_Brake : 48|8@1+ (1,0) [0|255] "" XXX + SG_ Left_Brake : 63|8@0+ (1,0) [0|255] "" XXX + SG_ Brake_Light : 35|1@1+ (1,0) [0|15] "" XXX + SG_ Brake_Related : 36|1@1+ (1,0) [0|255] "" XXX + +BO_ 211 Brake_Type: 8 XXX + SG_ NEW_SIGNAL_4 : 28|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3 : 7|1@0+ (1,0) [0|4294967295] "" XXX + SG_ NEW_SIGNAL_2 : 16|4@1+ (1,0) [0|15] "" XXX + SG_ Speed_Counter : 39|8@0+ (1,0) [0|15] "" XXX + SG_ Counter : 55|8@0+ (1,0) [0|255] "" XXX + SG_ Brake_Light : 21|1@0+ (1,0) [0|3] "" XXX + SG_ Brake_Cruise_On : 42|1@0+ (1,0) [0|3] "" XXX + SG_ Brake_Pedal_On : 46|1@0+ (1,0) [0|3] "" XXX + +BO_ 212 WHEEL_SPEEDS: 8 XXX + SG_ FL : 0|16@1+ (0.0592,0) [2|255] "KPH" XXX + SG_ FR : 16|16@1+ (0.0592,0) [0|255] "KPH" XXX + SG_ RL : 32|16@1+ (0.0592,0) [0|255] "" XXX + SG_ RR : 48|16@1+ (0.0592,0) [0|127] "KPH" XXX + +BO_ 320 Throttle: 8 XXX + SG_ Off_Throttle_2 : 56|1@1+ (1,0) [0|3] "" XXX + SG_ Throttle_Combo : 40|8@1+ (1,0) [0|255] "" XXX + SG_ Engine_RPM : 16|14@1+ (1,0) [0|15] "" XXX + SG_ Off_Throttle : 30|1@0+ (1,0) [0|3] "" XXX + SG_ Throttle_Cruise : 39|8@0+ (1,0) [0|255] "" XXX + SG_ Throttle_Body_ : 55|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_3 : 63|1@0+ (1,0) [0|15] "" XXX + SG_ Not_Full_Throttle : 14|1@0+ (1,0) [0|15] "" XXX + SG_ Counter : 8|4@1+ (1,0) [0|15] "" XXX + SG_ Throttle_Pedal : 0|8@1+ (0.392157,0) [0|255] "" XXX + +BO_ 321 undefined: 8 XXX + SG_ NEW_SIGNAL_7 : 59|2@1+ (1,0) [0|63] "" XXX + SG_ NEW_SIGNAL_6 : 46|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_2 : 47|1@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_5 : 48|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3 : 54|2@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_8 : 31|4@0+ (1,0) [0|15] "" XXX + SG_ Engine_RPM : 32|12@1+ (1,0) [0|8191] "" XXX + SG_ Engine_Torque : 0|15@1+ (1,0) [0|255] "" XXX + SG_ Wheel_Torque : 16|12@1+ (1,0) [0|255] "" XXX + SG_ Engine_Stop : 15|1@0+ (1,0) [0|3] "" XXX + +BO_ 324 CruiseControl_2015: 8 XXX + SG_ Cruise_On : 48|1@0+ (1,0) [0|3] "" XXX + SG_ OnOffButton : 2|1@1+ (1,0) [0|3] "" XXX + SG_ SET_BUTTON : 3|1@0+ (1,0) [0|3] "" XXX + SG_ RES_BUTTON : 4|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_6 : 50|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_4 : 28|5@0+ (1,0) [0|16777215] "" XXX + SG_ NEW_SIGNAL_9 : 15|1@1+ (1,0) [0|127] "" XXX + SG_ NEW_SIGNAL_2 : 8|1@0+ (1,0) [0|255] "" XXX + SG_ Cruise_Activated : 49|1@0+ (1,0) [0|7] "" XXX + SG_ Brake_Pedal_On : 51|1@0+ (1,0) [0|3] "" XXX + SG_ Button : 13|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_1 : 23|8@0+ (1,-124) [0|255] "" XXX + +BO_ 328 Transmission: 8 XXX + SG_ Counter : 11|4@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_7 : 63|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_4 : 15|4@0+ (1,0) [0|15] "" XXX + SG_ Paddle_Shift : 60|2@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_1 : 31|1@0+ (1,0) [0|3] "" XXX + SG_ Transmission_Engine : 16|15@1+ (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_5 : 43|1@0+ (1,0) [0|65535] "" XXX + SG_ Gear : 48|4@1+ (1,0) [0|15] "" XXX + SG_ Gear_2 : 52|4@1+ (1,0) [0|15] "" XXX + SG_ Manual_Gear : 4|4@1+ (1,0) [0|255] "" XXX + +BO_ 329 CVT_Ratio: 8 XXX + SG_ NEW_SIGNAL_2 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_3 : 40|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 0|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_5 : 11|4@0+ (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_1 : 39|8@0+ (1,0) [0|255] "" XXX + +BO_ 336 Brake_Pressure: 8 XXX + SG_ Brake_Pressure_Right : 0|8@1+ (1,0) [0|15] "" XXX + SG_ Brake_Pressure_Left : 15|8@0+ (1,0) [0|255] "" XXX + +BO_ 338 Stalk: 8 XXX + SG_ Wiper : 62|1@0+ (1,0) [0|3] "" XXX + SG_ brake_light : 52|1@0+ (1,0) [0|3] "" XXX + SG_ Headlights : 59|1@0+ (1,0) [0|3] "" XXX + SG_ Runlights : 58|1@1+ (1,0) [0|3] "" XXX + SG_ Highbeam : 60|1@0+ (1,0) [0|3] "" XXX + SG_ Counter : 15|4@0+ (1,0) [0|15] "" XXX + +BO_ 342 NEW_MSG_2: 8 XXX + +BO_ 346 Counter_3: 8 XXX + SG_ Counter : 0|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_1 : 20|1@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 57|1@1+ (1,0) [0|3] "" XXX + +BO_ 352 ES_Brake: 8 XXX + SG_ Counter : 48|3@1+ (1,0) [0|7] "" XXX + SG_ Checksum : 56|8@1+ (1,0) [0|7] "" XXX + SG_ Brake_On : 22|1@0+ (1,0) [0|3] "" XXX + SG_ Brake_Light : 20|1@0+ (1,0) [0|2047] "" XXX + SG_ Cruise_Activated : 23|1@0+ (1,0) [0|3] "" XXX + SG_ Brake_Pressure : 0|16@1+ (1,0) [0|255] "" XXX + +BO_ 353 ES_CruiseThrottle: 8 XXX + SG_ Throttle_Cruise : 0|12@1+ (1,0) [0|255] "" XXX + SG_ Counter_1 : 46|3@0+ (1,0) [0|15] "" XXX + SG_ Wheel_stop : 22|1@0+ (1,0) [0|3] "" XXX + SG_ CloseDistance : 24|8@1+ (1,0) [0|255] "" XXX + SG_ Unknown : 18|1@1+ (1,0) [0|7] "" XXX + SG_ Button_Speed_Down : 49|1@1+ (1,0) [0|3] "" XXX + SG_ Button_Resume : 50|1@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3_Blank : 15|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2_Blank : 48|1@1+ (1,0) [0|3] "" XXX + SG_ Checksum : 56|8@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_9 : 17|1@1+ (1,0) [0|3] "" XXX + SG_ Cruise_Activatedish : 16|1@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_6_Blank : 23|1@0+ (1,0) [0|7] "" XXX + SG_ DistanceSwap : 21|1@0+ (1,0) [0|3] "" XXX + SG_ Brake_On : 20|1@0+ (1,0) [0|7] "" XXX + +BO_ 354 ES_RPM: 8 XXX + SG_ Counter : 48|3@1+ (1,0) [0|7] "" XXX + SG_ Cruise_Activated : 9|1@0+ (1,0) [0|3] "" XXX + SG_ Checksum : 39|8@0+ (1,0) [0|65535] "" XXX + SG_ RPM : 16|16@1+ (1,0) [0|255] "" XXX + SG_ Brake : 8|1@1+ (1,0) [0|7] "" XXX + +BO_ 356 ES_LKAS: 8 XXX + SG_ Checksum : 56|8@1+ (1,0) [0|15] "" XXX + SG_ Counter : 0|3@1+ (1,0) [0|15] "" XXX + SG_ LKAS_Active : 24|1@1+ (1,0) [0|7] "" XXX + SG_ LKAS_Output : 8|13@1- (-1,0) [0|127] "" XXX + +BO_ 358 ES_Status: 8 XXX + SG_ Counter : 39|3@0+ (1,0) [0|7] "" XXX + SG_ Cruise_On : 16|1@1+ (1,0) [0|7] "" XXX + SG_ Cruise_Activated : 17|1@1+ (1,0) [0|3] "" XXX + SG_ Obstacle_Distance : 48|4@1+ (1,0) [0|15] "" XXX + SG_ Cruise_Off : 22|1@0+ (1,0) [0|3] "" XXX + SG_ Saved_Speed : 24|8@1+ (1,0) [0|255] "" XXX + SG_ Car_Follow : 46|1@1+ (1,0) [0|255] "" XXX + SG_ Driver_Input : 20|1@0+ (1,0) [0|15] "" XXX + SG_ WHEELS_MOVING_2015 : 19|1@1+ (1,0) [0|3] "" XXX + SG_ Untitled_Blank : 18|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_6_Blank : 21|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_5_Blank : 33|1@1+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_4_Blank : 34|1@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_1 : 35|1@0+ (1,0) [0|31] "" XXX + SG_ 3SecondDisengage : 13|2@0+ (1,0) [0|3] "" XXX + SG_ Not_Ready_Startup : 0|3@1+ (1,0) [0|7] "" XXX + SG_ Steep_Hill_Disengage : 44|1@0+ (1,0) [0|3] "" XXX + SG_ Disengage_Alert : 15|2@0+ (1,0) [0|3] "" XXX + +BO_ 359 ES_LDW: 8 XXX + SG_ 2Right_Depart : 50|1@1+ (1,0) [0|7] "" XXX + SG_ 2All_Depart : 28|1@0+ (1,0) [0|3] "" XXX + SG_ 3All_Depart : 52|1@0+ (1,0) [0|3] "" XXX + SG_ Left_Depart_Front : 51|1@0+ (1,0) [0|3] "" XXX + SG_ All_depart_2015 : 0|1@1+ (1,0) [0|255] "" XXX + SG_ LKAS_Inactive_2017 : 36|1@1+ (1,0) [0|3] "" XXX + SG_ LKAS_Steer_Active_2017 : 37|1@0+ (1,0) [0|3] "" XXX + SG_ Right_Line_2017 : 24|1@1+ (1,0) [0|7] "" XXX + SG_ Left_Line_2017 : 25|1@1+ (1,0) [0|3] "" XXX + SG_ 1All_Depart : 31|1@0+ (1,0) [0|15] "" XXX + SG_ 1Right_Depart_Front : 49|1@1+ (1,0) [0|3] "" XXX + SG_ 1Right_Depart : 48|1@1+ (1,0) [0|3] "" XXX + +BO_ 392 Counter_0: 8 XXX + SG_ Counter : 16|4@1+ (1,0) [0|15] "" XXX + +BO_ 554 NEW_MSG_3: 8 XXX + SG_ Counter : 0|4@1+ (1,0) [0|255] "" XXX + +BO_ 604 undefined: 8 XXX + +BO_ 640 NEW_MSG_10: 8 XXX + SG_ NEW_SIGNAL_2 : 24|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 54|2@0+ (1,0) [0|15] "" XXX + SG_ Counter : 8|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3 : 34|2@0+ (1,0) [0|63] "" XXX + SG_ NEW_SIGNAL_1 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_5 : 39|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_6 : 0|1@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_7 : 38|1@0+ (1,0) [0|3] "" XXX + +BO_ 642 Dashlights: 8 XXX + SG_ NEW_SIGNAL_2 : 32|1@1+ (1,0) [0|15] "" XXX + SG_ Counter : 15|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_1 : 0|12@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_3 : 34|2@1+ (1,0) [0|3] "" XXX + SG_ LEFT_BLINKER : 44|1@0+ (1,0) [0|3] "" XXX + SG_ RIGHT_BLINKER : 45|1@0+ (1,0) [0|3] "" XXX + +BO_ 644 NEW_MSG_8: 8 XXX + SG_ Counter : 8|4@1+ (1,0) [0|15] "" XXX + +BO_ 805 undefined: 8 XXX + +BO_ 880 Steer_Torque_2: 8 XXX + SG_ Counter : 40|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3 : 48|4@1- (1,0) [0|15] "" XXX + SG_ Steering_Voltage_Flat : 7|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_1 : 8|1@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 30|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_4_2017 : 52|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_5_2017 : 54|1@0+ (1,0) [0|3] "" XXX + SG_ Steer_Torque_Sensor : 32|8@1- (-1,0) [0|255] "" XXX + +BO_ 881 Steering_Torque: 8 XXX + SG_ Steering_Motor_Flat : 0|8@1+ (1,0) [0|255] "" XXX + SG_ Steer_Torque_Sensor : 39|8@0- (1,0) [0|255] "" XXX + SG_ Steering_Angle : 40|16@1- (-0.026,0) [0|255] "" XXX + SG_ Steering_Motor_LeftRight : 23|16@0- (-0.13,0) [0|255] "" XXX + +BO_ 882 Counter: 8 XXX + SG_ Counter : 15|4@0+ (1,0) [0|31] "" XXX + SG_ Something : 16|2@1+ (1,0) [0|255] "" XXX + +BO_ 884 DOORS_STATUS: 8 XXX + SG_ DOOR_OPEN_FR : 24|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_FL : 25|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_RL : 26|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_RR : 27|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_Hatch : 28|1@0+ (1,0) [0|1] "" XXX + +BO_ 886 undefined: 8 XXX + +BO_ 864 Engine_Temp: 8 XXX + SG_ NEW_SIGNAL_1 : 32|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 44|1@1+ (1,0) [0|3] "" XXX + SG_ Saved_Speed : 56|8@1+ (1,0) [0|255] "" XXX + SG_ Cruise_Activated : 45|1@1+ (1,0) [0|3] "" XXX + SG_ Oil_Temp : 16|8@1+ (1,-40) [0|255] "" XXX + SG_ Coolant_Temp : 24|8@1+ (1,-40) [0|255] "" XXX + +BO_ 865 NEW_MSG_16: 8 XXX + SG_ NEW_SIGNAL_1 : 16|8@1+ (1,0) [0|255] "" XXX + SG_ Counter : 32|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 12|1@1+ (1,0) [0|255] "" XXX + +BO_ 866 Fuel__: 8 XXX + SG_ NEW_SIGNAL_2 : 55|8@0+ (1,0) [0|16777215] "" XXX + SG_ NEW_SIGNAL_3 : 35|4@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 47|8@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 0|16@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_5 : 24|1@0+ (1,0) [0|32767] "" XXX + +BO_ 872 NEW_MSG_15: 8 XXX + SG_ NEW_SIGNAL_1 : 31|8@0+ (1,0) [0|65535] "" XXX + +BO_ 977 NEW_MSG_12: 8 XXX + SG_ NEW_SIGNAL_1 : 0|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 16|12@1+ (1,0) [0|255] "" XXX + +BO_ 1632 Huge_Counter: 8 XXX + SG_ NEW_SIGNAL_1 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ Counter : 55|16@0+ (1,0) [0|255] "" XXX + +BO_ 1745 NEW_MSG_11: 8 XXX + SG_ NEW_SIGNAL_1 : 24|6@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 40|6@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3 : 0|8@1+ (1,0) [0|255] "" XXX + +BO_ 1786 NEW_MSG_9: 8 XXX + SG_ Counter : 3|4@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 8|16@1+ (1,0) [0|15] "" XXX + + + + +CM_ SG_ 2 NEW_SIGNAL_1 ""; +CM_ SG_ 211 Brake_Pedal_On "high tolerance"; +CM_ SG_ 212 FL "KPH"; +CM_ SG_ 212 RL "KPH"; +CM_ SG_ 320 Off_Throttle_2 "Less sensitive"; +CM_ SG_ 320 Throttle_Body_ "Throttle related"; +CM_ SG_ 328 Gear "15 = P, 14 = R, 0 = N, 1-6=gear"; +CM_ SG_ 328 Gear_2 "15 = P, 14 = R, 0 = N, 1-6=gear"; +CM_ SG_ 338 Wiper "0 off, 1 on"; +CM_ SG_ 353 NEW_SIGNAL_3_Blank "always 2"; +CM_ SG_ 353 NEW_SIGNAL_2_Blank "0"; +CM_ SG_ 353 NEW_SIGNAL_9 "flipped around quick engagement"; +CM_ SG_ 353 NEW_SIGNAL_6_Blank "always 1"; +CM_ SG_ 353 Brake_On "long activatedish"; +CM_ SG_ 354 RPM "20hz version of Transmission_Engine under Transmission"; +CM_ SG_ 358 Cruise_Activated "is 1 when cruise is able to go"; +CM_ SG_ 358 Car_Follow "front car detected"; +CM_ SG_ 358 3SecondDisengage "seatbelt disengage"; +CM_ SG_ 358 Disengage_Alert "seatbelt and steep hill disengage"; +CM_ SG_ 359 2All_Depart "Left and right depart"; +CM_ SG_ 359 Left_Depart_Front "warning after acceleration into car in front and left depart"; +CM_ SG_ 359 All_depart_2015 "always 1 on 2017"; +CM_ SG_ 359 LKAS_Inactive_2017 "1 when not steering, 0 when lkas steering"; +CM_ SG_ 359 1All_Depart "Left and right depart"; +CM_ SG_ 359 1Right_Depart_Front "object in front, right depart, hill steep and seatbelt disengage alert "; +CM_ SG_ 359 1Right_Depart "right depart, hill steep and seatbelt disengage"; +CM_ SG_ 642 Counter "Affected by signals"; +CM_ SG_ 642 RIGHT_BLINKER "0 off, 2 right, 1 left"; +CM_ SG_ 880 Steering_Voltage_Flat "receives later than 371"; +CM_ SG_ 880 NEW_SIGNAL_1 "0 in 2017"; +CM_ SG_ 880 NEW_SIGNAL_2 ""; +CM_ SG_ 880 NEW_SIGNAL_4_2017 "1 in 2017"; +CM_ SG_ 880 NEW_SIGNAL_5_2017 "1 in 2017"; +CM_ SG_ 881 Steering_Motor_Flat "Possibly motor voltage"; +CM_ SG_ 881 Steering_Angle "Missing extra larger bits"; diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc index 32e2b8018..cef7138b2 100644 --- a/opendbc/toyota_avalon_2017_pt_generated.dbc +++ b/opendbc/toyota_avalon_2017_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc index 4495f4373..6361a100a 100644 --- a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/toyota_chr_2018_pt_generated.dbc b/opendbc/toyota_chr_2018_pt_generated.dbc index 9606941a2..7ef8f3f94 100644 --- a/opendbc/toyota_chr_2018_pt_generated.dbc +++ b/opendbc/toyota_chr_2018_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON @@ -319,11 +319,15 @@ BO_ 610 EPS_STATUS: 8 EPS SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX BO_ 956 GEAR_PACKET: 8 XXX + SG_ SPORT_ON : 2|1@0+ (1,0) [0|1] "" XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX + SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 548 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; -VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; +VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; +VAL_ 956 SPORT_ON 0 "off" 1 "on"; +VAL_ 956 ECON_ON 0 "off" 1 "on"; diff --git a/opendbc/toyota_chr_hybrid_2018_pt_generated.dbc b/opendbc/toyota_chr_hybrid_2018_pt_generated.dbc index 780f7b316..5fb07ee64 100644 --- a/opendbc/toyota_chr_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_chr_hybrid_2018_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc index 32e823ee1..2bc435c23 100644 --- a/opendbc/toyota_corolla_2017_pt_generated.dbc +++ b/opendbc/toyota_corolla_2017_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc index eb5abc3b7..d263b1526 100644 --- a/opendbc/toyota_highlander_2017_pt_generated.dbc +++ b/opendbc/toyota_highlander_2017_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc index 76000cd79..f923c5c51 100644 --- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc index d2e716786..a78a149d8 100644 --- a/opendbc/toyota_prius_2017_pt_generated.dbc +++ b/opendbc/toyota_prius_2017_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc index 3cb6920a7..0448e9de2 100644 --- a/opendbc/toyota_rav4_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_2017_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc index ace29b5db..eb332a27c 100644 --- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON @@ -318,7 +318,14 @@ BO_ 610 EPS_STATUS: 5 EPS SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX BO_ 956 GEAR_PACKET: 8 XXX + SG_ SPORT_ON : 2|1@0+ (1,0) [0|1] "" XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX + SG_ SPORT_GEAR_ON : 33|1@0+ (1,0) [0|1] "" XXX + SG_ SPORT_GEAR : 38|3@0+ (1,0) [0|7] "" XXX + SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX + SG_ DRIVE_ENGAGED : 47|1@0+ (1,0) [0|1] "" XXX + + CM_ SG_ 550 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 550 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; @@ -326,4 +333,9 @@ CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered"; CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; +VAL_ 956 SPORT_ON 0 "off" 1 "on"; VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; +VAL_ 956 SPORT_GEAR_ON 0 "off" 1 "on"; +VAL_ 956 SPORT_GEAR 1 "S1" 2 "S2" 3 "S3" 4 "S4" 5 "S5" 6 "S6"; +VAL_ 956 ECON_ON 0 "off" 1 "on"; +VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on"; diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc index 7b2f0d747..67291ab31 100644 --- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc +++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc @@ -15,14 +15,14 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON diff --git a/panda/.circleci/config.yml b/panda/.circleci/config.yml index 2c7805a5b..2289ad8aa 100644 --- a/panda/.circleci/config.yml +++ b/panda/.circleci/config.yml @@ -29,9 +29,13 @@ jobs: command: | docker run panda_build /bin/bash -c "cd /panda/board; make bin" - run: - name: Build Pedal STM image + name: Build Honda Pedal STM image command: | - docker run panda_build /bin/bash -c "cd /panda/board/pedal; make obj/comma.bin" + docker run panda_build /bin/bash -c "cd /panda/board/pedal_honda; make obj/comma.bin" + - run: + name: Build Toyota Pedal STM image + command: | + docker run panda_build /bin/bash -c "cd /panda/board/pedal_toyota; make obj/comma.bin" - run: name: Build NEO STM image command: | diff --git a/panda/VERSION b/panda/VERSION index 02e8c95da..0408c30b4 100644 --- a/panda/VERSION +++ b/panda/VERSION @@ -1 +1 @@ -v1.1.8 \ No newline at end of file +v1.2.0 \ No newline at end of file diff --git a/panda/board/main.c b/panda/board/main.c index b1033bb73..16d64916a 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -559,6 +559,7 @@ int main() { usb_init(); // default to silent mode to prevent issues with Ford + // hardcode a specific safety mode if you want to force the panda to be in a specific mode safety_set_mode(SAFETY_NOOUTPUT, 0); can_silent = ALL_CAN_SILENT; can_init_all(); diff --git a/panda/board/pedal/.gitignore b/panda/board/pedal_honda/.gitignore similarity index 100% rename from panda/board/pedal/.gitignore rename to panda/board/pedal_honda/.gitignore diff --git a/panda/board/pedal/Makefile b/panda/board/pedal_honda/Makefile similarity index 100% rename from panda/board/pedal/Makefile rename to panda/board/pedal_honda/Makefile diff --git a/panda/board/pedal/README b/panda/board/pedal_honda/README similarity index 100% rename from panda/board/pedal/README rename to panda/board/pedal_honda/README diff --git a/panda/board/pedal/main.c b/panda/board/pedal_honda/main.c similarity index 100% rename from panda/board/pedal/main.c rename to panda/board/pedal_honda/main.c diff --git a/panda/board/pedal/obj/.gitkeep b/panda/board/pedal_honda/obj/.gitkeep similarity index 100% rename from panda/board/pedal/obj/.gitkeep rename to panda/board/pedal_honda/obj/.gitkeep diff --git a/panda/board/pedal_toyota/.gitignore b/panda/board/pedal_toyota/.gitignore new file mode 100644 index 000000000..94053f292 --- /dev/null +++ b/panda/board/pedal_toyota/.gitignore @@ -0,0 +1 @@ +obj/* diff --git a/panda/board/pedal_toyota/Makefile b/panda/board/pedal_toyota/Makefile new file mode 100644 index 000000000..1235cc715 --- /dev/null +++ b/panda/board/pedal_toyota/Makefile @@ -0,0 +1,58 @@ +# :set noet +PROJ_NAME = comma + +CFLAGS = -O2 -Wall -std=gnu11 -DPEDAL +CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m3 +CFLAGS += -msoft-float -DSTM32F2 -DSTM32F205xx +CFLAGS += -I ../inc -I ../ -I ../../ -nostdlib +CFLAGS += -T../stm32_flash.ld + +STARTUP_FILE = startup_stm32f205xx + +CC = arm-none-eabi-gcc +OBJCOPY = arm-none-eabi-objcopy +OBJDUMP = arm-none-eabi-objdump +DFU_UTIL = "dfu-util" + +# pedal only uses the debug cert +CERT = ../../certs/debug +CFLAGS += "-DALLOW_DEBUG" + +canflash: obj/$(PROJ_NAME).bin + ../../tests/pedal/enter_canloader.py $< + +usbflash: obj/$(PROJ_NAME).bin + ../../tests/pedal/enter_canloader.py; sleep 0.5 + PYTHONPATH=../../ python -c "from python import Panda; p = [x for x in [Panda(x) for x in Panda.list()] if x.bootstub]; assert(len(p)==1); p[0].flash('obj/$(PROJ_NAME).bin', reconnect=False)" + +recover: obj/bootstub.bin obj/$(PROJ_NAME).bin + ../../tests/pedal/enter_canloader.py --recover; sleep 0.5 + $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin + $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.bin + +obj/main.o: main.c ../*.h + mkdir -p obj + $(CC) $(CFLAGS) -o $@ -c $< + +obj/bootstub.o: ../bootstub.c ../*.h + mkdir -p obj + $(CC) $(CFLAGS) -o $@ -c $< + +obj/$(STARTUP_FILE).o: ../$(STARTUP_FILE).s + $(CC) $(CFLAGS) -o $@ -c $< + +obj/%.o: ../../crypto/%.c + $(CC) $(CFLAGS) -o $@ -c $< + +obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.o + # hack + $(CC) -Wl,--section-start,.isr_vector=0x8004000 $(CFLAGS) -o obj/$(PROJ_NAME).elf $^ + $(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin + SETLEN=1 ../../crypto/sign.py obj/code.bin $@ $(CERT) + +obj/bootstub.bin: obj/$(STARTUP_FILE).o obj/bootstub.o obj/sha.o obj/rsa.o + $(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^ + $(OBJCOPY) -v -O binary obj/bootstub.$(PROJ_NAME).elf $@ + +clean: + rm -f obj/* diff --git a/panda/board/pedal_toyota/README b/panda/board/pedal_toyota/README new file mode 100644 index 000000000..f7c56c3bf --- /dev/null +++ b/panda/board/pedal_toyota/README @@ -0,0 +1,30 @@ +MOVE ALL FILES TO board/pedal TO FLASH + + +This is the firmware for the comma pedal. It borrows a lot from panda. + +The comma pedal is a gas pedal interceptor for Honda/Acura. It allows you to "virtually" press the pedal. + +This is the open source software. Note that it is not ready to use yet. + +== Test Plan == + +* Startup +** Confirm STATE_FAULT_STARTUP +* Timeout +** Send value +** Confirm value is output +** Stop sending messages +** Confirm value is passthru after 100ms +** Confirm STATE_FAULT_TIMEOUT +* Random values +** Send random 6 byte messages +** Confirm random values cause passthru +** Confirm STATE_FAULT_BAD_CHECKSUM +* Same message lockout +** Send same message repeated +** Confirm timeout behavior +* Don't set enable +** Confirm no output +* Set enable and values +** Confirm output diff --git a/panda/board/pedal_toyota/main.c b/panda/board/pedal_toyota/main.c new file mode 100644 index 000000000..a9b6cec46 --- /dev/null +++ b/panda/board/pedal_toyota/main.c @@ -0,0 +1,291 @@ +//#define DEBUG +//#define CAN_LOOPBACK_MODE +//#define USE_INTERNAL_OSC + +#include "../config.h" + +#include "drivers/drivers.h" +#include "drivers/llgpio.h" +#include "gpio.h" + +#define CUSTOM_CAN_INTERRUPTS + +#include "libc.h" +#include "safety.h" +#include "drivers/adc.h" +#include "drivers/uart.h" +#include "drivers/dac.h" +#include "drivers/can.h" +#include "drivers/timer.h" + +#define CAN CAN1 + +//#define PEDAL_USB + +#ifdef PEDAL_USB + #include "drivers/usb.h" +#endif + +#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef +uint32_t enter_bootloader_mode; + +void __initialize_hardware_early() { + early(); +} + +// ********************* serial debugging ********************* + +void debug_ring_callback(uart_ring *ring) { + char rcv; + while (getc(ring, &rcv)) { + putc(ring, rcv); + } +} + +#ifdef PEDAL_USB + +int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) { return 0; } +void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) {} +void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) {} +void usb_cb_enumeration_complete() {} + +int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { + int resp_len = 0; + uart_ring *ur = NULL; + switch (setup->b.bRequest) { + // **** 0xe0: uart read + case 0xe0: + ur = get_ring_by_number(setup->b.wValue.w); + if (!ur) break; + if (ur == &esp_ring) uart_dma_drain(); + // read + while ((resp_len < min(setup->b.wLength.w, MAX_RESP_LEN)) && + getc(ur, (char*)&resp[resp_len])) { + ++resp_len; + } + break; + } + return resp_len; +} + +#endif + +// ***************************** toyota can checksum **************************** + +int can_cksum(uint8_t *dat, uint8_t len, uint16_t addr) +{ + uint8_t checksum = 0; + checksum =((addr & 0xFF00) >> 8) + (addr & 0x00FF) + len + 1; + //uint16_t temp_msg = msg; + + for (int ii = 0; ii < len; ii++) + { + checksum += (dat[ii]); + //temp_msg = temp_msg >> 8; + } + //return ((msg & ~0xFF) & (checksum & 0xFF)); + return checksum; +} + +// ***************************** can port ***************************** + +// addresses to be used on CAN +#define CAN_GAS_INPUT 0x200 +#define CAN_GAS_OUTPUT 0x201 + +void CAN1_TX_IRQHandler() { + // clear interrupt + CAN->TSR |= CAN_TSR_RQCP0; +} + +// two independent values +uint16_t gas_set_0 = 0; +uint16_t gas_set_1 = 0; + +#define MAX_TIMEOUT 10 +uint32_t timeout = 0; + +#define NO_FAULT 0 +#define FAULT_BAD_CHECKSUM 1 +#define FAULT_SEND 2 +#define FAULT_SCE 3 +#define FAULT_STARTUP 4 +#define FAULT_TIMEOUT 5 +#define FAULT_INVALID 6 +uint8_t state = FAULT_STARTUP; + +void CAN1_RX0_IRQHandler() { + while (CAN->RF0R & CAN_RF0R_FMP0) { + #ifdef DEBUG + puts("CAN RX\n"); + #endif + uint32_t address = CAN->sFIFOMailBox[0].RIR>>21; + if (address == CAN_GAS_INPUT) { + // softloader entry + if (CAN->sFIFOMailBox[0].RDLR == 0xdeadface) { + if (CAN->sFIFOMailBox[0].RDHR == 0x0ab00b1e) { + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + } else if (CAN->sFIFOMailBox[0].RDHR == 0x02b00b1e) { + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + } + } + + // normal packet + uint8_t *dat = (uint8_t *)&CAN->sFIFOMailBox[0].RDLR; + uint8_t *dat2 = (uint8_t *)&CAN->sFIFOMailBox[0].RDHR; + uint16_t value_0 = (dat[0] << 8) | dat[1]; + uint16_t value_1 = (dat[2] << 8) | dat[3]; + uint8_t enable = (dat2[0] >> 7) & 1; + uint8_t index = 0; + if (can_cksum(dat, 5, CAN_GAS_INPUT) == dat2[1]) { + if (index == 0) { + #ifdef DEBUG + puts("setting gas "); + puth(value); + puts("\n"); + #endif + if (enable) { + gas_set_0 = value_0; + gas_set_1 = value_1; + } else { + // clear the fault state if values are 0 + if (value_0 == 0 && value_1 == 0) { + state = NO_FAULT; + } else { + state = FAULT_INVALID; + } + gas_set_0 = gas_set_1 = 0; + } + // clear the timeout + timeout = 0; + } + + } else { + // wrong checksum = fault + state = FAULT_BAD_CHECKSUM; + } + } + // next + CAN->RF0R |= CAN_RF0R_RFOM0; + } +} + +void CAN1_SCE_IRQHandler() { + state = FAULT_SCE; + can_sce(CAN); +} + +int pdl0 = 0, pdl1 = 0; + + +int led_value = 0; + +void TIM3_IRQHandler() { + #ifdef DEBUG + puth(TIM3->CNT); + puts(" "); + puth(pdl0); + puts(" "); + puth(pdl1); + puts("\n"); + #endif + + // check timer for sending the user pedal and clearing the CAN + if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { + uint8_t dat[8]; + dat[0] = (pdl0>>8)&0xFF; + dat[1] = (pdl0>>0)&0xFF; + dat[2] = (pdl1>>8)&0xFF; + dat[3] = (pdl1>>0)&0xFF; + dat[4] = state; + dat[5] = can_cksum(dat, 5, CAN_GAS_OUTPUT); + 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 + CAN->sTxMailBox[0].TIR = (CAN_GAS_OUTPUT << 21) | 1; + } else { + // old can packet hasn't sent! + state = FAULT_SEND; + #ifdef DEBUG + puts("CAN MISS\n"); + #endif + } + + // blink the LED + set_led(LED_GREEN, led_value); + led_value = !led_value; + + TIM3->SR = 0; + + // up timeout for gas set + if (timeout == MAX_TIMEOUT) { + state = FAULT_TIMEOUT; + } else { + timeout += 1; + } +} + +// ***************************** main code ***************************** + +void pedal() { + // read/write + pdl0 = adc_get(ADCCHAN_ACCEL0); + pdl1 = adc_get(ADCCHAN_ACCEL1); + + // write the pedal to the DAC + if (state == NO_FAULT) { + dac_set(0, max(gas_set_0, pdl0)); + dac_set(1, max(gas_set_1, pdl1)); + } else { + dac_set(0, pdl0); + dac_set(1, pdl1); + } + + // feed the watchdog + IWDG->KR = 0xAAAA; +} + +int main() { + __disable_irq(); + + // init devices + clock_init(); + periph_init(); + gpio_init(); + +#ifdef PEDAL_USB + // enable USB + usb_init(); +#endif + + // pedal stuff + dac_init(); + adc_init(); + + // init can + can_silent = ALL_CAN_LIVE; + can_init(0); + + // 48mhz / 65536 ~= 732 + timer_init(TIM3, 15); + NVIC_EnableIRQ(TIM3_IRQn); + + // setup watchdog + IWDG->KR = 0x5555; + IWDG->PR = 0; // divider /4 + // 0 = 0.125 ms, let's have a 50ms watchdog + IWDG->RLR = 400 - 1; + IWDG->KR = 0xCCCC; + + puts("**** INTERRUPTS ON ****\n"); + __enable_irq(); + + // main pedal loop + while (1) { + pedal(); + } + + return 0; +} \ No newline at end of file diff --git a/panda/board/pedal_toyota/obj/.gitkeep b/panda/board/pedal_toyota/obj/.gitkeep new file mode 100644 index 000000000..e69de29bb diff --git a/panda/board/safety.h b/panda/board/safety.h index 4d2b46899..6e5dc8e36 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -57,12 +57,14 @@ int controls_allowed = 0; #ifdef PANDA #include "safety/safety_toyota_ipas.h" #include "safety/safety_tesla.h" +#include "safety/safety_gm_ascm.h" #endif #include "safety/safety_gm.h" #include "safety/safety_ford.h" #include "safety/safety_cadillac.h" #include "safety/safety_hyundai.h" #include "safety/safety_chrysler.h" +#include "safety/safety_subaru.h" #include "safety/safety_elm327.h" const safety_hooks *current_hooks = &nooutput_hooks; @@ -104,6 +106,8 @@ typedef struct { #define SAFETY_HYUNDAI 7 #define SAFETY_TESLA 8 #define SAFETY_CHRYSLER 9 +#define SAFETY_SUBARU 10 +#define SAFETY_GM_ASCM 0x1334 #define SAFETY_TOYOTA_IPAS 0x1335 #define SAFETY_TOYOTA_NOLIMITS 0x1336 #define SAFETY_ALLOUTPUT 0x1337 @@ -119,9 +123,11 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_CADILLAC, &cadillac_hooks}, {SAFETY_HYUNDAI, &hyundai_hooks}, {SAFETY_CHRYSLER, &chrysler_hooks}, + {SAFETY_SUBARU, &subaru_hooks}, {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks}, #ifdef PANDA {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks}, + {SAFETY_GM_ASCM, &gm_ascm_hooks}, {SAFETY_TESLA, &tesla_hooks}, #endif {SAFETY_ALLOUTPUT, &alloutput_hooks}, diff --git a/panda/board/safety/safety_gm_ascm.h b/panda/board/safety/safety_gm_ascm.h new file mode 100644 index 000000000..70a042ec5 --- /dev/null +++ b/panda/board/safety/safety_gm_ascm.h @@ -0,0 +1,52 @@ +// BUS 0 is on the LKAS module (ASCM) side +// BUS 2 is on the actuator (EPS) side + +static int gm_ascm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + + uint32_t addr = to_fwd->RIR>>21; + + if (bus_num == 0) { + + // do not propagate lkas messages from ascm to actuators + // block 0x152 and 0x154, which are the lkas command from ASCM1 and ASCM2 + // block 0x315 and 0x2cb, which are the brake and accel commands from ASCM1 + //if ((addr == 0x152) || (addr == 0x154) || (addr == 0x315) || (addr == 0x2cb)) { + if ((addr == 0x152) || (addr == 0x154)) { + int supercruise_on = (to_fwd->RDHR>>4) & 0x1; // bit 36 + if (!supercruise_on) return -1; + } + + // on the chassis bus, the OBDII port is on the module side, so we need to read + // the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to + // the actuator as 0x152 and 0x154 + if (addr == 0x151) { + to_fwd->RIR = (0x152 << 21) | (to_fwd->RIR & 0x1fffff); + } + if (addr == 0x153) { + to_fwd->RIR = (0x154 << 21) | (to_fwd->RIR & 0x1fffff); + } + + // brake + if (addr == 0x314) { + to_fwd->RIR = (0x315 << 21) | (to_fwd->RIR & 0x1fffff); + } + + return 2; + } + + if (bus_num == 2) { + return 0; + } + + return -1; +} + +const safety_hooks gm_ascm_hooks = { + .init = nooutput_init, + .rx = default_rx_hook, + .tx = alloutput_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .ignition = default_ign_hook, + .fwd = gm_ascm_fwd_hook, +}; + diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index caac72730..b67632141 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -1,4 +1,4 @@ -const int HYUNDAI_MAX_STEER = 250; +const int HYUNDAI_MAX_STEER = 255; // like stock const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks const int32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks const int HYUNDAI_MAX_RATE_UP = 3; diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h new file mode 100644 index 000000000..3d21d2269 --- /dev/null +++ b/panda/board/safety/safety_subaru.h @@ -0,0 +1,49 @@ +void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {} + +// FIXME +// *** all output safety mode *** + +static void subaru_init(int16_t param) { + controls_allowed = 1; +} + +static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + return true; +} + +static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + + // shifts bits 29 > 11 + int32_t addr = to_fwd->RIR >> 21; + + // forward CAN 0 > 1 + if (bus_num == 0) { + return 1; // ES CAN + } + // forward CAN 1 > 0, except ES_LKAS + else if (bus_num == 1) { + + // outback 2015 + if (addr == 0x164) { + return -1; + } + // global platform + if (addr == 0x122) { + return -1; + } + + return 0; // Main CAN + } + + // fallback to do not forward + return -1; +} + +const safety_hooks subaru_hooks = { + .init = subaru_init, + .rx = subaru_rx_hook, + .tx = subaru_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .ignition = default_ign_hook, + .fwd = subaru_fwd_hook, +}; \ No newline at end of file diff --git a/panda/crypto/sha.c b/panda/crypto/sha.c index 47676df4b..8e1715525 100644 --- a/panda/crypto/sha.c +++ b/panda/crypto/sha.c @@ -127,10 +127,36 @@ const uint8_t* SHA_final(SHA_CTX* ctx) { while ((ctx->count & 63) != 56) { SHA_update(ctx, (uint8_t*)"\0", 1); } - for (i = 0; i < 8; ++i) { - uint8_t tmp = (uint8_t) (cnt >> ((7 - i) * 8)); - SHA_update(ctx, &tmp, 1); - } + + /* Hack - right shift operator with non const argument requires + * libgcc.a which is missing in EON + * thus expanding for loop from + + for (i = 0; i < 8; ++i) { + uint8_t tmp = (uint8_t) (cnt >> ((7 - i) * 8)); + SHA_update(ctx, &tmp, 1); + } + + to + */ + + uint8_t tmp = 0; + tmp = (uint8_t) (cnt >> ((7 - 0) * 8)); + SHA_update(ctx, &tmp, 1); + tmp = (uint8_t) (cnt >> ((7 - 1) * 8)); + SHA_update(ctx, &tmp, 1); + tmp = (uint8_t) (cnt >> ((7 - 2) * 8)); + SHA_update(ctx, &tmp, 1); + tmp = (uint8_t) (cnt >> ((7 - 3) * 8)); + SHA_update(ctx, &tmp, 1); + tmp = (uint8_t) (cnt >> ((7 - 4) * 8)); + SHA_update(ctx, &tmp, 1); + tmp = (uint8_t) (cnt >> ((7 - 5) * 8)); + SHA_update(ctx, &tmp, 1); + tmp = (uint8_t) (cnt >> ((7 - 6) * 8)); + SHA_update(ctx, &tmp, 1); + tmp = (uint8_t) (cnt >> ((7 - 7) * 8)); + SHA_update(ctx, &tmp, 1); for (i = 0; i < 5; i++) { uint32_t tmp = ctx->state[i]; diff --git a/panda/tests/safety/test_hyundai.py b/panda/tests/safety/test_hyundai.py index 0a6ce0f91..bcf9b7611 100644 --- a/panda/tests/safety/test_hyundai.py +++ b/panda/tests/safety/test_hyundai.py @@ -5,7 +5,7 @@ import libpandasafety_py MAX_RATE_UP = 3 MAX_RATE_DOWN = 7 -MAX_STEER = 250 +MAX_STEER = 255 MAX_RT_DELTA = 112 RT_INTERVAL = 250000 diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt index 2e892560f..8cadf936e 100644 --- a/requirements_openpilot.txt +++ b/requirements_openpilot.txt @@ -15,7 +15,7 @@ cffi==1.11.5 contextlib2==0.5.4 crc16==0.1.1 crcmod==1.7 -cryptography==1.4 +cryptography==2.1.4 cycler==0.10.0 decorator==4.0.10 docopt==0.6.2 @@ -31,7 +31,7 @@ nose==1.3.7 numpy==1.11.1 pause==0.1.2 py==1.4.31 -pyOpenSSL==16.0.0 +pyOpenSSL==17.5.0 pyasn1-modules==0.0.8 pyasn1==0.1.9 pycapnp==0.6.3 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index d0246dd16..67a224c0d 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python from common.realtime import sec_since_boot -from cereal import car, log +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 @@ -256,7 +256,7 @@ class CarInterface(object): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c, perception_state=log.Live20Data.new_message()): + def apply(self, c): if (self.CS.frame == -1): return False # if we haven't seen a frame 220, then do not update. diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 633ce4b8a..0e29ea626 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python from common.realtime import sec_since_boot -from cereal import car, log +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 @@ -210,7 +210,7 @@ class CarInterface(object): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c, perception_state=log.Live20Data.new_message()): + def apply(self, c): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.hudControl.visualAlert, c.cruiseControl.cancel) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index fd90ffd4d..b242bc57b 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -4,22 +4,24 @@ from selfdrive.config import Conversions as CV from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan -from selfdrive.car.gm.values import CAR, DBC +from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS from selfdrive.can.packer import CANPacker class CarControllerParams(): def __init__(self, car_fingerprint): - if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): - self.STEER_MAX = 300 - self.STEER_STEP = 2 # how often we update the steer cmd - self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s) - self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero - elif car_fingerprint == CAR.CADILLAC_CT6: + if car_fingerprint in SUPERCRUISE_CARS: self.STEER_MAX = 150 self.STEER_STEP = 1 # how often we update the steer cmd self.STEER_DELTA_UP = 2 # 0.75s time to peak torque self.STEER_DELTA_DOWN = 5 # 0.3s from peak torque to zero + self.MIN_STEER_SPEED = -1. # can steer down to zero + else: + self.STEER_MAX = 300 + self.STEER_STEP = 2 # how often we update the steer cmd + self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s) + self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero + self.MIN_STEER_SPEED = 3. self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily @@ -94,7 +96,7 @@ class CarController(object): ### STEER ### if (frame % P.STEER_STEP) == 0: - lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3. + lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED if lkas_enabled: apply_steer = actuators.steer * P.STEER_MAX apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) @@ -104,16 +106,16 @@ class CarController(object): self.apply_steer_last = apply_steer idx = (frame / P.STEER_STEP) % 4 - if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): - can_sends.append(gmcan.create_steering_control(self.packer_pt, - canbus.powertrain, apply_steer, idx, lkas_enabled)) - if self.car_fingerprint == CAR.CADILLAC_CT6: + 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) + else: + can_sends.append(gmcan.create_steering_control(self.packer_pt, + canbus.powertrain, apply_steer, idx, lkas_enabled)) ### GAS/BRAKE ### - if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): + if self.car_fingerprint not in SUPERCRUISE_CARS: # no output if not enabled, but keep sending keepalive messages # treat pedals as one final_pedal = actuators.gas - actuators.brake @@ -164,17 +166,17 @@ class CarController(object): if frame % P.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(canbus.powertrain) - # Show green icon when LKA torque is applied, and - # alarming orange icon when approaching torque limit. - # If not sent again, LKA icon disappears in about 5 seconds. - # Conveniently, sending camera message periodically also works as a keepalive. - lka_active = CS.lkas_status == 1 - lka_critical = lka_active and abs(actuators.steer) > 0.9 - 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)) - self.lka_icon_status_last = lka_icon_status + # Show green icon when LKA torque is applied, and + # alarming orange icon when approaching torque limit. + # If not sent again, LKA icon disappears in about 5 seconds. + # Conveniently, sending camera message periodically also works as a keepalive. + lka_active = CS.lkas_status == 1 + lka_critical = lka_active and abs(actuators.steer) > 0.9 + 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)) + self.lka_icon_status_last = lka_icon_status # Send chimes if self.chime != chime: diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 24c73a307..d5fa975de 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -5,7 +5,7 @@ from selfdrive.config import Conversions as CV from selfdrive.can.parser import CANParser from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \ CruiseButtons, is_eps_status_ok, \ - STEER_THRESHOLD + STEER_THRESHOLD, SUPERCRUISE_CARS def get_powertrain_can_parser(CP, canbus): # this function generates lists for signal, messages and initial values @@ -35,17 +35,17 @@ def get_powertrain_can_parser(CP, canbus): signals += [ ("RegenPaddle", "EBCMRegenPaddle", 0), ] - if CP.carFingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): + if CP.carFingerprint in SUPERCRUISE_CARS: + signals += [ + ("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0) + ] + else: signals += [ ("TractionControlOn", "ESPStatus", 0), ("EPBClosed", "EPBStatus", 0), ("CruiseMainOn", "ECMEngineStatus", 0), ("CruiseState", "AcceleratorPedal2", 0), ] - if CP.carFingerprint == CAR.CADILLAC_CT6: - signals += [ - ("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0) - ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain) @@ -121,7 +121,14 @@ class CarState(object): self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1 self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2 - if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): + 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) + else: self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed'] self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'] self.acc_active = False @@ -131,13 +138,6 @@ class CarState(object): self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) else: self.regen_pressed = False - if self.car_fingerprint == CAR.CADILLAC_CT6: - 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) # Brake pedal's potentiometer returns near-zero reading # even when pedal is not pressed. diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 9ec1b9e5d..83e90c3c7 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,10 +1,10 @@ #!/usr/bin/env python -from cereal import car, log +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 create_event, EventTypes as ET from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD +from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, SUPERCRUISE_CARS from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser try: @@ -107,6 +107,15 @@ class CarInterface(object): ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 + elif candidate == CAR.BUICK_REGAL: + ret.minEnableSpeed = 18 * CV.MPH_TO_MS + ret.mass = 3779. * CV.LB_TO_KG + std_cargo # (3849+3708)/2 + ret.safetyModel = car.CarParams.SafetyModels.gm + ret.wheelbase = 2.83 #111.4 inches in meters + ret.steerRatio = 14.4 # guess for tourx + ret.steerRatioRear = 0. + ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx + elif candidate == CAR.CADILLAC_ATS: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1601 + std_cargo @@ -285,7 +294,13 @@ class CarInterface(object): if ret.seatbeltUnlatched: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): + if self.CS.car_fingerprint in SUPERCRUISE_CARS: + if self.CS.acc_active and not self.acc_active_prev: + events.append(create_event('pcmEnable', [ET.ENABLE])) + if not self.CS.acc_active: + 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: @@ -318,13 +333,6 @@ class CarInterface(object): if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) - if self.CS.car_fingerprint == CAR.CADILLAC_CT6: - - if self.CS.acc_active and not self.acc_active_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - if not self.CS.acc_active: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - ret.events = events # update previous brake/gas pressed @@ -337,7 +345,7 @@ class CarInterface(object): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c, perception_state=log.Live20Data.new_message()): + def apply(self, c): hud_v_cruise = c.hudControl.setSpeed if hud_v_cruise > 70: hud_v_cruise = 0 diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 17260fe27..27ca73ff6 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -10,6 +10,9 @@ class CAR: CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018" MALIBU = "CHEVROLET MALIBU PREMIER 2017" ACADIA = "GMC ACADIA DENALI 2018" + BUICK_REGAL = "BUICK REGAL ESSENCE 2018" + +SUPERCRUISE_CARS = [CAR.CADILLAC_CT6] class CruiseButtons: UNPRESS = 1 @@ -39,10 +42,10 @@ AUDIO_HUD = { def is_eps_status_ok(eps_status, car_fingerprint): valid_eps_status = [] - if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): - valid_eps_status += [0, 1] - elif car_fingerprint == CAR.CADILLAC_CT6: + if car_fingerprint in SUPERCRUISE_CARS: valid_eps_status += [0, 1, 4, 5, 6] + else: + valid_eps_status += [0, 1] return eps_status in valid_eps_status def parse_gear_shifter(can_gear): @@ -71,6 +74,11 @@ FINGERPRINTS = { { 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 }], + CAR.BUICK_REGAL : [ + # Regal TourX Essence w/ ACC 2018 + { + 190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 8, 419: 8, 422: 4, 426: 8, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 463: 3, 479: 8, 481: 7, 485: 8, 487: 8, 489: 8, 495: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 884: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 8, 969: 8, 977: 8, 979: 8, 985: 8, 1001: 8, 1005: 6, 1009: 8, 1011: 8, 1013: 3, 1017: 8, 1020: 8, 1024: 8, 1025: 8, 1026: 8, 1027: 8, 1028: 8, 1029: 8, 1030: 8, 1031: 8, 1032: 2, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 8, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 8, 1263: 8, 1265: 8, 1267: 8, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1602: 8, 1603: 7, 1611: 8, 1618: 8, 1906: 8, 1907: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1930: 7, 2016: 8, 2018: 8, 2019: 8, 2024: 8, 2026: 8 + }], CAR.CADILLAC_ATS: [ # Cadillac ATS Coupe Premium Performance 3.6L RWD w/ ACC 2018 { @@ -99,6 +107,7 @@ STOCK_CONTROL_MSGS = { CAR.MALIBU: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" CAR.ACADIA: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" CAR.CADILLAC_ATS: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" + CAR.BUICK_REGAL: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" CAR.CADILLAC_CT6: [], # CT6 does not require ASCMs to be disconnected } @@ -108,5 +117,6 @@ DBC = { CAR.MALIBU: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), CAR.CADILLAC_CT6: dbc_dict('cadillac_ct6_powertrain', 'cadillac_ct6_object', chassis_dbc='cadillac_ct6_chassis'), } diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 1cb9830e3..f87f56e49 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -86,7 +86,7 @@ class CarController(object): def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ - radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \ + hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_alert, snd_beep, snd_chime): """ Controls thread """ @@ -121,7 +121,7 @@ class CarController(object): # For lateral control-only, send chimes as a beep since we don't send 0x1fa if CS.CP.radarOffCan: - snd_beep = snd_beep if snd_beep is not 0 else snd_chime + snd_beep = snd_beep if snd_beep != 0 else snd_chime #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index a064a0b72..00aa358d8 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -48,6 +48,7 @@ def get_can_signals(CP): ("ESP_DISABLED", "VSA_STATUS", 1), ("HUD_LEAD", "ACC_HUD", 0), ("USER_BRAKE", "VSA_STATUS", 0), + ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0), ("STEER_STATUS", "STEER_STATUS", 5), ("GEAR_SHIFTER", "GEARBOX", 0), ("PEDAL_GAS", "POWERTRAIN_DATA", 0), @@ -76,7 +77,6 @@ def get_can_signals(CP): signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_FEEDBACK", 0), ("EPB_STATE", "EPB_STATUS", 0), - ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0), ("CRUISE_SPEED", "ACC_HUD", 0)] checks += [("GAS_PEDAL_2", 100)] else: @@ -101,8 +101,7 @@ def get_can_signals(CP): if CP.carFingerprint == CAR.CIVIC: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_FEEDBACK", 0), - ("EPB_STATE", "EPB_STATUS", 0), - ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)] + ("EPB_STATE", "EPB_STATUS", 0)] elif CP.carFingerprint == CAR.ACURA_ILX: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_BUTTONS", 0)] @@ -110,8 +109,7 @@ def get_can_signals(CP): signals += [("MAIN_ON", "SCM_BUTTONS", 0)] elif CP.carFingerprint == CAR.ODYSSEY: signals += [("MAIN_ON", "SCM_FEEDBACK", 0), - ("EPB_STATE", "EPB_STATUS", 0), - ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)] + ("EPB_STATE", "EPB_STATUS", 0)] checks += [("EPB_STATUS", 50)] elif CP.carFingerprint == CAR.PILOT: signals += [("MAIN_ON", "SCM_BUTTONS", 0), @@ -248,14 +246,13 @@ class CarState(object): 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'] + 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): self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 - self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] else: self.park_brake = 0 # TODO - self.brake_hold = 0 # TODO self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER']) @@ -303,8 +300,9 @@ class CarState(object): self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD'] - - # gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models + + # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models + # TODO: this should be ok for all cars. Verify it. if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE): if self.user_brake > 0.05: self.brake_pressed = 1 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 19b0cd9eb..3acd2c741 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import os import numpy as np -from cereal import car, log +from cereal import car from common.numpy_fast import clip, interp from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog @@ -575,7 +575,7 @@ class CarInterface(object): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c, perception_state=log.Live20Data.new_message()): + def apply(self, c): if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: @@ -584,19 +584,19 @@ class CarInterface(object): hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw] snd_beep, snd_chime = AUDIO_HUD[c.hudControl.audibleAlert.raw] - pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6) + pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6) - self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \ - c.actuators, \ - c.cruiseControl.speedOverride, \ - c.cruiseControl.override, \ - c.cruiseControl.cancel, \ - pcm_accel, \ - perception_state.radarErrors, \ - hud_v_cruise, c.hudControl.lanesVisible, \ - hud_show_car = c.hudControl.leadVisible, \ - hud_alert = hud_alert, \ - snd_beep = snd_beep, \ - snd_chime = snd_chime) + self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, + c.actuators, + c.cruiseControl.speedOverride, + c.cruiseControl.override, + c.cruiseControl.cancel, + pcm_accel, + hud_v_cruise, + c.hudControl.lanesVisible, + hud_show_car=c.hudControl.leadVisible, + hud_alert=hud_alert, + snd_beep=snd_beep, + snd_chime=snd_chime) self.frame += 1 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 3cd8d136f..627e27b4c 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -10,7 +10,7 @@ from selfdrive.can.packer import CANPacker # Steer torque limits class SteerLimitParams: - STEER_MAX = 250 # 409 is the max + STEER_MAX = 255 # 409 is the max, 255 is stock STEER_DELTA_UP = 3 STEER_DELTA_DOWN = 7 STEER_DRIVER_ALLOWANCE = 50 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 01fd28a58..3b423ae04 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -50,6 +50,7 @@ def get_can_parser(CP): ("CF_Clu_InhibitR", "CLU15", 0), ("CF_Lvr_Gear","LVR12",0), + ("CUR_GR", "TCU12",0), ("ACCEnable", "TCS13", 0), ("ACC_REQ", "TCS13", 0), @@ -234,6 +235,17 @@ class CarState(object): else: self.gear_shifter_cluster = "unknown" + # Gear Selecton via TCU12 + gear2 = cp.vl["TCU12"]["CUR_GR"] + if gear2 == 0: + self.gear_tcu = "park" + elif gear2 == 14: + self.gear_tcu = "reverse" + elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently + self.gear_tcu = "drive" + else: + self.gear_tcu = "unknown" + # save the entire LKAS11 and CLU11 self.lkas11 = cp_cam.vl["LKAS11"] self.clu11 = cp.vl["CLU11"] diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 5399d8276..a854eebbc 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,11 +1,11 @@ #!/usr/bin/env python -from cereal import car, log +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 CAMERA_MSGS, CAR, get_hud_alerts +from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES try: from selfdrive.car.hyundai.carcontroller import CarController @@ -113,6 +113,14 @@ class CarInterface(object): ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerKpV, ret.steerKiV = [[0.16], [0.01]] ret.minSteerSpeed = 35 * CV.MPH_TO_MS + elif candidate == CAR.KIA_OPTIMA: + ret.steerKf = 0.00005 + ret.mass = 3558 * CV.LB_TO_KG + ret.wheelbase = 2.80 + ret.steerRatio = 13.75 + tire_stiffness_factor = 0.5 + ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] + ret.steerKpV, ret.steerKiV = [[0.25], [0.05]] elif candidate == CAR.KIA_STINGER: ret.steerKf = 0.00005 ret.steerRateCost = 0.5 @@ -192,8 +200,10 @@ class CarInterface(object): ret.wheelSpeeds.rr = self.CS.v_wheel_rr # gear shifter - if self.CP.carFingerprint == CAR.ELANTRA: + 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 @@ -300,7 +310,7 @@ class CarInterface(object): return ret.as_reader() - def apply(self, c, perception_state=log.Live20Data.new_message()): + def apply(self, c): hud_alert = get_hud_alerts(c.hudControl.visualAlert, c.hudControl.audibleAlert) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 56b6e6053..1756976e9 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -13,6 +13,7 @@ def get_hud_alerts(visual_alert, audible_alert): class CAR: ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017" GENESIS = "HYUNDAI GENESIS 2018" + KIA_OPTIMA = "KIA OPTIMA SX 2019" KIA_SORENTO = "KIA SORENTO GT LINE 2018" KIA_STINGER = "KIA STINGER GT2 2018" SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" @@ -33,6 +34,9 @@ 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_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 }], @@ -53,12 +57,18 @@ CAMERA_MSGS = [832, 1156, 1191, 1342] CHECKSUM = { "crc8": [CAR.SANTA_FE], "6B": [CAR.KIA_SORENTO, CAR.GENESIS], - "7B": [CAR.KIA_STINGER, CAR.ELANTRA], + "7B": [CAR.KIA_STINGER, CAR.ELANTRA, CAR.KIA_OPTIMA], +} + +FEATURES = { + "use_cluster_gears": [CAR.ELANTRA], # Use Cluster for Gear Selection, rather than Transmission + "use_tcu_gears": [CAR.KIA_OPTIMA], # Use TCU Message for Gear Selection } DBC = { CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), CAR.GENESIS: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None), CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None), CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None), diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index b7d60a425..3a5e05257 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python import zmq -from cereal import car, log +from cereal import car from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.swaglog import cloudlog @@ -118,6 +118,6 @@ class CarInterface(object): return ret.as_reader() - def apply(self, c, perception_state=log.Live20Data.new_message()): + def apply(self, c): # in mock no carcontrols return False diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 620c9a2dd..8c1ed1192 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -5,7 +5,7 @@ from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ create_steer_command, create_ui_command, \ create_ipas_steer_command, create_accel_command, \ - create_fcw_command + create_fcw_command, create_gas_command from selfdrive.car.toyota.values import ECU, STATIC_MSGS from selfdrive.can.packer import CANPacker @@ -129,7 +129,16 @@ class CarController(object): # *** compute control surfaces *** # gas and brake - apply_accel = actuators.gas - actuators.brake + + apply_gas = clip(actuators.gas, 0., 1.) + + if CS.CP.enableGasInterceptor: + # send only negative accel if interceptor is detected. otherwise, send the regular value + # +0.06 offset to reduce ABS pump usage when OP is engaged + apply_accel = 0.06 - actuators.brake + else: + apply_accel = actuators.gas - actuators.brake + apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) @@ -213,6 +222,11 @@ class CarController(object): else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) + if CS.CP.enableGasInterceptor: + # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. + # This prevents unexpected pedal range rescaling + can_sends.append(create_gas_command(self.packer, apply_gas)) + if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera: for addr in TARGET_IDS: can_sends.append(create_video_target(frame/10, addr)) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 71d33b0ab..bb3b3f052 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -62,6 +62,11 @@ def get_can_parser(CP): 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)) + checks.append(("GAS_SENSOR", 50)) return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) @@ -112,7 +117,10 @@ class CarState(object): self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] - self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] + if self.CP.enableGasInterceptor: + self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + else: + self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] self.car_gas = self.pedal_gas self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 03a19b8a2..8b6cb0a01 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python from common.realtime import sec_since_boot -from cereal import car, log +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 @@ -61,7 +61,7 @@ class CarInterface(object): ret.safetyModel = car.CarParams.SafetyModels.toyota # pedal - ret.enableCruise = True + ret.enableCruise = not ret.enableGasInterceptor # FIXME: hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars @@ -77,6 +77,7 @@ class CarInterface(object): ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay if candidate == CAR.PRIUS: + stop_and_go = True ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 15.00 # unknown end-to-end spec @@ -88,6 +89,7 @@ class CarInterface(object): ret.steerActuatorDelay = 0.25 elif candidate in [CAR.RAV4, CAR.RAV4H]: + stop_and_go = True if (candidate in CAR.RAV4H) else False ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.65 ret.steerRatio = 16.30 # 14.5 is spec end-to-end @@ -97,6 +99,7 @@ class CarInterface(object): ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 elif candidate == CAR.COROLLA: + stop_and_go = False ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 17.8 @@ -106,6 +109,7 @@ class CarInterface(object): ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 elif candidate == CAR.LEXUS_RXH: + stop_and_go = True ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.79 ret.steerRatio = 16. # 14.8 is spec end-to-end @@ -115,6 +119,7 @@ class CarInterface(object): ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 elif candidate in [CAR.CHR, CAR.CHRH]: + stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.63906 ret.steerRatio = 13.6 @@ -124,6 +129,7 @@ class CarInterface(object): ret.steerKf = 0.00006 elif candidate in [CAR.CAMRY, CAR.CAMRYH]: + stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.82448 ret.steerRatio = 13.7 @@ -133,6 +139,7 @@ class CarInterface(object): ret.steerKf = 0.00006 elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: + stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.78 ret.steerRatio = 16.0 @@ -147,14 +154,12 @@ class CarInterface(object): ret.longPidDeadzoneBP = [0., 9.] ret.longPidDeadzoneV = [0., .15] + #detect the Pedal address + ret.enableGasInterceptor = 0x201 in fingerprint + # 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. - # hybrid models can't do stop and go even though the stock ACC can't - if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.CHR, - CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.HIGHLANDERH, CAR.HIGHLANDER]: - ret.minEnableSpeed = -1. - elif candidate in [CAR.RAV4, CAR.COROLLA]: # TODO: hack ICE to do stop and go - ret.minEnableSpeed = 19. * CV.MPH_TO_MS + ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for @@ -178,8 +183,6 @@ class CarInterface(object): # 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] @@ -190,15 +193,24 @@ class CarInterface(object): cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu) cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs) + cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.steerLimitAlert = False + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKiBP = [0., 35.] ret.stoppingControl = False ret.startAccel = 0.0 - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [3.6, 2.4, 1.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.54, 0.36] + if ret.enableGasInterceptor: + ret.gasMaxBP = [0., 9., 35] + ret.gasMaxV = [0.2, 0.5, 0.7] + ret.longitudinalKpV = [1.2, 0.8, 0.5] + ret.longitudinalKiV = [0.18, 0.12] + else: + ret.gasMaxBP = [0.] + ret.gasMaxV = [0.5] + ret.longitudinalKpV = [3.6, 2.4, 1.5] + ret.longitudinalKiV = [0.54, 0.36] return ret @@ -234,7 +246,11 @@ class CarInterface(object): # gas pedal ret.gas = self.CS.car_gas - ret.gasPressed = self.CS.pedal_gas > 0 + 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 @@ -253,9 +269,11 @@ class CarInterface(object): 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 [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER]: + + if self.CP.carFingerprint in [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER] 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 @@ -346,7 +364,7 @@ class CarInterface(object): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c, perception_state=log.Live20Data.new_message()): + def apply(self, c): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 720c0ef60..ae257940a 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -78,6 +78,18 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead): } return packer.make_can_msg("ACC_CONTROL", 0, values) +def create_gas_command(packer, gas_amount): + """Creates a CAN message for the Pedal DBC GAS_COMMAND.""" + enable = gas_amount > 0.001 + + values = {"ENABLE": enable} + + if enable: + values["GAS_COMMAND"] = gas_amount * 255. + values["GAS_COMMAND2"] = gas_amount * 255. + + return packer.make_can_msg("GAS_COMMAND", 0, values) + def create_fcw_command(packer, fcw): values = { diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index e6dd026b0..e03c4a3a8 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -79,7 +79,7 @@ def check_ecu_msgs(fingerprint, ecu): FINGERPRINTS = { CAR.RAV4: [{ - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 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, 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 + 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, 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 }], 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, 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 @@ -89,22 +89,23 @@ FINGERPRINTS = { 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 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: [{ - 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, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 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 + 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, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 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 }, # Prius Prime { - 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, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 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, 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, 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 + 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, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 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, 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, 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 }, # Taiwanese Prius Prime { - 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, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 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 + 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, 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 }], + #Corolla w/ added Pedal Support (512L and 513L) CAR.COROLLA: [{ - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 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: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 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, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + 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, 608: 8, 610: 5, 643: 7, 705: 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: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 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, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, - # Corolla LE 2017 + # Corolla LE 2017 w/ added Pedal Support (512L and 513L) { - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 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: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8 + 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, 608: 8, 610: 5, 643: 7, 705: 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: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8 }], CAR.LEXUS_RXH: [{ 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: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 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, 975: 6, 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, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 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, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8 diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 43c8a9340..b1deb2116 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.5.8-release" +#define COMMA_VERSION "0.5.9-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 061b9ff81..7a17556ec 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -11,7 +11,6 @@ import selfdrive.messaging as messaging from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.car.car_helpers import get_car -from selfdrive.controls.lib.planner import Planner from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \ get_events, \ create_event, \ @@ -23,6 +22,7 @@ from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.driver_monitor import DriverStatus +from selfdrive.controls.lib.planner import _DT_MPC from selfdrive.locationd.calibration_helpers import Calibration, Filter ThermalStatus = log.ThermalData.ThermalStatus @@ -39,9 +39,9 @@ def isEnabled(state): return (isActive(state) or state == State.preEnabled) -def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_location, +def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor, poller, cal_status, cal_perc, overtemp, free_space, low_battery, - driver_status, geofence, state, mismatch_counter, params): + driver_status, state, mismatch_counter, params, plan, path_plan): """Receive data from sockets and create events for battery, temperature and disk space""" # Update carstate from CAN and create events @@ -54,7 +54,6 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati cal = None hh = None dm = None - gps = None for socket, event in poller.poll(0): if socket is thermal: @@ -65,8 +64,10 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati hh = messaging.recv_one(socket) elif socket is driver_monitor: dm = messaging.recv_one(socket) - elif socket is gps_location: - gps = messaging.recv_one(socket) + elif socket is plan_sock: + plan = messaging.recv_one(socket) + elif socket is path_plan_sock: + path_plan = messaging.recv_one(socket) if td is not None: overtemp = td.thermal.thermalStatus >= ThermalStatus.red @@ -110,32 +111,7 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati if dm is not None: driver_status.get_pose(dm.driverMonitoring, params) - # Geofence - if geofence is not None and gps is not None: - geofence.update_geofence_status(gps.gpsLocationExternal, params) - if geofence is not None and not geofence.in_geofence: - events.append(create_event('geofence', [ET.NO_ENTRY, ET.WARNING])) - - return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter - - -def calc_plan(CS, CP, VM, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence): - """Calculate a longitudinal plan using MPC""" - - # Slow down when based on driver monitoring or geofence - force_decel = driver_status.awareness < 0. or (geofence is not None and not geofence.in_geofence) - - # Update planner - plan_packet = PL.update(CS, CP, VM, LaC, LoC, v_cruise_kph, force_decel) - plan = plan_packet.plan - plan_ts = plan_packet.logMonoTime - events += list(plan.events) - - # Only allow engagement with brake pressed when stopped behind another stopped car - if CS.brakePressed and plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: - events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - - return plan, plan_ts + return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): @@ -225,8 +201,8 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last -def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc): +def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, + driver_status, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -264,18 +240,25 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM.add(e, enabled, extra_text_2=extra_text) # Run angle offset learner at 20 Hz - if rk.frame % 5 == 2 and plan.lateralValid: + if rk.frame % 5 == 2: angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset, - PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle, + path_plan.cPoly, path_plan.cProb, CS.steeringAngle, CS.steeringPressed) + cur_time = sec_since_boot() # TODO: This won't work in replay + mpc_time = plan.l20MonoTime / 1e9 + _DT = 0.01 # 100Hz + + dt = min(cur_time - mpc_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps + a_acc_sol = plan.aStart + (dt / _DT_MPC) * (plan.aTarget - plan.aStart) + v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 + # Gas/Brake PID loop actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, - v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget, - CP, PL.lead_1) + v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, - CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL) + CS.steeringPressed, CP, VM, path_plan) # Send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: @@ -294,13 +277,15 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM.process_alerts(sec_since_boot()) - return actuators, v_cruise_kph, driver_status, angle_offset + return actuators, v_cruise_kph, driver_status, angle_offset, v_acc_sol, a_acc_sol -def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, - carcontrol, live100, livempc, AM, driver_status, - LaC, LoC, angle_offset, passive, start_time): +def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, + carcontrol, live100, AM, driver_status, + LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc): """Send actuators and hud commands to the car, send live100 and MPC logging""" + plan_ts = plan.logMonoTime + plan = plan.plan CC = car.CarControl.new_message() @@ -320,13 +305,15 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = plan.hasLead - CC.hudControl.rightLaneVisible = plan.hasRightLane - CC.hudControl.leftLaneVisible = plan.hasLeftLane + CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5) + CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5) CC.hudControl.visualAlert = AM.visual_alert CC.hudControl.audibleAlert = AM.audible_alert # send car controls over can - CI.apply(CC, perception_state) + CI.apply(CC) + + force_decel = driver_status.awareness < 0. # live100 dat = messaging.new_message() @@ -343,6 +330,7 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac "driverMonitoringOn": bool(driver_status.monitor_on), "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": plan_ts, + "pathPlanMonoTime": path_plan.logMonoTime, "enabled": isEnabled(state), "active": isActive(state), "vEgo": CS.vEgo, @@ -362,8 +350,8 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac "upSteer": float(LaC.pid.p), "uiSteer": float(LaC.pid.i), "ufSteer": float(LaC.pid.f), - "vTargetLead": float(plan.vTarget), - "aTarget": float(plan.aTarget), + "vTargetLead": float(v_acc), + "aTarget": float(a_acc), "jerkFactor": float(plan.jerkFactor), "angleOffset": float(angle_offset), "gpsPlannerActive": plan.gpsPlannerActive, @@ -372,6 +360,7 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac "cumLagMs": -rk.remaining * 1000., "startMonoTime": start_time, "mapValid": plan.mapValid, + "forceDecel": bool(force_decel), } live100.send(dat.to_bytes()) @@ -388,21 +377,13 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac cc_send.carControl = CC carcontrol.send(cc_send.to_bytes()) - # send MPC when updated (20 Hz) - if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated: - dat = messaging.new_message() - dat.init('liveMpc') - dat.liveMpc.x = list(LaC.mpc_solution[0].x) - dat.liveMpc.y = list(LaC.mpc_solution[0].y) - dat.liveMpc.psi = list(LaC.mpc_solution[0].psi) - dat.liveMpc.delta = list(LaC.mpc_solution[0].delta) - dat.liveMpc.cost = LaC.mpc_solution[0].cost - livempc.send(dat.to_bytes()) + if (rk.frame % 36000) == 0: # update angle offset every 6 minutes + params.put("ControlsParams", json.dumps({'angle_offset': angle_offset})) return CC -def controlsd_thread(gctx=None, rate=100, default_bias=0.): +def controlsd_thread(gctx=None, rate=100): gc.disable() # start the loop @@ -415,7 +396,6 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) - livempc = messaging.pub_sock(context, service_list['liveMpc'].port) is_metric = params.get("IsMetric") == "1" passive = params.get("Passive") != "0" @@ -432,7 +412,8 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller) cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller) driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller) - gps_location = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller) + plan_sock = messaging.sub_sock(context, service_list['plan'].port, conflate=True, poller=poller) + path_plan_sock = messaging.sub_sock(context, service_list['pathPlan'].port, conflate=True, poller=poller) logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() @@ -449,11 +430,6 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): if passive: CP.safetyModel = car.CarParams.SafetyModels.noOutput - # Get FCW toggle from settings - fcw_enabled = params.get("IsFcwEnabled") == "1" - geofence = None - - PL = Planner(CP, fcw_enabled) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(CP) @@ -478,17 +454,19 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): mismatch_counter = 0 low_battery = False - rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) + plan = messaging.new_message() + plan.init('plan') + path_plan = messaging.new_message() + path_plan.init('pathPlan') - # Read angle offset from previous drive, fallback to default - angle_offset = default_bias - calibration_params = params.get("CalibrationParams") - if calibration_params: - try: - calibration_params = json.loads(calibration_params) - angle_offset = calibration_params["angle_offset2"] - except (ValueError, KeyError): - pass + rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) + controls_params = params.get("ControlsParams") + # Read angle offset from previous drive + if controls_params is not None: + controls_params = json.loads(controls_params) + angle_offset = controls_params['angle_offset'] + else: + angle_offset = 0. prof = Profiler(False) # off by default @@ -497,13 +475,21 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events - CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter = data_sample(CI, CC, thermal, cal, health, - driver_monitor, gps_location, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, geofence, state, mismatch_counter, params) + CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan =\ + data_sample(CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor, + poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, + state, mismatch_counter, params, plan, path_plan) prof.checkpoint("Sample") - # Define longitudinal plan (MPC) - plan, plan_ts = calc_plan(CS, CP, VM, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence) - prof.checkpoint("Plan") + path_plan_age = (start_time - path_plan.logMonoTime) / 1e9 + plan_age = (start_time - plan.logMonoTime) / 1e9 + if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5: + events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + events += list(plan.plan.events) + + # Only allow engagement with brake pressed when stopped behind another stopped car + if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: + events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not passive: # update control state @@ -512,13 +498,16 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph, - v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc) + actuators, v_cruise_kph, driver_status, angle_offset, v_acc, a_acc = \ + state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph, + v_cruise_kph_last, AM, rk, driver_status, + LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc) + prof.checkpoint("State Control") # Publish data - CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, - live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive, start_time) + CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, + live100, AM, driver_status, LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc) prof.checkpoint("Sent") rk.keep_time() # Run at 100Hz diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 81a371f78..cc06fd40d 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,23 +1,11 @@ -import math -import numpy as np from selfdrive.controls.lib.pid import PIController -from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT -from selfdrive.controls.lib.lateral_mpc import libmpc_py from common.numpy_fast import interp -from common.realtime import sec_since_boot -from selfdrive.swaglog import cloudlog from cereal import car _DT = 0.01 # 100Hz _DT_MPC = 0.05 # 20Hz -def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): - states[0].x = v_ego * delay - states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay - return states - - def get_steer_max(CP, v_ego): return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) @@ -28,75 +16,12 @@ class LatControl(object): (CP.steerKiBP, CP.steerKiV), k_f=CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 - self.setup_mpc(CP.steerRateCost) - - def setup_mpc(self, steer_rate_cost): - self.libmpc = libmpc_py.libmpc - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) - - self.mpc_solution = libmpc_py.ffi.new("log_t *") - self.cur_state = libmpc_py.ffi.new("state_t *") - self.mpc_updated = False - self.mpc_nans = False - self.cur_state[0].x = 0.0 - self.cur_state[0].y = 0.0 - self.cur_state[0].psi = 0.0 - self.cur_state[0].delta = 0.0 - - self.last_mpc_ts = 0.0 - self.angle_steers_des = 0.0 - self.angle_steers_des_mpc = 0.0 - self.angle_steers_des_prev = 0.0 - self.angle_steers_des_time = 0.0 + self.angle_steers_des = 0. def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL): - cur_time = sec_since_boot() - self.mpc_updated = False - # TODO: this creates issues in replay when rewinding time: mpc won't run - if self.last_mpc_ts < PL.last_md_ts: - self.last_mpc_ts = PL.last_md_ts - self.angle_steers_des_prev = self.angle_steers_des_mpc - - curvature_factor = VM.curvature_factor(v_ego) - - l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly)) - r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly)) - p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly)) - - # account for actuation delay - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay) - - v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed - self.libmpc.run_mpc(self.cur_state, self.mpc_solution, - l_poly, r_poly, p_poly, - PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width) - - # reset to current steer angle if not active or overriding - if active: - delta_desired = self.mpc_solution[0].delta[1] - else: - delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio - - self.cur_state[0].delta = delta_desired - - self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset) - self.angle_steers_des_time = cur_time - self.mpc_updated = True - - # Check for infeasable MPC solution - self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) - t = sec_since_boot() - if self.mpc_nans: - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) - self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio - - if t > self.last_cloudlog_t + 5.0: - self.last_cloudlog_t = t - cloudlog.warning("Lateral mpc - nan: True") - + def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan): if v_ego < 0.3 or not active: output_steer = 0.0 self.pid.reset() @@ -105,7 +30,8 @@ class LatControl(object): # constant for 0.05s. #dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) - self.angle_steers_des = self.angle_steers_des_mpc + self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner + steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 092edd27a..da324a39a 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -71,7 +71,7 @@ class LongControl(object): self.pid.reset() self.v_pid = v_pid - def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, lead_1): + def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Actuation limits gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) diff --git a/selfdrive/controls/lib/model_parser.py b/selfdrive/controls/lib/model_parser.py new file mode 100644 index 000000000..747e918f7 --- /dev/null +++ b/selfdrive/controls/lib/model_parser.py @@ -0,0 +1,65 @@ +from common.numpy_fast import interp +from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv + +CAMERA_OFFSET = 0.06 # m from center car to camera + + +class ModelParser(object): + def __init__(self): + self.d_poly = [0., 0., 0., 0.] + self.c_poly = [0., 0., 0., 0.] + self.c_prob = 0. + self.last_model = 0. + self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 + self._path_pinv = compute_path_pinv() + + self.lane_width_estimate = 3.7 + self.lane_width_certainty = 1.0 + self.lane_width = 3.7 + self.l_prob = 0. + self.r_prob = 0. + + def update(self, v_ego, md): + if md is not None: + p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path + l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line + r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line + + # only offset left and right lane lines; offsetting p_poly does not make sense + l_poly[3] += CAMERA_OFFSET + r_poly[3] += CAMERA_OFFSET + + p_prob = 1. # model does not tell this probability yet, so set to 1 for now + l_prob = md.model.leftLane.prob # left line prob + r_prob = md.model.rightLane.prob # right line prob + + # Find current lanewidth + lr_prob = l_prob * r_prob + self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty) + current_lane_width = abs(l_poly[3] - r_poly[3]) + self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) + speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8]) + self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ + (1 - self.lane_width_certainty) * speed_lane_width + + lane_width_diff = abs(self.lane_width - current_lane_width) + lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0]) + + r_prob *= lane_r_prob + + self.lead_dist = md.model.lead.dist + self.lead_prob = md.model.lead.prob + self.lead_var = md.model.lead.std**2 + + # compute target path + self.d_poly, self.c_poly, self.c_prob = calc_desired_path( + l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width) + + self.r_poly = r_poly + self.r_prob = r_prob + + self.l_poly = l_poly + self.l_prob = l_prob + + self.p_poly = p_poly + self.p_prob = p_prob diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 450a308ae..55fd2afae 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,64 +1,122 @@ -from common.numpy_fast import interp -from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv +import zmq +import math +import numpy as np + +from common.realtime import sec_since_boot +from selfdrive.services import service_list +from selfdrive.swaglog import cloudlog +from selfdrive.controls.lib.lateral_mpc import libmpc_py +from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT +from selfdrive.controls.lib.model_parser import ModelParser +import selfdrive.messaging as messaging + + +def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): + states[0].x = v_ego * delay + states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay + return states -CAMERA_OFFSET = 0.06 # m from center car to camera class PathPlanner(object): - def __init__(self): - self.d_poly = [0., 0., 0., 0.] - self.c_poly = [0., 0., 0., 0.] - self.c_prob = 0. - self.last_model = 0. - self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 - self._path_pinv = compute_path_pinv() + def __init__(self, CP): + self.MP = ModelParser() - self.lane_width_estimate = 3.7 - self.lane_width_certainty = 1.0 - self.lane_width = 3.7 - self.l_prob = 0. - self.r_prob = 0. + self.last_cloudlog_t = 0 - def update(self, v_ego, md): - if md is not None: - p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path - l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line - r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line + context = zmq.Context() + self.plan = messaging.pub_sock(context, service_list['pathPlan'].port) + self.livempc = messaging.pub_sock(context, service_list['liveMpc'].port) - # only offset left and right lane lines; offsetting p_poly does not make sense - l_poly[3] += CAMERA_OFFSET - r_poly[3] += CAMERA_OFFSET + self.setup_mpc(CP.steerRateCost) + self.invalid_counter = 0 - p_prob = 1. # model does not tell this probability yet, so set to 1 for now - l_prob = md.model.leftLane.prob # left line prob - r_prob = md.model.rightLane.prob # right line prob + def setup_mpc(self, steer_rate_cost): + self.libmpc = libmpc_py.libmpc + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) - # Find current lanewidth - lr_prob = l_prob * r_prob - self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty) - current_lane_width = abs(l_poly[3] - r_poly[3]) - self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) - speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8]) - self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ - (1 - self.lane_width_certainty) * speed_lane_width + self.mpc_solution = libmpc_py.ffi.new("log_t *") + self.cur_state = libmpc_py.ffi.new("state_t *") + self.cur_state[0].x = 0.0 + self.cur_state[0].y = 0.0 + self.cur_state[0].psi = 0.0 + self.cur_state[0].delta = 0.0 - lane_width_diff = abs(self.lane_width - current_lane_width) - lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0]) + self.angle_steers_des = 0.0 + self.angle_steers_des_mpc = 0.0 + self.angle_steers_des_prev = 0.0 + self.angle_steers_des_time = 0.0 - r_prob *= lane_r_prob + def update(self, CP, VM, CS, md, live100): + v_ego = CS.carState.vEgo + angle_steers = CS.carState.steeringAngle + active = live100.live100.active + angle_offset = live100.live100.angleOffset + self.MP.update(v_ego, md) - self.lead_dist = md.model.lead.dist - self.lead_prob = md.model.lead.prob - self.lead_var = md.model.lead.std**2 + # Run MPC + self.angle_steers_des_prev = self.angle_steers_des_mpc + curvature_factor = VM.curvature_factor(v_ego) - # compute target path - self.d_poly, self.c_poly, self.c_prob = calc_desired_path( - l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width) + l_poly = libmpc_py.ffi.new("double[4]", list(self.MP.l_poly)) + r_poly = libmpc_py.ffi.new("double[4]", list(self.MP.r_poly)) + p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly)) - self.r_poly = r_poly - self.r_prob = r_prob + # account for actuation delay + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay) - self.l_poly = l_poly - self.l_prob = l_prob + v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed + self.libmpc.run_mpc(self.cur_state, self.mpc_solution, + l_poly, r_poly, p_poly, + self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width) - self.p_poly = p_poly - self.p_prob = p_prob + # reset to current steer angle if not active or overriding + if active: + delta_desired = self.mpc_solution[0].delta[1] + else: + delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio + + self.cur_state[0].delta = delta_desired + + self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset) + + # Check for infeasable MPC solution + mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) + t = sec_since_boot() + if mpc_nans: + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) + self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio + + if t > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = t + cloudlog.warning("Lateral mpc - nan: True") + + if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge + self.invalid_counter += 1 + else: + self.invalid_counter = 0 + + plan_valid = self.invalid_counter < 2 + + plan_send = messaging.new_message() + plan_send.init('pathPlan') + plan_send.pathPlan.laneWidth = float(self.MP.lane_width) + plan_send.pathPlan.dPoly = map(float, self.MP.d_poly) + plan_send.pathPlan.cPoly = map(float, self.MP.c_poly) + plan_send.pathPlan.cProb = float(self.MP.c_prob) + plan_send.pathPlan.lPoly = map(float, l_poly) + plan_send.pathPlan.lProb = float(self.MP.l_prob) + plan_send.pathPlan.rPoly = map(float, r_poly) + plan_send.pathPlan.rProb = float(self.MP.r_prob) + plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) + plan_send.pathPlan.valid = bool(plan_valid) + + self.plan.send(plan_send.to_bytes()) + + dat = messaging.new_message() + dat.init('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) + dat.liveMpc.delta = list(self.mpc_solution[0].delta) + dat.liveMpc.cost = self.mpc_solution[0].cost + self.livempc.send(dat.to_bytes()) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 7cb7b7898..e0a53e997 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -1,10 +1,7 @@ #!/usr/bin/env python -import os import zmq import math import numpy as np -from copy import copy -from cereal import log from collections import defaultdict from common.params import Params from common.realtime import sec_since_boot @@ -14,23 +11,17 @@ from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.controls.lib.drive_helpers import create_event, MPC_COST_LONG, EventTypes as ET -from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.speed_smoother import speed_smoother from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU -# Max lateral acceleration, used to caclulate how much to slow down in turns -A_Y_MAX = 1.85 # m/s^2 NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS -_DT = 0.01 # 100Hz _DT_MPC = 0.2 # 5Hz MAX_SPEED_ERROR = 2.0 AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted -GPS_PLANNER_ADDR = "192.168.5.1" - # lookup tables VS speed to determine min and max accels in cruise # make sure these accelerations are smaller than mpc limits _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] @@ -98,7 +89,7 @@ class FCWChecker(object): # then limit ARel so that v_lead will get to zero in no sooner than t_decel. # This helps underweighting ARel when v_lead is close to zero. t_decel = 2. - a_rel = np.minimum(a_rel, v_lead/t_decel) + a_rel = np.minimum(a_rel, v_lead / t_decel) # delta of the quadratic equation to solve for ttc delta = v_rel**2 + 2 * x_lead * a_rel @@ -186,6 +177,8 @@ class LongitudinalMpc(object): self.cur_state[0].a_ego = a def update(self, CS, lead, v_cruise_setpoint): + v_ego = CS.carState.vEgo + # Setup current mpc state self.cur_state[0].x_ego = 0.0 @@ -194,7 +187,6 @@ class LongitudinalMpc(object): v_lead = max(0.0, lead.vLead) a_lead = lead.aLeadK - if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): v_lead = 0.0 a_lead = 0.0 @@ -213,7 +205,7 @@ class LongitudinalMpc(object): self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 - self.cur_state[0].v_l = CS.vEgo + 10.0 + self.cur_state[0].v_l = v_ego + 10.0 a_lead = 0.0 self.a_lead_tau = _LEAD_ACCEL_TAU @@ -242,10 +234,10 @@ class LongitudinalMpc(object): self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.cur_state[0].v_ego = CS.vEgo + self.cur_state[0].v_ego = v_ego self.cur_state[0].a_ego = 0.0 - self.v_mpc = CS.vEgo - self.a_mpc = CS.aEgo + self.v_mpc = v_ego + self.a_mpc = CS.carState.aEgo self.prev_lead_status = False @@ -255,38 +247,20 @@ class Planner(object): self.CP = CP self.poller = zmq.Poller() - self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) - self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller) - self.live_map_data = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=self.poller) - - if os.environ.get('GPS_PLANNER_ACTIVE', False): - self.gps_planner_plan = messaging.sub_sock(context, service_list['gpsPlannerPlan'].port, conflate=True, poller=self.poller, addr=GPS_PLANNER_ADDR) - else: - self.gps_planner_plan = None - self.plan = messaging.pub_sock(context, service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port) - self.last_md_ts = 0 - self.last_l20_ts = 0 - self.last_model = 0. - self.last_l20 = 0. - self.model_dead = True - self.radar_dead = True self.radar_errors = [] - self.PP = PathPlanner() self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) self.v_acc_start = 0.0 self.a_acc_start = 0.0 - self.acc_start_time = sec_since_boot() + self.v_acc = 0.0 - self.v_acc_sol = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 - self.a_acc_sol = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 @@ -298,11 +272,6 @@ class Planner(object): self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled - self.last_gps_planner_plan = None - self.gps_planner_active = False - self.last_live_map_data = None - self.perception_state = log.Live20Data.new_message() - self.params = Params() self.v_curvature = NO_CURVATURE_SPEED self.v_speedlimit = NO_CURVATURE_SPEED @@ -319,12 +288,6 @@ class Planner(object): slowest = min(solutions, key=solutions.get) - """ - print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol - print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise - print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise - """ - self.longitudinalPlanSource = slowest # Choose lowest of MPC and cruise @@ -340,201 +303,144 @@ class Planner(object): self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) - # this runs whenever we get a packet that can change the plan - def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel): - cur_time = sec_since_boot() + def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data): + """Gets called when new live20 is available""" + cur_time = live20.logMonoTime / 1e9 + v_ego = CS.carState.vEgo + + long_control_state = live100.live100.longControlState + v_cruise_kph = live100.live100.vCruise + force_slow_decel = live100.live100.forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS - md = None - l20 = None - gps_planner_plan = None + self.last_md_ts = md.logMonoTime - for socket, event in self.poller.poll(0): - if socket is self.model: - md = messaging.recv_one(socket) - elif socket is self.live20: - l20 = messaging.recv_one(socket) - elif socket is self.gps_planner_plan: - gps_planner_plan = messaging.recv_one(socket) - elif socket is self.live_map_data: - self.last_live_map_data = messaging.recv_one(socket).liveMapData + self.radar_errors = list(live20.live20.radarErrors) - if gps_planner_plan is not None: - self.last_gps_planner_plan = gps_planner_plan + self.lead_1 = live20.live20.leadOne + self.lead_2 = live20.live20.leadTwo - if md is not None: - self.last_md_ts = md.logMonoTime - self.last_model = cur_time - self.model_dead = False + enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) + following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0 - self.PP.update(CS.vEgo, md) + self.v_speedlimit = NO_CURVATURE_SPEED + self.v_curvature = NO_CURVATURE_SPEED + self.map_valid = live_map_data.liveMapData.mapValid - if self.last_gps_planner_plan is not None: - plan = self.last_gps_planner_plan.gpsPlannerPlan - self.gps_planner_active = plan.valid - if plan.valid: - self.PP.d_poly = plan.poly - self.PP.p_poly = plan.poly - self.PP.c_poly = plan.poly - self.PP.l_prob = 0.0 - self.PP.r_prob = 0.0 - self.PP.c_prob = 1.0 + # Speed limit and curvature + set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None + if set_speed_limit_active: + if live_map_data.liveMapData.speedLimitValid: + speed_limit = live_map_data.liveMapData.speedLimit + offset = float(self.params.get("SpeedLimitOffset")) + self.v_speedlimit = speed_limit + offset - if l20 is not None: - self.perception_state = copy(l20.live20) - self.last_l20_ts = l20.logMonoTime - self.last_l20 = cur_time - self.radar_dead = False - self.radar_errors = list(l20.live20.radarErrors) + if live_map_data.liveMapData.curvatureValid: + curvature = abs(live_map_data.liveMapData.curvature) + a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph + v_curvature = math.sqrt(a_y_max / max(1e-4, curvature)) + self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature) - self.v_acc_start = self.v_acc_sol - self.a_acc_start = self.a_acc_sol - self.acc_start_time = cur_time + self.decel_for_turn = bool(self.v_curvature < min([v_cruise_setpoint, self.v_speedlimit, v_ego + 1.])) + v_cruise_setpoint = min([v_cruise_setpoint, self.v_curvature, self.v_speedlimit]) - self.lead_1 = l20.live20.leadOne - self.lead_2 = l20.live20.leadTwo + # Calculate speed for normal cruise control + if enabled: + accel_limits = map(float, calc_cruise_accel_limits(v_ego, following)) + jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning + accel_limits = limit_accel_in_turns(v_ego, CS.carState.steeringAngle, accel_limits, self.CP) - enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping) - following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0 + if force_slow_decel: + # if required so, force a smooth deceleration + accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) + accel_limits[0] = min(accel_limits[0], accel_limits[1]) - if self.last_live_map_data: - self.v_speedlimit = NO_CURVATURE_SPEED - self.v_curvature = NO_CURVATURE_SPEED - self.map_valid = self.last_live_map_data.mapValid + # Change accel limits based on time remaining to turn + if self.decel_for_turn: + time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.)) + required_decel = min(0, (self.v_curvature - self.v_cruise) / time_to_turn) + accel_limits[0] = max(accel_limits[0], required_decel) - # Speed limit - if self.last_live_map_data.speedLimitValid: - speed_limit = self.last_live_map_data.speedLimit - set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None + self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, + v_cruise_setpoint, + accel_limits[1], accel_limits[0], + jerk_limits[1], jerk_limits[0], + _DT_MPC) + # cruise speed can't be negative even is user is distracted + self.v_cruise = max(self.v_cruise, 0.) + else: + starting = long_control_state == LongCtrlState.starting + a_ego = min(CS.carState.aEgo, 0.0) + reset_speed = MIN_CAN_SPEED if starting else v_ego + reset_accel = self.CP.startAccel if starting else a_ego + self.v_acc = reset_speed + self.a_acc = reset_accel + self.v_acc_start = reset_speed + self.a_acc_start = reset_accel + self.v_cruise = reset_speed + self.a_cruise = reset_accel - if set_speed_limit_active: - offset = float(self.params.get("SpeedLimitOffset")) - self.v_speedlimit = speed_limit + offset + self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) + self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) - # Curvature - if self.last_live_map_data.curvatureValid: - curvature = abs(self.last_live_map_data.curvature) - v_curvature = math.sqrt(A_Y_MAX / max(1e-4, curvature)) - self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature) + self.mpc1.update(CS, self.lead_1, v_cruise_setpoint) + self.mpc2.update(CS, self.lead_2, v_cruise_setpoint) - # leave 1m/s margin on vEgo to asses if turn is limiting our speed. - self.decel_for_turn = bool(self.v_curvature < min([v_cruise_setpoint, self.v_speedlimit, CS.vEgo + 1.])) - v_cruise_setpoint = min([v_cruise_setpoint, self.v_curvature, self.v_speedlimit]) + self.choose_solution(v_cruise_setpoint, enabled) - # Calculate speed for normal cruise control - if enabled: - accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following)) - # TODO: make a separate lookup for jerk tuning - jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] - accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) + # determine fcw + if self.mpc1.new_lead: + self.fcw_checker.reset_lead(cur_time) - if force_slow_decel: - # if required so, force a smooth deceleration - accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) - accel_limits[0] = min(accel_limits[0], accel_limits[1]) + blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker + self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo, + self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, + self.lead_1.yRel, self.lead_1.vLat, + self.lead_1.fcw, blinkers) and not CS.carState.brakePressed + if self.fcw: + cloudlog.info("FCW triggered %s", self.fcw_checker.counters) - # Change accel limits based on time remaining to turn - if self.decel_for_turn: - time_to_turn = max(1.0, self.last_live_map_data.distToTurn / max(self.v_cruise, 1.)) - required_decel = min(0, (self.v_curvature - self.v_cruise) / time_to_turn) - accel_limits[0] = max(accel_limits[0], required_decel) + model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5 - self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, - v_cruise_setpoint, - accel_limits[1], accel_limits[0], - jerk_limits[1], jerk_limits[0], - _DT_MPC) - # cruise speed can't be negative even is user is distracted - self.v_cruise = max(self.v_cruise, 0.) - else: - starting = LoC.long_control_state == LongCtrlState.starting - a_ego = min(CS.aEgo, 0.0) - reset_speed = MIN_CAN_SPEED if starting else CS.vEgo - reset_accel = self.CP.startAccel if starting else a_ego - self.v_acc = reset_speed - self.a_acc = reset_accel - self.v_acc_start = reset_speed - self.a_acc_start = reset_accel - self.v_cruise = reset_speed - self.a_cruise = reset_accel - self.v_acc_sol = reset_speed - self.a_acc_sol = reset_accel - - self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) - self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) - - self.mpc1.update(CS, self.lead_1, v_cruise_setpoint) - self.mpc2.update(CS, self.lead_2, v_cruise_setpoint) - - self.choose_solution(v_cruise_setpoint, enabled) - - # determine fcw - if self.mpc1.new_lead: - self.fcw_checker.reset_lead(cur_time) - - blinkers = CS.leftBlinker or CS.rightBlinker - self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo, - self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, - self.lead_1.yRel, self.lead_1.vLat, - self.lead_1.fcw, blinkers) \ - and not CS.brakePressed - if self.fcw: - cloudlog.info("FCW triggered %s", self.fcw_checker.counters) - - if cur_time - self.last_model > 0.5: - self.model_dead = True - - if cur_time - self.last_l20 > 0.5: - self.radar_dead = True # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') + # TODO: Move all these events to controlsd. This has nothing to do with planning events = [] - if self.model_dead: + if model_dead: events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if self.radar_dead or 'commIssue' in self.radar_errors: - events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if 'fault' in self.radar_errors: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if LaC.mpc_solution[0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge - events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - - # Interpolation of trajectory - dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps - self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) - self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 plan_send.plan.events = events - plan_send.plan.mdMonoTime = self.last_md_ts - plan_send.plan.l20MonoTime = self.last_l20_ts - - # lateral plan - plan_send.plan.lateralValid = not self.model_dead - plan_send.plan.dPoly = map(float, self.PP.d_poly) - plan_send.plan.laneWidth = float(self.PP.lane_width) + plan_send.plan.mdMonoTime = md.logMonoTime + plan_send.plan.l20MonoTime = live20.logMonoTime # longitudal plan - plan_send.plan.longitudinalValid = not self.radar_dead plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise - plan_send.plan.vTarget = self.v_acc_sol - plan_send.plan.aTarget = self.a_acc_sol + plan_send.plan.vStart = self.v_acc_start + plan_send.plan.aStart = self.a_acc_start + plan_send.plan.vTarget = self.v_acc + plan_send.plan.aTarget = self.a_acc plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status - plan_send.plan.hasLeftLane = bool(self.PP.l_prob > 0.5) - plan_send.plan.hasRightLane = bool(self.PP.r_prob > 0.5) plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource - plan_send.plan.gpsPlannerActive = self.gps_planner_active - plan_send.plan.vCurvature = self.v_curvature plan_send.plan.decelForTurn = self.decel_for_turn plan_send.plan.mapValid = self.map_valid # Send out fcw - fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off) + fcw = self.fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) - return plan_send + + # Interpolate 0.05 seconds and save as starting point for next iteration + dt = 0.05 # s + a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) + v_acc_sol = self.v_acc_start + dt * (a_acc_sol + self.a_acc_start) / 2.0 + self.v_acc_start = v_acc_sol + self.a_acc_start = a_acc_sol diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py new file mode 100755 index 000000000..c6f9040b2 --- /dev/null +++ b/selfdrive/controls/plannerd.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python +import zmq + +from cereal import car +from common.params import Params +from selfdrive.swaglog import cloudlog +from selfdrive.services import service_list +from selfdrive.controls.lib.planner import Planner +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.controls.lib.pathplanner import PathPlanner +import selfdrive.messaging as messaging + + +def plannerd_thread(): + context = zmq.Context() + params = Params() + + # Get FCW toggle from settings + fcw_enabled = params.get("IsFcwEnabled") == "1" + + cloudlog.info("plannerd is waiting for CarParams") + CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + cloudlog.info("plannerd got CarParams: %s", CP.carName) + + PL = Planner(CP, fcw_enabled) + PP = PathPlanner(CP) + + VM = VehicleModel(CP) + + poller = zmq.Poller() + car_state_sock = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller) + live100_sock = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) + live20_sock = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=poller) + model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) + live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller) + + car_state = messaging.new_message() + car_state.init('carState') + live100 = messaging.new_message() + live100.init('live100') + model = messaging.new_message() + model.init('model') + live20 = messaging.new_message() + live20.init('live20') + live_map_data = messaging.new_message() + live_map_data.init('liveMapData') + + while True: + for socket, event in poller.poll(): + if socket is live100_sock: + live100 = messaging.recv_one(socket) + elif socket is car_state_sock: + car_state = messaging.recv_one(socket) + elif socket is model_sock: + model = messaging.recv_one(socket) + PP.update(CP, VM, car_state, model, live100) + elif socket is live_map_data_sock: + live_map_data = messaging.recv_one(socket) + elif socket is live20_sock: + live20 = messaging.recv_one(socket) + PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data) + + +def main(gctx=None): + plannerd_thread() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index c157ba908..3952a913a 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -8,7 +8,7 @@ from fastcluster import linkage_vector import selfdrive.messaging as messaging from selfdrive.services import service_list from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset -from selfdrive.controls.lib.pathplanner import PathPlanner +from selfdrive.controls.lib.model_parser import ModelParser from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, \ RDR_TO_LDR, NO_FUSION_SCORE from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -66,7 +66,7 @@ def radard_thread(gctx=None): model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) - PP = PathPlanner() + MP = ModelParser() RI = RadarInterface(CP) last_md_ts = 0 @@ -134,26 +134,26 @@ def radard_thread(gctx=None): last_md_ts = md.logMonoTime # *** get path prediction from the model *** - PP.update(v_ego, md) + MP.update(v_ego, md) # run kalman filter only if prob is high enough - if PP.lead_prob > 0.7: - reading = speedSensorV.read(PP.lead_dist, covar=np.matrix(PP.lead_var)) + if MP.lead_prob > 0.7: + reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var)) ekfv.update_scalar(reading) ekfv.predict(tsv) # When changing lanes the distance to the lead car can suddenly change, # which makes the Kalman filter output large relative acceleration - if mocked and abs(PP.lead_dist - ekfv.state[XV]) > 2.0: - ekfv.state[XV] = PP.lead_dist - ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) + if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0: + ekfv.state[XV] = MP.lead_dist + ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. - ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), + ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])), float(ekfv.state[SPEEDV]), False) else: - ekfv.state[XV] = PP.lead_dist - ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) + ekfv.state[XV] = MP.lead_dist + ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. if VISION_POINT in ar_pts: @@ -162,7 +162,7 @@ def radard_thread(gctx=None): # *** compute the likely path_y *** if (active and not steer_override) or mocked: # use path from model (always when mocking as steering is too noisy) - path_y = np.polyval(PP.d_poly, path_x) + path_y = np.polyval(MP.d_poly, path_x) else: # use path from steer, set angle_offset to 0 it does not only report the physical offset path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0] diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index 80ac6e4b5..92b10cadc 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -26,6 +26,7 @@ if __name__ == "__main__": parser.add_argument('--proxy', action='store_true', help='republish on localhost') parser.add_argument('--map', action='store_true') parser.add_argument('--addr', default='127.0.0.1') + parser.add_argument('--values', help='values to monitor (instead of entire event)') parser.add_argument("socket", type=str, nargs='*', help="socket name") args = parser.parse_args() @@ -53,6 +54,10 @@ if __name__ == "__main__": server_thread.start() print 'server running' + values = None + if args.values: + values = [s.strip().split(".") for s in args.values.split(",")] + while 1: polld = poller.poll(timeout=1000) for sock, mode in polld: @@ -79,5 +84,14 @@ if __name__ == "__main__": print(json.loads(msg)) elif args.dump_json: print json.dumps(evt.to_dict()) + elif values: + print "logMonotime = {}".format(evt.logMonoTime) + for value in values: + if hasattr(evt, value[0]): + item = evt + for key in value: + item = getattr(item, key) + print "{} = {}".format(".".join(value), item) + print "" else: print evt diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 6c395efed..41911a6b7 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -1,208 +1,77 @@ #!/usr/bin/env python import os import zmq -import cv2 import copy import json import numpy as np import selfdrive.messaging as messaging -from selfdrive.locationd.calibration_helpers import Calibration, Filter +from selfdrive.locationd.calibration_helpers import Calibration from selfdrive.swaglog import cloudlog from selfdrive.services import service_list from common.params import Params -from common.ffi_wrapper import ffi_wrap -import common.transformations.orientation as orient from common.transformations.model import model_height, get_camera_frame_from_model_frame, get_camera_frame_from_bigmodel_frame from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \ - eon_intrinsics, get_calib_from_vp, normalize, denormalize, H, W + eon_intrinsics, get_calib_from_vp, H, W - -FRAMES_NEEDED = 120 # allow to update VP every so many frames -VP_CYCLES_NEEDED = 2 -CALIBRATION_CYCLES_NEEDED = FRAMES_NEEDED * VP_CYCLES_NEEDED -DT = 0.1 # nominal time step of 10Hz (orbd_live runs slower on pc) -VP_RATE_LIM = 2. * DT # 2 px/s +MPH_TO_MS = 0.44704 +MIN_SPEED_FILTER = 15 * MPH_TO_MS +MAX_YAW_RATE_FILTER = np.radians(2) # per second +INPUTS_NEEDED = 300 # allow to update VP every so many frames +INPUTS_WANTED = 600 # We want a little bit more than we need for stability +WRITE_CYCLES = 400 # write every 400 cycles VP_INIT = np.array([W/2., H/2.]) -EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__)) -# big model is 864x288 -VP_VALIDITY_CORNERS = np.array([[-150., -200.], [150., 200.]]) + VP_INIT -GRID_WEIGHT_INIT = 2e6 -MAX_LINES = 500 # max lines to avoid over computation -HOOD_HEIGHT = H*3/4 # the part of image usually free from the car's hood +# These validity corners were chosen by looking at 1000 +# and taking most extreme cases with some margin. +VP_VALIDITY_CORNERS = np.array([[W/2 - 150, 280], [W/2 + 150, 540]]) DEBUG = os.getenv("DEBUG") is not None -# Wrap c code for slow grid incrementation -c_header = "\nvoid increment_grid(double *grid, double *lines, long long n);" -c_code = "#define H %d\n" % H -c_code += "#define W %d\n" % W -c_code += "\n" + open(os.path.join(EXTERNAL_PATH, "get_vp.c")).read() -ffi, lib = ffi_wrap('get_vp', c_code, c_header) - - -def increment_grid_c(grid, lines, n): - lib.increment_grid(ffi.cast("double *", grid.ctypes.data), - ffi.cast("double *", lines.ctypes.data), - ffi.cast("long long", n)) - -def get_lines(p): - A = (p[:,0,1] - p[:,1,1]) - B = (p[:,1,0] - p[:,0,0]) - C = (p[:,0,0]*p[:,1,1] - p[:,1,0]*p[:,0,1]) - return np.column_stack((A, B, -C)) - -def correct_pts(pts, rot_speeds, dt): - pts = np.hstack((pts, np.ones((pts.shape[0],1)))) - cam_rot = dt * view_frame_from_device_frame.dot(rot_speeds) - rot = orient.rot_matrix(*cam_rot.T).T - pts_corrected = rot.dot(pts.T).T - pts_corrected[:,0] /= pts_corrected[:,2] - pts_corrected[:,1] /= pts_corrected[:,2] - return pts_corrected[:,:2] - -def gaussian_kernel(sizex, sizey, stdx, stdy, dx, dy): - y, x = np.mgrid[-sizey:sizey+1, -sizex:sizex+1] - g = np.exp(-((x - dx)**2 / (2. * stdx**2) + (y - dy)**2 / (2. * stdy**2))) - return g / g.sum() - -def gaussian_kernel_1d(kernel): - #creates separable gaussian filter - u,s,v = np.linalg.svd(kernel) - x = u[:,0]*np.sqrt(s[0]) - y = np.sqrt(s[0])*v[0,:] - return x, y - -def blur_image(img, kernel_x, kernel_y): - return cv2.sepFilter2D(img.astype(np.uint16), -1, kernel_x, kernel_y) def is_calibration_valid(vp): return vp[0] > VP_VALIDITY_CORNERS[0,0] and vp[0] < VP_VALIDITY_CORNERS[1,0] and \ vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1] + class Calibrator(object): def __init__(self, param_put=False): self.param_put = param_put - self.speed = 0 - self.vp_cycles = 0 - self.angle_offset = 0. - self.yaw_rate = 0. - self.l100_last_updated = 0 - self.prev_orbs = None - self.kernel = gaussian_kernel(11, 11, 2.35, 2.35, 0, 0) - self.kernel_x, self.kernel_y = gaussian_kernel_1d(self.kernel) - self.vp = copy.copy(VP_INIT) + self.vps = [] self.cal_status = Calibration.UNCALIBRATED - self.frame_counter = 0 + self.write_counter = 0 self.params = Params() calibration_params = self.params.get("CalibrationParams") if calibration_params: try: calibration_params = json.loads(calibration_params) self.vp = np.array(calibration_params["vanishing_point"]) - self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID - self.vp_cycles = VP_CYCLES_NEEDED - self.frame_counter = CALIBRATION_CYCLES_NEEDED + self.vps = np.tile(self.vp, (calibration_params['valid_points'], 1)).tolist() + self.update_status() except Exception: cloudlog.exception("CalibrationParams file found but error encountered") - self.vp_unfilt = self.vp - self.orb_last_updated = 0. - self.reset_grid() - def reset_grid(self): - if self.cal_status == Calibration.UNCALIBRATED: - # empty grid - self.grid = np.zeros((H+1, W+1), dtype=np.float) - else: - # gaussian grid, centered at vp - self.grid = gaussian_kernel(W/2., H/2., 16, 16, self.vp[0] - W/2., self.vp[1] - H/2.) * GRID_WEIGHT_INIT - - def rescale_grid(self): - self.grid *= 0.9 - - def update(self, uvs, yaw_rate, speed): - if len(uvs) < 10 or \ - abs(yaw_rate) > Filter.MAX_YAW_RATE or \ - speed < Filter.MIN_SPEED: - return - rot_speeds = np.array([0.,0.,-yaw_rate]) - uvs[:,1,:] = denormalize(correct_pts(normalize(uvs[:,1,:]), rot_speeds, self.dt)) - # exclude tracks where: - # - pixel movement was less than 10 pixels - # - tracks are in the "hood region" - good_tracks = np.all([np.linalg.norm(uvs[:,1,:] - uvs[:,0,:], axis=1) > 10, - uvs[:,0,1] < HOOD_HEIGHT, - uvs[:,1,1] < HOOD_HEIGHT], axis = 0) - uvs = uvs[good_tracks] - if uvs.shape[0] > MAX_LINES: - uvs = uvs[np.random.choice(uvs.shape[0], MAX_LINES, replace=False), :] - lines = get_lines(uvs) - - increment_grid_c(self.grid, lines, len(lines)) - self.frame_counter += 1 - if (self.frame_counter % FRAMES_NEEDED) == 0: - grid = blur_image(self.grid, self.kernel_x, self.kernel_y) - argmax_vp = np.unravel_index(np.argmax(grid), grid.shape)[::-1] - self.rescale_grid() - self.vp_unfilt = np.array(argmax_vp) - self.vp_cycles += 1 - if (self.vp_cycles - VP_CYCLES_NEEDED) % 10 == 0: # update file every 10 - # skip rate_lim before writing the file to avoid writing a lagged vp - if self.cal_status != Calibration.CALIBRATED: - self.vp = self.vp_unfilt - if self.param_put: - cal_params = {"vanishing_point": list(self.vp), "angle_offset2": self.angle_offset} - self.params.put("CalibrationParams", json.dumps(cal_params)) - return self.vp_unfilt - - def parse_orb_features(self, log): - matches = np.array(log.orbFeatures.matches) - n = len(log.orbFeatures.matches) - t = float(log.orbFeatures.timestampLastEof)*1e-9 - if t == 0 or n == 0: - return t, np.zeros((0,2,2)) - orbs = denormalize(np.column_stack((log.orbFeatures.xs, - log.orbFeatures.ys))) - if self.prev_orbs is not None: - valid_matches = (matches > -1) & (matches < len(self.prev_orbs)) - tracks = np.hstack((orbs[valid_matches], self.prev_orbs[matches[valid_matches]])).reshape((-1,2,2)) - else: - tracks = np.zeros((0,2,2)) - self.prev_orbs = orbs - return t, tracks - - def update_vp(self): - # rate limit to not move VP too fast - self.vp = np.clip(self.vp_unfilt, self.vp - VP_RATE_LIM, self.vp + VP_RATE_LIM) - if self.vp_cycles < VP_CYCLES_NEEDED: + def update_status(self): + if len(self.vps) < INPUTS_NEEDED: self.cal_status = Calibration.UNCALIBRATED else: self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID - def handle_orb_features(self, log): - self.update_vp() - self.time_orb, frame_raw = self.parse_orb_features(log) - self.dt = min(self.time_orb - self.orb_last_updated, 1.) - self.orb_last_updated = self.time_orb - if self.time_orb - self.l100_last_updated < 1: - return self.update(frame_raw, self.yaw_rate, self.speed) - - def handle_live100(self, log): - self.l100_last_updated = self.time_orb - self.speed = log.live100.vEgo - self.angle_offset = log.live100.angleOffset - self.yaw_rate = log.live100.curvature * self.speed - - def handle_debug(self): - grid_blurred = blur_image(self.grid, self.kernel_x, self.kernel_y) - grid_grey = np.clip(grid_blurred/(0.1 + np.max(grid_blurred))*255, 0, 255) - grid_color = np.repeat(grid_grey[:,:,np.newaxis], 3, axis=2) - grid_color[:,:,0] = 0 - cv2.circle(grid_color, tuple(self.vp.astype(np.int64)), 2, (255, 255, 0), -1) - cv2.imshow("debug", grid_color.astype(np.uint8)) - - cv2.waitKey(1) + def handle_cam_odom(self, log): + trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot + if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER: + new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans)) + new_vp = new_vp[:2]/new_vp[2] + self.vps.append(new_vp) + self.vps = self.vps[-INPUTS_WANTED:] + self.vp = np.mean(self.vps, axis=0) + self.update_status() + self.write_counter += 1 + if self.param_put and self.write_counter % WRITE_CYCLES == 0: + cal_params = {"vanishing_point": list(self.vp), + "valid_points": len(self.vps)} + self.params.put("CalibrationParams", json.dumps(cal_params)) + return new_vp def send_data(self, livecalibration): calib = get_calib_from_vp(self.vp) @@ -214,7 +83,7 @@ class Calibrator(object): cal_send = messaging.new_message() cal_send.init('liveCalibration') cal_send.liveCalibration.calStatus = self.cal_status - cal_send.liveCalibration.calPerc = min(self.frame_counter * 100 / CALIBRATION_CYCLES_NEEDED, 100) + cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 / INPUTS_NEEDED, 100) cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten()) cal_send.liveCalibration.warpMatrixBig = map(float, warp_matrix_big.flatten()) cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten()) @@ -225,24 +94,17 @@ class Calibrator(object): def calibrationd_thread(gctx=None, addr="127.0.0.1"): context = zmq.Context() - live100 = messaging.sub_sock(context, service_list['live100'].port, addr=addr, conflate=True) - orbfeatures = messaging.sub_sock(context, service_list['orbFeatures'].port, addr=addr, conflate=True) + cameraodometry = messaging.sub_sock(context, service_list['cameraOdometry'].port, addr=addr, conflate=True) livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port) - - calibrator = Calibrator(param_put = True) + calibrator = Calibrator(param_put=True) # buffer with all the messages that still need to be input into the kalman while 1: - of = messaging.recv_one(orbfeatures) - l100 = messaging.recv_one_or_none(live100) + co = messaging.recv_one(cameraodometry) - new_vp = calibrator.handle_orb_features(of) + new_vp = calibrator.handle_cam_odom(co) if DEBUG and new_vp is not None: print 'got new vp', new_vp - if l100 is not None: - calibrator.handle_live100(l100) - if DEBUG: - calibrator.handle_debug() calibrator.send_data(livecalibration) @@ -250,6 +112,6 @@ def calibrationd_thread(gctx=None, addr="127.0.0.1"): def main(gctx=None, addr="127.0.0.1"): calibrationd_thread(gctx, addr) + if __name__ == "__main__": main() - diff --git a/selfdrive/locationd/ublox.py b/selfdrive/locationd/ublox.py index 228dfc843..06f174fba 100644 --- a/selfdrive/locationd/ublox.py +++ b/selfdrive/locationd/ublox.py @@ -459,7 +459,7 @@ msg_types = { UBloxDescriptor('RXM_RAW', ';);out; + >;);out;""" + """is_in""" + lat_lon + """;area._[admin_level~"[24]"]; + convert area ::id = id(), admin_level = t['admin_level'], + name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out; """ return q @@ -97,7 +100,7 @@ def query_thread(): cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) dist = np.linalg.norm(cur_ecef - prev_ecef) - if dist < 1000: + if dist < 1000: #updated when we are 1km from the edge of the downloaded circle continue if dist > 3000: @@ -111,6 +114,7 @@ def query_thread(): nodes = [] real_nodes = [] node_to_way = defaultdict(list) + location_info = {} for n in new_result.nodes: nodes.append((float(n.lat), float(n.lon), 0)) @@ -120,12 +124,18 @@ def query_thread(): for n in way.nodes: node_to_way[n.id].append(way) + for area in new_result.areas: + if area.tags.get('admin_level', '') == "2": + location_info['country'] = area.tags.get('ISO3166-1:alpha2', '') + if area.tags.get('admin_level', '') == "4": + location_info['region'] = area.tags.get('name', '') + nodes = np.asarray(nodes) nodes = geodetic2ecef(nodes) tree = spatial.cKDTree(nodes) query_lock.acquire() - last_query_result = new_result, tree, real_nodes, node_to_way + last_query_result = new_result, tree, real_nodes, node_to_way, location_info last_query_pos = last_gps cache_valid = True query_lock.release() @@ -182,7 +192,7 @@ def mapsd_thread(): query_lock.acquire() cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way) if cur_way is not None: - pnts, curvature_valid = cur_way.get_lookahead(last_query_result, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) xs = pnts[:, 0] ys = pnts[:, 1] @@ -251,11 +261,24 @@ def mapsd_thread(): dat.liveMapData.wayId = cur_way.id # Seed limit - max_speed = cur_way.max_speed + max_speed = cur_way.max_speed() if max_speed is not None: dat.liveMapData.speedLimitValid = True dat.liveMapData.speedLimit = max_speed + # TODO: use the function below to anticipate upcoming speed limits + #max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + #if max_speed_ahead is not None and max_speed_ahead_dist is not None: + # dat.liveMapData.speedLimitAheadValid = True + # dat.liveMapData.speedLimitAhead = float(max_speed_ahead) + # dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist) + + + advisory_max_speed = cur_way.advisory_max_speed() + if advisory_max_speed is not None: + dat.liveMapData.speedAdvisoryValid = True + dat.liveMapData.speedAdvisory = advisory_max_speed + # Curvature dat.liveMapData.curvatureValid = curvature_valid dat.liveMapData.curvature = float(upcoming_curvature) diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index 09239f6a2..0a462765e 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -1,13 +1,23 @@ import math +import json import numpy as np from datetime import datetime - +from common.basedir import BASEDIR from selfdrive.config import Conversions as CV from common.transformations.coordinates import LocalCoord, geodetic2ecef LOOKAHEAD_TIME = 10. MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME +DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json" +DEFAULT_SPEEDS = {} +with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS = json.loads(f.read()) + +DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +DEFAULT_SPEEDS_BY_REGION = {} +with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS_BY_REGION = json.loads(f.read()) def circle_through_points(p1, p2, p3): """Fits a circle through three points @@ -23,28 +33,90 @@ def circle_through_points(p1, p2, p3): return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) - def parse_speed_unit(max_speed): """Converts a maxspeed string to m/s based on the unit present in the input. OpenStreetMap defaults to kph if no unit is present. """ + if not max_speed: + return None + conversion = CV.KPH_TO_MS if 'mph' in max_speed: max_speed = max_speed.replace(' mph', '') conversion = CV.MPH_TO_MS - try: - max_speed = float(max_speed) * conversion + return float(max_speed) * conversion except ValueError: - max_speed = None + return None +def parse_speed_tags(tags): + """Parses tags on a way to find the maxspeed string""" + max_speed = None + + if 'maxspeed' in tags: + max_speed = tags['maxspeed'] + + if 'maxspeed:conditional' in tags: + try: + max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ') + cond = cond[1:-1] + + start, end = cond.split('-') + now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays + start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + + if start <= now <= end: + max_speed = max_speed_cond + except ValueError: + pass + + if not max_speed and 'source:maxspeed' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None) + if not max_speed and 'maxspeed:type' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None) + + max_speed = parse_speed_unit(max_speed) return max_speed +def geocode_maxspeed(tags, location_info): + max_speed = None + try: + geocode_country = location_info.get('country', '') + geocode_region = location_info.get('region', '') + + country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {}) + country_defaults = country_rules.get('Default', []) + for rule in country_defaults: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].iteritems() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching country + + region_rules = country_rules.get(geocode_region, []) + for rule in region_rules: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].iteritems() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching region + except KeyError: + pass + max_speed = parse_speed_unit(max_speed) + return max_speed class Way: - def __init__(self, way): + def __init__(self, way, query_results): self.id = way.id self.way = way + self.query_results = query_results points = list() @@ -55,7 +127,7 @@ class Way: @classmethod def closest(cls, query_results, lat, lon, heading, prev_way=None): - results, tree, real_nodes, node_to_way = query_results + results, tree, real_nodes, node_to_way, location_info = query_results cur_pos = geodetic2ecef((lat, lon, 0)) nodes = tree.query_ball_point(cur_pos, 500) @@ -73,7 +145,7 @@ class Way: closest_way = None best_score = None for way in ways: - way = Way(way) + way = Way(way, query_results) points = way.points_in_car_frame(lat, lon, heading) on_way = way.on_way(lat, lon, heading, points) @@ -124,35 +196,65 @@ class Way: def __str__(self): return "%s %s" % (self.id, self.way.tags) - @property def max_speed(self): """Extracts the (conditional) speed limit from a way""" if not self.way: return None - tags = self.way.tags - max_speed = None - - if 'maxspeed' in tags: - max_speed = parse_speed_unit(tags['maxspeed']) - - try: - if 'maxspeed:conditional' in tags: - max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ') - cond = cond[1:-1] - - start, end = cond.split('-') - now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays - start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day) - end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day) - - if start <= now <= end: - max_speed = parse_speed_unit(max_speed_cond) - except ValueError: - pass + max_speed = parse_speed_tags(self.way.tags) + if not max_speed: + location_info = self.query_results[4] + max_speed = geocode_maxspeed(self.way.tags, location_info) return max_speed + def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): + """Look ahead for a max speed""" + if not self.way: + return None + + speed_ahead = None + speed_ahead_dist = None + lookahead_ways = 5 + way = self + for i in range(lookahead_ways): + way_pts = way.points_in_car_frame(lat, lon, heading) + + # Check current lookahead distance + max_dist = np.linalg.norm(way_pts[-1, :]) + + if max_dist > 2 * lookahead: + break + + if 'maxspeed' in way.way.tags: + spd = parse_speed_tags(way.way.tags) + if not spd: + location_info = self.query_results[4] + spd = geocode_maxspeed(way.way.tags, location_info) + if spd < current_speed_limit: + speed_ahead = spd + min_dist = np.linalg.norm(way_pts[1, :]) + speed_ahead_dist = min_dist + break + # Find next way + way = way.next_way() + if not way: + break + + return speed_ahead, speed_ahead_dist + + def advisory_max_speed(self): + if not self.way: + return None + + tags = self.way.tags + adv_speed = None + + if 'maxspeed:advisory' in tags: + adv_speed = tags['maxspeed:advisory'] + adv_speed = parse_speed_unit(adv_speed) + return adv_speed + def on_way(self, lat, lon, heading, points=None): if points is None: points = self.points_in_car_frame(lat, lon, heading) @@ -186,8 +288,8 @@ class Way: return points_carframe - def next_way(self, query_results, lat, lon, heading, backwards=False): - results, tree, real_nodes, node_to_way = query_results + def next_way(self, backwards=False): + results, tree, real_nodes, node_to_way, location_info = self.query_results if backwards: node = self.way.nodes[0] @@ -215,18 +317,20 @@ class Way: # Filter on number of lanes cur_num_lanes = int(self.way.tags['lanes']) if len(ways) > 1: - ways = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes] + ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes] + if len(ways_same_lanes) == 1: + ways = ways_same_lanes if len(ways) > 1: ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes] if len(ways) == 1: - way = Way(ways[0]) + way = Way(ways[0], self.query_results) except (KeyError, ValueError): pass return way - def get_lookahead(self, query_results, lat, lon, heading, lookahead): + def get_lookahead(self, lat, lon, heading, lookahead): pnts = None way = self valid = False @@ -249,7 +353,7 @@ class Way: break # Find next way - way = way.next_way(query_results, lat, lon, heading) + way = way.next_way() if not way: break diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index 76862e44c..7c4dc9110 100755 Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index dd95ea9d1..692a23654 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index 7a875d219..627d40df8 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -72,6 +72,9 @@ orbFeaturesSummary: [8062, true] driverMonitoring: [8063, true] liveParameters: [8064, true] liveMapData: [8065, true] +cameraOdometry: [8066, true] +pathPlan: [8067, true] +kalmanOdometry: [8068, true] testModel: [8040, false] testLiveLocation: [8045, false] diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py index e5affb627..e3abd6bf7 100755 --- a/selfdrive/test/tests/plant/test_longitudinal.py +++ b/selfdrive/test/tests/plant/test_longitudinal.py @@ -293,14 +293,17 @@ class LongitudinalControl(unittest.TestCase): manager.gctx = {} manager.prepare_managed_process('radard') manager.prepare_managed_process('controlsd') + manager.prepare_managed_process('plannerd') manager.start_managed_process('radard') manager.start_managed_process('controlsd') + manager.start_managed_process('plannerd') @classmethod def tearDownClass(cls): manager.kill_managed_process('radard') manager.kill_managed_process('controlsd') + manager.kill_managed_process('plannerd') time.sleep(5) # hack @@ -321,4 +324,3 @@ for k in xrange(WORKERS): if __name__ == "__main__": unittest.main() - diff --git a/selfdrive/visiond/build_from_src.mk b/selfdrive/visiond/build_from_src.mk index efa151d1d..e23887f27 100644 --- a/selfdrive/visiond/build_from_src.mk +++ b/selfdrive/visiond/build_from_src.mk @@ -97,7 +97,7 @@ else OPENGL_LIBS = -lGLESv3 -lEGL SNPE_FLAGS = -I$(PHONELIBS)/snpe/include/ - SNPE_LIBS = -L$(PHONELIBS)/snpe/lib -lSNPE -lsymphony-cpu -lsymphonypower + SNPE_LIBS = -lSNPE -lsymphony-cpu -lsymphonypower OTHER_LIBS = -lz -lcutils -lm -llog -lui -ladreno_utils @@ -137,7 +137,7 @@ OBJS += $(PLATFORM_OBJS) \ $(CEREAL_OBJS) #MODEL_DATA = ../../models/driving_bigmodel.dlc ../../models/monitoring_model.dlc -MODEL_DATA = ../../models/driving_model.dlc ../../models/monitoring_model.dlc +MODEL_DATA = ../../models/driving_model.dlc ../../models/monitoring_model.dlc ../../models/posenet.dlc MODEL_OBJS = $(MODEL_DATA:.dlc=.o) OBJS += $(MODEL_OBJS) diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/visiond/visiond.cc index a64eaa054..af10d760c 100644 --- a/selfdrive/visiond/visiond.cc +++ b/selfdrive/visiond/visiond.cc @@ -49,6 +49,8 @@ #include "cereal/gen/cpp/log.capnp.h" +#define M_PI 3.14159265358979323846 + #define UI_BUF_COUNT 4 //#define DUMP_RGB @@ -166,6 +168,9 @@ struct VisionState { zsock_t *recorder_sock; void* recorder_sock_raw; + zsock_t *posenet_sock; + void* posenet_sock_raw; + pthread_mutex_t clients_lock; VisionClientState clients[MAX_CLIENTS]; @@ -894,6 +899,15 @@ void* frontview_thread(void *arg) { return NULL; } +#define POSENET + +#ifdef POSENET +#include "snpemodel.h" +extern const uint8_t posenet_model_data[] asm("_binary_posenet_dlc_start"); +extern const uint8_t posenet_model_end[] asm("_binary_posenet_dlc_end"); +const size_t posenet_model_size = posenet_model_end - posenet_model_data; +#endif + void* processing_thread(void *arg) { int err; VisionState *s = (VisionState*)arg; @@ -924,6 +938,14 @@ void* processing_thread(void *arg) { FILE *dump_rgb_file = fopen("/sdcard/dump.rgb", "wb"); #endif +#ifdef POSENET + int posenet_counter = 0; + float pose_output[12]; + float *posenet_input = (float*)malloc(2*200*532*sizeof(float)); + SNPEModel *posenet = new SNPEModel(posenet_model_data, posenet_model_size, + pose_output, sizeof(pose_output)/sizeof(float)); +#endif + // init the net LOG("processing start!"); @@ -947,11 +969,6 @@ void* processing_thread(void *arg) { int ui_idx = tbuffer_select(&s->ui_tb); int rgb_idx = ui_idx; - // printf("idx %d\n", rgb_idx); - - /*FILE *f = fopen("/tmp/test_dump", "wb"); - fwrite(s->camera_bufs[buf_idx].addr, 1, s->camera_bufs[buf_idx].len, f); - fclose(f);*/ cl_event debayer_event; if (s->cameras.rear.ci.bayer) { @@ -1069,6 +1086,89 @@ void* processing_thread(void *arg) { } +#ifdef POSENET + double pt1 = 0, pt2 = 0, pt3 = 0; + pt1 = millis_since_boot(); + + // move second frame to first frame + memmove(&posenet_input[0], &posenet_input[1], sizeof(float)*(200*532*2 - 1)); + + // fill posenet input + float a; + // posenet uses a half resolution cropped frame + // with upper left corner: [50, 237] and + // bottom right corner: [1114, 637] + // So the resulting crop is 532 X 200 + for (int y=237; y<637; y+=2) { + int yy = (y-237)/2; + for (int x = 50; x < 1114; x+=2) { + int xx = (x-50)/2; + a = 0; + a += yuv_ptr_y[s->yuv_width*(y+0) + (x+1)]; + a += yuv_ptr_y[s->yuv_width*(y+1) + (x+1)]; + a += yuv_ptr_y[s->yuv_width*(y+0) + (x+0)]; + a += yuv_ptr_y[s->yuv_width*(y+1) + (x+0)]; + // The posenet takes a normalized image input + // like the driving model so [0,255] is remapped + // to [-1,1] + posenet_input[(yy*532+xx)*2 + 1] = (a/512.0 - 1.0); + } + } + //FILE *fp; + //fp = fopen( "testing" , "r" ); + //fread(posenet_input , sizeof(float) , 200*532*2 , fp); + //fclose(fp); + //sleep(5); + + pt2 = millis_since_boot(); + + posenet_counter++; + + if (posenet_counter % 5 == 0){ + // run posenet + //printf("avg %f\n", pose_output[0]); + posenet->execute(posenet_input); + + + // fix stddevs + for (int i = 6; i < 12; i++) { + pose_output[i] = log1p(exp(pose_output[i])) + 1e-6; + } + // to radians + for (int i = 3; i < 6; i++) { + pose_output[i] = M_PI * pose_output[i] / 180.0; + } + // to radians + for (int i = 9; i < 12; i++) { + pose_output[i] = M_PI * pose_output[i] / 180.0; + } + + // send posenet event + { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto posenetd = event.initCameraOdometry(); + kj::ArrayPtr trans_vs(&pose_output[0], 3); + posenetd.setTrans(trans_vs); + kj::ArrayPtr rot_vs(&pose_output[3], 3); + posenetd.setRot(rot_vs); + kj::ArrayPtr trans_std_vs(&pose_output[6], 3); + posenetd.setTransStd(trans_std_vs); + kj::ArrayPtr rot_std_vs(&pose_output[9], 3); + posenetd.setRotStd(rot_std_vs); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + zmq_send(s->posenet_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); + } + pt3 = millis_since_boot(); + LOGD("pre: %.2fms | posenet: %.2fms", (pt2-pt1), (pt3-pt1)); + } +#endif + + tbuffer_dispatch(&s->ui_tb, ui_idx); // auto exposure over big box @@ -1319,6 +1419,10 @@ int main(int argc, char **argv) { assert(s->monitoring_sock); s->monitoring_sock_raw = zsock_resolve(s->monitoring_sock); + s->posenet_sock = zsock_new_pub("@tcp://*:8066"); + assert(s->posenet_sock); + s->posenet_sock_raw = zsock_resolve(s->posenet_sock); + cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0]); if (test_run) {