Preserve Toyota AOL intent through held-brake source gaps

openpilot/starpilot/aol/source_availability.py

@@ -28,11 +28,11 @@ def stock_lateral_role(self, CS, native, intent=None, *, allow_independent_cance
   if allow_independent_brake:
     from opendbc.car.hyundai.canfd_angle_aol import command_allowed
     from opendbc.car.hyundai.canfd_stock_aol import qualified as stock_canfd_qualified
-    from openpilot.starpilot.car.toyota.aol import qualified as toyota_qualified
+    from openpilot.starpilot.car.toyota.aol import qualified as toyota_qualified, held_lateral_brake
 
     settings = getattr(self, 'aol_settings', None)
     previous = getattr(self, 'aol_axis_decision', None)
-    brake_role = bool(settings is not None and settings.enabled and CS.brakePressed and not CS.regenBraking and
+    brake_role = held_lateral_brake(self, CS) or bool(settings is not None and settings.enabled and CS.brakePressed and not CS.regenBraking and
       previous is not None and previous.lateral_active and previous.native_acknowledged and not previous.longitudinal_active and
       (toyota_qualified(self.CP, marked_only=True) and CS.standstill and not CS.cruiseState.enabled and
        CS.gearShifter == car.CarState.GearShifter.drive and isfinite(CS.vEgoRaw) and isfinite(CS.vEgo) or
@@ -112,7 +112,8 @@ def ordinary_source_availability(self, CS, now_ns, native=None, *, replay: bool,
   companion = getattr(self, 'aol_intent_companion', None)
   events = self.events.to_msg()
   from openpilot.starpilot.car.gm.aol import held_sdgm_lateral_pedal
-  held_pedal = held_sdgm_lateral_pedal(self, CS)
+  from openpilot.starpilot.car.toyota.aol import held_lateral_brake
+  held_pedal = held_sdgm_lateral_pedal(self, CS) or held_lateral_brake(self, CS)
   if held_pedal:
     events = [event for event in events if not (event.name == EventName.pedalPressed and
       event.userDisable and not event.softDisable and not event.immediateDisable)]
@@ -121,7 +122,7 @@ def ordinary_source_availability(self, CS, now_ns, native=None, *, replay: bool,
     # 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):
+    if stock_lateral_role(self, CS, native, allow_independent_brake=held_pedal):
       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)]

openpilot/starpilot/aol/tests/test_toyota_brake_source_gap.py

@@ -0,0 +1,107 @@
+"""RAV4 route1d: held brake plus source timeout must withdraw output, not intent."""
+from contextlib import ExitStack
+
+import pytest
+from opendbc.car.toyota.interface import CarInterface
+from opendbc.car.toyota.values import CAR
+from openpilot.cereal import log
+from openpilot.common.prefix import OpenpilotPrefix
+from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
+from openpilot.starpilot.aol.tests.test_input_deadline_continuity import BASE, intent
+from openpilot.starpilot.aol.tests.test_toyota_parking_brake_deadline import refresh_saved_settings
+
+
+def rolling(brake=True):
+  state = fixture.car_state(brake=brake)
+  state.vEgo = state.vEgoRaw = .63179683685
+  state.standstill = False
+  state.cruiseState.available = True
+  state.cruiseState.enabled = False
+  return state
+
+
+@pytest.mark.parametrize('brake', (False, True))
+def test_rav4_held_brake_source_gap_preserves_armed_intent(brake):
+  with ExitStack() as cleanup:
+    cleanup.enter_context(OpenpilotPrefix())
+    case = fixture.IoniqAvailabilityTests(methodName='runTest')
+    cleanup.callback(case.doCleanups)
+    cp = CarInterface.get_params(CAR.TOYOTA_RAV4_TSS2, {0: {}, 1: {}, 2: {}}, [], False, False, False)
+    sd, controls, drive = case._authority_callers(False, cp=cp, genuine_events=True)
+    refresh_saved_settings(sd)
+    for tick in (0, 1):
+      axis, _, command = drive(tick, observed_state=rolling(brake), brake=brake, native_request_long=False)
+      assert axis.lateralActive and command.latActive and not command.longActive
+    prior, _, command = drive(2, offset_ns=5_209_427, state_stamp=BASE,
+      intent_message=intent(BASE, 3), observed_state=rolling(brake), brake=brake, native_request_long=False)
+    assert prior.lateralActive and command.latActive
+    # Recorded gap: source32.074ms old, prior command still3.135ms live.
+    axis, host, command = drive(5, offset_ns=2_073_802, timeout=True,
+      state_stamp=BASE, intent_message=intent(BASE, 3), brake=brake, native_request_long=False)
+    assert log.OnroadEvent.EventName.controlsMismatch not in sd.events.names, 'RAV4_HELD_BRAKE_SOURCE_GAP'
+    assert str(axis.faultReason) == 'availabilityPause' and axis.sourceCarStateMonoTime == 0
+    assert not axis.lateralActive and not command.latActive and not command.longActive and not host.enabled
+    assert axis.desiredLateral and axis.nativeAcknowledged and not sd.aol_authority_lost
+    source = BASE + 28_454_011
+    axis, host, command = drive(6, state_stamp=source, intent_message=intent(source, 4),
+      observed_state=rolling(brake), brake=brake, native_request_long=False)
+    assert axis.lateralActive and command.latActive and not command.longActive and not host.enabled
+    assert not sd.aol_authority_lost
+
+
+def test_rav4_brake_source_pause_reaches_actual_card_without_disarming():
+  from openpilot.starpilot.aol.tests.stock_lateral_test_helpers import callers
+  with ExitStack() as cleanup:
+    sd, _, drive, card, receive = callers(cleanup, 'toyota', True)
+    refresh_saved_settings(sd)
+    prior, _, command = drive(2, offset_ns=5_209_427, state_stamp=BASE,
+      intent_message=intent(BASE, 3), observed_state=rolling(), brake=True, native_request_long=False)
+    assert prior.lateralActive and command.latActive
+    assert receive(BASE + 5_209_427, state=rolling())
+    axis, host, command = drive(5, offset_ns=2_073_802, timeout=True,
+      state_stamp=BASE, intent_message=intent(BASE, 3), brake=True, native_request_long=False)
+    assert str(axis.faultReason) == 'availabilityPause' and axis.sourceCarStateMonoTime == 0
+    assert not command.latActive and not command.longActive and not host.enabled
+    assert receive(BASE + 32_073_802, state=rolling()), 'RAV4_CARD_BRAKE_SOURCE_PAUSE'
+    assert card.aol_card_intent.allowed_latch
+    source = BASE + 28_454_011
+    axis, host, command = drive(6, state_stamp=source, intent_message=intent(source, 4),
+      observed_state=rolling(), brake=True, native_request_long=False)
+    assert axis.lateralActive and command.latActive and not command.longActive and not host.enabled
+    assert receive(BASE + 40_000_000, state=rolling())
+
+
+@pytest.mark.parametrize('fault', ('native_denied', 'native_invalid', 'rx_fault', 'relay_fault',
+                                  'latch_withdrawn', 'missing_anchor', 'pause_setting', 'dm_lockout'))
+def test_rav4_brake_source_gap_does_not_waive_restrictions(fault):
+  with ExitStack() as cleanup:
+    cleanup.enter_context(OpenpilotPrefix())
+    case = fixture.IoniqAvailabilityTests(methodName='runTest')
+    cleanup.callback(case.doCleanups)
+    cp = CarInterface.get_params(CAR.TOYOTA_RAV4_TSS2, {0: {}, 1: {}, 2: {}}, [], False, False, False)
+    sd, _, drive = case._authority_callers(False, cp=cp, genuine_events=True)
+    refresh_saved_settings(sd)
+    for tick in (0, 1):
+      axis, _, command = drive(tick, observed_state=rolling(), brake=True, native_request_long=False)
+      assert axis.lateralActive and command.latActive
+    prior, _, command = drive(2, offset_ns=5_209_427, state_stamp=BASE,
+      intent_message=intent(BASE, 3), observed_state=rolling(), brake=True, native_request_long=False)
+    assert prior.lateralActive and command.latActive
+    if fault == 'missing_anchor':
+      sd.aol_ordinary_source_anchor = None
+    elif fault == 'pause_setting':
+      sd.params.put('PauseAOLOnBrake', 30., block=True)
+      refresh_saved_settings(sd)
+    elif fault == 'dm_lockout':
+      sd.aol_dm_lateral_inhibit = True
+    axis, host, command = drive(5, offset_ns=2_073_802, timeout=True,
+      state_stamp=BASE, intent_message=intent(BASE, 3, latched=fault != 'latch_withdrawn'),
+      brake=True, native_request_long=False, allowed=fault != 'native_denied',
+      native_valid=fault != 'native_invalid', panda_fault=fault == 'rx_fault',
+      relay_fault=fault == 'relay_fault')
+    assert not axis.lateralActive and not command.latActive
+    assert not axis.longitudinalActive and not command.longActive and not host.enabled
+    if fault == 'latch_withdrawn':
+      assert not axis.desiredLateral and sd.aol_last_intent is None
+    else:
+      assert str(axis.faultReason) != 'availabilityPause'

openpilot/starpilot/aol/transport_pause.py

@@ -133,7 +133,7 @@ def stock_lateral_off(cp, cs, native, *, allow_independent_cancel=False, allow_i
 
   independent_angle = angle_qualified(cp, marked_only=True)
   independent_brake = independent_angle or (allow_independent_brake and (
-    toyota_qualified(cp, marked_only=True) and cs.standstill and not cs.cruiseState.enabled or
+    toyota_qualified(cp, marked_only=True) and not cs.cruiseState.enabled or
     cp.brand == 'hyundai' and len(cp.safetyConfigs) == 1 and stock_canfd_qualified(cp, marked_only=True)))
   return bool(cp.pcmCruise and cs.cruiseState.available and
     (not cs.cruiseState.enabled or independent_brake and allow_independent_brake and cs.brakePressed) and
@@ -955,10 +955,14 @@ class TransportPauseFeedback:
     # owner, not an independent LAT cancellation. Qualify only this inactive source-zero
     # proof against its accepted LAT-only baseline and current real native grant.
     previous = self.signature
+    from openpilot.starpilot.car.toyota.aol import qualified as toyota_qualified
+    toyota_brake = bool(toyota_qualified(cp, marked_only=True) and cs.brakePressed and
+      not cs.regenBraking and not cs.gasPressed and self.lateral_only_baseline and
+      not native.requestedLongitudinal and not native.longitudinalAllowed)
     stock_user = stock_lateral_user_event(cp)
     stock_lateral = bool(reason == 'availabilityPause' and axis.sourceCarStateMonoTime == 0 and
       not axis.lateralActive and not axis.longitudinalActive and not axis.desiredLongitudinal and
-      stock_lateral_off(cp, cs, native) and previous is not None and
+      stock_lateral_off(cp, cs, native, allow_independent_brake=toyota_brake) and previous is not None and
       previous[0] == self.session and previous[7] == self.session and
       previous[14] == intent_producer_session and previous[15] == self.availability_epoch and
       (previous[11] and not previous[12] and previous[13] and not previous[10] and
@@ -977,10 +981,11 @@ class TransportPauseFeedback:
        0 <= now_ns - previous[8] <= SAFETY_MAX_AGE_NS))
     if stock_lateral:
       users = [event for event in events if event.userDisable]
-      if any(event.name != stock_user or event.softDisable or event.immediateDisable
-             for event in users):
+      if any((event.name != stock_user and not (toyota_brake and event.name == log.OnroadEvent.EventName.pedalPressed)) or
+             event.softDisable or event.immediateDisable for event in users):
         return False
-      events = [event for event in events if not (event.name == stock_user and
+      events = [event for event in events if not ((event.name == stock_user or
+        toyota_brake and event.name == log.OnroadEvent.EventName.pedalPressed) and
         event.userDisable and not event.softDisable and not event.immediateDisable)]
     if cp.brand == 'gm':
       from openpilot.starpilot.car.gm.aol import sdgm_lateral_pedal_source_zero

openpilot/starpilot/car/toyota/aol.py

@@ -231,3 +231,28 @@ def configuration_settings_policy(cp):
       cp.steerControlType == structs.CarParams.SteerControlType.torque):
     return AolVehiclePolicy(settings_supported=True, fixed_cruise_buttons=True, distance_pause_only=True)
   return None
+
+
+def held_lateral_brake(owner, cs):
+  """An already accepted brake may retain LAT intent, never stale output."""
+  from math import isfinite
+  from openpilot.starpilot.nostalgia import physical_cancel
+  anchor = getattr(owner, 'aol_ordinary_source_anchor', None)
+  settings = getattr(owner, 'aol_settings', None)
+  if not qualified(owner.CP, marked_only=True) or anchor is None:
+    return False
+  proof, native, accepted, _, state = anchor
+  return bool(cs is state and not getattr(owner, 'car_state_samples', ()) and
+    proof[8] and not proof[9] and not owner.enabled and not owner.active and
+    native is not None and native.requestedLateral and native.lateralAllowed and
+    not native.requestedLongitudinal and not native.longitudinalAllowed and
+    accepted is not None and accepted.allowedLatch and accepted.lateralArmed and
+    accepted.settingsQualified and not accepted.pauseLateral and not accepted.pauseLongitudinal and
+    settings is not None and settings.enabled and
+    not getattr(owner, 'aol_dm_lateral_inhibit', False) and
+    cs.brakePressed and not cs.regenBraking and not cs.gasPressed and
+    cs.cruiseState.available and not cs.cruiseState.enabled and
+    cs.gearShifter == structs.CarState.GearShifter.drive and
+    isfinite(cs.vEgoRaw) and isfinite(cs.vEgo) and
+    (cs.standstill or cs.vEgo >= settings.pause_brake_mps) and
+    not cs.seatbeltUnlatched and not physical_cancel(cs, owner.CP))