diff --git a/openpilot/starpilot/flm/steerlearnerd.py b/openpilot/starpilot/flm/steerlearnerd.py index f9ad4a16cb..e8f4844264 100644 --- a/openpilot/starpilot/flm/steerlearnerd.py +++ b/openpilot/starpilot/flm/steerlearnerd.py @@ -544,12 +544,17 @@ def run( def current_tracking_offer(): return live_offer - def renew_tracking(): + def renew_tracking(*, refresh=False): nonlocal live_offer, next_tracking_status_ns, tracking_needs_native if live_offer is None or live_actor is None or live_history.offer is not None: tracking_needs_native = False return False - valid, stamp = source_current() + if refresh: + source_delivery.refresh(sm) + if (not sm.updated['deviceMotion'] and not tracking_needs_native and + not getattr(collector, '_original_source_pose_queue', ())): + return False + valid, stamp = source_current(read=refresh) if not valid or collector is None: tracking_needs_native = False return False @@ -753,9 +758,9 @@ def run( application = read_restore_application(event.starpilotLateralState, now_ns=received_now, offer=live_history.offer) if application is not None and current_native_health(sm, CP, evidence, received_now): live_history.acknowledge(live_actor, application, now_ns=received_now) - if (live_actor is not None and offered is not None and admitted and - (tracking_needs_native or _pending_pose_bracket_ready(collector, prepared, clock()))): - renew_tracking() + if live_actor is not None and offered is not None and admitted: + # Motion may arrive while the original FIFO is still draining. + renew_tracking(refresh=True) def output(event, current, stamp): valid_source, received_now = source_current() diff --git a/openpilot/starpilot/flm/tests/test_pending_motion_publication.py b/openpilot/starpilot/flm/tests/test_pending_motion_publication.py index 4f242a860d..ef00b00d9c 100644 --- a/openpilot/starpilot/flm/tests/test_pending_motion_publication.py +++ b/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']) +@pytest.mark.parametrize('missing', ['state_bracket', 'native_pair', 'delivery_in_batch']) @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,14 +43,17 @@ def test_actual_callbacks_publish_new_witness_before_remaining_fifo_work(monkeyp class SM: updated = {'deviceMotion': False} - logMonoTime = {'deviceMotion': BASE} + logMonoTime = {'deviceMotion': BASE-50_000_000 if missing == 'delivery_in_batch' else BASE} def update(self, timeout=None): if timeout == 0: - self.updated['deviceMotion'] = False + self.updated['deviceMotion'] = missing == 'delivery_in_batch' and now[0] >= BASE+30_000_000 + if self.updated['deviceMotion']: + self.logMonoTime['deviceMotion'] = BASE else: now[0] += 25_000_000 self.updated['deviceMotion'] = True - self.logMonoTime['deviceMotion'] = BASE if retained_header else now[0] + self.logMonoTime['deviceMotion'] = (BASE-50_000_000 if missing == 'delivery_in_batch' else + BASE if retained_header else now[0]) def __getitem__(self, key): assert key == 'pandaStates' return [] @@ -105,11 +108,13 @@ 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): - return missing == 'state_bracket' or now[0] <= BASE+10_000_000 or control.control_ns >= BASE+25_000_000 + 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])) def early(*args, **kwargs): + if missing == 'delivery_in_batch' and args[0].logMonoTime['deviceMotion'] < BASE: + return None if not collector.right_available: collector._original_source_pose_queue = (S(mono_ns=BASE, source_upper_ns=BASE),) return None diff --git a/prebuilt.json b/prebuilt.json index 0ad4d20f36..8cb94e3045 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "692a6d59cbffa4c190442b73db9aa66aa7bda18fca2bc591dbd676cf91aa0160", "schema_version": 1} +{"protected_inventory_sha256": "2a8b56bd85134982fdb6508d288beef4709cb4288c2bdc1e67220bd49f96f924", "schema_version": 1}