Keep Ford path angle within sign-correct wire bounds

openpilot/starpilot/car/ford/starpilot_controller.py

@@ -40,9 +40,10 @@ from opendbc.bluepilot_lateral.core.values_ext import (
 from opendbc.bluepilot_lateral.core.values_ext import MODEL_T_IDXS
 from opendbc.bluepilot_lateral.params.manifest import default
 
-# DBC ``LatCtlPath_An_Actl`` (rad) — panda safety uses the same in ``ford.h``; PSCM enforces in firmware.
-FORD_DBC_PATH_ANGLE_MIN = -0.5
-FORD_DBC_PATH_ANGLE_MAX = 0.5235
+# The host negates path angle before packing the DBC range [-0.5, 0.5235] rad.
+# Keep saturation and clamping in the internal convention to prevent unsigned wrap.
+FORD_PATH_ANGLE_MIN = -0.5235
+FORD_PATH_ANGLE_MAX = 0.5
 
 
 # Default blend ratio validated on F-150 fleet data (0.5s lookup time).
@@ -376,8 +377,8 @@ class StarPilotFordController:
     # That caused a positive-feedback flat-line: under-steer → CtrSat → path_angle frozen → more under-steer.
     # Use DBC-limit proximity instead: only block when path_angle is already near the ±0.5 rad CAN limits,
     # which is the only condition where the anti-snap unwind rate cap makes physical sense.
-    _dbc_sat = (self.shared.path_angle_last >= FORD_DBC_PATH_ANGLE_MAX * 0.90 or
-                self.shared.path_angle_last <= FORD_DBC_PATH_ANGLE_MIN * 0.90)
+    _dbc_sat = (self.shared.path_angle_last >= FORD_PATH_ANGLE_MAX * 0.90 or
+                self.shared.path_angle_last <= FORD_PATH_ANGLE_MIN * 0.90)
     _in_hard_sat = _pscm_lim >= 2 or _dbc_sat
     # BluePilot: per-call delta threshold. The original 0.002 was authored 2026-05-07 on
     # bp-sid-simple (9c3d000fd), which ran STEER_STEP=1 (true 100Hz, switched 2026-04-22) -- so it
@@ -481,7 +482,7 @@ class StarPilotFordController:
     elif _pscm_lim >= 1:  # LimitClose (F150/non-angle-mode only): block increases only
       path_angle = float(clip(path_angle, -abs(self.shared.path_angle_last), abs(self.shared.path_angle_last)))
 
-    path_angle = min(FORD_DBC_PATH_ANGLE_MAX, max(FORD_DBC_PATH_ANGLE_MIN, path_angle))
+    path_angle = min(FORD_PATH_ANGLE_MAX, max(FORD_PATH_ANGLE_MIN, path_angle))
 
     # Soft ROC limit — unconditional, slightly tighter than ford.h, applied before the
     # hardware bypass in ford.h is re-enabled.  Lets us observe whether the limit would

openpilot/starpilot/tests/test_ford_path_angle_bounds.py

@@ -0,0 +1,86 @@
+"""Representable Ford path-angle commands across steering directions and buses."""
+from types import SimpleNamespace
+
+import pytest
+
+from opendbc.can import CANPacker, CANParser
+from opendbc.car import gen_empty_fingerprint, structs
+from opendbc.car.ford.fordcan import CanBus
+from opendbc.car.ford.interface import CarInterface
+from opendbc.car.ford.tests.test_bluepilot_lateral import STARTUP, StraightRoadInputs, settings
+from opendbc.car.ford.values import CAR, FordFlags
+from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import init_ford_safety_param_starpilot
+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
+
+
+@pytest.mark.parametrize('car', [CAR.FORD_ESCAPE_MK4, CAR.FORD_F_150_MK14, CAR.FORD_MUSTANG_MACH_E_MK1])
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('pinion', [False, True])
+@pytest.mark.parametrize('approach', [.449, .469])
+def test_path_angle_near_encoding_bounds_keeps_direction_and_passes_native(car, sign, pinion, approach):
+  cp = CarInterface.get_params(car, gen_empty_fingerprint(), [], False, False, False)
+  startup = STARTUP['on+pinion' if pinion else 'on']
+  init_ford_safety_param_starpilot(cp, startup)
+  packer, can = CANPacker('ford_lincoln_base_pt'), CanBus(cp)
+  source = SimpleNamespace(controller_choice=FordController.STARPILOT, current_inputs_valid=True)
+  host = FordBluePilotController(cp, packer, can, source)
+  road = StraightRoadInputs(settings(startup, True))
+  speed = 5.
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(out=structs.CarState(vEgo=speed, vEgoRaw=speed, canValid=True), lat_ctl_lim_stat=0)
+  message = 'LateralMotionControl2' if cp.flags & FordFlags.CANFD else 'LateralMotionControl'
+  parser = CANParser('ford_lincoln_base_pt', [(message, 0)], can.main)
+  safety = libsafety_py.libsafety
+  safety.init_tests()
+  safety.set_alternative_experience(0)
+  assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, int(cp.safetyConfigs[-1].safetyParam)) == 0
+  safety.set_controls_allowed(True)
+  wire_angles = []
+  try:
+    for frame in range(200):
+      safety.set_timer(1_000_000 + frame * 10_000)
+      messages: list[tuple[str, dict[str, float]]] = [
+        ('BrakeSysFeatures', {'Veh_V_ActlBrk': speed * 3.6, 'VehVActlBrk_D_Qf': 3, 'VehVActlBrk_No_Cnt': (frame // 2) % 16}),
+        ('EngVehicleSpThrottle2', {'Veh_V_ActlEng': speed * 3.6, 'VehVActlEng_D_Qf': 3}),
+        ('Yaw_Data_FD1', {'VehYaw_W_Actl': 0., 'VehYawWActl_D_Qf': 3, 'VehRollYaw_No_Cnt': frame % 256}),
+        ('DesiredTorqBrk', {'VehStop_D_Stat': 0}),
+        ('EngVehicleSpThrottle', {'ApedPos_Pc_ActlArb': 0}),
+        ('EngBrakeData', {'BpedDrvAppl_D_Actl': 1, 'CcStat_D_Actl': 5}),
+      ]
+      if pinion:
+        messages.append(('SteeringPinion_Data', {'StePinComp_An_Est': 0., 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': frame % 16}))
+      for name, fields in messages:
+        if name in ('BrakeSysFeatures', 'EngVehicleSpThrottle2') and frame % 2:
+          continue
+        assert fields.keys() <= packer.dbc.name_to_msg[name].sigs.keys()
+        address, data, bus = packer.make_can_msg(name, can.main, fields)
+        assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
+      # Settle just below the saturation boundary, then request a stronger turn.
+      curvature = sign * (approach if frame < 125 else .8) / (speed * 1.3)
+      cc.actuators.curvature = curvature
+      inputs = road.update()
+      inputs.lateralInputs.pathYawRate = [curvature * speed] * 33
+      host.update(inputs, frame)
+      cc.latActive = frame >= 15
+      packets = []
+      if frame % 5 == 0:
+        lateral, _ = host.step_lateral(cc, cs, cc.actuators, 0.)
+        packets.extend(lateral)
+        for address, data, bus in lateral:
+          parser.update([(1_000_000_000 + frame * 10_000_000, [(address, data, bus)])])
+          wire = parser.vl[message]['LatCtlPath_An_Actl']
+          assert -.5 <= wire <= 2047 * .0005 - .5
+          assert wire == pytest.approx(-host.angle.bp_path_angle_final, abs=.00025)
+          if cc.latActive:
+            assert wire * sign <= 0.
+            wire_angles.append(wire)
+      if frame % 3 == 0:
+        packets.append(host.step_lka(cc, cc.hudControl))
+      for address, data, bus in packets:
+        assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)), (frame, hex(address), data.hex())
+    assert max(abs(value) for value in wire_angles) >= approach - .00025
+    assert max(abs(b - a) for a, b in zip(wire_angles[:-1], wire_angles[1:], strict=True)) <= .0555
+  finally:
+    safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "c54f64d8c44db040567a25497ed148a4a065f6cac71f775d766c7b389721d3d7", "schema_version": 1}
+{"protected_inventory_sha256": "ae133de01c07ae992ebe38c06962a4e2eddaf13ad7a21698072fec47ac5011d4", "schema_version": 1}