diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 49b9fd88b..9ae137878 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -532,11 +532,11 @@ IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT = 0.14 IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04 IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED = 23.5 IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH = 2.0 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.14 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 0.13 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.035 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.16 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.10 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.18 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 1.05 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.22 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.24 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.14 IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.088 IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28 IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.06 diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 78894a615..ece5510ab 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -462,6 +462,48 @@ def test_post_stop_speed_trigger_is_suppressed_after_red_light_release(monkeypat assert cem.status_value == conditional_experimental_mode_module.CEStatus["SPEED"] +def test_post_stop_slow_lead_trigger_is_suppressed_after_red_light_release(monkeypatch): + cem = make_cem(model_length=80.0, model_stopped=False) + toggles = make_update_toggles() + toggles.conditional_lead = True + toggles.conditional_slower_lead = True + toggles.conditional_stopped_lead = True + standstill_sm = make_update_sm(standstill=True) + moving_sm = make_update_sm(standstill=False) + + now = [100.0] + monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) + + def hold_red_light(*args, **kwargs): + cem.stop_light_detected = True + + def clear_red_light(*args, **kwargs): + cem.stop_light_detected = False + cem.stop_light_model_detected = False + + def detect_slow_lead(*args, **kwargs): + cem.slow_lead_detected = True + + monkeypatch.setattr(cem, "stop_sign_and_light", hold_red_light) + cem.update(0.0, standstill_sm, toggles) + assert cem.experimental_mode + + monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light) + monkeypatch.setattr(cem, "slow_lead", detect_slow_lead) + + launch_v_ego = 8.0 * CV.MPH_TO_MS + + now[0] = 100.1 + cem.update(launch_v_ego, moving_sm, toggles) + assert not cem.experimental_mode + assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"] + + now[0] = 102.5 + cem.update(launch_v_ego, moving_sm, toggles) + assert cem.experimental_mode + assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"] + + def test_standstill_update_can_activate_exp_from_dashboard_stop_sign(monkeypatch): cem = make_cem(model_length=80.0, model_stopped=False) toggles = make_update_toggles() diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index c74de7bbc..201a00a8e 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -170,6 +170,44 @@ def test_slc_lead_drop_relaxed_target_bails_out_for_threatening_lead(): assert relaxed == pytest.approx(raw_target) +def test_slc_lead_drop_relaxed_target_softens_for_far_slower_lead_if_new_limit_is_still_below_lead_speed(): + raw_target = 45.0 * CV.MPH_TO_MS + previous_target = 56.0 * CV.MPH_TO_MS + v_ego = 56.0 * CV.MPH_TO_MS + lead = SimpleNamespace(status=True, dRel=113.0, vLead=50.0 * CV.MPH_TO_MS, aLeadK=-0.15) + + relaxed = get_slc_lead_drop_relaxed_target( + raw_target, + previous_target, + v_ego, + tracking_lead=True, + lead=lead, + override_active=False, + source="Map Data", + ) + + assert raw_target < relaxed < previous_target + + +def test_slc_lead_drop_relaxed_target_still_bails_if_lead_is_slower_than_new_limit(): + raw_target = 45.0 * CV.MPH_TO_MS + previous_target = 56.0 * CV.MPH_TO_MS + v_ego = 56.0 * CV.MPH_TO_MS + lead = SimpleNamespace(status=True, dRel=113.0, vLead=39.0 * CV.MPH_TO_MS, aLeadK=-0.05) + + relaxed = get_slc_lead_drop_relaxed_target( + raw_target, + previous_target, + v_ego, + tracking_lead=True, + lead=lead, + override_active=False, + source="Map Data", + ) + + assert relaxed == pytest.approx(raw_target) + + def test_slc_lead_drop_relaxed_target_bails_out_without_tracking_lead(): raw_target = 55.0 * CV.MPH_TO_MS previous_target = 65.0 * CV.MPH_TO_MS diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index ee66e189c..a4f5f2b90 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -57,7 +57,7 @@ class ConditionalExperimentalMode: SLOW_LEAD_FORCE_CLEAR_TIME = 0.75 SLOW_LEAD_MIN_CLOSING_SPEED = 0.75 SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5 - POST_STOP_SPEED_TRIGGER_SUPPRESS_TIME = 2.0 + POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME = 2.0 TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0 @@ -107,7 +107,7 @@ class ConditionalExperimentalMode: self._prev_ce_status = None self.prev_standstill = False self.prev_standstill_stop_hold = False - self.post_stop_speed_trigger_suppress_until = 0.0 + self.post_stop_launch_trigger_suppress_until = 0.0 def update(self, v_ego, sm, starpilot_toggles): now = time.monotonic() @@ -116,7 +116,7 @@ class ConditionalExperimentalMode: released_standstill_stop_hold = self.prev_standstill and self.prev_standstill_stop_hold and not standstill if released_standstill_stop_hold: - self.post_stop_speed_trigger_suppress_until = now + self.POST_STOP_SPEED_TRIGGER_SUPPRESS_TIME + self.post_stop_launch_trigger_suppress_until = now + self.POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME self.mode_hold_until = 0.0 self.mode_false_since = 0.0 self.prev_experimental_mode = False @@ -204,9 +204,9 @@ class ConditionalExperimentalMode: return bool(self.stop_light_detected or force_stop_active or model_stopped) def check_conditions(self, v_ego, sm, starpilot_toggles): - speed_trigger_suppressed = time.monotonic() < self.post_stop_speed_trigger_suppress_until - below_speed = not speed_trigger_suppressed and starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead - below_speed_with_lead = not speed_trigger_suppressed and starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead + launch_trigger_suppressed = time.monotonic() < self.post_stop_launch_trigger_suppress_until + below_speed = not launch_trigger_suppressed and starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead + below_speed_with_lead = not launch_trigger_suppressed and starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead if below_speed or below_speed_with_lead: self.status_value = CEStatus["SPEED"] return True @@ -221,7 +221,7 @@ class ConditionalExperimentalMode: self.status_value = CEStatus["CURVATURE"] return True - if starpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 35.31: + if not launch_trigger_suppressed and starpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 35.31: self.status_value = CEStatus["LEAD"] return True diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 45e657b62..2f5487b41 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -16,7 +16,7 @@ STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0 SLC_LEAD_DROP_RELAXATION_MIN_SPEED = 20.0 * CV.MPH_TO_MS SLC_LEAD_DROP_RELAXATION_MIN_DISTANCE = 30.0 SLC_LEAD_DROP_RELAXATION_MIN_HEADWAY = 1.2 -SLC_LEAD_DROP_RELAXATION_MAX_CLOSING_SPEED = 0.35 +SLC_LEAD_DROP_RELAXATION_MAX_POST_DROP_CLOSING_SPEED = 0.35 SLC_LEAD_DROP_RELAXATION_MAX_LEAD_BRAKE = 0.25 SLC_LEAD_DROP_RELAXATION_OVERSPEED_BP = [0.0, 5.0 * CV.MPH_TO_MS, 10.0 * CV.MPH_TO_MS, 15.0 * CV.MPH_TO_MS] SLC_LEAD_DROP_RELAXATION_DECEL_V = [0.7, 0.9, 1.15, 1.35] @@ -109,7 +109,7 @@ def get_slc_lead_drop_relaxed_target(raw_target, previous_target, v_ego, trackin return raw_target v_lead = float(getattr(lead, "vLead", 0.0)) - if v_lead < float(v_ego) - SLC_LEAD_DROP_RELAXATION_MAX_CLOSING_SPEED: + if v_lead < float(raw_target) - SLC_LEAD_DROP_RELAXATION_MAX_POST_DROP_CLOSING_SPEED: return raw_target lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))