Takeoff standstill tweak

openpilot/starpilot/conditional_mode/projection.py

@@ -25,7 +25,7 @@ from openpilot.starpilot.conditional_mode.stop import StopFrame, StopLead, StopL
 from openpilot.starpilot.conditional_mode.turn_scene import committed_turn_scene
 from openpilot.starpilot.model_geometry import (
   STANDSTILL_MODEL_PREFIX_NOISE_M as STANDSTILL_MODEL_PREFIX_NOISE_M, STANDSTILL_SPEED_NOISE_MPS, normalized_origin,
-  standstill_clear_horizon as _standstill_clear_horizon,
+  standstill_clear_horizon as _standstill_clear_horizon, standstill_release_speed,
 )
 
 
@@ -449,9 +449,8 @@ class SceneProjector:
         horizon = _horizon(model)
         stop_horizon = horizon
         model_should_stop = _boolean(_field(_field(model, 'action'), 'shouldStop'))
-        if (horizon is None and self.stop_detector.committed and model_should_stop is False and
-            speed is not None and speed <= STANDSTILL_SPEED_NOISE_MPS and
-            _field(car, 'standstill') is True and _number(_field(car, 'vEgoRaw'), high=80.0) == 0.0):
+        if (horizon is None and self.stop_detector.committed and model_should_stop is False and speed is not None and
+            standstill_release_speed(_field(car, 'standstill'), _field(car, 'vEgo'), _field(car, 'vEgoRaw'))):
           # A stationary model can backstep millimeters near its origin before
           # projecting forward. Use that endpoint only to clear an existing stop;
           # Curve geometry and cold stop acquisition retain strict validation.

openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py

@@ -231,8 +231,8 @@ class TestStopCommitTransport(unittest.TestCase):
     cases = ((RED_97_X, RED_97_V, False, 0., 0.),
              (CLEAR_97_X, CLEAR_97_V, True, 0., 0.),
              (material_tail, CLEAR_97_V, False, 0., 0.),
-             (CLEAR_97_X, CLEAR_97_V, False, .02, 0.),
-             (CLEAR_97_X, CLEAR_97_V, False, 0., .01))
+             (CLEAR_97_X, CLEAR_97_V, False, .06, 0.),
+             (CLEAR_97_X, CLEAR_97_V, False, 0., .06))
     for position, velocity, should_stop, speed, raw in cases:
       for _ in range(16):
         _, result = self.sample(standstill=True, ego_speed=speed, raw_speed=raw,

openpilot/starpilot/lead_tracking.py

@@ -12,7 +12,7 @@ import math
 import numpy as np
 
 from openpilot.common.realtime import DT_MDL
-from openpilot.starpilot.model_geometry import STANDSTILL_SPEED_NOISE_MPS, normalized_origin, standstill_clear_horizon
+from openpilot.starpilot.model_geometry import STANDSTILL_RELEASE_SPEED_MPS, normalized_origin, standstill_clear_horizon
 
 
 STOP_DISTANCE_M = 6.0
@@ -159,7 +159,7 @@ class LeadDetector:
     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
-    if path is None and model_fresh and standstill is True and speed is not None and speed <= STANDSTILL_SPEED_NOISE_MPS:
+    if path is None and model_fresh and standstill is True and speed is not None and speed <= STANDSTILL_RELEASE_SPEED_MPS:
       # Only retain a stopped track; these coordinates never enter interpolation.
       horizon = standstill_clear_horizon(model)
       try:

openpilot/starpilot/longitudinal/force_stop_runtime.py

@@ -10,7 +10,7 @@ from openpilot.starpilot.conditional_mode.stop import (
 )
 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 STANDSTILL_SPEED_NOISE_MPS, road_curvature, standstill_clear_horizon
+from openpilot.starpilot.model_geometry import road_curvature, standstill_clear_horizon, standstill_release_speed
 from openpilot.starpilot.saved_source import read_saved
 from openpilot.starpilot.longitudinal.stop_resume import StopResume
 
@@ -22,7 +22,7 @@ SOURCE_AGE_NS = 150_000_000
 
 def standstill_takeoff_light(model, car) -> bool | None:
   """Fresh takeoff metadata only; Force Stop keeps its strict spatial policy."""
-  if car.standstill is not True or not 0.0 <= car.vEgo <= STANDSTILL_SPEED_NOISE_MPS:
+  if not standstill_release_speed(car.standstill, car.vEgo, car.vEgoRaw):
     return None
   should_stop = model.action.shouldStop
   if type(should_stop) is not bool:

openpilot/starpilot/longitudinal/tests/test_lead_takeoff.py

@@ -215,6 +215,17 @@ def test_takeoff_light_carrier_rejects_corrupt_stop_preferences(tmp_path):
   assert path.read_bytes() == b'{'
 
 
+@pytest.mark.parametrize('standstill,speed,raw,expected', [
+  (True, -0.0008, 0., False), (True, .044, .044, False), (True, .06, .06, None),
+  (True, -0.0008, .02, None), (True, -.02, 0., None), (False, 0., 0., None)])
+def test_takeoff_metadata_accepts_recorded_standstill_speed_noise(standstill, speed, raw, expected):
+  from openpilot.starpilot.longitudinal.force_stop_runtime import standstill_takeoff_light
+  xs = [70. * index / 32 for index in range(33)]
+  xs[1:3] = [0.0005, 0.0002]
+  model = NS(position=NS(x=xs), action=NS(shouldStop=False))
+  assert standstill_takeoff_light(model, NS(standstill=standstill, vEgo=speed, vEgoRaw=raw)) is expected
+
+
 @pytest.mark.parametrize('stop,horizon,expected', [(False, 70., False), (True, 70., True), (False, 53.9, True)])
 def test_actual_takeoff_metadata_uses_fresh_prefix_without_force_stop_authority(tmp_path, stop, horizon, expected):
   from openpilot.common.params import Params

openpilot/starpilot/model_geometry.py

@@ -4,6 +4,7 @@ import math
 
 ORIGIN_ROUNDOFF_M = 1e-6
 STANDSTILL_SPEED_NOISE_MPS = 0.01
+STANDSTILL_RELEASE_SPEED_MPS = 0.05
 STANDSTILL_MODEL_PREFIX_NOISE_M = 0.01
 
 
@@ -14,6 +15,12 @@ def normalized_origin(distances: tuple) -> tuple:
   return distances
 
 
+def standstill_release_speed(standstill: object, v_ego: object, v_ego_raw: object) -> bool:
+  speed = _finite(v_ego, low=-STANDSTILL_SPEED_NOISE_MPS, high=STANDSTILL_RELEASE_SPEED_MPS)
+  raw = _finite(v_ego_raw, low=0.0, high=STANDSTILL_RELEASE_SPEED_MPS)
+  return standstill is True and speed is not None and raw is not None and (speed >= 0.0 or raw == 0.0)
+
+
 def _finite(value: object, *, low: float, high: float) -> float | None:
   if isinstance(value, bool) or not isinstance(value, (int, float)):
     return None

openpilot/starpilot/tests/test_lead_tracking.py

@@ -193,7 +193,7 @@ class TestLeadDetector(unittest.TestCase):
     self.assertFalse(detector.tracked)
 
   def test_stopped_track_retains_only_fresh_bounded_origin_geometry(self):
-    for variation in ('clear', 'stale', 'moving', 'bad_y', 'tail_backstep', 'flag_speed'):
+    for variation in ('clear', 'quantized', 'stale', 'moving', 'bad_y', 'tail_backstep', 'flag_speed'):
       detector = LeadDetector()
       for tick in range(15):
         step(detector, tick, lead=wire_lead(distance=5, speed=0), speed=2.)
@@ -207,9 +207,9 @@ class TestLeadDetector(unittest.TestCase):
       event.modelV2.position.y = [float('nan') if variation == 'bad_y' else 0.] * 33
       model = messaging.log_from_bytes(event.to_bytes()).modelV2
       observation = step(detector, 15, model=model, lead=wire_lead(distance=5, speed=.8),
-                         speed=.5 if variation in ('moving', 'flag_speed') else 0.,
+                         speed={'moving': .5, 'flag_speed': .5, 'quantized': .044}.get(variation, 0.),
                          standstill=variation != 'moving', model_fresh=variation != 'stale')
-      if variation == 'clear':
+      if variation in ('clear', 'quantized'):
         self.assertTrue(observation.tracked)
       else:
         self.assertIsNone(observation.tracked)