diff --git a/openpilot/starpilot/flm/fullrate_collector.py b/openpilot/starpilot/flm/fullrate_collector.py index 6a52260ed6..826a90dd60 100644 --- a/openpilot/starpilot/flm/fullrate_collector.py +++ b/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]) diff --git a/openpilot/starpilot/flm/steerlearnerd.py b/openpilot/starpilot/flm/steerlearnerd.py index 3d97d9657c..98a13f9e86 100644 --- a/openpilot/starpilot/flm/steerlearnerd.py +++ b/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 diff --git a/openpilot/starpilot/flm/tests/_drain_fixture.py b/openpilot/starpilot/flm/tests/_drain_fixture.py index e2ca91d024..765b8eebb0 100644 --- a/openpilot/starpilot/flm/tests/_drain_fixture.py +++ b/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 diff --git a/openpilot/starpilot/flm/tests/test_paired_drain_time_slices.py b/openpilot/starpilot/flm/tests/test_paired_drain_time_slices.py index 301f7c7ece..c099f42003 100644 --- a/openpilot/starpilot/flm/tests/test_paired_drain_time_slices.py +++ b/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 diff --git a/prebuilt.json b/prebuilt.json index fbd91afd1d..c0fd1dc77c 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "2cf347236d514931aed950b4cb4821430b89d8fcda7e381449f69bbc604eff11", "schema_version": 1} +{"protected_inventory_sha256": "74af05e5b83efaa60f7554da44b73875bc75bc155ef6bc4509ea796f50625738", "schema_version": 1}