from collections import deque import copy from cereal import car from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine from selfdrive.car.hyundai.values import HyundaiFlags, DBC, FEATURES, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams from selfdrive.car.interfaces import CarStateBase PREV_BUTTON_SAMPLES = 8 class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) if CP.carFingerprint in CANFD_CAR: self.shifter_values = can_define.dv["GEAR_SHIFTER"]["GEAR"] elif 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"] self.brake_error = False self.park_brake = False self.buttons_counter = 0 self.params = CarControllerParams(CP) def update(self, cp, cp_cam): if self.CP.carFingerprint in CANFD_CAR: return self.update_canfd(cp, cp_cam) ret = car.CarState.new_message() cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp 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 = self.get_wheel_speeds( cp.vl["WHL_SPD11"]["WHL_SPD_FL"], cp.vl["WHL_SPD11"]["WHL_SPD_FR"], cp.vl["WHL_SPD11"]["WHL_SPD_RL"], cp.vl["WHL_SPD11"]["WHL_SPD_RR"], ) 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.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"] ret.steeringRateDeg = cp.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.vl["MDPS12"]["CR_Mdps_StrColTq"] ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"] ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0 # cruise state if self.CP.openpilotLongitudinalControl: # These are not used for engage/disengage since openpilot keeps track of state using the buttons ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1 ret.cruiseState.standstill = False else: ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1 ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0 ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4. speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv # TODO: Find brake pressure ret.brake = 0 ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0 ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1 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"]) # 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"] ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear)) if not self.CP.openpilotLongitudinalControl: if self.CP.carFingerprint in FEATURES["use_fca"]: ret.stockAeb = cp_cruise.vl["FCA11"]["FCA_CmdAct"] != 0 ret.stockFcw = cp_cruise.vl["FCA11"]["CF_VSM_Warn"] == 2 else: ret.stockAeb = cp_cruise.vl["SCC12"]["AEB_CmdAct"] != 0 ret.stockFcw = cp_cruise.vl["SCC12"]["CF_VSM_Warn"] == 2 if self.CP.enableBsm: ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0 ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0 # save the entire LKAS11 and CLU11 self.lkas11 = copy.copy(cp_cam.vl["LKAS11"]) self.clu11 = copy.copy(cp.vl["CLU11"]) self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) return ret def update_canfd(self, cp, cp_cam): ret = car.CarState.new_message() if self.CP.flags & HyundaiFlags.CANFD_HDA2: ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255. else: ret.gas = cp.vl["ACCELERATOR_ALT"]["ACCELERATOR_PEDAL"] / 1023. ret.gasPressed = ret.gas > 1e-5 ret.brakePressed = cp.vl["BRAKE"]["BRAKE_PRESSED"] == 1 ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR_OPEN"] == 1 ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT_LATCHED"] == 0 gear = cp.vl["GEAR_SHIFTER"]["GEAR"] ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear)) # TODO: figure out positions ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"], cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"], cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"], cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"], ) 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.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"] ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1 ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"] ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"] ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"], cp.vl["BLINKERS"]["RIGHT_LAMP"]) ret.cruiseState.available = True ret.cruiseState.enabled = cp.vl["SCC1"]["CRUISE_ACTIVE"] == 1 cp_cruise_info = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam speed_factor = CV.MPH_TO_MS if cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] == 1 else CV.KPH_TO_MS ret.cruiseState.speed = cp_cruise_info.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor ret.cruiseState.standstill = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1 cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"]) self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"]) self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"] self.cruise_info_copy = copy.copy(cp_cruise_info.vl["CRUISE_INFO"]) if self.CP.flags & HyundaiFlags.CANFD_HDA2: self.cam_0x2a4 = copy.copy(cp_cam.vl["CAM_0x2a4"]) return ret @staticmethod def get_can_parser(CP): if CP.carFingerprint in CANFD_CAR: return CarState.get_can_parser_canfd(CP) signals = [ # signal_name, signal_address ("WHL_SPD_FL", "WHL_SPD11"), ("WHL_SPD_FR", "WHL_SPD11"), ("WHL_SPD_RL", "WHL_SPD11"), ("WHL_SPD_RR", "WHL_SPD11"), ("YAW_RATE", "ESP12"), ("CF_Gway_DrvSeatBeltInd", "CGW4"), ("CF_Gway_DrvSeatBeltSw", "CGW1"), ("CF_Gway_DrvDrSw", "CGW1"), # Driver Door ("CF_Gway_AstDrSw", "CGW1"), # Passenger Door ("CF_Gway_RLDrSw", "CGW2"), # Rear left Door ("CF_Gway_RRDrSw", "CGW2"), # Rear right Door ("CF_Gway_TurnSigLh", "CGW1"), ("CF_Gway_TurnSigRh", "CGW1"), ("CF_Gway_ParkBrakeSw", "CGW1"), ("CYL_PRES", "ESP12"), ("CF_Clu_CruiseSwState", "CLU11"), ("CF_Clu_CruiseSwMain", "CLU11"), ("CF_Clu_SldMainSW", "CLU11"), ("CF_Clu_ParityBit1", "CLU11"), ("CF_Clu_VanzDecimal" , "CLU11"), ("CF_Clu_Vanz", "CLU11"), ("CF_Clu_SPEED_UNIT", "CLU11"), ("CF_Clu_DetentOut", "CLU11"), ("CF_Clu_RheostatLevel", "CLU11"), ("CF_Clu_CluInfo", "CLU11"), ("CF_Clu_AmpInfo", "CLU11"), ("CF_Clu_AliveCnt1", "CLU11"), ("ACCEnable", "TCS13"), ("ACC_REQ", "TCS13"), ("DriverBraking", "TCS13"), ("StandStill", "TCS13"), ("PBRAKE_ACT", "TCS13"), ("ESC_Off_Step", "TCS15"), ("AVH_LAMP", "TCS15"), ("CR_Mdps_StrColTq", "MDPS12"), ("CF_Mdps_ToiActive", "MDPS12"), ("CF_Mdps_ToiUnavail", "MDPS12"), ("CF_Mdps_ToiFlt", "MDPS12"), ("CR_Mdps_OutTq", "MDPS12"), ("SAS_Angle", "SAS11"), ("SAS_Speed", "SAS11"), ] checks = [ # address, frequency ("MDPS12", 50), ("TCS13", 50), ("TCS15", 10), ("CLU11", 50), ("ESP12", 100), ("CGW1", 10), ("CGW2", 5), ("CGW4", 5), ("WHL_SPD11", 50), ("SAS11", 100), ] if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR: signals += [ ("MainMode_ACC", "SCC11"), ("VSetDis", "SCC11"), ("SCCInfoDisplay", "SCC11"), ("ACC_ObjDist", "SCC11"), ("ACCMode", "SCC12"), ] checks += [ ("SCC11", 50), ("SCC12", 50), ] if CP.carFingerprint in FEATURES["use_fca"]: signals += [ ("FCA_CmdAct", "FCA11"), ("CF_VSM_Warn", "FCA11"), ] checks.append(("FCA11", 50)) else: signals += [ ("AEB_CmdAct", "SCC12"), ("CF_VSM_Warn", "SCC12"), ] if CP.enableBsm: signals += [ ("CF_Lca_IndLeft", "LCA11"), ("CF_Lca_IndRight", "LCA11"), ] checks.append(("LCA11", 50)) if CP.carFingerprint in (HYBRID_CAR | EV_CAR): if CP.carFingerprint in HYBRID_CAR: signals.append(("CR_Vcu_AccPedDep_Pos", "E_EMS11")) else: signals.append(("Accel_Pedal_Pos", "E_EMS11")) checks.append(("E_EMS11", 50)) else: signals += [ ("PV_AV_CAN", "EMS12"), ("CF_Ems_AclAct", "EMS16"), ] checks += [ ("EMS12", 100), ("EMS16", 100), ] if CP.carFingerprint in FEATURES["use_cluster_gears"]: signals.append(("CF_Clu_Gear", "CLU15")) checks.append(("CLU15", 5)) elif CP.carFingerprint in FEATURES["use_tcu_gears"]: signals.append(("CUR_GR", "TCU12")) checks.append(("TCU12", 100)) elif CP.carFingerprint in FEATURES["use_elect_gears"]: signals.append(("Elect_Gear_Shifter", "ELECT_GEAR")) checks.append(("ELECT_GEAR", 20)) else: signals.append(("CF_Lvr_Gear", "LVR12")) checks.append(("LVR12", 100)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod def get_cam_can_parser(CP): if CP.carFingerprint in CANFD_CAR: return CarState.get_cam_can_parser_canfd(CP) signals = [ # signal_name, signal_address ("CF_Lkas_LdwsActivemode", "LKAS11"), ("CF_Lkas_LdwsSysState", "LKAS11"), ("CF_Lkas_SysWarning", "LKAS11"), ("CF_Lkas_LdwsLHWarning", "LKAS11"), ("CF_Lkas_LdwsRHWarning", "LKAS11"), ("CF_Lkas_HbaLamp", "LKAS11"), ("CF_Lkas_FcwBasReq", "LKAS11"), ("CF_Lkas_HbaSysState", "LKAS11"), ("CF_Lkas_FcwOpt", "LKAS11"), ("CF_Lkas_HbaOpt", "LKAS11"), ("CF_Lkas_FcwSysState", "LKAS11"), ("CF_Lkas_FcwCollisionWarning", "LKAS11"), ("CF_Lkas_FusionState", "LKAS11"), ("CF_Lkas_FcwOpt_USM", "LKAS11"), ("CF_Lkas_LdwsOpt_USM", "LKAS11"), ] checks = [ ("LKAS11", 100) ] if not CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR: signals += [ ("MainMode_ACC", "SCC11"), ("VSetDis", "SCC11"), ("SCCInfoDisplay", "SCC11"), ("ACC_ObjDist", "SCC11"), ("ACCMode", "SCC12"), ] checks += [ ("SCC11", 50), ("SCC12", 50), ] if CP.carFingerprint in FEATURES["use_fca"]: signals += [ ("FCA_CmdAct", "FCA11"), ("CF_VSM_Warn", "FCA11"), ] checks.append(("FCA11", 50)) else: signals += [ ("AEB_CmdAct", "SCC12"), ("CF_VSM_Warn", "SCC12"), ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) @staticmethod def get_can_parser_canfd(CP): cruise_btn_msg = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" signals = [ ("WHEEL_SPEED_1", "WHEEL_SPEEDS"), ("WHEEL_SPEED_2", "WHEEL_SPEEDS"), ("WHEEL_SPEED_3", "WHEEL_SPEEDS"), ("WHEEL_SPEED_4", "WHEEL_SPEEDS"), ("GEAR", "GEAR_SHIFTER"), ("BRAKE_PRESSED", "BRAKE"), ("STEERING_RATE", "STEERING_SENSORS"), ("STEERING_ANGLE", "STEERING_SENSORS"), ("STEERING_COL_TORQUE", "MDPS"), ("STEERING_OUT_TORQUE", "MDPS"), ("CRUISE_ACTIVE", "SCC1"), ("COUNTER", cruise_btn_msg), ("CRUISE_BUTTONS", cruise_btn_msg), ("ADAPTIVE_CRUISE_MAIN_BTN", cruise_btn_msg), ("DISTANCE_UNIT", "CLUSTER_INFO"), ("LEFT_LAMP", "BLINKERS"), ("RIGHT_LAMP", "BLINKERS"), ("DRIVER_DOOR_OPEN", "DOORS_SEATBELTS"), ("DRIVER_SEATBELT_LATCHED", "DOORS_SEATBELTS"), ] checks = [ ("WHEEL_SPEEDS", 100), ("GEAR_SHIFTER", 100), ("BRAKE", 100), ("STEERING_SENSORS", 100), ("MDPS", 100), ("SCC1", 50), (cruise_btn_msg, 50), ("CLUSTER_INFO", 4), ("BLINKERS", 4), ("DOORS_SEATBELTS", 4), ] if CP.flags & HyundaiFlags.CANFD_HDA2: signals += [ ("ACCELERATOR_PEDAL", "ACCELERATOR"), ("GEAR", "ACCELERATOR"), ("SET_SPEED", "CRUISE_INFO"), ("CRUISE_STANDSTILL", "CRUISE_INFO"), ] checks += [ ("CRUISE_INFO", 50), ("ACCELERATOR", 100), ] else: signals += [ ("ACCELERATOR_PEDAL", "ACCELERATOR_ALT"), ] checks += [ ("ACCELERATOR_ALT", 100), ] bus = 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4 return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, bus) @staticmethod def get_cam_can_parser_canfd(CP): if CP.flags & HyundaiFlags.CANFD_HDA2: signals = [(f"BYTE{i}", "CAM_0x2a4") for i in range(3, 24)] checks = [("CAM_0x2a4", 20)] else: signals = [ ("COUNTER", "CRUISE_INFO"), ("NEW_SIGNAL_1", "CRUISE_INFO"), ("CRUISE_MAIN", "CRUISE_INFO"), ("CRUISE_STATUS", "CRUISE_INFO"), ("CRUISE_INACTIVE", "CRUISE_INFO"), ("NEW_SIGNAL_2", "CRUISE_INFO"), ("CRUISE_STANDSTILL", "CRUISE_INFO"), ("NEW_SIGNAL_3", "CRUISE_INFO"), ("BYTE11", "CRUISE_INFO"), ("SET_SPEED", "CRUISE_INFO"), ("NEW_SIGNAL_4", "CRUISE_INFO"), ] signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(3, 7)] signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(13, 31)] checks = [ ("CRUISE_INFO", 50), ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6)