mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-02-18 16:23:52 +08:00
Add volvo support to Dragonpilot
This commit is contained in:
chiachunli08
@@ -555,6 +555,8 @@ struct CarParams {
|
||||
hyundaiLegacy @23;
|
||||
hyundaiCommunity @24;
|
||||
stellantis @25;
|
||||
volvoC1 @26;
|
||||
volvoEUCD @27;
|
||||
}
|
||||
|
||||
enum SteerControlType {
|
||||
|
||||
363
opendbc/volvo_v40_2017_pt.dbc
Normal file
363
opendbc/volvo_v40_2017_pt.dbc
Normal file
@@ -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" ;
|
||||
230
opendbc/volvo_v60_2015_pt.dbc
Normal file
230
opendbc/volvo_v60_2015_pt.dbc
Normal file
@@ -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";
|
||||
@@ -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
|
||||
};
|
||||
|
||||
|
||||
599
panda/board/safety/safety_volvo.h
Normal file
599
panda/board/safety/safety_volvo.h
Normal file
@@ -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,
|
||||
};
|
||||
0
selfdrive/car/volvo/__init__.py
Normal file
0
selfdrive/car/volvo/__init__.py
Normal file
296
selfdrive/car/volvo/carcontroller.py
Normal file
296
selfdrive/car/volvo/carcontroller.py
Normal file
@@ -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
|
||||
407
selfdrive/car/volvo/carstate.py
Normal file
407
selfdrive/car/volvo/carstate.py
Normal file
@@ -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)
|
||||
139
selfdrive/car/volvo/interface.py
Normal file
139
selfdrive/car/volvo/interface.py
Normal file
@@ -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
|
||||
5
selfdrive/car/volvo/radar_interface.py
Normal file
5
selfdrive/car/volvo/radar_interface.py
Normal file
@@ -0,0 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
153
selfdrive/car/volvo/values.py
Normal file
153
selfdrive/car/volvo/values.py
Normal file
@@ -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
|
||||
#[<car.capnp:CarParams.CarFw builder (ecu = engine, fwVersion = "31432422 CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", address = 2016, subAddress = 0)>,
|
||||
# <car.capnp:CarParams.CarFw builder (ecu = eps, fwVersion = "31288595 AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", address = 1840, subAddress = 0)>,
|
||||
# <car.capnp:CarParams.CarFw builder (ecu = unknown, fwVersion = "31453061 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", address = 1830, subAddress = 0)>,
|
||||
# <car.capnp:CarParams.CarFw builder (ecu = fwdCamera, fwVersion = "31400454 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", address = 1892, subAddress = 0)>]
|
||||
|
||||
|
||||
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),
|
||||
}
|
||||
109
selfdrive/car/volvo/volvocan.py
Normal file
109
selfdrive/car/volvo/volvocan.py
Normal file
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user