From 773dce507082d931236b64dca8024dce9625446f Mon Sep 17 00:00:00 2001 From: chiachunli08 <57161492+chiachunli08@users.noreply.github.com> Date: Mon, 22 Aug 2022 14:12:01 +0800 Subject: [PATCH] Add volvo support to Dragonpilot --- cereal/car.capnp | 2 + opendbc/volvo_v40_2017_pt.dbc | 363 +++++++++++++++ opendbc/volvo_v60_2015_pt.dbc | 230 ++++++++++ panda/board/safety.h | 5 + panda/board/safety/safety_volvo.h | 599 +++++++++++++++++++++++++ selfdrive/car/volvo/__init__.py | 0 selfdrive/car/volvo/carcontroller.py | 296 ++++++++++++ selfdrive/car/volvo/carstate.py | 407 +++++++++++++++++ selfdrive/car/volvo/interface.py | 139 ++++++ selfdrive/car/volvo/radar_interface.py | 5 + selfdrive/car/volvo/values.py | 153 +++++++ selfdrive/car/volvo/volvocan.py | 109 +++++ 12 files changed, 2308 insertions(+) create mode 100644 opendbc/volvo_v40_2017_pt.dbc create mode 100644 opendbc/volvo_v60_2015_pt.dbc create mode 100644 panda/board/safety/safety_volvo.h create mode 100644 selfdrive/car/volvo/__init__.py create mode 100644 selfdrive/car/volvo/carcontroller.py create mode 100644 selfdrive/car/volvo/carstate.py create mode 100644 selfdrive/car/volvo/interface.py create mode 100644 selfdrive/car/volvo/radar_interface.py create mode 100644 selfdrive/car/volvo/values.py create mode 100644 selfdrive/car/volvo/volvocan.py diff --git a/cereal/car.capnp b/cereal/car.capnp index ebc897fc6..784fd04cc 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -555,6 +555,8 @@ struct CarParams { hyundaiLegacy @23; hyundaiCommunity @24; stellantis @25; + volvoC1 @26; + volvoEUCD @27; } enum SteerControlType { diff --git a/opendbc/volvo_v40_2017_pt.dbc b/opendbc/volvo_v40_2017_pt.dbc new file mode 100644 index 000000000..f3febb732 --- /dev/null +++ b/opendbc/volvo_v40_2017_pt.dbc @@ -0,0 +1,363 @@ +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 BCM CEM CVM DIM ECM FSM PSCM SAS SRS TCM + +BO_ 8 SAS0: 8 SAS + SG_ SteeringDirection : 42|1@0+ (1,0) [0|1] "" XXX + SG_ RelativeTurnDirection : 43|1@0+ (1,0) [0|1] "" XXX + SG_ SteeringAngle : 53|14@0+ (0.04395,0) [0|65535] "degrees" XXX + SG_ NEW_SIGNAL_1 : 47|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 39|4@0+ (1,0) [0|15] "" XXX + SG_ AngleRate : 21|14@0+ (0.075,0) [0|1500] "deg/S" XXX + +BO_ 16 CCButtons: 8 CEM + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|5@0+ (1,0) [0|31] "" XXX + SG_ B7b0 : 56|1@0+ (1,0) [0|1] "" XXX + SG_ B7b1 : 57|1@0+ (1,0) [0|1] "" XXX + SG_ B7b6 : 62|1@0+ (1,0) [0|1] "" XXX + SG_ ACCOnOffBtn : 58|1@0+ (1,0) [0|1] "" XXX + SG_ ACCSetBtn : 63|1@0+ (1,0) [0|1] "" XXX + SG_ ACCStopBtn : 60|1@0+ (1,0) [0|1] "" XXX + SG_ ACCResumeBtn : 61|1@0+ (1,0) [0|1] "" XXX + SG_ ACCMinusBtn : 48|1@0+ (1,0) [0|1] "" XXX + SG_ TimeGapIncreaseBtn : 49|1@0+ (1,0) [0|1] "" XXX + SG_ TimeGapDecreaseBtn : 50|1@0+ (1,0) [0|1] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ B7b3 : 59|1@0+ (1,0) [0|1] "" XXX + +BO_ 48 FSM0: 8 FSM + SG_ ACCStatusTracking : 56|1@0+ (1,0) [0|1] "" XXX + SG_ ACCStatusOnOff : 57|1@0+ (1,0) [0|1] "" XXX + SG_ ACCStatusActive : 58|1@0+ (1,0) [0|1] "" XXX + SG_ FCWSomething : 25|3@0+ (1,0) [0|3] "" XXX + SG_ StatusSomething : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 64 TCM0: 8 TCM + SG_ RPMSomething : 42|11@0+ (1,0) [0|2047] "" XXX + SG_ GearShifter : 46|2@0+ (1,0) [0|3] "" XXX + +BO_ 85 PedalandBrake: 8 ECM + SG_ AccPedal : 9|10@0+ (0.1,0) [0|1023] "%" XXX + SG_ BrakePedalActive2 : 24|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 35|12@0+ (1,0) [0|4095] "" XXX + SG_ BrakePedalActive : 38|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 101 EngineInfo: 8 XXX + SG_ NEW_SIGNAL_1 : 17|10@0+ (1,-512) [0|1023] "" XXX + SG_ EngineSpeed : 52|13@0+ (1,0) [0|1023] "" XXX + +BO_ 112 NEW_MSG_4: 8 XXX + SG_ NEW_SIGNAL_1 : 39|8@0+ (1,0) [0|255] "" XXX + +BO_ 114 ECM1: 8 ECM1 + SG_ NEW_SIGNAL_1 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ ECM_ACC_ONOFF_INV : 43|1@0+ (1,0) [0|1] "" XXX + SG_ ECM_ACC_RESUME_INV : 45|1@0+ (1,0) [0|1] "" XXX + SG_ ECM_ACC_SET_INV : 47|1@0+ (1,0) [0|1] "" XXX + SG_ ECM_ACC_TIMEGAP_INC_INV : 33|1@0+ (1,0) [0|1] "" XXX + SG_ ECM_ACC_DEC_INV : 32|1@0+ (1,0) [0|1] "" XXX + SG_ ECM_ACC_TIMEGAP_DEC_INV : 34|1@0+ (1,0) [0|1] "" XXX + +BO_ 117 ECM1: 8 ECM + +BO_ 128 NEW_MSG_5: 8 XXX + SG_ NEW_SIGNAL_2 : 26|11@0+ (1,0) [0|2047] "" XXX + SG_ NEW_SIGNAL_1 : 52|13@0+ (1,0) [0|8191] "" XXX + +BO_ 176 ECM2: 8 ECM + SG_ NEW_SIGNAL_1 : 50|11@0+ (1,0) [0|2047] "" XXX + SG_ NEW_SIGNAL_2 : 47|8@0+ (1,0) [0|63] "" XXX + +BO_ 192 Gear: 8 XXX + SG_ TransmissionGear : 36|3@0+ (1,1) [0|7] "" XXX + +BO_ 208 FSM1: 8 FSM + SG_ SET_X_E3 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ SET_X_B4 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ SET_X_08 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ TrqLim : 31|8@0+ (1,-128) [0|255] "" XXX + SG_ Checksum : 55|8@0+ (1,0) [0|255] "" XXX + SG_ LKASteerDirection : 57|2@0+ (1,0) [0|2] "" XXX + SG_ SET_X_25 : 63|6@0+ (1,0) [0|63] "" XXX + SG_ LKAAngleReq : 37|14@0+ (0.04395,-360.0384) [-360.0384|359.99445] "degrees" XXX + SG_ SET_X_02 : 39|2@0+ (1,0) [0|3] "" XXX + +BO_ 224 PSCM0: 8 PSCM + SG_ NEW_SIGNAL_2 : 12|5@0+ (1,0) [0|31] "" XXX + SG_ counter_07 : 15|3@0+ (1,0) [0|7] "" XXX + SG_ counter2_07 : 37|3@0+ (1,0) [0|16383] "" XXX + SG_ rate_of_something : 46|7@0+ (1,0) [0|62] "" XXX + SG_ OneDuringDriving : 49|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 63|1@0+ (1,0) [0|1] "" XXX + +BO_ 245 wheelspeed0: 8 BCM + SG_ counter1 : 21|6@0+ (1,0) [0|65535] "" XXX + SG_ counter0 : 7|16@0+ (1,0) [0|65535] "" XXX + SG_ WhlSpdLF : 39|16@0+ (1,0) [0|65535] "" XXX + SG_ WhlSpdRF : 55|16@0+ (1,0) [0|65535] "" XXX + +BO_ 272 SpeedSignal0: 8 XXX + SG_ VehicleSpeedSignal : 55|16@0+ (0.01,0) [0|65535] "" XXX + +BO_ 288 wheel_speed1: 8 BCM + SG_ WhlSpdLR : 39|16@0+ (1,0) [0|65535] "" XXX + SG_ WhlSpdRR : 55|16@0+ (1,0) [0|65535] "" XXX + +BO_ 293 PSCM1: 8 PSCM + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + SG_ LKATorque : 11|12@0+ (1,-2000) [0|4095] "" XXX + SG_ SteeringAngleServo : 47|16@0+ (0.04395,-1440.1536) [0|65535] "deg" XXX + SG_ LKAActive : 15|4@0+ (1,0) [0|15] "" XXX + +BO_ 304 VehicleSpeed0: 8 BCM + SG_ NEW_SIGNAL_2 : 15|16@0+ (1,0) [0|65535] "" XXX + SG_ VehicleSpeed : 31|16@0+ (0.01,0) [0|65535] "km/h" XXX + SG_ NEW_SIGNAL_1 : 55|16@0+ (1,0) [0|65535] "" XXX + +BO_ 325 ECM3: 8 ECM + +BO_ 336 VehicleSpeed1: 8 BCM + SG_ NEW_SIGNAL_2 : 31|16@0+ (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_1 : 15|16@0+ (1,0) [0|65535] "" XXX + SG_ VehicleSpeed : 55|16@0+ (0.01,0) [0|65535] "" XXX + +BO_ 352 FSM2: 8 FSM + SG_ LkaDimLine : 51|2@1+ (1,0) [0|127] "" XXX + SG_ NEW_SIGNAL_2 : 56|7@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_1 : 55|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 7|24@0+ (1,0) [0|16777215] "" XXX + SG_ NEW_SIGNAL_4 : 36|5@0+ (1,0) [0|31] "" XXX + +BO_ 432 BrakeMessages: 8 BCM + SG_ BrakePress0 : 1|10@0+ (1,0) [0|1023] "" XXX + SG_ BrakePress1 : 33|10@0+ (1,0) [0|1023] "" XXX + SG_ BrakeStatus : 18|3@0+ (1,0) [0|7] "" XXX + +BO_ 464 DIM0: 8 DIM + +BO_ 480 BCM0: 8 BCM + +BO_ 528 CEM0: 8 CEM + +BO_ 608 CVM0: 8 CVM + SG_ NEW_SIGNAL_1 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 15|5@0+ (1,0) [0|32] "" XXX + SG_ Distance : 10|11@0+ (1,0) [0|2048] "" XXX + +BO_ 624 FSM3: 8 FSM + SG_ NEW_SIGNAL_1 : 23|8@0+ (1,0) [0|255] "" XXX + +BO_ 640 FSM4: 8 FSM + SG_ SpeedTarget : 47|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_1 : 49|10@0+ (1,0) [0|255] "" XXX + +BO_ 648 SRS0: 8 SRS + +BO_ 652 ECM4: 8 ECM + +BO_ 656 ECM5: 8 ECM + +BO_ 657 ECM6: 8 ECM + +BO_ 681 MiscCarInfo: 8 CEM + SG_ TurnSignal : 1|2@0+ (1,0) [0|3] "" XXX + SG_ HighBeamOn : 52|1@0+ (1,0) [0|1] "" XX + +BO_ 693 ECM7: 8 ECM + +BO_ 709 ACC: 8 ECM + SG_ SpeedTargetACC : 0|9@0+ (0.5,0) [0|511] "" XXX + +BO_ 853 FSM5: 8 FSM + SG_ TargetSpeedOdo : 23|8@0+ (1,0) [0|63] "kph" XXX + SG_ SpeedSign : 36|5@0+ (5,0) [0|32] "" XXX + SG_ TextUnderSign : 37|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_6 : 39|3@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ LaneMarkingsOdo : 15|4@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_2 : 11|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_1 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 55|16@0+ (1,0) [0|65535] "" XXX + +BO_ 864 CEM1: 8 CEM + +BO_ 912 DIM1: 8 DIM + +BO_ 968 SRS1: 8 SRS + SG_ PassengerSeatBelt : 22|1@0+ (1,0) [0|1] "" XXX + SG_ DriverSeatBelt : 19|1@0+ (1,0) [0|1] "" XXX + +BO_ 1029 CEMBCM0: 8 CEM + +BO_ 1344 NEW_MSG_1: 8 XXX + SG_ NEW_SIGNAL_1 : 4|13@0+ (1,0) [0|8191] "" XXX + +BO_ 1830 diagCEMReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 1838 diagCEMResp: 8 XXX + SG_ byte03 : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ byte47 : 39|32@0+ (1,0) [0|4294967295] "" XXX + +BO_ 1840 diagPSCMReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 1848 diagPSCMResp: 8 XXX + SG_ byte03 : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ byte47 : 39|32@0+ (1,0) [0|4294967295] "" XXX + +BO_ 1892 diagFSMReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 1900 diagFSMResp: 8 XXX + SG_ byte03 : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ byte47 : 39|32@0+ (1,0) [0|4294967295] "" XXX + +BO_ 1939 diagCVMReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 1947 diagCVMResp: 8 XXX + SG_ byte03 : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ byte47 : 39|32@0+ (1,0) [0|4294967295] "" XXX + +BO_ 2015 diagGlobalReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + + + + +CM_ SG_ 85 BrakePedalActive2 "Active during braking"; +CM_ SG_ 85 NEW_SIGNAL_1 "Not yet figured out."; +CM_ SG_ 85 BrakePedalActive "Brake pedal pushed"; +CM_ SG_ 8 SteeringDirection "1=Right turn, 0=Left turn. Steering wheel pointing left or right from center (0 deg)."; +CM_ SG_ 8 RelativeTurnDirection "1=Right turn, 0=Left turn. Steering wheel currently turning the way."; +CM_ SG_ 101 NEW_SIGNAL_1 "Rate of something?"; +CM_ SG_ 192 TransmissionGear "0 = 1st gear, 1= 2nd gear..."; +CM_ SG_ 681 TurnSignal "0 = Nothing, 1= Left, 3=Right"; +CM_ SG_ 681 HighBeamOn "1=HighBeam On, 0=HighBeam Off"; +CM_ SG_ 48 ACCStatusTracking "ACC Tracking vehicle, distance control."; +CM_ SG_ 48 ACCStatusOnOff "Turns one after pressing on/off button on steering wheeel"; +CM_ SG_ 48 ACCStatusActive "ACC Active"; +CM_ SG_ 48 FCWSomething "All bit set during fcw"; +CM_ SG_ 48 StatusSomething "Some status changes when zeroing DTCs"; +CM_ SG_ 208 TrqLim "Used in checksum calculation, Limit directional torque based on the number."; +CM_ SG_ 208 Checksum "Checksum calculated as a one-complement addition of LKAAngleRequest+LKADirection+Unkown, Zeros used to pad missing bits."; +CM_ SG_ 208 SET_X_02 "Bit 0 = Vibrate steering wheel., Bit 1 = Heartbeat"; +CM_ SG_ 352 LkaDimLine "Not true, but follows lka steer direction."; +CM_ SG_ 352 NEW_SIGNAL_1 "Turned one. Got LKA service needed can this be the one?"; +CM_ SG_ 640 SpeedTarget "SpeedTarget ACC (noisy bf starting acc Jumps from 0->252->0)"; +CM_ SG_ 853 TargetSpeedOdo "Probably target speed odo"; +CM_ SG_ 853 LaneMarkingsOdo "Bit 3=Left lane, Bit 2=Right lane, Bit 1=LKA on?, Bit 0=?"; +CM_ SG_ 709 SpeedTargetACC "SpeedTargetACC"; +CM_ SG_ 224 rate_of_something "Seems to be some kind of torque rather than rate."; +CM_ SG_ 224 OneDuringDriving "Set to 1 when vehicle is rolling."; +CM_ SG_ 293 byte7 "Bit0=0 when gearshift in park, else 1"; +CM_ SG_ 293 LKAActive "Bit0=0 when gear in park otherwise =1, Bit1=1 when LKA Active, 0 when not active. Bit2=? Bit3=?"; +CM_ SG_ 16 ACCOnOffBtn "Cruise control on/off button pressed"; +CM_ SG_ 16 ACCSetBtn "Acc Set button (+) pressed"; +CM_ SG_ 16 ACCStopBtn "ACC Stop button pressed"; +CM_ SG_ 16 ACCResumeBtn "ACC Resume button pressed"; +CM_ SG_ 16 ACCMinusBtn "ACC Minus (-) button pressed"; +CM_ SG_ 16 TimeGapIncreaseBtn "Increase the time gap on ACC"; +CM_ SG_ 16 TimeGapDecreaseBtn "Decrease the time gap on ACC"; +CM_ SG_ 245 counter0 "Speed based counter"; +CM_ SG_ 245 WhlSpdLF "Wheel speed left front"; +CM_ SG_ 245 WhlSpdRF "Wheel speed right front"; +CM_ SG_ 288 WhlSpdLR "Wheel speed left rear"; +CM_ SG_ 288 WhlSpdRR "Wheel speed right rear"; +CM_ SG_ 64 RPMSomething "TransmissionOutput?"; +CM_ SG_ 64 GearShifter "P=0, R=1, N=2, D=3"; +CM_ SG_ 272 VehicleSpeedSignal "km/h"; +CM_ SG_ 432 BrakePress0 "Brake being pressed"; +CM_ SG_ 432 BrakePress1 "Brake being pressed"; +CM_ SG_ 432 BrakeStatus "ACC brake?"; +CM_ SG_ 437 Counter0 "Related to braking? Maybe one per wheel?"; +CM_ SG_ 437 Counter1 "Related to braking? Maybe one per wheel?"; +CM_ SG_ 437 Counter2 "Related to braking? Maybe one per wheel?"; +CM_ SG_ 437 Counter3 "Related to braking? Maybe one per wheel?"; +CM_ SG_ 114 NEW_SIGNAL_1 "Jumped from 0 -> 120 during start. Makes triangle from time to time"; +CM_ SG_ 608 NEW_SIGNAL_1 "Status?"; +CM_ SG_ 608 NEW_SIGNAL_2 "Classification of object?"; +CM_ SG_ 608 Distance "Distance to object in front."; +CM_ SG_ 968 PassengerSeatBelt "1 = Seatbalt latched"; +CM_ SG_ 968 DriverSeatBelt "1=Seatbelt latched"; +VAL_ 64 GearShifter 0 "P" 1 "R" 2 "N" 3 "D" ; diff --git a/opendbc/volvo_v60_2015_pt.dbc b/opendbc/volvo_v60_2015_pt.dbc new file mode 100644 index 000000000..f95eff00c --- /dev/null +++ b/opendbc/volvo_v60_2015_pt.dbc @@ -0,0 +1,230 @@ +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 BCM CEM FSM PSCM SAS + +BO_ 16 SAS0: 8 SAS + SG_ Counter0 : 3|8@0+ (1,0) [0|511] "" XXX + SG_ RateOfChangeOrTorque : 39|16@0+ (1,-32768) [0|65535] "" XXX + SG_ NEW_SIGNAL_1 : 22|15@0+ (1,0) [0|65535] "" XXX + SG_ SteeringDirection : 6|1@0+ (1,0) [0|1] "" XXX + SG_ SteeringAngle : 53|14@0+ (0.0445,0) [0|65535] "degrees" XXX + +BO_ 32 AccPedal: 8 XXX + SG_ AccPedal : 17|10@0+ (0.1,0) [0|1023] "%" XXX + +BO_ 81 FSM0: 8 FSM + SG_ ACCStatus : 18|3@0+ (1,0) [0|7] "" XXX + +BO_ 277 NEW_MSG_7: 8 XXX + SG_ NEW_SIGNAL_1 : 39|16@0+ (1,0) [0|65535] "" XXX + +BO_ 295 CCButtons: 8 CEM + SG_ ACCMinusBtn : 48|1@0+ (1,0) [0|1] "" XXX + SG_ ACCSetBtn : 63|1@0+ (1,0) [0|1] "" XXX + SG_ ACCOnOffBtn : 59|1@0+ (1,0) [0|1] "" XXX + SG_ ACCResumeBtn : 61|1@0+ (1,0) [0|1] "" XXX + SG_ TimeGapDecreaseBtnInv : 34|1@0+ (1,0) [0|1] "" XXX + SG_ ACCOnOffBtnInv : 43|1@0+ (1,0) [0|1] "" XXX + SG_ ACCResumeBtnInv : 45|1@0+ (1,0) [0|1] "" XXX + SG_ ACCSetBtnInv : 47|1@0+ (1,0) [0|1] "" XXX + SG_ TimeGapIncreaseBtn : 49|1@0+ (1,0) [0|1] "" XXX + SG_ TimeGapDecreaseBtn : 50|1@0+ (1,0) [0|1] "" XXX + SG_ ACCMinusBtnInv : 32|1@0+ (1,0) [0|1] "" XXX + SG_ TimeGapIncreaseBtnInv : 33|1@0+ (1,0) [0|1] "" XXX + +BO_ 298 NEW_MSG_5: 8 XXX + SG_ EngineRpm : 52|13@0+ (1,0) [0|8000] "" XXX + +BO_ 336 NEW_MSG_8: 8 XXX + +BO_ 328 VehicleSpeed1: 8 XXX + SG_ VehicleSpeed : 55|16@0+ (0.01,0) [0|65535] "" XXX + +BO_ 465 NEW_MSG_4: 8 XXX + SG_ NEW_SIGNAL_1 : 55|16@0+ (1,0) [0|4095] "" XXX + +BO_ 544 wheelspeed1: 8 BCM + SG_ WhlSpdRR : 39|16@0+ (0.01,-327.68) [0|65535] "" XXX + SG_ WhlSpdLR : 55|16@0+ (0.01,-327.68) [0|65535] "" XXX + +BO_ 565 wheelspeed0: 8 BCM + SG_ WhlSpdLF : 55|16@0+ (0.01,-327.68) [0|65535] "" XXX + SG_ WhlSpdRF : 39|16@0+ (0.01,-327.68) [0|65535] "" XXX + +BO_ 582 PSCM1: 8 PSCM + SG_ byte4 : 39|4@0+ (1,0) [0|15] "" XXX + SG_ LKATorque : 35|12@0+ (1,-2000) [0|65535] "" XXX + SG_ SteeringAngleServo : 23|16@0+ (0.0447,-1468) [0|65535] "deg" XXX + SG_ SteeringWheelRateOfChange : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ LKAActive : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 608 FSM1: 8 FSM + SG_ ACC_Tracking : 7|8@0+ (1,0) [0|255] "" XXX + +BO_ 609 fromWhere: 8 XXX + SG_ SteeringAngle : 21|14@0+ (0.1,-1021) [0|65535] "deg" XXX + +BO_ 610 FSM2: 8 FSM + SG_ TrqLim : 23|8@0+ (1,-128) [0|255] "" PSCM + SG_ Checksum : 55|8@0+ (1,0) [0|255] "" PSCM + SG_ LKAAngleReq : 29|14@0+ (0.04,-327.68) [0|16383] "" PSCM + SG_ SET_X_02 : 31|2@0+ (1,0) [0|3] "" XXX + SG_ SET_X_10 : 47|6@0+ (1,0) [0|63] "" XXX + SG_ SET_X_A4 : 63|8@0+ (1,0) [0|255] "" XXX + SG_ SET_X_22 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ LKASteerDirection : 41|2@0+ (1,0) [0|3] "" PSCM + +BO_ 624 FSM3: 8 FSM + SG_ ACC_SOMETHING : 15|8@0+ (1,0) [0|255] "" XXX + SG_ ACC_Some : 17|10@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_3 : 47|8@0+ (1,0) [0|255] "" XXX + +BO_ 648 BrakePedal: 8 XXX + SG_ Counter : 0|3@1+ (1,0) [0|7] "" XXX + SG_ BrakePedal : 24|8@1+ (1,4) [0|255] "" XXX + +BO_ 794 FSM4: 8 FSM + SG_ NEW_SIGNAL_1 : 47|16@0+ (1,-46090) [0|16383] "" XXX + SG_ NEW_SIGNAL_2 : 32|4@1+ (1,0) [0|15] "" XXX + +BO_ 819 PSCM0: 8 PSCM + +BO_ 923 NEW_MSG_1: 8 XXX + SG_ NEW_SIGNAL_1 : 15|16@0+ (1,0) [0|65535] "" XXX + +BO_ 1021 FSM5: 8 FSM + +BO_ 1039 MiscCarInfo: 8 XXX + SG_ TurnSignal : 33|2@0+ (1,0) [0|3] "" XXX + +BO_ 1279 PSCM3: 8 PSCM + +BO_ 1830 diagCEMReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 1838 diagCEMResp: 8 XXX + SG_ byte03 : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ byte47 : 39|32@0+ (1,0) [0|4294967295] "" XXX + +BO_ 1840 diagPSCMReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 1848 diagPSCMResp: 8 XXX + SG_ byte03 : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ byte47 : 39|32@0+ (1,0) [0|4294967295] "" XXX + +BO_ 1892 diagFSMReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 1900 diagFSMResp: 8 XXX + SG_ byte03 : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ byte47 : 39|32@0+ (1,0) [0|4294967295] "" XXX + +BO_ 1939 diagCVMReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 1947 diagCVMResp: 8 XXX + SG_ byte03 : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ byte47 : 39|32@0+ (1,0) [0|4294967295] "" XXX + +BO_ 2015 diagGlobalReq: 8 XXX + SG_ byte0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ byte1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ byte2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ byte3 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ byte4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ byte5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ byte6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ byte7 : 63|8@0+ (1,0) [0|255] "" XXX + + + + +CM_ SG_ 16 RateOfChangeOrTorque "Rate of change? Torque?"; +CM_ SG_ 16 SteeringDirection "0 = CCW, 1=CW (turning left or right of center)"; +CM_ SG_ 298 EngineRpm "Might be engine rpm. But behaves abit weird."; +CM_ SG_ 582 byte4 "High nibble"; +CM_ SG_ 582 SteeringWheelRateOfChange "Some rate of change for steering wheel? Torque?"; +CM_ SG_ 582 byte0 "0=CCW, 1=CW, bit 2,"; +CM_ SG_ 582 LKAActive "Bit 1, 1 When LKA Active, Bit 3, 1 When denying?"; +CM_ SG_ 81 ACCStatus "0=Acc Unavailable, 1=???, 2=Acc Ready, 3,4=???, 6= Acc Active, 7=Acc active tracking object (probably) "; +CM_ SG_ 608 ACC_Tracking "Seems to track distance, or speed of vehicle in front."; +CM_ SG_ 610 SET_X_22 "0x20 Heartbeat, VEgo <58kph = 0x03, VEgo >65kph = 0x04, 0x05"; +CM_ SG_ 624 ACC_SOMETHING "Might be some acc speed, moved abit after activating acc"; +CM_ SG_ 624 ACC_Some "Jumps to life after activating ACC, 0 when not active"; +CM_ SG_ 295 ACCMinusBtn "ACC Minus (-) button pressed"; +CM_ SG_ 295 ACCSetBtn "Acc Set button (+) pressed"; +CM_ SG_ 295 ACCOnOffBtn "Cruise control on/off button pressed"; +CM_ SG_ 295 ACCResumeBtn "ACC Resume button pressed"; +CM_ SG_ 295 TimeGapDecreaseBtnInv "Active zero when button pressed."; +CM_ SG_ 295 ACCOnOffBtnInv "Active zero when button pressed."; +CM_ SG_ 295 ACCResumeBtnInv "Active zero when button pressed."; +CM_ SG_ 295 ACCSetBtnInv "Active zero when button pressed."; +CM_ SG_ 295 TimeGapIncreaseBtn "Increase the time gap on ACC"; +CM_ SG_ 295 TimeGapDecreaseBtn "Decrease the time gap on ACC"; +CM_ SG_ 295 ACCMinusBtnInv "Active zero when button pressed."; +CM_ SG_ 295 TimeGapIncreaseBtnInv "Active zero when button pressed."; +CM_ SG_ 1039 TurnSignal "0 = Nothing, 1= Left, 3=Right"; diff --git a/panda/board/safety.h b/panda/board/safety.h index b53f39587..9a6175011 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -15,6 +15,7 @@ #include "safety/safety_volkswagen_mqb.h" #include "safety/safety_volkswagen_pq.h" #include "safety/safety_elm327.h" +#include "safety/safety_volvo.h" // from cereal.car.CarParams.SafetyModel #define SAFETY_SILENT 0U @@ -40,6 +41,8 @@ #define SAFETY_HYUNDAI_LEGACY 23U #define SAFETY_HYUNDAI_COMMUNITY 24U #define SAFETY_STELLANTIS 25U +#define SAFETY_VOLVO_C1 26U +#define SAFETY_VOLVO_EUCD 27U uint16_t current_safety_mode = SAFETY_SILENT; int16_t current_safety_param = 0; @@ -257,6 +260,8 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_VOLKSWAGEN_PQ, &volkswagen_pq_hooks}, {SAFETY_ALLOUTPUT, &alloutput_hooks}, {SAFETY_FORD, &ford_hooks}, + {SAFETY_VOLVO_C1, &volvo_c1_hooks}, + {SAFETY_VOLVO_EUCD, &volvo_eucd_hooks}, #endif }; diff --git a/panda/board/safety/safety_volvo.h b/panda/board/safety/safety_volvo.h new file mode 100644 index 000000000..713a3b156 --- /dev/null +++ b/panda/board/safety/safety_volvo.h @@ -0,0 +1,599 @@ +//#define DEBUG_VOLVO + +#ifdef DEBUG_VOLVO +int giraffe_forward_camera_volvo_prev = 0; +bool controls_allowed_prev_v = 0; +bool relay_malfunction_prev = false; +bool valid_prev = false; +bool ok_to_send_prev = false; +int tx_prev = 0; +int counterloop = 0; +#endif + +/* +Volvo Electronic Control Units abbreviations and network topology +Platforms C1/EUCD + +Look in selfdrive/car/volvo/values.py for more information. +*/ + +// Globals +int giraffe_forward_camera_volvo = 0; +int acc_active_prev_volvo = 0; +int acc_ped_val_prev = 0; +int volvo_desired_angle_last = 0; +float volvo_speed = 0; + +// diagnostic msgs +#define MSG_DIAG_CEM 0x726 +#define MSG_DIAG_PSCM 0x730 +#define MSG_DIAG_FSM 0x764 +#define MSG_DIAG_CVM 0x793 +#define MSG_DIAG_BROADCAST 0x7df + +// platform C1 +// msg ids +#define MSG_BTNS_VOLVO_C1 0x10 // Steering wheel buttons +#define MSG_FSM0_VOLVO_C1 0x30 // ACC status message +#define MSG_FSM1_VOLVO_C1 0xd0 // LKA steering message +#define MSG_FSM2_VOLVO_C1 0x160 +#define MSG_FSM3_VOLVO_C1 0x270 +#define MSG_FSM4_VOLVO_C1 0x280 +#define MSG_FSM5_VOLVO_C1 0x355 +#define MSG_PSCM0_VOLVO_C1 0xe0 +#define MSG_PSCM1_VOLVO_C1 0x125 // Steering +#define MSG_ACC_PEDAL_VOLVO_C1 0x55 // Gas pedal +#define MSG_SPEED_VOLVO_C1 0x130 // Speed signal + +// platform eucd +// msg ids +#define MSG_FSM0_VOLVO_V60 0x51 // ACC status message +#define MSG_FSM1_VOLVO_V60 0x260 +#define MSG_FSM2_VOLVO_V60 0x262 // LKA steering message +#define MSG_FSM3_VOLVO_V60 0x270 +#define MSG_FSM4_VOLVO_V60 0x31a +#define MSG_FSM5_VOLVO_V60 0x3fd +#define MSG_PSCM1_VOLVO_V60 0x246 +#define MSG_ACC_PEDAL_VOLVO_V60 0x20 // Gas pedal +#define MSG_BTNS_VOLVO_V60 0x127 // Steering wheel buttons + +// safety params +const float DEG_TO_CAN_VOLVO_C1 = 1/0.04395; // 22.75312855517634‬, inverse of dbc scaling +const int VOLVO_MAX_DELTA_OFFSET_ANGLE = 20/0.04395-1; // max degrees divided by k factor in dbc 0.04395. -1 to give a little safety margin. + // 25 degrees allowed, more will trigger disengage by servo. +const int VOLVO_MAX_ANGLE_REQ = 8189; // max, min angle req, set at 2steps from max and min values. +const int VOLVO_MIN_ANGLE_REQ = -8190; // 14 bits long, min -8192 -> 8191. + +const struct lookup_t VOLVO_LOOKUP_ANGLE_RATE_UP = { + {7., 17., 36.}, // 25.2, 61.2, 129.6 km/h + {2, .25, .1} +}; +const struct lookup_t VOLVO_LOOKUP_ANGLE_RATE_DOWN = { + {7., 17., 36.}, + {2, .25, .1} +}; + +struct sample_t volvo_angle_meas; // last 3 steer angles + +/* +// saved for documentation purpose +// allowed messages to forward from bus 0 -> 2 +const int ALLOWED_MSG_C1[] = { +0x8, // SAS +0x10, // CEM +0x40, // TCM +0x55, // ECM, Test failed ACC seems to work, when standing still +//0x65, +//0x70, +0x72, // ECM, Test failed ACC seems to work, when standing still +0x75, // ECM +//0x80, +0xb0, // ECM +//0xc0, +0xe0, // PSCM +//0xf0, +0xf5, // BCM +//0x100, +//0x110, +0x120, // BCM +//0x123, +//0x125, // PSCM - Do not forward. FSM sets fault code if it sees LKAActive and LKATorque when not requesting steering, + // Forwarding and manipulation of bits done in carcontroller.py +0x130, // BCM critical for ACC +0x145, // ECM critical for ACC +0x150, // BCM +//0x1a8, +0x1b0, // BCM +0x1b5, // BCM +0x1d0, // DIM, Infotainment A , ACC ok, blis lka fcw off. +//0x1d8, +0x1e0, // BCM , blis fcw nok. +0x210, // CEM, Infotainment A, ACC ok, lka blis fcw off. +0x260, // ECM (When disconnecting CVM this message disappears. Why fault code for ECM?) +0x288, // SRS +0x28c, // ECM +0x290, // ECM +0x291, // ECM +0x2a9, // CEM, not critical +0x2b5, // ECM +//0x2c0, +//0x2c3, +0x2c5, // ECM +//0x330, +//0x340, +//0x350, +0x360, // CEM +//0x370, +0x390, // CEM, DIM +//0x3a0, +//0x3af, +//0x3b0, +0x3c8, // SRS +//0x3ca, +//0x3d0, +//0x3e0, +//0x3e5, +//0x400, +0x405, // CEM, BCM, CanBus System Program failure, C0 01 55, +//0x425, +//0x430, +//0x581, +0x764, // Diagnostic messages +0x7df, // Diagnostic messages +}; */ + +const int ALLOWED_MSG_EUCD[] = { +0x10, // SAS +0x20, +0x63, +0x68, +0x70, +0x90, +0x115, +0x127, +0x12A, +0x133, +0x140, +0x148, +0x150, +0x157, +0x160, +0x167, +0x180, +0x1D1, +0x1FF, +0x20A, +0x220, +0x235, +//0x246, PSCM1 +0x261, +0x264, +0x265, +0x272, +0x27B, +0x288, +0x299, +0x2A1, +0x2C0, +0x2C2, +0x2C4, +0x2EE, +0x2EF, +0x30A, +0x314, +0x31D, +0x322, +0x323, +0x325, +0x327, +0x333, +0x334, +0x335, +0x391, +0x39B, +0x3D2, +0x3D3, +0x3EE, +0x400, +0x405, +0x40F, +0x412, +0x415, +0x471, +0x475, +0x480, +0x496, +0x4A3, +0x4AE, +0x4BE, +0x4C1, +0x4CA, +0x4D8, +0x4FF, +0x581, +0x764, // Diagnostic messages +0x7df, // Diagnostic messages +}; + + +//const int ALLOWED_MSG_C1_LEN = sizeof(ALLOWED_MSG_C1) / sizeof(ALLOWED_MSG_C1[0]); +const int ALLOWED_MSG_EUCD_LEN = sizeof(ALLOWED_MSG_EUCD) / sizeof(ALLOWED_MSG_EUCD[0]); + +// TX checks +// platform c1 +const CanMsg VOLVO_C1_TX_MSGS[] = { {MSG_FSM0_VOLVO_C1, 0, 8}, {MSG_FSM1_VOLVO_C1, 0, 8}, + {MSG_FSM2_VOLVO_C1, 0, 8}, {MSG_FSM3_VOLVO_C1, 0, 8}, + {MSG_FSM4_VOLVO_C1, 0, 8}, + {MSG_BTNS_VOLVO_C1, 0, 8}, + {MSG_PSCM0_VOLVO_C1, 2, 8}, {MSG_PSCM1_VOLVO_C1, 2, 8}, + {MSG_DIAG_FSM, 2, 8}, {MSG_DIAG_PSCM, 0, 8}, + {MSG_DIAG_CEM, 0, 8}, {MSG_DIAG_CVM, 0, 8}, + {MSG_DIAG_BROADCAST, 0, 8}, {MSG_DIAG_BROADCAST, 2, 8}, + }; + +const int VOLVO_C1_TX_MSGS_LEN = sizeof(VOLVO_C1_TX_MSGS) / sizeof(VOLVO_C1_TX_MSGS[0]); +// platform eucd +const CanMsg VOLVO_EUCD_TX_MSGS[] = { {MSG_FSM0_VOLVO_V60, 0, 8}, {MSG_FSM1_VOLVO_V60, 0, 8}, + {MSG_FSM2_VOLVO_V60, 0, 8}, {MSG_FSM3_VOLVO_V60, 0, 8}, + {MSG_FSM4_VOLVO_V60, 0, 8}, {MSG_FSM5_VOLVO_V60, 0, 8}, + {MSG_PSCM1_VOLVO_V60, 2, 8}, + {MSG_BTNS_VOLVO_V60, 0, 8}, + {MSG_DIAG_FSM, 2, 8}, {MSG_DIAG_PSCM, 0, 8}, + {MSG_DIAG_CEM, 0, 8}, {MSG_DIAG_CVM, 0, 8}, + {MSG_DIAG_BROADCAST, 0, 8}, {MSG_DIAG_BROADCAST, 2, 8}, + }; +const int VOLVO_EUCD_TX_MSGS_LEN = sizeof(VOLVO_EUCD_TX_MSGS) / sizeof(VOLVO_EUCD_TX_MSGS[0]); + +// expected_timestep in microseconds between messages. +AddrCheckStruct volvo_c1_checks[] = { + {.msg = {{MSG_FSM0_VOLVO_C1, 2, 8, .check_checksum = false, .expected_timestep = 10000U}}}, + {.msg = {{MSG_FSM1_VOLVO_C1, 2, 8, .check_checksum = false, .expected_timestep = 20000U}}}, + {.msg = {{MSG_PSCM0_VOLVO_C1, 0, 8, .check_checksum = false, .expected_timestep = 20000U}}}, + {.msg = {{MSG_PSCM1_VOLVO_C1, 0, 8, .check_checksum = false, .expected_timestep = 20000U}}}, + {.msg = {{MSG_ACC_PEDAL_VOLVO_C1, 0, 8, .check_checksum = false, .expected_timestep = 20000U}}}, +}; + +AddrCheckStruct volvo_eucd_checks[] = { + {.msg = {{MSG_PSCM1_VOLVO_V60, 0, 8, .check_checksum = false, .expected_timestep = 20000U}}}, + {.msg = {{MSG_FSM0_VOLVO_V60, 2, 8, .check_checksum = false, .expected_timestep = 10000U}}}, + {.msg = {{MSG_ACC_PEDAL_VOLVO_V60, 0, 8, .check_checksum = false, .expected_timestep = 10000U}}}, +}; + +#define VOLVO_C1_RX_CHECKS_LEN sizeof(volvo_c1_checks) / sizeof(volvo_c1_checks[0]) +#define VOLVO_EUCD_RX_CHECKS_LEN sizeof(volvo_eucd_checks) / sizeof(volvo_eucd_checks[0]) + +addr_checks volvo_c1_rx_checks = {volvo_c1_checks, VOLVO_C1_RX_CHECKS_LEN}; +addr_checks volvo_eucd_rx_checks = {volvo_eucd_checks, VOLVO_EUCD_RX_CHECKS_LEN}; + +// Check for value in a array +/* static int val_in_arr(int val, const int arr[], const int arr_len) { + int i; + for(i = 0; i < arr_len; i++) { + if(arr[i] == val) { + return 1; + } + } + return 0; +} + */ + + +static const addr_checks* volvo_c1_init(int16_t param) { + UNUSED(param); + controls_allowed = 0; + relay_malfunction_reset(); + giraffe_forward_camera_volvo = 0; + return &volvo_c1_rx_checks; +} + +static const addr_checks* volvo_eucd_init(int16_t param) { + UNUSED(param); + controls_allowed = 0; + relay_malfunction_reset(); + giraffe_forward_camera_volvo = 0; + return &volvo_eucd_rx_checks; +} + + +static int volvo_c1_rx_hook(CANPacket_t *to_push) { + + bool valid = addr_safety_check(to_push, &volvo_c1_rx_checks, + NULL, NULL, NULL); + + if( valid ) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + // check acc status + if( (addr == MSG_FSM0_VOLVO_C1) && (bus == 2) ) { + giraffe_forward_camera_volvo = 1; + bool acc_active = (GET_BYTE(to_push, 7) & 0x04); + + // only allow lateral control when acc active + if(acc_active && !acc_active_prev_volvo) { + controls_allowed = 1; + } + if( !acc_active ) { + controls_allowed = 0; + } + acc_active_prev_volvo = acc_active; + } + + if( bus == 0 ) { + // Current steering angle + if (addr == MSG_PSCM1_VOLVO_C1) { + // 2bytes long. + int angle_meas_new = (GET_BYTE(to_push, 5) << 8) | (GET_BYTE(to_push, 6)); + // Remove offset. + angle_meas_new = angle_meas_new-32768; + + // update array of samples + update_sample(&volvo_angle_meas, angle_meas_new); + } + + // Get current speed + if (addr == MSG_SPEED_VOLVO_C1) { + // Factor 0.01 + volvo_speed = ((GET_BYTE(to_push, 3) << 8) | (GET_BYTE(to_push, 4))) * 0.01 / 3.6; + //vehicle_moving = volvo_speed > 0.; + } + + // Disengage when accelerator pedal pressed + if( addr == MSG_ACC_PEDAL_VOLVO_C1 ) { + int hbyte = (GET_BYTE(to_push, 1) & 0x03) << 8; + int acc_ped_val = hbyte + GET_BYTE(to_push, 2); + if( (acc_ped_val > 50) && (acc_ped_val_prev <= 50) && (volvo_speed > 1) ) { + controls_allowed = 0; + } + acc_ped_val_prev = acc_ped_val; + } + + // dont forward if message is on bus 0 + if( addr == MSG_FSM0_VOLVO_C1 ) { + giraffe_forward_camera_volvo = 0; + } + + // If LKA msg is on bus 0, then relay is unexpectedly closed + if( (safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == MSG_FSM1_VOLVO_C1) ) { + relay_malfunction_set(); + } + } + + } + + #ifdef DEBUG_VOLVO + bool flag = false; + if( controls_allowed != controls_allowed_prev_v ) { + puts("controls_allowed:"); puth(controls_allowed); puts(" prev:"); puth(controls_allowed_prev_v); puts("\n"); + flag = true; + } + + if( relay_malfunction != relay_malfunction_prev ) { + puts("Relay malfunction:"); puth(relay_malfunction); puts(" prev:"); puth(relay_malfunction_prev); puts("\n"); + flag = true; + } + + if( giraffe_forward_camera_volvo != giraffe_forward_camera_volvo_prev ) { + puts("Giraffe forward camera volvo:"); puth(giraffe_forward_camera_volvo); puts(" prev:"); puth(relay_malfunction_prev); puts("\n"); + //puts("VOLVO V40/V60 ACC Status msg id seen on bus 0. Don't forward!\n"); + flag = true; + } + + if( valid != valid_prev ) { + puts("Valid:"); puth(valid); puts("\n"); + flag = true; + } + if( flag ) { + puts("Loop no:"); puth(counterloop); puts("\n"); + } + + counterloop++; + + // Update old values + relay_malfunction_prev = relay_malfunction; + giraffe_forward_camera_volvo_prev = giraffe_forward_camera_volvo; + controls_allowed_prev_v = controls_allowed; + valid_prev = valid; + #endif + + return valid; +} + + +static int volvo_eucd_rx_hook(CANPacket_t *to_push) { + + bool valid = addr_safety_check(to_push, &volvo_eucd_rx_checks, + NULL, NULL, NULL); + + if( valid ) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + // check acc status + if( (addr == MSG_FSM0_VOLVO_V60) && (bus == 2) ) { + giraffe_forward_camera_volvo = 1; + int acc_status = (GET_BYTE(to_push, 2) & 0x07); + bool acc_active = (acc_status >= 6) ? true : false; + + // only allow lateral control when acc active + if( acc_active && !acc_active_prev_volvo ) { + controls_allowed = 1; + } + if( !acc_active ) { + controls_allowed = 0; + } + acc_active_prev_volvo = acc_active; + } + + // Disengage when accelerator pedal pressed + if( (addr == MSG_ACC_PEDAL_VOLVO_V60) && (bus == 0) ) { + int acc_ped_val = ((GET_BYTE(to_push, 2) & 0x03) << 8) | GET_BYTE(to_push, 3); + if( (acc_ped_val > 100) && (acc_ped_val_prev <= 100) ) { + controls_allowed = 0; + } + acc_ped_val_prev = acc_ped_val; + } + + // dont forward if message is on bus 0 + if( (addr == MSG_FSM0_VOLVO_V60) && (bus == 0) ) { + giraffe_forward_camera_volvo = 0; + } + + // If LKA msg is on bus 0, then relay is unexpectedly closed + if( (safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == MSG_FSM2_VOLVO_V60) && (bus == 0) ) { + relay_malfunction_set(); + } + } + return valid; +} + + +static int volvo_c1_tx_hook(CANPacket_t *to_send) { + + int tx = 1; + //int bus = GET_BUS(to_send); + int addr = GET_ADDR(to_send); + bool violation = 0; + + if ( !msg_allowed(to_send, VOLVO_C1_TX_MSGS, VOLVO_C1_TX_MSGS_LEN) || relay_malfunction ) { + tx = 0; + } + + if ( addr == MSG_FSM1_VOLVO_C1 ) { + int desired_angle = ((GET_BYTE(to_send, 4) & 0x3f) << 8) | (GET_BYTE(to_send, 5)); // 14 bits long + bool lka_active = (GET_BYTE(to_send, 7) & 0x3) > 0; // Steer direction bigger than 0, commanding lka to steer + + // remove offset + desired_angle = desired_angle-8192; + + if (controls_allowed && lka_active) { + // add 1 to not false trigger the violation + float delta_angle_float; + delta_angle_float = (interpolate(VOLVO_LOOKUP_ANGLE_RATE_UP, volvo_speed) * DEG_TO_CAN_VOLVO_C1) + 1.; + int delta_angle_up = (int)(delta_angle_float); + delta_angle_float = (interpolate(VOLVO_LOOKUP_ANGLE_RATE_DOWN, volvo_speed) * DEG_TO_CAN_VOLVO_C1) + 1.; + int delta_angle_down = (int)(delta_angle_float); + int highest_desired_angle = volvo_desired_angle_last + ((volvo_desired_angle_last > 0) ? delta_angle_up : delta_angle_down); + int lowest_desired_angle = volvo_desired_angle_last - ((volvo_desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); + + // max request offset from actual angle + int hi_angle_req = MIN(desired_angle + VOLVO_MAX_DELTA_OFFSET_ANGLE, VOLVO_MAX_ANGLE_REQ); + int lo_angle_req = MAX(desired_angle - VOLVO_MAX_DELTA_OFFSET_ANGLE, VOLVO_MIN_ANGLE_REQ); + + // check for violation; + violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); + violation |= max_limit_check(desired_angle, hi_angle_req, lo_angle_req); + } + volvo_desired_angle_last = desired_angle; + + // desired steer angle should be the same as steer angle measured when controls are off + // dont check when outside of measurable range. desired_angle can only be -8192->8191 (+-360). + if ((!controls_allowed) + && ((volvo_angle_meas.min - 1) >= VOLVO_MAX_ANGLE_REQ) + && ((volvo_angle_meas.max + 1) <= VOLVO_MIN_ANGLE_REQ) + && ((desired_angle < (volvo_angle_meas.min - 1)) || (desired_angle > (volvo_angle_meas.max + 1)))) { + violation = 1; + } + + // no lka_enabled bit if controls not allowed + if (!controls_allowed && lka_active) { + violation = 1; + } + } + + // acc button check, only allow cancel button to be sent + if (addr == MSG_BTNS_VOLVO_C1) { + // Violation if any button other than cancel is pressed + violation |= ((GET_BYTE(to_send, 7) & 0xef) > 0) | (GET_BYTE(to_send, 6) > 0); + } + + if (violation) { + controls_allowed = 0; + tx = 0; + } + + return tx; +} + + +static int volvo_eucd_tx_hook(CANPacket_t *to_send) { + + //int bus = GET_BUS(to_send); + //int addr = GET_ADDR(to_send); + + int tx = 1; + + if ( !msg_allowed(to_send, VOLVO_EUCD_TX_MSGS, VOLVO_EUCD_TX_MSGS_LEN) || relay_malfunction ) { + tx = 0; + } + + return tx; +} + + +static int volvo_c1_fwd_hook(int bus_num, CANPacket_t *to_fwd) { + + int bus_fwd = -1; // fallback to do not forward + int addr = GET_ADDR(to_fwd); + + if( !relay_malfunction && giraffe_forward_camera_volvo ) { + if( bus_num == 0 ){ + bool block_msg = (addr == MSG_PSCM1_VOLVO_C1); + if ( !block_msg ) { + bus_fwd = 2; // forward 0 -> 2 + } + //val_in_arr(addr, ALLOWED_MSG_C1, ALLOWED_MSG_C1_LEN); // block not relevant msgs + //bool allw_msg = val_in_arr(addr, ALLOWED_MSG_C1, ALLOWED_MSG_C1_LEN); // block not relevant msgs + //bus_fwd = allw_msg ? 2 : -1; // forward bus 0 -> 2 + } + + if( bus_num == 2 ) { + bool block_msg = (addr == MSG_FSM1_VOLVO_C1); // block if lkas msg + if( !block_msg ) { + bus_fwd = 0; // forward bus 2 -> 0 + } + } + } + return bus_fwd; +} + + +static int volvo_eucd_fwd_hook(int bus_num, CANPacket_t *to_fwd) { + + int bus_fwd = -1; // fallback to do not forward + int addr = GET_ADDR(to_fwd); + + if( !relay_malfunction && giraffe_forward_camera_volvo ) { + if( bus_num == 0 ){ + bool block_msg = (addr == MSG_PSCM1_VOLVO_V60); + //bool allw_msg = val_in_arr(addr, ALLOWED_MSG_EUCD, ALLOWED_MSG_EUCD_LEN); // block not relevant msgs + bus_fwd = block_msg ? -1 : 2; // forward bus 0 -> 2 + } + + if( bus_num == 2 ) { + bool block_msg = (addr == MSG_FSM2_VOLVO_V60); // block if lkas msg + if( !block_msg ) { + bus_fwd = 0; // forward bus 2 -> 0 + } + } + } + return bus_fwd; +} + + +const safety_hooks volvo_c1_hooks = { + .init = volvo_c1_init, + .rx = volvo_c1_rx_hook, + .tx = volvo_c1_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .fwd = volvo_c1_fwd_hook, +}; + + +const safety_hooks volvo_eucd_hooks = { + .init = volvo_eucd_init, + .rx = volvo_eucd_rx_hook, + .tx = volvo_eucd_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .fwd = volvo_eucd_fwd_hook, +}; diff --git a/selfdrive/car/volvo/__init__.py b/selfdrive/car/volvo/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/volvo/carcontroller.py b/selfdrive/car/volvo/carcontroller.py new file mode 100644 index 000000000..1c2da2df8 --- /dev/null +++ b/selfdrive/car/volvo/carcontroller.py @@ -0,0 +1,296 @@ +from common.numpy_fast import clip, interp +from selfdrive.car.volvo.values import CAR, PLATFORM, DBC, CarControllerParams as CCP +from selfdrive.car.volvo import volvocan +from opendbc.can.packer import CANPacker +from collections import deque +from common.dp_common import common_controller_ctrl + +class SteerCommand: + angle_request = 0 + steer_direction = 0 + trqlim = 0 + + +class CarController(): + def __init__(self, dbc_name, CP, VW): + # dp + self.last_blinker_on = False + self.blinker_end_frame = 0. + + # state + self.acc_enabled_prev = 0 + + # steering related + self.angle_request_prev = 0 + + # Direction change statemachine + self.UNBLOCKED = 0 + self.BLOCKED = 1 + self.BLOCK_LEN = CCP.BLOCK_LEN # Block steer direction change for x samples + + self.dir_state = 0 + self.block_steering = 0 + self.steer_direction_bf_block = 0 + self.des_steer_direction_prev = 0 + + # SteerCommand + self.SteerCommand = SteerCommand + self.trq_fifo = deque([]) + self.fault_frame = -200 + + # Diag + self.doDTCRequests = True # Turn on and off DTC requests + self.checkPN = False # Check partnumbers + self.clearDtcs = False # Set false to stop sending diagnostic requests + self.timeout = 0 # Set to 0 as init + self.diagRequest = { + "byte0": 0x03, + "byte1": 0x19, + "byte2": 0x02, + "byte3": 0x02, + } + self.flowControl = { + "byte0": 0x30, + "byte1": 0x00, + "byte2": 0x00, + "byte3": 0x00, + } + self.clearDTC = { + "byte0": 0x04, + "byte1": 0x14, + "byte2": 0xFF, + "byte3": 0xFF, + "byte4": 0xFF, + } + + # Part number + self.cnt = 0 # Init at 0 always + self.sndNxtFrame = 0 # Init at low value + self.dictKeys = ["byte"+str(x) for x in range(8)] + startdid = 0xf1a1 # Start with this DID (Data IDentifier, read UDS Spec for more info) + self.dids = [x for x in range(startdid, startdid+9)] + + # Setup detection helper. Routes commands to + # an appropriate CAN bus number. + self.CP = CP + self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) + + + def max_angle_req(self, current_steer_angle, angle_request_prev, CCP): + """ + Calculate maximum angle request delta/offset from current steering angle. + + This is just a helper function that calculates the boundary for min and max + steering angle request. It uses the parameters CCP.MAX_ACT_ANGLE_REQUEST_DIFF + and CCP.STEER_ANGLE_DELTA_REQ_DIFF. To calculate the max and min allowed delta/offset request. + + The delta request is just a rate limiter. The request angle cant change more + than CCP.STEER_ANGLE_DELTA_REQ_DIFF per loop. + + """ + + # determine max and min allowed lka angle request + # based on delta per sample + max_delta_right = angle_request_prev-CCP.STEER_ANGLE_DELTA_REQ_DIFF + max_delta_left = angle_request_prev+CCP.STEER_ANGLE_DELTA_REQ_DIFF + + # based on distance from actual steering angle + max_right = current_steer_angle-CCP.MAX_ACT_ANGLE_REQUEST_DIFF + max_left = current_steer_angle+CCP.MAX_ACT_ANGLE_REQUEST_DIFF + + return max_right, max_left, max_delta_right, max_delta_left + + def dir_change(self, steer_direction, error): + """ Filters out direction changes + + Uses a simple state machine to determine if we should + block or allow the steer_direction bits to pass thru. + + """ + + dessd = steer_direction + dzError = 0 if abs(error) < CCP.DEADZONE else error + tState = -1 + + # Update prev with desired if just enabled. + self.des_steer_direction_prev = steer_direction if not self.acc_enabled_prev else self.des_steer_direction_prev + + # Check conditions for state change + if self.dir_state == self.UNBLOCKED: + tState = self.BLOCKED if (steer_direction != self.des_steer_direction_prev and dzError != 0) else tState + elif self.dir_state == self.BLOCKED: + if (steer_direction == self.steer_direction_bf_block) or (self.block_steering <= 0) or (dzError == 0): + tState = self.UNBLOCKED + + # State transition + if tState == self.UNBLOCKED: + self.dir_state = self.UNBLOCKED + elif tState == self.BLOCKED: + self.steer_direction_bf_block = self.des_steer_direction_prev + self.block_steering = self.BLOCK_LEN + self.dir_state = self.BLOCKED + + # Run actions in state + if self.dir_state == self.UNBLOCKED: + if dzError == 0: + steer_direction = self.des_steer_direction_prev # Set old request when inside deadzone + if self.dir_state == self.BLOCKED: + self.block_steering -= 1 + steer_direction = CCP.STEER_NO + + #print("State:{} Sd:{} Sdp:{} Bs:{} Dz:{:.2f} Err:{:.2f}".format(self.dir_state, steer_direction, self.des_steer_direction_prev, self.block_steering, dzError, error)) + return steer_direction + + def update(self, enabled, CS, frame, + actuators, + visualAlert, leftLaneVisible, + rightLaneVisible, leadVisible, + leftLaneDepart, rightLaneDepart, dragonconf): + """ Controls thread """ + + # Send CAN commands. + can_sends = [] + + # run at 50hz + if (frame % 2 == 0): + fingerprint = self.CP.carFingerprint + + if enabled and CS.out.vEgo > self.CP.minSteerSpeed: + current_steer_angle = CS.out.steeringAngleDeg + self.SteerCommand.angle_request = actuators.steeringAngleDeg # Desired value from pathplanner + + # # windup slower + if self.angle_request_prev * self.SteerCommand.angle_request > 0. and abs(self.SteerCommand.angle_request) > abs(self.angle_request_prev): + angle_rate_lim = interp(CS.out.vEgo, CCP.ANGLE_DELTA_BP, CCP.ANGLE_DELTA_V) + else: + angle_rate_lim = interp(CS.out.vEgo, CCP.ANGLE_DELTA_BP, CCP.ANGLE_DELTA_VU) + + self.SteerCommand.angle_request = clip(self.SteerCommand.angle_request, self.angle_request_prev - angle_rate_lim, self.angle_request_prev + angle_rate_lim) + + # Create trqlim from angle request (before constraints) + self.SteerCommand.trqlim = 0 + if fingerprint in PLATFORM.C1: + #self.SteerCommand.trqlim = -127 if current_steer_angle > self.SteerCommand.angle_request else 127 + self.SteerCommand.steer_direction = CCP.STEER + elif fingerprint in PLATFORM.EUCD: + # MIGHT be needed for EUCD + self.SteerCommand.steer_direction = CCP.STEER_RIGHT if current_steer_angle > self.SteerCommand.angle_request else CCP.STEER_LEFT + self.SteerCommand.steer_direction = self.dir_change(self.SteerCommand.steer_direction, current_steer_angle-self.SteerCommand.angle_request) # Filter the direction change + + else: + self.SteerCommand.steer_direction = CCP.STEER_NO + self.SteerCommand.trqlim = 0 + if fingerprint in PLATFORM.C1: + self.SteerCommand.angle_request = clip(CS.out.steeringAngleDeg, -359.95, 359.90) # Cap values at max min values (Cap 2 steps from max min). Max=359.99445, Min=-360.0384 + else: + self.SteerCommand.angle_request = 0 + + + # Count no of consequtive samples of zero torque by lka. + # Try to recover, blocking steering request for 2 seconds. + if fingerprint in PLATFORM.C1: + if enabled and CS.out.vEgo > self.CP.minSteerSpeed: + self.trq_fifo.append(CS.PSCMInfo.LKATorque) + if len(self.trq_fifo) > CCP.N_ZERO_TRQ: + self.trq_fifo.popleft() + else: + self.trq_fifo.clear() + self.fault_frame = -200 + + if (self.trq_fifo.count(0) >= CCP.N_ZERO_TRQ) and (self.fault_frame == -200): + self.fault_frame = frame+100 + + if enabled and (frame < self.fault_frame): + self.SteerCommand.steer_direction = CCP.STEER_NO + + if frame > self.fault_frame+8: # Ignore steerWarning for another 8 samples. + self.fault_frame = -200 + + + # update stored values + self.acc_enabled_prev = enabled + self.angle_request_prev = self.SteerCommand.angle_request + if self.SteerCommand.steer_direction == CCP.STEER_RIGHT or self.SteerCommand.steer_direction == CCP.STEER_LEFT: # TODO: Move this inside dir_change, think it should work? + self.des_steer_direction_prev = self.SteerCommand.steer_direction # Used for dir_change function + + # Manipulate data from servo to FSM + # Avoid fault codes, that will stop LKA + can_sends.append(volvocan.manipulateServo(self.packer, self.CP.carFingerprint, CS)) + + # send can, add to list. + can_sends.append(volvocan.create_steering_control(self.packer, frame, self.CP.carFingerprint, self.SteerCommand, CS.FSMInfo)) + + + # dp + #blinker_on = CS.out.leftBlinker or CS.out.rightBlinker + #if not enabled: + # self.blinker_end_frame = 0 + #if self.last_blinker_on and not blinker_on: + # self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay + #apply_steer = common_controller_ctrl(enabled, + # dragonconf, + # blinker_on or frame < self.blinker_end_frame, + # apply_steer, CS.out.vEgo) + #self.last_blinker_on = blinker_on + + # Cancel ACC if engaged when OP is not. + if not enabled and CS.out.cruiseState.enabled: + can_sends.append(volvocan.cancelACC(self.packer, self.CP.carFingerprint, CS)) + + + # Send diagnostic requests + if(self.doDTCRequests): + if(frame % 100 == 0) and (not self.clearDtcs): + # Request diagnostic codes, 2 Hz + can_sends.append(self.packer.make_can_msg("diagFSMReq", 2, self.diagRequest)) + #can_sends.append(self.packer.make_can_msg("diagGlobalReq", 2, self.diagRequest)) + can_sends.append(self.packer.make_can_msg("diagGlobalReq", 0, self.diagRequest)) + #can_sends.append(self.packer.make_can_msg("diagPSCMReq", 0, self.diagRequest)) + #can_sends.append(self.packer.make_can_msg("diagCEMReq", 0, self.diagRequest)) + #can_sends.append(self.packer.make_can_msg("diagCVMReq", 0, self.diagRequest)) + self.timeout = frame + 5 # Set wait time + + # Handle flow control in case of many DTC + if frame > self.timeout and self.timeout > 0: # Wait fix time before sending flow control, otherwise just spamming... + self.timeout = 0 + if (CS.diag.diagFSMResp & 0x10000000): + can_sends.append(self.packer.make_can_msg("diagFSMReq", 2, self.flowControl)) + if (CS.diag.diagCEMResp & 0x10000000): + can_sends.append(self.packer.make_can_msg("diagCEMReq", 0, self.flowControl)) + if (CS.diag.diagPSCMResp & 0x10000000): + can_sends.append(self.packer.make_can_msg("diagPSCMReq", 0, self.flowControl)) + if (CS.diag.diagCVMResp & 0x10000000): + can_sends.append(self.packer.make_can_msg("diagCVMReq", 0, self.flowControl)) + + # Check part numbers + if self.checkPN and frame > 100 and frame > self.sndNxtFrame: + if self.cnt < len(self.dids): + did = [0x03, 0x22, (self.dids[self.cnt] & 0xff00)>>8, self.dids[self.cnt] & 0x00ff] # Create diagnostic command + did.extend([0]*(8-len(did))) + diagReq = dict(zip(self.dictKeys,did)) + #can_sends.append(self.packer.make_can_msg("diagGlobalReq", 2, diagReq)) + #can_sends.append(self.packer.make_can_msg("diagGlobalReq", 0, diagReq)) + can_sends.append(self.packer.make_can_msg("diagFSMReq", 2, diagReq)) + can_sends.append(self.packer.make_can_msg("diagCEMReq", 0, diagReq)) + can_sends.append(self.packer.make_can_msg("diagPSCMReq", 0, diagReq)) + can_sends.append(self.packer.make_can_msg("diagCVMReq", 0, diagReq)) + self.cnt += 1 + self.timeout = frame+5 # When to send flowControl + self.sndNxtFrame = self.timeout+5 # When to send next part number request + + elif True: # Stop when list has been looped thru. + self.checkPN = False + + # Clear DTCs in FSM on start + # TODO check for engine running before clearing dtc. + if(self.clearDtcs and (frame > 0) and (frame % 500 == 0)): + can_sends.append(self.packer.make_can_msg("diagGlobalReq", 0, self.clearDTC)) + can_sends.append(self.packer.make_can_msg("diagFSMReq", 2, self.clearDTC)) + #can_sends.append(self.packer.make_can_msg("diagPSCMReq", 0, self.clearDTC)) + #can_sends.append(self.packer.make_can_msg("diagCEMReq", 0, self.clearDTC)) + self.clearDtcs = False + + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = self.SteerCommand.angle_request + + return new_actuators, can_sends diff --git a/selfdrive/car/volvo/carstate.py b/selfdrive/car/volvo/carstate.py new file mode 100644 index 000000000..0807dd5e2 --- /dev/null +++ b/selfdrive/car/volvo/carstate.py @@ -0,0 +1,407 @@ +from cereal import car +from common.kalman.simple_kalman import KF1D +from selfdrive.config import Conversions as CV +from selfdrive.car.interfaces import CarStateBase +from opendbc.can.parser import CANParser +from opendbc.can.can_define import CANDefine +from selfdrive.car.volvo.values import CAR, PLATFORM, DBC, BUTTON_STATES, CarControllerParams as CCP +from collections import deque + +class diagInfo(): + def __init__(self): + self.diagFSMResp = 0 + self.diagCEMResp = 0 + self.diagPSCMResp = 0 + self.diagCVMResp = 0 + +class PSCMInfo(): + def __init__(self): + # Common + self.byte0 = 0 + self.byte4 = 0 + self.byte7 = 0 + self.LKAActive = 0 + self.LKATorque = 0 + self.SteeringAngleServo = 0 + + # C1 + self.byte3 = 0 + + # EUCD + self.SteeringWheelRateOfChange = 0 + +class FSMInfo(): + def __init__(self): + # Common + self.TrqLim = 0 + self.LKAAngleReq = 0 + self.Checksum = 0 + self.LKASteerDirection = 0 + + # C1 + self.SET_X_E3 = 0 + self.SET_X_B4 = 0 + self.SET_X_08 = 0 + self.SET_X_02 = 0 + self.SET_X_25 = 0 + + # EUCD + self.SET_X_22 = 0 + self.SET_X_02 = 0 + self.SET_X_10 = 0 + self.SET_X_A4 = 0 + +class CCButtons(): + def __init__(self): + # Common + self.ACCOnOffBtn = 0 + self.ACCSetBtn = 0 + self.ACCResumeBtn = 0 + self.ACCMinusBtn = 0 + self.TimeGapIncreaseBtn = 0 + self.TimeGapDecreaseBtn = 0 + + # C1 + self.ACCStopBtn = 0 + self.byte0 = 0 + self.byte1 = 0 + self.byte2 = 0 + self.byte3 = 0 + self.byte4 = 0 + self.byte5 = 0 + self.byte6 = 0 + self.B7b0 = 0 + self.B7b1 = 0 + self.B7b3 = 0 + self.B7b6 = 0 + + # EUCD + # TODO + # Inv = Inverted state of button, set to passive as default. + self.ACCOnOffBtnInv = 1 + self.ACCSetBtnInv = 1 + self.ACCResumeBtnInv = 1 + self.ACCMinusBtnInv = 1 + self.TimeGapIncreaseBtnInv = 1 + self.TimeGapDecreaseBtnInv = 1 + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + self.diag = diagInfo() + self.PSCMInfo = PSCMInfo() + self.FSMInfo = FSMInfo() + self.CCBtns = CCButtons() + + self.trq_fifo = deque([]) + + self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) + self.buttonStates = BUTTON_STATES.copy() + + def update(self, cp, cp_cam): + ret = car.CarState.new_message() + + # Speeds + ret.vEgoRaw = cp.vl["VehicleSpeed1"]['VehicleSpeed'] * CV.KPH_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.1 + + # Steering + ret.steeringAngleDeg = cp.vl["PSCM1"]['SteeringAngleServo'] + ret.steeringTorque = cp.vl["PSCM1"]['LKATorque'] # Needed? No signal to check against yet + ret.steeringPressed = bool(cp.vl["CCButtons"]['ACCSetBtn'] or \ + cp.vl["CCButtons"]['ACCMinusBtn'] or \ + cp.vl["CCButtons"]['ACCResumeBtn']) + + # Update gas and brake + if self.CP.carFingerprint in PLATFORM.C1: + ret.gas = cp.vl["PedalandBrake"]['AccPedal'] / 102.3 + ret.gasPressed = ret.gas > 0.05 + elif self.CP.carFingerprint in PLATFORM.EUCD: + ret.gas = cp.vl["AccPedal"]['AccPedal'] / 102.3 + ret.gasPressed = ret.gas > 0.1 + ret.brakePressed = False + + # Update gear position + if self.CP.carFingerprint in PLATFORM.C1: + self.shifter_values = self.can_define.dv["TCM0"]['GearShifter'] + can_gear = int(cp.vl["TCM0"]["GearShifter"]) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + elif self.CP.carFingerprint in PLATFORM.EUCD: + ret.gearShifter = self.parse_gear_shifter('D') # TODO: Gear EUCD + + # Belt and doors + ret.doorOpen = False + + # Check seatbelts + ret.seatbeltUnlatched = False # No signal yet. + + # ACC status from camera + if self.CP.carFingerprint in PLATFORM.C1: + ret.cruiseState.available = bool(cp_cam.vl["FSM0"]['ACCStatusOnOff']) + ret.cruiseState.enabled = bool(cp_cam.vl["FSM0"]['ACCStatusActive']) + # dp + ret.cruiseActualEnabled = ret.cruiseState.enabled + ret.cruiseState.speed = cp.vl["ACC"]['SpeedTargetACC'] * CV.KPH_TO_MS + + elif self.CP.carFingerprint in PLATFORM.EUCD: + accStatus = cp_cam.vl["FSM0"]['ACCStatus'] + + if accStatus == 2: + # Acc in ready mode + ret.cruiseState.available = True + ret.cruiseState.enabled = False + elif accStatus >= 6: + # Acc active + ret.cruiseState.available = True + ret.cruiseState.enabled = True + else: + # Acc in a unkown mode + ret.cruiseState.available = False + ret.cruiseState.enabled = False + + # Button and blinkers. + self.buttonStates['altButton1'] = bool(cp.vl["CCButtons"]['ACCOnOffBtn']) + self.buttonStates['accelCruise'] = bool(cp.vl["CCButtons"]['ACCSetBtn']) + self.buttonStates['decelCruise'] = bool(cp.vl["CCButtons"]['ACCMinusBtn']) + self.buttonStates['setCruise'] = bool(cp.vl["CCButtons"]['ACCSetBtn']) + self.buttonStates['resumeCruise'] = bool(cp.vl["CCButtons"]['ACCResumeBtn']) + #self.buttonStates['cancel'] = bool(cp.vl["CCButtons"]['ACCStopBtn']) No cancel button in V60. + self.buttonStates['gapAdjustCruise'] = bool(cp.vl["CCButtons"]['TimeGapIncreaseBtn']) or bool(cp.vl["CCButtons"]['TimeGapDecreaseBtn']) + ret.leftBlinker = cp.vl["MiscCarInfo"]['TurnSignal'] == 1 + ret.rightBlinker = cp.vl["MiscCarInfo"]['TurnSignal'] == 3 + + # Diagnostics, for debugging + self.diag.diagFSMResp = int(cp_cam.vl["diagFSMResp"]["byte03"]) + self.diag.diagCEMResp = int(cp.vl["diagCEMResp"]["byte03"]) + self.diag.diagCVMResp = int(cp.vl["diagCVMResp"]["byte03"]) + self.diag.diagPSCMResp = int(cp.vl["diagPSCMResp"]["byte03"]) + + # ACC Buttons + if self.CP.carFingerprint in PLATFORM.C1: + self.CCBtns.ACCStopBtn = bool(cp.vl["CCButtons"]['ACCStopBtn']) + + # PSCMInfo + # Common + self.PSCMInfo.byte0 = int(cp.vl['PSCM1']['byte0']) + self.PSCMInfo.byte4 = int(cp.vl['PSCM1']['byte4']) + self.PSCMInfo.byte7 = int(cp.vl['PSCM1']['byte7']) + self.PSCMInfo.LKATorque = int(cp.vl['PSCM1']['LKATorque']) + self.PSCMInfo.LKAActive = int(cp.vl['PSCM1']['LKAActive']) + self.PSCMInfo.SteeringAngleServo = float(cp.vl['PSCM1']['SteeringAngleServo']) + + # Platform specific + if self.CP.carFingerprint in PLATFORM.C1: + self.PSCMInfo.byte3 = int(cp.vl['PSCM1']['byte3']) + elif self.CP.carFingerprint in PLATFORM.EUCD: + self.PSCMInfo.SteeringWheelRateOfChange = float(cp.vl['PSCM1']['SteeringWheelRateOfChange']) + + # FSMInfo + # Common both platforms + + if self.CP.carFingerprint in PLATFORM.C1: + # TODO Why use these? In future shold be ok to delete. + self.FSMInfo.TrqLim = int(cp_cam.vl['FSM1']['TrqLim']) + self.FSMInfo.LKAAngleReq = float(cp_cam.vl['FSM1']['LKAAngleReq']) + self.FSMInfo.Checksum = int(cp_cam.vl['FSM1']['Checksum']) + self.FSMInfo.LKASteerDirection = int(cp_cam.vl['FSM1']['LKASteerDirection']) + self.FSMInfo.SET_X_E3 = int(cp_cam.vl['FSM1']['SET_X_E3']) + self.FSMInfo.SET_X_B4 = int(cp_cam.vl['FSM1']['SET_X_B4']) + self.FSMInfo.SET_X_08 = int(cp_cam.vl['FSM1']['SET_X_08']) + self.FSMInfo.SET_X_02 = int(cp_cam.vl['FSM1']['SET_X_02']) + self.FSMInfo.SET_X_25 = int(cp_cam.vl['FSM1']['SET_X_25']) + + elif self.CP.carFingerprint in PLATFORM.EUCD: + self.FSMInfo.TrqLim = int(cp_cam.vl['FSM2']['TrqLim']) + self.FSMInfo.LKAAngleReq = float(cp_cam.vl['FSM2']['LKAAngleReq']) + self.FSMInfo.Checksum = int(cp_cam.vl['FSM2']['Checksum']) + self.FSMInfo.LKASteerDirection = int(cp_cam.vl['FSM2']['LKASteerDirection']) + # Must use until understand the messaging scheme more... + self.FSMInfo.SET_X_22 = int(cp_cam.vl['FSM2']['SET_X_22']) + self.FSMInfo.SET_X_02 = int(cp_cam.vl['FSM2']['SET_X_02']) + self.FSMInfo.SET_X_A4 = int(cp_cam.vl['FSM2']['SET_X_A4']) + self.FSMInfo.SET_X_10 = int(cp_cam.vl['FSM2']['SET_X_10']) + + # Check if servo stops responding when acc is active. + # If N_ZERO_TRQ 0 torque samples in a row is detected, + # set steerUnavailable. Same logic in carcontroller to + # decide when to start to recover steering. + # TODO: Add EUCD + if self.CP.carFingerprint in PLATFORM.C1: + if ret.cruiseState.enabled and ret.vEgo > self.CP.minSteerSpeed: + self.trq_fifo.append(self.PSCMInfo.LKATorque) + ret.steerWarning = True if (self.trq_fifo.count(0) >= CCP.N_ZERO_TRQ*2) else False # *2, runs at 100hz + if len(self.trq_fifo) > CCP.N_ZERO_TRQ*2: # vs 50hz in CarController + self.trq_fifo.popleft() + else: + self.trq_fifo.clear() + ret.steerWarning = False + + # dp - brake lights + ret.brakeLights = ret.brakePressed + + return ret + + @staticmethod + def get_can_parser(CP): + # ptcan on bus 0 + # this function generates lists for signal, messages and initial values + + # Common signals for both platforms + signals = [ + # sig_name, sig_address, default + ("VehicleSpeed", "VehicleSpeed1", 0), + ("TurnSignal", "MiscCarInfo", 0), + ("ACCOnOffBtn", "CCButtons", 0), + ("ACCResumeBtn", "CCButtons", 0), + ("ACCSetBtn", "CCButtons", 0), + ("ACCMinusBtn", "CCButtons", 0), + ("TimeGapIncreaseBtn", "CCButtons", 0), + ("TimeGapDecreaseBtn", "CCButtons", 0), + + # Common PSCM signals + ("SteeringAngleServo", "PSCM1", 0), + ("LKATorque", "PSCM1", 0), + ("LKAActive", "PSCM1", 0), + ("byte0", "PSCM1", 0), + ("byte4", "PSCM1", 0), + ("byte7", "PSCM1", 0), + + # diagnostic + ("byte03", "diagCEMResp", 0), + ("byte47", "diagCEMResp", 0), + ("byte03", "diagPSCMResp", 0), + ("byte47", "diagPSCMResp", 0), + ("byte03", "diagCVMResp", 0), + ("byte47", "diagCVMResp", 0), + ] + + checks = [ + # sig_address, frequency + ("CCButtons", 100), + ("PSCM1", 50), + ("VehicleSpeed1", 50), + ("MiscCarInfo", 25), + ("diagCEMResp", 0), + ("diagPSCMResp", 0), + ("diagCVMResp", 0), + + ] + + # Car specific signals + if CP.carFingerprint in PLATFORM.C1: + signals.append(("SpeedTargetACC", "ACC", 0)) + signals.append(("BrakePedalActive2", "PedalandBrake", 0)) + signals.append(("AccPedal", "PedalandBrake", 0)) + signals.append(("BrakePress0", "BrakeMessages", 0)) + signals.append(("BrakePress1", "BrakeMessages", 0)) + signals.append(("BrakeStatus", "BrakeMessages", 0)) + signals.append(("GearShifter", "TCM0", 0)) + + # Servo + signals.append(("byte3", "PSCM1", 0)) + + # Buttons + signals.append(('ACCStopBtn', "CCButtons", 0)) + + # Checks + checks.append(("BrakeMessages", 50)) + checks.append(("ACC", 17)) + checks.append(("PedalandBrake", 100)) + checks.append(("TCM0", 10)) + + if CP.carFingerprint in PLATFORM.EUCD: + # Gas / Brake + signals.append(("AccPedal", "AccPedal", 0)) + signals.append(("BrakePedal", "BrakePedal", 0)) + + # Servo + signals.append(("SteeringWheelRateOfChange", "PSCM1", 0)) + + # Buttons + # Inv = Inverted state, init value set to passive + signals.append(("ACCOnOffBtnInv", "CCButtons", 1)) + signals.append(("ACCResumeBtnInv", "CCButtons", 1)) + signals.append(("ACCSetBtnInv", "CCButtons", 1)) + signals.append(("ACCMinusBtnInv", "CCButtons", 1)) + signals.append(("TimeGapDecreaseBtnInv", "CCButtons", 1)) + signals.append(("TimeGapIncreaseBtnInv", "CCButtons", 1)) + + # Checks + checks.append(("AccPedal", 100)) + checks.append(("BrakePedal", 50)) + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + + @staticmethod + def get_adas_can_parser(CP): + # radar on bus 1, not decoded yet + # this function generates lists for signal, messages and initial values + signals = [ + # sig_name, sig_address, default + ] + + checks = [ + # sig_address, frequency + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1) + + @staticmethod + def get_cam_can_parser(CP): + # camera on bus 2 + # Common signals + signals = [ + # sig_name, sig_address, default + ("byte03", "diagFSMResp", 0), + ("byte47", "diagFSMResp", 0), + + ] + # Common checks + checks = [ + # sig_address, frequency + ("diagFSMResp", 0), + ] + + # Car specific + if CP.carFingerprint in PLATFORM.C1: + # LKA Request + signals.append(("TrqLim", "FSM1", 0x80)) + signals.append(("LKAAngleReq", "FSM1", 0x2000)) + signals.append(("Checksum", "FSM1", 0x5f)) + signals.append(("LKASteerDirection", "FSM1", 0x00)) + signals.append(("SET_X_E3", "FSM1", 0xE3)) + signals.append(("SET_X_B4", "FSM1", 0xB4)) + signals.append(("SET_X_08", "FSM1", 0x08)) + signals.append(("SET_X_02", "FSM1", 0x02)) + signals.append(("SET_X_25", "FSM1", 0x25)) + + # ACC Status + signals.append(("ACCStatusOnOff", "FSM0", 0x00)) + signals.append(("ACCStatusActive", "FSM0", 0x00)) + + # Checks + checks.append(('FSM0', 100)) + checks.append(('FSM1', 50)) + + # TODO add checks and signals nescessary + elif CP.carFingerprint in PLATFORM.EUCD: + # ACC Status + signals.append(("ACCStatus", "FSM0", 0)) + + # LKA Request + signals.append(("TrqLim", "FSM2", 0x80)) + signals.append(("LKAAngleReq", "FSM2", 0x2000)) + signals.append(("Checksum", "FSM2", 0x5f)) + signals.append(("LKASteerDirection", "FSM2", 0x00)) + signals.append(("SET_X_22", "FSM2", 0x00)) + signals.append(("SET_X_02", "FSM2", 0x00)) + signals.append(("SET_X_10", "FSM2", 0x00)) + signals.append(("SET_X_A4", "FSM2", 0x00)) + + # Checks + checks.append(('FSM0', 100)) + checks.append(('FSM2', 50)) + + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/volvo/interface.py b/selfdrive/car/volvo/interface.py new file mode 100644 index 000000000..25fd830a7 --- /dev/null +++ b/selfdrive/car/volvo/interface.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +from cereal import car +from selfdrive.config import Conversions as CV +from selfdrive.car.volvo.values import CAR, PLATFORM, BUTTON_STATES +from selfdrive.car import STD_CARGO_KG, get_safety_config, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint +from selfdrive.car.interfaces import CarInterfaceBase +from common.dp_common import common_interface_atl, common_interface_get_params_lqr + +EventName = car.CarEvent.EventName + +class CarInterface(CarInterfaceBase): + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) + + # Create variables + self.cruiseState_enabled_prev = False + self.buttonStatesPrev = BUTTON_STATES.copy() + + @staticmethod + def compute_gb(accel, speed): + return float(accel) / 4.0 + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): # pylint: disable=dangerous-default-value + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + + # Volvo port is a community feature, since we don't own one to test + #ret.communityFeature = True + + if candidate in PLATFORM.C1: + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volvoC1)] + + if candidate == CAR.V40: + # Technical specifications + ret.mass = 1610 + STD_CARGO_KG + ret.wheelbase = 2.647 + ret.centerToFront = ret.wheelbase * 0.44 + ret.steerRatio = 14.7 + + elif candidate in PLATFORM.EUCD: + ret.dashcamOnly = True + #ret.safetyModel = car.CarParams.SafetyModel.volvoEUCD + + if candidate == CAR.V60: + ret.mass = 1750 + STD_CARGO_KG # All data found at https://www.media.volvocars.com/global/en-gb/models/old-v60/2014/specifications + ret.wheelbase = 2.776 + ret.centerToFront = ret.wheelbase * 0.44 + ret.steerRatio = 15 + + # Common parameters + ret.carName = "volvo" + ret.radarOffCan = True # No radar objects on can + ret.lateralTuning.init('pid') + # Steering settings - tuning parameters for lateral control. + #ret.steerLimitAlert = True # Do this do anything? + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.minSteerSpeed = 1. * CV.KPH_TO_MS + ret.steerRateCost = 1. # Used in pathplanner for punishing? Steering derivative? + ret.steerActuatorDelay = 0.2 # Actuator delay from input to output. + + # No PID control used. Set to a value, otherwise pid loop crashes. + #ret.steerMaxBP = [0.] # m/s + #ret.steerMaxV = [1.] + ret.lateralTuning.pid.kpBP = [0.] + ret.lateralTuning.pid.kiBP = [0.] + # Tuning factors + ret.lateralTuning.pid.kf = 0.0 + ret.lateralTuning.pid.kpV = [0.0] + ret.lateralTuning.pid.kiV = [0.0] + + # Assuming all is automatic + ret.transmissionType = car.CarParams.TransmissionType.automatic + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + # dp + ret = common_interface_get_params_lqr(ret) + + return ret + + # returns a car.CarState + def update(self, c, can_strings, dragonconf): + #canMonoTimes = [] + buttonEvents = [] + + # Process the most recent CAN message traffic, and check for validity + self.cp.update_strings(can_strings) + self.cp_cam.update_strings(can_strings) + + ret = self.CS.update(self.cp, self.cp_cam) + # dp + self.dragonconf = dragonconf + ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl) + ret.canValid = self.cp.can_valid and self.cp_cam.can_valid + + # Check for and process state-change events (button press or release) from + # the turn stalk switch or ACC steering wheel/control stalk buttons. + for button in self.CS.buttonStates: + if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: + be = car.CarState.ButtonEvent.new_message() + be.type = button + be.pressed = self.CS.buttonStates[button] + buttonEvents.append(be) + + # Engagement and longitudinal control using stock ACC. Make sure OP is + # disengaged if stock ACC is disengaged. + #if not ret.cruiseState.enabled: + # events.add(EventName.) + # Attempt OP engagement only on rising edge of stock ACC engagement. + #elif not self.cruiseState_enabled_prev: + # events.add(EventName.) + + ret.events = self.create_common_events(ret).to_msg() + ret.buttonEvents = buttonEvents + #ret.canMonoTimes = canMonoTimes + + # update previous values + self.gas_pressed_prev = ret.gasPressed + self.cruiseState_enabled_prev = ret.cruiseState.enabled + self.buttonStatesPrev = self.CS.buttonStates.copy() + + # cast to reader so it can't be modified + self.CS.out = ret.as_reader() + return self.CS.out + + def apply(self, c): + can_sends = self.CC.update(c.enabled, self.CS, self.frame, + c.actuators, + c.hudControl.visualAlert, c.hudControl.leftLaneVisible, + c.hudControl.rightLaneVisible, c.hudControl.leadVisible, + c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart, self.dragonconf) + self.frame += 1 + return can_sends diff --git a/selfdrive/car/volvo/radar_interface.py b/selfdrive/car/volvo/radar_interface.py new file mode 100644 index 000000000..b2f765113 --- /dev/null +++ b/selfdrive/car/volvo/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/volvo/values.py b/selfdrive/car/volvo/values.py new file mode 100644 index 000000000..fdc7456e3 --- /dev/null +++ b/selfdrive/car/volvo/values.py @@ -0,0 +1,153 @@ +from selfdrive.car import dbc_dict +from cereal import car +Ecu = car.CarParams.Ecu + +""" +Volvo Electronic Control Units abbreviations and network topology +Platforms C1/EUCD + +Three main CAN network buses + 1. Powertrain + 2. Chassis (also called MS* CAN) *MS=Medium Speed + 3. Extended +Only mentioning control units of interest on the network buses. + +Powertrain CAN + BCM - Brake Control Module + CEM - Central Electronic Module + CVM - Closing Velocity Module (low speed auto emergency braking <30kph) + FSM - Forward Sensing Module (camera mounted in windscreen) + PPM - Pedestrian Protection Module (controls pedestrian airbag under the engine hood) + PSCM - Power Steering Control Module (EPS - Electronic Power Steering) + SAS - Steering Angle Sensor Module + SRS - Supplemental Restraint System Module (seatbelts, airbags...) + TCM - Transmission Control Module + +Chassis CAN + CEM - Central Electronic Module + DIM - Driver Information Module (the instrument cluster with odo and speedometer, relayed thru CEM) + PAM - Parking Assistance Module (automatic parking, relayed thru CEM) + +Extended CAN + CEM - Central Electronic Module + SODL - Side Object Detection Left (relayed thru CEM) + SODR - Side Object Detection Right (relayed thru CEM) +""" + +class CarControllerParams(): + # constants, collected from v40 dbc lka_direction. + # Normally the car uses STEER_RIGHT and STEER_LEFT. + # However it's possible to use STEER in C1MCA. + # Servo then accepts steering in both directions. + STEER_NO = 0 + STEER_RIGHT = 1 + STEER_LEFT = 2 + STEER = 3 + + # maximum degress offset/rate of change on request from current/last steering angle + # Not used, old code + #MAX_ACT_ANGLE_REQUEST_DIFF = 3 # A bigger angle difference will trigger disengage. + #STEER_ANGLE_DELTA_REQ_DIFF = 0.25 # Not used for C1MCA + + # Limits + ANGLE_DELTA_BP = [0., 8.33, 13.89, 19.44, 25., 30.55, 36.1] # 0, 30, 50, 70, 90, 110, 130 km/h + ANGLE_DELTA_V = [2., 1.2, .25, .20, .15, .10, .10] # windup limit + ANGLE_DELTA_VU = [2., 1.2, .25, .20, .15, .10, .10] # unwind limit + + # number of 0 torque samples in a row before trying to restore steering. + # Got one false trigger on motorway with 10. + # Increase to 12 is probably a good tradeoff between false triggers + # and detecting fault. + N_ZERO_TRQ = 12 + + # EUCD + # When changing steer direction steering request need to be blocked. + # Otherwise servo wont "listen" to the request. + # This calibration sets the number of samples to block steering request. + BLOCK_LEN = 8 + # When close to desired steering angle, don't change steer direction inside deadzone. + # Since we need to release control of the steering wheel for a brief moment, steering wheel will + # unwind by itself. + DEADZONE = 0.1 + + +BUTTON_STATES = { + "altButton1": False, # On/Off button + #"cancel": False, Not present in V60 + "setCruise": False, + "resumeCruise": False, + "accelCruise": False, + "decelCruise": False, + "gapAdjustCruise": False, +} + + +class CAR: + V40 = "VOLVO V40 2017" + V60 = "VOLVO V60 2015" + +class PLATFORM: + C1 = [CAR.V40] + EUCD = [CAR.V60] + +ECU_ADDRESS = { + CAR.V40: {"BCM": 0x760, "ECM": 0x7E0, "DIM": 0x720, "CEM": 0x726, "FSM": 0x764, "PSCM": 0x730, "TCM": 0x7E1, "CVM": 0x793}, + } + +# TODO: Find good DID for identifying SW version +# 0xf1a1, 0xf1a3 not on CVM. +# Possible options 0xf1a2, 0xf1a4, 0xf1a5 +# Decided to use DID 0xf1a2 as a start. +# +# Response is 27 bytes. But panda only has 24 bytes in response. +# Probably some timeout or max byte limit. +# +# Create byte string (pad with 00 until length=24) +# s='32233863 AA' +# b = bytes([ord(s[a]) for a in range(len(s))]) + b'\x00' * 13 +# +FW_VERSIONS = { + CAR.V40: { + (Ecu.unknown, ECU_ADDRESS[CAR.V40]["CEM"], None): [b'31453061 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a2 + #(Ecu.unknown, ECU_ADDRESS[CAR.V40]["CEM"], None): [b'31453132 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a4 + #(Ecu.unknown, ECU_ADDRESS[CAR.V40]["CEM"], None): [b'32233863 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a5 + #(Ecu.eps, ECU_ADDRESS[CAR.V40]["PSCM"], None): [b'31288595 AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a2 + (Ecu.eps, ECU_ADDRESS[CAR.V40]["PSCM"], None): [b'31288595 AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a2 + #(Ecu.eps, ECU_ADDRESS[CAR.V40]["PSCM"], None): [b'31678017\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a4 + #(Ecu.eps, ECU_ADDRESS[CAR.V40]["PSCM"], None): [b'31681147 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a5 + (Ecu.fwdCamera, ECU_ADDRESS[CAR.V40]["FSM"], None): [b'31400454 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a2 + #(Ecu.fwdCamera, ECU_ADDRESS[CAR.V40]["FSM"], None): [b'31660982 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a4 + #(Ecu.fwdCamera, ECU_ADDRESS[CAR.V40]["FSM"], None): [b'31660983 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a5 + #(Ecu.cvm, ECU_ADDRESS[CAR.V40]["CVM"], None): [b'31360093 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a2 Could be used in future. + #(Ecu.cvm, ECU_ADDRESS[CAR.V40]["CVM"], None): [b'31360888 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a4 Could be used in future. + #(Ecu.cvm, ECU_ADDRESS[CAR.V40]["CVM"], None): [b'31360340 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a5 Could be used in future. + } +} + +# Result from black panda +#[, +# , +# , +# ] + + +FINGERPRINTS = { + CAR.V40: [ + # V40 2017 + {8: 8, 16: 8, 48: 8, 64: 8, 85: 8, 101: 8, 112: 8, 114: 8, 117: 8, 128: 8, 176: 8, 192: 8, 208: 8, 224: 8, 240: 8, 245: 8, 256: 8, 272: 8, 288: 8, 291: 8, 293: 8, 304: 8, 325: 8, 336: 8, 352: 8, 424: 8, 432: 8, 437: 8, 464: 8, 472: 8, 480: 8, 528: 8, 608: 8, 624: 8, 640: 8, 648: 8, 652: 8, 656: 8, 657: 8, 681: 8, 693: 8, 704: 8, 707: 8, 709: 8, 816: 8, 832: 8, 848: 8, 853: 8, 864: 8, 880: 8, 912: 8, 928: 8, 943: 8, 944: 8, 968: 8, 970: 8, 976: 8, 992: 8, 997: 8, 1024: 8, 1029: 8, 1061: 8, 1072: 8, 1409: 8}, + # V40 2015 + {8: 8, 16: 8, 64: 8, 85: 8, 101: 8, 112: 8, 114: 8, 117: 8, 128: 8, 176: 8, 192: 8, 224: 8, 240: 8, 245: 8, 256: 8, 272: 8, 288: 8, 291: 8, 293: 8, 304: 8, 325: 8, 336: 8, 424: 8, 432: 8, 437: 8, 464: 8, 472: 8, 480: 8, 528: 8, 608: 8, 648: 8, 652: 8, 656: 8, 657: 8, 681: 8, 693: 8, 704: 8, 707: 8, 709: 8, 816: 8, 832: 8, 864: 8, 880: 8, 912: 8, 928: 8, 943: 8, 944: 8, 968: 8, 970: 8, 976: 8, 992: 8, 997: 8, 1024: 8, 1029: 8, 1061: 8, 1072: 8, 1409: 8}, + # V40 2014 + {8: 8, 16: 8, 64: 8, 85: 8, 101: 8, 112: 8, 114: 8, 117: 8, 128: 8, 176: 8, 192: 8, 224: 8, 240: 8, 245: 8, 256: 8, 272: 8, 288: 8, 291: 8, 293: 8, 304: 8, 325: 8, 336: 8, 424: 8, 432: 8, 437: 8, 464: 8, 472: 8, 480: 8, 528: 8, 608: 8, 648: 8, 652: 8, 657: 8, 681: 8, 693: 8, 704: 8, 707: 8, 709: 8, 816: 8, 864: 8, 880: 8, 912: 8, 928: 8, 943: 8, 944: 8, 968: 8, 970: 8, 976: 8, 992: 8, 997: 8, 1024: 8, 1029: 8, 1072: 8, 1409: 8}, +], + CAR.V60: [ + {0: 8, 16: 8, 32: 8, 81: 8, 99: 8, 104: 8, 112: 8, 144: 8, 277: 8, 295: 8, 298: 8, 307: 8, 320: 8, 328: 8, 336: 8, 343: 8, 352: 8, 359: 8, 384: 8, 465: 8, 511: 8, 522: 8, 544: 8, 565: 8, 582: 8, 608: 8, 609: 8, 610: 8, 612: 8, 613: 8, 624: 8, 626: 8, 635: 8, 648: 8, 665: 8, 673: 8, 704: 8, 706: 8, 708: 8, 750: 8, 751: 8, 778: 8, 788: 8, 794: 8, 797: 8, 802: 8, 803: 8, 805: 8, 807: 8, 819: 8, 820: 8, 821: 8, 913: 8, 923: 8, 978: 8, 979: 8, 1006: 8, 1021: 8, 1024: 8, 1029: 8, 1039: 8, 1042: 8, 1045: 8, 1137: 8, 1141: 8, 1152: 8, 1174: 8, 1187: 8, 1198: 8, 1214: 8, 1217: 8, 1226: 8, 1240: 8, 1409: 8}, + ], +} + + +DBC = { + # dbc_dict( powertrain_dbc, radar_dbc ) + CAR.V40: dbc_dict('volvo_v40_2017_pt', None), + CAR.V60: dbc_dict('volvo_v60_2015_pt', None), +} diff --git a/selfdrive/car/volvo/volvocan.py b/selfdrive/car/volvo/volvocan.py new file mode 100644 index 000000000..0294927d6 --- /dev/null +++ b/selfdrive/car/volvo/volvocan.py @@ -0,0 +1,109 @@ +from selfdrive.car.volvo.values import PLATFORM + +def cancelACC(packer, car_fingerprint, CS): + # Send cancel button to disengage ACC + # TODO add support for EUCD + msg = {} + + if car_fingerprint in PLATFORM.C1: + msg["ACCStopBtn"] = 1 + + elif car_fingerprint in PLATFORM.EUCD: + msg["ACCOnOffBtn"] = 1 + msg["ACCOnOffBtnInv"] = 0 + + return packer.make_can_msg("CCButtons", 0, msg) + + +def manipulateServo(packer, car_fingerprint, CS): + # Manipulate data from servo to FSM + # Set LKATorque and LKAActive to zero otherwise LKA will be disabled. (Check dbc) + msg = { + "LKATorque" : 0, + "SteeringAngleServo" : CS.PSCMInfo.SteeringAngleServo, + "byte0" : CS.PSCMInfo.byte0, + "byte4" : CS.PSCMInfo.byte4, + "byte7" : CS.PSCMInfo.byte7, + } + + if car_fingerprint in PLATFORM.C1: + msg["LKAActive"] = CS.PSCMInfo.LKAActive & 0xFD + msg["byte3"] = CS.PSCMInfo.byte3 + elif car_fingerprint in PLATFORM.EUCD: + msg["LKAActive"] = CS.PSCMInfo.LKAActive & 0xF5 # Filter out bit 1 and 3 + msg["SteeringWheelRateOfChange"] = CS.PSCMInfo.SteeringWheelRateOfChange + + return packer.make_can_msg("PSCM1", 2, msg) + + +def create_chksum(dat, car_fingerprint): + # Input: dat byte array, and fingerprint + # Steering direction = 0 -> 3 + # TrqLim = 0 -> 255 + # Steering angle request = -360 -> 360 + + # Extract LKAAngleRequest, LKADirection and Unknown + if car_fingerprint in PLATFORM.C1: + steer_angle_request = ((dat[4] & 0x3F) << 8) + dat[5] + steering_direction_request = dat[7] & 0x03 + trqlim = dat[3] + elif car_fingerprint in PLATFORM.EUCD: + steer_angle_request = ((dat[3] & 0x3F) << 8) + dat[4] + steering_direction_request = dat[5] & 0x03 + trqlim = dat[2] + + # Sum of all bytes, carry ignored. + s = (trqlim + steering_direction_request + steer_angle_request + (steer_angle_request >> 8)) & 0xFF + # Checksum is inverted sum of all bytes + return s ^ 0xFF + + +def create_steering_control(packer, frame, car_fingerprint, SteerCommand, FSMInfo): + + # Set common parameters + values = { + "LKAAngleReq": SteerCommand.angle_request, + "LKASteerDirection": SteerCommand.steer_direction, + "TrqLim": SteerCommand.trqlim, + } + + # Set car specific parameters + if car_fingerprint in PLATFORM.C1: + values_static = { + "SET_X_E3": 0xE3, + "SET_X_B4": 0xB4, + "SET_X_08": 0x08, + "SET_X_02": 0x02, + "SET_X_25": 0x25, + } + elif car_fingerprint in PLATFORM.EUCD: + values_static = { + "SET_X_22": 0x25, # Test these values: 0x24, 0x22 + "SET_X_02": 0, # Test 0x00, 0x02 + "SET_X_10": 0x10, # Test 0x10, 0x1c, 0x18, 0x00 + "SET_X_A4": 0xa7, # Test 0xa4, 0xa6, 0xa5, 0xe5, 0xe7 + #"SET_X_22": FSMInfo.SET_X_22, + #"SET_X_02": FSMInfo.SET_X_02, + #"SET_X_10": FSMInfo.SET_X_10, + #"SET_X_A4": FSMInfo.SET_X_A4, + } + # Which numbers stops lka? X_22? X_02? X_10? X_A4? + # From working test to change one field at a time. When does it stop to work? + # Do any of the changes make the CCP.STEER command work? + + # Combine common and static parameters + values.update(values_static) + + # Create can message with "translated" can bytes. + if car_fingerprint in PLATFORM.C1: + dat = packer.make_can_msg("FSM1", 0, values)[2] + elif car_fingerprint in PLATFORM.EUCD: + dat = packer.make_can_msg("FSM2", 0, values)[2] + + values["Checksum"] = create_chksum(dat, car_fingerprint) + + if car_fingerprint in PLATFORM.C1: + return packer.make_can_msg("FSM1", 0, values) + elif car_fingerprint in PLATFORM.EUCD: + return packer.make_can_msg("FSM2", 0, values) +