Preserve stock CAN-FD AOL intent through held-gas recovery

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

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):

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)

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

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "5ee191de2be6e6c25a0d884b343a5a82a93e70d2fea1b8189d4733d54f7a1706", "schema_version": 1}
+{"protected_inventory_sha256": "e6bd92c79fc8e1ad9bdc647dfcef99a0d2a99d62a3b0fc515f6f9525c2ccfdfb", "schema_version": 1}