From f10a7e229bf94a340ed2b03c29bbbae0e4bd17bb Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 31 Oct 2026 16:45:21 -0500 Subject: [PATCH] Preserve stock CAN-FD AOL intent through held-gas recovery --- openpilot/selfdrive/selfdrived/selfdrived.py | 2 +- .../starpilot/aol/source_availability.py | 7 +- .../tests/test_stock_canfd_gas_recovery.py | 93 +++++++++++++++++++ openpilot/starpilot/aol/transport_pause.py | 2 +- prebuilt.json | 2 +- 5 files changed, 101 insertions(+), 5 deletions(-) create mode 100644 openpilot/starpilot/aol/tests/test_stock_canfd_gas_recovery.py diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index 649c1c3662..5ecb1a3404 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -1230,7 +1230,7 @@ class SelfdriveD: not 0 < intent.carStateLogMonoTime <= now_ns or now_ns - intent.carStateLogMonoTime > INTENT_MAX_AGE_NS or intent.carStateLogMonoTime <= anchor[2].carStateLogMonoTime or intent.producerSessionId != anchor[0][4] or companion is None or companion.integrity_epoch != anchor[0][5] or - (self.events.contains(ET.USER_DISABLE) and not stock_lateral_role(self, CS, native, intent)) or + (self.events.contains(ET.USER_DISABLE) and not stock_lateral_role(self, CS, native, intent, allow_independent_gas=True)) or self.events.contains(ET.IMMEDIATE_DISABLE) or native is None or native is not getattr(self.aol_transport_pause, 'observed_native', None) or native.pandaSerial != anchor[1].pandaSerial or native.observedMonoTime < anchor[1].observedMonoTime or diff --git a/openpilot/starpilot/aol/source_availability.py b/openpilot/starpilot/aol/source_availability.py index e5d086caae..5196778a77 100644 --- a/openpilot/starpilot/aol/source_availability.py +++ b/openpilot/starpilot/aol/source_availability.py @@ -131,11 +131,13 @@ def ordinary_source_availability(self, CS, now_ns, native=None, *, replay: bool, events = [event for event in events if not (event.name == EventName.pedalPressed and event.userDisable and not event.softDisable and not event.immediateDisable)] stock_user = None + stock_gas = False if any(event.name in (EventName.pcmDisable, EventName.cruiseDisabled) and event.userDisable for event in events): # Resolve the native value once, as this classifier already does below. # Normal healthy frames never enter this nominal stock-role branch. native = native or native_provider(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_session_id) - if stock_lateral_role(self, CS, native, allow_independent_brake=held_pedal): + if stock_lateral_role(self, CS, native, allow_independent_brake=held_pedal, allow_independent_gas=True): + stock_gas = bool(CS.gasPressed and anchor is not None and anchor[4].gasPressed) stock_user = stock_lateral_user_event(self.CP) events = [event for event in events if not (event.name == stock_user and event.userDisable and not event.softDisable and not event.immediateDisable)] @@ -210,7 +212,8 @@ def ordinary_source_availability(self, CS, now_ns, native=None, *, replay: bool, if any(not valid or not sample.canValid or sample.canTimeout or sample.steerFaultPermanent or sample.steerFaultTemporary or sample.accFaulted or sample.doorOpen or sample.seatbeltUnlatched or (sample.brakePressed or sample.regenBraking) and not held_pedal or - sample.gasPressed and not toyota_pedal or physical_cancel(sample, self.CP) or + sample.gasPressed and not (toyota_pedal or stock_gas) or + stock_gas and sample.gasPressed != accepted_state.gasPressed or physical_cancel(sample, self.CP) or sample.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.sport, car.CarState.GearShifter.low, car.CarState.GearShifter.brake) for sample, valid in samples): diff --git a/openpilot/starpilot/aol/tests/test_stock_canfd_gas_recovery.py b/openpilot/starpilot/aol/tests/test_stock_canfd_gas_recovery.py new file mode 100644 index 0000000000..d0709d3294 --- /dev/null +++ b/openpilot/starpilot/aol/tests/test_stock_canfd_gas_recovery.py @@ -0,0 +1,93 @@ +from contextlib import ExitStack +from dataclasses import replace + +from unittest import mock + +import pytest + +from openpilot.cereal import log +from openpilot.starpilot.aol.tests.test_stock_location_family_intent import stock_family_callers +from openpilot.starpilot.aol.tests.test_input_deadline_continuity import BASE + + +FAMILIES = ['canfd_radar', 'canfd_lka', 'canfd_lka_alt', 'canfd_camera', 'canfd_ccnc_camera'] + + +@pytest.mark.parametrize('family', FAMILIES) +@pytest.mark.parametrize('interruption', ['source_expiry', 'temporary_eps']) +def test_held_gas_keeps_stock_canfd_aol_intent_through_recoverable_withdrawal(family, interruption): + with ExitStack() as cleanup: + sd, drive, card, receive, state = stock_family_callers(cleanup, family) + sd.aol_settings = card.aol_card_intent.settings + state.gasPressed = True + for tick in range(2, 5): + axis, _, command = drive(tick, observed_state=state, gas=True, native_request_long=False, panda_controls_allowed=False) + assert axis.lateralActive and command.latActive and not command.longActive + assert receive(BASE + tick * 10_000_000, state=state, + axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + deadline = axis.validUntilMonoTime + if interruption == 'source_expiry': + with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=BASE + 70_011_000): + sd.publish_axis_proof(sd.CS_prev) + axis = sd.pm.messages['aolAxisState'].aolAxisState + assert deadline < axis.observedMonoTime + assert not axis.lateralActive and not axis.longitudinalActive + assert not sd.aol_authority_lost and str(axis.faultReason) == 'availabilityPause' + assert log.OnroadEvent.EventName.controlsMismatch not in sd.events.names + assert receive(BASE + 70_011_000, state=state, + axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + recovery_tick = 8 + else: + state.steerFaultTemporary = True + for tick in range(5, 21): + axis, _, command = drive(tick, observed_state=state, gas=True, native_request_lat=False, + allowed=False, native_request_long=False, panda_controls_allowed=False) + assert not axis.lateralActive and not command.latActive and not command.longActive + assert receive(BASE + tick * 10_000_000, state=state, + axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + state.steerFaultTemporary = False + assert state.gasPressed + assert receive(BASE + 210_000_000, state=state, + axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + axis, _, command = drive(21, observed_state=state, gas=True, native_request_lat=False, + allowed=False, native_request_long=False, panda_controls_allowed=False) + assert not axis.lateralActive and not command.latActive and not command.longActive + assert receive(BASE + 210_000_001, state=state, + axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + recovery_tick = 22 + assert card.aol_card_intent.allowed_latch + for tick in range(recovery_tick, recovery_tick + 3): + axis, _, command = drive(tick, observed_state=state, gas=True, native_request_long=False, panda_controls_allowed=False) + assert receive(BASE + tick * 10_000_000, state=state, + axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + assert axis.lateralActive and command.latActive and not command.longActive and not sd.enabled + + +@pytest.mark.parametrize('family', FAMILIES) +def test_old_eps_notification_does_not_disarm_recovered_stock_canfd_while_gas_held(family): + from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback + from openpilot.starpilot.aol.tests.test_stock_eps_recovery import recovered_pause + from openpilot.starpilot.tests.test_ford_reverse_intent import NOW, PRODUCER, sample, change, native_change + + with ExitStack() as cleanup: + _, _, card, _, state = stock_family_callers(cleanup, family) + state.cruiseState.available = True + feedback = TransportPauseFeedback(card.CP) + def matching_sample(*args, **kwargs): + peers, native = sample(*args, **kwargs) + slots = tuple(replace(slot, alternativeExperience=card.CP.alternativeExperience) for slot in native.pandaInventory) + native = native_change(peers, native, pandaInventory=slots) + change(peers, 'pandaStates', lambda pandas: setattr(pandas[0], 'alternativeExperience', card.CP.alternativeExperience)) + return peers, native + sm, _ = matching_sample(card.CP, NOW, NOW, 1) + assert not feedback.observe(sm, card.CP, state, NOW, latched=True, intent_producer_session=PRODUCER) + assert feedback.lateral_only_baseline + state.gasPressed = True + with mock.patch('openpilot.starpilot.aol.tests.test_stock_eps_recovery.sample', side_effect=matching_sample): + sm, native, now = recovered_pause(card.CP, state, feedback) + assert state.gasPressed and not state.steerFaultTemporary + baseline = (feedback.lateral_baseline_ns, feedback.lateral_source_ns, feedback.lateral_native_ns) + assert not feedback.observe(sm, card.CP, state, now, latched=True, intent_producer_session=PRODUCER) + assert feedback.temporary_host_qualified and not feedback.failed + assert not native.lateralAllowed and not native.longitudinalAllowed + assert baseline == (feedback.lateral_baseline_ns, feedback.lateral_source_ns, feedback.lateral_native_ns) diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index 03f324b89c..2948f7dd6a 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -546,7 +546,7 @@ class TransportPauseFeedback: cs.cruiseState.available and not cs.cruiseState.enabled and cs.gearShifter in (gears.drive, gears.low, gears.sport, gears.brake) and not cs.doorOpen and (not cs.seatbeltUnlatched or belt_eps_recovery) and not cs.brakePressed and not cs.regenBraking and - (not cs.gasPressed or stock_angle) and + (not cs.gasPressed or stock_angle or eps_recovery and stock_canfd_qualified(cp, marked_only=True)) and all(event.name == stock_user and not event.softDisable and not event.immediateDisable for event in users)) from openpilot.starpilot.car.ford.aol import temporary_eps_pedal_recovery diff --git a/prebuilt.json b/prebuilt.json index 52e6207c6b..1b710413d6 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "5ee191de2be6e6c25a0d884b343a5a82a93e70d2fea1b8189d4733d54f7a1706", "schema_version": 1} +{"protected_inventory_sha256": "e6bd92c79fc8e1ad9bdc647dfcef99a0d2a99d62a3b0fc515f6f9525c2ccfdfb", "schema_version": 1}