Force Stop: Fast Approach

Force Stop is now better at faster approach speeds. It has more authority on braking in general.

Force Stop Distance Offset: still the lever that moves the ending point sooner or later.

Radar bug fixed

cereal/custom.capnp

@@ -221,6 +221,11 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
   trackingLead @36 :Bool;
   stopSignConfirmed @37 :Bool;
   pulseGlideCoasting @38 :Bool;  # developer-only P&G phase for on-road status UI
+  # Curve Speed Controller diagnostics, for tuning and rollout validation
+  cscOverridden @39 :Bool;          # driver cancelled this curve with RES+
+  cscLearnedLatAccel @40 :Float32;  # learned comfort at the current curvature, before margin
+  cscBindingDistance @41 :Float32;  # distance to the horizon point setting the target, m
+  approachStopLength @42 :Float32;  # pre-commit distance to a detected stop, m; 0 when off
 }
 
 struct StarPilotRadarState @0xb86e6369214c01c8 {

selfdrive/controls/lib/longitudinal_planner.py

@@ -9,6 +9,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.starpilot.common.model_versions import is_tinygrad_model_version
+from openpilot.starpilot.controls.lib.starpilot_vcruise import FT_TO_M, OFFSET_FT_MAX, OFFSET_FT_MIN
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
@@ -381,6 +382,11 @@ def get_vehicle_min_accel(CP, v_ego):
 
 # Restored planner constants retained by CEM, stop, and departure paths.
 A_CRUISE_MIN = -1.0
+# The stop distance runs ~9 m long through the mid-approach, which leaves the obstacle slack
+# so it stays silent and deceleration sags. Multiplicative so the trim scales with what is
+# left. Note the car parks where the obstacle sits, so this is also a placement bias — 0.85
+# stopped ~4.6 m short, 0.93 ~1.6 m.
+FORCE_STOP_OBSTACLE_TRIM = 0.93
 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25
 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08
 STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1
@@ -2217,8 +2223,17 @@ 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:
+      # ForceStopDistanceOffset shifts the perceived line for the v_cruise ceiling, so it has
+      # to shift the obstacle too or the slider barely moves anything now that stop_x leads.
+      offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, int(getattr(starpilot_toggles, 'force_stop_distance_offset', 0) or 0)))
       force_stop_x = (
-        float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE +
+        stop_length * FORCE_STOP_OBSTACLE_TRIM + offset_ft * FT_TO_M + STOP_DISTANCE +
         get_force_stop_distance_bias(self.CP.carFingerprint)
       )
 

selfdrive/controls/radard.py

@@ -53,6 +53,8 @@ ADJACENT_STOP_REST_FRAMES = 15
 ADJACENT_STOP_MIN_Y = 1.8         # m — inside this is our own lane
 ADJACENT_STOP_MAX_Y = 7.5         # m — beyond this is roadside, not an adjacent lane
 ADJACENT_STOP_MAX_D = 110.0       # m
+ADJACENT_STOP_QUEUE_GAP_M = 5.0   # m — anything stopped beyond the furthest qualifier means
+                                  # the bar is past it too, so the hint would stop us short
 
 
 class KalmanParams:
@@ -179,6 +181,10 @@ class Track:
     if self.leadTrackID == self.identifier:
       return False
 
+    return self.in_adjacent_lane(model_data)
+
+  def in_adjacent_lane(self, model_data: capnp._DynamicStructReader):
+    """Lane geometry only, no deceleration history — also used to spot a queue ahead."""
     if not (ADJACENT_STOP_MIN_Y < abs(self.yRel) < ADJACENT_STOP_MAX_Y):
       return False
 
@@ -398,9 +404,10 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca
 def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStructReader) -> dict[str, Any]:
   """Stop-line hint: a vehicle that decelerated to a stop in a neighbouring lane.
 
-  Takes the FARTHEST qualifying vehicle: in a queue the front car sits at the bar and the
-  rest are closer to us, so the nearest one underestimates the distance. The consumer only
-  shortens with this, so underestimating is the harmful direction.
+  Takes the FARTHEST qualifying vehicle, then drops the hint entirely if a queue reaches
+  past it. Cars already stopped when we acquire them never show the moving -> stopped
+  transition, so the qualifying set is biased toward the back of a line; without this the
+  hint marks a mid-queue bumper and stops us short of the bar.
   """
   if len(model_data.laneLines) < 4:
     return {'status': False}
@@ -410,6 +417,11 @@ def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStr
     return {'status': False}
 
   furthest = max(candidates, key=lambda c: c.dRel)
+  for c in tracks.values():
+    if (c.dRel > furthest.dRel + ADJACENT_STOP_QUEUE_GAP_M and
+        abs(c.vLead) < ADJACENT_STOP_REST_V and
+        c.in_adjacent_lane(model_data)):
+      return {'status': False}
   return {
     'status': True,
     'dRel': float(furthest.dRel),

selfdrive/controls/tests/test_longitudinal_planner.py

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

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -7,6 +7,7 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME
 from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController
 from openpilot.starpilot.controls.lib.starpilot_vcruise import (
+  FORCE_STOP_CAP_SLACK_M,
   FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME,
   STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME,
   StarPilotVCruise,
@@ -58,6 +59,8 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
   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
+  # what the not-committed branch would have left behind on the frame before commit
+  vcruise.force_stop_distance_cap = planner.model_length
   return planner, vcruise
 
 
@@ -524,6 +527,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
   sm = make_sm(standstill=False)
   sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
 
@@ -573,6 +577,37 @@ def test_force_stop_does_not_reanchor_inside_reanchor_floor():
   assert vcruise.tracked_model_length < 25.0
 
 
+def test_force_stop_reanchor_bounded_by_distance_driven():
+  # The line can't recede: a ballooning horizon may not push the stop past where it was at
+  # commit minus the distance driven since.
+  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 = 70.0
+  sm = make_sm(standstill=False)
+  sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
+
+  update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=10.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():
   planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
   planner.model_length = 40.0

starpilot/common/accel_profile.py

@@ -3,8 +3,6 @@ from __future__ import annotations
 
 import math
 
-from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
-
 ACCELERATION_PROFILES = {
   "STANDARD": 0,
   "ECO": 1,

starpilot/controls/lib/starpilot_vcruise.py

@@ -59,6 +59,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
@@ -76,7 +78,14 @@ FORCE_STOP_TURN_VETO_STEERING_ANGLE = 25.0
 FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003
 FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0
 FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP = 3.0  # m — ignore small model-horizon noise
-FORCE_STOP_REANCHOR_MIN_M = 40.0
+FORCE_STOP_REANCHOR_MIN_M = 40.0  # m — inside this only ratchet down; shouldStop doesn't
+                          # assert until ~10 m, so horizon jitter would release the stop
+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
@@ -180,9 +189,11 @@ 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
+    self.force_stop_distance_cap = 0.0  # odometry ceiling, re-seeded until commit
 
     self.stop_sign_confirmed = False
     self.stop_seen_on_approach_at = None
@@ -606,6 +617,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
@@ -645,6 +659,12 @@ class StarPilotVCruise:
           self.tracked_model_length = model_length
         else:
           self.tracked_model_length = min(self.tracked_model_length, model_length)
+        # Odometry ceiling: the line can't recede, so a re-anchor may never exceed what we
+        # 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)
+        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)
@@ -678,6 +698,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
 
       targets = [v_cruise]
       if self.csc_target >= CSC_MIN_SPEED:
@@ -723,6 +744,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

@@ -31,7 +31,10 @@ from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise
 from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker
 
 RADARLESS_TRACK_HOLD_TIME = 0.45
-FORCE_STOP_JERK_SCALE = 0.32  # accel-change cost multiplier while forcing_stop (125 -> ~40)
+FORCE_STOP_JERK_SCALE = 0.20  # accel-change cost multiplier for the whole stop approach,
+                              # envelope included (125 -> 25). Lower = reaches the braking
+                              # target sooner; it does not make the target deeper. Response
+                              # is super-linear here, so raise it if onset feels like a step.
 FORCE_STOP_JERK_SCALE_OVERRIDES = {
   # The Elantra's current force-stop ramp is smooth, but it waits too long
   # before building decel and then arrives at the initial brake too abruptly.
@@ -308,7 +311,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
@@ -346,6 +351,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()