huge if work

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:

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

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

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)

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

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";

panda/board/obj/version

@@ -1 +1 @@
-DEV-25633be9-DEBUG
+DEV-0ce8161c-DEBUG

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()
 

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