diff --git a/openpilot/starpilot/speed_limits/physical_actions.py b/openpilot/starpilot/speed_limits/physical_actions.py index c5af5b58ad..25f3ca6b57 100644 --- a/openpilot/starpilot/speed_limits/physical_actions.py +++ b/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 diff --git a/openpilot/starpilot/speed_limits/tests/test_engagement.py b/openpilot/starpilot/speed_limits/tests/test_engagement.py index 166c7e90f4..a14b6f618d 100644 --- a/openpilot/starpilot/speed_limits/tests/test_engagement.py +++ b/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.)], diff --git a/openpilot/starpilot/speed_limits/tests/test_physical_actions.py b/openpilot/starpilot/speed_limits/tests/test_physical_actions.py index 5a92948245..f9f71568a0 100644 --- a/openpilot/starpilot/speed_limits/tests/test_physical_actions.py +++ b/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