Low Speed Twitches

1. Rolling stops fixed: a latched sign survives the light clearing, and the stop target can't jump farther away.

2. Sign over a long road: a sign can latch even when the model sees far ahead, once Force Stop is already slowing.

3. Vision distance: sign distance comes from the box size, averaged with the dashboard's 27 m.

4. Sign behind a car: if you're stopped behind a car at a sign, it stops at the line once that car leaves.

5. False early sign: an early dashboard sign is ignored if stopping for it would need hard braking.

openpilot/selfdrive/controls/controlsd.py

@@ -478,22 +478,25 @@ class Controls:
       new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
     assist = self.turn_assist_enabled()
     rolling = math.isfinite(CS.vEgo) and CS.vEgo > max(0.1 * CV.MPH_TO_MS, self.CP.minSteerSpeed)
-    if (
+    assist_ready = (
       assist
-      and rolling
       and self.sm.all_checks(['modelV2'])
       and CC.latActive
-      and not CS.standstill
       and not CS.steerFaultTemporary
       and not CS.steerFaultPermanent
       and CS.gearShifter in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
-    ):
+    )
+    if assist_ready and rolling and not CS.standstill:
       self.model_turn_assist.curvature = self.curvature
-      self.model_turn_assist.twitch_guard_remaining = update_twitch_guard(self.model_turn_assist.twitch_guard_remaining, CS.vEgo, CS.standstill)
+      self.model_turn_assist.twitch_guard_armed = update_twitch_guard(self.model_turn_assist.twitch_guard_armed, CS.vEgo, CS.standstill)
       new_desired_curvature = self.model_turn_assist.update(CS, model_v2, CC, new_desired_curvature)
+    elif assist_ready:
+      self.model_turn_assist.curvature = self.curvature
+      new_desired_curvature = self.model_turn_assist.update_stopped(CS, new_desired_curvature)
+      self.model_turn_assist.twitch_guard_armed = update_twitch_guard(self.model_turn_assist.twitch_guard_armed, CS.vEgo, CS.standstill)
     elif getattr(self, 'model_turn_assist', None) is not None:
       self.model_turn_assist.reset()
-      self.model_turn_assist.twitch_guard_remaining = update_twitch_guard(0.0, CS.vEgo, CS.standstill)
+      self.model_turn_assist.twitch_guard_armed = update_twitch_guard(False, CS.vEgo, CS.standstill)
     lane_base_curvature = new_desired_curvature
     self.lane_centering_applied = 0.0
     self.lane_centering_inactive_reason = 'inactive'

openpilot/starpilot/lateral/model_turn_assist.py

@@ -28,12 +28,17 @@ CURVATURE_HOLD_CONFIRM_MIN = 0.003
 CURVATURE_HOLD_CONFIRM_SWEPT = 0.6
 TWITCH_GUARD_MAX_SPEED = 4.0
 TWITCH_GUARD_FADE_SPEED = 3.0
-TWITCH_GUARD_DURATION = 1.5
 TWITCH_GUARD_PLAN_RATIO = 4.0
 TWITCH_GUARD_FLOOR = 0.002
 TWITCH_GUARD_STRAIGHT_LO = 0.005
 TWITCH_GUARD_STRAIGHT_HI = 0.014
 TWITCH_GUARD_MIN_REACH = 12.0
+STOP_HOLD_MAX_SPEED = 2.5
+STOP_HOLD_DIST = 1.5
+STOP_HOLD_RELEASE_DIST = 3.0
+STOP_HOLD_MIN_DECEL = 0.3
+STOP_HOLD_MAX_CURVATURE = 0.01
+STOP_GUARD_DIST = 5.0
 
 
 def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
@@ -118,12 +123,18 @@ def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float:
   return curvature + (math.copysign(limit, curvature) - curvature) * fade
 
 
-def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> float:
-  if not (math.isfinite(remaining) and math.isfinite(v_ego)):
-    return 0.0
+def update_twitch_guard(armed: bool, v_ego: float, standstill: bool) -> bool:
+  if not math.isfinite(v_ego):
+    return False
   if standstill or abs(v_ego) <= 0.3:
-    return TWITCH_GUARD_DURATION
-  return max(remaining - DT_CTRL, 0.0)
+    return True
+  return armed and v_ego < TWITCH_GUARD_MAX_SPEED
+
+
+def stopping_distance(v_ego: float, a_ego: float, max_speed: float = STOP_HOLD_MAX_SPEED) -> float:
+  if not (math.isfinite(v_ego) and math.isfinite(a_ego)) or v_ego >= max_speed:
+    return math.inf
+  return v_ego ** 2 / (2.0 * max(-a_ego, 0.01))
 TURN_LEAD_T = 1.3
 TURN_LEAD_MIN_M = 4.0
 TURN_LEAD_MAX_M = 14.0
@@ -149,14 +160,46 @@ class ModelTurnAssist:
     self.turn_hold_swept = 0.0
     self.turn_hold_handoff_t = 0.0
     self.turn_blinker_swept = 0.0
-    self.twitch_guard_remaining = 0.0
+    self.twitch_guard_armed = False
+    self.stop_hold_curvature = None
+    self.output_curvature = None
     self.curvature = 0.0
     self.turn_hold_done = False
 
+  def update_stopped(self, CS, new_desired_curvature):
+    if CS.leftBlinker or CS.rightBlinker or self.output_curvature is None:
+      self.reset()
+      return new_desired_curvature
+    if CS.steeringPressed:
+      self.output_curvature = self.curvature
+    return self.output_curvature
+
   def update(self, CS, model_v2, CC, new_desired_curvature):
     blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker)
-    if CC.latActive and self.twitch_guard_remaining > 0.0 and (blinker_dir == 0.0) and (self.turn_hold_curvature == 0.0):
-      new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo)
+    stop_dist = stopping_distance(CS.vEgo, CS.aEgo)
+    entering = CS.aEgo < -STOP_HOLD_MIN_DECEL and stop_dist < STOP_HOLD_DIST and abs(self.curvature) < STOP_HOLD_MAX_CURVATURE
+    holding = self.stop_hold_curvature is not None and stop_dist < STOP_HOLD_RELEASE_DIST
+    if blinker_dir != 0.0:
+      self.twitch_guard_armed = False
+    elif (CS.aEgo < -STOP_HOLD_MIN_DECEL and abs(self.curvature) < TWITCH_GUARD_STRAIGHT_LO and
+          stopping_distance(CS.vEgo, CS.aEgo, TWITCH_GUARD_MAX_SPEED) < STOP_GUARD_DIST):
+      self.twitch_guard_armed = True
+    if CC.latActive and blinker_dir == 0.0 and self.turn_hold_curvature == 0.0 and (entering or holding):
+      if CS.steeringPressed:
+        self.stop_hold_curvature = self.curvature
+      elif self.stop_hold_curvature is None:
+        if self.output_curvature is not None:
+          self.stop_hold_curvature = self.output_curvature
+        elif self.twitch_guard_armed:
+          self.stop_hold_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo)
+        else:
+          self.stop_hold_curvature = new_desired_curvature
+      new_desired_curvature = self.stop_hold_curvature
+      self.twitch_guard_armed = True
+    else:
+      self.stop_hold_curvature = None
+      if CC.latActive and self.twitch_guard_armed and (blinker_dir == 0.0) and (self.turn_hold_curvature == 0.0):
+        new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo)
     if blinker_dir == 0.0:
       self.turn_blinker_swept = 0.0
     else:
@@ -250,6 +293,7 @@ class ModelTurnAssist:
           if CS.vEgo < CURVATURE_HOLD_RELEASE_SPEED and (not self.turn_hold_done) and (lead_curvature * blinker_dir > abs(self.turn_hold_curvature)):
             held_mag = min(lead_curvature * blinker_dir, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL)
             self.turn_hold_curvature = math.copysign(held_mag, lead_curvature)
+    self.output_curvature = new_desired_curvature
     return new_desired_curvature
 
 

openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py

@@ -0,0 +1,129 @@
+import unittest
+from types import SimpleNamespace
+
+from openpilot.starpilot.lateral.model_turn_assist import (
+  STOP_HOLD_MAX_SPEED, TWITCH_GUARD_FLOOR, TWITCH_GUARD_MAX_SPEED, ModelTurnAssist, update_twitch_guard,
+)
+
+STRAIGHT = SimpleNamespace(position=SimpleNamespace(x=[0., 1., 2., 3.], y=[0., 0., 0., 0.]),
+                           meta=SimpleNamespace(laneChangeState=0))
+LONG_STRAIGHT = SimpleNamespace(position=SimpleNamespace(x=[float(x) for x in range(0, 80, 4)], y=[0.] * 20),
+                                meta=SimpleNamespace(laneChangeState=0))
+
+
+def car(speed, accel, blinker=0, pressed=False):
+  return SimpleNamespace(vEgo=speed, aEgo=accel, rightBlinker=blinker == 1, leftBlinker=blinker == -1,
+                         steeringPressed=pressed, steeringTorque=0.)
+
+
+def step(assist, speed, accel, desired, active=True, model=STRAIGHT, **kwargs):
+  return assist.update(car(speed, accel, **kwargs), model, SimpleNamespace(latActive=active), desired)
+
+
+class TestStopHold(unittest.TestCase):
+  def test_holds_command_through_final_metre_of_stop(self):
+    assist = ModelTurnAssist()
+    self.assertEqual(step(assist, 2.0, -2.0, .002), .002)
+    for speed, desired in ((1.5, .004), (1.0, .010), (.6, .014)):
+      self.assertEqual(step(assist, speed, -2.0, desired), .002)
+
+  def test_releases_when_car_stops_slowing(self):
+    assist = ModelTurnAssist()
+    step(assist, 1.0, -2.0, .001)
+    self.assertEqual(step(assist, 1.0, -.5, .01), .001)
+    self.assertEqual(step(assist, 1.0, 0., .01), .01)
+    self.assertIsNone(assist.stop_hold_curvature)
+
+  def test_does_not_engage_far_from_stop_or_above_speed(self):
+    for speed, accel in ((2.0, -.5), (1.0, -.2), (STOP_HOLD_MAX_SPEED, -5.), (1.0, 1.)):
+      with self.subTest(speed=speed, accel=accel):
+        assist = ModelTurnAssist()
+        self.assertEqual(step(assist, speed, accel, .01), .01)
+        self.assertIsNone(assist.stop_hold_curvature)
+
+  def test_blinker_or_inactive_never_holds(self):
+    for direction in (-1, 1):
+      assist = ModelTurnAssist()
+      step(assist, 1.0, -2.0, .001)
+      self.assertNotEqual(step(assist, 1.0, -2.0, direction * .02, blinker=direction), .001)
+      self.assertIsNone(assist.stop_hold_curvature)
+    assist = ModelTurnAssist()
+    self.assertEqual(step(assist, 1.0, -2.0, .01, active=False), .01)
+    self.assertIsNone(assist.stop_hold_curvature)
+
+  def test_driver_steering_moves_hold_to_measured_curvature(self):
+    assist = ModelTurnAssist()
+    step(assist, 1.0, -2.0, .001)
+    assist.curvature = -.003
+    self.assertEqual(step(assist, .8, -2.0, .015, pressed=True), -.003)
+    assist.curvature = .009
+    self.assertEqual(step(assist, .6, -2.0, .015), -.003)
+
+  def test_latches_guarded_command_not_raw_model(self):
+    assist = ModelTurnAssist()
+    assist.twitch_guard_armed = True
+    self.assertEqual(step(assist, 1.0, 0., .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR)
+    self.assertEqual(step(assist, .6, -2.0, .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR)
+
+  def test_does_not_freeze_a_turn_in_progress(self):
+    assist = ModelTurnAssist()
+    assist.curvature = .02
+    self.assertEqual(step(assist, 1.0, -2.0, .04), .04)
+    self.assertIsNone(assist.stop_hold_curvature)
+
+
+class TestTwitchGuardArming(unittest.TestCase):
+  def test_armed_from_standstill_until_max_speed(self):
+    self.assertTrue(update_twitch_guard(False, 0., True))
+    self.assertTrue(update_twitch_guard(False, .3, False))
+    self.assertFalse(update_twitch_guard(False, 1., False))
+    for speed in (.5, 2., TWITCH_GUARD_MAX_SPEED - .01):
+      self.assertTrue(update_twitch_guard(True, speed, False))
+    self.assertFalse(update_twitch_guard(True, TWITCH_GUARD_MAX_SPEED, False))
+    self.assertFalse(update_twitch_guard(True, float('nan'), False))
+
+  def test_released_stop_hold_hands_off_to_guard(self):
+    assist = ModelTurnAssist()
+    step(assist, 1.0, -2.0, .001, model=LONG_STRAIGHT)
+    self.assertTrue(assist.twitch_guard_armed)
+    self.assertEqual(step(assist, 1.0, 0., .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR)
+
+  def test_unarmed_guard_leaves_creep_command(self):
+    assist = ModelTurnAssist()
+    self.assertEqual(step(assist, 1.0, 0., .012, model=LONG_STRAIGHT), .012)
+
+  def test_stop_approach_arms_guard_only_with_straight_wheel(self):
+    assist = ModelTurnAssist()
+    self.assertEqual(step(assist, 3.0, -1.5, .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR)
+    assist = ModelTurnAssist()
+    assist.curvature = .006
+    self.assertEqual(step(assist, 3.0, -1.5, .012, model=LONG_STRAIGHT), .012)
+    assist = ModelTurnAssist()
+    self.assertEqual(step(assist, 3.0, -.2, .012, model=LONG_STRAIGHT), .012)
+
+  def test_standstill_keeps_last_command_instead_of_raw_model(self):
+    assist = ModelTurnAssist()
+    assist.twitch_guard_armed = True
+    self.assertEqual(step(assist, 1.0, 0., .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR)
+    self.assertEqual(assist.update_stopped(car(0., 0.), .012), TWITCH_GUARD_FLOOR)
+    assist.curvature = -.004
+    self.assertEqual(assist.update_stopped(car(0., 0., pressed=True), .012), -.004)
+    self.assertEqual(assist.update_stopped(car(0., 0.), .012), -.004)
+
+  def test_standstill_passes_raw_model_with_blinker_or_no_history(self):
+    self.assertEqual(ModelTurnAssist().update_stopped(car(0., 0.), .012), .012)
+    assist = ModelTurnAssist()
+    step(assist, 1.0, 0., .003)
+    self.assertEqual(assist.update_stopped(car(0., 0., blinker=1), .012), .012)
+    self.assertIsNone(assist.output_curvature)
+
+  def test_blinker_disarms_guard(self):
+    assist = ModelTurnAssist()
+    assist.twitch_guard_armed = True
+    step(assist, 2.0, 0., .012, model=LONG_STRAIGHT, blinker=1)
+    self.assertFalse(assist.twitch_guard_armed)
+    self.assertEqual(step(assist, 2.0, 0., .012, model=LONG_STRAIGHT), .012)
+
+
+if __name__ == '__main__':
+  unittest.main()

openpilot/starpilot/longitudinal/force_stop.py

@@ -10,8 +10,18 @@ STANDSTILL_CLEAR_NS = 750_000_000
 SIGN_MAX_SPEED_MPS = 17.88
 SIGN_MODEL_AGREE_M = 50.0
 SIGN_SEED_M = 27.0
+SIGN_ESTIMATE_HOLD_NS = 2_000_000_000
+# Over a long model path a sign is only believed if stopping at its estimate would be a normal stop.
+SIGN_LONG_PATH_MAX_DECEL = 2.5
 SIGN_APPROACH_DECEL = 1.0
 MODEL_APPROACH_DECEL = .65
+# Stopped behind a car at a sign: the line is assumed one car length past that car's rear.
+QUEUE_LEAD_MAX_M = 8.0
+QUEUE_LEAD_LENGTH_M = 4.5
+# A car still inside this margin past the line may just be moving up the queue, not through the intersection.
+QUEUE_CLEAR_M = 5.0
+QUEUE_SIGN_RECENT_NS = 10_000_000_000
+QUEUE_MAX_SPEED_MPS = 6.0
 
 
 @dataclass(frozen=True)
@@ -56,6 +66,8 @@ class StopFrame:
   resume_requested: bool = False
   perception_available: bool = True
   sign: bool = False
+  dashboard_sign: bool = False
+  vision_distance_m: float | None = None
 
 
 @dataclass(frozen=True)
@@ -96,6 +108,48 @@ class ForceStop:
     self.previously_enabled = False
     self.distance = 0.0
     self.distance_cap = 0.0
+    self.queue_sign_ns = 0
+    self.queue_line = None
+    self._clear_sign_estimate()
+
+  def _clear_sign_estimate(self):
+    self.dash_distance = None
+    self.vision_distance = None
+    self.vision_samples = 0
+    self.sign_evidence_ns = 0
+
+  def _sign_estimate(self, frame: StopFrame, now: int, travel: float) -> float | None:
+    if self.sign_evidence_ns and now - self.sign_evidence_ns > SIGN_ESTIMATE_HOLD_NS:
+      self._clear_sign_estimate()
+    if self.dash_distance is not None:
+      self.dash_distance = max(0.0, self.dash_distance - travel)
+    if self.vision_distance is not None:
+      self.vision_distance = max(0.0, self.vision_distance - travel)
+    vision = frame.vision_distance_m
+    if vision is not None and math.isfinite(vision) and vision >= 0:
+      self.vision_samples += 1
+      self.vision_distance = vision if self.vision_distance is None else (
+        self.vision_distance + (vision - self.vision_distance) / self.vision_samples)
+    if frame.dashboard_sign or frame.sign and self.vision_distance is None:
+      self.dash_distance = SIGN_SEED_M if self.dash_distance is None else min(self.dash_distance, SIGN_SEED_M)
+    if frame.sign or vision is not None:
+      self.sign_evidence_ns = now
+    estimates = [value for value in (self.dash_distance, self.vision_distance) if value is not None]
+    return sum(estimates) / len(estimates) if estimates else None
+
+  def _queue_step(self, frame: StopFrame, now: int, travel: float, lead_blocked: bool):
+    if frame.sign and lead_blocked:
+      self.queue_sign_ns = now
+    queued_lead = frame.lead_present and frame.lead_speed_mps < 1.0 and frame.lead_distance_m < QUEUE_LEAD_MAX_M
+    arm = frame.standstill and self.queue_sign_ns and now - self.queue_sign_ns <= QUEUE_SIGN_RECENT_NS
+    if queued_lead and (arm or self.queue_line is not None):
+      self.queue_line = frame.lead_distance_m + QUEUE_LEAD_LENGTH_M
+    elif self.queue_line is not None:
+      self.queue_line -= travel
+    if self.queue_line is not None and (frame.pedal_override or frame.resume_requested or self.queue_line <= 0 or
+                                        frame.speed_mps > QUEUE_MAX_SPEED_MPS):
+      self.queue_line = None
+      self.queue_sign_ns = 0
 
   def _clear_standstill(self):
     self.standstill_since_ns = 0
@@ -121,7 +175,10 @@ class ForceStop:
       self.reset()
       return StopPlan()
     available = frame.perception_available
-    if available and (frame.light or frame.raw_model_stopped) or frame.sign:
+    sign_estimate = self._sign_estimate(frame, now, max(0.0, speed) * dt)
+    lead = (frame.lead_present and frame.lead_distance_m < tune.lead_veto_m and frame.lead_speed_mps < speed + 2)
+    self._queue_step(frame, now, max(0.0, speed) * dt, lead or frame.tracking_lead)
+    if available and (frame.light or frame.raw_model_stopped) or frame.sign or self.queue_line is not None:
       if not frame.standstill:
         self.stop_seen_ns = now
     if frame.standstill:
@@ -129,16 +186,21 @@ class ForceStop:
     stop_then_turn = bool(self.stop_seen_ns and now - self.stop_seen_ns < 4_000_000_000)
     turn = speed <= 18 * 0.44704 and frame.blinker and abs(frame.steering_angle_deg) >= 25 and not stop_then_turn
     curved = available and abs(curvature) >= .003 and not stop_then_turn
-    lead = (frame.lead_present and frame.lead_distance_m < tune.lead_veto_m and frame.lead_speed_mps < speed + 2)
     model_active = available and horizon < (108.0 if self.activation_gate and frame.light else 100.0)
     self.activation_gate = model_active and frame.light
     permitted = (available and now >= self.override_until_ns and not frame.driving_in_curve and not turn and
                  not frame.tracking_lead and not lead)
     model_path = frame.light and model_active and permitted and not curved
-    sign_path = frame.sign and permitted and speed < SIGN_MAX_SPEED_MPS and horizon < SIGN_MODEL_AGREE_M
-    if sign_path:
+    plausible = sign_estimate is None or speed * speed <= 2 * SIGN_LONG_PATH_MAX_DECEL * max(sign_estimate, 1.0)
+    sign_path = (frame.sign and permitted and speed < SIGN_MAX_SPEED_MPS and
+                 (horizon < SIGN_MODEL_AGREE_M or self.forcing and plausible))
+    line_blocked = (self.queue_line is not None and frame.lead_present and
+                    frame.lead_distance_m < self.queue_line + QUEUE_CLEAR_M)
+    queue_path = (self.queue_line is not None and not line_blocked and not frame.standstill and available and
+                  now >= self.override_until_ns and not frame.driving_in_curve and not turn)
+    if sign_path or queue_path:
       self.sign_confirmed = True
-    active = model_path or sign_path
+    active = model_path or sign_path or queue_path
     if model_path:
       self.from_light = True
       self.light_clear_ns = 0
@@ -177,7 +239,7 @@ class ForceStop:
     if not available and not self.forcing:
       self.timer = 0
     elif active and not frame.standstill:
-      self.timer = min(2.0, self.timer + DT_MDL * (2 if frame.sign else 1))
+      self.timer = min(2.0, self.timer + DT_MDL * (2 if frame.sign or queue_path else 1))
     elif turn and not self.forcing and not frame.standstill:
       self.timer = 0
     elif held:
@@ -187,7 +249,7 @@ class ForceStop:
     else:
       self.timer = max(0, self.timer - DT_MDL * .25)
     force = self.timer >= .5 or self.forcing and not frame.standstill or held
-    light_cleared = (not held and self.forcing and self.from_light and not frame.standstill and
+    light_cleared = (not held and self.forcing and self.from_light and not frame.standstill and not self.sign_confirmed and
                      available and not frame.light and not frame.sign and go_scene)
     low_speed_hold = (light_cleared and tune.low_speed_hold_mps is not None and self.entry_speed is not None and
                       speed <= tune.low_speed_hold_mps and speed < self.entry_speed - .25)
@@ -224,12 +286,12 @@ class ForceStop:
         elif available:
           self.distance = min(self.distance, horizon)
         self.distance_cap = max(0.0, self.distance_cap - speed * dt)
-        self.distance = min(self.distance, self.distance_cap + 15 * min(self.distance_cap / 60, 1))
-        if frame.sign and available:
-          if horizon < SIGN_MODEL_AGREE_M:
-            self.distance = min(self.distance, SIGN_SEED_M)
-          if horizon < SIGN_SEED_M:
-            self.distance = horizon
+        cap = self.distance_cap + 15 * min(self.distance_cap / 60, 1)
+        self.distance = min(self.distance, cap)
+        if sign_estimate is not None and (self.sign_confirmed or frame.sign and available and horizon < SIGN_MODEL_AGREE_M):
+          self.distance = min(sign_estimate, cap)
+        if self.queue_line is not None and self.sign_confirmed:
+          self.distance = self.queue_line
         effective = self.distance + offset + tune.distance_bias_m
         decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL
         ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m))
@@ -245,10 +307,12 @@ class ForceStop:
         if effective > tune.handoff_m:
           ceiling = math.sqrt(2 * MODEL_APPROACH_DECEL * (effective - tune.handoff_m))
     self.previously_enabled = frame.enabled
-    stop_length = self.distance if self.forcing and self.distance > tune.handoff_m else approach
+    stop_length = self.distance if self.forcing else approach
     # The MPC obstacle includes its standing-distance term; adding it at the
     # native boundary avoids moving the actual stop line by that buffer.
-    obstacle = stop_length * .93 + offset + tune.distance_bias_m if stop_length > tune.handoff_m else None
+    # The offset moves the handoff with the ceiling; otherwise a positive offset creeps with no stop target.
+    obstacle = (stop_length * .93 + offset + tune.distance_bias_m
+                if stop_length > 0 and stop_length + offset > tune.handoff_m else None)
     return StopPlan(now, ceiling, obstacle, self.forcing, bool(self.forcing and ceiling is not None and ceiling <= .5),
                     self.distance if self.forcing else 0.0, approach,
                     tune.jerk_scale if self.forcing or approach > 0 else 1.0, bool(held and self.forcing and not overridden),

openpilot/starpilot/longitudinal/force_stop_runtime.py

@@ -51,6 +51,7 @@ class ForceStopRuntime:
     self.policy = ForceStop()
     self.resume = StopResume()
     self.vision_stop = StopConfirmation()
+    self.vision_ranged_ns = 0
     self.light = StopLightDetector()
     self.lead = LeadDetector()
     self.plan = StopPlan()
@@ -66,6 +67,7 @@ class ForceStopRuntime:
 
   def reset(self):
     self.vision_stop.reset()
+    self.vision_ranged_ns = 0
     self.resume.reset()
     self.policy.reset()
     self.light.reset()
@@ -184,14 +186,19 @@ class ForceStopRuntime:
         return self.plan
       vision_sign = self.vision_stop.observe(
         read_stop_evidence(sm, now_ns=now_ns, now_boot_ns=now_boot_ns, drive_id=drive_id), now_ns=now_ns)
+      vision_fresh = bool(vision_sign and 0 <= now_ns - vision_sign <= CONTROL_STOP_MAX_AGE_NS)
+      vision_distance = None
+      if vision_fresh and vision_sign != self.vision_ranged_ns and self.vision_stop.distance_m is not None:
+        self.vision_ranged_ns = vision_sign
+        vision_distance = self.vision_stop.distance_m - float(car.vEgo) * (stamp - vision_sign) / 1e9
+      dashboard_sign = dashboard_stop_sign(sm, now_ns=now_ns, drive_id=drive_id)
       frame = StopFrame(stamp, float(car.vEgo), horizon, light_context, horizon is not None and horizon < 50,
                         bool(model.action.shouldStop), bool(car.standstill), True, bool(car.gasPressed),
                         tracked is not None and tracked.tracked is True, bool(lead.present), float(lead.dRel), float(lead.vLead),
                         raw_curve[1] if raw_curve is not None else False, curvature[0] if curvature is not None else None,
                         bool(car.leftBlinker or car.rightBlinker), float(car.steeringAngleDeg), resume_requested=resume_requested,
-                        perception_available=perception_available,
-                        sign=dashboard_stop_sign(sm, now_ns=now_ns, drive_id=drive_id) or
-                        bool(vision_sign and 0 <= now_ns - vision_sign <= CONTROL_STOP_MAX_AGE_NS))
+                        perception_available=perception_available, sign=dashboard_sign or vision_fresh,
+                        dashboard_sign=dashboard_sign, vision_distance_m=vision_distance)
       self.plan = self.policy.step(frame, vehicle_tune(str(cp.carFingerprint)), self.offset)
       if takeoff_enabled:
         self.plan = replace(self.plan, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light)

openpilot/starpilot/longitudinal/tests/test_force_stop.py

@@ -229,6 +229,138 @@ class ForceStopTests(unittest.TestCase):
           plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune)
           self.assertFalse(plan.forcing)
 
+  def test_latched_sign_survives_light_and_sign_dropout(self):
+    for tick in range(15):
+      self.owner.step(frame(tick, speed_mps=8., horizon_m=45., sign=True), self.tune)
+    self.assertTrue(self.owner.sign_confirmed)
+    for tick in range(15, 45):
+      plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=190., light=False), self.tune)
+      self.assertTrue(plan.forcing)
+
+  def test_sign_seed_never_moves_the_stop_farther(self):
+    for tick in range(50):
+      previous = self.owner.step(frame(tick, speed_mps=8., horizon_m=45., sign=True), self.tune)
+    self.assertLess(previous.tracked_distance_m, 20.)
+    plan = self.owner.step(frame(50, speed_mps=8., horizon_m=20., sign=True), self.tune)
+    self.assertLessEqual(plan.tracked_distance_m, previous.tracked_distance_m)
+
+  def test_sign_latches_and_seeds_over_long_path_once_light_committed(self):
+    for tick in range(15):
+      plan = self.owner.step(frame(tick, horizon_m=60.), self.tune)
+    self.assertTrue(plan.forcing)
+    self.assertGreater(plan.tracked_distance_m, 50.)
+    plan = self.owner.step(frame(15, horizon_m=60., sign=True), self.tune)
+    self.assertTrue(self.owner.sign_confirmed)
+    self.assertLessEqual(plan.tracked_distance_m, 27.)
+    plan = self.owner.step(frame(16, horizon_m=60., light=False), self.tune)
+    self.assertLess(plan.tracked_distance_m, 27.)
+
+  def test_long_path_sign_needing_a_hard_stop_is_ignored(self):
+    for tick in range(15):
+      plan = self.owner.step(frame(tick, speed_mps=13., horizon_m=90.), self.tune)
+    self.assertTrue(plan.forcing)
+    for tick in range(15, 55):
+      plan = self.owner.step(frame(tick, speed_mps=13., horizon_m=90., sign=True, dashboard_sign=True), self.tune)
+      self.assertFalse(self.owner.sign_confirmed)
+      self.assertGreater(plan.tracked_distance_m, 50.)
+
+  def test_long_path_sign_alone_does_not_commit(self):
+    for tick in range(20):
+      plan = self.owner.step(frame(tick, horizon_m=60., light=False, sign=True), self.tune)
+    self.assertFalse(plan.forcing)
+    self.assertFalse(self.owner.sign_confirmed)
+
+  def test_vision_distance_replaces_assumed_sign_distance(self):
+    for tick in range(15):
+      plan = self.owner.step(frame(tick, horizon_m=60.), self.tune)
+    plan = self.owner.step(frame(15, horizon_m=60., sign=True, vision_distance_m=35.), self.tune)
+    self.assertTrue(self.owner.sign_confirmed)
+    self.assertAlmostEqual(plan.tracked_distance_m, 35.)
+    plan = self.owner.step(frame(16, horizon_m=60., sign=True), self.tune)
+    self.assertAlmostEqual(plan.tracked_distance_m, 34.5)
+    plan = self.owner.step(frame(17, horizon_m=60., sign=True, vision_distance_m=31.), self.tune)
+    self.assertAlmostEqual(plan.tracked_distance_m, 32.5)
+
+  def test_dashboard_and_vision_distances_are_averaged(self):
+    for tick in range(15):
+      self.owner.step(frame(tick, horizon_m=60.), self.tune)
+    plan = self.owner.step(frame(15, horizon_m=60., sign=True, dashboard_sign=True), self.tune)
+    self.assertAlmostEqual(plan.tracked_distance_m, 27.)
+    plan = self.owner.step(frame(16, horizon_m=60., sign=True, dashboard_sign=True, vision_distance_m=35.), self.tune)
+    self.assertAlmostEqual(plan.tracked_distance_m, (26.5 + 35.) / 2)
+
+  def test_sign_distance_expires_without_evidence_and_never_jumps_back(self):
+    for tick in range(15):
+      self.owner.step(frame(tick, horizon_m=60.), self.tune)
+    self.owner.step(frame(15, horizon_m=60., sign=True, vision_distance_m=30.), self.tune)
+    for tick in range(16, 70):
+      plan = self.owner.step(frame(tick, speed_mps=4., horizon_m=60., light=True), self.tune)
+      self.assertLess(plan.tracked_distance_m, 30.)
+    self.assertIsNone(self.owner.vision_distance)
+
+  def queue_behind_lead(self, sign=True):
+    for tick in range(10):
+      plan = self.owner.step(frame(tick, speed_mps=8., horizon_m=45., light=False, sign=sign, lead_present=True,
+                                   lead_distance_m=20., lead_speed_mps=0.), self.tune)
+      self.assertFalse(plan.forcing)
+    for tick in range(10, 30):
+      plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=5., light=False, standstill=True, lead_present=True,
+                                   lead_distance_m=5., lead_speed_mps=0.), self.tune)
+      self.assertFalse(plan.forcing)
+
+  def test_sign_behind_stopped_lead_stops_at_line_after_lead_leaves(self):
+    self.queue_behind_lead()
+    self.assertAlmostEqual(self.owner.queue_line, 9.5)
+    for tick in range(30, 35):
+      plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=12., light=False, standstill=True, lead_present=True,
+                                   lead_distance_m=7., lead_speed_mps=3.), self.tune)
+      self.assertFalse(plan.forcing)
+    for tick in range(35, 45):
+      plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False), self.tune)
+    self.assertTrue(plan.forcing)
+    self.assertTrue(self.owner.sign_confirmed)
+    self.assertAlmostEqual(plan.tracked_distance_m, 9.5 - 10 * .05)
+    for tick in range(45, 100):
+      plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=15., light=False, standstill=True), self.tune)
+      self.assertTrue(plan.manual_hold)
+    released = self.owner.step(frame(100, speed_mps=0., horizon_m=15., light=False, standstill=True,
+                                     pedal_override=True), self.tune)
+    self.assertFalse(released.manual_hold)
+    self.assertIsNone(self.owner.queue_line)
+
+  def test_lead_stop_without_sign_is_not_remembered(self):
+    self.queue_behind_lead(sign=False)
+    self.assertIsNone(self.owner.queue_line)
+    for tick in range(30, 50):
+      plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False), self.tune)
+      self.assertFalse(plan.forcing)
+
+  def test_lead_moving_up_the_queue_is_followed_not_stopped_for(self):
+    self.queue_behind_lead()
+    for tick in range(30, 60):
+      plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False, lead_present=True,
+                                   lead_distance_m=6., lead_speed_mps=1.5), self.tune)
+      self.assertFalse(plan.forcing)
+    plan = self.owner.step(frame(60, speed_mps=0., horizon_m=5., light=False, standstill=True, lead_present=True,
+                                 lead_distance_m=4., lead_speed_mps=0.), self.tune)
+    self.assertFalse(plan.forcing)
+    self.assertAlmostEqual(self.owner.queue_line, 8.5)
+
+  def test_driver_gas_while_queued_forgets_the_sign(self):
+    self.queue_behind_lead()
+    self.owner.step(frame(30, speed_mps=0., horizon_m=5., light=False, standstill=True, pedal_override=True), self.tune)
+    self.assertIsNone(self.owner.queue_line)
+    for tick in range(31, 50):
+      plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False), self.tune)
+      self.assertFalse(plan.forcing)
+
+  def test_positive_offset_keeps_stop_target_until_native_stop(self):
+    for tick in range(40):
+      plan = self.owner.step(frame(tick, horizon_m=12.), self.tune, 8)
+      if plan.forcing:
+        self.assertTrue(plan.should_stop or plan.obstacle_m is not None)
+    self.assertTrue(plan.should_stop)
+
   def test_odometry_ceiling_bounds_reopened_model(self):
     for tick in range(15):
       self.owner.step(frame(tick), self.tune)

openpilot/starpilot/speed_limits/vision/stop_observation.py

@@ -2,12 +2,17 @@
 from dataclasses import dataclass
 import math
 
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.starpilot.speed_limits.vision.observation import MODEL_ID, MAX_PAIR_SKEW_NS
 
 STOP_MAX_AGE_NS = 1_500_000_000
 CONTROL_STOP_MAX_AGE_NS = 750_000_000
 RECEIPT_MAX_AGE_NS = 1_000_000_000
 STOP_CONFIRM_CONFIDENCE = .7
+STOP_SIGN_HEIGHT_M = .762
+# Fitted on 17 confirmed sign approaches: where the car stops relative to the box range.
+STOP_RANGE_OFFSET_M = 2.0
+ROAD_FOCAL_PX = {camera.narrow_road.size: camera.narrow_road.focal_length for camera in DEVICE_CAMERAS.values()}
 
 
 @dataclass(frozen=True)
@@ -66,6 +71,16 @@ def read_stop_evidence(sm, *, now_ns: int, now_boot_ns: int, drive_id: int,
     return None
 
 
+def stop_distance(sample: StopEvidence, box: StopBox) -> float | None:
+  focal = ROAD_FOCAL_PX.get((sample.width, sample.height)) if sample.stream == 'road' else None
+  x1, y1, x2, y2 = box.bbox
+  if not focal or y2 <= y1:
+    return None
+  across = ((x1 + x2) / 2 - sample.width / 2) / focal
+  up = ((y1 + y2) / 2 - sample.height / 2) / focal
+  return focal * STOP_SIGN_HEIGHT_M / (y2 - y1) / math.sqrt(1 + across ** 2 + up ** 2) + STOP_RANGE_OFFSET_M
+
+
 def _matches(a: StopBox, b: StopBox) -> bool:
   ax, ay, ar, ab = a.bbox
   bx, by, br, bb = b.bbox
@@ -83,6 +98,7 @@ class StopConfirmation:
     self.previous: StopEvidence | None = None
     self.confirmed_ns = 0
     self.first_seen_ns = 0
+    self.distance_m: float | None = None
 
   def observe(self, sample: StopEvidence | None, *, now_ns: int) -> int:
     if sample is None or not 0 <= now_ns - sample.observed_ns < STOP_MAX_AGE_NS:
@@ -102,13 +118,15 @@ class StopConfirmation:
       self.reset()
       return 0
     candidates = tuple(box for box in sample.boxes if box.confidence >= STOP_CONFIRM_CONFIDENCE)
-    coherent = (previous is not None and 100_000_000 <= sample.observed_ns - previous.observed_ns < STOP_MAX_AGE_NS and
-                any(_matches(old, new) for old in previous.boxes if old.confidence >= STOP_CONFIRM_CONFIDENCE for new in candidates))
+    matched = tuple(new for new in candidates if previous is not None and
+                    100_000_000 <= sample.observed_ns - previous.observed_ns < STOP_MAX_AGE_NS and
+                    any(_matches(old, new) for old in previous.boxes if old.confidence >= STOP_CONFIRM_CONFIDENCE))
     if not candidates:
       self.first_seen_ns = 0
-    elif not coherent:
+    elif not matched:
       self.first_seen_ns = sample.observed_ns
     self.previous = sample
-    self.confirmed_ns = (sample.observed_ns if candidates and coherent and
+    self.confirmed_ns = (sample.observed_ns if matched and
                          sample.observed_ns - self.first_seen_ns >= 500_000_000 else 0)
+    self.distance_m = stop_distance(sample, max(matched, key=lambda box: box.confidence)) if self.confirmed_ns else None
     return self.confirmed_ns

openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py

@@ -121,6 +121,21 @@ class TestStopObservation(unittest.TestCase):
       owner.observe(evidence(), now_ns=BASE)
       self.assertEqual(owner.observe(evidence(BASE + 700_000_000, frame=15, boxes=(box,)), now_ns=BASE + 800_000_000), 0)
 
+  def test_confirmed_road_box_carries_forward_distance(self):
+    box = StopBox(.875, (1214, 400, 1294, 480))
+    for stream, expected in (('road', 27.025), ('wideRoad', None)):
+      owner = StopConfirmation()
+      owner.observe(evidence(boxes=(box,), stream=stream, width=1928, height=1208), now_ns=BASE)
+      self.assertIsNone(owner.distance_m)
+      second = evidence(BASE + 700_000_000, frame=15, boxes=(box,), stream=stream, width=1928, height=1208)
+      self.assertEqual(owner.observe(second, now_ns=BASE + 800_000_000), second.observed_ns)
+      if expected is None:
+        self.assertIsNone(owner.distance_m)
+      else:
+        self.assertAlmostEqual(owner.distance_m, expected, places=2)
+      owner.observe(None, now_ns=BASE + 900_000_000)
+      self.assertIsNone(owner.distance_m)
+
   def test_brief_warning_burst_cannot_confirm_but_sustained_sign_can(self):
     owner = StopConfirmation()
     for i in range(3):