CEM standstill, road data, and flicker left

openpilot/starpilot/conditional_mode/projection.py

@@ -35,6 +35,8 @@ V_CRUISE_UNSET_KPH = 255.0
 FROZEN_CURVE_ACCEL_MPS2 = 1.3
 STANDSTILL_SPEED_NOISE_MPS = 0.01
 STANDSTILL_MODEL_PREFIX_NOISE_M = 0.01
+# Recorded standstill paths dip up to 0.46 m below their running max and 0.15 m below zero.
+STANDSTILL_PATH_JITTER_M = 0.5
 FROZEN_RAW_STOP_DISTANCE_M = 5.0 * ModelConstants.T_IDXS[-1]
 
 
@@ -217,6 +219,26 @@ def _horizon(model) -> float | None:
     return None
 
 
+def _standstill_path_extent(model) -> float | None:
+  """Uncommitted standstill hold only; too lenient for stop acquisition or release."""
+  try:
+    distances = normalized_origin(tuple(model.position.x))
+    if len(distances) != ModelConstants.IDX_N:
+      return None
+    parsed = tuple(_number(value, low=-STANDSTILL_PATH_JITTER_M, high=500.0) for value in distances)
+    if any(value is None for value in parsed):
+      return None
+    finite = tuple(value for value in parsed if value is not None)
+    peak = 0.0
+    for value in finite:
+      peak = max(peak, value)
+      if peak - value > STANDSTILL_PATH_JITTER_M:
+        return None
+    return peak
+  except (AttributeError, TypeError, ValueError, OverflowError):
+    return None
+
+
 def _standstill_clear_horizon(model) -> float | None:
   """Stop-release endpoint only; this never qualifies full spatial geometry."""
   try:
@@ -625,6 +647,8 @@ class SceneProjector:
       model_tick_mono_s=model_stamp / 1e9 if model_stamp is not None else None,
       model_should_stop=current_stop,
       stationary_launch_clear=stationary_launch_clear,
+      standstill_path_extent_m=(_standstill_path_extent(model) if model_transport_current and car_valid and
+                                _field(car, 'standstill') is True else None),
     )
     if repeated_model:
       self.stop_detector.commit_clear_since_s = None

openpilot/starpilot/conditional_mode/status.py

@@ -15,7 +15,8 @@ from openpilot.starpilot.conditional_mode.runtime_settings import (
 )
 
 
-LIFETIME_NS = 100_000_000
+# Proposals reach selfdrived 30-70 ms after observation; 100 ms expired most before their successor arrived.
+LIFETIME_NS = 150_000_000
 MODEL_MAX_AGE_NS = 150_000_000
 CHOICES = {ModeChoice.STOCK: 'stock', ModeChoice.CEM: 'conditionalExperimental', ModeChoice.CCM: 'conditionalChill'}
 _HEX = re.compile(r'[0-9a-f]{64}\Z')

openpilot/starpilot/conditional_mode/stop.py

@@ -57,6 +57,7 @@ class StopFrame:
   model_tick_mono_s: float | None = None
   model_should_stop: bool | None = None
   stationary_launch_clear: bool = False
+  standstill_path_extent_m: float | None = None
 
 
 @dataclass(frozen=True)
@@ -241,6 +242,12 @@ class StopLightDetector:
         self.light_detected = True
         hold, reason = self._standstill(frame, True)
         return StopObservation(True, hold, reason, None)
+      extent = _finite(frame.standstill_path_extent_m, 0.0, 500.0)
+      if horizon is None and frame.standstill is True and frame.pedal_override is False and extent is not None:
+        release_distance = FROZEN_RAW_STOP_DISTANCE_M + (STOP_MODEL_RELEASE_MARGIN_M if self.standstill_model_stopped else 0.0)
+        self.standstill_model_stopped = extent < release_distance
+        hold, reason = self._standstill(frame, self.standstill_model_stopped or forcing_stop)
+        return StopObservation(None, hold, reason, None)
       self.invalidate()
       return unknown
     # At standstill the speed-scaled detector has a zero threshold. Its raw

openpilot/starpilot/conditional_mode/tests/test_standstill_hold_and_clock.py

@@ -0,0 +1,89 @@
+import unittest
+
+from openpilot.cereal import messaging
+from openpilot.starpilot.conditional_mode.consumer import ModeConsumer
+from openpilot.starpilot.conditional_mode.policy import ModeChoice
+from openpilot.starpilot.conditional_mode.projection import STANDSTILL_PATH_JITTER_M, _horizon, _standstill_path_extent
+from openpilot.starpilot.conditional_mode.stop import StopFrame, StopLead, StopLightDetector
+from openpilot.starpilot.conditional_mode.tests.test_stop_commit_transport import CLEAR_97_X
+from openpilot.starpilot.conditional_mode.tests.test_waiting_barrier import DRIVE, FINGERPRINT, MS, T, proposal
+
+
+def sample(consumer, event, now, boot, skew):
+  value = event.slcState.conditionalMode
+  return consumer.sample(event.slcState, now_ns=now, now_boot_ns=boot, sample_skew_ns=skew,
+    message_ns=int(event.logMonoTime), receipt_ns=int(event.logMonoTime), drive_id=DRIVE,
+    model_ns=int(value.modelMonoTime), car_state_ns=int(value.carStateMonoTime),
+    authority=True, stock_experimental=False, choice=ModeChoice.CEM, settings_fingerprint=FINGERPRINT)
+
+
+def stop_frame(t, *, horizon=None, extent=None, standstill=True, gas=False):
+  return StopFrame(observed_mono_s=t, now_mono_s=t, speed_mps=0.0, model_horizon_m=horizon, model_stop_time_s=8.0,
+    traffic_mode=False, stop_sign_confirmed=False, forcing_stop=False, lead=StopLead(False), standstill=standstill,
+    left_blinker=False, right_blinker=False, steering_angle_deg=0.0, driving_in_curve=False,
+    car_fingerprint='HYUNDAI_IONIQ_6', dashboard_stop_sign=False, pedal_override=gas, model_tick_mono_s=t,
+    model_should_stop=False, standstill_path_extent_m=extent)
+
+
+class TestPairedClockMidpoint(unittest.TestCase):
+  def warm(self, boot_offset):
+    consumer = ModeConsumer()
+    result = None
+    for index in range(3):
+      observed = T + index * 50 * MS
+      now = observed + 30 * MS
+      result = sample(consumer, proposal(observed - 2 * MS, observed - MS, observed, index + 1), now, now + boot_offset, 4_000)
+    return consumer, result
+
+  def test_boottime_trailing_midpoint_within_skew_is_accepted(self):
+    consumer, result = self.warm(-2_000)
+    self.assertTrue(result.accepted and result.experimental)
+    self.assertIsNone(consumer.rejection)
+
+  def test_boottime_trailing_midpoint_beyond_skew_is_rejected(self):
+    consumer, _ = self.warm(-2_000)
+    observed = T + 150 * MS
+    now = observed + 30 * MS
+    result = sample(consumer, proposal(observed - 2 * MS, observed - MS, observed, 4), now, now - 4_001, 4_000)
+    self.assertFalse(result.accepted)
+    self.assertEqual(consumer.rejection, 'clock_inputs')
+
+
+class TestUncommittedStandstillHold(unittest.TestCase):
+  def run_frames(self, detector, extents, start=100.0):
+    return [detector.step(stop_frame(start + index * 0.05, extent=extent)) for index, extent in enumerate(extents)]
+
+  def test_short_jittery_path_holds_and_extending_path_releases_with_hysteresis(self):
+    detector = StopLightDetector()
+    observations = self.run_frames(detector, [3.0] * 5 + [52.0] * 5 + [60.0] * 5)
+    self.assertTrue(all(o.standstill_hold is True for o in observations[:10]))
+    self.assertEqual(observations[0].standstill_reason, 'light')
+    self.assertTrue(all(o.standstill_hold is False for o in observations[10:]))
+    self.assertFalse(detector.committed)
+
+  def test_missing_extent_stays_unknown(self):
+    observations = self.run_frames(StopLightDetector(), [None] * 5)
+    self.assertTrue(all(o.standstill_hold is None for o in observations))
+
+  def test_tolerant_extent_cannot_release_a_committed_stop(self):
+    detector = StopLightDetector()
+    detector.committed = True
+    detector.commit_distance_m = 20.0
+    observations = self.run_frames(detector, [80.0] * 20)
+    self.assertTrue(all(o.standstill_hold is True and o.light_detected is True for o in observations))
+    self.assertTrue(detector.committed)
+
+
+class TestStandstillPathExtent(unittest.TestCase):
+  def test_recorded_stationary_jitter_has_extent_but_no_spatial_horizon(self):
+    model = messaging.new_message('modelV2').modelV2
+    model.position.x = CLEAR_97_X
+    self.assertIsNone(_horizon(model))
+    self.assertEqual(_standstill_path_extent(model), max(CLEAR_97_X))
+
+  def test_dip_beyond_jitter_is_rejected(self):
+    model = messaging.new_message('modelV2').modelV2
+    values = list(CLEAR_97_X)
+    values[20] = values[19] - STANDSTILL_PATH_JITTER_M - 0.01
+    model.position.x = values
+    self.assertIsNone(_standstill_path_extent(model))

openpilot/starpilot/conditional_mode/tests/test_waiting_barrier.py

@@ -129,8 +129,9 @@ class TestWaitingBarrier(unittest.TestCase):
 
   def test_original_proposal_expiry_cannot_be_renewed(self):
     event = proposal(T + 50 * MS, T + 49 * MS, T + 55 * MS, 4)
-    now = T + 155 * MS + 1
-    result = self.sample(event, now, model=T + 150 * MS, car=now - MS, receipt=T + 100 * MS)
+    now = T + 55 * MS + LIFETIME_NS + 1
+    result = self.sample(event, now, model=now - 5 * MS, car=now - MS, receipt=T + 100 * MS)
     self.assertFalse(result.accepted)
+    self.assertEqual(self.consumer.rejection, 'proposal_invalid')
     self.assertIsNone(self.consumer.last)
     self.assertEqual(self.consumer.barrier_ns, T)