diff --git a/openpilot/starpilot/aol/source_availability.py b/openpilot/starpilot/aol/source_availability.py index 2738c96b60..9df6d8f208 100644 --- a/openpilot/starpilot/aol/source_availability.py +++ b/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)] diff --git a/openpilot/starpilot/aol/tests/test_toyota_brake_source_gap.py b/openpilot/starpilot/aol/tests/test_toyota_brake_source_gap.py new file mode 100644 index 0000000000..7d38b16ee4 --- /dev/null +++ b/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' diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index 78e1b7bdac..312af9442b 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/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 diff --git a/openpilot/starpilot/car/toyota/aol.py b/openpilot/starpilot/car/toyota/aol.py index f6d6855448..3a7996b8a2 100644 --- a/openpilot/starpilot/car/toyota/aol.py +++ b/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))