From 04dbd8d0fd28708c347ce5ec3b4ee8eff01226cd Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 29 Apr 2026 14:22:52 -0500 Subject: [PATCH] HKG --- .../opendbc/car/hyundai/carcontroller.py | 108 +++++++++++++++++- .../opendbc/car/hyundai/hyundaican.py | 24 ++-- .../opendbc/car/hyundai/tests/test_hyundai.py | 47 +++++++- 3 files changed, 165 insertions(+), 14 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 12d35788b..2bd281fec 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -23,6 +23,12 @@ MAX_ANGLE_CONSECUTIVE_FRAMES = 2 CANFD_BLINDSPOT_STATUS_STALE_NS = 200_000_000 HYUNDAI_CANFD_SCC_ACCEL_STEP = 5.0 / 50.0 HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0 +HYUNDAI_LONG_MIN_JERK = 0.5 +HYUNDAI_LONG_JERK_LIMIT = 4.0 +HYUNDAI_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0] +HYUNDAI_LONG_LOOKAHEAD_JERK_V = [0.3, 0.45, 0.6] +HYUNDAI_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -0.005] +HYUNDAI_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5] IONIQ_6_LONG_MIN_JERK = 0.5 IONIQ_6_LONG_JERK_LIMIT = 4.0 IONIQ_6_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0] @@ -43,11 +49,32 @@ class Ioniq6LongitudinalTuningState: long_control_state_last: LongCtrlState = LongCtrlState.off +@dataclass +class HyundaiLongitudinalTuningState: + desired_accel: float = 0.0 + actual_accel: float = 0.0 + accel_last: float = 0.0 + jerk_upper: float = 0.0 + jerk_lower: float = 0.0 + comfort_band_upper: float = 0.0 + comfort_band_lower: float = 0.0 + stopping: bool = False + stopping_count: int = 0 + long_control_state_last: LongCtrlState = LongCtrlState.off + + def _jerk_limited_integrator(desired_accel: float, last_accel: float, jerk_upper: float, jerk_lower: float) -> float: step = (jerk_upper if desired_accel >= last_accel else jerk_lower) * DT_CTRL * 5.0 return float(np.clip(desired_accel, last_accel - step, last_accel + step)) +def _calculate_hyundai_dynamic_lower_jerk(accel_error: float) -> float: + if accel_error < 0.0: + scaled_values = np.array(HYUNDAI_DYNAMIC_LOWER_JERK_V) * (HYUNDAI_LONG_JERK_LIMIT / HYUNDAI_DYNAMIC_LOWER_JERK_V[0]) + return float(np.interp(accel_error, HYUNDAI_DYNAMIC_LOWER_JERK_BP, scaled_values)) + return HYUNDAI_LONG_MIN_JERK + + def _calculate_ioniq_6_dynamic_lower_jerk(accel_error: float) -> float: if accel_error < 0.0: scaled_values = np.array(IONIQ_6_DYNAMIC_LOWER_JERK_V) * (IONIQ_6_LONG_JERK_LIMIT / IONIQ_6_DYNAMIC_LOWER_JERK_V[0]) @@ -105,6 +132,65 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc return state +def update_hyundai_longitudinal_tuning(state: HyundaiLongitudinalTuningState, accel_cmd: float, v_ego: float, a_ego: float, + long_control_state: LongCtrlState, long_active: bool, radar_unavailable: bool) -> HyundaiLongitudinalTuningState: + stopping = long_control_state == LongCtrlState.stopping + + if not long_active: + state.desired_accel = 0.0 + state.actual_accel = 0.0 + state.accel_last = 0.0 + state.jerk_upper = 0.0 + state.jerk_lower = 0.0 + state.comfort_band_upper = 0.0 + state.comfort_band_lower = 0.0 + state.stopping = False + state.stopping_count = 0 + state.long_control_state_last = long_control_state + return state + + if not stopping: + state.stopping = False + state.stopping_count = 0 + elif state.long_control_state_last == LongCtrlState.off: + state.stopping = True + else: + if state.stopping_count > 1 / (DT_CTRL * 5): + state.stopping = True + state.stopping_count += 1 + + upper_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0])) if long_control_state == LongCtrlState.pid else HYUNDAI_LONG_MIN_JERK + lower_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0])) + + future_t_upper = float(np.interp(v_ego, HYUNDAI_LONG_LOOKAHEAD_JERK_BP, HYUNDAI_LONG_LOOKAHEAD_JERK_V)) + future_t_lower = float(np.interp(v_ego, HYUNDAI_LONG_LOOKAHEAD_JERK_BP, HYUNDAI_LONG_LOOKAHEAD_JERK_V)) + + accel_error = accel_cmd - state.accel_last + j_ego_upper = float(np.clip(accel_error / future_t_upper, -HYUNDAI_LONG_JERK_LIMIT, HYUNDAI_LONG_JERK_LIMIT)) + j_ego_lower = float(np.clip(accel_error / future_t_lower, -HYUNDAI_LONG_JERK_LIMIT, HYUNDAI_LONG_JERK_LIMIT)) + desired_jerk_upper = min(max(j_ego_upper, HYUNDAI_LONG_MIN_JERK), upper_speed_limit) + + dynamic_lower_jerk = _calculate_hyundai_dynamic_lower_jerk(a_ego - state.accel_last) + state.jerk_upper = desired_jerk_upper + state.jerk_lower = 5.0 if radar_unavailable else min(dynamic_lower_jerk, lower_speed_limit) + + if state.stopping: + state.desired_accel = 0.0 + else: + state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) + + state.actual_accel = _jerk_limited_integrator(state.desired_accel, state.accel_last, state.jerk_upper, state.jerk_lower) + state.accel_last = state.actual_accel + + accel_vals = [0.0, 0.3, 0.6, 0.9, 1.2, 2.0] + decel_vals = [-3.5, -2.5, -1.5, -1.0, -0.5, -0.05] + comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10] + state.comfort_band_upper = float(np.interp(a_ego, accel_vals, comfort_band_vals)) + state.comfort_band_lower = float(np.interp(a_ego, decel_vals, comfort_band_vals[::-1])) + state.long_control_state_last = long_control_state + return state + + def process_hud_alert(enabled, fingerprint, hud_control): sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)) @@ -149,6 +235,7 @@ class CarController(CarControllerBase): self.long_active_ecu = self.CP.openpilotLongitudinalControl self._ioniq_6_lane_change_ui_side = None self._ioniq_6_lane_change_ui_trigger_frames = 0 + self._hyundai_long_tuning = HyundaiLongitudinalTuningState() self._ioniq_6_long_tuning = Ioniq6LongitudinalTuningState() def update(self, CC, CS, now_nanos, starpilot_toggles): @@ -211,6 +298,18 @@ class CarController(CarControllerBase): # longitudinal messages - stock ECU is still active and these would conflict self.long_active_ecu = self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed + use_classic_hyundai_long_tuning = not (self.CP.flags & HyundaiFlags.CANFD) and self.long_active_ecu + if use_classic_hyundai_long_tuning and self.frame % 5 == 0: + self._hyundai_long_tuning = update_hyundai_longitudinal_tuning(self._hyundai_long_tuning, accel_cmd, + CS.out.vEgo, CS.out.aEgo, + actuators.longControlState, CC.longActive, + self.CP.radarUnavailable) + accel = self._hyundai_long_tuning.actual_accel + stopping = self._hyundai_long_tuning.stopping + elif use_classic_hyundai_long_tuning: + accel = self._hyundai_long_tuning.actual_accel + stopping = self._hyundai_long_tuning.stopping + use_ioniq_6_dynamic_long_tuning = self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu and \ actuators.longControlState == LongCtrlState.pid if use_ioniq_6_dynamic_long_tuning and self.frame % 5 == 0: @@ -299,7 +398,14 @@ class CarController(CarControllerBase): use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), hud_control, set_speed_in_units, stopping, - CC.cruiseControl.override, use_fca, self.CP)) + CC.cruiseControl.override, use_fca, self.CP, + main_mode_acc=int(CS.out.cruiseState.available), + desired_accel=self._hyundai_long_tuning.desired_accel, + actual_accel=self._hyundai_long_tuning.actual_accel, + jerk_lower=self._hyundai_long_tuning.jerk_lower, + comfort_band_upper=self._hyundai_long_tuning.comfort_band_upper, + comfort_band_lower=self._hyundai_long_tuning.comfort_band_lower, + stop_req=self._hyundai_long_tuning.stopping)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaican.py b/opendbc_repo/opendbc/car/hyundai/hyundaican.py index 8f3a049cd..770f6a70f 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaican.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaican.py @@ -125,27 +125,29 @@ def create_lfahda_mfc(packer, enabled): return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP, + main_mode_acc=1, desired_accel=None, actual_accel=None, jerk_lower=5.0, + comfort_band_upper=0.0, comfort_band_lower=0.0, stop_req=None): commands = [] scc11_values = { - "MainMode_ACC": 1, + "MainMode_ACC": main_mode_acc, "TauGapSet": hud_control.leadDistanceBars, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, - "ObjValid": 1, # close lead makes controls tighter - "ACC_ObjStatus": 1, # close lead makes controls tighter + "ObjValid": int(hud_control.leadVisible), # close lead makes controls tighter + "ACC_ObjStatus": int(hud_control.leadVisible), # close lead makes controls tighter "ACC_ObjLatPos": 0, "ACC_ObjRelSpd": 0, - "ACC_ObjDist": 1, # close lead makes controls tighter + "ACC_ObjDist": 1 if hud_control.leadVisible else 0, # close lead makes controls tighter } commands.append(packer.make_can_msg("SCC11", 0, scc11_values)) scc12_values = { "ACCMode": 2 if enabled and long_override else 1 if enabled else 0, - "StopReq": 1 if stopping else 0, - "aReqRaw": accel, - "aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw + "StopReq": 1 if (stopping if stop_req is None else stop_req) else 0, + "aReqRaw": accel if desired_accel is None else desired_accel, + "aReqValue": accel if actual_accel is None else actual_accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw "CR_VSM_Alive": idx % 0xF, } @@ -161,10 +163,10 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se commands.append(packer.make_can_msg("SCC12", 0, scc12_values)) scc14_values = { - "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values - "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values + "ComfortBandUpper": comfort_band_upper, # stock usually is 0 but sometimes uses higher values + "ComfortBandLower": comfort_band_lower, # stock usually is 0 but sometimes uses higher values "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values - "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values + "JerkLowerLimit": jerk_lower, # stock usually is 0.5 but sometimes uses higher values "ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage "ObjGap": 2 if hud_control.leadVisible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead } diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index c6f5b53ce..7d1a27047 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -7,10 +7,11 @@ from opendbc.can import CANPacker, CANParser from opendbc.car import Bus, ButtonType, gen_empty_fingerprint from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car -from opendbc.car.hyundai.carcontroller import Ioniq6LongitudinalTuningState, update_ioniq_6_longitudinal_tuning +from opendbc.car.hyundai.carcontroller import HyundaiLongitudinalTuningState, Ioniq6LongitudinalTuningState, \ + update_hyundai_longitudinal_tuning, update_ioniq_6_longitudinal_tuning from opendbc.car.hyundai.carstate import CarState, decode_ioniq_6_blindspot_radar_state from opendbc.car.hyundai.interface import CarInterface -from opendbc.car.hyundai import hyundaicanfd +from opendbc.car.hyundai import hyundaican, hyundaicanfd from opendbc.car.hyundai.hyundaicanfd import CanBus from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \ @@ -281,6 +282,24 @@ class TestHyundaiFingerprint: assert state.jerk_upper == pytest.approx(0.0) assert state.jerk_lower == pytest.approx(0.0) + def test_hyundai_longitudinal_tuning_helper_softens_stopping(self): + state = HyundaiLongitudinalTuningState(accel_last=-3.5) + + state = update_hyundai_longitudinal_tuning(state, accel_cmd=-3.5, v_ego=0.9, a_ego=-1.0, + long_control_state=LongCtrlState.stopping, long_active=True, + radar_unavailable=True) + assert state.stopping + assert state.desired_accel == pytest.approx(0.0) + assert state.actual_accel > -3.5 + assert state.jerk_lower == pytest.approx(5.0) + + state = update_hyundai_longitudinal_tuning(state, accel_cmd=1.0, v_ego=0.8, a_ego=0.1, + long_control_state=LongCtrlState.stopping, long_active=True, + radar_unavailable=True) + assert state.stopping + assert state.desired_accel == pytest.approx(0.0) + assert state.actual_accel <= 0.0 + def test_canfd_acc_control_uses_direct_accel(self): CP = CarParams.new_message() CP.carFingerprint = CAR.KIA_EV6 @@ -302,6 +321,30 @@ class TestHyundaiFingerprint: assert parser.vl["SCC_CONTROL"]["JerkLowerLimit"] == pytest.approx(5.0) assert parser.vl["SCC_CONTROL"]["JerkUpperLimit"] == pytest.approx(1.0) + def test_can_acc_commands_use_tuned_values(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.GENESIS_G90 + + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("SCC11", 0), ("SCC12", 0), ("SCC14", 0)], 0) + + msgs = hyundaican.create_acc_commands(packer, enabled=True, accel=-1.0, upper_jerk=2.5, idx=3, + hud_control=SimpleNamespace(leadDistanceBars=3, leadVisible=False), set_speed=42, + stopping=False, long_override=False, use_fca=False, CP=CP, + main_mode_acc=0, desired_accel=0.0, actual_accel=-0.35, + jerk_lower=4.0, comfort_band_upper=0.08, comfort_band_lower=0.02, stop_req=True) + parser.update([(1, msgs)]) + + assert parser.can_valid + assert parser.vl["SCC11"]["MainMode_ACC"] == 0 + assert parser.vl["SCC11"]["ObjValid"] == 0 + assert parser.vl["SCC12"]["StopReq"] == 1 + assert parser.vl["SCC12"]["aReqRaw"] == pytest.approx(0.0) + assert parser.vl["SCC12"]["aReqValue"] == pytest.approx(-0.35) + assert parser.vl["SCC14"]["ComfortBandUpper"] == pytest.approx(0.08) + assert parser.vl["SCC14"]["ComfortBandLower"] == pytest.approx(0.02) + assert parser.vl["SCC14"]["JerkLowerLimit"] == pytest.approx(4.0) + def test_sportage_angle_steering_uses_adas_cmd_with_send_lfa(self): fingerprint = gen_empty_fingerprint() cam_can = CanBus(None, fingerprint).CAM