From d161c01d5b289d1a32562dbd5f1e6558dbd4e370 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Tue, 25 Aug 2026 05:54:24 -0500 Subject: [PATCH] force stop: fast approach 3 --- .../controls/tests/test_starpilot_vcruise.py | 22 ++++++++++++++++--- starpilot/controls/lib/starpilot_vcruise.py | 8 +++++-- starpilot/controls/starpilot_planner.py | 4 +++- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index fa444e63b..9abbd1992 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -59,7 +59,7 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False 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 # what the not-committed branch would have left behind on the frame before commit - vcruise.force_stop_distance_cap = planner.model_length + FORCE_STOP_CAP_SLACK_M + vcruise.force_stop_distance_cap = planner.model_length return planner, vcruise @@ -756,7 +756,7 @@ def test_force_stop_reanchors_when_model_reopens_path_without_stop_action(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) planner.model_length = 90.0 vcruise.tracked_model_length = 60.0 - vcruise.force_stop_distance_cap = 90.0 + FORCE_STOP_CAP_SLACK_M + vcruise.force_stop_distance_cap = 90.0 sm = make_sm(standstill=False) sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) @@ -790,7 +790,23 @@ def test_force_stop_reanchor_bounded_by_distance_driven(): update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=10.0) - assert vcruise.tracked_model_length <= 70.0 + assert vcruise.tracked_model_length <= 70.0 + FORCE_STOP_CAP_SLACK_M + assert vcruise.tracked_model_length < 100.0 # nowhere near the 200 m the horizon claimed + + +def test_force_stop_cap_slack_tapers_near_the_line(): + # Slack protects against an under-read at commit; held near the line it would just aim the + # solver that far past the stop bar. + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + planner.model_length = 200.0 + vcruise.tracked_model_length = 60.0 + vcruise.force_stop_distance_cap = 12.0 + sm = make_sm(standstill=False) + sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) + + update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=5.0) + + assert vcruise.tracked_model_length < 12.0 + FORCE_STOP_CAP_SLACK_M / 2.0 def test_force_stop_does_not_reanchor_committed_model_stop(): diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 45824af02..040abe1eb 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -86,6 +86,9 @@ FORCE_STOP_REANCHOR_MIN_M = 40.0 # m — inside this only ratchet down; shouldS FORCE_STOP_CAP_SLACK_M = 15.0 # m — the line can't move away, so tracked can never exceed # what it was at commit minus distance driven. Slack covers an # under-read at commit; without it that would stop us short. +FORCE_STOP_CAP_TAPER_M = 60.0 # m — slack fades to 0 as the cap closes. The solver aims at + # tracked, so slack held near the line is braking for a stop bar + # that far past the real one. # Knob bounds (mirror of UI slider; defense in depth) OFFSET_FT_MIN = -20 @@ -683,7 +686,8 @@ class StarPilotVCruise: # had at commit minus what we've driven. Bounds a ballooning horizon (seen +95 m) # that the REANCHOR_MIN floor can't catch, since that floor trusts the estimate. self.force_stop_distance_cap = max(self.force_stop_distance_cap - (v_ego * DT_MDL), 0.0) - self.tracked_model_length = min(self.tracked_model_length, self.force_stop_distance_cap) + cap_slack = FORCE_STOP_CAP_SLACK_M * min(self.force_stop_distance_cap / FORCE_STOP_CAP_TAPER_M, 1.0) + self.tracked_model_length = min(self.tracked_model_length, self.force_stop_distance_cap + cap_slack) if dash_active: if model_length < DASH_MODEL_AGREE_M: self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M) @@ -716,7 +720,7 @@ class StarPilotVCruise: self.stop_sign_confirmed = False self.tracked_model_length = self.starpilot_planner.model_length - self.force_stop_distance_cap = self.tracked_model_length + FORCE_STOP_CAP_SLACK_M + self.force_stop_distance_cap = self.tracked_model_length targets = [v_cruise] if self.csc_target >= CSC_MIN_SPEED: diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 69478f905..e67d88227 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -307,7 +307,9 @@ class StarPilotPlanner: except (KeyError, IndexError, TypeError, AttributeError): car_params = None - if self.starpilot_vcruise.forcing_stop: + # Also while the far-approach envelope is running: at onset the ramp reaches only + # ~-0.5 m/s^2 after a second, so the first seconds of a detected red are mostly lost. + if self.starpilot_vcruise.forcing_stop or self.starpilot_vcruise.approach_stop_length > 0.0: jerk_scale = get_force_stop_jerk_scale(car_params) elif self.tracking_lead: # Elantra vision leads can hand off from cruise to lead0 while closing