Order StarPilot Ford metadata before same-cycle steering

openpilot/selfdrive/car/card.py

@@ -92,7 +92,7 @@ from openpilot.starpilot.feature_runtime import (requested as feature_requested,
 from openpilot.starpilot.saved_source import read_saved
 from openpilot.starpilot.vehicle_selection import startup_candidate
 from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
-from openpilot.starpilot.controller_extensions import configure_controller, observe_controller_can
+from openpilot.starpilot.controller_extensions import configure_controller, observe_controller_can, order_controller_can
 from openpilot.starpilot.vehicle_startup import VehicleStartupOwner
 from openpilot.starpilot.controllers.wheel_actions import WheelPublisher, key_for
 from openpilot.starpilot.controllers.cruise_action import CruiseActionConsumer, eligible as cruise_action_eligible
@@ -1472,6 +1472,8 @@ class Car:
       self._clear_learning_evidence()
 
   def publish_sendcan(self, frames, valid=True):
+    if valid:
+      frames = order_controller_can(getattr(self, 'CI', None), frames)
     steering_authority = getattr(getattr(getattr(self, 'CI', None), 'CS', None), 'steering_authority', None)
     monitored_steering = (getattr(getattr(self, 'CP', None), 'brand', None) == 'gm' and
                           steering_authority is not None and steering_authority.enabled)
@@ -1902,10 +1904,8 @@ class Car:
       time.sleep(0.1)
 
   def card_thread(self):
-    self._cycle_timing = None
-    if os.getenv('CARD_TIMING') == '1':
-      from openpilot.starpilot.diagnostics.card_timing import CardTiming
-      self._cycle_timing = CardTiming(cloudlog.event)
+    from openpilot.starpilot.diagnostics.card_timing import CardTiming
+    self._cycle_timing = CardTiming(cloudlog.event)
     self.configure_conditional_settings_worker()
     e = threading.Event()
     t = threading.Thread(target=self.params_thread, args=(e, ))

openpilot/starpilot/car/ford/bluepilot.py

@@ -3,6 +3,7 @@
 from collections import deque
 from dataclasses import replace
 import math
+from typing import TYPE_CHECKING, cast
 import numpy as np
 
 import openpilot.cereal.messaging as messaging
@@ -23,6 +24,10 @@ from opendbc.bluepilot_lateral.core.carcontroller_ext import pinion_vehicle_mode
 from opendbc.car.lateral import ISO_LATERAL_ACCEL
 from openpilot.starpilot.car.ford.controller_selection import FordController, read_controller
 
+if TYPE_CHECKING:
+  from openpilot.starpilot.car.ford.starpilot_controller import StarPilotFordController
+
+
 STARTUP_KEYS = ("FordPrefSteerAngleCurvature",)
 # Global StarPilot lane settings are the only lane-positioning owner.
 HOST_SETTINGS = {
@@ -203,7 +208,8 @@ class FordBluePilotController(BluePilotLateral):
     if self.starpilot_selected and (not self.starpilot_frame_valid or not self.angle.inputs_valid(CS, actuators)):
       CC = structs.CarControl.new_message(latActive=False)
     if self.starpilot_selected:
-      self.angle.native_curvature_bounds = None
+      angle = cast("StarPilotFordController", self.angle)
+      angle.native_curvature_bounds = None
       if self.native_pinion_model is not None and self.measured_seen and CS.out.vEgoRaw > 10.:
         # Pinion native limits use 150 CAN curvature units, without learned geometry.
         error = 150 * .00002
@@ -211,10 +217,29 @@ class FordBluePilotController(BluePilotLateral):
         high = min(-min(self.measured_history) * .00002 + error, measured + CarControllerParams.CURVATURE_ERROR)
         # Tighten the host envelope only where both measurements agree on a command.
         if low <= high:
-          self.angle.native_curvature_bounds = (low, high)
+          angle.native_curvature_bounds = (low, high)
     packets, applied = super().step_lateral(CC, CS, actuators, apply_curvature_last)
     return self._constrain_curvature(CC, CS, actuators, apply_curvature_last, packets, applied)
 
+  def order_can_sends(self, frames):
+    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
+    lka, lmc = [], []
+    steering_address = 0x3D6 if self.CP.flags & FordFlags.CANFD else 0x3D3
+    for index, (address, data, bus) in enumerate(frames):
+      if bus == self.CAN.main and len(data) == 8:
+        if address == 0x3CA:
+          lka.append(index)
+        elif address == steering_address:
+          lmc.append(index)
+    if len(lka) != 1 or len(lmc) != 1 or lka[0] < lmc[0]:
+      return frames
+    # Both were computed this tick; Panda must see the current shadow before its steering command.
+    reordered = list(frames)
+    reordered.insert(lmc[0], reordered.pop(lka[0]))
+    return reordered
+
   def _constrain_curvature(self, CC, CS, actuators, apply_curvature_last, packets, applied):
     state = CS.out
     if (not CC.latActive or (not self.CP.flags & FordFlags.CANFD and not self.starpilot_selected) or

openpilot/starpilot/car/ford/starpilot_controller.py

@@ -103,7 +103,7 @@ class StarPilotFordController:
   def __init__(self, CP, inputs, shared):
     self.CP = CP
     self.conditioner = CurvatureConditioner()
-    self.native_curvature_bounds = None
+    self.native_curvature_bounds: tuple[float, float] | None = None
     # StarPilot uses the platform defaults, not hidden BluePilot user adjustment knobs.
     for attr, key in ANGLE_SETTINGS:
       setattr(self, attr, default(key))

openpilot/starpilot/controller_extensions.py

@@ -180,3 +180,12 @@ def observe_controller_can(CI, can_list):
     from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
     if isinstance(getattr(controller, 'bp', None), FordBluePilotController):
       controller.bp.observe_can(can_list)
+
+
+def order_controller_can(CI, frames):
+  controller = getattr(CI, "CC", None)
+  if controller is not None and getattr(getattr(CI, 'CP', None), 'brand', None) == 'ford':
+    from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
+    if isinstance(getattr(controller, 'bp', None), FordBluePilotController):
+      return controller.bp.order_can_sends(frames)
+  return frames

openpilot/starpilot/diagnostics/card_timing.py

@@ -1,4 +1,4 @@
-"""Opt-in, bounded Card phase diagnostics; never changes control policy or scheduling."""
+"""Temporary bounded Card phase diagnostics; never changes control policy or scheduling."""
 
 import os
 import resource

openpilot/starpilot/tests/test_card_timing.py

@@ -138,9 +138,8 @@ def test_actual_card_step_preserves_action_order_with_optional_marks():
   assert trace == ['state_update', 'publish', 'control', 'radar_update', 'radar_publish']
 
 
-@pytest.mark.parametrize('enabled', [False, True])
 @pytest.mark.parametrize('fail_startup', [False, True])
-def test_actual_card_loop_keeps_control_errors_and_cleans_probe(enabled, fail_startup):
+def test_actual_card_loop_keeps_control_errors_and_cleans_probe(fail_startup):
   from openpilot.selfdrive.car.card import Car
 
   class StopLoop(Exception):
@@ -151,12 +150,32 @@ def test_actual_card_loop_keeps_control_errors_and_cleans_probe(enabled, fail_st
   fake = NS(configure_conditional_settings_worker=Mock(), params_thread=Mock(),
             vehicle_startup=NS(check=startup), step=Mock(), rk=NS(monitor_time=Mock(side_effect=StopLoop('monitor'))),
             ioniq6_keepalive=None)
-  with patch.dict('os.environ', {'CARD_TIMING': '1' if enabled else '0'}), \
+  with patch.dict('os.environ', {}, clear=True), \
        patch('threading.Thread'), patch('openpilot.starpilot.diagnostics.card_timing.CardTiming', return_value=probe) as factory:
     with pytest.raises(StopLoop, match='startup' if fail_startup else 'monitor'):
       Car.card_thread(cast(Car, fake))
-  assert factory.call_count == int(enabled)
+  assert factory.call_count == 1
   assert fake.step.call_count == int(not fail_startup)
-  assert probe.close.call_count == int(enabled)
-  assert probe.begin.call_count == int(enabled)
-  assert probe.finish.call_count == int(enabled and not fail_startup)
+  assert probe.close.call_count == 1
+  assert probe.begin.call_count == 1
+  assert probe.finish.call_count == int(not fail_startup)
+
+
+def test_actual_card_loop_removes_expired_capture_without_restarting_it():
+  from openpilot.selfdrive.car.card import Car
+
+  class StopLoop(Exception):
+    pass
+
+  sample = Mock(return_value=Sample(CAPTURE_NS, 0, None, None, None))
+  probe = CardTiming(Mock(), sample)
+  probe.deadline = CAPTURE_NS
+  fake = NS(configure_conditional_settings_worker=Mock(), params_thread=Mock(), vehicle_startup=NS(check=Mock()),
+            step=Mock(), rk=NS(monitor_time=Mock(side_effect=[None, StopLoop()])), ioniq6_keepalive=None)
+  with patch('threading.Thread'), patch('openpilot.starpilot.diagnostics.card_timing.CardTiming', return_value=probe):
+    with pytest.raises(StopLoop):
+      Car.card_thread(cast(Car, fake))
+  assert fake._cycle_timing is None
+  assert not probe.enabled and not probe.samples and probe.fd is None
+  assert fake.step.call_count == 2
+  sample.assert_called_once()

openpilot/starpilot/tests/test_ford_native_pinion_guard.py

@@ -95,7 +95,9 @@ def test_pinion_guard_preserves_host_and_native_bounds(car, speed, sign, ratio,
       assert controller.bp.angle.native_curvature_bounds is None
     else:
       assert not blocked, blocked
-      low, high = controller.bp.angle.native_curvature_bounds
+      bounds = controller.bp.angle.native_curvature_bounds
+      assert bounds is not None
+      low, high = bounds
       assert low <= controller.bp.angle.bp_kappa_cmd <= high
       measured = controller.bp.inputs.get_current_curvature(state)
       assert abs(controller.bp.angle.bp_kappa_cmd - measured) <= CarControllerParams.CURVATURE_ERROR + 1e-12

openpilot/starpilot/tests/test_ford_starpilot_ordering.py

@@ -0,0 +1,150 @@
+"""Current-cycle StarPilot metadata precedes steering without changing CAN traffic."""
+
+from collections import Counter
+from types import SimpleNamespace as NS
+from typing import Any, cast
+
+import pytest
+
+from opendbc.can import CANPacker
+from opendbc.car import gen_empty_fingerprint, structs
+from opendbc.car.ford.carcontroller import CarController
+from opendbc.car.ford.carstate import CarState
+from opendbc.car.ford.interface import CarInterface
+from opendbc.car.ford.tests.test_bluepilot_lateral import STARTUP
+from opendbc.car.ford.values import CAR, DBC, FordFlags
+from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import init_ford_safety_param_starpilot
+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.controller_extensions import order_controller_can
+from openpilot.starpilot.tests.test_ford_native_pinion_guard import LearnedGeometryInputs
+
+
+def native_run(identity, direction, *, ordered, invalid_shadow=False, withdraw=False):
+  fp = gen_empty_fingerprint()
+  fp[0][0x5A] = 8
+  fp[2].update({0x3D6: 8, 0x186: 8})
+  cp = CarInterface.get_params(identity, fp, [], False, False, False)
+  init_ford_safety_param_starpilot(cp, STARTUP['on+pinion'])
+  controller = CarController(DBC[cp.carFingerprint], cp)
+  inputs = LearnedGeometryInputs(cp, .85, -direction * .04)
+  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, packer = libsafety_py.libsafety, CANPacker('ford_lincoln_base_pt')
+  safety.init_tests()
+  safety.set_alternative_experience(0)
+  assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam) == 0
+  # Standalone native diagnostic; the actual factory/authority lifecycle is tested separately.
+  safety.set_controls_allowed(True)
+  blocked, traffic, changes = [], [], []
+  try:
+    for frame in range(45):
+      safety.set_timer(1_000_000 + frame * 10_000)
+      received = []
+
+      def rx(name, values, received=received):
+        address, data, bus = packer.make_can_msg(name, 0, values)
+        assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
+        received.append((address, data, bus))
+
+      if frame % 2 == 0:
+        rx('BrakeSysFeatures', {'Veh_V_ActlBrk': 108., 'VehVActlBrk_D_Qf': 3, 'VehVActlBrk_No_Cnt': frame // 2 % 16})
+        rx('EngVehicleSpThrottle2', {'Veh_V_ActlEng': 108., 'VehVActlEng_D_Qf': 3})
+        rx('DesiredTorqBrk', {'VehStop_D_Stat': 0})
+      rx('Yaw_Data_FD1', {'VehYaw_W_Actl': 0., 'VehYawWActl_D_Qf': 3, 'VehRollYaw_No_Cnt': frame % 256})
+      rx('EngVehicleSpThrottle', {'ApedPos_Pc_ActlArb': 0})
+      if frame % 10 == 0:
+        rx('EngBrakeData', {'BpedDrvAppl_D_Actl': 1, 'CcStat_D_Actl': 5})
+        rx('Steering_Data_FD1', {})
+      angle = direction * (30. - .5 * max(0, min(frame - 24, 6)))
+      rx('SteeringPinion_Data', {'StePinComp_An_Est': angle, 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': frame % 16})
+      controller.bp.observe_can([(0, received)])
+      cs = structs.CarState(vEgo=30., vEgoRaw=30., steeringAngleDeg=angle, canValid=True,
+                            gearShifter=structs.CarState.GearShifter.drive)
+      cs.cruiseState.available = cs.cruiseState.enabled = True
+      state.out = cs.as_reader()
+      cc = structs.CarControl(enabled=True, latActive=True)
+      cc.actuators.curvature = -direction * .02 if 25 <= frame < 30 else 0.
+      _, original = controller.update(cc.as_reader(), state, (1_000_000 + frame * 10_000) * 1000)
+      packets = order_controller_can(ci, original) if ordered else original
+      assert Counter(packets) == Counter(original)
+      if packets != original:
+        changes.append(frame)
+      traffic.extend((frame, address, bus) for address, _, bus in packets)
+      if frame == 30 and invalid_shadow:
+        packets = [(a, d[:5] + b'\x7f\xff' + d[7:] if a == 0x3CA else d, b) for a, d, b in packets]
+      if frame == 30 and withdraw:
+        safety.set_controls_allowed(False)
+      for address, data, bus in packets:
+        if address in (0x3CA, 0x3D3, 0x3D6) and not safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)):
+          blocked.append((frame, address))
+      if frame == 30 and withdraw:
+        assert not safety.get_controls_allowed()
+    return blocked, traffic, changes, 0x3D6 if cp.flags & FordFlags.CANFD else 0x3D3
+  finally:
+    safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
+
+
+@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):
+  previous, old_traffic, _, address = native_run(identity, direction, ordered=False)
+  current, traffic, changes, current_address = native_run(identity, direction, ordered=True)
+  assert address == current_address
+  assert previous == [(30, address)]
+  assert not current, current
+  assert Counter(traffic) == Counter(old_traffic)
+  assert changes and all(frame % 15 == 0 for frame in changes)
+
+
+@pytest.mark.parametrize('negative', ['shadow', 'permission'])
+def test_ordering_cannot_make_invalid_metadata_or_denied_authority_steer(negative):
+  blocked, _, _, address = native_run(CAR.FORD_MUSTANG_MACH_E_MK1, 1., ordered=True,
+                             invalid_shadow=negative == 'shadow', withdraw=negative == 'permission')
+  assert (30, address) in blocked
+
+
+@pytest.mark.parametrize('main', [0, 4])
+@pytest.mark.parametrize('canfd', [False, True])
+def test_ordering_preserves_buses_payloads_and_noncoincident_cadences(main, canfd):
+  controller = cast(Any, FordBluePilotController.__new__(FordBluePilotController))
+  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)
+  lmc = (0x3D6 if canfd else 0x3D3, bytes(8), main)
+  lka = (0x3CA, bytes(8), main)
+  unrelated = (0x165, b'12345678', main + 2)
+  for frame in range(15):
+    controller.frame = frame
+    packets = [unrelated]
+    if frame % 5 == 0:
+      packets.append(lmc)
+    if frame % 3 == 0:
+      packets.append(lka)
+    result = controller.order_can_sends(packets)
+    assert Counter(result) == Counter(packets)
+    assert result[0] == unrelated
+    if frame == 0:
+      assert result == [unrelated, lka, lmc]
+    else:
+      assert result is packets
+  controller.frame = 0
+  for packets in ([lmc, (lka[0], lka[1], main + 2)], [lmc, lka, lka], [lmc, lmc, lka], [lka, lmc]):
+    assert controller.order_can_sends(packets) is packets
+  controller.starpilot_selected = False
+  assert controller.order_can_sends([lmc, lka]) == [lmc, lka]
+  controller.starpilot_selected = True
+  controller.curv.primary_lateral_control = PrimaryLateralControl.curvature
+  assert controller.order_can_sends([lmc, lka]) == [lmc, lka]
+
+
+def test_steering_ordering_is_required_in_both_native_ci_builds():
+  from tools.ci.run_vehicle_tests import targets_for
+  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

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "9f5a884ceff1e1cab9b18b69c107eccb983613ea1443c9faafe82ab03eb1ec40", "schema_version": 1}
+{"protected_inventory_sha256": "6c2dbe430eb0e186f9ebf3c180d102f24c0ca4486096191add5d2018c735b5e0", "schema_version": 1}

tools/ci/run_vehicle_tests.py

@@ -34,7 +34,9 @@ FORD_BLUEPILOT_TARGETS = (
 
 # Ordinary native suites include inherited, intentionally unsupported cases; RELEASE preserves those skips.
 FORD_BLUEPILOT_NATIVE_TARGETS = tuple(str(path.relative_to(ROOT)) for path in sorted(
-  (ROOT / 'opendbc_repo/opendbc/bluepilot_lateral/safety/tests').glob('test_*.py')))
+  (ROOT / 'opendbc_repo/opendbc/bluepilot_lateral/safety/tests').glob('test_*.py'))) + (
+  'openpilot/starpilot/tests/test_ford_starpilot_ordering.py',
+)
 
 # Representative production callers plus their source/native refusal cases.
 # The emitted selected_test_ids record exact parametrized configurations.