class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
#Auto detection for setup
self.no_radar = CP.sccBus == -1
self.mdps_bus = CP.mdpsBus
self.sas_bus = CP.sasBus
self.scc_bus = CP.sccBus
self.lkas_button_on = True
self.has_scc13 = CP.carFingerprint in FEATURES["has_scc13"]
self.has_scc14 = CP.carFingerprint in FEATURES["has_scc14"]
self.cruise_main_button = 0
self.mdps_error_cnt = 0
self.spas_enabled = CP.spasEnabled
self.acc_active = False
self.cruiseState_standstill = False
self.cruiseState_modeSel = 0
self.SC = SpdController()
self.driverAcc_time = 0
# blinker
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.apply_steer = 0.
self.steer_anglecorrection = float(int(Params().get("OpkrSteerAngleCorrection", encoding='utf8')) * 0.1)
self.gear_correction = Params().get("JustDoGearD", encoding='utf8') == "1"
def update(self, cp, cp2, cp_cam):
cp_mdps = cp2 if self.mdps_bus else cp
cp_sas = cp2 if self.sas_bus else cp
cp_scc = cp2 if self.scc_bus == 1 else cp_cam if self.scc_bus == 2 else cp
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_main_button = self.cruise_main_button
self.prev_lkas_button_on = self.lkas_button_on
ret = car.CarState.new_message()
ret.doorOpen = any([cp.vl["CGW1"]['CF_Gway_DrvDrSw'], cp.vl["CGW1"]['CF_Gway_AstDrSw'],
cp.vl["CGW2"]['CF_Gway_RLDrSw'], cp.vl["CGW2"]['CF_Gway_RRDrSw']])
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.vEgo = cp.vl["CLU11"]["CF_Clu_Vanz"] * CV.KPH_TO_MS
ret.standstill = ret.vEgoRaw < 0.1
ret.standStill = self.CP.standStill
ret.steeringAngleDeg = cp_sas.vl["SAS11"]['SAS_Angle'] - self.steer_anglecorrection
ret.steeringRateDeg = cp_sas.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
ret.steeringTorque = cp_mdps.vl["MDPS12"]['CR_Mdps_StrColTq']
ret.steeringTorqueEps = cp_mdps.vl["MDPS12"]['CR_Mdps_OutTq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
self.mdps_error_cnt += 1 if cp_mdps.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0 else -self.mdps_error_cnt
ret.steerWarning = self.mdps_error_cnt > 100 #cp_mdps.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0
ret.leftBlinker, ret.rightBlinker = self.update_blinker(cp)
self.VSetDis = cp_scc.vl["SCC11"]['VSetDis']
ret.vSetDis = self.VSetDis
self.clu_Vanz = cp.vl["CLU11"]["CF_Clu_Vanz"]
lead_objspd = cp_scc.vl["SCC11"]['ACC_ObjRelSpd']
self.lead_objspd = lead_objspd * CV.MS_TO_KPH
self.Mdps_ToiUnavail = cp_mdps.vl["MDPS12"]['CF_Mdps_ToiUnavail']
self.driverOverride = cp.vl["TCS13"]["DriverOverride"]
if self.driverOverride == 1:
self.driverAcc_time = 100
elif self.driverAcc_time:
self.driverAcc_time -= 1
# cruise state
ret.cruiseState.enabled = (cp_scc.vl["SCC12"]['ACCMode'] != 0) if not self.no_radar else \
cp.vl["LVR12"]['CF_Lvr_CruiseSet'] != 0
ret.cruiseState.available = (cp_scc.vl["SCC11"]["MainMode_ACC"] != 0) if not self.no_radar else \
cp.vl['EMS16']['CRUISE_LAMP_M'] != 0
ret.cruiseState.standstill = cp_scc.vl["SCC11"]['SCCInfoDisplay'] == 4 if not self.no_radar else False
self.cruiseState_standstill = ret.cruiseState.standstill
self.is_set_speed_in_mph = bool(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
self.acc_active = ret.cruiseState.enabled
self.cruiseState_modeSel, speed_kph = self.SC.update_cruiseSW(self)
ret.cruiseState.modeSel = self.cruiseState_modeSel
if ret.cruiseState.enabled:
speed_conv = CV.MPH_TO_MS if self.is_set_speed_in_mph else CV.KPH_TO_MS
ret.cruiseState.speed = speed_kph * speed_conv if not self.no_radar else \
cp.vl["LVR12"]["CF_Lvr_CruiseSet"] * speed_conv
else:
ret.cruiseState.speed = 0
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
ret.cruiseButtons = self.cruise_buttons
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
# TODO: Check this
ret.brakeLights = bool(cp.vl["TCS13"]['BrakeLight'] or ret.brakePressed)
if self.CP.carFingerprint in FEATURES["use_elect_ems"]:
ret.gas = cp.vl["E_EMS11"]['Accel_Pedal_Pos'] / 256.
ret.gasPressed = ret.gas > 5
else:
ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# TPMS code added from OPKR
if cp.vl["TPMS11"]['UNIT'] == 0.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL']
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR']
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL']
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR']
elif cp.vl["TPMS11"]['UNIT'] == 1.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL'] * 5 * 0.145038
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR'] * 5 * 0.145038
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL'] * 5 * 0.145038
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR'] * 5 * 0.145038
elif cp.vl["TPMS11"]['UNIT'] == 2.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL'] / 10 * 14.5038
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR'] / 10 * 14.5038
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL'] / 10 * 14.5038
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR'] / 10 * 14.5038
self.cruiseGapSet = cp_scc.vl["SCC11"]['TauGapSet']
# TODO: refactor gear parsing in function
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
ret.gearShifter = GearShifter.drive
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
ret.gearShifter = GearShifter.neutral
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
ret.gearShifter = GearShifter.park
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
ret.gearShifter = GearShifter.reverse
else:
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton via TCU12
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
if gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 14:
ret.gearShifter = GearShifter.reverse
elif gear > 0 and gear < 9: # unaware of anything over 8 currently
ret.gearShifter = GearShifter.drive
else:
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton - This is only compatible with optima hybrid 2017
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
if gear in (5, 8): # 5: D, 8: sport mode
ret.gearShifter = GearShifter.drive
elif gear == 6:
ret.gearShifter = GearShifter.neutral
elif gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 7:
ret.gearShifter = GearShifter.reverse
else:
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if gear in (5, 8): # 5: D, 8: sport mode
ret.gearShifter = GearShifter.drive
elif gear == 6:
ret.gearShifter = GearShifter.neutral
elif gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 7:
ret.gearShifter = GearShifter.reverse
else:
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
if self.CP.carFingerprint in FEATURES["use_fca"]:
ret.stockAeb = cp.vl["FCA11"]['FCA_CmdAct'] != 0
ret.stockFcw = cp.vl["FCA11"]['CF_VSM_Warn'] == 2
else:
ret.stockAeb = cp.vl["SCC12"]['AEB_CmdAct'] != 0
ret.stockFcw = cp.vl["SCC12"]['CF_VSM_Warn'] == 2
# Blind Spot Detection and Lane Change Assist signals
self.lca_state = cp.vl["LCA11"]["CF_Lca_Stat"]
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11, CLU11, SCC12 and MDPS12
self.lkas11 = cp_cam.vl["LKAS11"]
self.clu11 = cp.vl["CLU11"]
self.scc11 = cp_scc.vl["SCC11"]
self.scc12 = cp_scc.vl["SCC12"]
self.mdps12 = cp_mdps.vl["MDPS12"]
#self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
self.park_brake = cp.vl["TCS13"]['PBRAKE_ACT'] == 1
self.steer_state = cp_mdps.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
self.cruise_unavail = cp.vl["TCS13"]['CF_VSM_Avail'] != 1
self.lead_distance = cp_scc.vl["SCC11"]['ACC_ObjDist'] if not self.no_radar else 0
self.brake_hold = cp.vl["TCS15"]['AVH_LAMP'] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
self.brake_error = cp.vl["TCS13"]['ACCEnable'] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
ret.radarDistance = cp_scc.vl["SCC11"]['ACC_ObjDist'] if not self.no_radar else 0
self.lkas_error = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"] == 7
if not self.lkas_error:
self.lkas_button_on = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"]
if self.has_scc13:
self.scc13 = cp_scc.vl["SCC13"]
if self.has_scc14:
self.scc14 = cp_scc.vl["SCC14"]
if self.spas_enabled:
self.ems11 = cp.vl["EMS11"]
self.mdps11_strang = cp_mdps.vl["MDPS11"]["CR_Mdps_StrAng"]
self.mdps11_stat = cp_mdps.vl["MDPS11"]["CF_Mdps_Stat"]
ret.cruiseAccStatus = cp_scc.vl["SCC12"]['ACCMode'] == 1
ret.driverAcc = self.driverOverride == 1
return ret
def update_blinker(self, cp):
self.TSigLHSw = cp.vl["CGW1"]['CF_Gway_TSigLHSw']
self.TSigRHSw = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
leftBlinker = cp.vl["CGW1"]['CF_Gway_TurnSigLh'] != 0
rightBlinker = cp.vl["CGW1"]['CF_Gway_TurnSigRh'] != 0
if leftBlinker and not rightBlinker:
self.left_blinker_flash = 50
self.right_blinker_flash = 0
elif rightBlinker and not leftBlinker:
self.right_blinker_flash = 50
self.left_blinker_flash = 0
elif leftBlinker and rightBlinker:
self.left_blinker_flash = 50
self.right_blinker_flash = 50
if self.left_blinker_flash:
self.left_blinker_flash -= 1
if self.right_blinker_flash:
self.right_blinker_flash -= 1
leftBlinker = self.left_blinker_flash != 0
rightBlinker = self.right_blinker_flash != 0
return leftBlinker, rightBlinker
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("WHL_SPD_FL", "WHL_SPD11", 0),
("WHL_SPD_FR", "WHL_SPD11", 0),
("WHL_SPD_RL", "WHL_SPD11", 0),
("WHL_SPD_RR", "WHL_SPD11", 0),
("YAW_RATE", "ESP12", 0),
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
("CF_Gway_DrvSeatBeltSw", "CGW1", 0), # Driver Seatbelt
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door is open
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door is open
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door is open
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door is open
("CF_Gway_TSigLHSw", "CGW1", 0),
("CF_Gway_TurnSigLh", "CGW1", 0),
("CF_Gway_TSigRHSw", "CGW1", 0),
("CF_Gway_TurnSigRh", "CGW1", 0),
("CF_Gway_ParkBrakeSw", "CGW1", 0), # Parking Brake
("CYL_PRES", "ESP12", 0),
("CF_Clu_CruiseSwState", "CLU11", 0),
("CF_Clu_CruiseSwMain", "CLU11", 0),
("CF_Clu_SldMainSW", "CLU11", 0),
("CF_Clu_ParityBit1", "CLU11", 0),
("CF_Clu_VanzDecimal" , "CLU11", 0),
("CF_Clu_Vanz", "CLU11", 0),
("CF_Clu_SPEED_UNIT", "CLU11", 0),
("CF_Clu_DetentOut", "CLU11", 0),
("CF_Clu_RheostatLevel", "CLU11", 0),
("CF_Clu_CluInfo", "CLU11", 0),
("CF_Clu_AmpInfo", "CLU11", 0),
("CF_Clu_AliveCnt1", "CLU11", 0),
("ACCEnable", "TCS13", 0),
("ACC_REQ", "TCS13", 0),
("BrakeLight", "TCS13", 0),
("DriverBraking", "TCS13", 0),
("StandStill", "TCS13", 0),
("PBRAKE_ACT", "TCS13", 0),
("DriverOverride", "TCS13", 0),
("CF_VSM_Avail", "TCS13", 0),
("ESC_Off_Step", "TCS15", 0),
("AVH_LAMP", "TCS15", 0),
("CF_Lvr_GearInf", "LVR11", 0), # Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev)
("CF_Lca_Stat", "LCA11", 0),
("CF_Lca_IndLeft", "LCA11", 0),
("CF_Lca_IndRight", "LCA11", 0),
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150), #TK211X value is 204.6
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("UNIT", "TPMS11", 0),
("PRESSURE_FL", "TPMS11", 0),
("PRESSURE_FR", "TPMS11", 0),
("PRESSURE_RL", "TPMS11", 0),
("PRESSURE_RR", "TPMS11", 0),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0), #aReqMax
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0), #aReqMin
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
]
checks = [
# address, frequency
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("ESP12", 100),
("CGW1", 10),
("CGW4", 5),
("WHL_SPD11", 50),
]
if CP.sccBus == 0 and CP.enableCruise:
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.mdpsBus == 0:
signals += [
("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)
]
checks += [
("MDPS12", 50)
]
if CP.sasBus == 0:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [
("SAS11", 100)
]
if CP.sccBus == -1:
signals += [
("CRUISE_LAMP_M", "EMS16", 0),
("CF_Lvr_CruiseSet", "LVR12", 0),
]
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
("CF_Clu_InhibitD", "CLU15", 0),
("CF_Clu_InhibitP", "CLU15", 0),
("CF_Clu_InhibitN", "CLU15", 0),
("CF_Clu_InhibitR", "CLU15", 0),
]
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
signals += [
("CUR_GR", "TCU12",0),
]
elif CP.carFingerprint in FEATURES["use_elect_gears"]:
signals += [
("Elect_Gear_Shifter", "ELECT_GEAR", 0),
]
else:
signals += [
("CF_Lvr_Gear","LVR12",0),
]
if CP.carFingerprint not in FEATURES["use_elect_ems"]:
signals += [
("PV_AV_CAN", "EMS12", 0),
("CF_Ems_AclAct", "EMS16", 0),
]
checks += [
("EMS12", 100),
("EMS16", 100),
]
else:
signals += [
("Accel_Pedal_Pos","E_EMS11",0),
("Brake_Pedal_Pos","E_EMS11",0),
]
checks += [
("E_EMS11", 100),
]
if CP.carFingerprint in FEATURES["use_fca"]:
signals += [
("FCA_CmdAct", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
]
checks += [("FCA11", 50)]
if CP.carFingerprint in [CAR.SANTA_FE]:
checks.remove(("TCS13", 50))
if CP.spasEnabled:
if CP.mdpsBus == 1:
signals += [
("SWI_IGK", "EMS11", 0),
("F_N_ENG", "EMS11", 0),
("ACK_TCS", "EMS11", 0),
("PUC_STAT", "EMS11", 0),
("TQ_COR_STAT", "EMS11", 0),
("RLY_AC", "EMS11", 0),
("F_SUB_TQI", "EMS11", 0),
("TQI_ACOR", "EMS11", 0),
("N", "EMS11", 0),
("TQI", "EMS11", 0),
("TQFR", "EMS11", 0),
("VS", "EMS11", 0),
("RATIO_TQI_BAS_MAX_STND", "EMS11", 0),
]
checks += [("EMS11", 100)]
elif CP.mdpsBus == 0:
signals += [
("CR_Mdps_StrAng", "MDPS11", 0),
("CF_Mdps_Stat", "MDPS11", 0),
]
checks += [("MDPS11", 100)]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@staticmethod
def get_can2_parser(CP):
signals = []
checks = []
if CP.mdpsBus == 1:
signals += [
("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)
]
checks += [
("MDPS12", 50)
]
if CP.spasEnabled:
signals += [
("CR_Mdps_StrAng", "MDPS11", 0),
("CF_Mdps_Stat", "MDPS11", 0),
]
checks += [
("MDPS11", 100),
]
if CP.sasBus == 1:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [
("SAS11", 100)
]
if CP.sccBus == 1:
signals += [
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150.),
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0), #aReqMax
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0), #aReqMin
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
]
checks += [
("SCC11", 50),
("SCC12", 50),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("CF_Lkas_LdwsActivemode", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
("CF_Lkas_HbaLamp", "LKAS11", 0),
("CF_Lkas_FcwBasReq", "LKAS11", 0),
("CF_Lkas_ToiFlt", "LKAS11", 0),
("CF_Lkas_HbaSysState", "LKAS11", 0),
("CF_Lkas_FcwOpt", "LKAS11", 0),
("CF_Lkas_HbaOpt", "LKAS11", 0),
("CF_Lkas_FcwSysState", "LKAS11", 0),
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
("CF_Lkas_MsgCount", "LKAS11", 0),
("CF_Lkas_FusionState", "LKAS11", 0),
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
]
checks = [
("LKAS11", 100)
]
if CP.sccBus == 2:
signals += [
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150.),
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0), #aReqMax
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0), #aReqMin
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
]
checks += [
("SCC11", 50),
("SCC12", 50),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
self.cruise_main_button = False
self.cruise_buttons = False
self.lkas_button_on = False
self.lkas_error = False
self.prev_cruise_main_button = False
self.prev_cruise_buttons = False
self.main_on = False
self.acc_active = False
self.cruiseState_modeSel = 0
self.Mdps_ToiUnavail = 0
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.steerWarning = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.driverAcc_time = 0
self.gearShifter = 0
self.leftBlindspot_time = 0
self.rightBlindspot_time = 0
self.SC = SpdController()
def update(self, cp, cp_cam):
self.prev_cruise_main_button = self.cruise_main_button
self.prev_cruise_buttons = self.cruise_buttons
ret = car.CarState.new_message()
ret.doorOpen = any([
cp.vl["CGW1"]['CF_Gway_DrvDrSw'], cp.vl["CGW1"]['CF_Gway_AstDrSw'],
cp.vl["CGW2"]['CF_Gway_RLDrSw'], cp.vl["CGW2"]['CF_Gway_RRDrSw']
])
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr +
ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.1
ret.steeringAngle = cp.vl["SAS11"][
'SAS_Angle'] - self.CP.lateralsRatom.steerOffset
ret.steeringRate = cp.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
ret.steeringTorque = cp.vl["MDPS12"]['CR_Mdps_StrColTq']
ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.leftBlinker, ret.rightBlinker = self.update_blinker(cp)
self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist']
lead_objspd = cp.vl["SCC11"]['ACC_ObjRelSpd']
self.lead_objspd = lead_objspd * CV.MS_TO_KPH
self.VSetDis = cp.vl["SCC11"]['VSetDis']
self.Mdps_ToiUnavail = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
self.clu_Vanz = cp.vl["CLU11"]["CF_Clu_Vanz"]
ret.vEgo = self.clu_Vanz * CV.KPH_TO_MS
steerWarning = False
if ret.vEgo < 5 or not self.Mdps_ToiUnavail:
self.steerWarning = 0
elif self.steerWarning >= 2:
steerWarning = True
else:
self.steerWarning += 1
ret.steerWarning = steerWarning
# cruise state
#ret.cruiseState.available = True
#ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0
self.main_on = (cp.vl["SCC11"]["MainMode_ACC"] != 0)
self.acc_active = (cp.vl["SCC12"]['ACCMode'] != 0)
self.update_atom(cp, cp_cam)
ret.cruiseState.available = self.main_on and self.cruiseState_modeSel != 3
ret.cruiseState.enabled = ret.cruiseState.available #and self.gearShifter == GearShifter.drive
ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
# most HKG cars has no long control, it is safer and easier to engage by main on
self.cruiseState_modeSel, speed_kph = self.SC.update_cruiseSW(self)
ret.cruiseState.modeSel = self.cruiseState_modeSel
ret.cruiseState.cruiseSwState = self.cruise_buttons
if self.acc_active:
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
ret.cruiseState.speed = speed_kph * speed_conv
else:
ret.cruiseState.speed = 0
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
# TODO: Check this
ret.brakeLights = bool(cp.vl["TCS13"]['BrakeLight']
or ret.brakePressed)
#TODO: find pedal signal for EV/HYBRID Cars
if self.CP.carFingerprint in EV_HYBRID:
ret.gas = cp.vl["E_EMS11"]['Accel_Pedal_Pos'] / 256.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# TODO: refactor gear parsing in function
self.gearShifter = self.get_gearShifter(cp)
ret.gearShifter = self.gearShifter
# Blind Spot Detection and Lane Change Assist signals
ret.leftBlindspot, ret.rightBlindspot = self.get_Blindspot(cp)
# save the entire LKAS11 and CLU11
self.lkas11 = cp_cam.vl["LKAS11"]
self.clu11 = cp.vl["CLU11"]
self.mdps12 = cp.vl["MDPS12"]
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
self.steer_state = cp.vl["MDPS12"][
'CF_Mdps_ToiActive'] # 0 NOT ACTIVE, 1 ACTIVE
self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist']
return ret
def update_blinker(self, cp):
self.TSigLHSw = cp.vl["CGW1"]['CF_Gway_TSigLHSw']
self.TSigRHSw = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
leftBlinker = cp.vl["CGW1"]['CF_Gway_TurnSigLh'] != 0
rightBlinker = cp.vl["CGW1"]['CF_Gway_TurnSigRh'] != 0
if leftBlinker:
self.left_blinker_flash = 300
elif self.left_blinker_flash:
self.left_blinker_flash -= 1
if rightBlinker:
self.right_blinker_flash = 300
elif self.right_blinker_flash:
self.right_blinker_flash -= 1
leftBlinker = self.left_blinker_flash != 0
rightBlinker = self.right_blinker_flash != 0
return leftBlinker, rightBlinker
def update_atom(self, cp, cp_cam):
# atom append
self.driverOverride = cp.vl["TCS13"][
"DriverOverride"] # 1 Acc, 2 bracking, 0 Normal
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"][
"CF_Clu_CruiseSwState"] # clu_CruiseSwState
self.Lkas_LdwsSysState = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"]
self.lkas_error = self.Lkas_LdwsSysState == 7
if not self.lkas_error:
self.lkas_button_on = self.Lkas_LdwsSysState
if self.driverOverride == 1:
self.driverAcc_time = 100
elif self.driverAcc_time:
self.driverAcc_time -= 1
def get_gearShifter(self, cp):
gearShifter = GearShifter.unknown
# TODO: refactor gear parsing in function
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
gearShifter = GearShifter.drive
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
gearShifter = GearShifter.neutral
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
gearShifter = GearShifter.park
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
gearShifter = GearShifter.reverse
# Gear Selecton via TCU12
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
if gear == 0:
gearShifter = GearShifter.park
elif gear == 14:
gearShifter = GearShifter.reverse
elif gear > 0 and gear < 9: # unaware of anything over 8 currently
gearShifter = GearShifter.drive
# Gear Selecton - This is only compatible with optima hybrid 2017
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
if gear in (5, 8): # 5: D, 8: sport mode
gearShifter = GearShifter.drive
elif gear == 6:
gearShifter = GearShifter.neutral
elif gear == 0:
gearShifter = GearShifter.park
elif gear == 7:
gearShifter = GearShifter.reverse
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if gear in (5, 8): # 5: D, 8: sport mode
gearShifter = GearShifter.drive
elif gear == 6:
gearShifter = GearShifter.neutral
elif gear == 0:
gearShifter = GearShifter.park
elif gear == 7:
gearShifter = GearShifter.reverse
return gearShifter
def get_Blindspot(self, cp):
if cp.vl["LCA11"]['CF_Lca_IndLeft'] != 0:
self.leftBlindspot_time = 200
elif self.leftBlindspot_time:
self.leftBlindspot_time -= 1
if cp.vl["LCA11"]['CF_Lca_IndRight'] != 0:
self.rightBlindspot_time = 200
elif self.rightBlindspot_time:
self.rightBlindspot_time -= 1
leftBlindspot = self.leftBlindspot_time != 0
rightBlindspot = self.rightBlindspot_time != 0
return leftBlindspot, rightBlindspot
@staticmethod
def get_parser_gears(CP, signals, checks):
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
("CF_Clu_InhibitD", "CLU15", 0),
("CF_Clu_InhibitP", "CLU15", 0),
("CF_Clu_InhibitN", "CLU15", 0),
("CF_Clu_InhibitR", "CLU15", 0),
]
checks += [("CLU15", 5)]
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
signals += [("CUR_GR", "TCU12", 0)]
checks += [("TCU12", 100)]
elif CP.carFingerprint in FEATURES["use_elect_gears"]:
signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)]
checks += [("ELECT_GEAR", 20)]
else:
signals += [("CF_Lvr_Gear", "LVR12", 0)]
checks += [("LVR12", 100)]
return signals, checks
@staticmethod
def get_parser_ev_hybrid(CP, signals, checks):
if CP.carFingerprint in EV_HYBRID:
signals += [
("Accel_Pedal_Pos", "E_EMS11", 0),
]
checks += [
("E_EMS11", 50),
]
else:
signals += [
("PV_AV_CAN", "EMS12", 0),
("CF_Ems_AclAct", "EMS16", 0),
]
checks += [
("EMS12", 100),
("EMS16", 100), # 608
]
return signals, checks
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("WHL_SPD_FL", "WHL_SPD11", 0),
("WHL_SPD_FR", "WHL_SPD11", 0),
("WHL_SPD_RL", "WHL_SPD11", 0),
("WHL_SPD_RR", "WHL_SPD11", 0),
("YAW_RATE", "ESP12", 0),
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door
("CF_Gway_TSigLHSw", "CGW1", 0),
("CF_Gway_TurnSigLh", "CGW1", 0),
("CF_Gway_TSigRHSw", "CGW1", 0),
("CF_Gway_TurnSigRh", "CGW1", 0),
("CF_Gway_ParkBrakeSw", "CGW1", 0),
("CYL_PRES", "ESP12", 0),
("CF_Clu_CruiseSwState", "CLU11", 0),
("CF_Clu_CruiseSwMain", "CLU11", 0),
("CF_Clu_SldMainSW", "CLU11", 0),
("CF_Clu_ParityBit1", "CLU11", 0),
("CF_Clu_VanzDecimal", "CLU11", 0),
("CF_Clu_Vanz", "CLU11", 0),
("CF_Clu_SPEED_UNIT", "CLU11", 0),
("CF_Clu_DetentOut", "CLU11", 0),
("CF_Clu_RheostatLevel", "CLU11", 0),
("CF_Clu_CluInfo", "CLU11", 0),
("CF_Clu_AmpInfo", "CLU11", 0),
("CF_Clu_AliveCnt1", "CLU11", 0),
("ACCEnable", "TCS13", 0),
("BrakeLight", "TCS13", 0),
("DriverBraking", "TCS13", 0),
("DriverOverride", "TCS13", 0),
("ESC_Off_Step", "TCS15", 0),
("CF_Lvr_GearInf", "LVR11",
0), # Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev)
("CF_Lca_IndLeft", "LCA11", 0),
("CF_Lca_IndRight", "LCA11", 0),
("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0), #
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0), #
("CF_Mdps_Chksum2", "MDPS12", 0), #
("CF_Mdps_ToiFlt", "MDPS12", 0), #
("CF_Mdps_SErr", "MDPS12", 0), #
("CR_Mdps_StrTq", "MDPS12", 0), #
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0),
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
("MainMode_ACC", "SCC11", 0),
("VSetDis", "SCC11", 0),
("SCCInfoDisplay", "SCC11", 0),
("ACC_ObjDist", "SCC11", 0),
("ACC_ObjRelSpd", "SCC11", 0),
("ACCMode", "SCC12", 1),
]
checks = [
# address, frequency
("MDPS12", 50), # 593
("TCS13", 50), # 916
("TCS15", 10),
("CLU11", 50),
("ESP12", 100),
("CGW1", 10),
("CGW4", 5),
("WHL_SPD11", 50), # 902
("SAS11", 100),
("SCC11", 50),
("SCC12", 50), # 1057
("LCA11", 50),
]
signals, checks = CarState.get_parser_ev_hybrid(CP, signals, checks)
signals, checks = CarState.get_parser_gears(CP, signals, checks)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("CF_Lkas_Bca_R", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
("CF_Lkas_HbaLamp", "LKAS11", 0),
("CF_Lkas_FcwBasReq", "LKAS11", 0),
("CF_Lkas_ToiFlt", "LKAS11", 0), # append
("CF_Lkas_HbaSysState", "LKAS11", 0),
("CF_Lkas_FcwOpt", "LKAS11", 0),
("CF_Lkas_HbaOpt", "LKAS11", 0),
("CF_Lkas_FcwSysState", "LKAS11", 0),
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
("CF_Lkas_MsgCount", "LKAS11", 0), # append
("CF_Lkas_FusionState", "LKAS11", 0),
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
]
checks = []
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
else: # preferred and elect gear methods use same definition
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
#Auto detection for setup
self.no_radar = CP.sccBus == -1
self.lkas_button_on = True
self.cruise_main_button = 0
self.mdps_error_cnt = 0
self.cruise_unavail_cnt = 0
self.acc_active = False
self.cruiseState_standstill = False
self.cruiseState_modeSel = 0
self.SC = SpdController()
self.driverAcc_time = 0
self.apply_steer = 0.
self.steer_anglecorrection = float(
int(Params().get("OpkrSteerAngleCorrection", encoding="utf8")) *
0.1)
self.gear_correction = Params().get_bool("JustDoGearD")
self.steer_wind_down = Params().get_bool("SteerWindDown")
self.brake_check = False
self.cancel_check = False
self.safety_sign_check = 0
self.safety_sign = 0
self.safety_sign_last = 0
self.safety_dist = 0
self.safety_block_remain_dist = 0
self.is_highway = False
self.on_speed_control = False
def update(self, cp, cp2, cp_cam):
cp_mdps = cp2 if self.CP.mdpsBus == 1 else cp
cp_sas = cp2 if self.CP.sasBus else cp
cp_scc = cp_cam if ((self.CP.sccBus == 2)
or self.CP.radarOffCan) else cp
cp_fca = cp_cam if (self.CP.fcaBus == 2) else cp
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_main_button = self.cruise_main_button
self.prev_lkas_button_on = self.lkas_button_on
ret = car.CarState.new_message()
ret.doorOpen = any([
cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]
])
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]["WHL_SPD_FL"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]["WHL_SPD_FR"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]["WHL_SPD_RL"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]["WHL_SPD_RR"] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr +
ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.vEgo = cp.vl["CLU11"]["CF_Clu_Vanz"] * CV.KPH_TO_MS
ret.standstill = ret.vEgoRaw < 0.1
ret.standStill = self.CP.standStill
ret.steeringAngleDeg = cp_sas.vl["SAS11"][
"SAS_Angle"] - self.steer_anglecorrection
ret.steeringRateDeg = cp_sas.vl["SAS11"]["SAS_Speed"]
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"],
cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp_mdps.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp_mdps.vl["MDPS12"]["CR_Mdps_OutTq"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
if self.steer_wind_down:
ret.steerWarning = cp_mdps.vl["MDPS12"][
"CF_Mdps_ToiUnavail"] != 0 or cp_mdps.vl["MDPS12"][
"CF_Mdps_ToiFlt"] != 0
else:
self.mdps_error_cnt += 1 if cp_mdps.vl["MDPS12"][
"CF_Mdps_ToiUnavail"] != 0 else -self.mdps_error_cnt
ret.steerWarning = self.mdps_error_cnt > 100 #cp_mdps.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0
self.VSetDis = cp_scc.vl["SCC11"]["VSetDis"]
ret.vSetDis = self.VSetDis
self.clu_Vanz = cp.vl["CLU11"]["CF_Clu_Vanz"]
lead_objspd = cp_scc.vl["SCC11"]["ACC_ObjRelSpd"]
self.lead_objspd = lead_objspd * CV.MS_TO_KPH
self.Mdps_ToiUnavail = cp_mdps.vl["MDPS12"]["CF_Mdps_ToiUnavail"]
self.driverOverride = cp.vl["TCS13"]["DriverOverride"]
if self.driverOverride == 1:
self.driverAcc_time = 100
elif self.driverAcc_time:
self.driverAcc_time -= 1
# cruise state
ret.cruiseState.enabled = (cp_scc.vl["SCC12"]["ACCMode"] != 0) if not self.no_radar else \
cp.vl["LVR12"]["CF_Lvr_CruiseSet"] != 0
ret.cruiseState.available = (cp_scc.vl["SCC11"]["MainMode_ACC"] != 0) if not self.no_radar else \
cp.vl["EMS16"]["CRUISE_LAMP_M"] != 0
ret.cruiseState.standstill = cp_scc.vl["SCC11"][
"SCCInfoDisplay"] == 4. if not self.no_radar else False
self.cruiseState_standstill = ret.cruiseState.standstill
self.is_set_speed_in_mph = bool(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
self.acc_active = ret.cruiseState.enabled
if self.acc_active:
self.brake_check = False
self.cancel_check = False
self.cruiseState_modeSel, speed_kph = self.SC.update_cruiseSW(self)
ret.cruiseState.modeSel = self.cruiseState_modeSel
if ret.cruiseState.enabled and (self.brake_check == False
or self.cancel_check == False):
speed_conv = CV.MPH_TO_MS if self.is_set_speed_in_mph else CV.KPH_TO_MS
ret.cruiseState.speed = speed_kph * speed_conv if not self.no_radar else \
cp.vl["LVR12"]["CF_Lvr_CruiseSet"] * speed_conv
else:
ret.cruiseState.speed = 0
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
ret.cruiseButtons = self.cruise_buttons
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
self.brakeUnavailable = cp.vl["TCS13"]["ACCEnable"] == 3
if ret.brakePressed:
self.brake_check = True
if self.cruise_buttons == 4:
self.cancel_check = True
# TODO: Check this
ret.brakeLights = bool(cp.vl["TCS13"]["BrakeLight"]
or ret.brakePressed)
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254.
else:
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100.
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
ret.espDisabled = (cp.vl["TCS15"]["ESC_Off_Step"] != 0)
self.parkBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
# TPMS code added from OPKR
if cp.vl["TPMS11"]["UNIT"] == 0.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]["PRESSURE_FL"]
ret.tpmsPressureFr = cp.vl["TPMS11"]["PRESSURE_FR"]
ret.tpmsPressureRl = cp.vl["TPMS11"]["PRESSURE_RL"]
ret.tpmsPressureRr = cp.vl["TPMS11"]["PRESSURE_RR"]
elif cp.vl["TPMS11"]["UNIT"] == 1.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]["PRESSURE_FL"] * 5 * 0.145038
ret.tpmsPressureFr = cp.vl["TPMS11"]["PRESSURE_FR"] * 5 * 0.145038
ret.tpmsPressureRl = cp.vl["TPMS11"]["PRESSURE_RL"] * 5 * 0.145038
ret.tpmsPressureRr = cp.vl["TPMS11"]["PRESSURE_RR"] * 5 * 0.145038
elif cp.vl["TPMS11"]["UNIT"] == 2.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]["PRESSURE_FL"] / 10 * 14.5038
ret.tpmsPressureFr = cp.vl["TPMS11"]["PRESSURE_FR"] / 10 * 14.5038
ret.tpmsPressureRl = cp.vl["TPMS11"]["PRESSURE_RL"] / 10 * 14.5038
ret.tpmsPressureRr = cp.vl["TPMS11"]["PRESSURE_RR"] / 10 * 14.5038
# OPKR
self.safety_dist = cp.vl["NAVI"]["OPKR_S_Dist"]
self.safety_sign_check = cp.vl["NAVI"]["OPKR_S_Sign"]
self.safety_block_remain_dist = cp.vl["NAVI"]["OPKR_SBR_Dist"]
self.is_highway = cp_scc.vl["SCC11"]["Navi_SCC_Camera_Act"] != 0.
if self.safety_sign_check in [25.] and not self.is_highway:
self.safety_sign = 30.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [1.] and not self.is_highway:
self.safety_sign = 40.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [8., 9., 10.] and not self.is_highway:
self.safety_sign = 50.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [16., 17., 18.] and not self.is_highway:
self.safety_sign = 60.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [24.] and not self.is_highway:
self.safety_sign = 70.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [0., 1., 2.]:
self.safety_sign = 80.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [8., 10.]:
self.safety_sign = 90.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [16., 17., 18.] and self.is_highway:
self.safety_sign = 100.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [24., 25., 26.] and self.is_highway:
self.safety_sign = 110.
self.safety_sign_last = self.safety_sign
elif self.safety_block_remain_dist < 255.:
self.safety_sign = self.safety_sign_last
else:
self.safety_sign = 0.
cam_distance_calc = interp(ret.vEgo * CV.MS_TO_KPH, [30, 60, 100, 160],
[3.6, 5.35, 5.8, 6.8])
consider_speed = interp((ret.vEgo * CV.MS_TO_KPH - self.safety_sign),
[10, 30], [1, 1.25])
if self.safety_sign > 29 and self.safety_dist < cam_distance_calc * consider_speed * ret.vEgo * CV.MS_TO_KPH:
ret.safetySign = self.safety_sign
ret.safetyDist = self.safety_dist
self.on_speed_control = True
elif self.safety_sign > 29 and self.safety_block_remain_dist < 255.:
ret.safetySign = self.safety_sign
ret.safetyDist = self.safety_dist
self.on_speed_control = True
elif self.safety_sign > 29 and self.safety_dist < 600.:
ret.safetySign = self.safety_sign
ret.safetyDist = self.safety_dist
self.on_speed_control = False
else:
ret.safetySign = 0
ret.safetyDist = 0
self.on_speed_control = False
self.cruiseGapSet = cp_scc.vl["SCC11"]["TauGapSet"]
ret.cruiseGapSet = self.cruiseGapSet
# TODO: refactor gear parsing in function
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
gear = cp.vl["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = self.parse_gear_shifter(
self.shifter_values.get(gear))
if self.CP.fcaBus != -1 or self.CP.carFingerprint in FEATURES[
"use_fca"]:
ret.stockAeb = cp_fca.vl["FCA11"]["FCA_CmdAct"] != 0
ret.stockFcw = cp_fca.vl["FCA11"]["CF_VSM_Warn"] == 2
elif not self.CP.radarOffCan:
ret.stockAeb = cp_scc.vl["SCC12"]["AEB_CmdAct"] != 0
ret.stockFcw = cp_scc.vl["SCC12"]["CF_VSM_Warn"] == 2
# Blind Spot Detection and Lane Change Assist signals
if self.CP.bsmAvailable or self.CP.enableBsm:
self.lca_state = cp.vl["LCA11"]["CF_Lca_Stat"]
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11, CLU11, SCC12 and MDPS12
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.scc11 = copy.copy(cp_scc.vl["SCC11"])
self.scc12 = copy.copy(cp_scc.vl["SCC12"])
self.scc13 = copy.copy(cp_scc.vl["SCC13"])
self.scc14 = copy.copy(cp_scc.vl["SCC14"])
self.fca11 = copy.copy(cp_fca.vl["FCA11"])
self.mdps12 = copy.copy(cp_mdps.vl["MDPS12"])
self.scc11init = copy.copy(cp.vl["SCC11"])
self.scc12init = copy.copy(cp.vl["SCC12"])
self.fca11init = copy.copy(cp.vl["FCA11"])
ret.brakeHold = cp.vl["TCS15"][
"AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
self.brakeHold = ret.brakeHold
self.brake_error = cp.vl["TCS13"][
"ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
self.steer_state = cp_mdps.vl["MDPS12"][
"CF_Mdps_ToiActive"] #0 NOT ACTIVE, 1 ACTIVE
self.cruise_unavail_cnt += 1 if cp.vl["TCS13"][
"CF_VSM_Avail"] != 1 and cp.vl["TCS13"][
"ACCEnable"] != 0 else -self.cruise_unavail_cnt
self.cruise_unavail = self.cruise_unavail_cnt > 100
self.lead_distance = cp_scc.vl["SCC11"][
"ACC_ObjDist"] if not self.no_radar else 0
ret.radarDistance = cp_scc.vl["SCC11"][
"ACC_ObjDist"] if not self.no_radar else 0
self.lkas_error = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"] == 7
if not self.lkas_error:
self.lkas_button_on = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"]
ret.cruiseAccStatus = cp_scc.vl["SCC12"]["ACCMode"] == 1
ret.driverAcc = self.driverOverride == 1
return ret
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("WHL_SPD_FL", "WHL_SPD11", 0),
("WHL_SPD_FR", "WHL_SPD11", 0),
("WHL_SPD_RL", "WHL_SPD11", 0),
("WHL_SPD_RR", "WHL_SPD11", 0),
("YAW_RATE", "ESP12", 0),
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door
("CF_Gway_TurnSigLh", "CGW1", 0),
("CF_Gway_TurnSigRh", "CGW1", 0),
("CF_Gway_ParkBrakeSw", "CGW1", 0),
("CYL_PRES", "ESP12", 0),
("AVH_STAT", "ESP11", 0),
("CF_Clu_CruiseSwState", "CLU11", 0),
("CF_Clu_CruiseSwMain", "CLU11", 0),
("CF_Clu_SldMainSW", "CLU11", 0),
("CF_Clu_ParityBit1", "CLU11", 0),
("CF_Clu_VanzDecimal", "CLU11", 0),
("CF_Clu_Vanz", "CLU11", 0),
("CF_Clu_SPEED_UNIT", "CLU11", 0),
("CF_Clu_DetentOut", "CLU11", 0),
("CF_Clu_RheostatLevel", "CLU11", 0),
("CF_Clu_CluInfo", "CLU11", 0),
("CF_Clu_AmpInfo", "CLU11", 0),
("CF_Clu_AliveCnt1", "CLU11", 0),
("ACCEnable", "TCS13", 0),
("BrakeLight", "TCS13", 0),
("DriverBraking", "TCS13", 0),
("DriverOverride", "TCS13", 0),
("PBRAKE_ACT", "TCS13", 0),
("CF_VSM_Avail", "TCS13", 0),
("ESC_Off_Step", "TCS15", 0),
("AVH_LAMP", "TCS15", 0),
("CF_Lvr_CruiseSet", "LVR12", 0),
("CRUISE_LAMP_M", "EMS16", 0),
("CR_FCA_Alive", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150), #TK211X value is 204.6
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0), #aReqMax
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0), #aReqMin
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
("UNIT", "TPMS11", 0),
("PRESSURE_FL", "TPMS11", 0),
("PRESSURE_FR", "TPMS11", 0),
("PRESSURE_RL", "TPMS11", 0),
("PRESSURE_RR", "TPMS11", 0),
("OPKR_S_Dist", "NAVI", 0),
("OPKR_S_Sign", "NAVI", 31),
("OPKR_SBR_Dist", "NAVI", 0),
]
checks = [
# address, frequency
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("ESP12", 100),
("CGW1", 10),
("CGW2", 5),
("CGW4", 5),
("WHL_SPD11", 50),
]
if CP.sccBus == 0 and CP.pcmCruise:
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.fcaBus == 0:
signals += [
("FCA_CmdAct", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
]
checks += [("FCA11", 50)]
if CP.mdpsBus == 0:
signals += [("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)]
checks += [("MDPS12", 50)]
if CP.sasBus == 0:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [("SAS11", 100)]
if CP.bsmAvailable or CP.enableBsm:
signals += [
("CF_Lca_Stat", "LCA11", 0),
("CF_Lca_IndLeft", "LCA11", 0),
("CF_Lca_IndRight", "LCA11", 0),
]
checks += [("LCA11", 50)]
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if CP.carFingerprint in HYBRID_CAR:
signals += [("CR_Vcu_AccPedDep_Pos", "E_EMS11", 0)]
else:
signals += [("Accel_Pedal_Pos", "E_EMS11", 0)]
checks += [
("E_EMS11", 50),
]
else:
signals += [
("PV_AV_CAN", "EMS12", 0),
("CF_Ems_AclAct", "EMS16", 0),
]
checks += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
("CF_Clu_Gear", "CLU15", 0),
]
checks += [("CLU15", 5)]
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
signals += [("CUR_GR", "TCU12", 0)]
checks += [("TCU12", 100)]
elif CP.carFingerprint in FEATURES["use_elect_gears"]:
signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)]
checks += [("ELECT_GEAR", 20)]
else:
signals += [("CF_Lvr_Gear", "LVR12", 0)]
checks += [("LVR12", 100)]
return CANParser(DBC[CP.carFingerprint]["pt"],
signals,
checks,
0,
enforce_checks=False)
@staticmethod
def get_can2_parser(CP):
signals = []
checks = []
if CP.mdpsBus == 1:
signals += [("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)]
checks += [("MDPS12", 50)]
if CP.sasBus == 1:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [("SAS11", 100)]
return CANParser(DBC[CP.carFingerprint]["pt"],
signals,
checks,
1,
enforce_checks=False)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("CF_Lkas_LdwsActivemode", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
("CF_Lkas_HbaLamp", "LKAS11", 0),
("CF_Lkas_FcwBasReq", "LKAS11", 0),
("CF_Lkas_ToiFlt", "LKAS11", 0),
("CF_Lkas_HbaSysState", "LKAS11", 0),
("CF_Lkas_FcwOpt", "LKAS11", 0),
("CF_Lkas_HbaOpt", "LKAS11", 0),
("CF_Lkas_FcwSysState", "LKAS11", 0),
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
("CF_Lkas_MsgCount", "LKAS11", 0),
("CF_Lkas_FusionState", "LKAS11", 0),
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
]
checks = [("LKAS11", 100)]
if CP.sccBus == 2:
signals += [
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150.),
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0),
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0),
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
("ACCMode", "SCC14", 0),
("ObjGap", "SCC14", 0),
("CF_VSM_Prefill", "FCA11", 0),
("CF_VSM_HBACmd", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
("CF_VSM_BeltCmd", "FCA11", 0),
("CR_VSM_DecCmd", "FCA11", 0),
("FCA_Status", "FCA11", 2),
("FCA_CmdAct", "FCA11", 0),
("FCA_StopReq", "FCA11", 0),
("FCA_DrvSetStatus", "FCA11", 1),
("CF_VSM_DecCmdAct", "FCA11", 0),
("FCA_Failinfo", "FCA11", 0),
("FCA_RelativeVelocity", "FCA11", 0),
("FCA_TimetoCollision", "FCA11", 2540.),
("CR_FCA_Alive", "FCA11", 0),
("CR_FCA_ChkSum", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
("PAINT1_Status", "FCA11", 1),
]
if CP.sccBus == 2:
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.fcaBus == 2:
checks += [("FCA11", 50)]
return CANParser(DBC[CP.carFingerprint]["pt"],
signals,
checks,
2,
enforce_checks=False)
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
self.read_distance_lines = 0
#Auto detection for setup
self.cruise_main_button = 0
self.cruise_buttons = 0
self.allow_nonscc_available = False
self.lkasstate = 0
self.lead_distance = 150.
self.radar_obj_valid = 0.
self.vrelative = 0.
self.prev_cruise_buttons = 0
self.prev_gap_button = 0
self.cancel_button_count = 0
self.cancel_button_timer = 0
self.leftblinkerflashdebounce = 0
self.rightblinkerflashdebounce = 0
self.SC = SpdController()
self.driverAcc_time = 0
self.brake_check = 0
self.mainsw_check = 0
self.steer_anglecorrection = int(
Params().get('OpkrSteerAngleCorrection')) * 0.1
self.cruise_gap = int(Params().get('OpkrCruiseGapSet'))
self.pm = messaging.PubMaster(
['liveTrafficData', 'dynamicFollowButton'])
self.sm = messaging.SubMaster(['liveMapData'])
def update(self, cp, cp2, cp_cam):
cp_mdps = cp2 if self.CP.mdpsHarness else cp
cp_sas = cp2 if self.CP.sasBus else cp
cp_scc = cp_cam if ((self.CP.sccBus == 2)
or self.CP.radarOffCan) else cp
cp_fca = cp_cam if (self.CP.fcaBus == 2) else cp
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_main_button = self.cruise_main_button
self.prev_lkasstate = self.lkasstate
ret = car.CarState.new_message()
ret.doorOpen = any([
cp.vl["CGW1"]['CF_Gway_DrvDrSw'], cp.vl["CGW1"]['CF_Gway_AstDrSw'],
cp.vl["CGW2"]['CF_Gway_RLDrSw'], cp.vl["CGW2"]['CF_Gway_RRDrSw']
])
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr +
ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.1
ret.standStill = self.CP.standStill
ret.steeringAngle = cp_sas.vl["SAS11"][
'SAS_Angle'] - self.steer_anglecorrection
ret.steeringRate = cp_sas.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
self.leftblinkerflash = cp.vl["CGW1"][
'CF_Gway_TurnSigLh'] != 0 and cp.vl["CGW1"]['CF_Gway_TSigLHSw'] == 0
self.rightblinkerflash = cp.vl["CGW1"][
'CF_Gway_TurnSigRh'] != 0 and cp.vl["CGW1"]['CF_Gway_TSigRHSw'] == 0
if self.leftblinkerflash:
self.leftblinkerflashdebounce = 50
elif self.leftblinkerflashdebounce > 0:
self.leftblinkerflashdebounce -= 1
if self.rightblinkerflash:
self.rightblinkerflashdebounce = 50
elif self.rightblinkerflashdebounce > 0:
self.rightblinkerflashdebounce -= 1
ret.leftBlinker = cp.vl["CGW1"][
'CF_Gway_TSigLHSw'] != 0 or self.leftblinkerflashdebounce > 0
ret.rightBlinker = cp.vl["CGW1"][
'CF_Gway_TSigRHSw'] != 0 or self.rightblinkerflashdebounce > 0
ret.steeringTorque = cp_mdps.vl["MDPS12"]['CR_Mdps_StrColTq']
ret.steeringTorqueEps = cp_mdps.vl["MDPS12"]['CR_Mdps_OutTq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp_mdps.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0
self.VSetDis = cp_scc.vl["SCC11"]['VSetDis']
ret.vSetDis = self.VSetDis
self.clu_Vanz = ret.vEgo * CV.MS_TO_KPH
lead_objspd = cp_scc.vl["SCC11"]['ACC_ObjRelSpd']
self.lead_objspd = lead_objspd * CV.MS_TO_KPH
self.driverOverride = cp.vl["TCS13"]["DriverOverride"]
if self.driverOverride == 1:
self.driverAcc_time = 100
elif self.driverAcc_time:
self.driverAcc_time -= 1
ret.brakeHold = cp.vl["ESP11"]['AVH_STAT'] == 1
self.brakeHold = ret.brakeHold
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
if not self.cruise_main_button:
if self.cruise_buttons == 4 and self.prev_cruise_buttons != 4 and self.cancel_button_count < 3:
self.cancel_button_count += 1
self.cancel_button_timer = 100
elif self.cancel_button_count == 3:
self.cancel_button_count = 0
if self.cancel_button_timer <= 100 and self.cancel_button_count:
self.cancel_button_timer = max(0, self.cancel_button_timer - 1)
if self.cancel_button_timer == 0:
self.cancel_button_count = 0
else:
self.cancel_button_count = 0
if self.prev_gap_button != self.cruise_buttons:
if self.cruise_buttons == 3:
self.cruise_gap -= 1
if self.cruise_gap < 1:
self.cruise_gap = 4
self.prev_gap_button = self.cruise_buttons
# cruise state
if not self.CP.enableCruise:
if self.cruise_buttons == 1 or self.cruise_buttons == 2:
self.allow_nonscc_available = True
self.brake_check = 0
self.mainsw_check = 0
ret.cruiseState.available = self.allow_nonscc_available != 0
ret.cruiseState.enabled = ret.cruiseState.available
elif not self.CP.radarOffCan:
ret.cruiseState.available = (cp_scc.vl["SCC11"]["MainMode_ACC"] !=
0)
ret.cruiseState.enabled = (cp_scc.vl["SCC12"]['ACCMode'] != 0)
self.lead_distance = cp_scc.vl["SCC11"]['ACC_ObjDist']
self.vrelative = cp_scc.vl["SCC11"]['ACC_ObjRelSpd']
self.radar_obj_valid = cp_scc.vl["SCC11"]['ObjValid']
ret.cruiseState.standstill = cp_scc.vl["SCC11"]['SCCInfoDisplay'] == 4.
self.is_set_speed_in_mph = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]
self.acc_active = ret.cruiseState.enabled
speed_kph = self.SC.update_cruiseSW(self)
if ret.cruiseState.enabled and (self.brake_check == 0
or self.mainsw_check == 0):
speed_conv = CV.MPH_TO_MS if self.is_set_speed_in_mph else CV.KPH_TO_MS
if self.CP.radarOffCan:
ret.cruiseState.speed = (cp.vl["LVR12"]["CF_Lvr_CruiseSet"] *
speed_conv) + 1
else:
ret.cruiseState.speed = cp_scc.vl["SCC11"][
'VSetDis'] * speed_conv
else:
ret.cruiseState.speed = 0
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
ret.cruiseButtons = self.cruise_buttons
if self.cruise_main_button != 0:
self.mainsw_check = 1
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
self.brakeUnavailable = cp.vl["TCS13"]['ACCEnable'] == 3
if ret.brakePressed:
self.brake_check = 1
# TODO: Check this
ret.brakeLights = bool(cp.vl["TCS13"]['BrakeLight']
or ret.brakePressed)
if self.CP.carFingerprint in ELEC_VEH:
ret.gas = cp.vl["E_EMS11"]['Accel_Pedal_Pos'] / 256.
elif self.CP.carFingerprint in HYBRID_VEH:
ret.gas = cp.vl["EV_PC4"]['CR_Vcu_AccPedDep_Pc']
elif self.CP.emsAvailable:
ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100
ret.gasPressed = (cp.vl["TCS13"]["DriverOverride"] == 1)
if self.CP.emsAvailable:
ret.gasPressed = ret.gasPressed or bool(
cp.vl["EMS16"]["CF_Ems_AclAct"])
ret.espDisabled = (cp.vl["TCS15"]['ESC_Off_Step'] != 0)
self.parkBrake = (cp.vl["CGW1"]['CF_Gway_ParkBrakeSw'] != 0)
#TPMS
if cp.vl["TPMS11"]['PRESSURE_FL'] > 43:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL'] * 5 * 0.145
else:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL']
if cp.vl["TPMS11"]['PRESSURE_FR'] > 43:
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR'] * 5 * 0.145
else:
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR']
if cp.vl["TPMS11"]['PRESSURE_RL'] > 43:
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL'] * 5 * 0.145
else:
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL']
if cp.vl["TPMS11"]['PRESSURE_RR'] > 43:
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR'] * 5 * 0.145
else:
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR']
ret.cruiseGapSet = self.cruise_gap
if self.read_distance_lines != self.cruise_gap:
self.read_distance_lines = self.cruise_gap
msg_df = messaging.new_message('dynamicFollowButton')
msg_df.dynamicFollowButton.status = max(
self.read_distance_lines - 1, 0)
self.pm.send('dynamicFollowButton', msg_df)
# TODO: refactor gear parsing in function
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
ret.gearShifter = GearShifter.drive
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
ret.gearShifter = GearShifter.neutral
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
ret.gearShifter = GearShifter.park
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton via TCU12
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
if gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 14:
ret.gearShifter = GearShifter.reverse
elif gear > 0 and gear < 9: # unaware of anything over 8 currently
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton - This is only compatible with optima hybrid 2017
elif self.CP.evgearAvailable:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
if gear in (5, 8): # 5: D, 8: sport mode
ret.gearShifter = GearShifter.drive
elif gear == 6:
ret.gearShifter = GearShifter.neutral
elif gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 7:
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
elif self.CP.lvrAvailable:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if gear in (5, 8): # 5: D, 8: sport mode
ret.gearShifter = GearShifter.drive
elif gear == 6:
ret.gearShifter = GearShifter.neutral
elif gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 7:
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.unknown
if self.CP.fcaBus != -1:
ret.stockAeb = cp_fca.vl["FCA11"]['FCA_CmdAct'] != 0
ret.stockFcw = cp_fca.vl["FCA11"]['CF_VSM_Warn'] == 2
elif not self.CP.radarOffCan:
ret.stockAeb = cp_scc.vl["SCC12"]['AEB_CmdAct'] != 0
ret.stockFcw = cp_scc.vl["SCC12"]['CF_VSM_Warn'] == 2
if self.CP.bsmAvailable:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
self.lkasstate = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"]
self.lkasbutton = self.lkasstate != self.prev_lkasstate and (
self.lkasstate == 0 or self.prev_lkasstate == 0)
# save the entire LKAS11, CLU11, SCC12 and MDPS12
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.scc11 = copy.copy(cp_scc.vl["SCC11"])
self.scc12 = copy.copy(cp_scc.vl["SCC12"])
self.scc13 = copy.copy(cp_scc.vl["SCC13"])
self.scc14 = copy.copy(cp_scc.vl["SCC14"])
self.fca11 = copy.copy(cp_fca.vl["FCA11"])
self.mdps12 = copy.copy(cp_mdps.vl["MDPS12"])
self.scc11init = copy.copy(cp.vl["SCC11"])
self.scc12init = copy.copy(cp.vl["SCC12"])
self.fca11init = copy.copy(cp.vl["FCA11"])
return ret
@staticmethod
def get_can_parser(CP):
checks = []
signals = [
# sig_name, sig_address, default
("WHL_SPD_FL", "WHL_SPD11", 0),
("WHL_SPD_FR", "WHL_SPD11", 0),
("WHL_SPD_RL", "WHL_SPD11", 0),
("WHL_SPD_RR", "WHL_SPD11", 0),
("YAW_RATE", "ESP12", 0),
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door
("CF_Gway_TSigLHSw", "CGW1", 0),
("CF_Gway_TurnSigLh", "CGW1", 0),
("CF_Gway_TSigRHSw", "CGW1", 0),
("CF_Gway_TurnSigRh", "CGW1", 0),
("CF_Gway_ParkBrakeSw", "CGW1", 0),
("CYL_PRES", "ESP12", 0),
("AVH_STAT", "ESP11", 0),
("CF_Clu_CruiseSwState", "CLU11", 0),
("CF_Clu_CruiseSwMain", "CLU11", 0),
("CF_Clu_SldMainSW", "CLU11", 0),
("CF_Clu_ParityBit1", "CLU11", 0),
("CF_Clu_VanzDecimal", "CLU11", 0),
("CF_Clu_Vanz", "CLU11", 0),
("CF_Clu_SPEED_UNIT", "CLU11", 0),
("CF_Clu_DetentOut", "CLU11", 0),
("CF_Clu_RheostatLevel", "CLU11", 0),
("CF_Clu_CluInfo", "CLU11", 0),
("CF_Clu_AmpInfo", "CLU11", 0),
("CF_Clu_AliveCnt1", "CLU11", 0),
("ACCEnable", "TCS13", 0),
("BrakeLight", "TCS13", 0),
("DriverBraking", "TCS13", 0),
("DriverOverride", "TCS13", 0),
("ESC_Off_Step", "TCS15", 0),
("CF_Lvr_CruiseSet", "LVR12", 0),
("CRUISE_LAMP_M", "EMS16", 0),
("CR_VSM_Alive", "SCC12", 0),
("AliveCounterACC", "SCC11", 0),
("CR_FCA_Alive", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
("PRESSURE_FL", "TPMS11", 0),
("PRESSURE_FR", "TPMS11", 0),
("PRESSURE_RL", "TPMS11", 0),
("PRESSURE_RR", "TPMS11", 0),
]
checks = [
# address, frequency
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("ESP12", 100),
("CGW1", 10),
("CGW4", 5),
("WHL_SPD11", 50),
]
if CP.sccBus == 0:
signals += [
("MainMode_ACC", "SCC11", 0),
("VSetDis", "SCC11", 0),
("SCCInfoDisplay", "SCC11", 0),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjDist", "SCC11", 0),
("ObjValid", "SCC11", 0),
("ACC_ObjRelSpd", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("ACCMode", "SCC12", 1),
("AEB_CmdAct", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
]
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.fcaBus == 0:
signals += [
("FCA_CmdAct", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
("CR_FCA_Alive", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
]
checks += [("FCA11", 50)]
if not CP.mdpsHarness:
signals += [("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)]
checks += [("MDPS12", 50)]
if CP.sasBus == 0:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [("SAS11", 100)]
if CP.bsmAvailable:
signals += [
("CF_Lca_IndLeft", "LCA11", 0),
("CF_Lca_IndRight", "LCA11", 0),
]
checks += [("LCA11", 50)]
if CP.carFingerprint in ELEC_VEH:
signals += [
("Accel_Pedal_Pos", "E_EMS11", 0),
]
checks += [
("E_EMS11", 50),
]
elif CP.carFingerprint in HYBRID_VEH:
signals += [
("CR_Vcu_AccPedDep_Pc", "EV_PC4", 0),
]
checks += [
("EV_PC4", 50),
]
elif CP.emsAvailable:
signals += [
("PV_AV_CAN", "EMS12", 0),
("CF_Ems_AclAct", "EMS16", 0),
]
checks += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
("CF_Clu_InhibitD", "CLU15", 0),
("CF_Clu_InhibitP", "CLU15", 0),
("CF_Clu_InhibitN", "CLU15", 0),
("CF_Clu_InhibitR", "CLU15", 0),
]
checks += [("CLU15", 5)]
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
signals += [("CUR_GR", "TCU12", 0)]
checks += [("TCU12", 100)]
elif CP.evgearAvailable:
signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)]
checks += [("ELECT_GEAR", 20)]
elif CP.lvrAvailable:
signals += [("CF_Lvr_Gear", "LVR12", 0)]
checks += [("LVR12", 100)]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@staticmethod
def get_can2_parser(CP):
signals = []
checks = []
if CP.mdpsHarness:
signals += [("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)]
checks += [("MDPS12", 50)]
if CP.sasBus == 1:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [("SAS11", 100)]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("CF_Lkas_LdwsActivemode", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
("CF_Lkas_HbaLamp", "LKAS11", 0),
("CF_Lkas_FcwBasReq", "LKAS11", 0),
("CF_Lkas_ToiFlt", "LKAS11", 0),
("CF_Lkas_HbaSysState", "LKAS11", 0),
("CF_Lkas_FcwOpt", "LKAS11", 0),
("CF_Lkas_HbaOpt", "LKAS11", 0),
("CF_Lkas_FcwSysState", "LKAS11", 0),
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
("CF_Lkas_MsgCount", "LKAS11", 0),
("CF_Lkas_FusionState", "LKAS11", 0),
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
]
checks = [("LKAS11", 100)]
if CP.sccBus == 2 or CP.radarOffCan:
signals += [
("MainMode_ACC", "SCC11", 0),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 0),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150.),
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("ACCMode", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0),
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0),
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 1),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("Lead_Veh_Dep_Alert_USM", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
("ACCMode", "SCC14", 0),
("ObjGap", "SCC14", 0),
("CF_VSM_Prefill", "FCA11", 0),
("CF_VSM_HBACmd", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
("CF_VSM_BeltCmd", "FCA11", 0),
("CR_VSM_DecCmd", "FCA11", 0),
("FCA_Status", "FCA11", 2),
("FCA_CmdAct", "FCA11", 0),
("FCA_StopReq", "FCA11", 0),
("FCA_DrvSetStatus", "FCA11", 1),
("CF_VSM_DecCmdAct", "FCA11", 0),
("FCA_Failinfo", "FCA11", 0),
("FCA_RelativeVelocity", "FCA11", 0),
("FCA_TimetoCollision", "FCA11", 2540.),
("CR_FCA_Alive", "FCA11", 0),
("CR_FCA_ChkSum", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
("PAINT1_Status", "FCA11", 1),
]
if CP.sccBus == 2:
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.fcaBus == 2:
checks += [("FCA11", 50)]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)