diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 84520892cd..6e4b8e8856 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -41,7 +41,9 @@ 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] +IONIQ_6_STOP_HOLD_SPEED_V = [-0.12, -0.10, -0.05, 0.0] +IONIQ_6_STOP_RELEASE_JERK_BP = [0.0, 0.15, 0.5] +IONIQ_6_STOP_RELEASE_JERK_V = [3.6, 4.2, 4.8] @dataclass @@ -80,6 +82,8 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc long_control_state: LongCtrlState, long_active: bool) -> Ioniq6LongitudinalTuningState: starting = long_control_state == LongCtrlState.starting stopping = long_control_state == LongCtrlState.stopping + restart_from_stop = state.long_control_state_last in (LongCtrlState.stopping, LongCtrlState.starting) and \ + long_control_state in (LongCtrlState.starting, LongCtrlState.pid) and accel_cmd > 0.0 and v_ego < 0.5 if not long_active or not stopping: state.stopping = False @@ -125,13 +129,15 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc if state.stopping: 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]))) + state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, [0.0, 1.2], [0.45, 0.65]))) 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) + if restart_from_stop: + state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, IONIQ_6_STOP_RELEASE_JERK_BP, IONIQ_6_STOP_RELEASE_JERK_V))) state.actual_accel = _jerk_limited_integrator(state.desired_accel, state.accel_last, state.jerk_upper, state.jerk_lower) state.accel_last = state.actual_accel diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 0b9dfd0d52..d31c7eb78f 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -292,6 +292,19 @@ class TestHyundaiFingerprint: assert state.desired_accel > 0.3 assert state.actual_accel > 0.24 + def test_ioniq_6_longitudinal_tuning_helper_softens_stop_release_handoff(self): + state = Ioniq6LongitudinalTuningState(actual_accel=-0.12, accel_last=-0.12, + stopping=True, stopping_count=25, + long_control_state_last=LongCtrlState.stopping) + + 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.actual_accel == pytest.approx(0.06) + + 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.actual_accel == pytest.approx(0.24) + def test_genesis_g90_longitudinal_tuning_softens_final_stop_hold(self): state = GenesisG90LongitudinalTuningState() diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 1f0919b6fe..23f7f56a93 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.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_TURN_IN_BOOST_LEFT = 0.88 +IONIQ_6_TURN_IN_BOOST_RIGHT = 0.92 +IONIQ_6_UNWIND_TAPER_LEFT = 1.52 +IONIQ_6_UNWIND_TAPER_RIGHT = 2.92 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.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_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.24 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.34 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.40 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 3.10 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.11 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.18 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.22 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 2.58 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.66 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.44 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.72 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.58 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 76df081dc3..3b0e65db78 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -140,7 +140,7 @@ def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead() model_length=v_ego * 5.0, tracking_lead=True, lead_status=True, - lead_d_rel=v_ego * 5.0, + lead_d_rel=v_ego * 3.5, lead_v_lead=8.0 * CV.MPH_TO_MS, lead_model_prob=0.95, ) @@ -175,6 +175,46 @@ def test_slow_lead_does_not_linger_at_crawl_when_stopped_lead_disabled(): assert not cem.slow_lead_detected +def test_far_untracked_slow_lead_does_not_trigger_slow_lead(): + v_ego = 50 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 5.0, + tracking_lead=False, + lead_status=True, + lead_d_rel=v_ego * 4.8, + lead_v_lead=v_ego - 2.0, + lead_model_prob=0.97, + ) + 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 + + +def test_pace_matched_lead_clears_slow_lead_quickly(): + v_ego = 30 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 4.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=24.0, + lead_v_lead=v_ego + 0.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 b901ef1203..15d40b713f 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -49,8 +49,10 @@ class ConditionalExperimentalMode: 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_MAX_DISTANCE_TIME = 4.0 SLOW_LEAD_CONTINUITY_MIN_EGO = 2.5 + SLOW_LEAD_MIN_CLOSING_SPEED = 0.75 + SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5 # ===== END TUNING PARAMETERS ===== @@ -166,6 +168,8 @@ class ConditionalExperimentalMode: lead_distance = float(getattr(lead, "dRel", float("inf"))) lead_speed = float(getattr(lead, "vLead", float("inf"))) lead_prob = float(getattr(lead, "modelProb", 1.0)) + closing_speed = max(0.0, v_ego - lead_speed) + min_closing_speed = max(self.SLOW_LEAD_MIN_CLOSING_SPEED, 0.04 * v_ego) if not starpilot_toggles.conditional_stopped_lead and v_ego < self.SLOW_LEAD_CONTINUITY_MIN_EGO: self.slow_lead_filter.update(False) @@ -179,12 +183,18 @@ class ConditionalExperimentalMode: 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 + closing_speed >= min_closing_speed 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 lead_status and not slower_lead and not stopped_lead and closing_speed < (min_closing_speed * self.SLOW_LEAD_CLEAR_FASTER_FACTOR): + self.slow_lead_filter.update(False) + self.slow_lead_detected = False + return + 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)