oscarpilot/selfdrive/car/hyundai org/carstate.org

from collections import deque import copy import math

from cereal import car from openpilot.common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \ CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController

from openpilot.common.watcher import Watcher

PREV_BUTTON_SAMPLES = 8 CLUSTER_SAMPLE_RATE = 20 # frames STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS

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)

self.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \ "GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \ "GEAR_SHIFTER" if CP.carFingerprint in CANFD_CAR: self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"] elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]: self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"] elif self.CP.carFingerprint in CAN_GEARS["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.accelerator_msg_canfd = "ACCELERATOR" if CP.carFingerprint in EV_CAR else \ "ACCELERATOR_ALT" if CP.carFingerprint in HYBRID_CAR else \ "ACCELERATOR_BRAKE_ALT" self.cruise_btns_msg_canfd = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else \ "CRUISE_BUTTONS" self.is_metric = False self.buttons_counter = 0

self.cruise_info = {}

self.cluster_speed = 0 self.cluster_speed_counter = CLUSTER_SAMPLE_RATE

self.params = CarControllerParams(CP)

def update(self, cp, cp_cam): Watcher.log_watch("hyundai_carstate_update_cp", cp) Watcher.log_watch("hyundai_carstate_update_cp_cam", 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 self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0 speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS

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.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD

self.cluster_speed_counter += 1 if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE: self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] self.cluster_speed_counter = 0

if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,): self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)

ret.vEgoCluster = self.cluster_speed * speed_conv

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 = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0

if self.CP.openpilotLongitudinalControl:

ret.cruiseState.available = self.main_enabled ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1 ret.cruiseState.standstill = False ret.cruiseState.nonAdaptive = 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. ret.cruiseState.nonAdaptive = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] = 2. # Shows 'Cruise Control' on dash ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv

ret.brake = 0 ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] = 2 # 2 includes regen braking by user on HEV/EV ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] = 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] = 1 ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] ! 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED

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"])

if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR): gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"] elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]: gear = cp.vl["CLU15"]["CF_Clu_Gear"] elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]: gear = cp.vl["TCU12"]["CUR_GR"] else: gear = cp.vl["LVR12"]["CF_Lvr_Gear"]

ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))

if not self.CP.openpilotLongitudinalControl: aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "SCC12" aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct" aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0 scc_warning = cp_cruise.vl["SCC12"]["TakeOverReq"] = 1 # sometimes only SCC system shows an FCW aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] ! 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0 ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking ret.stockAeb = aeb_warning and aeb_braking

if self.CP.enableBsm: ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0 ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0

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.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) self.prev_main_buttons = self.main_buttons[-1] self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) if self.prev_main_buttons = 0 and self.main_buttons[-1] ! 0: self.main_enabled = not self.main_enabled

self.oscar_lane_center_btn_pressed= False self.oscar_slc_decel = False self.oscar_slc_accel = False

self.param_memory.put("oscar_debug", "Hello World")

Watcher.log_watch("hyundai_carstate_update_self", self) return ret

def update_canfd(self, cp, cp_cam): Watcher.log_watch("hyundai_carstate_update_canfd_cp", cp) Watcher.log_watch("hyundai_carstate_update_canfd_cp_cam", cp_cam)

ret = car.CarState.new_message()

self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1 speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS

if self.CP.carFingerprint in (EV_CAR | HYBRID_CAR): offset = 255. if self.CP.carFingerprint in EV_CAR else 1023. ret.gas = cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL"] / offset ret.gasPressed = ret.gas > 1e-5 else: ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"])

ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1

ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR"] = 1 ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT"] = 0

gear = cp.vl[self.gear_msg_canfd]["GEAR"] ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))

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.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD

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 = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0

left_blinker_sig, right_blinker_sig = "LEFT_LAMP", "RIGHT_LAMP" if self.CP.carFingerprint == CAR.KONA_EV_2ND_GEN: left_blinker_sig, right_blinker_sig = "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT" ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"][left_blinker_sig], cp.vl["BLINKERS"][right_blinker_sig]) if self.CP.enableBsm: ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0 ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0

ret.cruiseState.available = self.main_enabled if self.CP.openpilotLongitudinalControl:

ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1 ret.cruiseState.standstill = False else: cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2) ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1 ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])

if self.CP.carFingerprint in EV_CAR: ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1

self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]) self.prev_main_buttons = self.main_buttons[-1] self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"]) if self.prev_main_buttons = 0 and self.main_buttons[-1] ! 0: self.main_enabled = not self.main_enabled self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"] ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED

if self.CP.flags & HyundaiFlags.CANFD_HDA2: self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else cp_cam.vl["CAM_0x2a4"])

SpeedLimitController.load_state() SpeedLimitController.car_speed_limit = self.calculate_speed_limit_canfd(self.CP, cp, cp_cam) * speed_factor SpeedLimitController.write_car_state()

self.oscar_lane_center_btn_pressed = False

if ret.cruiseState.available:

if self.cruise_buttons[-1] = Buttons.GAP_DIST and self.prev_cruise_buttons = 0: self.oscar_lane_center_btn_pressed = True

lkas_pressed = False try: lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"] except: nothing = 0

if lkas_pressed: floog=norpdywoop

Watcher.log_watch("hyundai_carstate_update_canfd_self", self)

return ret

def calculate_speed_limit_canfd(self, CP, cp, cp_cam): try: self._speed_limit_clu = cp.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"] return self._speed_limit_clu if self._speed_limit_clu not in (0, 255) else 0 except: return 0

def get_can_parser(self, CP): if CP.carFingerprint in CANFD_CAR: return self.get_can_parser_canfd(CP)

messages = [

("MDPS12", 50), ("TCS13", 50), ("TCS15", 10), ("CLU11", 50), ("CLU15", 5), ("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: messages += [ ("SCC11", 50), ("SCC12", 50), ] if CP.flags & HyundaiFlags.USE_FCA.value: messages.append(("FCA11", 50))

if CP.enableBsm: messages.append(("LCA11", 50))

if CP.carFingerprint in (HYBRID_CAR | EV_CAR): messages.append(("E_EMS11", 50)) else: messages += [ ("EMS12", 100), ("EMS16", 100), ]

if CP.carFingerprint in (HYBRID_CAR | EV_CAR): messages.append(("ELECT_GEAR", 20)) elif CP.carFingerprint in CAN_GEARS["use_cluster_gears"]: pass elif CP.carFingerprint in CAN_GEARS["use_tcu_gears"]: messages.append(("TCU12", 100)) else: messages.append(("LVR12", 100))

messages.append(("BCM_PO_11", 50))

return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)

@staticmethod def get_cam_can_parser(CP): if CP.carFingerprint in CANFD_CAR: return CarState.get_cam_can_parser_canfd(CP)

messages = [ ("LKAS11", 100) ]

if not CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR: messages += [ ("SCC11", 50), ("SCC12", 50), ]

if CP.flags & HyundaiFlags.USE_FCA.value: messages.append(("FCA11", 50))

return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)

def get_can_parser_canfd(self, CP): messages = [ (self.gear_msg_canfd, 100), (self.accelerator_msg_canfd, 100), ("WHEEL_SPEEDS", 100), ("STEERING_SENSORS", 100), ("MDPS", 100), ("TCS", 50), ("CRUISE_BUTTONS_ALT", 50), ("BLINKERS", 4), ("DOORS_SEATBELTS", 4), ]

if CP.carFingerprint in EV_CAR: messages += [ ("MANUAL_SPEED_LIMIT_ASSIST", 10), ]

if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS): messages += [ ("CRUISE_BUTTONS", 50) ]

if CP.enableBsm: messages += [ ("BLINDSPOTS_REAR_CORNERS", 20), ]

if not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and not CP.openpilotLongitudinalControl: messages += [ ("SCC_CONTROL", 50), ]

return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)

@staticmethod def get_cam_can_parser_canfd(CP): messages = [] if CP.flags & HyundaiFlags.CANFD_HDA2: block_lfa_msg = "CAM_0x362" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "CAM_0x2a4" messages += [(block_lfa_msg, 20)] elif CP.flags & HyundaiFlags.CANFD_CAMERA_SCC: messages += [ ("SCC_CONTROL", 50), ]

return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)