Fix Ford CAN-FD cluster ownership and parked startup warnings

opendbc_repo/opendbc/car/ford/carcontroller.py

@@ -338,9 +338,11 @@ class CarController(CarControllerBase):
     ### ui ###
     # Acquire the display after the first active replacement, then retain it while
     # AOL stays active, including a manual-turn steering handoff (as on Dom).
-    aol_mache_ui = self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and qualified_aol(self.CP, marked_only=True)
-    ui_enabled = CC.latActive and (not aol_mache_ui or self.mache_ui_owned)
-    if not aol_mache_ui or ui_enabled:
+    aol_owned_ui = qualified_aol(self.CP, marked_only=True) and any(
+      s.safetyModel == structs.CarParams.SafetyModel.ford and (s.safetyParam & ~(1 | 128 | 256)) in (18, 66)
+      for s in self.CP.safetyConfigs)
+    ui_enabled = CC.latActive and (not aol_owned_ui or self.mache_ui_owned)
+    if not aol_owned_ui or ui_enabled:
       send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != ui_enabled) or (self.steer_alert_last != steer_alert)
       # send lkas ui msg at 1Hz or if ui state changes
       if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:

opendbc_repo/opendbc/safety/modes/ford_aol.h

@@ -13,6 +13,11 @@ static bool ford_aol_button_previous = false;
 static bool ford_aol_session = false;
 static bool ford_aol_request_seen = false;
 static uint16_t ford_aol_param = 0U;
+
+static bool ford_aol_retained_display_profile(void) {
+  const uint16_t base = ford_aol_param & 0xFFFEU;
+  return (base == 18U) || (base == 66U);
+}
 static bool ford_aol_drive = false;
 static uint8_t ford_aol_eps = 0U;
 static bool ford_aol_lka_ready = false;
@@ -62,7 +67,7 @@ static void ford_aol_arm(void) {
 
 static void ford_aol_host_request(uint8_t mask) {
   ford_aol_request_seen |= mask != 0U;
-  if (((ford_aol_param & 0xFFFEU) == 18U) && ((mask & 1U) == 0U)) {
+  if (ford_aol_retained_display_profile() && ((mask & 1U) == 0U)) {
     ford_aol_owned &= FORD_AOL_STEERING_OWNED;
   }
 }
@@ -176,7 +181,7 @@ static void ford_aol_rx(const CANPacket_t *msg) {
     }
     // A transient physical veto revokes only display ownership, even when a
     // healthy packet follows in the same RX batch. AOL intent is retained.
-    if (((ford_aol_param & 0xFFFEU) == 18U) &&
+    if (ford_aol_retained_display_profile() &&
         (!ford_aol_main_available || !ford_aol_drive || (ford_aol_eps != 0U) || !ford_aol_lka_ready)) {
       ford_aol_owned &= FORD_AOL_STEERING_OWNED;
     }
@@ -238,8 +243,7 @@ static void ford_aol_tx(const CANPacket_t *msg, bool accepted) {
     const bool active = (msg->addr == 0x3CAU) ? (((msg->data[0] >> 5U) == 2U) || ((msg->data[0] >> 5U) == 4U)) :
                        ((msg->addr == 0x3D3U) ? (((msg->data[4] >> 2U) & 7U) != 0U) :
                                                (((msg->data[0] >> 4U) & 7U) != 0U));
-    const bool mache = (ford_aol_param & 0xFFFEU) == 18U;
-    const bool retained_display = mache && ((ford_aol_owned & FORD_AOL_DISPLAY_OWNED) != 0U) &&
+    const bool retained_display = ford_aol_retained_display_profile() && ((ford_aol_owned & FORD_AOL_DISPLAY_OWNED) != 0U) &&
       (safety_get_ts_elapsed(microsecond_timer_get(), ford_aol_accepted_ts) <= AOL_HOST_REQUEST_TIMEOUT_US);
     const uint8_t permission = (accepted && (active || retained_display)) ? ford_aol_permission_mask() : 0U;
     const bool steering_owned = accepted && active && (permission != 0U) && !relay_malfunction;
@@ -253,18 +257,18 @@ static bool ford_aol_fwd(int bus, int addr) {
   bool blocked = false;
   if (ford_aol_enabled && (bus == 2) && (ford_aol_owned != 0U) && (ford_aol_permission_mask() != 0U) &&
       (safety_get_ts_elapsed(microsecond_timer_get(), ford_aol_accepted_ts) <= AOL_HOST_REQUEST_TIMEOUT_US)) {
-    const bool display_permitted = ((ford_aol_param & 0xFFFEU) != 18U) || ((ford_aol_permission_mask() & 1U) != 0U);
+    const bool display_permitted = !ford_aol_retained_display_profile() || ((ford_aol_permission_mask() & 1U) != 0U);
     blocked = (((ford_aol_owned & FORD_AOL_STEERING_OWNED) != 0U) && (addr == (int)ford_aol_replacement())) ||
       (((ford_aol_owned & FORD_AOL_DISPLAY_OWNED) != 0U) && display_permitted && ((addr == 0x3D8) || (addr == 0x18A)));
   }
   return blocked;
 }
 
-// An unowned Mach-E display must remain stock, including a rejected first
+// An unowned extended CAN-FD display must remain stock, including a rejected first
 // steering command. This cannot grant steering or longitudinal permission.
 static bool ford_aol_display_tx_allowed(void) {
   bool allowed = true;
-  if (ford_aol_enabled && ((ford_aol_param & 0xFFFEU) == 18U)) {
+  if (ford_aol_enabled && ford_aol_retained_display_profile()) {
     allowed = ((ford_aol_owned & FORD_AOL_DISPLAY_OWNED) != 0U) &&
       ((ford_aol_permission_mask() & 1U) != 0U) && !relay_malfunction &&
       (safety_get_ts_elapsed(microsecond_timer_get(), ford_aol_accepted_ts) <= AOL_HOST_REQUEST_TIMEOUT_US);

opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py

@@ -8,7 +8,7 @@ from opendbc.safety.tests.libsafety import libsafety_py
 from opendbc.safety.tests import test_ford_aol
 
 
-@pytest.fixture(params=(18, 19, 274, 275))
+@pytest.fixture(params=(18, 19, 274, 275, 66, 67, 322, 323))
 def owner(request):
   native = test_ford_aol.TestFordAolDriverIntent()
   native.setUp()
@@ -113,7 +113,7 @@ def test_display_drops_on_actual_authority_loss_and_does_not_reacquire_inactive(
   stock_display(owner, relay=loss == 'relay')
 
 
-@pytest.mark.parametrize('word', (8, 66))
+@pytest.mark.parametrize('word', (8,))
 def test_other_ford_owners_keep_original_inactive_forwarding(word):
   native = test_ford_aol.TestFordAolDriverIntent()
   native.setUp()

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -2081,6 +2081,7 @@ class SelfdriveD:
       if ET.WARNING not in self.state_machine.current_alert_types:
         self.state_machine.current_alert_types.append(ET.WARNING)
     if (getattr(self, 'aol_fault_reason', 'none') == 'availabilityPause' and
+        (self.CP.brand != 'ford' or self.enabled or intent is not None and intent.allowedLatch) and
         ET.WARNING not in self.state_machine.current_alert_types):
       self.state_machine.current_alert_types.append(ET.WARNING)
     axis_published = bool(not REPLAY and self.aol_replay and self.axis_transport_required)

openpilot/starpilot/tests/ford_lifecycle_helpers.py

@@ -264,7 +264,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
       route45_trace = []
 
       def step(*, engaged=False, brake=False, reverse=False, warmup=False, fault=None, main_off=False,
-               availability_transition=False, steering=False, cancel=False):
+               availability_transition=False, steering=False, cancel=False, park=False):
         nonlocal steps, warmup_waits, qualified_samples, active_lat_frames, active_long_frames, nonzero_after_brake, brake_seen
         brake_seen |= brake
         steps += 1
@@ -318,7 +318,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
           native.pump(
             1,
             main=0 if main_off else 5 if engaged else 3,
-            gear=1 if reverse else 3,
+            gear=0 if park else 1 if reverse else 3,
             mask=request,
             heartbeat=heartbeat and fault != 'heartbeat',
             speed=(3.1 + .12 * ((steps // 2) % 12) / 11 if getattr(monkeypatch, "ford_speed_window", False) else 15.0),
@@ -497,7 +497,9 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
             'display', 'route24', 'route24-mixed-eps', 'route2a', 'route2a-main', 'route2a-main-fault'):
           ui = [f for f in published['sendcan'].sendcan if f.address in (0x3D8, 0x18A)]
           owns = all(safety.safety_fwd_hook(2, address) == -1 for address in (0x3D8, 0x18A))
-          assert host.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not ui or owns, (
+          assert host.CP.carFingerprint not in (CAR.FORD_MUSTANG_MACH_E_MK1, CAR.FORD_F_150_LIGHTNING_MK1,
+                                                  CAR.FORD_ESCAPE_MK4_5, CAR.FORD_EXPEDITION_MK4,
+                                                  CAR.FORD_F_150_MK14, CAR.FORD_RANGER_MK2) or not ui or owns, (
             'Host injected cluster UI while native still forwarded stock UI',
             steps,
             bool(command.latActive),
@@ -536,6 +538,29 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
       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 == 'startup-warning':
+        if getattr(monkeypatch, 'ford_startup_armed', False):
+          for _ in range(40):
+            state, axis, sd_state, command = step(engaged=True)
+          assert host.aol_card_intent.allowed_latch and command.latActive
+          warning_seen = False
+          warning_types = set()
+          for _ in range(60):
+            state, axis, sd_state, command = step(fault='temporary-eps')
+            warning_types.add(sd_state.alertType)
+            warning_seen |= sd_state.alertType in ('steerTempUnavailableSilent/warning', 'steerTempUnavailable/softDisable')
+          assert state.steerFaultTemporary and host.aol_card_intent.allowed_latch
+          assert 'warning' in sd.state_machine.current_alert_types
+          assert warning_seen and not command.latActive and not command.longActive and joined.last_permission == 0, (
+            warning_types, command.latActive, command.longActive, joined.last_permission)
+        else:
+          for _ in range(30):
+            state, axis, sd_state, command = step(main_off=True, park=True, fault='temporary-eps')
+          assert str(state.gearShifter) == 'park' and not state.cruiseState.available
+          assert state.steerFaultTemporary and not host.aol_card_intent.allowed_latch
+          assert not command.enabled and not command.latActive and not command.longActive
+          assert sd_state.alertType != 'steerTempUnavailableSilent/warning', 'Unarmed parked startup shows steering warning'
+        return
       if scenario == 'route22':
         assert host.CP.openpilotLongitudinalControl
         latch_trace = []

openpilot/starpilot/tests/test_ford_bluepilot_aol.py

@@ -228,3 +228,23 @@ def test_route22_native_fault_still_withdraws_aol(ford_joined, monkeypatch, faul
   monkeypatch.ford_nonzero = True
   monkeypatch.ford_route22_final_fault = fault
   run_actual_producer(ford_joined, monkeypatch, scenario='route22')
+
+
+@pytest.mark.parametrize('identity', [(CAR.FORD_F_150_LIGHTNING_MK1, 66), (CAR.FORD_ESCAPE_MK4_5, 66),
+                                     (CAR.FORD_EXPEDITION_MK4, 66), (CAR.FORD_F_150_MK14, 66), (CAR.FORD_RANGER_MK2, 66)])
+@pytest.mark.parametrize('alpha', [False, True])
+@pytest.mark.parametrize('hands_free', [False, True])
+@pytest.mark.parametrize('topology', ['automatic'])
+def test_lightning_cluster_startup_and_aol_recovery(ford_joined, monkeypatch):
+  monkeypatch.ford_nonzero = True
+  run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity')
+
+
+@pytest.mark.parametrize('identity', [(CAR.FORD_F_150_LIGHTNING_MK1, 66)])
+@pytest.mark.parametrize('alpha', [False])
+@pytest.mark.parametrize('hands_free', [False, True])
+@pytest.mark.parametrize('topology', ['automatic'])
+@pytest.mark.parametrize('armed', [False, True])
+def test_lightning_unarmed_startup_does_not_show_armed_eps_warning(ford_joined, monkeypatch, armed):
+  monkeypatch.ford_startup_armed = armed
+  run_actual_producer(ford_joined, monkeypatch, scenario='startup-warning')

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "ac7c85b608097912bf473e5e66eaf4006cd3642ba874d7b5a5a1f978faeb8cf9", "schema_version": 1}
+{"protected_inventory_sha256": "b6dbedd7e09e9c0ccb699e58b2d3626bd3883bc6c44e998e239790056489d51c", "schema_version": 1}