From 21589861fda8d7c5fcd6e40b19b603cb5a338c62 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 11 May 2026 17:50:09 -0500 Subject: [PATCH] huge if work --- .../opendbc/car/hyundai/carcontroller.py | 80 ++++++++- opendbc_repo/opendbc/car/hyundai/carstate.py | 22 +++ .../opendbc/car/hyundai/hyundaican.py | 7 +- .../opendbc/car/hyundai/hyundaicanfd.py | 39 +++- .../opendbc/car/hyundai/tests/test_hyundai.py | 168 +++++++++++++++++- panda/board/obj/gitversion.h | 2 +- panda/board/obj/version | 2 +- selfdrive/car/card.py | 24 ++- selfdrive/ui/lib/starpilot_status.py | 2 + 9 files changed, 324 insertions(+), 22 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 6f69c8943..f6a385122 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -22,6 +22,10 @@ MAX_ANGLE_FRAMES = 89 MAX_ANGLE_CONSECUTIVE_FRAMES = 2 CANFD_BLINDSPOT_STATUS_STALE_NS = 200_000_000 CANFD_BLINKER_STALKS_STALE_NS = 200_000_000 +CANFD_CAMERA_LEAD_STALE_NS = 300_000_000 +CANFD_LEAD_MIN_DISTANCE = 0.1 +CANFD_FALLBACK_LEAD_DISTANCE = 20.0 +HYUNDAI_DASH_DISENGAGE_BLINK_TIME = 1.0 HYUNDAI_CANFD_SCC_ACCEL_STEP = 5.0 / 50.0 HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0 IONIQ_6_RESPONSE_MULTIPLIER = 1.2 @@ -62,6 +66,19 @@ IONIQ_6_IPEDAL_REGEN_STATE = 0x50 IONIQ_6_IPEDAL_REGEN_STATE_2_PENDING = 0x01 IONIQ_6_IPEDAL_PROGRESS_RETRY_WAIT_FRAMES = 10 IONIQ_6_IPEDAL_RETRY_WAIT_FRAMES = 30 + + +def get_canfd_lead_distance_setting(lead_distance: float | None, default_setting: int) -> int: + default = int(np.clip(default_setting, 0, 3)) + if lead_distance is None or lead_distance <= CANFD_LEAD_MIN_DISTANCE: + return default + if lead_distance < 20.0: + return 1 + if lead_distance < 30.0: + return 2 + return 3 + + @dataclass class Ioniq6LongitudinalTuningState: desired_accel: float = 0.0 @@ -248,6 +265,46 @@ class CarController(CarControllerBase): self._ioniq_6_ipedal_latch_counter = -1 self._ioniq_6_last_gear = structs.CarState.GearShifter.unknown self._genesis_g90_long_tuning = GenesisG90LongitudinalTuningState() + self._dash_lat_disengage_blink_frame = 0 + self._dash_lat_disengage_init = False + self._dash_prev_lat_active = False + + def _update_dash_icon_state(self, CC): + if CC.latActive: + self._dash_lat_disengage_init = False + elif self._dash_prev_lat_active: + self._dash_lat_disengage_init = True + + if not self._dash_lat_disengage_init: + self._dash_lat_disengage_blink_frame = self.frame + + disengaging = self._dash_lat_disengage_init and \ + (self.frame - self._dash_lat_disengage_blink_frame) * DT_CTRL < HYUNDAI_DASH_DISENGAGE_BLINK_TIME + self._dash_prev_lat_active = CC.latActive + lat_or_enabled = CC.enabled or CC.latActive + lka_icon = 2 if lat_or_enabled else 3 if disengaging else 1 + lfa_icon = 2 if lat_or_enabled else 3 if disengaging else 0 + + return lka_icon, lfa_icon + + def _get_canfd_scc_lead_state(self, CC, CS, now_nanos): + default_distance_setting = int(np.clip(getattr(CC.hudControl, "leadDistanceBars", 0), 0, 3)) + openpilot_lead_visible = bool(getattr(CS, "openpilot_lead_visible", False) or CC.hudControl.leadVisible) + openpilot_lead_distance = float(np.clip(getattr(CS, "openpilot_lead_distance", 0.0), 0.0, 204.7)) + openpilot_lead_rel_speed = float(np.clip(getattr(CS, "openpilot_lead_rel_speed", 0.0), -16.4, 34.7)) + stock_camera_lead_fresh = now_nanos - getattr(CS, "stock_camera_lead_ts", 0) <= CANFD_CAMERA_LEAD_STALE_NS + stock_camera_lead_visible = stock_camera_lead_fresh and getattr(CS, "stock_camera_lead_visible", False) + + if openpilot_lead_visible and openpilot_lead_distance > CANFD_LEAD_MIN_DISTANCE: + return True, openpilot_lead_distance, openpilot_lead_rel_speed, get_canfd_lead_distance_setting(openpilot_lead_distance, default_distance_setting) + if stock_camera_lead_visible: + lead_distance = float(np.clip(getattr(CS, "stock_camera_lead_distance", 0.0), 0.0, 204.7)) + lead_rel_speed = float(np.clip(getattr(CS, "stock_camera_lead_rel_speed", 0.0), -16.4, 34.7)) + return True, lead_distance, lead_rel_speed, get_canfd_lead_distance_setting(lead_distance, default_distance_setting) + if openpilot_lead_visible: + return True, CANFD_FALLBACK_LEAD_DISTANCE, 0.0, default_distance_setting + + return False, 0.0, 0.0, default_distance_setting def _reset_ioniq_6_always_ipedal(self) -> None: self._ioniq_6_always_ipedal_pending = False @@ -372,6 +429,7 @@ class CarController(CarControllerBase): def update(self, CC, CS, now_nanos, starpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl + lka_icon, lfa_icon = self._update_dash_icon_state(CC) self.params = CarControllerParams(self.CP, CS.out.vEgoRaw) apply_angle = CS.out.steeringAngleDeg @@ -486,10 +544,10 @@ class CarController(CarControllerBase): # *** CAN/CAN FD specific *** if self.CP.flags & HyundaiFlags.CANFD: can_sends.extend(self.create_canfd_msgs(now_nanos, apply_steer_req, apply_torque, apply_angle, set_speed_in_units, accel, - stopping, hud_control, CS, CC, starpilot_toggles)) + stopping, hud_control, CS, CC, starpilot_toggles, lka_icon, lfa_icon)) else: can_sends.extend(self.create_can_msgs(apply_steer_req, apply_torque, torque_fault, set_speed_in_units, accel, - stopping, hud_control, actuators, CS, CC)) + stopping, hud_control, actuators, CS, CC, lfa_icon)) new_actuators = actuators.as_builder() if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: @@ -504,7 +562,7 @@ class CarController(CarControllerBase): self.frame += 1 return new_actuators, can_sends - def create_can_msgs(self, apply_steer_req, apply_torque, torque_fault, set_speed_in_units, accel, stopping, hud_control, actuators, CS, CC): + def create_can_msgs(self, apply_steer_req, apply_torque, torque_fault, set_speed_in_units, accel, stopping, hud_control, actuators, CS, CC, lfa_icon): can_sends = [] can_canfd_blended = bool(self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED) @@ -554,7 +612,7 @@ class CarController(CarControllerBase): # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: - can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, self.frame, self.CP)) + can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, self.frame, self.CP, lfa_icon)) # 5 Hz ACC options if self.frame % 20 == 0 and self.long_active_ecu and not can_canfd_blended: @@ -566,7 +624,8 @@ class CarController(CarControllerBase): return can_sends - def create_canfd_msgs(self, now_nanos, apply_steer_req, apply_torque, apply_angle, set_speed_in_units, accel, stopping, hud_control, CS, CC, starpilot_toggles): + def create_canfd_msgs(self, now_nanos, apply_steer_req, apply_torque, apply_angle, set_speed_in_units, accel, stopping, + hud_control, CS, CC, starpilot_toggles, lka_icon, lfa_icon): can_sends = [] lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING @@ -580,7 +639,8 @@ class CarController(CarControllerBase): can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, apply_angle, CS.stock_lfa_msg, - CS.stock_lkas_msg if preserve_stock_lkas else None)) + CS.stock_lkas_msg if preserve_stock_lkas else None, + lka_icon=lka_icon)) # prevent LFA from activating on LKA steering cars by sending "no lane lines detected" to ADAS ECU if self.frame % 5 == 0 and lka_steering: @@ -589,7 +649,8 @@ class CarController(CarControllerBase): # LFA and HDA icons if self.frame % 5 == 0 and (not lka_steering or lka_steering_long): - can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled, CS.stock_lfahda_cluster_msg)) + can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled, CS.stock_lfahda_cluster_msg, + lfa_icon=lfa_icon)) # blinkers if lka_steering and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: @@ -642,11 +703,16 @@ class CarController(CarControllerBase): CC.leftBlinker, CC.rightBlinker)) if self.frame % 2 == 0: + lead_visible, lead_distance, lead_rel_speed, lead_distance_setting = self._get_canfd_scc_lead_state(CC, CS, now_nanos) acc_kwargs = { "main_mode_acc": int(CS.out.cruiseState.available), "direct_accel": True, "jerk_lower": 5.0, "jerk_upper": 3.0 if CC.actuators.longControlState == LongCtrlState.pid else 1.0, + "distance_setting": lead_distance_setting, + "lead_distance": lead_distance, + "lead_rel_speed": lead_rel_speed, + "lead_visible": lead_visible, } if self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6: if use_ioniq_6_smoothed_accel: diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 927581adc..e7dda05bd 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -30,6 +30,7 @@ IONIQ_6_IPEDAL_INTERMEDIATE_REGEN_STATE = 0x50 IONIQ_6_IPEDAL_INTERMEDIATE_REGEN_STATE_2 = 0x01 IONIQ_6_IPEDAL_REGEN_STATE = 0x50 IONIQ_6_IPEDAL_REGEN_STATE_2 = 0x03 +CANFD_CAMERA_LEAD_MIN_DISTANCE = 0.1 def calculate_canfd_speed_limit(CP, FPCP, cp, cp_cam, speed_factor): @@ -61,6 +62,14 @@ def decode_ioniq_6_ipedal_intermediate_state(regen_state: int, regen_state_2: in return int(regen_state) == IONIQ_6_IPEDAL_INTERMEDIATE_REGEN_STATE and int(regen_state_2) == IONIQ_6_IPEDAL_INTERMEDIATE_REGEN_STATE_2 +def decode_canfd_camera_lead(distance: float, rel_speed: float) -> tuple[bool, float, float]: + lead_distance = float(distance) + lead_visible = lead_distance > CANFD_CAMERA_LEAD_MIN_DISTANCE + if not lead_visible: + return False, 0.0, 0.0 + return True, lead_distance, float(rel_speed) + + class CarState(CarStateBase): @staticmethod def get_canfd_blinker_sig_names(car_fingerprint, use_alt_lamp: bool) -> tuple[str, str]: @@ -116,6 +125,10 @@ class CarState(CarStateBase): self.stock_lkas_msg = {} self.stock_lfa_msg = {} self.stock_lfahda_cluster_msg = {} + self.stock_camera_lead_visible = False + self.stock_camera_lead_distance = 0.0 + self.stock_camera_lead_rel_speed = 0.0 + self.stock_camera_lead_ts = 0 self.stock_blinker_stalks_ts = 0 self.blindspots_rear_corners = {} self.blindspots_front_corner_1 = {} @@ -440,6 +453,13 @@ class CarState(CarStateBase): else cp_cam.vl["CAM_0x2a4"]) if cp_cam.ts_nanos[lkas_msg]["CHECKSUM"] > 0: self.stock_lkas_msg = copy.copy(cp_cam.vl[lkas_msg]) + lead_cp = cp if self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING else cp_cam + if lead_cp.ts_nanos["FR_CMR_03_50ms"]["FR_CMR_Crc3Val"] > 0: + self.stock_camera_lead_ts = lead_cp.ts_nanos["FR_CMR_03_50ms"]["FR_CMR_Crc3Val"] + self.stock_camera_lead_visible, self.stock_camera_lead_distance, self.stock_camera_lead_rel_speed = decode_canfd_camera_lead( + lead_cp.vl["FR_CMR_03_50ms"]["Longitudinal_Distance"], + lead_cp.vl["FR_CMR_03_50ms"]["Relative_Velocity"], + ) if cp.ts_nanos["LFA"]["CHECKSUM"] > 0: self.stock_lfa_msg = copy.copy(cp.vl["LFA"]) if cp.ts_nanos["LFAHDA_CLUSTER"]["CHECKSUM"] > 0: @@ -484,9 +504,11 @@ class CarState(CarStateBase): ] if CP.flags & HyundaiFlags.CANFD_LKA_STEERING: msgs.append(("FR_CMR_02_100ms", 10)) + msgs.append(("FR_CMR_03_50ms", 0)) cam_msgs.append(("LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS", 0)) else: cam_msgs.append(("FR_CMR_02_100ms", 0)) # optional: not all non-LKA CANFD cars have this on CAM bus + cam_msgs.append(("FR_CMR_03_50ms", 0)) # optional: camera lead/cipv data is not present on every CAN-FD trim msgs += [ ("LFA", 0), # optional: may stop once OP takes over, but preserve stock UI fields when present ("LFAHDA_CLUSTER", 0), # optional: carries cluster icon state on some variants diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaican.py b/opendbc_repo/opendbc/car/hyundai/hyundaican.py index ab0c1e194..964579a45 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaican.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaican.py @@ -165,9 +165,12 @@ def create_clu11(packer, frame, clu11, button, CP): return packer.make_can_msg("CLU11", bus, values) -def create_lfahda_mfc(packer, enabled, frame=None, CP=None): +def create_lfahda_mfc(packer, enabled, frame=None, CP=None, lfa_icon=None): + if lfa_icon is None: + lfa_icon = 2 if enabled else 0 + values = { - "LFA_Icon_State": 2 if enabled else 0, + "LFA_Icon_State": lfa_icon, } can_canfd_blended = CP is not None and bool(CP.flags & HyundaiFlags.CAN_CANFD_BLENDED) bus = CanBus(CP).ECAN if can_canfd_blended else 0 diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index a2a2042eb..f627dfd26 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -92,10 +92,13 @@ def _create_angle_adas_cmd_msg(packer, CAN, apply_angle: float, lat_active: bool def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, apply_angle, - lfa_base_values=None, lkas_base_values=None): + lfa_base_values=None, lkas_base_values=None, lka_icon=None): + if lka_icon is None: + lka_icon = 2 if enabled else 1 + control_values = { "LKA_MODE": 2, - "LKA_ICON": 2 if enabled else 1, + "LKA_ICON": lka_icon, "TORQUE_REQUEST": 0 if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else apply_torque, "LKA_ASSIST": 0, "STEER_REQ": 0 if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else (1 if lat_active else 0), @@ -275,11 +278,14 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy): return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_lfahda_cluster(packer, CAN, enabled, base_values=None): +def create_lfahda_cluster(packer, CAN, enabled, base_values=None, lfa_icon=None): + if lfa_icon is None: + lfa_icon = 2 if enabled else 0 + values = {k: v for k, v in base_values.items() if k not in ("CHECKSUM", "COUNTER")} if base_values else {} values.update({ "HDA_ICON": 1 if enabled else 0, - "LFA_ICON": 2 if enabled else 0, + "LFA_ICON": lfa_icon, }) return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) @@ -397,7 +403,9 @@ def create_ioniq_6_cluster_lane_change_messages(CAN, frame, side=None, trigger=F def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, - main_mode_acc=1, jerk_lower=None, jerk_upper=None, direct_accel=False): + main_mode_acc=1, jerk_lower=None, jerk_upper=None, direct_accel=False, + distance_setting=None, + lead_distance=None, lead_rel_speed=None, lead_visible=None): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -409,6 +417,18 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov a_raw = accel a_val = np.clip(accel, accel_last - jn, accel_last + jn) + if lead_distance is None and lead_rel_speed is None and lead_visible is None: + acc_obj_dist = 1.0 + acc_obj_rel_spd = 0.0 + obj_valid = 0 + obj_status = 2 + else: + lead_visible = bool(lead_visible) + acc_obj_dist = float(np.clip(lead_distance if lead_visible else 0.0, 0.0, 204.7)) + acc_obj_rel_spd = float(np.clip(lead_rel_speed if lead_visible else 0.0, -16.4, 34.7)) + obj_valid = int(not lead_visible) + obj_status = 0 if not (enabled and lead_visible) else (1 if gas_override else 2) + values = { "ACCMode": 0 if not enabled else (2 if gas_override else 1), "MainMode_ACC": main_mode_acc, @@ -419,13 +439,14 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov "JerkLowerLimit": jerk_lower if jerk_lower is not None else (jerk if enabled else 1), "JerkUpperLimit": jerk_upper if jerk_upper is not None else 3.0, - "ACC_ObjDist": 1, - "ObjValid": 0, - "OBJ_STATUS": 2, + "ACC_ObjDist": acc_obj_dist, + "ACC_ObjRelSpd": acc_obj_rel_spd, + "ObjValid": obj_valid, + "OBJ_STATUS": obj_status, "SET_ME_2": 0x4, "SET_ME_3": 0x3, "SET_ME_TMP_64": 0x64, - "DISTANCE_SETTING": hud_control.leadDistanceBars, + "DISTANCE_SETTING": int(np.clip(hud_control.leadDistanceBars if distance_setting is None else distance_setting, 0, 3)), } return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 365a244a8..5f4f91bac 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -8,10 +8,11 @@ from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs from opendbc.car.structs import CarControl, CarParams from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \ + get_canfd_lead_distance_setting, \ IONIQ_6_IPEDAL_PADDLE_BURST_COUNT, \ update_ioniq_6_longitudinal_tuning, \ update_genesis_g90_longitudinal_tuning -from opendbc.car.hyundai.carstate import CarState, decode_ioniq_6_blindspot_radar_state, decode_ioniq_6_ipedal_intermediate_state, \ +from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, decode_ioniq_6_ipedal_intermediate_state, \ decode_ioniq_6_ipedal_state, decode_ioniq_6_max_regen_state from opendbc.car.hyundai.interface import CarInterface from opendbc.car.hyundai import hyundaican, hyundaicanfd @@ -721,6 +722,120 @@ class TestHyundaiFingerprint: assert parser.vl["SCC_CONTROL"]["JerkLowerLimit"] == pytest.approx(5.0) assert parser.vl["SCC_CONTROL"]["JerkUpperLimit"] == pytest.approx(1.0) + def test_canfd_acc_control_accepts_lead_object_override(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.KIA_EV6 + CP.flags = int(HyundaiFlags.CANFD) + + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + can_bus = CanBus(CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("SCC_CONTROL", 0)], can_bus.ECAN) + + msg = hyundaicanfd.create_acc_control(packer, can_bus, enabled=True, accel_last=0.0, accel=0.1, stopping=False, + gas_override=False, set_speed=42, hud_control=SimpleNamespace(leadDistanceBars=3), + direct_accel=True, lead_distance=27.5, lead_rel_speed=-1.2, lead_visible=True) + parser.update([(1, [msg])]) + + assert parser.can_valid + assert parser.vl["SCC_CONTROL"]["ACC_ObjDist"] == pytest.approx(27.5) + assert parser.vl["SCC_CONTROL"]["ACC_ObjRelSpd"] == pytest.approx(-1.2) + assert parser.vl["SCC_CONTROL"]["ObjValid"] == 0 + assert parser.vl["SCC_CONTROL"]["OBJ_STATUS"] == 2 + + def test_canfd_acc_control_hides_lead_object_when_not_visible(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.KIA_EV6 + CP.flags = int(HyundaiFlags.CANFD) + + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + can_bus = CanBus(CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("SCC_CONTROL", 0)], can_bus.ECAN) + + msg = hyundaicanfd.create_acc_control(packer, can_bus, enabled=True, accel_last=0.0, accel=0.1, stopping=False, + gas_override=False, set_speed=42, hud_control=SimpleNamespace(leadDistanceBars=3), + direct_accel=True, lead_distance=27.5, lead_rel_speed=-1.2, lead_visible=False) + parser.update([(1, [msg])]) + + assert parser.can_valid + assert parser.vl["SCC_CONTROL"]["ACC_ObjDist"] == pytest.approx(0.0) + assert parser.vl["SCC_CONTROL"]["ACC_ObjRelSpd"] == pytest.approx(0.0) + assert parser.vl["SCC_CONTROL"]["ObjValid"] == 1 + assert parser.vl["SCC_CONTROL"]["OBJ_STATUS"] == 0 + + def test_canfd_acc_control_allows_distance_setting_override(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.KIA_EV6 + CP.flags = int(HyundaiFlags.CANFD) + + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + can_bus = CanBus(CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("SCC_CONTROL", 0)], can_bus.ECAN) + + msg = hyundaicanfd.create_acc_control(packer, can_bus, enabled=True, accel_last=0.0, accel=0.1, stopping=False, + gas_override=False, set_speed=42, hud_control=SimpleNamespace(leadDistanceBars=1), + direct_accel=True, distance_setting=3, + lead_distance=41.0, lead_rel_speed=-0.5, lead_visible=True) + parser.update([(1, [msg])]) + + assert parser.can_valid + assert parser.vl["SCC_CONTROL"]["DISTANCE_SETTING"] == 3 + assert parser.vl["SCC_CONTROL"]["ACC_ObjDist"] == pytest.approx(41.0) + + def test_canfd_lead_distance_setting_uses_detected_range(self): + assert get_canfd_lead_distance_setting(None, 2) == 2 + assert get_canfd_lead_distance_setting(0.0, 2) == 2 + assert get_canfd_lead_distance_setting(12.0, 3) == 1 + assert get_canfd_lead_distance_setting(24.0, 1) == 2 + assert get_canfd_lead_distance_setting(38.0, 1) == 3 + + def test_canfd_scc_lead_state_prefers_openpilot_lead_distance(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.KIA_EV6 + CP.flags = int(HyundaiFlags.CANFD) + + controller = CarController(DBC[CP.carFingerprint], CP) + cc = SimpleNamespace(hudControl=SimpleNamespace(leadVisible=True, leadDistanceBars=2)) + cs = SimpleNamespace( + openpilot_lead_visible=True, + openpilot_lead_distance=37.5, + openpilot_lead_rel_speed=-1.3, + stock_camera_lead_ts=0, + stock_camera_lead_visible=False, + stock_camera_lead_distance=0.0, + stock_camera_lead_rel_speed=0.0, + ) + + lead_visible, lead_distance, lead_rel_speed, distance_setting = controller._get_canfd_scc_lead_state(cc, cs, now_nanos=1_000_000_000) + + assert lead_visible + assert lead_distance == pytest.approx(37.5) + assert lead_rel_speed == pytest.approx(-1.3) + assert distance_setting == 3 + + def test_canfd_scc_lead_state_falls_back_to_hud_lead_when_no_distance_available(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.KIA_EV6 + CP.flags = int(HyundaiFlags.CANFD) + + controller = CarController(DBC[CP.carFingerprint], CP) + cc = SimpleNamespace(hudControl=SimpleNamespace(leadVisible=True, leadDistanceBars=2)) + cs = SimpleNamespace( + openpilot_lead_visible=True, + openpilot_lead_distance=0.0, + openpilot_lead_rel_speed=0.0, + stock_camera_lead_ts=0, + stock_camera_lead_visible=False, + stock_camera_lead_distance=0.0, + stock_camera_lead_rel_speed=0.0, + ) + + lead_visible, lead_distance, lead_rel_speed, distance_setting = controller._get_canfd_scc_lead_state(cc, cs, now_nanos=1_000_000_000) + + assert lead_visible + assert lead_distance == pytest.approx(20.0) + assert lead_rel_speed == pytest.approx(0.0) + assert distance_setting == 2 + def test_can_acc_commands_use_default_values(self): CP = CarParams.new_message() CP.carFingerprint = CAR.GENESIS_G90 @@ -871,6 +986,25 @@ class TestHyundaiFingerprint: assert parser.vl["LFA"]["STEER_REQ"] == 1 assert parser.vl["LFA"]["LKA_ICON"] == 2 + def test_ioniq_6_lfa_helper_allows_lka_icon_override(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.HYUNDAI_IONIQ_6 + CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING) + CP.openpilotLongitudinalControl = True + + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + can_bus = CanBus(CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LFA", 0)], can_bus.ECAN) + + msgs = hyundaicanfd.create_steering_messages(packer, CP, can_bus, False, False, 0, 0.0, lka_icon=3) + lfa_msgs = [msg for msg in msgs if msg[0] == 0x12A] + assert len(lfa_msgs) == 1 + + parser.update([(1, lfa_msgs)]) + + assert parser.can_valid + assert parser.vl["LFA"]["LKA_ICON"] == 3 + def test_ioniq_6_lkas_alt_helper_preserves_stock_camera_fields(self): CP = CarParams.new_message() CP.carFingerprint = CAR.HYUNDAI_IONIQ_6 @@ -919,6 +1053,34 @@ class TestHyundaiFingerprint: assert parser.vl["LKAS_ALT"]["STEER_REQ"] == 1 assert parser.vl["LKAS_ALT"]["LKA_ICON"] == 2 + def test_ioniq_6_lfahda_cluster_allows_lfa_icon_override(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.HYUNDAI_IONIQ_6 + CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING) + + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + can_bus = CanBus(CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LFAHDA_CLUSTER", 0)], can_bus.ECAN) + + msg = hyundaicanfd.create_lfahda_cluster(packer, can_bus, False, lfa_icon=3) + parser.update([(1, [msg])]) + + assert parser.can_valid + assert parser.vl["LFAHDA_CLUSTER"]["LFA_ICON"] == 3 + + def test_g90_lfahda_mfc_allows_lfa_icon_override(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.GENESIS_G90 + + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LFAHDA_MFC", 0)], 0) + + msg = hyundaican.create_lfahda_mfc(packer, False, frame=7, CP=CP, lfa_icon=3) + parser.update([(1, [msg])]) + + assert parser.can_valid + assert parser.vl["LFAHDA_MFC"]["LFA_Icon_State"] == 3 + def test_ioniq_6_blindspot_status_helper_regenerates_counter_checksum(self): CP = CarParams.new_message() CP.carFingerprint = CAR.HYUNDAI_IONIQ_6 @@ -1002,6 +1164,10 @@ class TestHyundaiFingerprint: assert decode_ioniq_6_blindspot_radar_state(0x1A) == (True, True) assert decode_ioniq_6_blindspot_radar_state(10.0) == (False, True) + def test_canfd_camera_lead_decode(self): + assert decode_canfd_camera_lead(0.0, -1.0) == (False, 0.0, 0.0) + assert decode_canfd_camera_lead(25.0, -1.5) == (True, 25.0, -1.5) + def test_ioniq_6_cluster_blindspot_helper_uses_captured_stock_sequences(self): CP = CarParams.new_message() CP.carFingerprint = CAR.HYUNDAI_IONIQ_6 diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 96fd41a09..21f2273fb 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-25633be9-DEBUG"; +const uint8_t gitversion[19] = "DEV-0ce8161c-DEBUG"; diff --git a/panda/board/obj/version b/panda/board/obj/version index 826584508..ea6b4d0f7 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-25633be9-DEBUG \ No newline at end of file +DEV-0ce8161c-DEBUG \ No newline at end of file diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index edd5e8eda..b3cba235e 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -27,6 +27,7 @@ from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles from openpilot.starpilot.controls.starpilot_card import StarPilotCard REPLAY = "REPLAY" in os.environ +OPENPILOT_LEAD_MIN_DISTANCE = 0.1 EventName = log.OnroadEvent.EventName @@ -72,7 +73,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'radarState']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks']) self.can_rcv_cum_timeout_counter = 0 @@ -338,11 +339,32 @@ class Car: if self.sm.all_alive(['carControl']): # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) + self._update_openpilot_lead_state(CC) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.starpilot_toggles) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.CC_prev = CC + def _update_openpilot_lead_state(self, CC: car.CarControl) -> None: + lead_visible = bool(CC.hudControl.leadVisible) + lead_distance = 0.0 + lead_rel_speed = 0.0 + + if self.sm.seen['radarState'] and self.sm.valid['radarState']: + lead = self.sm['radarState'].leadOne + if lead.status: + lead_visible = True + lead_distance = max(float(lead.dRel), 0.0) + lead_rel_speed = float(lead.vRel) + + if lead_distance <= OPENPILOT_LEAD_MIN_DISTANCE: + lead_distance = 0.0 + lead_rel_speed = 0.0 + + self.CI.CS.openpilot_lead_visible = lead_visible + self.CI.CS.openpilot_lead_distance = lead_distance + self.CI.CS.openpilot_lead_rel_speed = lead_rel_speed + def step(self): CS, RD, FPCS = self.state_update() diff --git a/selfdrive/ui/lib/starpilot_status.py b/selfdrive/ui/lib/starpilot_status.py index 5dadd5e11..b93dd5e61 100644 --- a/selfdrive/ui/lib/starpilot_status.py +++ b/selfdrive/ui/lib/starpilot_status.py @@ -89,4 +89,6 @@ def get_mode_transition_banner_text(state: UIState): return "OVERRIDDEN" if enabled and state.sm["selfdriveState"].experimentalMode: return "EXPERIMENTAL" + if enabled: + return "CHILL" return None