Force Stop: fast approach

cereal/custom.capnp

@@ -224,6 +224,7 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
   cscOverridden @38 :Bool;          # driver cancelled this curve with RES+
   cscLearnedLatAccel @39 :Float32;  # learned comfort at the current curvature, before margin
   cscBindingDistance @40 :Float32;  # distance to the horizon point setting the target, m
+  approachStopLength @41 :Float32;  # pre-commit distance to a detected stop, m; 0 when off
 }
 
 struct StarPilotRadarState @0xb86e6369214c01c8 {

selfdrive/controls/lib/longitudinal_planner.py

@@ -2196,9 +2196,14 @@ class LongitudinalPlanner:
     force_stop_x = None
     force_stop_handoff_m = get_force_stop_handoff_distance(self.CP.carFingerprint)
     if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > force_stop_handoff_m:
+      stop_length = float(sm['starpilotPlan'].forcingStopLength)
+    else:
+      # pre-commit the envelope is only a speed ceiling, which the solver tracks with a lag;
+      # getattr so a stale cereal build degrades to the old behaviour instead of raising
+      stop_length = float(getattr(sm['starpilotPlan'], 'approachStopLength', 0.0))
+    if stop_length > force_stop_handoff_m:
       force_stop_x = (
-        float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE +
-        get_force_stop_distance_bias(self.CP.carFingerprint)
+        stop_length + STOP_DISTANCE + get_force_stop_distance_bias(self.CP.carFingerprint)
       )
 
     self.mpc.update(sm['radarState'], v_cruise, x, v, a, j,

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -434,6 +434,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
       forcingStop=False,
       redLight=False,
       forcingStopLength=2,
+      approachStopLength=0.0,
     ),
   }
 

starpilot/controls/lib/starpilot_vcruise.py

@@ -62,6 +62,8 @@ LEAD_VETO_M_OVERRIDES = {
 }
 FORCE_STOP_APPROACH_DECEL = 0.65  # m/s^2 — speed ceiling before commit. LOWER = more early
                           # braking; don't go under FORCE_STOP_MODEL_APPROACH_DECEL
+# approachStopLength is published RAW: model_length converges from above, so rate-limiting
+# it inward freezes it far out and the constraint never binds. Tried, measured, don't re-add.
 ADAS_MAX_MS = 17.88       # 40 mph — cross-street ADAS guard
 DASH_SEED_M = 27.0        # ~88 ft — typical ADAS detection distance, used to snap
                           # tracked length closer when dashboard confirms a sign
@@ -183,6 +185,7 @@ class StarPilotVCruise:
     self.force_stop_from_light = False
     self.force_stop_light_clear_since = None
     self.controls_enabled_previously = False
+    self.approach_stop_length = 0.0  # published as starpilotPlan.approachStopLength
     # Kinematic distance estimator. Same attribute also published as
     # starpilotPlan.forcingStopLength, so the existing reader keeps working.
     self.tracked_model_length = 0.0
@@ -637,6 +640,9 @@ class StarPilotVCruise:
     offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, offset_ft_raw))
     offset_m = offset_ft * FT_TO_M
 
+    # cleared on every path; only the far-approach envelope below republishes it
+    self.approach_stop_length = 0.0
+
     if force_standstill_enabled and not self.override_force_standstill:
       self.forcing_stop = True
       self.tracked_model_length = 0.0
@@ -746,6 +752,8 @@ class StarPilotVCruise:
         adjacent_stop_d = self._get_adjacent_stop_distance(sm)
         if adjacent_stop_d is not None:
           approach_d = min(approach_d, adjacent_stop_d)
+        # pre-offset, so it hands off to forcingStopLength at commit without a step
+        self.approach_stop_length = max(approach_d, 0.0)
         approach_d += offset_m + force_stop_distance_bias_m
         if approach_d > force_stop_handoff_m:
           targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - force_stop_handoff_m)))

starpilot/controls/starpilot_planner.py

@@ -344,6 +344,7 @@ class StarPilotPlanner:
 
     starpilotPlan.forcingStop = self.starpilot_vcruise.forcing_stop
     starpilotPlan.forcingStopLength = self.starpilot_vcruise.tracked_model_length
+    starpilotPlan.approachStopLength = float(self.starpilot_vcruise.approach_stop_length)
     starpilotPlan.stopSignConfirmed = self.starpilot_vcruise.stop_sign_confirmed
 
     starpilotPlan.starpilotEvents = self.starpilot_events.events.to_msg()