Arm Ford AOL when cruise engages before the automatic MAIN pulse

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)
 
 

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

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')

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

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "265686c2707d037554f661eb5143f867e57f2d0d3fa9208cf9db33638e6fac47", "schema_version": 1}
+{"protected_inventory_sha256": "3e02a8051673491682d545d1563dfa2c463d9496a9d4b9bf9e5e3cef4d3e6722", "schema_version": 1}