You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
255 lines
12 KiB
255 lines
12 KiB
from cereal import car
|
|
from common.conversions import Conversions as CV
|
|
from opendbc.can.can_define import CANDefine
|
|
from opendbc.can.parser import CANParser
|
|
from selfdrive.car.interfaces import CarStateBase
|
|
from selfdrive.car.ford.values import CANBUS, DBC, CarControllerParams
|
|
|
|
GearShifter = car.CarState.GearShifter
|
|
TransmissionType = car.CarParams.TransmissionType
|
|
|
|
|
|
class CarState(CarStateBase):
|
|
def __init__(self, CP):
|
|
super().__init__(CP)
|
|
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
|
if CP.transmissionType == TransmissionType.automatic:
|
|
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]
|
|
|
|
def update(self, cp, cp_cam):
|
|
ret = car.CarState.new_message()
|
|
|
|
# car speed
|
|
ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
|
|
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
|
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
|
|
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
|
|
|
|
# gas pedal
|
|
ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100.
|
|
ret.gasPressed = ret.gas > 1e-6
|
|
|
|
# brake pedal
|
|
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm
|
|
ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2
|
|
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
|
|
|
|
# steering wheel
|
|
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
|
|
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
|
|
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
|
|
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
|
|
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
|
|
# ret.espDisabled = False # TODO: find traction control signal
|
|
|
|
# cruise state
|
|
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS
|
|
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
|
|
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5)
|
|
ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
|
|
ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
|
|
|
|
# gear
|
|
if self.CP.transmissionType == TransmissionType.automatic:
|
|
gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"])
|
|
ret.gearShifter = self.parse_gear_shifter(gear)
|
|
elif self.CP.transmissionType == TransmissionType.manual:
|
|
ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0
|
|
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
|
|
ret.gearShifter = GearShifter.reverse
|
|
else:
|
|
ret.gearShifter = GearShifter.drive
|
|
|
|
# safety
|
|
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
|
|
ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled
|
|
|
|
# button presses
|
|
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
|
|
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
|
|
# TODO: block this going to the camera otherwise it will enable stock TJA
|
|
ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
|
|
|
|
# lock info
|
|
ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
|
|
cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]])
|
|
ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2
|
|
|
|
# blindspot sensors
|
|
if self.CP.enableBsm:
|
|
ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
|
|
ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
|
|
|
|
# Stock steering buttons so that we can passthru blinkers etc.
|
|
self.buttons_stock_values = cp.vl["Steering_Data_FD1"]
|
|
# Stock values from IPMA so that we can retain some stock functionality
|
|
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
|
|
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
|
|
|
|
return ret
|
|
|
|
@staticmethod
|
|
def get_can_parser(CP):
|
|
signals = [
|
|
# sig_name, sig_address
|
|
("Veh_V_ActlBrk", "BrakeSysFeatures"), # ABS vehicle speed (kph)
|
|
("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s)
|
|
("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped
|
|
("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status
|
|
("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"), # PCM throttle (pct)
|
|
("BrkTot_Tq_Actl", "BrakeSnData_4"), # ABS brake torque (Nm)
|
|
("BpedDrvAppl_D_Actl", "EngBrakeData"), # PCM driver brake pedal pressed
|
|
("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph)
|
|
# The units might change with IPC settings?
|
|
("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status
|
|
("AccStopMde_D_Rq", "EngBrakeData"), # PCM ACC standstill
|
|
("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"), # PCM ACC enable
|
|
("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg)
|
|
# Calculates steering angle (and offset) from pinion
|
|
# angle and driving measurements.
|
|
# StePinRelInit_An_Sns is the pinion angle, initialised
|
|
# to zero at the beginning of the drive.
|
|
("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm)
|
|
("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status
|
|
("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch
|
|
("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle
|
|
("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver
|
|
("DrStatPsngr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, passenger
|
|
("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left
|
|
("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right
|
|
("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver
|
|
("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthrough the remaining buttons
|
|
("WiprFront_D_Stat", "Steering_Data_FD1"),
|
|
("LghtAmb_D_Sns", "Steering_Data_FD1"),
|
|
("AccButtnGapDecPress", "Steering_Data_FD1"),
|
|
("AccButtnGapIncPress", "Steering_Data_FD1"),
|
|
("AslButtnOnOffCnclPress", "Steering_Data_FD1"),
|
|
("AslButtnOnOffPress", "Steering_Data_FD1"),
|
|
("CcAslButtnCnclPress", "Steering_Data_FD1"),
|
|
("LaSwtchPos_D_Stat", "Steering_Data_FD1"),
|
|
("CcAslButtnCnclResPress", "Steering_Data_FD1"),
|
|
("CcAslButtnDeny_B_Actl", "Steering_Data_FD1"),
|
|
("CcAslButtnIndxDecPress", "Steering_Data_FD1"),
|
|
("CcAslButtnIndxIncPress", "Steering_Data_FD1"),
|
|
("CcAslButtnOffCnclPress", "Steering_Data_FD1"),
|
|
("CcAslButtnOnOffCncl", "Steering_Data_FD1"),
|
|
("CcAslButtnOnPress", "Steering_Data_FD1"),
|
|
("CcAslButtnResDecPress", "Steering_Data_FD1"),
|
|
("CcAslButtnResIncPress", "Steering_Data_FD1"),
|
|
("CcAslButtnSetDecPress", "Steering_Data_FD1"),
|
|
("CcAslButtnSetIncPress", "Steering_Data_FD1"),
|
|
("CcAslButtnSetPress", "Steering_Data_FD1"),
|
|
("CcAsllButtnResPress", "Steering_Data_FD1"),
|
|
("CcButtnOffPress", "Steering_Data_FD1"),
|
|
("CcButtnOnOffCnclPress", "Steering_Data_FD1"),
|
|
("CcButtnOnOffPress", "Steering_Data_FD1"),
|
|
("CcButtnOnPress", "Steering_Data_FD1"),
|
|
("HeadLghtHiFlash_D_Actl", "Steering_Data_FD1"),
|
|
("HeadLghtHiOn_B_StatAhb", "Steering_Data_FD1"),
|
|
("AhbStat_B_Dsply", "Steering_Data_FD1"),
|
|
("AccButtnGapTogglePress", "Steering_Data_FD1"),
|
|
("WiprFrontSwtch_D_Stat", "Steering_Data_FD1"),
|
|
("HeadLghtHiCtrl_D_RqAhb", "Steering_Data_FD1"),
|
|
]
|
|
|
|
checks = [
|
|
# sig_address, frequency
|
|
("BrakeSysFeatures", 50),
|
|
("Yaw_Data_FD1", 100),
|
|
("DesiredTorqBrk", 50),
|
|
("EngVehicleSpThrottle", 100),
|
|
("BrakeSnData_4", 50),
|
|
("EngBrakeData", 10),
|
|
("Cluster_Info1_FD1", 10),
|
|
("SteeringPinion_Data", 100),
|
|
("EPAS_INFO", 50),
|
|
("Lane_Assist_Data3_FD1", 33),
|
|
("Steering_Data_FD1", 10),
|
|
("BodyInfo_3_FD1", 2),
|
|
("RCMStatusMessage2_FD1", 10),
|
|
]
|
|
|
|
if CP.transmissionType == TransmissionType.automatic:
|
|
signals += [
|
|
("TrnRng_D_RqGsm", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position
|
|
]
|
|
checks += [
|
|
("Gear_Shift_by_Wire_FD1", 10),
|
|
]
|
|
elif CP.transmissionType == TransmissionType.manual:
|
|
signals += [
|
|
("CluPdlPos_Pc_Meas", "Engine_Clutch_Data"), # PCM clutch (pct)
|
|
("RvrseLghtOn_B_Stat", "BCM_Lamp_Stat_FD1"), # BCM reverse light
|
|
]
|
|
checks += [
|
|
("Engine_Clutch_Data", 33),
|
|
("BCM_Lamp_Stat_FD1", 1),
|
|
]
|
|
|
|
if CP.enableBsm:
|
|
signals += [
|
|
("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left
|
|
("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right
|
|
]
|
|
checks += [
|
|
("Side_Detect_L_Stat", 5),
|
|
("Side_Detect_R_Stat", 5),
|
|
]
|
|
|
|
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.main)
|
|
|
|
@staticmethod
|
|
def get_cam_can_parser(CP):
|
|
signals = [
|
|
# sig_name, sig_address
|
|
("HaDsply_No_Cs", "ACCDATA_3"),
|
|
("HaDsply_No_Cnt", "ACCDATA_3"),
|
|
("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message
|
|
("AccTrgDist2_D_Dsply", "ACCDATA_3"), # ACC target distance
|
|
("AccStopRes_B_Dsply", "ACCDATA_3"),
|
|
("TjaWarn_D_Rq", "ACCDATA_3"), # TJA warning
|
|
("Tja_D_Stat", "ACCDATA_3"), # TJA status
|
|
("TjaMsgTxt_D_Dsply", "ACCDATA_3"), # TJA text
|
|
("IaccLamp_D_Rq", "ACCDATA_3"), # iACC status icon
|
|
("AccMsgTxt_D2_Rq", "ACCDATA_3"), # ACC text
|
|
("FcwDeny_B_Dsply", "ACCDATA_3"), # FCW disabled
|
|
("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting
|
|
("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting
|
|
("CadsAlignIncplt_B_Actl", "ACCDATA_3"),
|
|
("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting
|
|
("CadsRadrBlck_B_Actl", "ACCDATA_3"),
|
|
("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status
|
|
("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting
|
|
("FcwMemSens_D_Actl", "ACCDATA_3"), # FCW sensitivity setting
|
|
("FcwMsgTxt_D_Rq", "ACCDATA_3"), # FCW text
|
|
("AccWarn_D_Dsply", "ACCDATA_3"), # ACC warning
|
|
("FcwVisblWarn_B_Rq", "ACCDATA_3"), # FCW visible alert
|
|
("FcwAudioWarn_B_Rq", "ACCDATA_3"), # FCW audio alert
|
|
("AccTGap_D_Dsply", "ACCDATA_3"), # ACC time gap
|
|
("AccMemEnbl_B_RqDrv", "ACCDATA_3"), # ACC adaptive/normal setting
|
|
("FdaMem_B_Stat", "ACCDATA_3"), # FDA enabled setting
|
|
|
|
("FeatConfigIpmaActl", "IPMA_Data"),
|
|
("FeatNoIpmaActl", "IPMA_Data"),
|
|
("PersIndexIpma_D_Actl", "IPMA_Data"),
|
|
("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping
|
|
("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines)
|
|
("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error
|
|
("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime
|
|
("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater?
|
|
("CamraStats_D_Dsply", "IPMA_Data"), # Camera status
|
|
("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level
|
|
("DasStats_D_Dsply", "IPMA_Data"), # DAS status
|
|
("DasWarn_D_Dsply", "IPMA_Data"), # DAS warning
|
|
("AhbHiBeam_D_Rq", "IPMA_Data"), # AHB status
|
|
("Passthru_63", "IPMA_Data"),
|
|
("Passthru_48", "IPMA_Data"),
|
|
]
|
|
|
|
checks = [
|
|
# sig_address, frequency
|
|
("ACCDATA_3", 5),
|
|
("IPMA_Data", 1),
|
|
]
|
|
|
|
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.camera)
|
|
|