From edc9a9ce60104a11f1461e6f829858a27b5fca41 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 30 Apr 2026 10:18:17 -0500 Subject: [PATCH] I6 and CEM --- .../opendbc/car/hyundai/carcontroller.py | 37 +++++++++++++--- .../opendbc/car/hyundai/tests/test_hyundai.py | 16 +++++++ selfdrive/controls/lib/latcontrol_torque.py | 28 ++++++------- .../test_conditional_experimental_mode.py | 42 +++++++++++++++++++ .../lib/conditional_experimental_mode.py | 37 ++++++++++++---- 5 files changed, 131 insertions(+), 29 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 5b56014a3..a02a39e03 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -24,12 +24,18 @@ CANFD_BLINDSPOT_STATUS_STALE_NS = 200_000_000 CANFD_BLINKER_STALKS_STALE_NS = 200_000_000 HYUNDAI_CANFD_SCC_ACCEL_STEP = 5.0 / 50.0 HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0 +IONIQ_6_CANFD_SCC_ACCEL_STEP = 6.0 / 50.0 +IONIQ_6_CANFD_SCC_DECEL_STEP = 15.0 / 50.0 IONIQ_6_LONG_MIN_JERK = 0.5 -IONIQ_6_LONG_JERK_LIMIT = 4.0 +IONIQ_6_LONG_JERK_LIMIT = 4.8 IONIQ_6_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0] IONIQ_6_LONG_LOOKAHEAD_JERK_V = [0.3, 0.45, 0.6] IONIQ_6_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -0.005] IONIQ_6_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5] +IONIQ_6_LAUNCH_HOLD_SPEED_BP = [0.0, 0.6, 1.25, 2.5] +IONIQ_6_LAUNCH_HOLD_SPEED_V = [0.75, 0.6, 0.4, 0.0] +IONIQ_6_STOP_HOLD_SPEED_BP = [0.0, 0.25, 0.6, 1.2] +IONIQ_6_STOP_HOLD_SPEED_V = [-0.18, -0.15, -0.08, 0.0] @dataclass @@ -39,6 +45,7 @@ class Ioniq6LongitudinalTuningState: accel_last: float = 0.0 jerk_upper: float = 0.0 jerk_lower: float = 0.0 + launch_active: bool = False stopping: bool = False stopping_count: int = 0 long_control_state_last: LongCtrlState = LongCtrlState.off @@ -58,6 +65,7 @@ def _calculate_ioniq_6_dynamic_lower_jerk(accel_error: float) -> float: def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, accel_cmd: float, v_ego: float, a_ego: float, long_control_state: LongCtrlState, long_active: bool) -> Ioniq6LongitudinalTuningState: + starting = long_control_state == LongCtrlState.starting stopping = long_control_state == LongCtrlState.stopping if not long_active or not stopping: @@ -76,9 +84,16 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc state.accel_last = 0.0 state.jerk_upper = 0.0 state.jerk_lower = 0.0 + state.launch_active = False state.long_control_state_last = long_control_state return state + if accel_cmd <= 0.0 or v_ego >= IONIQ_6_LAUNCH_HOLD_SPEED_BP[-1]: + state.launch_active = False + elif starting or (state.launch_active and v_ego < IONIQ_6_LAUNCH_HOLD_SPEED_BP[-1]) or \ + (state.long_control_state_last == LongCtrlState.starting and long_control_state == LongCtrlState.pid and v_ego < IONIQ_6_LAUNCH_HOLD_SPEED_BP[-1]): + state.launch_active = True + 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 IONIQ_6_LONG_MIN_JERK lower_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0])) @@ -96,9 +111,14 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc state.jerk_lower = min(dynamic_lower_jerk, lower_speed_limit) if state.stopping: - state.desired_accel = 0.0 + state.desired_accel = float(np.interp(v_ego, IONIQ_6_STOP_HOLD_SPEED_BP, IONIQ_6_STOP_HOLD_SPEED_V)) + state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, [0.0, 1.2], [0.25, 0.5]))) else: state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) + if state.launch_active: + state.desired_accel = max(state.desired_accel, float(np.interp(v_ego, IONIQ_6_LAUNCH_HOLD_SPEED_BP, IONIQ_6_LAUNCH_HOLD_SPEED_V))) + state.jerk_upper = max(state.jerk_upper, float(np.interp(v_ego, [0.0, 2.5], [4.8, 3.2]))) + state.jerk_lower = max(state.jerk_lower, 1.0) state.actual_accel = _jerk_limited_integrator(state.desired_accel, state.accel_last, state.jerk_upper, state.jerk_lower) state.accel_last = state.actual_accel @@ -213,25 +233,30 @@ class CarController(CarControllerBase): self.long_active_ecu = self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed use_ioniq_6_dynamic_long_tuning = self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu and \ - actuators.longControlState == LongCtrlState.pid + actuators.longControlState in (LongCtrlState.starting, LongCtrlState.pid, LongCtrlState.stopping) if use_ioniq_6_dynamic_long_tuning and self.frame % 5 == 0: self._ioniq_6_long_tuning = update_ioniq_6_longitudinal_tuning(self._ioniq_6_long_tuning, accel_cmd, CS.out.vEgo, CS.out.aEgo, actuators.longControlState, self.long_active_ecu) - use_ioniq_6_smoothed_accel = use_ioniq_6_dynamic_long_tuning and accel_cmd >= self._ioniq_6_long_tuning.actual_accel + use_ioniq_6_smoothed_accel = use_ioniq_6_dynamic_long_tuning and ( + accel_cmd >= self._ioniq_6_long_tuning.actual_accel or + self._ioniq_6_long_tuning.launch_active or + self._ioniq_6_long_tuning.stopping + ) if self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu: if use_ioniq_6_smoothed_accel: accel = self._ioniq_6_long_tuning.actual_accel stopping = self._ioniq_6_long_tuning.stopping elif use_ioniq_6_dynamic_long_tuning: accel = float(np.clip(accel_cmd, - self.accel_last - HYUNDAI_CANFD_SCC_DECEL_STEP, - self.accel_last + HYUNDAI_CANFD_SCC_ACCEL_STEP)) + self.accel_last - IONIQ_6_CANFD_SCC_DECEL_STEP, + self.accel_last + IONIQ_6_CANFD_SCC_ACCEL_STEP)) self._ioniq_6_long_tuning.desired_accel = accel_cmd self._ioniq_6_long_tuning.actual_accel = accel self._ioniq_6_long_tuning.accel_last = accel self._ioniq_6_long_tuning.jerk_upper = 3.0 self._ioniq_6_long_tuning.jerk_lower = 5.0 if CC.enabled else 1.0 + self._ioniq_6_long_tuning.launch_active = False self._ioniq_6_long_tuning.stopping = stopping self._ioniq_6_long_tuning.long_control_state_last = actuators.longControlState diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 232b8b43c..b35a60b2c 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -275,6 +275,22 @@ class TestHyundaiFingerprint: assert state.jerk_upper == pytest.approx(0.0) assert state.jerk_lower == pytest.approx(0.0) + def test_ioniq_6_longitudinal_tuning_helper_holds_launch_through_starting_handoff(self): + state = Ioniq6LongitudinalTuningState() + + state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.0, v_ego=0.0, a_ego=0.0, + long_control_state=LongCtrlState.starting, long_active=True) + assert state.launch_active + assert state.actual_accel == pytest.approx(0.24) + assert state.jerk_upper == pytest.approx(4.8) + assert state.jerk_lower == pytest.approx(1.0) + + state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=0.3, v_ego=0.25, a_ego=1.2, + long_control_state=LongCtrlState.pid, long_active=True) + assert state.launch_active + assert state.desired_accel > 0.3 + assert state.actual_accel > 0.24 + def test_canfd_acc_control_uses_direct_accel(self): CP = CarParams.new_message() CP.carFingerprint = CAR.KIA_EV6 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d4d983864..1f0919b6f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -211,21 +211,21 @@ IONIQ_6_FF_CUTOFF = 0.48 IONIQ_6_FF_CUTOFF_WIDTH = 0.12 IONIQ_6_TRANSITION_SPEED = 10.0 IONIQ_6_PHASE_SCALE = 0.10 -IONIQ_6_TURN_IN_BOOST_LEFT = 0.76 -IONIQ_6_TURN_IN_BOOST_RIGHT = 0.76 -IONIQ_6_UNWIND_TAPER_LEFT = 1.36 -IONIQ_6_UNWIND_TAPER_RIGHT = 2.40 +IONIQ_6_TURN_IN_BOOST_LEFT = 0.82 +IONIQ_6_TURN_IN_BOOST_RIGHT = 0.84 +IONIQ_6_UNWIND_TAPER_LEFT = 1.44 +IONIQ_6_UNWIND_TAPER_RIGHT = 2.70 IONIQ_6_FRICTION_MULT = 0.995 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.20 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.26 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.20 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 2.45 -IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.09 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.14 -IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.02 -IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 1.96 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.22 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.30 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.30 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 2.80 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.10 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.16 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.12 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 2.28 IONIQ_6_CENTER_TAPER_MAX = 0.042 IONIQ_6_CENTER_TAPER_LAT = 0.18 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02 @@ -242,8 +242,8 @@ IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90 IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.08 IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.05 IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.44 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.60 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.30 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.66 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.44 IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90 diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 4b6ea7af1..76df081dc 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -18,6 +18,7 @@ def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: model_stopped=model_stopped, tracking_lead=tracking_lead, starpilot_vcruise=SimpleNamespace(stop_sign_confirmed=False), + starpilot_following=SimpleNamespace(slower_lead=False, following_lead=False), lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel, vLead=lead_v_lead, modelProb=lead_model_prob, radar=lead_radar), ) @@ -133,6 +134,47 @@ def test_stop_light_latch_holds_slow_high_confidence_vision_lead_during_model_fl assert cem.stop_light_detected +def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead(): + v_ego = 35 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 5.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=v_ego * 5.0, + lead_v_lead=8.0 * CV.MPH_TO_MS, + lead_model_prob=0.95, + ) + toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) + + cem.slow_lead_filter.x = 1.0 + cem.slow_lead_detected = True + + cem.starpilot_planner.tracking_lead = False + cem.starpilot_planner.starpilot_following.slower_lead = False + cem.slow_lead(toggles, v_ego) + assert cem.slow_lead_detected + + +def test_slow_lead_does_not_linger_at_crawl_when_stopped_lead_disabled(): + v_ego = 1.5 + cem = make_cem( + model_length=20.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=8.0, + lead_v_lead=1.2, + lead_model_prob=0.99, + ) + toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) + + cem.slow_lead_filter.x = 1.0 + cem.slow_lead_detected = True + cem.starpilot_planner.starpilot_following.slower_lead = False + cem.slow_lead(toggles, v_ego) + + assert not cem.slow_lead_detected + + class DummyThemeManager: def update_wheel_image(self, *args, **kwargs): pass diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index c56bda3e8..b901ef120 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -48,6 +48,9 @@ class ConditionalExperimentalMode: STOP_APPROACH_LATCH_TIME = 1.0 STOP_APPROACH_MAX_LEAD_SPEED = 4.5 STOP_APPROACH_MIN_MODEL_PROB = 0.9 + SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB = 0.85 + SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME = 7.0 + SLOW_LEAD_CONTINUITY_MIN_EGO = 2.5 # ===== END TUNING PARAMETERS ===== @@ -158,19 +161,35 @@ class ConditionalExperimentalMode: self.curve_detected = bool(self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED) def slow_lead(self, starpilot_toggles, v_ego): - if self.starpilot_planner.tracking_lead: - slower_lead = starpilot_toggles.conditional_slower_lead and self.starpilot_planner.starpilot_following.slower_lead - stopped_lead = starpilot_toggles.conditional_stopped_lead and self.starpilot_planner.lead_one.vLead < 1 - lead_threshold = scale_threshold(v_ego) + lead = self.starpilot_planner.lead_one + lead_status = bool(getattr(lead, "status", False)) + lead_distance = float(getattr(lead, "dRel", float("inf"))) + lead_speed = float(getattr(lead, "vLead", float("inf"))) + lead_prob = float(getattr(lead, "modelProb", 1.0)) - # Adjust threshold based on lead probability for vision-only accuracy - lead_prob = getattr(self.starpilot_planner.lead_one, 'modelProb', 1.0) - adjusted_threshold = lead_threshold * (1.0 + 0.2 * (1.0 - lead_prob)) # Higher threshold for lower confidence + if not starpilot_toggles.conditional_stopped_lead and v_ego < self.SLOW_LEAD_CONTINUITY_MIN_EGO: + self.slow_lead_filter.update(False) + self.slow_lead_detected = False + return - self.slow_lead_filter.update(slower_lead or stopped_lead) + slower_lead = starpilot_toggles.conditional_slower_lead and self.starpilot_planner.starpilot_following.slower_lead + stopped_lead = starpilot_toggles.conditional_stopped_lead and lead_speed < 1 + raw_vision_slow_lead = bool( + starpilot_toggles.conditional_slower_lead and + lead_status and + lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and + lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) and + lead_speed < max(v_ego - 0.5, 2.0) + ) + + lead_threshold = scale_threshold(v_ego) + adjusted_threshold = lead_threshold * (1.0 + 0.2 * (1.0 - lead_prob)) # Higher threshold for lower confidence + + if self.starpilot_planner.tracking_lead or raw_vision_slow_lead or stopped_lead: + self.slow_lead_filter.update(slower_lead or raw_vision_slow_lead or stopped_lead) self.slow_lead_detected = bool(self.slow_lead_filter.x >= adjusted_threshold) else: - self.slow_lead_filter.x = 0 + self.slow_lead_filter.update(False) self.slow_lead_detected = False def stop_sign_and_light(self, v_ego, sm, model_time):