From 407288852f5837310ff342cc94224e02c8aa3e00 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 1 Nov 2026 16:52:24 -0600 Subject: [PATCH] Preserve Ford AOL through startup and brake transitions --- openpilot/selfdrive/car/card.py | 10 +++++-- openpilot/starpilot/aol/transport_pause.py | 9 ++++-- openpilot/starpilot/car/ford/aol.py | 10 ++++++- .../starpilot/tests/ford_lifecycle_helpers.py | 28 +++++++++++++++-- .../test_ford_bluepilot_library_lifecycle.py | 16 +++++++++- .../starpilot/tests/test_ford_main_pulse.py | 30 ++++++++++++++++--- prebuilt.json | 2 +- 7 files changed, 90 insertions(+), 15 deletions(-) diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 425a7b55e3..134ae3ef0e 100644 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -68,6 +68,7 @@ from openpilot.starpilot.car.nissan.aol import ( ) from openpilot.starpilot.car.hyundai.aol import aol_safety_config as hyundai_aol_safety_config, pending_stock_angle_arm from openpilot.starpilot.car.hyundai.aol import retained_shared_long_cancel_intent as hkg_cancel_intent_retained +from openpilot.starpilot.car.ford.aol import brake_pause_active as ford_brake_pause_active from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, current_axis, current_native from openpilot.starpilot.aol.vehicle import create_intent as create_aol_intent, native_latch_rejected, policy_for as aol_policy_for from openpilot.starpilot.aol.wire import IntentState, encode_intent @@ -624,7 +625,8 @@ class Car: intent_producer_session=getattr(self, 'slc_producer_session', ''), main_arm_ns=getattr(self.aol_card_intent, 'main_arm_ns', 0), startup_calibration_pending=getattr(self.aol_card_intent, 'startup_calibration_pending', False), - intent_paused=getattr(self.aol_card_intent, 'pause_lateral', False)) + intent_paused=(getattr(self.aol_card_intent, 'pause_lateral', False) or + ford_brake_pause_active(self.CP, CS, self.aol_settings))) fault_active = True if transport_reset else None event_ns = int(self.sm.logMonoTime['onroadEvents']) if (getattr(self.aol_card_intent, 'requires_fault_observation', self.aol_card_intent.explicit_latch) and @@ -676,8 +678,9 @@ class Car: panda_ready=angle_panda_ready, restricted=(getattr(self.aol_transport_feedback, 'ordinary_warning_qualified', False) or restrict(self.CP, CS) or - ((angle_aol or mazda_aol or preap_aol or rivian_aol) and self.aol_card_intent.pause_lateral) or + ((angle_aol or ford_aol or mazda_aol or preap_aol or rivian_aol) and self.aol_card_intent.pause_lateral) or (angle_aol and CS.brakePressed and (not CS.standstill) and (CS.vEgo < self.aol_settings.pause_brake_mps)) or + ford_brake_pause_active(self.CP, CS, self.aol_settings) or (volkswagen_aol and (self.aol_card_intent.pause_lateral or (CS.brakePressed and (not CS.standstill) and (CS.vEgo < self.aol_card_intent.settings.pause_brake_mps))))), now_ns=now_ns, @@ -931,7 +934,8 @@ class Car: intent_producer_session=getattr(self, 'slc_producer_session', ''), main_arm_ns=getattr(self.aol_card_intent, 'main_arm_ns', 0), startup_calibration_pending=getattr(self.aol_card_intent, 'startup_calibration_pending', False), - intent_paused=getattr(self.aol_card_intent, 'pause_lateral', False)): + intent_paused=(getattr(self.aol_card_intent, 'pause_lateral', False) or + ford_brake_pause_active(self.CP, CS, self.aol_settings))): return True events = self.sm['onroadEvents'] if self.aol_transport_feedback.retention_qualified or self.aol_transport_feedback.retention_pending: diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index 9678b23331..0adc07652b 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -350,7 +350,7 @@ class TransportPauseFeedback: # Before 10Hz LONG echo: physical pedal/cancel retains only real independent LAT intent. ford_withdrawal = bool(cp.brand == 'ford' and ford_qualified(cp, marked_only=True) and cp.openpilotLongitudinalControl and (cs.brakePressed or not cs.cruiseState.enabled or physical_cancel(cs, cp) or - not axis.desiredLongitudinal and not axis.longitudinalActive) and not (cs.brakePressed and cs.gasPressed) and not cs.regenBraking and + not axis.desiredLongitudinal and not axis.longitudinalActive) and not cs.regenBraking and cs.cruiseState.available and not cs.doorOpen and not cs.seatbeltUnlatched and not cs.vehicleSensorsInvalid and cs.gearShifter == structs.CarState.GearShifter.drive) # Ford angle/curvature share native independent axes: intent-only 10Hz LONG withdrawal/recovery echo join. @@ -1534,7 +1534,8 @@ class TransportPauseFeedback: native = _healthy_transport_native(sm, cp, cs, str(axis.sessionId), now_ns) if (native is None and intent_paused and cs.canValid and not cs.canTimeout and not cs.steerFaultPermanent and not any(signature[9:13])): - native = _configured_transport_native(sm, cp, str(axis.sessionId), now_ns) + native = _configured_transport_native(sm, cp, str(axis.sessionId), now_ns, + require_longitudinal=not (cp.brand == 'ford' and cs.brakePressed)) from opendbc.car import structs previous = self.signature inactive = bool(not self.failed and self.session and previous is not None and @@ -1557,7 +1558,9 @@ class TransportPauseFeedback: event.noEntry and not event.userDisable) and (not event.userDisable or event.name in (log.OnroadEvent.EventName.pcmDisable, log.OnroadEvent.EventName.buttonCancel, - log.OnroadEvent.EventName.reverseGear) or intent_paused and event.name == log.OnroadEvent.EventName.wrongGear) + log.OnroadEvent.EventName.reverseGear) or intent_paused and + (event.name == log.OnroadEvent.EventName.wrongGear or cp.brand == 'ford' and cs.brakePressed and + event.name == log.OnroadEvent.EventName.pedalPressed)) for event in sm['onroadEvents']) and native is not None and native.pandaInventory and self.healthy_inventory == (native.pandaSerial, tuple(slot.hardwareSerial for slot in native.pandaInventory)) and diff --git a/openpilot/starpilot/car/ford/aol.py b/openpilot/starpilot/car/ford/aol.py index 28e1af2963..4910867350 100644 --- a/openpilot/starpilot/car/ford/aol.py +++ b/openpilot/starpilot/car/ford/aol.py @@ -1,4 +1,5 @@ from dataclasses import replace +from math import isfinite from opendbc.car import structs from opendbc.car.ford.aol import qualified, AOL_WORDS, MANUAL, temporary_restriction, aol_safety_config, profile_word from openpilot.starpilot.aol.intent import AolCardIntent, AOL_TOGGLE @@ -70,7 +71,7 @@ class FordCardIntent(AolCardIntent): axis.intentProducerSessionId != producer or stamp != axis.observedMonoTime or not 0 < axis.sourceCarStateMonoTime <= stamp < now_ns <= axis.validUntilMonoTime or now_ns - axis.sourceCarStateMonoTime > 30_000_000 or axis.validUntilMonoTime - stamp > 30_000_000 or - str(axis.faultReason) != 'none' or str(axis.mode) != 'off' or axis.nativeAcknowledged or + str(axis.faultReason) != 'none' or str(axis.mode) != 'off' or axis.desiredLateral or axis.desiredLongitudinal or axis.lateralActive or axis.longitudinalActive or not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents']): return @@ -202,3 +203,10 @@ def physical_cancel_lateral_allowed(cp, cs, intent, native, *, settings, previou native.validUntilMonoTime - native.observedMonoTime <= SAFETY_MAX_AGE_NS) except (AttributeError, KeyError, IndexError, TypeError, ValueError, OverflowError): return False + + +def brake_pause_active(cp, state, settings): + """A configured brake pause withdraws commands without discarding armed intent.""" + return bool(settings is not None and qualified(cp, marked_only=True) and + state.brakePressed and not state.standstill and isfinite(state.vEgo) and + isfinite(settings.pause_brake_mps) and 0 <= state.vEgo < settings.pause_brake_mps) diff --git a/openpilot/starpilot/tests/ford_lifecycle_helpers.py b/openpilot/starpilot/tests/ford_lifecycle_helpers.py index e28f599535..c76dde0a19 100644 --- a/openpilot/starpilot/tests/ford_lifecycle_helpers.py +++ b/openpilot/starpilot/tests/ford_lifecycle_helpers.py @@ -104,6 +104,8 @@ def ford_joined(identity, alpha, hands_free, monkeypatch, record_property): for key, value in (('PauseAOLOnBrake', b'0.0'), ('CancelButtonControl', b'0'), ('MainCruiseButtonControl', b'0')): Path(saved.get_param_path(key)).write_bytes(value) + if hasattr(monkeypatch, 'ford_brake_pause'): + Path(saved.get_param_path('AolBrakePauseSpeedMps')).write_bytes(str(monkeypatch.ford_brake_pause).encode()) Path(saved.get_param_path("FordHandsFreeCluster")).write_bytes(b"1" if hands_free else b"0") fp = gen_empty_fingerprint() topology = getattr(monkeypatch, 'ford_topology', 'automatic') @@ -561,14 +563,14 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag # A real native fault can precede the next 10Hz host receipt. Active # requests in that gap must be rejected, never treated as accepted. fault_before_receipt = (getattr(monkeypatch, 'ford_native_10hz', False) and - scenario == 'route22' and fault in ('heartbeat', 'source-loss') and permission == 0) + scenario in ('route22', 'brake-overlap', 'brake-pause') and fault in ('heartbeat', 'source-loss') and permission == 0) if fault_before_receipt and frame.address in (0x3D3, 0x3D6): mode = ((frame.dat[4] >> 2) & 7) if frame.address == 0x3D3 else ((frame.dat[0] >> 4) & 7) if mode: assert not accepted, 'Native fault admitted active steering' if not accepted: display_only = frame.address in (0x18A, 0x3D8) - display_veto = (display_only and scenario == 'route22' and + display_veto = (display_only and scenario in ('route22', 'brake-overlap', 'brake-pause') and fault in ('heartbeat', 'source-loss', 'rx-quality') and permission == 0) assert display_veto or (fault_before_receipt and not safety.get_controls_allowed()), ( steps, fault, hex(frame.address), bytes(frame.dat).hex(), request, native_request, permission) @@ -857,6 +859,28 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag assert state.steeringPressed assert sd_state.alertType != 'steerSaturated/warning' return + if scenario in ('brake-overlap', 'brake-pause'): + gas = 'gas' if scenario == 'brake-overlap' else None + for _ in range(50): + state, axis, sd_state, command = step(engaged=True, fault=gas) + assert host.aol_card_intent.allowed_latch and command.latActive + if scenario == 'brake-pause': + joined.speed = .2 + for _ in range(60): + state, axis, sd_state, command = step(brake=True, fault=gas) + assert host.aol_card_intent.allowed_latch, (host.aol_card_intent.latch_clear_reason, transition_trace[-4:]) + if state.brakePressed: + assert not command.longActive + paused = not state.standstill and state.vEgo < .44704 + assert command.latActive == (not paused) + for _ in range(50): + state, axis, sd_state, command = step() + assert host.aol_card_intent.allowed_latch + assert command.latActive and not command.longActive + for _ in range(30): + state, axis, sd_state, command = step(fault='heartbeat') + assert not host.aol_card_intent.allowed_latch and not command.latActive and not command.longActive + return if scenario == 'bluepilot-pause': for _ in range(41): state, axis, sd_state, command = step(engaged=True) diff --git a/openpilot/starpilot/tests/test_ford_bluepilot_library_lifecycle.py b/openpilot/starpilot/tests/test_ford_bluepilot_library_lifecycle.py index eb56fee213..8151401c5a 100644 --- a/openpilot/starpilot/tests/test_ford_bluepilot_library_lifecycle.py +++ b/openpilot/starpilot/tests/test_ford_bluepilot_library_lifecycle.py @@ -6,7 +6,7 @@ from openpilot.starpilot.tests.ford_lifecycle_helpers import ford_joined as ford @pytest.fixture(autouse=True) -def library_settings(monkeypatch, mode, pinion, topology): +def library_settings(monkeypatch, mode, pinion, topology, request): monkeypatch.ford_bluepilot = True monkeypatch.ford_angle = mode == 'angle' monkeypatch.ford_pinion = pinion @@ -14,6 +14,8 @@ def library_settings(monkeypatch, mode, pinion, topology): monkeypatch.ford_nonzero = True monkeypatch.ford_native_first_pedal = True monkeypatch.ford_native_10hz = True + if request.node.originalname == "test_brake_retains_independent_lateral_intent": + monkeypatch.ford_brake_pause = .44704 @pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 2), (CAR.FORD_F_150_MK14, 2), (CAR.FORD_ESCAPE_MK4, 0)]) @@ -115,3 +117,15 @@ def test_reengaging_in_a_manual_curve_respects_native_convergence(ford_joined, m monkeypatch.ford_curve_gas = gas monkeypatch.ford_curve_manual = manual run_actual_producer(ford_joined, monkeypatch, scenario='curvature-handoff') + + +@pytest.mark.parametrize('identity,hands_free', [ + ((CAR.FORD_MUSTANG_MACH_E_MK1, 2), True), +]) +@pytest.mark.parametrize('alpha', [False, True]) +@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) +@pytest.mark.parametrize('mode', ['curvature', 'angle']) +@pytest.mark.parametrize('pinion', [False, True]) +@pytest.mark.parametrize('brake_pause', [False, True], ids=['overlap', 'pause']) +def test_brake_retains_independent_lateral_intent(ford_joined, monkeypatch, brake_pause): + run_actual_producer(ford_joined, monkeypatch, scenario='brake-pause' if brake_pause else 'brake-overlap') diff --git a/openpilot/starpilot/tests/test_ford_main_pulse.py b/openpilot/starpilot/tests/test_ford_main_pulse.py index e873f92f14..3fbd791115 100644 --- a/openpilot/starpilot/tests/test_ford_main_pulse.py +++ b/openpilot/starpilot/tests/test_ford_main_pulse.py @@ -98,7 +98,7 @@ def startup_receipt(source_pause): intent.update(state(available=False), now_ns=wire.NOW - 10_000_000, fault_active=True) sm, native = wire.sample(cp, wire.NOW, wire.NOW + 10_000_000, 1, active=False, requested=False, allowed=False) def off(axis): - axis.nativeAcknowledged = False + axis.nativeAcknowledged = True if source_pause: axis.sourceCarStateMonoTime = 0 axis.faultReason = 'availabilityPause' @@ -125,7 +125,7 @@ def test_first_main_qualifies_only_current_off_receipt(): STARTUP_REFUSALS = ( 'stale', 'future', 'deadline', 'producer', 'session', 'event-stamp', 'wrong-event', 'critical', 'soft', 'can', 'timeout', 'permanent', 'temporary', 'acc', 'sensor', 'door', 'gear', 'disengage', - 'native-rx', 'heartbeat', 'native-request', 'native-grant', 'axis-active', 'axis-ack', 'panda-missing', + 'native-rx', 'heartbeat', 'native-request', 'native-grant', 'axis-active', 'axis-ack-no-native', 'panda-missing', ) @@ -165,8 +165,11 @@ def test_first_main_does_not_admit_faulted_or_unowned_receipt(source_pause, faul wire.change(sm, 'pandaStates', lambda pandas: setattr(pandas[0], 'safetyRxChecksInvalid' if fault == 'native-rx' else 'heartbeatLost', True)) elif fault in ('native-request', 'native-grant'): native = wire.native_change(sm, native, **{'requestedLateral' if fault == 'native-request' else 'lateralAllowed': True}) - elif fault in ('axis-active', 'axis-ack'): - wire.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'lateralActive' if fault == 'axis-active' else 'nativeAcknowledged', True)) + elif fault == 'axis-active': + wire.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'lateralActive', True)) + elif fault == 'axis-ack-no-native': + wire.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'nativeAcknowledged', True)) + native = None elif fault == 'panda-missing': panda_ready = False before = native @@ -186,3 +189,22 @@ def test_source_zero_availability_fault_cannot_qualify_first_main(): intent.update(cs.as_reader(), fault_active=True, now_ns=now) intent.update(cs.as_reader(), fault_active=False, now_ns=now + 10_000_000) assert not intent.allowed_latch and not intent.main_pending_ns + + +@pytest.mark.parametrize('acknowledged', [False, True]) +@pytest.mark.parametrize('gas', [False, True]) +def test_acknowledged_idle_main_arms_before_cruise_brake_withdrawal(acknowledged, gas): + from openpilot.starpilot.tests import test_ford_reverse_intent as wire + intent, cp, cs, sm, native, now = startup_receipt(False) + cs.gasPressed = gas + wire.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'nativeAcknowledged', acknowledged)) + original_native = native + original_stamps = dict(sm.logMonoTime) + intent.observe_startup_readiness(sm, cs, native=native, panda_ready=True, now_ns=now, producer=wire.PRODUCER) + assert intent.main_idle_transition_qualified + assert not intent.allowed_latch and native == original_native and sm.logMonoTime == original_stamps + intent.update(cs.as_reader(), fault_active=True, now_ns=now, standard_enabled=True) + assert intent.main_pending_ns == now and not intent.allowed_latch + intent.update(cs.as_reader(), fault_active=False, now_ns=now + 10_000_000, standard_enabled=True) + assert intent.allowed_latch and not intent.pause_lateral and not intent.main_pending_ns + assert intent.physical_arm_ns == now + 10_000_000 diff --git a/prebuilt.json b/prebuilt.json index 3a8bef29ea..d36e0c962e 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "abf414e89e892227c9c235b8dd5a7c8891726459b52617cd571428cfbafb3b38", "schema_version": 1} +{"protected_inventory_sha256": "c11fbe2409cae4ba5c1fc311a5715d93588886e26625717f7bc2997a0557d680", "schema_version": 1}