Renew FLM from already received motion during FIFO ingest

openpilot/starpilot/flm/steerlearnerd.py

@@ -450,6 +450,7 @@ def run(
   retained_event = pending_event = None
   live_offer = None
   previous_tracking_pose_header = 0
+  last_tracking_observation_header = 0
   next_tracking_status_ns = 0
   study_health_control = None
   saved_live_targets = set()
@@ -548,13 +549,14 @@ def run(
     return live_offer
 
   def renew_tracking(*, refresh=False, incoming=None):
-    nonlocal live_offer, next_tracking_status_ns, tracking_needs_native
+    nonlocal live_offer, next_tracking_status_ns, tracking_needs_native, last_tracking_observation_header
     if live_offer is None or live_actor is None or live_history.offer is not None:
       tracking_needs_native = False
       return False
     if refresh:
       source_delivery.refresh(sm)
-      if (not sm.updated['deviceMotion'] and not tracking_needs_native and
+      if (not sm.updated['deviceMotion'] and int(sm.logMonoTime['deviceMotion']) <= last_tracking_observation_header and
+          not tracking_needs_native and
           not _pending_pose_bracket_ready(collector, incoming, clock())):
         return False
     valid, stamp = source_current(read=refresh)
@@ -566,6 +568,9 @@ def run(
     if tracking_needs_native:
       return False
     early = _early_tracking_offer(sm, CP, collector, live_actor, source, now_ns=stamp, clock=clock)
+    # A subsequent refresh can clear updated after receiving real new motion.
+    # Retain only its consumed header; all pose/native/lease checks still run.
+    last_tracking_observation_header = max(last_tracking_observation_header, int(sm.logMonoTime['deviceMotion']))
     if early is None:
       return False
     live_offer = early

openpilot/starpilot/flm/tests/test_pending_motion_publication.py

@@ -27,7 +27,7 @@ def test_absent_pending_pose_does_not_start_extra_observation():
   assert not steerlearnerd._pending_pose_bracket_ready(S(), S(state_ns=BASE), BASE)
 
 
-@pytest.mark.parametrize('missing', ['state_bracket', 'native_pair', 'delivery_in_batch'])
+@pytest.mark.parametrize('missing', ['state_bracket', 'native_pair', 'delivery_in_batch', 'delivery_received_by_native_refresh'])
 @pytest.mark.parametrize('retained_header', [False, True])
 def test_actual_callbacks_publish_new_witness_before_remaining_fifo_work(monkeypatch, missing, retained_header):
   now = [BASE-25_000_000]
@@ -43,16 +43,19 @@ def test_actual_callbacks_publish_new_witness_before_remaining_fifo_work(monkeyp
 
   class SM:
     updated = {'deviceMotion': False}
-    logMonoTime = {'deviceMotion': BASE-50_000_000 if missing == 'delivery_in_batch' else BASE}
+    logMonoTime = {'deviceMotion': BASE-50_000_000 if missing in ('delivery_in_batch', 'delivery_received_by_native_refresh') else BASE}
+    delivered_once = False
     def update(self, timeout=None):
       if timeout == 0:
-        self.updated['deviceMotion'] = missing == 'delivery_in_batch' and now[0] >= BASE+30_000_000
+        self.updated['deviceMotion'] = (now[0] >= BASE+30_000_000 and
+          (missing == 'delivery_in_batch' or missing == 'delivery_received_by_native_refresh' and not self.delivered_once))
         if self.updated['deviceMotion']:
           self.logMonoTime['deviceMotion'] = BASE
+          self.delivered_once = True
       else:
         now[0] += 25_000_000
         self.updated['deviceMotion'] = True
-        self.logMonoTime['deviceMotion'] = (BASE-50_000_000 if missing == 'delivery_in_batch' else
+        self.logMonoTime['deviceMotion'] = (BASE-50_000_000 if missing in ('delivery_in_batch', 'delivery_received_by_native_refresh') else
           BASE if retained_header else now[0])
     def __getitem__(self, key):
       assert key == 'pandaStates'
@@ -109,13 +112,15 @@ def test_actual_callbacks_publish_new_witness_before_remaining_fifo_work(monkeyp
   monkeypatch.setattr('openpilot.starpilot.flm.source_plant_runtime.start_original_capture', lambda *args:True)
   monkeypatch.setattr('openpilot.starpilot.flm.source_plant_runtime.observe', lambda *args:None)
   def native(sm, cp, control, stamp):
+    if missing == 'delivery_received_by_native_refresh':
+      return stamp < BASE+30_000_000 or sm.logMonoTime['deviceMotion'] >= BASE
     return missing != 'native_pair' or now[0] <= BASE+10_000_000 or control.control_ns >= BASE+25_000_000
   monkeypatch.setattr('openpilot.starpilot.flm.native_health.current_native_health', native)
   monkeypatch.setattr(steerlearnerd, 'read_control', lambda *args:evidence())
   monkeypatch.setattr(steerlearnerd, 'read_applied', lambda *args:S(control_ns=now[0], apply_ns=now[0]))
   premature = []
   def early(*args, **kwargs):
-    if missing == 'delivery_in_batch' and args[0].logMonoTime['deviceMotion'] < BASE:
+    if missing in ('delivery_in_batch', 'delivery_received_by_native_refresh') and args[0].logMonoTime['deviceMotion'] < BASE:
       return None
     if not collector.right_available:
       if collector._original_source_pose_queue:

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "df20c562268a9a77c5f28cebfefaf91594010416c0a50ef59bdcf8f3bc759bb5", "schema_version": 1}
+{"protected_inventory_sha256": "f0549dd2e30990df108d65ab4321b4671fcf80b9b1564d47ed03b62892cb810b", "schema_version": 1}