Fix classic Ford dashboard handoff during lateral pause

openpilot/starpilot/car/ford/bluepilot.py

@@ -18,6 +18,7 @@ from opendbc.bluepilot_lateral.core.lateral_angle_ext import LateralAngleExt
 from opendbc.bluepilot_lateral.core.lateral_inputs import lateral_inputs_complete
 from opendbc.bluepilot_lateral.can.fordcan_ext import lat_ctl2_checksum, create_lat_ctl2_msg
 from opendbc.car.ford.values import CarControllerParams, FordFlags
+from opendbc.car.ford.aol import qualified as qualified_aol
 from opendbc.car import structs
 from opendbc.car.vehicle_model import calc_slip_factor
 from opendbc.bluepilot_lateral.core.carcontroller_ext import pinion_vehicle_model
@@ -43,6 +44,17 @@ HOST_SETTINGS = {
 SETTINGS_REFRESH_FRAMES = 300  # 3 s of card's 100 Hz loop
 
 
+def quiet_cluster_display(address, data):
+  if len(data) != 8:
+    return False
+  if address == 0x18A:
+    return ((data[5] >> 2) & 7) <= 1 and not (
+      data[0] & 0xE0 or data[1] & 1 or data[3] & 0xF0 or data[4] & 0xC0 or data[5] & 0x80 or data[6] & 0x3F)
+  if address == 0x3D8:
+    return data[6] >> 3 in (0, 30) and not (data[4] & 0x0D or data[5] & 0x18 or data[6] & 6)
+  return False
+
+
 def startup_params(params, *, enabled: bool) -> dict[str, str]:
   """The startup settings init_ford_safety_param_starpilot reads. BluePilot lateral stays off without the openpilot
   toggle, in Safe Mode, or when a startup setting can't be read. Legacy stock selection is retired."""
@@ -234,6 +246,9 @@ class FordBluePilotController(BluePilotLateral):
     return self._constrain_curvature(CC, CS, actuators, apply_curvature_last, packets, applied)
 
   def order_can_sends(self, frames):
+    if not self.CP.flags & (FordFlags.CANFD | FordFlags.NEW_PORT) and qualified_aol(self.CP, marked_only=True) and not self.lateral_active:
+      # Inactive classic steering returns display ownership to the camera.
+      frames = [(a, d, b) for a, d, b in frames if b != self.CAN.main or not quiet_cluster_display(a, d)]
     if (not self.starpilot_selected or self.curv.primary_lateral_control != PrimaryLateralControl.angle or
         self.frame % CarControllerParams.STEER_STEP or self.frame % CarControllerParams.LKA_STEP):
       return frames

openpilot/starpilot/tests/test_ford_starpilot_ordering.py

@@ -17,6 +17,7 @@ from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import init_ford_saf
 from opendbc.bluepilot_lateral.core.lateral_curv_ext import PrimaryLateralControl
 from opendbc.safety.tests.libsafety import libsafety_py
 from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
+from openpilot.starpilot.car.ford.controller_selection import FordController
 from openpilot.starpilot.controller_extensions import order_controller_can
 from openpilot.starpilot.tests.test_ford_native_pinion_guard import LearnedGeometryInputs
 
@@ -89,6 +90,27 @@ def native_run(identity, direction, *, ordered, invalid_shadow=False, withdraw=F
     safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
 
 
+@pytest.mark.parametrize('message,signal,value', [
+  ('ACCDATA_3', 'FcwVisblWarn_B_Rq', 1), ('ACCDATA_3', 'FcwAudioWarn_B_Rq', 1),
+  ('ACCDATA_3', 'TjaWarn_D_Rq', 3), ('ACCDATA_3', 'TjaMsgTxt_D_Dsply', 1),
+  ('ACCDATA_3', 'AccMsgTxt_D2_Rq', 10), ('ACCDATA_3', 'AccWarn_D_Dsply', 1),
+  ('ACCDATA_3', 'FcwMsgTxt_D_Rq', 2), ('ACCDATA_3', 'Tja_D_Stat', 5),
+  ('IPMA_Data', 'LaHandsOff_D_Dsply', 1), ('IPMA_Data', 'LaDenyStats_B_Dsply', 1),
+  ('IPMA_Data', 'CamraStats_D_Dsply', 3), ('IPMA_Data', 'DasWarn_D_Dsply', 2),
+  ('IPMA_Data', 'LaActvStats_D_Dsply', 3),
+])
+def test_classic_pause_preserves_dashboard_warnings(message, signal, value):
+  fp = gen_empty_fingerprint()
+  fp[0][0x5A] = 8
+  cp = CarInterface.get_params(CAR.FORD_EXPLORER_MK6, fp, [], True, False, False)
+  cp.alternativeExperience = 32
+  controller = cast(Any, FordBluePilotController.__new__(FordBluePilotController))
+  controller.CP, controller.CAN = cp, NS(main=0)
+  controller.lateral_active = controller.starpilot_selected = False
+  packet = CANPacker('ford_lincoln_base_pt').make_can_msg(message, 0, {signal: value})
+  assert controller.order_can_sends([packet]) == [packet]
+
+
 @pytest.mark.parametrize('identity', [CAR.FORD_MUSTANG_MACH_E_MK1, CAR.FORD_EXPLORER_MK6, CAR.FORD_F_150_MK14])
 @pytest.mark.parametrize('direction', [-1., 1.])
 def test_current_shadow_prevents_coincident_native_refusal_without_extra_packets(identity, direction):
@@ -115,7 +137,7 @@ def test_ordering_preserves_buses_payloads_and_noncoincident_cadences(main, canf
   controller.starpilot_selected = True
   controller.curv = NS(primary_lateral_control=PrimaryLateralControl.angle)
   controller.CAN = NS(main=main)
-  controller.CP = NS(flags=FordFlags.CANFD if canfd else 0)
+  controller.CP = structs.CarParams(flags=int(FordFlags.CANFD if canfd else 0)).as_reader()
   lmc = (0x3D6 if canfd else 0x3D3, bytes(8), main)
   lka = (0x3CA, bytes(8), main)
   unrelated = (0x165, b'12345678', main + 2)
@@ -148,3 +170,58 @@ def test_steering_ordering_is_required_in_both_native_ci_builds():
   path = 'openpilot/starpilot/tests/test_ford_starpilot_ordering.py'
   assert targets_for('safety-debug').count(path) == 1
   assert targets_for('safety-release').count(path) == 1
+
+
+@pytest.mark.parametrize('identity', [CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_ESCAPE_MK4, CAR.FORD_EXPLORER_MK6,
+                                      CAR.FORD_FOCUS_MK4, CAR.FORD_MAVERICK_MK1,
+                                      CAR.FORD_MUSTANG_MACH_E_MK1, CAR.FORD_F_150_MK14])
+@pytest.mark.parametrize('alpha', [False, True])
+@pytest.mark.parametrize('choice', list(FordController))
+@pytest.mark.parametrize('aol', [False, True])
+def test_classic_aol_pause_returns_dashboard_to_camera_without_changing_actuators(identity, alpha, choice, aol):
+  fp = gen_empty_fingerprint()
+  fp[0][0x5A] = 8
+  fp[2].update({0x3D3: 8, 0x186: 8})
+  cp = CarInterface.get_params(identity, fp, [], alpha, False, False)
+  cp.alternativeExperience = 32 if aol else 0
+  init_ford_safety_param_starpilot(cp, STARTUP['on+pinion'])
+  controller = CarController(DBC[cp.carFingerprint], cp)
+  inputs = LearnedGeometryInputs(cp, 1., 0.)
+  inputs.controller_choice = choice
+  inputs.settings['FordPrefLateralControl'] = b'0' if choice == FordController.BLUEPILOT_CURVATURE else b'1'
+  controller.bluepilot_inputs = inputs
+  controller.bp = FordBluePilotController(cp, controller.packer, controller.CAN, inputs)
+  ci = NS(CP=cp, CC=controller)
+  state = CarState(cp)
+  state.update(state.get_can_parsers(cp))
+  safety = libsafety_py.libsafety
+  safety.init_tests()
+  safety.set_alternative_experience(cp.alternativeExperience)
+  assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam) == 0
+  try:
+    for frame in range(11):
+      active = frame < 5 or frame >= 10
+      cs = structs.CarState(vEgo=10. if active else 0., vEgoRaw=10. if active else 0., standstill=not active,
+                            canValid=True, gearShifter=structs.CarState.GearShifter.drive)
+      cs.cruiseState.available = cs.cruiseState.enabled = True
+      cs.cruiseState.standstill = not active
+      state.out = cs.as_reader()
+      cc = structs.CarControl(enabled=True, latActive=active, longActive=alpha)
+      cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.stopping
+      _, original = controller.update(cc.as_reader(), state, (1_000_000 + frame * 10_000) * 1000)
+      actual = order_controller_can(ci, original)
+      ui = [(a, d, b) for a, d, b in original if a in (0x18A, 0x3D8) and b == controller.CAN.main]
+      if active or not aol or cp.flags & FordFlags.CANFD:
+        assert Counter(actual) == Counter(original)
+      else:
+        assert Counter(actual) == Counter(original) - Counter(ui)
+        if frame == 5:
+          assert {a for a, _, _ in ui} == {0x18A, 0x3D8}
+          assert any(a == 0x3D3 for a, _, _ in actual)
+          # The native camera path stays available; no authority is seeded by this routing test.
+          for address in (0x18A, 0x3D8):
+            assert safety.safety_fwd_hook(2, address) == 0
+        if frame == 6:
+          assert any(a == 0x186 for a, _, _ in actual) == alpha
+  finally:
+    safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "b93d12cdd85e78c16f978b68bd56777fab78fa2a3a94beb99a7ed0a2a635466f", "schema_version": 1}
+{"protected_inventory_sha256": "11d2c1f5dc49e617d15a743babaf1659da1179e9771734e45ed74491c64a03f0", "schema_version": 1}