From fc5875831c1b0419cbb137554f3134d2d64792bd Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 25 Oct 2026 14:10:02 -0500 Subject: [PATCH] Arm Ford AOL when cruise engages before the automatic MAIN pulse --- openpilot/starpilot/car/ford/aol.py | 3 + .../starpilot/tests/ford_lifecycle_helpers.py | 65 ++++++++-- .../tests/test_ford_configuration_parity.py | 14 ++- .../starpilot/tests/test_ford_main_pulse.py | 114 ++++++++++++++++++ prebuilt.json | 2 +- 5 files changed, 189 insertions(+), 9 deletions(-) diff --git a/openpilot/starpilot/car/ford/aol.py b/openpilot/starpilot/car/ford/aol.py index 8876ee65fb..5b8da6bc64 100644 --- a/openpilot/starpilot/car/ford/aol.py +++ b/openpilot/starpilot/car/ford/aol.py @@ -135,6 +135,9 @@ class FordCardIntent(AolCardIntent): if self.auto_main and not available: self.allowed_latch = False state.buttonEvents = events + if self.auto_main and self.main_pulse: + # Automatic MAIN-on arms AOL even if ordinary cruise engaged first. + kwargs['standard_enabled'] = False super().update(state.as_reader(), **kwargs) diff --git a/openpilot/starpilot/tests/ford_lifecycle_helpers.py b/openpilot/starpilot/tests/ford_lifecycle_helpers.py index f07ccdcf15..ee8bf783d6 100644 --- a/openpilot/starpilot/tests/ford_lifecycle_helpers.py +++ b/openpilot/starpilot/tests/ford_lifecycle_helpers.py @@ -143,7 +143,7 @@ def ford_joined(identity, alpha, hands_free, monkeypatch, record_property): preframes.append(frame) with patch.object(native, 'rx', side_effect=pre_rx): - native.pump(1, main=3, mask=0, heartbeat=False, speed=15.0) + native.pump(1, main=0 if getattr(monkeypatch, 'ford_first_main', False) else 3, mask=0, heartbeat=False, speed=15.0) extras: list[tuple[str, int, dict[str, float]]] = [ ('SteeringPinion_Data', 0, {'StePinCompAnEst_D_Qf': 3}), ('Cluster_Info1_FD1', 0, {'AccEnbl_B_RqDrv': 1}), @@ -263,7 +263,8 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag route45_phase = ['warmup'] route45_trace = [] - def step(*, engaged=False, brake=False, reverse=False, warmup=False, fault=None, main_off=False, availability_transition=False, steering=False): + def step(*, engaged=False, brake=False, reverse=False, warmup=False, fault=None, main_off=False, + availability_transition=False, steering=False, cancel=False): nonlocal steps, warmup_waits, qualified_samples, active_lat_frames, active_long_frames, nonzero_after_brake, brake_seen brake_seen |= brake steps += 1 @@ -286,7 +287,9 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag fields['CcStat_D_Actl'] = 0 if scenario == 'route45' and refusal == 'cancel' and name == 'Steering_Data_FD1': fields['CcAslButtnCnclPress'] = 1 - if scenario == 'route45' and name == 'EPAS_INFO': + if scenario == 'route49' and name == 'Steering_Data_FD1': + fields['CcAslButtnCnclResPress'] = int(cancel) + if scenario in ('route45', 'route49') and name == 'EPAS_INFO': fields['SteeringColumnTorque'] = 2 if steering else 0 if scenario == 'route45' and name == 'Lane_Assist_Data3_FD1': fields['LatCtlSte_D_Stat'] = 0 if fault in ('temporary-lkas', 'temporary-lkas-gas') else 1 @@ -410,7 +413,8 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag host.state_publish(state, None) assert 'aolIntentWire' in published parser_ready = bool( - state.canValid and not state.canTimeout and state.cruiseState.available and str(state.gearShifter) == 'drive' and safety.safety_config_valid() + state.canValid and not state.canTimeout and (state.cruiseState.available or scenario == 'route49') and + str(state.gearShifter) == 'drive' and safety.safety_config_valid() ) if warmup and not parser_ready: assert steps <= start_tick + 120 and not engaged and not brake and not reverse @@ -422,9 +426,9 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag assert state.canValid and not state.canTimeout qualified_samples += 1 parsed_before_producer = state.to_dict() - assert (main_off or state.cruiseState.available or availability_transition and not state.cruiseState.enabled + assert (main_off or scenario == 'route49' or state.cruiseState.available or availability_transition and not state.cruiseState.enabled or scenario == 'route45' and getattr(monkeypatch, 'ford_route45_refusal', None) == 'main-off') - assert scenario == 'route45' or not state.steeringPressed + assert scenario in ('route45', 'route49') or not state.steeringPressed assert bool(state.gasPressed) == (fault in ('gas', 'temporary-eps-gas', 'temporary-lkas-gas') and not any(frame[0] == 0x204 for frame in delayed_pedal_frames)) axis, sd_state, command = drive( @@ -528,10 +532,57 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag return libsafety_py.make_CANPacket(frame[0], frame[2], frame[1]) for _ in range(120): - step(warmup=True) + step(warmup=True, main_off=scenario == 'route49') assert 0 <= warmup_waits < 120 and qualified_samples >= 100 assert host.ci_initialized # Read-only witness: real controls_update completed init. assert not host.aol_card_intent.allowed_latch + if scenario == 'route49': + assert not host.CP.openpilotLongitudinalControl + trace = [] + original_intent_update = host.aol_card_intent.update + + def trace_first_main(cs, **kwargs): + before = host.aol_card_intent + snapshot = {'tick': steps, 'main': cs.cruiseState.available, + 'ordinary': cs.cruiseState.enabled, 'fault': kwargs.get('fault_active'), + 'native_rejection': kwargs.get('native_rejection_ns'), + 'idle_ready': before.main_idle_transition_qualified, + 'pending_before': before.main_pending_ns, + 'axis_fault': str(host.sm['aolAxisState'].faultReason)} + result = original_intent_update(cs, **kwargs) + snapshot.update(armed=before.allowed_latch, reason=before.latch_clear_reason, + pending_after=before.main_pending_ns, inhibit=before._fault_inhibit) + trace.append(snapshot) + return result + + local.setattr(host.aol_card_intent, 'update', trace_first_main) + # Recorded first MAIN edge is raw0->5, not an already-idle MAIN3 fixture. + # Existing real Card/SD/native receipts, CAN and clocks remain in charge. + for _ in range(40): + state, axis, sd_state, command = step(engaged=True, fault='gas', steering=True) + assert state.cruiseState.available and state.cruiseState.enabled + assert command.enabled + assert host.aol_card_intent.allowed_latch, ('First MAIN0->5 did not arm AOL', trace[:12]) + cancel_seen = False + for _ in range(10): + state, axis, sd_state, command = step(engaged=True, fault='gas', cancel=True) + cancel_seen |= host.CI.CS.cancel_button + assert host.aol_card_intent.allowed_latch + assert cancel_seen + for _ in range(30): + state, axis, sd_state, command = step(fault='gas') + assert host.aol_card_intent.allowed_latch + assert not state.cruiseState.enabled and not command.enabled + assert command.latActive and joined.last_permission & 1 + for _ in range(20): + state, axis, sd_state, command = step(brake=True) + assert host.aol_card_intent.allowed_latch + for _ in range(30): + state, axis, sd_state, command = step() + assert host.aol_card_intent.allowed_latch + assert not command.enabled and command.latActive and joined.last_permission & 1 + joined.route49_trace = trace + return if scenario == 'route45': assert host.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 assert not host.CP.openpilotLongitudinalControl diff --git a/openpilot/starpilot/tests/test_ford_configuration_parity.py b/openpilot/starpilot/tests/test_ford_configuration_parity.py index 9d0a84a291..2b4fae2a48 100644 --- a/openpilot/starpilot/tests/test_ford_configuration_parity.py +++ b/openpilot/starpilot/tests/test_ford_configuration_parity.py @@ -7,8 +7,11 @@ from openpilot.starpilot.tests.ford_lifecycle_helpers import FORD_FACTORIES, for @pytest.fixture(autouse=True) -def configuration_topology(monkeypatch, topology): +def configuration_topology(monkeypatch, topology, request): monkeypatch.ford_topology = topology + if request.node.name.startswith('test_route49_first_main_engagement'): + monkeypatch.ford_first_main = True + monkeypatch.ford_angle = request.node.callspec.params['angle'] @pytest.mark.parametrize('identity', FORD_FACTORIES) @@ -17,3 +20,12 @@ def configuration_topology(monkeypatch, topology): @pytest.mark.parametrize('topology', ['automatic', 'manual', 'multi-panda', 'manual-multi']) def test_actual_configuration_lifecycle(ford_joined, monkeypatch): run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity') + + +@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) +@pytest.mark.parametrize('alpha', [False]) +@pytest.mark.parametrize('hands_free', [False, True]) +@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) +@pytest.mark.parametrize('angle', [False, True]) +def test_route49_first_main_engagement_arms_before_cruise_cancel(ford_joined, monkeypatch, angle): + run_actual_producer(ford_joined, monkeypatch, scenario='route49') diff --git a/openpilot/starpilot/tests/test_ford_main_pulse.py b/openpilot/starpilot/tests/test_ford_main_pulse.py index 0d1450173c..e873f92f14 100644 --- a/openpilot/starpilot/tests/test_ford_main_pulse.py +++ b/openpilot/starpilot/tests/test_ford_main_pulse.py @@ -72,3 +72,117 @@ def test_fault_at_original_main_edge_cannot_arm_after_fault_clears(edge_fault): assert not intent.allowed_latch intent.update(state(available=True), now_ns=1_030_000_000, fault_active=False) assert not intent.allowed_latch + + +@pytest.mark.parametrize('standard_enabled', [False, True]) +@pytest.mark.parametrize('delay_ns', [10_000_000, 30_000_000, 30_000_001]) +def test_first_main_arms_when_ordinary_cruise_precedes_pulse(standard_enabled, delay_ns): + intent = FordCardIntent(None, AolSettings(True, 0.0, 0, 0, (0, 0, 0), (0, 0, 0))) + intent.update(state(available=False), now_ns=1_000_000_000, fault_active=True) + # Unit input for the readiness decision; the coupled regression obtains this + # decision from actual Card/SD publications and native receive hooks. + intent.main_idle_transition_qualified = True + edge = 1_010_000_000 + intent.update(state(available=True), now_ns=edge, fault_active=True, standard_enabled=standard_enabled) + assert intent.main_pending_ns == edge and not intent.allowed_latch + intent.update(state(available=True), now_ns=edge + delay_ns, fault_active=False, + standard_enabled=standard_enabled) + assert intent.allowed_latch == (delay_ns <= 30_000_000) + assert not intent.main_pending_ns + + +def startup_receipt(source_pause): + from openpilot.starpilot.tests import test_ford_reverse_intent as wire + cp, cs = wire.cp_state() + intent = FordCardIntent(cp, AolSettings(True, 0.0, 0, 0, (0, 0, 0), (0, 0, 0))) + 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 + if source_pause: + axis.sourceCarStateMonoTime = 0 + axis.faultReason = 'availabilityPause' + wire.change(sm, 'aolAxisState', off) + wire.change(sm, 'onroadEvents', lambda events: setattr(events[0], 'name', 'wrongCarMode')) + return intent, cp, cs, sm, native, wire.NOW + 10_000_000 + + +def test_first_main_qualifies_only_current_off_receipt(): + from openpilot.starpilot.tests import test_ford_reverse_intent as wire + intent, cp, cs, sm, native, now = startup_receipt(False) + 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 sm.logMonoTime == stamps + intent.update(cs.as_reader(), fault_active=True, now_ns=now) + assert intent.main_pending_ns == now and not intent.allowed_latch + # A genuine ongoing fault consumes the pending input, even after eligibility. + intent.update(cs.as_reader(), fault_active=True, now_ns=now + 10_000_000) + intent.update(cs.as_reader(), fault_active=False, now_ns=now + 20_000_000) + assert not intent.allowed_latch and not intent.main_pending_ns + + +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', +) + + +@pytest.mark.parametrize('source_pause', [False, True]) +@pytest.mark.parametrize('fault', STARTUP_REFUSALS) +def test_first_main_does_not_admit_faulted_or_unowned_receipt(source_pause, fault): + from openpilot.starpilot.tests import test_ford_reverse_intent as wire + intent, cp, cs, sm, native, now = startup_receipt(source_pause) + producer, panda_ready = wire.PRODUCER, True + if fault == 'stale': + now += 30_000_001 + elif fault == 'future': + now = wire.NOW + elif fault == 'deadline': + wire.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'validUntilMonoTime', wire.NOW + 30_000_001)) + elif fault == 'producer': + producer = 'other-card' + elif fault == 'session': + wire.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'faultSessionId', 'other-axis')) + elif fault == 'event-stamp': + wire.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'faultEventMonoTime', wire.NOW - 1)) + if not source_pause: + sm.logMonoTime['onroadEvents'] = wire.NOW + 1 + elif fault in ('wrong-event', 'critical', 'soft'): + def event_change(events): + events[0].name = 'controlsMismatch' + events[0].immediateDisable = fault == 'critical' + events[0].softDisable = fault == 'soft' + wire.change(sm, 'onroadEvents', event_change) + elif fault in ('can', 'timeout', 'permanent', 'temporary', 'acc', 'sensor', 'door', 'disengage'): + key = {'can': 'canValid', 'timeout': 'canTimeout', 'permanent': 'steerFaultPermanent', 'temporary': 'steerFaultTemporary', + 'acc': 'accFaulted', 'sensor': 'vehicleSensorsInvalid', 'door': 'doorOpen', 'disengage': 'steeringDisengage'}[fault] + setattr(cs, key, fault != 'can') + elif fault == 'gear': + cs.gearShifter = 'reverse' + elif fault in ('native-rx', 'heartbeat'): + 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 == 'panda-missing': + panda_ready = False + before = native + intent.observe_startup_readiness(sm, cs, native=native, panda_ready=panda_ready, now_ns=now, producer=producer) + assert not intent.main_idle_transition_qualified + assert native == before and not intent.allowed_latch + 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 + + +def test_source_zero_availability_fault_cannot_qualify_first_main(): + from openpilot.starpilot.tests import test_ford_reverse_intent as wire + intent, cp, cs, sm, native, now = startup_receipt(True) + intent.observe_startup_readiness(sm, cs, native=native, panda_ready=True, now_ns=now, producer=wire.PRODUCER) + assert not intent.main_idle_transition_qualified + 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 diff --git a/prebuilt.json b/prebuilt.json index 425a459309..89a3eb016f 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "265686c2707d037554f661eb5143f867e57f2d0d3fa9208cf9db33638e6fac47", "schema_version": 1} +{"protected_inventory_sha256": "3e02a8051673491682d545d1563dfa2c463d9496a9d4b9bf9e5e3cef4d3e6722", "schema_version": 1}