From 01bcb40c503e92d0c7fa2d5fa66df7e71196633e Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 13 May 2026 13:51:25 -0500 Subject: [PATCH] Longfixes --- .../controls/lib/longitudinal_planner.py | 8 +- .../test_conditional_experimental_mode.py | 34 +++++++ .../tests/test_longitudinal_planner.py | 23 +++++ .../controls/tests/test_starpilot_vcruise.py | 89 ++++++++++++++++++- .../lib/conditional_experimental_mode.py | 7 +- starpilot/controls/lib/starpilot_vcruise.py | 5 ++ starpilot/controls/starpilot_planner.py | 5 +- 7 files changed, 163 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 48ffa11dd..39405a6c6 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -100,16 +100,16 @@ VISION_LEAD_TFOLLOW_MAX_EXTRA_DELTA = 0.24 VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED = 20.0 VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN = 8.0 VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN = 0.35 -VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 5.5 -VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 1.75 +VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 6.5 +VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 3.25 VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB = 0.9 VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED = 0.35 -VISION_LOW_SPEED_STOP_BUFFER_BASE = 3.2 +VISION_LOW_SPEED_STOP_BUFFER_BASE = 3.8 VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN = 0.80 VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN = 0.25 VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.9 VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.8 -VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.1 +VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.25 VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25 LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5 diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index c71b3c8e3..58643db4b 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -286,6 +286,40 @@ def test_stopped_lead_handoff_does_not_hold_cem_on_empty_road(monkeypatch): assert not cem.stop_light_detected +def test_stopped_lead_toggle_does_not_self_trigger_without_a_real_lead(): + v_ego = 25 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 6.0, + tracking_lead=False, + lead_status=False, + lead_v_lead=0.0, + ) + toggles = SimpleNamespace(conditional_slower_lead=False, conditional_stopped_lead=True) + + for _ in range(24): + cem.slow_lead(toggles, v_ego) + + assert not cem.slow_lead_detected + + +def test_stopped_lead_toggle_still_triggers_for_real_close_stopped_lead(): + v_ego = 25 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 4.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=18.0, + lead_v_lead=0.4, + lead_model_prob=0.99, + ) + toggles = SimpleNamespace(conditional_slower_lead=False, conditional_stopped_lead=True) + + for _ in range(24): + cem.slow_lead(toggles, v_ego) + + assert cem.slow_lead_detected + + def test_borderline_empty_road_model_dip_does_not_refresh_long_hold(monkeypatch): v_ego = 20 * CV.MPH_TO_MS stop_threshold = v_ego * 7.0 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 9e3f3130a..2b253f6f5 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -758,6 +758,29 @@ def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap( assert planner.output_a_target < -1.0 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_acc_mode_low_speed_vision_stop_buffer_brakes_harder_for_close_slow_vision_lead(model_version): + v_ego = 6.2 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.1, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=9.35, v_lead=2.8, a_lead=-0.2, radar=False, model_prob=0.99), + ) + sm["starpilotPlan"].vCruise = v_ego + 4.0 + + planner.update(sm, make_toggles(model_version)) + + assert planner.mode == "acc" + assert planner.output_should_stop + assert planner.output_a_target <= -2.7 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version): v_ego = 0.0 diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 0ef1a7790..14338960b 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -1,7 +1,57 @@ import pytest from openpilot.common.constants import CV -from openpilot.starpilot.controls.lib.starpilot_vcruise import get_active_slc_control_target +from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise, get_active_slc_control_target +from types import SimpleNamespace + + +class FakeParams: + def get(self, *args, **kwargs): + return None + + def get_float(self, *args, **kwargs): + return 0.0 + + def put_nonblocking(self, *args, **kwargs): + pass + + +def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False): + planner = SimpleNamespace( + params=FakeParams(), + params_memory=FakeParams(), + lead_one=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0), + starpilot_cem=SimpleNamespace(stop_light_detected=red_light), + tracking_lead=False, + driving_in_curve=False, + model_length=60.0, + raw_model_stopped=raw_model_stopped, + road_curvature_detected=False, + ) + vcruise = StarPilotVCruise(planner) + vcruise.forcing_stop = forcing_stop + vcruise.force_stop_timer = 1.0 if forcing_stop else 0.0 + vcruise.tracked_model_length = 0.0 if forcing_stop else planner.model_length + return planner, vcruise + + +def make_sm(*, standstill=True): + return { + "carControl": SimpleNamespace(longActive=True), + "carState": SimpleNamespace(standstill=standstill, gasPressed=False, vCruiseCluster=0.0, vEgoCluster=0.0), + "starpilotCarState": SimpleNamespace(accelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0), + } + + +def make_toggles(): + return SimpleNamespace( + force_stops=True, + force_standstill=False, + curve_speed_controller=False, + speed_limit_controller=False, + show_speed_limits=False, + force_stop_distance_offset=0, + ) def test_active_slc_control_target_does_not_require_set_speed_limit(): @@ -28,3 +78,40 @@ def test_active_slc_control_target_applies_offset_and_cluster_diff(): ) assert target == pytest.approx((48.0 * CV.MPH_TO_MS) - 0.4) + + +def test_force_stop_clears_at_standstill_once_scene_opens(): + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=20.0, + v_ego=0.0, + sm=make_sm(standstill=True), + starpilot_toggles=make_toggles(), + ) + + assert result == pytest.approx(20.0) + assert vcruise.force_stop_timer == 0.0 + assert not vcruise.forcing_stop + assert vcruise.tracked_model_length == pytest.approx(planner.model_length) + + +def test_force_stop_stays_committed_while_model_still_sees_stop(): + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=True, forcing_stop=True) + + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=20.0, + v_ego=0.0, + sm=make_sm(standstill=True), + starpilot_toggles=make_toggles(), + ) + + assert result == pytest.approx(0.0) + assert vcruise.force_stop_timer >= 0.5 + assert vcruise.forcing_stop diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index 0d4c8cbda..04dd4fef0 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -239,7 +239,12 @@ class ConditionalExperimentalMode: return 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 + stopped_lead = bool( + starpilot_toggles.conditional_stopped_lead and + lead_status and + lead_speed < 1 and + lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) + ) vision_slow_lead_candidate = bool( lead_status and lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index d862a43f6..e26015b77 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -103,10 +103,15 @@ class StarPilotVCruise: if dash_path: self.stop_sign_confirmed = True + raw_model_stopped = bool(getattr(self.starpilot_planner, "raw_model_stopped", False)) + # Timer ramp. Faster commitment when the dashboard confirms. if force_stop_active and not sm["carState"].standstill: rate = DT_MDL * 2 if dash_active else DT_MDL self.force_stop_timer = min(self.force_stop_timer + rate, 2.0) + elif (self.forcing_stop and sm["carState"].standstill and not dash_active and + not self.starpilot_planner.starpilot_cem.stop_light_detected and not raw_model_stopped): + self.force_stop_timer = 0.0 else: self.force_stop_timer = max(self.force_stop_timer - DT_MDL * 0.25, 0.0) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index e8e2c854f..5aa84a029 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -56,6 +56,7 @@ class StarPilotPlanner: self.gps_valid = False self.lateral_check = False self.model_stopped = False + self.raw_model_stopped = False self.road_curvature_detected = False self.tracking_lead = False self._prev_gps_bearing = 0 @@ -129,8 +130,8 @@ class StarPilotPlanner: self.model_length = sm["modelV2"].position.x[-1] - self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME - self.model_stopped |= self.starpilot_vcruise.forcing_stop + self.raw_model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME + self.model_stopped = self.raw_model_stopped or self.starpilot_vcruise.forcing_stop self.road_curvature, self.time_to_curve = calculate_road_curvature(sm["modelV2"], v_ego)