force stop: fast approach 3

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():

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:

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