Match newer Dashboard packets to avoid stale SET

openpilot/starpilot/speed_limits/physical_actions.py

@@ -30,13 +30,17 @@ def adoption_speed(state):
 
 
 def source_matches(state, observation, producer_session: str, now_ns: int) -> bool:
+  observed_ns = int(state.sourceObservedMonoTime)
+  expiry_ns = int(state.sourceValidUntilMonoTime)
+  # Card can have a newer packet for the same episode before the planner publishes again.
   return (str(state.source) == 'dashboard' and str(state.observationKind) == 'valid' and
           str(state.sourceProducerSessionId) == producer_session and producer_session != '' and
           getattr(observation.status, 'value', observation.status) == 'valid' and int(state.sourceEpisode) > 0 and
           int(state.sourceEpisode) == int(observation.episode) and
-          int(state.sourceObservedMonoTime) == int(observation.observed_ns) and
-          int(state.sourceValidUntilMonoTime) == int(observation.valid_until_ns) and
-          0 < int(observation.observed_ns) <= now_ns <= int(observation.valid_until_ns) and
+          0 < observed_ns <= state.frameMonoTime and observed_ns <= observation.observed_ns <= now_ns and
+          observed_ns < expiry_ns and observation.observed_ns < observation.valid_until_ns and
+          now_ns <= min(expiry_ns, observation.valid_until_ns) and
+          (observed_ns != observation.observed_ns or expiry_ns == observation.valid_until_ns) and
           finite_speed(state.speedLimit) and finite_speed(observation.speed_mps) and
           abs(float(state.speedLimit) - float(observation.speed_mps)) <= 0.02)
 
@@ -85,7 +89,7 @@ def engagement_state(sm, observation, producer_session: str, now_ns: int, *, aft
       str(state.source) == 'online' or not state.hasPresentationAdjustedLimit):
     return None
   if str(state.source) == 'dashboard' and (
-      observation is None or observation.observed_ns <= after_ns or not source_matches(state, observation, producer_session, now_ns)):
+      observation is None or int(state.sourceObservedMonoTime) <= after_ns or not source_matches(state, observation, producer_session, now_ns)):
     return None
   if str(state.source) == 'vision' and not (
       state.sourceProducerSessionId and state.sourceEpisode > 0 and

openpilot/starpilot/speed_limits/tests/test_engagement.py

@@ -8,7 +8,7 @@ import math
 
 import pytest
 
-from opendbc.car.dashboard_speed_limit import Status
+from opendbc.car.dashboard_speed_limit import Status, Tracker
 from opendbc.car.structs import car
 from openpilot.cereal import messaging
 from openpilot.common.constants import CV
@@ -123,6 +123,34 @@ def test_match_on_engage_uses_saved_history_as_a_fallback(resume, live):
   assert bool(instance.slc_receipts) == (live and not resume)
 
 
+def test_new_dashboard_packet_in_same_episode_matches_set_over_saved_history():
+  runtime, sm, cp = drive(limit=45., offset=0.)
+  previous = AcceptedLimit(Candidate('dashboard', ObservationIdentity(IdentityKind.PRODUCER_EPISODE, value='old-road'), 30 * CV.MPH_TO_MS),
+                           'previous-drive', 0)
+  runtime = Runtime(runtime.settings, previous_limit=previous)
+  state = runtime.step(sm, cp, now_ns=START).message.slcState
+  sm.data['slcState'] = state
+  assert state.hasPresentationAdjustedLimit and state.acceptedMonoTime == 0
+  assert state.acceptedAdjustedLimit == pytest.approx(30 * CV.MPH_TO_MS)
+
+  tracker = Tracker()
+  tracker.update(state.sourceObservedMonoTime, Status.VALID, 45 * CV.MPH_TO_MS, valid_until_ns=state.sourceValidUntilMonoTime)
+  newer = tracker.update(START + 5_000_000, Status.VALID, 45 * CV.MPH_TO_MS, valid_until_ns=START + 300_000_000)
+  assert newer.episode == state.sourceEpisode and newer.observed_ns > state.sourceObservedMonoTime
+  instance = owner(sm, cp, resume=False)
+  instance.CI.CS.dashboard_limit.observation = newer
+
+  engage(instance)
+
+  assert instance.v_cruise_helper.v_cruise_kph == cruise.slc_target_kph(45 * CV.MPH_TO_MS, False)
+  event = receipt(instance)
+  assert event.presentation_id == state.presentationId
+  evidence = sm['slcDashboardObservation']
+  evidence.observedMonoTime, evidence.validUntilMonoTime = newer.observed_ns, newer.valid_until_ns
+  adopted = runtime.step(sm, cp, now_ns=event.observed_ns, cruise_event=event).message.slcState
+  assert adopted.actionStatus == 'adopt' and adopted.acceptedSpeedLimit == pytest.approx(45 * CV.MPH_TO_MS)
+
+
 @pytest.mark.parametrize('confirmation', [False, True])
 @pytest.mark.parametrize('pcm', [False, True])
 @pytest.mark.parametrize('resume,previous', [(False, cruise.V_CRUISE_UNSET), (True, 30.), (None, 105.)],

openpilot/starpilot/speed_limits/tests/test_physical_actions.py

@@ -3,6 +3,7 @@
 import math
 
 import pytest
+from opendbc.car.dashboard_speed_limit import Observation, Status
 
 from openpilot.cereal import messaging
 from openpilot.selfdrive.car.cruise import SlcPendingConfirmation
@@ -113,3 +114,33 @@ def test_engagement_rejects_unqualified_current_target(sm, field, value):
 def test_engagement_accepts_float32_cruise_boundaries(sm, target):
   sm['slcState'].hasPresentationAdjustedLimit, sm['slcState'].presentationAdjustedLimit = True, target
   assert physical_actions.engagement_state(sm, None, 'card', NOW) is not None
+
+
+@pytest.mark.parametrize('state_changes, local_changes, after_ns, matches', [
+  pytest.param({}, {}, 0, True, id='newer-same-episode'),
+  pytest.param({}, {'observed_ns': NOW - 100_000_000, 'valid_until_ns': NOW + 50_000_000}, 0, True, id='same-packet'),
+  pytest.param({}, {'observed_ns': NOW - 100_000_000}, 0, False, id='inconsistent-same-packet'),
+  pytest.param({'sourceValidUntilMonoTime': NOW}, {}, 0, True, id='at-planner-expiry'),
+  pytest.param({}, {'valid_until_ns': NOW}, 0, True, id='at-local-expiry'),
+  pytest.param({'sourceValidUntilMonoTime': NOW - 1}, {}, 0, False, id='expired-planner-sample'),
+  pytest.param({}, {'valid_until_ns': NOW - 1}, 0, False, id='expired-local-sample'),
+  pytest.param({}, {'observed_ns': NOW - 100_000_001}, 0, False, id='older-local-sample'),
+  pytest.param({}, {'observed_ns': NOW + 1}, 0, False, id='future-local-sample'),
+  pytest.param({'sourceObservedMonoTime': NOW - 10_000_000}, {}, 0, False, id='sample-after-planner-frame'),
+  pytest.param({}, {'episode': 2}, 0, False, id='new-episode'),
+  pytest.param({}, {'speed_mps': 24.}, 0, False, id='different-speed'),
+  pytest.param({'sourceProducerSessionId': 'old-card'}, {}, 0, False, id='different-producer'),
+  pytest.param({}, {}, NOW - 100_000_000, False, id='planner-sample-before-settings-change'),
+])
+def test_dashboard_engagement_qualifies_both_samples(sm, state_changes, local_changes, after_ns, matches):
+  state = sm['slcState']
+  state.source, state.sourceProducerSessionId, state.sourceEpisode = 'dashboard', 'card', 1
+  state.frameMonoTime = sm.logMonoTime['slcState'] = NOW - 20_000_000
+  state.sourceObservedMonoTime, state.sourceValidUntilMonoTime = NOW - 100_000_000, NOW + 50_000_000
+  state.hasPresentationAdjustedLimit, state.presentationAdjustedLimit = True, 25.
+  for field, value in state_changes.items():
+    setattr(state, field, value)
+  local = Observation(**({'status': Status.VALID, 'speed_mps': 25., 'episode': 1,
+                          'observed_ns': NOW - 5_000_000, 'valid_until_ns': NOW + 145_000_000} | local_changes))
+
+  assert (physical_actions.engagement_state(sm, local, 'card', NOW, after_ns=after_ns) is not None) == matches