Yield FLM queues only when actual motion needs service

openpilot/starpilot/flm/fullrate_collector.py

@@ -928,7 +928,8 @@ class PairedDrainState:
     self.exhausted = (False, False)
 
 
-def drain_pair(control_socket, control, applied_socket, applied, receive, collector, now_ns, state=None, *, budget_ns=None, clock=time.monotonic_ns):
+def drain_pair(control_socket, control, applied_socket, applied, receive, collector, now_ns, state=None, *,
+               budget_ns=None, yield_requested=None, clock=time.monotonic_ns):
   """Merge two bounded FIFO tapes by their original publisher headers.
 
   Only the two queue heads compete; never sort a producer's own tape. A held
@@ -940,7 +941,9 @@ def drain_pair(control_socket, control, applied_socket, applied, receive, collec
   A persistent state retains only the two already-read heads at an overrun.
   Their original wire and clocks are checked again on the next bounded drain.
   """
-  if budget_ns is not None and (state is None or type(budget_ns) is not int or budget_ns <= 0):
+  if (budget_ns is not None or yield_requested is not None) and state is None:
+    raise ValueError('yielding drain requires persistent FIFO state')
+  if budget_ns is not None and (type(budget_ns) is not int or budget_ns <= 0):
     raise ValueError('time-sliced drain requires persistent state and a positive budget')
   deadline = None if budget_ns is None else clock() + budget_ns
   sockets, callbacks = (control_socket, applied_socket), (control, applied)
@@ -978,7 +981,8 @@ def drain_pair(control_socket, control, applied_socket, applied, receive, collec
     head(0)
     head(1)
     while pending[0] is not None or pending[1] is not None:
-      if processed and deadline is not None and clock() >= deadline:
+      if processed and ((deadline is not None and clock() >= deadline) or
+                        (yield_requested is not None and yield_requested())):
         # Yield scheduling only. Preserve original FIFO heads and the SAME
         # per-stream overrun count; a slice cannot renew evidence or a batch.
         state.heads, state.counts = tuple(pending), (counts[0], counts[1])

openpilot/starpilot/flm/steerlearnerd.py

@@ -33,6 +33,7 @@ class _SourceDelivery:
     self.pose_header = 0
     self.evidence_poller = None
     self.evidence_sockets = ()
+    self.motion_poller = self.motion_socket = None
 
   def refresh(self, sm):
     sm.update(0)
@@ -40,6 +41,20 @@ class _SourceDelivery:
     if sm.updated['deviceMotion']:
       self.pose_header = int(sm.logMonoTime['deviceMotion'])
 
+  def motion_pending(self, sm):
+    # Scheduling hint only: poll readiness without consuming or inventing a pose.
+    if self.pose_header:
+      return True
+    try:
+      socket = sm.sock['deviceMotion']
+      if self.motion_poller is None or self.motion_socket is not socket:
+        self.motion_poller = messaging.Poller()
+        self.motion_poller.registerSocket(socket)
+        self.motion_socket = socket
+      return bool(self.motion_poller.poll(0))
+    except (AttributeError, KeyError, OSError, RuntimeError, TypeError, ValueError):
+      return False  # Ordinary bounded draining and live admission still run.
+
   def next(self, sm, *, pending_evidence=(), retained_evidence=False):
     if not self.pending:
       if retained_evidence:
@@ -701,7 +716,8 @@ def run(
           _collect_applied_output(collector, applied, sm, CP, received_now)
 
       drain_pair(control_socket, control, applied_socket, output, messaging.recv_one_or_none, sink, now, drain_state,
-        budget_ns=5_000_000 if live_actor is not None else None)
+        budget_ns=5_000_000 if live_actor is not None and (tracking_pose_header or pending_fresh) else None,
+        yield_requested=(lambda: source_delivery.motion_pending(sm)) if live_actor is not None else None)
       if collector is not None:
         # Decoding queued frames may outlast the sampled shared inputs. Consume
         # real pending deliveries before pose/native checks; keep their original

openpilot/starpilot/flm/tests/_drain_fixture.py

@@ -6,7 +6,7 @@ The actual FIFO merge and bounded wire receiver have their own transport tests.
 
 
 def separate_drain_fixture(single):
-  def paired(control_socket, control, applied_socket, applied, receive, collector, now_ns, _state=None, *, budget_ns=None):
+  def paired(control_socket, control, applied_socket, applied, receive, collector, now_ns, _state=None, *, budget_ns=None, yield_requested=None):
     single(control_socket, control, receive, collector, now_ns)
     single(applied_socket, applied, receive, collector, now_ns)
   return paired

openpilot/starpilot/flm/tests/test_paired_drain_time_slices.py

@@ -137,3 +137,47 @@ def test_retained_head_wakes_source_receiver_without_waiting_for_another_pose():
   receiver = _SourceDelivery()
   assert receiver.next(sm,retained_evidence=True) == BASE
   assert calls == [0] and not receiver.pending and receiver.pose_header == 0
+
+
+def test_new_pose_interrupts_busy_drain_and_absent_pose_does_not_fragment_it():
+  rows = [[small_event(BASE+2*i+side)for i in range(6)]for side in(0,1)]
+  sockets,receive,_ = transport(*rows)
+  state = PairedDrainState(*sockets)
+  collector = FullrateCollector(SyntheticAdapter())
+  seen = []
+  pose_waiting = [False]
+
+  def callback(event,_current,_stamp):
+    seen.append(event)
+    if len(seen)==2:
+      pose_waiting[0]=True
+
+  def consume():
+    return module.drain_pair(sockets[0],callback,sockets[1],callback,receive,collector,BASE,state,
+      yield_requested=lambda:pose_waiting[0])
+
+  assert not consume() and len(seen)==2 and collector.dropped==0
+  pose_waiting[0]=False
+  assert consume() and len(seen)==12 and collector.dropped==0
+  assert [event.logMonoTime for event in seen]==list(range(BASE,BASE+12))
+
+
+def test_motion_readiness_hint_never_consumes_or_modifies_source_state():
+  from unittest.mock import Mock,patch
+  from openpilot.starpilot.flm.steerlearnerd import _SourceDelivery
+  from openpilot.starpilot.flm import steerlearnerd
+  socket=object()
+  sm=SimpleNamespace(sock={'deviceMotion':socket},updated={'deviceMotion':False},
+    logMonoTime={'deviceMotion':BASE},freq_ok={'deviceMotion':False})
+  receiver=_SourceDelivery()
+  poller=Mock()
+  poller.poll.side_effect=[[],[socket]]
+  with patch.object(steerlearnerd.messaging,'Poller',return_value=poller):
+    assert not receiver.motion_pending(sm)
+    assert receiver.motion_pending(sm)
+  poller.registerSocket.assert_called_once_with(socket)
+  assert [call.args for call in poller.poll.call_args_list]==[(0,),(0,)]
+  assert sm.updated=={'deviceMotion':False} and sm.logMonoTime=={'deviceMotion':BASE}
+  assert sm.freq_ok=={'deviceMotion':False}
+  receiver.pose_header=BASE
+  assert receiver.motion_pending(sm) and poller.poll.call_count==2

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "2cf347236d514931aed950b4cb4821430b89d8fcda7e381449f69bbc604eff11", "schema_version": 1}
+{"protected_inventory_sha256": "74af05e5b83efaa60f7554da44b73875bc75bc155ef6bc4509ea796f50625738", "schema_version": 1}