Faster takeoffs: model-gated standstill light veto and breakaway

lead_takeoff was blocked at every standstill because Force Stop perception needs the strict model horizon. Fall back to the tolerant standstill horizon for the takeoff light veto only: veto when the model says stop or its path is under the CEM release distance.

While stationary behind a confirmed departing lead, raise the target to the profile ceiling so the car breaks away promptly; the planner's lead-follow target applies again once the car moves.

openpilot/starpilot/lead_tracking.py

@@ -158,10 +158,12 @@ class LeadDetector:
     lead_brake = _number(_get(radar_lead, 'aLeadK'), -20.0, 20.0)
     prob = _number(_get(radar_lead, 'modelProb'), 0.0, 1.0)
     y_rel = _number(_get(radar_lead, 'yRel'), -20.0, 20.0)
-    path = _path_at_distance(model, distance) if model_fresh and distance is not None else None
+    # A stopped model's path backsteps sub-millimetre near its origin.
+    moving = standstill is False
+    path = _path_at_distance(model, distance) if moving and model_fresh and distance is not None else None
     if (
       not radar_fresh
-      or not model_fresh
+      or (moving and not model_fresh)
       or not car_fresh
       or not headway_fresh
       or raw_present is None
@@ -172,13 +174,12 @@ class LeadDetector:
       or observed is None
       or now is None
       or not 0.0 <= now - observed <= SOURCE_MAX_AGE_S
-      or path is None
+      or (moving and path is None)
       or None in (distance, lead_speed, lead_brake, prob, y_rel)
     ):
       self.reset()
       return unknown
     assert distance is not None and lead_speed is not None and lead_brake is not None and prob is not None and y_rel is not None
-    model_length, path_y = path
     if self.last_observed_mono_s is not None:
       interval = observed - self.last_observed_mono_s
       if interval <= 0.0:
@@ -187,7 +188,9 @@ class LeadDetector:
         self.reset()
         return unknown
     self.last_observed_mono_s = observed
-    if not standstill:
+    if moving:
+      assert path is not None
+      model_length, path_y = path
       candidate = _should_track(raw_present, distance, model_length, speed, lead_speed, raw_radar)
       continuity = self.tracked or self.filter_value >= CONTINUITY_THRESHOLD
       if not candidate and continuity:

openpilot/starpilot/longitudinal/force_stop_runtime.py

@@ -4,8 +4,9 @@ from dataclasses import replace
 
 from openpilot.starpilot.conditional_mode.curve import frozen_raw_curve
 from openpilot.starpilot.conditional_mode.preferences import CEMOptions, decode_preferences, PreferenceError
-from openpilot.starpilot.conditional_mode.projection import _horizon
-from openpilot.starpilot.conditional_mode.stop import StopFrame as LightFrame, StopLead, StopLightDetector
+from openpilot.starpilot.conditional_mode.projection import STANDSTILL_SPEED_NOISE_MPS, _horizon, _standstill_clear_horizon
+from openpilot.starpilot.conditional_mode.stop import (FROZEN_RAW_STOP_DISTANCE_M, STOP_MODEL_RELEASE_MARGIN_M,
+                                                       StopFrame as LightFrame, StopLead, StopLightDetector)
 from openpilot.starpilot.lead_tracking import LeadDetector
 from openpilot.starpilot.longitudinal.force_stop import ForceStop, StopFrame, StopPlan, vehicle_tune
 from openpilot.starpilot.model_geometry import road_curvature
@@ -18,6 +19,16 @@ OFFSET_KEY = 'ForceStopDistanceOffset'
 SOURCE_AGE_NS = 150_000_000
 
 
+def standstill_takeoff_light(model, car) -> bool | None:
+  """Takeoff-only; Force Stop's own policy keeps strict geometry."""
+  if car.standstill is not True or not 0.0 <= car.vEgo <= STANDSTILL_SPEED_NOISE_MPS:
+    return None
+  horizon = _standstill_clear_horizon(model)
+  if horizon is None:
+    return None
+  return bool(model.action.shouldStop) or horizon < FROZEN_RAW_STOP_DISTANCE_M + STOP_MODEL_RELEASE_MARGIN_M
+
+
 class ForceStopRuntime:
   def __init__(self, params):
     self.params = params
@@ -130,12 +141,16 @@ class ForceStopRuntime:
       else:
         self.light.reset()
         self.lead.reset()
+      takeoff_observed, takeoff_light = perception_available, bool(light_context)
+      if not perception_available:
+        fallback = standstill_takeoff_light(model, car)
+        if fallback is not None:
+          takeoff_observed, takeoff_light = True, fallback
       if takeoff_enabled and not self.enabled:
         # Perception may veto takeoff while Force Stop stays entirely inactive.
         self.resume.reset()
         self.policy.reset()
-        self.plan = StopPlan(model_ns=stamp, takeoff_light_observed=perception_available,
-                             takeoff_light=bool(light_context))
+        self.plan = StopPlan(model_ns=stamp, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light)
         return self.plan
       # Model trajectories can oscillate around zero velocity at a predicted
       # stop. Invalid geometry cannot acquire a stop or prove that it is clear;
@@ -156,7 +171,7 @@ class ForceStopRuntime:
                         perception_available=perception_available)
       self.plan = self.policy.step(frame, vehicle_tune(str(cp.carFingerprint)), self.offset)
       if takeoff_enabled:
-        self.plan = replace(self.plan, takeoff_light_observed=perception_available, takeoff_light=bool(light_context))
+        self.plan = replace(self.plan, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light)
       return self.plan
     except (AttributeError, KeyError, TypeError, ValueError, OverflowError, RuntimeError):
       self.reset()

openpilot/starpilot/longitudinal/lead_takeoff.py

@@ -6,6 +6,9 @@ This does not modify MPC costs, obstacles, trajectories or ordinary following.
 from dataclasses import dataclass
 import math
 
+BREAKAWAY_MIN_DISTANCE = 3.75
+BREAKAWAY_MIN_LEAD_SPEED = .6
+
 
 @dataclass(frozen=True)
 class Lead:
@@ -146,6 +149,10 @@ class LeadTakeoff:
     result = max(target, bounded)
     # Modern stop threshold is .1 m/s²: a lower saved/physical cap cannot prove release.
     output_stop = False if bounded >= .1 else should_stop
+    if (f.standstill and not output_stop and lead.distance >= BREAKAWAY_MIN_DISTANCE and
+        lead.speed >= BREAKAWAY_MIN_LEAD_SPEED and lead.speed - f.speed >= .25 and lead.acceleration >= -.2):
+      # Breaking away from standstill needs more than the planner's lead-follow target.
+      result = max(result, f.acceleration_max)
     if self.previous_should_stop and not output_stop and release_ready:
       self.release_until = f.now_ns + 1_500_000_000
     self.previous_should_stop = output_stop

openpilot/starpilot/longitudinal/tests/test_lead_takeoff.py

@@ -23,6 +23,19 @@ def test_confirmed_departure_releases_stopped_plan_in_normal_acc():
   assert policy.step(True, frame(7), 0., True) == (.35, False)
 
 
+def test_departing_lead_breakaway_holds_profile_ceiling_only_while_stationary():
+  departing = replace(frame().leads[0], distance=5., speed=1., acceleration=.5)
+  assert LeadTakeoff().step(True, frame(leads=(departing,), acceleration_max=2.), .2, False) == (2., False)
+  assert LeadTakeoff().step(True, frame(leads=(departing,), acceleration_max=1.2), .2, False) == (1.2, False)
+  rolling = LeadTakeoff().step(True, frame(leads=(departing,), acceleration_max=2., standstill=False, speed=.3), .2, False)
+  assert rolling[0] < 1. and rolling[1] is False
+  close = replace(departing, distance=3.6)
+  assert LeadTakeoff().step(True, frame(leads=(close,), acceleration_max=2., model_accel=.2), .2, False)[0] < 1.
+  braking = replace(departing, acceleration=-.5)
+  assert LeadTakeoff().step(True, frame(leads=(braking,), acceleration_max=2.), .2, False) == (.2, False)
+  assert LeadTakeoff().step(True, frame(leads=(departing,), acceleration_max=2., blocked=True), 0., True) == (0., True)
+
+
 def test_creep_confirmation_and_profile_cap_remain_effective():
   policy = LeadTakeoff()
   lead = replace(frame().leads[0], distance=6., speed=.3, acceleration=.1)
@@ -145,6 +158,24 @@ def test_force_stops_off_still_exposes_current_red_light_and_never_owns_control(
     detect.assert_not_called()
 
 
+def test_standstill_takeoff_light_uses_model_when_strict_geometry_fails():
+  from openpilot.starpilot.longitudinal.force_stop_runtime import standstill_takeoff_light
+
+  def model(end, *, stop=False, backstep=True):
+    xs = [end * index / 32 for index in range(33)]
+    if backstep:
+      xs[1:3] = [0.0005, 0.0002]
+    return NS(position=NS(x=xs), action=NS(shouldStop=stop))
+
+  stopped = NS(standstill=True, vEgo=0.)
+  assert standstill_takeoff_light(model(70.), stopped) is False
+  assert standstill_takeoff_light(model(70., stop=True), stopped) is True
+  assert standstill_takeoff_light(model(40.), stopped) is True
+  assert standstill_takeoff_light(model(0.), stopped) is None
+  assert standstill_takeoff_light(model(70.), NS(standstill=False, vEgo=0.)) is None
+  assert standstill_takeoff_light(model(70.), NS(standstill=True, vEgo=.5)) is None
+
+
 def test_projection_red_light_missing_observation_and_offcenter_conflict_veto():
   from openpilot.starpilot.longitudinal.tests.test_conditional_handoff import Frame as NativeFrame
   sm = NativeFrame()

openpilot/starpilot/tests/test_lead_tracking.py

@@ -192,6 +192,23 @@ class TestLeadDetector(unittest.TestCase):
     self.assertIsNone(stale.tracked)
     self.assertFalse(detector.tracked)
 
+  def test_standstill_retains_track_without_monotonic_model_path(self):
+    detector = LeadDetector()
+    for tick in range(15):
+      last = step(detector, tick, lead=wire_lead(distance=5, speed=0), speed=2.0)
+    self.assertTrue(last.tracked)
+    stopped = wire_model(horizon=70.0)
+    stub = list(stopped.position.x)
+    event = messaging.new_message('modelV2', valid=True)
+    event.modelV2.position.x = [0.0, 0.0005, 0.0002] + stub[3:]
+    event.modelV2.position.y = [0.0] * 33
+    backstep = messaging.log_from_bytes(event.to_bytes()).modelV2
+    for tick, kwargs in ((15, {'model': backstep}), (16, {'model_fresh': False})):
+      held = step(detector, tick, lead=wire_lead(distance=5, speed=0.8), speed=0.0, standstill=True, **kwargs)
+      self.assertTrue(held.tracked)
+    moving = step(detector, 17, lead=wire_lead(distance=6, speed=1.5), model=backstep, speed=0.5)
+    self.assertIsNone(moving.tracked)
+
   def test_serialized_model_origin_roundoff_only(self):
     detector = LeadDetector()
     roundoff = step(detector, 0, model=wire_model(origin=-6.64888977208733e-11))