Preserve Ford AOL through startup and brake transitions

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:

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

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)

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)

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

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

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "abf414e89e892227c9c235b8dd5a7c8891726459b52617cd571428cfbafb3b38", "schema_version": 1}
+{"protected_inventory_sha256": "c11fbe2409cae4ba5c1fc311a5715d93588886e26625717f7bc2997a0557d680", "schema_version": 1}