Curve: fix live freshness, hold through stale frames, keep NoLead gate closed after lead resets

openpilot/starpilot/curve_speed/host.py

@@ -5,11 +5,14 @@ import math
 import time
 
 from openpilot.cereal.services import SERVICE_LIST
+from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.curve_speed.runtime import Frame, INPUT_MAX_AGE_NS, MODEL_MAX_AGE_NS, Runtime
 from openpilot.starpilot.curve_speed.target import CurveProfile
-from openpilot.starpilot.lead_tracking import LeadDetector
+from openpilot.starpilot.lead_tracking import FILTER_THRESHOLD, FILTER_TIME_S, LeadDetector
 
 CLOCK_PAIR_MAX_SKEW_NS = 5_000_000
+# Steps from a reset before the detector's filter can first report a present lead.
+LEAD_WARMUP_FRAMES = math.ceil(math.log(1 - FILTER_THRESHOLD) / math.log(1 - DT_MDL / (FILTER_TIME_S + DT_MDL)))
 
 
 def clock_pair_ns() -> tuple[int, int] | None:
@@ -67,12 +70,14 @@ class CurveHost:
     self.boot_offset_ns: int | None = None
     self.source_floor_ns = 0
     self.leads = LeadDetector()
+    self.lead_samples = 0
     self.lead_drive_id = 0
     self.previous_long_active = False
     self.previous_active_headway: float | None = None
 
   def _reset_leads(self) -> None:
     self.leads.reset()
+    self.lead_samples = 0
     self.previous_long_active = False
     self.previous_active_headway = None
 
@@ -101,22 +106,22 @@ class CurveHost:
       required = ('modelV2', 'carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState')
       try:
         camera_eof = sm['modelV2'].timestampEof
-        camera_current = (type(camera_eof) is int and 0 < camera_eof <= pair[1] and
-                          pair[1] - camera_eof <= MODEL_MAX_AGE_NS)
+        camera_age = now_ns + offset - camera_eof if type(camera_eof) is int and camera_eof > 0 else -1
+        camera_current = 0 <= camera_age <= MODEL_MAX_AGE_NS
       except (KeyError, AttributeError, TypeError, ValueError, OverflowError):
         camera_current = False
       # A newly published model envelope cannot renew the age of its camera
       # input. EOF uses BOOTTIME; only the bracketed clock pair may compare it.
       # Offline replay intentionally keeps its separate recorded timeline.
-      if (not camera_current or not 0 <= pair[0] - now_ns <= INPUT_MAX_AGE_NS or
-          not all(_fresh(sm, name, pair[0]) for name in required)):
+      # Ages are judged at now_ns: this runs after the MPC solve, which alone
+      # exceeds the 20 ms lifetime of a 100 Hz input.
+      if not camera_current or not 0 <= pair[0] - now_ns <= INPUT_MAX_AGE_NS:
         frame = replace(frame, inputs_fresh=False)
       if rearmed or any(type(sm.logMonoTime.get(name, 0)) is not int or sm.logMonoTime[name] <= self.source_floor_ns
              for name in required):
         frame = replace(frame, inputs_fresh=False)
-    if not frame.inputs_fresh or frame.profile is None:
-      self._reset_leads()
-    else:
+    # A skipped stale frame needs no reset: the detector resets itself on longer gaps.
+    if frame.inputs_fresh and frame.profile is not None:
       try:
         # Tracking precedes headway reset: the first inactive tick retains the prior headway,
         # then uses zero. Active control requires this cycle's MPC headway.
@@ -131,9 +136,16 @@ class CurveHost:
                                    observed_mono_s=frame.model_ns / 1e9, now_mono_s=now_ns / 1e9,
                                    radar_fresh=_fresh(sm, 'radarState', now_ns), model_fresh=_fresh(sm, 'modelV2', now_ns),
                                    car_fresh=_fresh(sm, 'carState', now_ns), headway_fresh=headway_fresh)
+        if self.leads.last_observed_mono_s is None:
+          self.lead_samples = 0
+        elif observed.tracked is not None and not sm['carState'].standstill:
+          self.lead_samples += 1
+        tracked = observed.tracked
         following = (observed.following if frame.long_active else
                      False if observed.tracked is not None else None)
-        frame = replace(frame, tracking_lead=observed.tracked, following_lead=following)
+        if self.lead_samples < LEAD_WARMUP_FRAMES and not tracked and observed.raw_present:
+          tracked = following = None
+        frame = replace(frame, tracking_lead=tracked, following_lead=following)
         self.previous_long_active = frame.long_active if observed.tracked is not None else False
         self.previous_active_headway = (float(detector_headway) if detector_headway is not None and frame.long_active and headway_fresh and
                                         observed.tracked is not None else None)

openpilot/starpilot/curve_speed/runtime.py

@@ -76,6 +76,7 @@ class Runtime:
     self.filter = TargetFilter()
     self.last_ns = 0
     self.last_model_ns = 0
+    self.last_valid_ns = 0
     self.drive_id = 0
     self.was_controlling = False
     self.was_long_active = False
@@ -94,6 +95,7 @@ class Runtime:
     self.pending_ns = 0
     self.pending_target: float | None = None
     self.pending_cap = False
+    self.pending_hold = False
     self.pending_cruise = 0.0
     self.pending_ego = 0.0
     self.glow = False
@@ -110,6 +112,7 @@ class Runtime:
     self.filter.reset()
     self.last_ns = 0
     self.last_model_ns = 0
+    self.last_valid_ns = 0
     self.drive_id = drive_id
     self.was_controlling = False
     self.was_long_active = False
@@ -127,6 +130,7 @@ class Runtime:
     self.pending_ns = 0
     self.pending_target = None
     self.pending_cap = False
+    self.pending_hold = False
     self.pending_cruise = 0.0
     self.pending_ego = 0.0
     self.glow = False
@@ -149,6 +153,8 @@ class Runtime:
       self.glow = False
       return False
     self.confirmed_ns = frame_ns
+    if self.pending_hold:
+      return True
     target = self.pending_target
     selected = applied and self.pending_cap and target is not None
     self.was_controlling = bool(selected and target < self.pending_cruise - 1.0 and
@@ -230,24 +236,32 @@ class Runtime:
     self.pending_ns = now
     self.pending_target = None
     self.pending_cap = False
+    self.pending_hold = False
     valid_profile = (frame.inputs_fresh and frame.profile is not None and
                      type(frame.model_ns) is int and 0 < frame.model_ns <= now and
                      now - frame.model_ns <= MODEL_MAX_AGE_NS and
                      frame.profile.observed_ns == frame.model_ns and
                      frame.model_ns > self.last_model_ns)
     if not valid_profile:
-      self.filter.reset()
       self.watch_until_ns = 0
       self.last_training_ns = 0
-      self.nudged = False
-      self.override = False
-      self.was_controlling = False
-      self.glow = False
-      self.was_long_active = frame.long_active
-      self.was_gas = frame.gas_pressed
-      self.was_brake = frame.brake_pressed
+      # Isolated late 100 Hz inputs occur on device; hold control state without a ceiling.
+      self.pending_hold = (self.filter.target is not None and
+                           now - self.last_valid_ns <= int(MAX_STEP_SECONDS * 1e9))
+      if self.pending_hold:
+        self.pending_target = self.filter.target
+      else:
+        self.filter.reset()
+        self.nudged = False
+        self.override = False
+        self.was_controlling = False
+        self.glow = False
+        self.was_long_active = frame.long_active
+        self.was_gas = frame.gas_pressed
+        self.was_brake = frame.brake_pressed
       return self._result(None, None, 0.0, 'stale_or_invalid_evidence', False, now)
     self.last_model_ns = frame.model_ns
+    self.last_valid_ns = now
     if (not finite_number(frame.cruise_mps) or frame.cruise_mps <= 0 or
         not finite_number(frame.ego_mps) or frame.ego_mps < 0 or
         frame.ego_mps > 100.0 or not finite_number(frame.measured_curvature) or

openpilot/starpilot/curve_speed/status.py

@@ -50,7 +50,8 @@ class StatusPublisher:
       'documentValid': bool(runtime.document_valid),
       'hasCandidate': result.candidate_mps is not None, 'candidateMps': float(result.candidate_mps or 0.0),
       'hasCeiling': result.ceiling_mps is not None, 'ceilingMps': float(result.ceiling_mps or 0.0),
-      'applied': bool(planner.last_curve_ceiling_applied), 'controlling': bool(runtime.was_controlling),
+      'applied': bool(planner.last_curve_ceiling_applied),
+      'controlling': bool(runtime.was_controlling and planner.last_curve_ceiling_applied),
       'training': bool(result.training), 'calibrationProgress': float(result.progress),
       'comfortAccel': float(runtime.curve.average_comfort), 'bindingDistance': float(result.binding_distance_m),
       'reason': result.reason if result.ceiling_mps is not None else result.learning_reason,

openpilot/starpilot/curve_speed/tests/test_host.py

@@ -101,6 +101,50 @@ class TestCurveHost(unittest.TestCase):
     self.assertFalse(seen[-1][1].following)
     self.assertFalse(seen[-1][1].tracked)
 
+  def capture_frames(self, host):
+    frames = []
+    original = host.runtime.step
+    def runtime_capture(frame, **kwargs):
+      frames.append(frame)
+      return original(frame, **kwargs)
+    self.enterContext(patch.object(host.runtime, 'step', side_effect=runtime_capture))
+    return frames
+
+  def present_lead(self, d_rel=30.0):
+    radar = self.sm.messages['radarState'].leadOne
+    radar.present = True
+    radar.dRel = d_rel
+
+  def test_stale_frame_keeps_lead_consensus(self):
+    host = CurveHost(enabled=True, no_lead=True, replay=True)
+    frames = self.capture_frames(host)
+    self.present_lead()
+    for n in range(1, 17):
+      stamp = NOW + n * 50_000_000
+      self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp)
+      host.sample(self.sm, self.cp, stamp, follow_time_s=1.45)
+    self.assertTrue(frames[-1].following_lead)
+    stamp += 45_000_000
+    self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp)
+    self.sm.logMonoTime['carState'] = stamp - 30_000_000
+    host.sample(self.sm, self.cp, stamp, follow_time_s=1.45)
+    stamp += 45_000_000
+    self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp)
+    host.sample(self.sm, self.cp, stamp, follow_time_s=1.45)
+    self.assertTrue(frames[-1].tracking_lead)
+    self.assertTrue(frames[-1].following_lead)
+
+  def test_present_lead_is_unknown_until_detector_warms_up(self):
+    host = CurveHost(enabled=True, no_lead=True, replay=True)
+    frames = self.capture_frames(host)
+    self.present_lead()
+    for n in range(1, 21):
+      stamp = NOW + n * 50_000_000
+      self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp)
+      self.assertIsNone(host.sample(self.sm, self.cp, stamp, follow_time_s=1.45).ceiling_mps)
+    self.assertNotIn(False, [frame.following_lead for frame in frames])
+    self.assertTrue(frames[-1].following_lead)
+
   def test_actual_serialized_schema_projection(self):
     events = {name: messaging.new_message(name) for name in self.sm.messages}
     events['modelV2'].modelV2.orientationRate.z = [0.25] * 33
@@ -178,6 +222,30 @@ class TestCurveHost(unittest.TestCase):
           self.assertFalse(result.training)
           self.assertEqual(result.reason, 'stale_or_invalid_evidence')
 
+  def test_live_planner_latency_does_not_age_out_loop_inputs(self):
+    # Measured on device: inputs ~5 ms old at loop start, Curve clock sampled ~35 ms later.
+    offset = 2_000_000_000
+    for latency, input_age, camera_age, expected in (
+        (35_000_000, 5_000_000, 60_000_000, True),
+        (35_000_000, 21_000_000, 60_000_000, False),
+        (35_000_000, 5_000_000, 150_000_001, False),
+        (150_000_001, 5_000_000, 60_000_000, False),
+    ):
+      with self.subTest(latency=latency, input_age=input_age, camera_age=camera_age):
+        tick = [NOW]
+        host = CurveHost(enabled=True, clock_pair=lambda: (tick[0] + latency, tick[0] + latency + offset))
+        result = None
+        for _ in range(4):
+          tick[0] += 50_000_000
+          self.sm.logMonoTime = dict.fromkeys(self.sm.messages, tick[0] - input_age)
+          self.sm.logMonoTime['modelV2'] = tick[0] - 3_000_000
+          self.sm.logMonoTime['radarState'] = tick[0] - 40_000_000
+          self.sm.messages['modelV2'].timestampEof = tick[0] + offset - camera_age
+          result = host.sample(self.sm, self.cp, tick[0])
+        self.assertEqual(result.ceiling_mps is not None, expected)
+        if not expected:
+          self.assertEqual(result.reason, 'stale_or_invalid_evidence')
+
 
 if __name__ == '__main__':
   unittest.main()

openpilot/starpilot/curve_speed/tests/test_runtime.py

@@ -1,7 +1,9 @@
 import unittest
 from dataclasses import replace
+from types import SimpleNamespace as NS
 
 from openpilot.starpilot.curve_speed.runtime import DriverEvent, Frame, Runtime
+from openpilot.starpilot.curve_speed.status import StatusPublisher, observation
 from openpilot.starpilot.curve_speed.target import CurveProfile
 
 
@@ -84,6 +86,58 @@ class TestCurveRuntime(unittest.TestCase):
     self.assertIsNotNone(rt.step(frame(4)).ceiling_mps)
     self.assertIsNotNone(rt.step(frame(20)).ceiling_mps)  # discontinuity resets filter, then fresh frame can rearm
 
+  def test_isolated_stale_frame_holds_target_without_ceiling(self):
+    rt = Runtime(enabled=True)
+    arm_selected(rt)
+    held = rt.filter.target
+    stale = frame(27, inputs_fresh=False)
+    self.assertIsNone(rt.step(stale).ceiling_mps)
+    rt.confirm_applied(stale.now_ns, applied=False)
+    self.assertTrue(rt.was_controlling)
+    self.assertTrue(rt.glow)
+    self.assertEqual(rt.filter.target, held)
+    resumed = rt.step(frame(28)).ceiling_mps
+    assert resumed is not None
+    self.assertLessEqual(resumed, held)
+
+  def test_stale_hold_publishes_valid_glowing_status(self):
+    rt = Runtime(enabled=True)
+    arm_selected(rt)
+    stale = frame(27, inputs_fresh=False)
+    result = rt.step(stale)
+    rt.confirm_applied(stale.now_ns, applied=False)
+    planner = NS(last_curve_ceiling_applied=False, last_curve_ceiling_status='absent')
+    event = StatusPublisher().attach(None, NS(runtime=rt), result, planner, now_ns=stale.now_ns,
+                                     model_ns=frame(26).now_ns, persistence_status='idle')
+    status = observation(event.slcState, stale.now_ns)
+    assert status is not None
+    self.assertTrue(status.glow)
+    self.assertFalse(status.controlling)
+
+  def test_stale_hold_expires_after_max_step(self):
+    rt = Runtime(enabled=True)
+    arm_selected(rt)
+    for n in range(27, 31):
+      item = frame(n, inputs_fresh=False)
+      rt.step(item)
+      rt.confirm_applied(item.now_ns, applied=False)
+    self.assertIsNotNone(rt.filter.target)
+    item = frame(31, inputs_fresh=False)
+    rt.step(item)
+    rt.confirm_applied(item.now_ns, applied=False)
+    self.assertIsNone(rt.filter.target)
+    self.assertFalse(rt.glow)
+    self.assertFalse(rt.was_controlling)
+
+  def test_gas_override_during_stale_hold_still_latches(self):
+    rt = Runtime(enabled=True)
+    arm_selected(rt)
+    stale = frame(27, inputs_fresh=False, gas_pressed=True)
+    rt.step(stale)
+    rt.confirm_applied(stale.now_ns, applied=False)
+    self.assertIsNone(rt.step(frame(28, gas_pressed=True)).ceiling_mps)
+    self.assertTrue(rt.override)
+
   def test_invalid_document_never_learns_or_persists(self):
     rt = Runtime({'version': 1, 'buckets': {'nan': {'average': 2.0, 'count': 1}}}, enabled=True)
     self.assertFalse(rt.document_valid)