Keep Toyota AOL armed through stationary output gaps

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -1269,8 +1269,10 @@ class SelfdriveD:
     if user_disable:
       from opendbc.car.hyundai.canfd_angle_aol import qualified as angle_qualified
       from opendbc.car.hyundai.canfd_stock_aol import qualified as stock_canfd_qualified
+      from openpilot.starpilot.car.toyota.aol import qualified as toyota_qualified
 
-      if (angle_qualified(self.CP, marked_only=True) or
+      if (toyota_qualified(self.CP, marked_only=True) and CS.standstill or
+          angle_qualified(self.CP, marked_only=True) or
           self.CP.brand == 'hyundai' and CS.brakePressed and len(self.CP.safetyConfigs) == 1 and
           stock_canfd_qualified(self.CP, marked_only=True)):
         native = current_native(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_session_id)

openpilot/starpilot/aol/source_availability.py

@@ -28,12 +28,15 @@ 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
 
     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
       previous is not None and previous.lateral_active and previous.native_acknowledged and not previous.longitudinal_active and
-      (CS.vEgo >= settings.pause_brake_mps and command_allowed(self.CP, CS) or
+      (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
+       CS.vEgo >= settings.pause_brake_mps and command_allowed(self.CP, CS) or
        len(self.CP.safetyConfigs) == 1 and stock_canfd_qualified(self.CP, marked_only=True) and
        CS.gearShifter == car.CarState.GearShifter.drive and isfinite(CS.vEgoRaw) and isfinite(CS.vEgo) and
        (settings.pause_brake_mps <= 0 or CS.vEgo >= settings.pause_brake_mps or CS.standstill)))

openpilot/starpilot/aol/tests/test_toyota_parking_brake_deadline.py

@@ -0,0 +1,121 @@
+"""Route0e: actual SD/Controls constructors, synthetic typed input/native transport."""
+from contextlib import ExitStack
+from unittest.mock import Mock, patch
+
+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
+
+
+
+def refresh_saved_settings(sd):
+  # The shared caller fixture clears constructor settings to None. Exercise
+  # the production saved-settings owner once instead of installing a field.
+  stop = Mock()
+  stop.is_set.side_effect = [False, True]
+  with patch('openpilot.selfdrive.selfdrived.selfdrived.time.sleep'):
+    sd.params_thread(stop)
+  assert sd.aol_settings is not None and sd.aol_settings.enabled
+
+
+def stopped(brake):
+  state = fixture.car_state(brake=brake)
+  state.vEgo = state.vEgoRaw = 0.
+  state.standstill = True
+  state.cruiseState.available = True
+  state.cruiseState.enabled = False
+  return state
+
+
+@pytest.mark.parametrize('identity', (CAR.TOYOTA_HIGHLANDER_TSS2, CAR.TOYOTA_COROLLA_TSS2))
+@pytest.mark.parametrize('brake', (False, True))
+@pytest.mark.parametrize('expired', (False, True))
+def test_stopped_pcm_off_fresh_input_after_prior_lateral_deadline(identity, brake, expired):
+  with ExitStack() as cleanup:
+    cleanup.enter_context(OpenpilotPrefix())
+    cleanup.enter_context(patch.dict('os.environ', {'SIMULATION': '0', 'REPLAY': '0'}))
+    case = fixture.IoniqAvailabilityTests(methodName='runTest')
+    cleanup.callback(case.doCleanups)
+    cp = CarInterface.get_params(identity, {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, host, command = drive(tick, observed_state=stopped(brake), brake=brake, native_request_long=False)
+      assert axis.lateralActive and command.latActive
+      assert not host.enabled and not command.longActive
+    prior, host, command = drive(2, offset_ns=8_437_887, state_stamp=BASE,
+      intent_message=intent(BASE, 3), observed_state=stopped(brake), brake=brake, native_request_long=False)
+    assert prior.lateralActive and command.latActive and not host.enabled and not command.longActive
+    assert prior.validUntilMonoTime == BASE + 38_437_887
+    # Exact recorded deltas at the takeover: current source9.155010ms old;
+    # prior output479133ns expired. No source/permission deadline is extended.
+    source = BASE + 29_762_010
+    now = BASE + (38_917_020 if expired else 33_917_020)
+    if expired:
+      with patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=now):
+        old, _ = controls.state_control()
+      assert not old.latActive and not old.longActive
+    axis, host, command = drive(5, offset_ns=8_917_020 if expired else 3_917_020,
+      state_stamp=source, intent_message=intent(source, 4), observed_state=stopped(brake),
+      brake=brake, native_request_long=False)
+    assert log.OnroadEvent.EventName.controlsMismatch not in sd.events.names, 'TOYOTA_HELD_BRAKE_EXPIRED_PRIOR_OUTPUT'
+    assert axis.sourceCarStateMonoTime == source and axis.lateralActive and command.latActive
+    assert not host.enabled and not host.active and not command.longActive and not axis.longitudinalActive
+    assert not sd.aol_authority_lost and sd.aol_last_intent.allowedLatch
+
+
+@pytest.mark.parametrize('restriction', ('native_denied', 'native_invalid', 'can_invalid', 'eps', 'cancel', 'intent_withdrawn', 'source_reversed', 'reverse'))
+def test_stopped_brake_current_restrictions_still_withdraw(restriction):
+  with ExitStack() as cleanup:
+    cleanup.enter_context(OpenpilotPrefix())
+    cleanup.enter_context(patch.dict('os.environ', {'SIMULATION': '0', 'REPLAY': '0'}))
+    case = fixture.IoniqAvailabilityTests(methodName='runTest')
+    cleanup.callback(case.doCleanups)
+    cp = CarInterface.get_params(CAR.TOYOTA_HIGHLANDER_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=stopped(True), brake=True, native_request_long=False)
+      assert axis.lateralActive and command.latActive
+    prior, _, command = drive(2, offset_ns=8_437_887, state_stamp=BASE,
+      intent_message=intent(BASE, 3), observed_state=stopped(True), brake=True, native_request_long=False)
+    assert prior.lateralActive and command.latActive
+    source = BASE - 1 if restriction == 'source_reversed' else BASE + 29_762_010
+    state = stopped(True)
+    if restriction == 'can_invalid':
+      state.canValid = False
+    elif restriction == 'eps':
+      state.steerFaultPermanent = True
+    elif restriction == 'cancel':
+      state.buttonEvents = [fixture.car.CarState.ButtonEvent(type='cancel', pressed=True)]
+    elif restriction == 'reverse':
+      state.gearShifter = fixture.car.CarState.GearShifter.reverse
+    axis, host, command = drive(5, offset_ns=8_917_020, state_stamp=source,
+      intent_message=intent(source, 4, latched=restriction != 'intent_withdrawn'),
+      observed_state=state, brake=True, native_request_long=False,
+      allowed=restriction != 'native_denied', native_valid=restriction != 'native_invalid')
+    assert not axis.lateralActive and not command.latActive
+    assert not axis.longitudinalActive and not command.longActive and not host.enabled
+
+
+def test_stopped_brake_replacement_keeps_actual_card_latch():
+  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=8_437_887, state_stamp=BASE,
+      intent_message=intent(BASE, 3), observed_state=stopped(True), brake=True, native_request_long=False)
+    assert prior.lateralActive and command.latActive
+    assert receive(BASE + 8_437_887, state=stopped(True))
+    source, now = BASE + 29_762_010, BASE + 38_917_020
+    axis, host, command = drive(5, offset_ns=8_917_020, state_stamp=source,
+      intent_message=intent(source, 4), observed_state=stopped(True), brake=True, native_request_long=False)
+    assert log.OnroadEvent.EventName.controlsMismatch not in sd.events.names
+    assert axis.lateralActive and command.latActive and not command.longActive and not host.enabled
+    assert receive(now, state=stopped(True))
+    assert card.aol_card_intent.allowed_latch

openpilot/starpilot/aol/transport_pause.py

@@ -111,9 +111,12 @@ def stock_lateral_off(cp, cs, native, *, allow_independent_cancel=False, allow_i
 
   from opendbc.car.hyundai.canfd_stock_aol import qualified as stock_canfd_qualified
 
+  from openpilot.starpilot.car.toyota.aol import qualified as toyota_qualified
+
   independent_angle = angle_qualified(cp, marked_only=True)
-  independent_brake = independent_angle or (allow_independent_brake and cp.brand == 'hyundai' and
-                                             len(cp.safetyConfigs) == 1 and stock_canfd_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
+    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
     cs.canValid and not cs.canTimeout and not cs.steerFaultPermanent and not cs.steerFaultTemporary and