Ford: gate coordinated C0/C1 encoder behind sunnylink trial toggle

SConstruct

@@ -288,6 +288,7 @@ if arch == "comma_arm64":
 SConscript([
   'openpilot/selfdrive/pandad/SConscript',
   'openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript',
+  'openpilot/selfdrive/controls/lib/ford_joint/SConscript',
   'openpilot/selfdrive/locationd/SConscript',
   'openpilot/selfdrive/modeld/SConscript',
   'openpilot/selfdrive/ui/SConscript',

docs/ford_joint_control_trial.md

@@ -0,0 +1,58 @@
+# Coordinated C0/C1 steering trial (v24)
+
+This default-off trial turns the normal action-derived desired steering angle into a jointly selected C0/C1 pair. It estimates the held fields and curvature filter of an older Ford PSCM and chooses one command whose predicted next-update error is no worse than a C1-anchored alternative. Among those candidates it minimizes error over the complete return to the steady pair, including the filter tail. It does not retain a future command plan or classify turns into maneuver states.
+
+## Select and revert
+
+In sunnylink → Vehicle → Ford Settings, while offroad:
+
+- **Selected-Action Path Tracking (Experimental): ON**
+- **Model Geometry Reference: OFF**
+- **Coordinated C0/C1 Steering (Experimental): ON**
+
+Apply with an offroad-to-onroad cycle. The new `FordPscmJointControl` parameter defaults to false. Turning it off restores v23 selected-action control. Turning the master Selected-Action setting off restores upstream Ford control, regardless of this stored setting. Geometry mode and Joystick Debug Mode keep their existing paths. C0 distance selection does not affect this trial.
+
+The integration accepts Ford CAN FD platforms, following the existing master gate. Compatibility with each PSCM calibration is not established.
+
+## Live integration
+
+`controlsd` retains the normal action source, curvature limits, vehicle-model conversion, input freshness checks and steering-angle target. With the trial selected, its existing Ford adapter is a validity carrier with zero P/I gains; it supplies no additional path correction. `card` performs the joint allocation immediately before the existing Ford CAN packer. It negates the encoder's CAN-coordinate output into the OP path interface; the packer negates it back on transmission.
+
+C2/C3 remain zero. Existing field bounds, mode 2 and immediate ramp remain unchanged. The observer advances at an estimated 8 ms firmware cadence using the decoded packets actually queued to panda. Queueing is not a PSCM acknowledgment. It retains held/filter estimates while disengaged or overridden and applies the recovered inactive slew to mode-0 packets. A fresh limitReached status does not freeze the target; driver override or denial inhibits this trial's lateral command. Longitudinal controls are untouched.
+
+A command-history gap over 100 ms, nonmonotonic clock or invalid prediction latches the trial inactive until the next onroad process start. The normal steering-fault alert reports the latched fault. There is no mid-turn switch to another controller. Diagnostics use `Ford joint path tracking` / `ford-joint-v24` and distinguish requested angle, reachable angle, estimated held state, proposed pair and packed pair.
+
+## What the estimate does and does not know
+
+Packaged numerical calibration comes from ML3V-14D003-BD / ML34-14D007-EDL. No executable firmware or private firmware image is shipped. Equivalence to the Lightning's RL38 firmware remains unverified. Internal integral is assumed initially zero and frozen; interaction is nominally 1.0; acceleration uses measured yaw × speed (no measured bank input); vehicle geometry comes from CarParams. These are explicit assumptions, not measured PSCM RAM. The model's 3 m/s² acceleration allowance is retained. It estimates an internal angle target before subsequent PSCM filtering/torque stages, not the future wheel angle.
+
+## Validation
+
+The production Python/C++ port reproduces every C0/C1 sample of the audited prototype in seven frozen windows from routes 166 and 16a. Held fields and filtered curvature also match; the largest angle-stage numerical difference is below 6e-14 degrees.
+
+A separate replay executes the actual 100 Hz adapter and Ford CAN packer with the reconstructed response ticking at 125 Hz. Mean internal-target error, degrees:
+
+| Window | Audited prototype | Actual 100 Hz adapter |
+|---|---:|---:|
+| 16a wobble 1 | 0.520 | 0.521 |
+| 16a wobble 2 | 0.240 | 0.248 |
+| 16a exit | 3.509 | 3.503 |
+| 16a turn | 9.762 | 9.981 |
+| 166 wobble 1 | 3.491 | 3.545 |
+| 166 wobble 2 | 0.530 | 0.587 |
+| 166 good left | 4.124 | 4.222 |
+
+These are frozen measurements and nominal active-mode reconstruction, not observed improvements on the truck. The actual adapter made 12,031 packed command updates without a latched fault. The slowest per-window 99th-percentile cycle was 0.95 ms on the development Mac; comma hardware timing is unmeasured. Firmware-rate/calibration uncertainty remains material.
+
+The focused suite exercises default-off selection, rollback, zero second PI, CAN signs/bounds/cadence, inactive state retention, interventions, limitReached, stale/nonfinite inputs, timing faults, real card hooks, parameter/schema registration, and inverse/forward numerical agreement. Run:
+
+```sh
+python -m pytest openpilot/selfdrive/car/tests/test_ford_joint_control.py \
+  openpilot/selfdrive/car/tests/test_ford_pscm_status.py \
+  openpilot/selfdrive/controls/tests/test_ford_model_action*.py \
+  openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py
+```
+
+Build the native kernel through SCons. On the development checkout, its real SConscript and native parameter library were compiled successfully. A complete root build could not run because the checkout lacks the msgq/rednose SCons tool submodules. No on-device build or road validation is claimed.
+
+The trial's question is whether coordinating both channels preserves entry while reducing unnecessary correction during release. Offline results justify the opt-in comparison; they cannot establish smoothness or closed-loop stability.

openpilot/common/params_keys.h

@@ -241,6 +241,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"FordModelActionController", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"FordC0TimeBased", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"FordGeometryReference", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"FordPscmJointControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}},
     {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},

openpilot/selfdrive/car/card.py

@@ -22,6 +22,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_to_capnp
 from openpilot.selfdrive.car.ford_pscm_status import populate_ford_pscm_status
+from openpilot.selfdrive.car.ford_joint_control import select_joint_control
 
 from openpilot.sunnypilot.mads.helpers import set_alternative_experience, set_car_specific_params
 from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
@@ -124,6 +125,7 @@ class Car:
       self.RI = RI
 
     self.CP.alternativeExperience = 0
+    self.ford_joint_control = select_joint_control(self.CP, self.params)
     # mads
     set_alternative_experience(self.CP, self.CP_SP, self.params)
     set_car_specific_params(self.CP, self.CP_SP, self.params)
@@ -200,6 +202,10 @@ class Car:
     CS, CS_SP = self.CI.update(can_list)
     CS_SP = convert_to_capnp(CS_SP)
     populate_ford_pscm_status(self.CP, self.CI.can_parsers, CS_SP, CS.canValid)
+    if self.ford_joint_control is not None and self.ford_joint_control.fault:
+      # Surface a latched loss of command history through the normal steering
+      # fault alert; never silently leave the UI engaged with this trial disabled.
+      CS.steerFaultTemporary = True
 
     # Update radar tracks from CAN
     RD: structs.RadarDataT | None = self.RI.update(can_list)
@@ -269,7 +275,7 @@ class Car:
     cs_sp_send.carStateSP = CS_SP
     self.pm.send('carStateSP', cs_sp_send)
 
-  def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.CarControlSP):
+  def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.CarControlSP, pscm_status=None):
     """control update loop, driven by carControl"""
 
     if not self.initialized_prev:
@@ -282,8 +288,16 @@ class Car:
     if self.sm.all_alive(['carControl']):
       # send car controls over can
       now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
-      self.last_actuators_output, can_sends = self.CI.apply(CC, convert_carControlSP(CC_SP), now_nanos)
+      control_sp = convert_carControlSP(CC_SP)
+      if self.ford_joint_control is not None:
+        CC = self.ford_joint_control.prepare(CC, control_sp, CS, now_nanos * 1e-9,
+                                            fresh=self.sm.all_checks(['carControl', 'carControlSP']), pscm_status=pscm_status)
+      self.last_actuators_output, can_sends = self.CI.apply(CC, control_sp, now_nanos)
       self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
+      if self.ford_joint_control is not None:
+        self.ford_joint_control.record_sent(can_sends, now_nanos)
+        if self.sm.frame % 20 == 0:
+          cloudlog.event('Ford joint path tracking', **self.ford_joint_control.diagnostics)
 
       self.CC_prev = CC
 
@@ -295,7 +309,7 @@ class Car:
     initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and
                    self.sm.seen['onroadEvents'])
     if not self.CP.passive and initialized:
-      self.controls_update(CS, self.sm['carControl'], self.sm['carControlSP'])
+      self.controls_update(CS, self.sm['carControl'], self.sm['carControlSP'], CS_SP.fordPscmStatus)
 
     self.initialized_prev = initialized
     self.CS_prev = CS

openpilot/selfdrive/car/ford_joint_control.py

@@ -0,0 +1,148 @@
+"""Default-off joint C0/C1 trial at the existing Ford transmit boundary.
+
+The observer consumes decoded packets queued to panda, not tentative requests.
+It estimates older-firmware state; it cannot observe ECU RAM or prove acceptance.
+"""
+
+import math
+
+from opendbc.car.ford.values import FordFlags
+
+
+def joint_control_enabled(CP, params):
+  return bool(
+    CP.brand == 'ford'
+    and CP.flags & FordFlags.CANFD
+    and params.get_bool('FordModelActionController')
+    and params.get_bool('FordPscmJointControl')
+    and not params.get_bool('FordGeometryReference')
+    and not params.get_bool('JoystickDebugMode')
+  )
+
+
+def select_joint_control(CP, params):
+  # No calibration or native-library load on the normal/default path.
+  return FordJointControl(CP) if joint_control_enabled(CP, params) else None
+
+
+class FordJointControl:
+  def __init__(self, CP):
+    from opendbc.can import CANParser
+    from opendbc.car.ford.fordcan import CanBus
+    from openpilot.selfdrive.controls.lib.ford_joint.model import MainRequest
+    from openpilot.selfdrive.controls.lib.ford_joint.angle import AngleModel
+    from openpilot.selfdrive.controls.lib.ford_joint.encoder import PairedRelease
+
+    self.wheelbase, self.ratio = CP.wheelbase, CP.steerRatio
+    if not all(math.isfinite(v) and v > 0 for v in (self.wheelbase, self.ratio)):
+      raise ValueError('Joint control requires finite positive vehicle geometry')
+    self.request = MainRequest(native_lookup=True)
+    self.angle = AngleModel(self.request.cal)
+    self.encoder = PairedRelease(self.request, preserve_now=True)
+    self.bus = CanBus(CP).main
+    self.parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], self.bus)
+    self.last_time = None
+    self.last_sent_time = None
+    self.phase = 0.0
+    self.sent = (0.0, 0.0, False)
+    self.measurement = (0.0, 0.0, 0.0)
+    self.fault = ''
+    self.diagnostics = {'hypothesis': 'ford-joint-v24', 'status': 'inactive'}
+
+  def advance(self, now):
+    """Retain channel/filter state through inactive commands and driver override."""
+    if self.last_time is not None:
+      elapsed = now - self.last_time
+      if not 0.0 <= elapsed <= 0.1:
+        # Lost command history cannot be repaired by pretending the held state
+        # reset. Disable this trial until the next onroad process start.
+        self.fault = 'command_timing_gap'
+      else:
+        self.phase += elapsed
+        ticks = int((self.phase + 1e-12) / 0.008)
+        self.phase -= ticks * 0.008
+        speed, angle, yaw = self.measurement
+        for _ in range(ticks):
+          r = self.request.step(speed, self.sent[0], self.sent[1], active=self.sent[2], freeze_i=True)
+          self.angle.step(speed, r['filtered_curvature'], angle, yaw, yaw * speed / 3.6, self.wheelbase, self.ratio)
+    self.last_time = now
+
+  def prepare(self, CC, CC_SP, CS, now, *, fresh=True, pscm_status=None):
+    from openpilot.selfdrive.controls.lib.ford_joint.inverse import invert_angle
+
+    self.advance(now)
+    path = CC_SP.fordLateralPath
+    target = float(CC.actuators.steeringAngleDeg)
+    finite = all(math.isfinite(v) for v in (CS.vEgo, CS.steeringAngleDeg, CS.yawRate, target))
+    if finite:
+      # The route extractor negated carState.yawRate; the audited inverse negated
+      # it back. Native carState yaw and wheel angle already share our CAN sign.
+      self.measurement = (max(0.0, CS.vEgo * 3.6), CS.steeringAngleDeg, CS.yawRate)
+    status_fresh = pscm_status is not None and pscm_status.valid and pscm_status.canMonoTime > 0 and -0.005 <= now - pscm_status.canMonoTime * 1e-9 <= 0.15
+    override = bool(CS.steeringPressed or (status_fresh and (pscm_status.limit == 3 or pscm_status.denied)))
+    valid = bool(
+      fresh
+      and CS.canValid
+      and finite
+      and 0.3 <= CS.vEgo <= 55
+      and abs(CS.yawRate) <= 3
+      and path.enabled
+      and path.valid
+      and not CS.steerFaultTemporary
+      and not CS.steerFaultPermanent
+    )
+    # A missing previous transmit receipt means the prediction is not synchronized.
+    if self.last_sent_time is not None and now - self.last_sent_time > 0.1:
+      self.fault = 'missing_transmit_history'
+    active = bool(CC.latActive and valid and not override and not self.fault)
+    command = (0.0, 0.0)
+    details = {}
+    if active:
+      try:
+        speed, angle, yaw = self.measurement
+        inverse = invert_angle(self.angle, speed, target, angle, yaw, yaw * speed / 3.6, self.wheelbase, self.ratio)
+        # Firmware phase is estimated from elapsed time. Cover the 1/2 firmware
+        # ticks before the next nominal 100 Hz transmit; all phases are tested.
+        ticks = max(1, min(2, int((self.phase + 0.01 + 1e-12) / 0.008)))
+        command, info = self.encoder.choose(speed, inverse['curvature'], phase=0 if ticks == 2 else 2)
+        details = {
+          'requested_angle': target,
+          'reachable_angle': inverse['reachable_target'],
+          'target_curvature': inverse['curvature'],
+          'predicted_curvature': float(info['first_state'][3]),
+          'accel_limited': inverse['accel_limited'],
+        }
+      except (ValueError, OverflowError, ArithmeticError):
+        self.fault = 'invalid_prediction'
+        active = False
+        command = (0.0, 0.0)
+    # The inverse operates in CAN/pinion coordinates. CarController negates
+    # FordLateralPath on TX, so negate here exactly once to retain the tested sign.
+    path.pathOffset, path.pathAngle = -float(command[0]), -float(command[1])
+    path.curvature = path.curvatureRate = 0.0
+    path.valid = active
+    cc = CC.as_reader().as_builder() if hasattr(CC, 'as_reader') else CC.as_builder()
+    cc.latActive = active
+    self.diagnostics = {
+      'hypothesis': 'ford-joint-v24',
+      'status': self.fault or ('active' if active else 'inactive'),
+      'driver_override': override,
+      'held_c0': self.request.c0,
+      'held_c1': self.request.c1,
+      'filtered_curvature': self.request.filtered,
+      'wire_command': tuple(map(float, command)),
+      **details,
+    }
+    return cc
+
+  def record_sent(self, can_sends, now_nanos):
+    packets = [msg for msg in can_sends if msg[0] == 0x3D6 and msg[2] == self.bus]
+    if not packets:
+      return
+    self.parser.update([now_nanos, packets])
+    values = self.parser.vl['LateralMotionControl2']
+    if values['LatCtlCurv_No_Actl'] != 0.0 or values['LatCtlCrv_NoRate2_Actl'] != 0.0:
+      self.fault = 'unexpected_curvature_channel'
+    self.sent = (values['LatCtlPathOffst_L_Actl'], values['LatCtlPath_An_Actl'], values['LatCtl_D2_Rq'] == 2)
+    self.last_sent_time = now_nanos * 1e-9
+    self.diagnostics['wire_sent'] = self.sent

openpilot/selfdrive/car/tests/test_ford_joint_control.py

@@ -0,0 +1,253 @@
+"""Exercise the opt-in trial through the production Ford CAN packer."""
+import copy
+import itertools
+import json
+import math
+from collections import defaultdict
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from opendbc.car import Bus, structs
+from opendbc.car.ford.carcontroller import CarController
+from opendbc.car.ford.values import FordFlags
+from openpilot.common.params import Params, ParamKeyFlag
+from openpilot.selfdrive.car.ford_joint_control import FordJointControl, joint_control_enabled, select_joint_control
+from openpilot.selfdrive.controls.lib.ford_joint.angle import AngleModel
+from openpilot.selfdrive.controls.lib.ford_joint.encoder import PairedRelease, state
+from openpilot.selfdrive.controls.lib.ford_joint.inverse import invert_angle
+from openpilot.selfdrive.controls.lib.ford_joint.model import MainRequest
+from openpilot.selfdrive.controls.tests.test_ford_model_action_adapter import update
+from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import car_params, startup
+
+
+def cp():
+  return structs.CarParams(brand='ford', flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1', wheelbase=3.7, steerRatio=16.9)
+
+
+def controls(target=30., active=True):
+  cc = structs.CarControl(latActive=active, longActive=True)
+  cc.actuators.steeringAngleDeg = target
+  cc.actuators.accel = .3
+  sp = structs.CarControlSP()
+  sp.fordLateralPath.enabled = sp.fordLateralPath.valid = True
+  return cc, sp
+
+
+class Pipeline:
+  def __init__(self):
+    self.joint = FordJointControl(cp())
+    self.sender = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp(), structs.CarParamsSP())
+    self.cs = structs.CarState(vEgo=5.36, vEgoRaw=5.36, canValid=True)
+    self.vehicle = SimpleNamespace(out=self.cs, acc_tja_status_stock_values=defaultdict(int),
+                                   lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
+
+  def tick(self, now, target=30., active=True, **kwargs):
+    cc, sp = controls(target, active)
+    result = self.joint.prepare(cc.as_reader(), sp, self.cs, now, **kwargs)
+    _, packets = self.sender.update(result.as_reader(), sp, self.vehicle, round(now * 1e9))
+    assert sum(p[0] == 0x3d6 for p in packets) == 1
+    self.joint.record_sent(packets, round(now * 1e9))
+    assert result.longActive and result.actuators.accel == cc.actuators.accel
+    wire = self.joint.parser.vl['LateralMotionControl2']
+    assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+    assert abs(wire['LatCtlPathOffst_L_Actl']) <= 5.11 + 1e-10
+    assert abs(wire['LatCtlPath_An_Actl']) <= .5 + 1e-10
+    assert wire['LatCtl_D2_Rq'] == (2 if result.latActive else 0)
+    assert self.joint.sent == pytest.approx((*self.joint.diagnostics['wire_command'], result.latActive))
+    return result
+
+
+@pytest.mark.parametrize('master,trial,geometry,joystick', itertools.product((False, True), repeat=4))
+@pytest.mark.parametrize('compatible', [False, True])
+def test_startup_gate_matches_both_processes(master, trial, geometry, joystick, compatible):
+  params = SimpleNamespace(get_bool=lambda k: {'FordModelActionController': master, 'FordPscmJointControl': trial,
+                                              'FordGeometryReference': geometry, 'JoystickDebugMode': joystick}.get(k, False))
+  params_cp = car_params(flags=FordFlags.CANFD if compatible else 0)
+  expected = compatible and master and trial and not geometry and not joystick
+  assert joint_control_enabled(params_cp, params) == expected
+  selected = startup(params_cp, params).ford_path_controller
+  assert bool(selected and selected.joint_control) == expected
+  if not expected:
+    assert select_joint_control(params_cp, params) is None
+  if not master:
+    assert selected is None
+
+
+def test_parameter_is_persistent_default_off(tmp_path):
+  params = Params(str(tmp_path))
+  assert params.get_default_value('FordPscmJointControl') is False
+  assert not params.get_bool('FordPscmJointControl')
+  for flag in (ParamKeyFlag.PERSISTENT, ParamKeyFlag.BACKUP):
+    assert b'FordPscmJointControl' in params.all_keys(flag)
+
+
+def test_reference_carrier_adds_no_second_feedback_loop():
+  from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
+  c = FordModelActionController(joint_control=True, c0_time_based=True)
+  for i in range(100):
+    p = update(c, now=1+i*.01, current_curvature=-.005)
+    assert p.valid and p.path_offset == p.path_angle == p.curvature == p.curvature_rate == 0.
+    assert c.core.correction == c.core.proportional == c.core.offset_proportional == 0.
+  assert not c.set_c0_time_based(True, lateral_engaged=False)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_can_coordinates_entry_reversal_release_and_100hz(sign):
+  p = Pipeline()
+  for i in range(250):
+    target = sign * (30. if i < 80 else -30. if i < 160 else 0.)
+    assert p.tick(1+i*.01, target).latActive
+    if i == 0:
+      # The existing sender negates OP path fields; the adapter compensates once.
+      assert sign * p.joint.sent[0] > 0 and sign * p.joint.sent[1] > 0
+    json.dumps(p.joint.diagnostics, allow_nan=False)
+  assert not p.joint.fault
+
+
+def test_observer_follows_packed_command_only_and_inactive_slew():
+  p = Pipeline()
+  cc, sp = controls()
+  before = state(p.joint.request).copy()
+  p.joint.prepare(cc, sp, p.cs, 1.)  # An untransmitted candidate is not held input.
+  p.joint.advance(1.01)
+  np.testing.assert_array_equal(state(p.joint.request), before)
+  for i in range(100):
+    p.tick(1.02+i*.01, 60.)
+  held = copy.copy(p.joint.request)
+  p.cs.steeringPressed = True
+  result = p.tick(2.02, 60.)
+  assert not result.latActive and p.joint.sent == (0., 0., False)
+  assert abs(p.joint.request.c0) > 0 or abs(p.joint.request.c1) > 0
+  held = copy.copy(p.joint.request)
+  phase = p.joint.phase
+  p.tick(2.03, 60.)
+  ticks = int((phase+.01+1e-12)/.008)
+  for _ in range(ticks):
+    held.step(5.36*3.6, 0., 0., active=False, freeze_i=True)
+  np.testing.assert_allclose(state(p.joint.request), state(held), atol=1e-12)
+  p.cs.steeringPressed = False
+  assert p.tick(2.04, -30.).latActive
+  assert p.joint.diagnostics['requested_angle'] == -30.
+
+
+@pytest.mark.parametrize('limit,denied,active', [(0, False, True), (2, False, True), (3, False, False), (0, True, False)])
+def test_limit_reached_does_not_freeze_but_override_inhibits(limit, denied, active):
+  p = Pipeline()
+  status = SimpleNamespace(valid=True, canMonoTime=1_000_000_000, limit=limit, denied=denied)
+  assert p.tick(1., pscm_status=status).latActive == active
+
+
+@pytest.mark.parametrize('field,value', [('vEgo', 0.), ('vEgo', .29), ('vEgo', 56.), ('yawRate', 3.1),
+                                       ('steeringAngleDeg', math.nan), ('vEgo', math.inf), ('canValid', False),
+                                       ('steerFaultTemporary', True), ('steerFaultPermanent', True)])
+def test_bad_measurements_never_send_previous_request(field, value):
+  p = Pipeline()
+  p.tick(1.)
+  setattr(p.cs, field, value)
+  assert not p.tick(1.01).latActive
+  assert p.joint.sent == (0., 0., False)
+  json.dumps(p.joint.diagnostics, allow_nan=False)
+
+
+@pytest.mark.parametrize('target', [math.nan, math.inf, -math.inf])
+def test_nonfinite_target_and_stale_control_are_inactive(target):
+  p = Pipeline()
+  assert not p.tick(1., target).latActive
+  assert not p.tick(1.01, fresh=False).latActive
+  assert p.tick(1.02).latActive
+
+
+@pytest.mark.parametrize('next_time', [.9, 1.101])
+def test_unknown_clock_history_latches_inactive(next_time):
+  p = Pipeline()
+  p.tick(1.)
+  assert not p.tick(next_time).latActive
+  assert not p.tick(next_time+.01).latActive
+  assert p.joint.fault
+
+
+def test_angle_inverse_matches_forward_stage_across_speeds():
+  rng = np.random.default_rng(418)
+  for _ in range(80):
+    m = AngleModel(MainRequest().cal)
+    m.yaw_acc, m.bank_residual, m.angle_bias = rng.uniform(-1., 1., 3)
+    speed = float(rng.uniform(1.1, 180.))
+    target, angle = map(float, rng.uniform(-300., 300., 2))
+    yaw = float(rng.uniform(-.5, .5))
+    result = invert_angle(m, speed, target, angle, yaw, yaw*speed/3.6, 3.7, 16.9)
+    actual = m.step(speed, result['curvature'], angle, yaw, yaw*speed/3.6, 3.7, 16.9)
+    assert actual == pytest.approx(result['reachable_target'], abs=1e-9)
+    assert abs(result['bounded_accel']) <= result['allowance']
+
+
+def test_candidate_does_not_mutate_state_and_native_step_matches_python():
+  rng = np.random.default_rng(919)
+  for j in range(40):
+    m = MainRequest(native_lookup=True)
+    m.c0, m.c1, m.filtered = map(float, rng.uniform([-5., -.49, -.08], [5., .49, .08]))
+    m.fast = bool(j % 2)
+    speed, curvature = float(rng.uniform(1.1, 180.)), float(rng.uniform(-.1, .1))
+    old = state(m).copy()
+    pair, info = PairedRelease(m).choose(speed, curvature, phase=j % 10)
+    np.testing.assert_array_equal(state(m), old)
+    for _ in range(info['count']):
+      m.step(speed, *pair, freeze_i=True)
+    np.testing.assert_allclose(state(m), info['first_state'], atol=1e-12, rtol=0)
+    assert abs(info['first_state'][3]-info['planned_target']) <= info['immediate_error_bound']+1e-12
+
+
+def test_card_transmit_hook_and_fault_alert_use_actual_source():
+  import ast
+  from pathlib import Path
+  from openpilot.cereal import custom
+  from openpilot.selfdrive.car.helpers import convert_carControlSP
+
+  source_path = Path(__file__).resolve().parents[1] / 'card.py'
+  tree = ast.parse(source_path.read_text())
+  car = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'Car')
+  method = next(n for n in car.body if isinstance(n, ast.FunctionDef) and n.name == 'controls_update')
+  branch = next(n for n in method.body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.sm.all_alive(['carControl'])")
+  code = compile(ast.Module(body=[branch], type_ignores=[]), str(source_path), 'exec')
+  p = Pipeline()
+  sent = []
+  owner = SimpleNamespace(ford_joint_control=p.joint, sm=SimpleNamespace(all_alive=lambda _: True, all_checks=lambda _: True, frame=1),
+                          CI=SimpleNamespace(apply=lambda cc, sp, now: p.sender.update(cc.as_reader(), sp, p.vehicle, now)),
+                          pm=SimpleNamespace(send=lambda service, data: sent.append((service, data))))
+  cc, _ = controls()
+  sp = custom.CarControlSP.new_message()
+  sp.fordLateralPath.enabled = sp.fordLateralPath.valid = True
+  exec(code, {'self': owner, 'CC': cc.as_reader(), 'CC_SP': sp, 'CS': p.cs, 'pscm_status': None, 'REPLAY': False,
+              'time': SimpleNamespace(monotonic=lambda: 1.), 'convert_carControlSP': convert_carControlSP,
+              'can_list_to_can_capnp': lambda data, **kwargs: data})
+  assert sent[0][0] == 'sendcan' and p.joint.sent[2]
+  assert p.joint.last_sent_time == 1.
+  assert owner.CC_prev.latActive
+
+  update_method = next(n for n in car.body if isinstance(n, ast.FunctionDef) and n.name == 'state_update')
+  alert = next(n for n in update_method.body if isinstance(n, ast.If) and 'self.ford_joint_control.fault' in ast.unparse(n.test))
+  alert_code = compile(ast.Module(body=[alert], type_ignores=[]), str(source_path), 'exec')
+  p.joint.fault = 'command_timing_gap'
+  exec(alert_code, {'self': owner, 'CS': p.cs})
+  assert p.cs.steerFaultTemporary
+
+
+def test_nonfinite_native_result_latches_inactive(monkeypatch):
+  from openpilot.selfdrive.controls.lib.ford_joint.encoder import LIB
+  p = Pipeline()
+  monkeypatch.setattr(LIB, 'paired_select', lambda *args: None)
+  assert not p.tick(1.).latActive
+  assert p.joint.fault == 'invalid_prediction'
+
+
+def test_duplicate_clock_and_missing_receipts_cannot_advance_unknown_history():
+  p = Pipeline()
+  p.tick(1.)
+  assert p.tick(1.).latActive
+  assert state(p.joint.request).tolist() == [0., 0., 0., 0., 0.]
+  # Prepare can run without sending, but stale TX history must end the trial.
+  for now in (1.05, 1.10, 1.15):
+    cc, sp = controls()
+    result = p.joint.prepare(cc, sp, p.cs, now)
+  assert not result.latActive and p.joint.fault == 'missing_transmit_history'

openpilot/selfdrive/controls/controlsd.py

@@ -57,7 +57,9 @@ class Controls(ControlsExt):
     self.desired_curvature = 0.0
     self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"),
                                                               c0_time_based=self.params.get_bool("FordC0TimeBased"),
-                                                              direct_path=self.params.get_bool("FordGeometryReference"))
+                                                              direct_path=self.params.get_bool("FordGeometryReference"),
+                                                              joint_control=self.params.get_bool("FordPscmJointControl") and
+                                                              not self.params.get_bool("JoystickDebugMode"))
     self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController)
     if self.CP.brand == "ford":
       cloudlog.event("Ford path controller selected",

openpilot/selfdrive/controls/lib/ford_joint/SConscript

@@ -0,0 +1,3 @@
+Import('env')
+
+env.SharedLibrary('encoder', 'encoder.cc', LIBS=[])

openpilot/selfdrive/controls/lib/ford_joint/angle.py

@@ -0,0 +1,31 @@
+"""Measurement states of the recovered curvature-to-angle stage, before torque."""
+
+import math
+
+from openpilot.selfdrive.controls.lib.ford_joint.model import bracket, interp_int, clip
+
+
+class AngleModel:
+  def __init__(self, cal):
+    self.cal = cal
+    self.speed_bp = cal.read(0xFEF25E40, '8H')
+    self.tables = {address: cal.read(address, '8H') for address in (0xFEF25CA0, 0xFEF25CB0)}
+    self.yaw_acc = self.bank_residual = self.angle_bias = 0.0
+
+  def step(self, speed_kmh, curvature, angle, yaw, accel, wheelbase, ratio):
+    c = self.cal
+    si, sf = bracket(int(speed_kmh * 256) & 0xFFFF, self.speed_bp)
+    v = speed_kmh / 3.6
+    self.yaw_acc += c.h(0xFEF25A90) / 65536 * (v * yaw - self.yaw_acc)
+    self.bank_residual += c.h(0xFEF25A88) / 65536 * (accel - v * yaw - self.bank_residual)
+    opposite = int(self.yaw_acc > 0) - int(self.yaw_acc < 0) != int(self.bank_residual > 0) - int(self.bank_residual < 0)
+    allowance = 3.0 + (abs(self.bank_residual) if opposite else 0.0)
+    requested_accel = clip(curvature * v * v, -allowance, allowance)
+    control_accel = requested_accel * c.b(0xFEF25AB1) / 128 + interp_int(self.tables[0xFEF25CA0], si, sf) / 2048 * (requested_accel - self.yaw_acc)
+    understeer = c.read(0xFEF25A9A, 'h')[0] / 16384
+    floor_v = max(v, c.b(0xFEF25AB7) / 32, 1e-6)
+    conversion = 180 / math.pi * ratio
+    target = ((self.bank_residual * c.read(0xFEF25AB0, 'b')[0] / 64 + control_accel) * understeer + control_accel / floor_v**2 * wheelbase) * conversion
+    error = (floor_v * understeer * yaw + yaw / floor_v * wheelbase) * conversion - angle - self.angle_bias
+    self.angle_bias += interp_int(self.tables[0xFEF25CB0], si, sf) / 65536 * error
+    return target + self.angle_bias * c.read(0xFEF25AB2, 'b')[0] / 64

openpilot/selfdrive/controls/lib/ford_joint/calibration.json

@@ -0,0 +1,475 @@
+{
+  "firmware": "ML3V-14D003-BD",
+  "calibration": "ML34-14D007-EDL",
+  "firmware_sha256": "8de3eb1f8191b13b57de07430c33f7b723f11b6ed2144991f91dc56476bbbf62",
+  "note": "Recovered numerical calibration only; RL38 equivalence unverified. No executable firmware.",
+  "entries": {
+    "0xfef25e40": {
+      "format": "8H",
+      "values": [
+        0,
+        3840,
+        7680,
+        15360,
+        25600,
+        33280,
+        40960,
+        56320
+      ]
+    },
+    "0xfef25ca0": {
+      "format": "8H",
+      "values": [
+        0,
+        410,
+        410,
+        410,
+        614,
+        1024,
+        410,
+        410
+      ]
+    },
+    "0xfef25cb0": {
+      "format": "8H",
+      "values": [
+        1311,
+        1311,
+        1147,
+        983,
+        819,
+        590,
+        393,
+        229
+      ]
+    },
+    "0xfef25d98": {
+      "format": "10f",
+      "values": [
+        10.0,
+        20.0,
+        30.0,
+        40.0,
+        50.0,
+        60.0,
+        70.0,
+        80.0,
+        100.0,
+        150.0
+      ]
+    },
+    "0xfef258fc": {
+      "format": "8B",
+      "values": [
+        3,
+        3,
+        4,
+        5,
+        7,
+        10,
+        11,
+        11
+      ]
+    },
+    "0xfef25b78": {
+      "format": "8f",
+      "values": [
+        0.0,
+        0.019999999552965164,
+        0.05000000074505806,
+        0.10000000149011612,
+        0.11749999970197678,
+        0.125,
+        0.125,
+        0.125
+      ]
+    },
+    "0xfef25b98": {
+      "format": "8f",
+      "values": [
+        15.0,
+        15.0,
+        25.0,
+        41.666664123535156,
+        55.55555725097656,
+        72.22222137451172,
+        88.88888549804688,
+        122.22222137451172
+      ]
+    },
+    "0xfef25bb8": {
+      "format": "8f",
+      "values": [
+        15.0,
+        15.0,
+        25.0,
+        41.666664123535156,
+        50.0,
+        46.94444274902344,
+        57.77777862548828,
+        79.44444274902344
+      ]
+    },
+    "0xfef25be8": {
+      "format": "8f",
+      "values": [
+        0.0,
+        0.0,
+        5.0,
+        5.0,
+        18.518518447875977,
+        24.074073791503906,
+        29.629629135131836,
+        29.0
+      ]
+    },
+    "0xfef25c60": {
+      "format": "8H",
+      "values": [
+        512,
+        512,
+        512,
+        1024,
+        1536,
+        1536,
+        1536,
+        1536
+      ]
+    },
+    "0xfef25d70": {
+      "format": "10f",
+      "values": [
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0
+      ]
+    },
+    "0xfef25dc0": {
+      "format": "10f",
+      "values": [
+        0.19599999487400055,
+        0.09799999743700027,
+        0.06530000269412994,
+        0.04899999871850014,
+        0.03920000046491623,
+        0.03269999846816063,
+        0.02800000086426735,
+        0.02449999935925007,
+        0.019600000232458115,
+        0.013100000098347664
+      ]
+    },
+    "0xfef25de8": {
+      "format": "10f",
+      "values": [
+        0.019999999552965164,
+        0.004999999888241291,
+        0.002222222276031971,
+        0.0012499999720603228,
+        0.0007999999797903001,
+        0.0005555555690079927,
+        0.0004081632650922984,
+        0.0003124999930150807,
+        0.00019999999494757503,
+        8.888888987712562e-05
+      ]
+    },
+    "0xfef25e20": {
+      "format": "8H",
+      "values": [
+        24576,
+        24576,
+        24576,
+        24576,
+        24576,
+        24576,
+        16384,
+        819
+      ]
+    },
+    "0xfef25e30": {
+      "format": "8H",
+      "values": [
+        24576,
+        24576,
+        24576,
+        24576,
+        20480,
+        18022,
+        16384,
+        819
+      ]
+    },
+    "0xfef25a58": {
+      "format": "f",
+      "values": [
+        2.0
+      ]
+    },
+    "0xfef259ec": {
+      "format": "f",
+      "values": [
+        1.0
+      ]
+    },
+    "0xfef259f0": {
+      "format": "f",
+      "values": [
+        2.0
+      ]
+    },
+    "0xfef259f4": {
+      "format": "f",
+      "values": [
+        0.6000000238418579
+      ]
+    },
+    "0xfef25a14": {
+      "format": "f",
+      "values": [
+        30.0
+      ]
+    },
+    "0xfef25a04": {
+      "format": "f",
+      "values": [
+        300.0
+      ]
+    },
+    "0xfef2596c": {
+      "format": "f",
+      "values": [
+        1200.0
+      ]
+    },
+    "0xfef259a0": {
+      "format": "f",
+      "values": [
+        0.20000000298023224
+      ]
+    },
+    "0xfef2599c": {
+      "format": "f",
+      "values": [
+        1.350000023841858
+      ]
+    },
+    "0xfef259a4": {
+      "format": "f",
+      "values": [
+        -10.0
+      ]
+    },
+    "0xfef259a8": {
+      "format": "f",
+      "values": [
+        0.15000000596046448
+      ]
+    },
+    "0xfef25a70": {
+      "format": "H",
+      "values": [
+        1229
+      ]
+    },
+    "0xfef25a72": {
+      "format": "H",
+      "values": [
+        0
+      ]
+    },
+    "0xfef25a60": {
+      "format": "H",
+      "values": [
+        256
+      ]
+    },
+    "0xfef25aa5": {
+      "format": "B",
+      "values": [
+        0
+      ]
+    },
+    "0xfef25aa6": {
+      "format": "B",
+      "values": [
+        38
+      ]
+    },
+    "0xfef25aa1": {
+      "format": "B",
+      "values": [
+        0
+      ]
+    },
+    "0xfef25aa2": {
+      "format": "B",
+      "values": [
+        160
+      ]
+    },
+    "0xfef25a9e": {
+      "format": "B",
+      "values": [
+        0
+      ]
+    },
+    "0xfef25a9f": {
+      "format": "B",
+      "values": [
+        32
+      ]
+    },
+    "0xfef25aa4": {
+      "format": "B",
+      "values": [
+        255
+      ]
+    },
+    "0xfef25aa7": {
+      "format": "B",
+      "values": [
+        26
+      ]
+    },
+    "0xfef25aa3": {
+      "format": "B",
+      "values": [
+        9
+      ]
+    },
+    "0xfef25aa0": {
+      "format": "B",
+      "values": [
+        38
+      ]
+    },
+    "0xfef25a74": {
+      "format": "H",
+      "values": [
+        51
+      ]
+    },
+    "0xfef25a00": {
+      "format": "f",
+      "values": [
+        0.30000001192092896
+      ]
+    },
+    "0xfef25a10": {
+      "format": "f",
+      "values": [
+        0.014999999664723873
+      ]
+    },
+    "0xfef259e8": {
+      "format": "f",
+      "values": [
+        0.0003000000142492354
+      ]
+    },
+    "0xfef259e4": {
+      "format": "f",
+      "values": [
+        0.003000000026077032
+      ]
+    },
+    "0xfef25a0c": {
+      "format": "f",
+      "values": [
+        0.30000001192092896
+      ]
+    },
+    "0xfef259fc": {
+      "format": "f",
+      "values": [
+        2.799999952316284
+      ]
+    },
+    "0xfef25968": {
+      "format": "f",
+      "values": [
+        -0.30000001192092896
+      ]
+    },
+    "0xfef25960": {
+      "format": "f",
+      "values": [
+        0.30000001192092896
+      ]
+    },
+    "0xfef25964": {
+      "format": "f",
+      "values": [
+        -0.4000000059604645
+      ]
+    },
+    "0xfef2595c": {
+      "format": "f",
+      "values": [
+        0.4000000059604645
+      ]
+    },
+    "0xfef25a08": {
+      "format": "f",
+      "values": [
+        0.10000000149011612
+      ]
+    },
+    "0xfef259f8": {
+      "format": "f",
+      "values": [
+        1.5
+      ]
+    },
+    "0xfef25a90": {
+      "format": "H",
+      "values": [
+        6554
+      ]
+    },
+    "0xfef25a88": {
+      "format": "H",
+      "values": [
+        1627
+      ]
+    },
+    "0xfef25ab7": {
+      "format": "B",
+      "values": [
+        32
+      ]
+    },
+    "0xfef25a9a": {
+      "format": "h",
+      "values": [
+        33
+      ]
+    },
+    "0xfef25ab1": {
+      "format": "B",
+      "values": [
+        128
+      ]
+    },
+    "0xfef25ab0": {
+      "format": "b",
+      "values": [
+        0
+      ]
+    },
+    "0xfef25ab2": {
+      "format": "b",
+      "values": [
+        -64
+      ]
+    }
+  }
+}

openpilot/selfdrive/controls/lib/ford_joint/encoder.cc

@@ -0,0 +1,100 @@
+// Opt-in Ford joint encoder. Numerical request state is estimated, not ECU RAM.
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+
+static double clip(double x, double lo, double hi) {
+  return std::min(std::max(x, lo), hi);
+}
+
+// State: held C0, held C1, internal I, filtered curvature, fast-slew latch.
+// Parameters: g0/g1, base alpha, heading low/high/gain, alpha max, speed,
+// interaction threshold/value, C0 slow/fast, C1 slow/fast, latch clear C0/C1,
+// opposite C0/I thresholds, I gain, delta low/high, I low/high, release rate.
+static void step(double *s, const double *p, double c0, double c1) {
+  if (p[9] < p[8]) {
+    s[4] = 1;
+  } else if (std::abs(c0) <= p[14] && std::abs(c1) <= p[15]) {
+    s[4] = 0;
+  }
+  double r0 = p[s[4] ? 11 : 10], r1 = p[s[4] ? 13 : 12];
+  s[0] += clip(c0 - s[0], -r0 * .008, r0 * .008);
+  s[1] += clip(c1 - s[1], -r1 * .008, r1 * .008);
+  if (std::abs(s[0]) > p[16] && std::abs(s[2]) > p[17] && s[0] * s[2] < 0) {
+    s[2] += clip(-s[2], -p[23], p[23]);
+  } else {
+    s[2] = clip(s[2] + clip(s[0], p[19], p[20]) * p[18], p[21], p[22]);
+  }
+  double raw = p[0] * (s[0] + s[2]) + p[1] * s[1];
+  double w = clip(std::abs(s[1] * p[7]) - p[3], 0, std::max(1e-6, p[4] - p[3])) / std::max(1e-6, p[4] - p[3]);
+  double alpha = std::min(p[6], p[2] + p[5] * w);
+  s[3] += alpha * (raw - s[3]);
+}
+
+extern "C" double paired_cost(const double *initial, const double *p, double target,
+                              const double *pref, int count, double c0, double c1, double *first) {
+  double s[5];
+  std::memcpy(s, initial, sizeof(s));
+  double steady_raw = p[0] * pref[0] + p[1] * pref[1];
+  double steady_error = (steady_raw - target) / .01, cost = 0;
+  auto tick = [&](double a, double b) {
+    step(s, p, a, b);
+    double error = (s[3] - target) / .01;
+    cost += .008 * (error * error - steady_error * steady_error);
+  };
+  for (int j = 0; j < count; j++) tick(c0, c1);
+  if (first) std::memcpy(first, s, sizeof(s));
+  // Include both channels' entire return, followed by the exact filter tail.
+  // There is no adjustable planning horizon or retained future command plan.
+  int n = 2 + std::ceil(std::max(std::abs(s[0] - pref[0]) / std::min(p[10], p[11]),
+                               std::abs(s[1] - pref[1]) / std::min(p[12], p[13])) / .008);
+  for (int j = 0; j < n; j++) tick(pref[0], pref[1]);
+  double w = clip(std::abs(s[1] * p[7]) - p[3], 0, p[4] - p[3]) / (p[4] - p[3]);
+  double alpha = std::min(p[6], p[2] + p[5] * w), a = 1 - alpha;
+  double d = (s[3] - steady_raw) / .01;
+  return cost + .008 * (2 * steady_error * d * a / alpha + d * d * a * a / (1 - a * a));
+}
+
+extern "C" void paired_select(const double *initial, const double *p, double target,
+                              const double *pref, int count, const double *c0s, int n0,
+                              const double *c1s, int n1, int preserve_now, double *result) {
+  double best = 1e300, best_move = 1e300, best_remaining = 1e300;
+  double first[5];
+  double max_error = 1e300, anchor_score = 1e300, anchor_move = 1e300, anchor_remaining = 1e300;
+  if (preserve_now) {
+    // Preserve the C1-anchored policy's immediate target accuracy.
+    // An inequality against a feasible reference, not a new gain/deadband.
+    for (int i = 0; i < n0; i++) {
+      double cost = paired_cost(initial, p, target, pref, count, c0s[i], pref[1], first);
+      double score = std::nearbyint(cost * 1e12) / 1e12;
+      double move = std::abs(c0s[i] - initial[0]), remaining = std::abs(c0s[i] - pref[0]);
+      if (score < anchor_score || (score == anchor_score && (move < anchor_move || (move == anchor_move && remaining < anchor_remaining)))) {
+        anchor_score = score;
+        anchor_move = move;
+        anchor_remaining = remaining;
+        max_error = std::abs(first[3] - target);
+      }
+    }
+  }
+  for (int i = 0; i < n0; i++) {
+    for (int j = 0; j < n1; j++) {
+      double cost = paired_cost(initial, p, target, pref, count, c0s[i], c1s[j], first);
+      if (std::abs(first[3] - target) > max_error + 1e-12) continue;
+      // Numerical equality only. Tie breaks cannot trade worse tracking for
+      // less channel motion; normalize them using the existing field spans.
+      double score = std::nearbyint(cost * 1e12) / 1e12;
+      double move = std::abs(c0s[i] - initial[0]) / 5.11 + std::abs(c1s[j] - initial[1]) / .5;
+      double remaining = std::abs(c0s[i] - pref[0]) / 5.11 + std::abs(c1s[j] - pref[1]) / .5;
+      if (score < best || (score == best && (move < best_move || (move == best_move && remaining < best_remaining)))) {
+        best = score;
+        best_move = move;
+        best_remaining = remaining;
+        result[0] = c0s[i];
+        result[1] = c1s[j];
+        result[2] = cost;
+        std::memcpy(result + 3, first, 5 * sizeof(double));
+      }
+    }
+  }
+  result[8] = max_error;
+}

openpilot/selfdrive/controls/lib/ford_joint/encoder.py

@@ -0,0 +1,112 @@
+"""Joint C0/C1 allocation: one update, then a fixed steady-return policy.
+
+No future tape, retained trajectory, maneuver modes or tunable tracking gain.
+The next-update accuracy constraint is enabled for the trial.
+"""
+
+import copy
+import ctypes
+import sys
+from pathlib import Path
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.ford_joint.model import DT, interp, interp_int
+from openpilot.selfdrive.controls.lib.ford_joint.inverse import static_pair, quantize
+
+ARRAY = np.ctypeslib.ndpointer(dtype=np.float64, flags='C_CONTIGUOUS')
+
+
+def state(m):
+  return np.array([m.c0, m.c1, m.c0_i, m.filtered, float(m.fast)], dtype=np.float64)
+
+
+def parameters(m, speed, freeze_i=True, interaction=1.0):
+  if m.c2 != 0 or m.c3 != 0:
+    raise ValueError('C2/C3 must be zero')
+  if abs(m.preview_t - m.cal.f(0xFEF25A58)) > 1e-9:
+    raise ValueError('Fixed preview time required')
+  c = m.cal
+  probe = copy.copy(m).step(speed, m.c0, m.c1, freeze_i=freeze_i, interaction=interaction)
+  si, sf = probe['speed_index'], probe['speed_fraction']
+  return np.array(
+    [
+      probe['g0'],
+      probe['g1'],
+      interp_int(m.tables[0xFEF258FC], si, sf) / 256,
+      c.b(0xFEF25AA5) / 128,
+      c.b(0xFEF25AA6) / 128,
+      c.b(0xFEF25AA7) / 256,
+      c.b(0xFEF25AA4) / 256,
+      speed * 0.28,
+      c.f(0xFEF259F4),
+      interaction,
+      c.f(0xFEF259F8),
+      c.f(0xFEF259FC),
+      c.f(0xFEF25A08),
+      c.f(0xFEF25A0C),
+      c.f(0xFEF25A00),
+      c.f(0xFEF25A10),
+      c.h(0xFEF25A70) / 8192,
+      c.h(0xFEF25A72) / 8192,
+      0.0 if freeze_i else interp(m.tables[0xFEF25B78], si, sf) * DT,
+      c.f(0xFEF25968),
+      c.f(0xFEF25960),
+      c.f(0xFEF25964),
+      c.f(0xFEF2595C),
+      interp_int(m.tables[0xFEF25C60], si, sf) / 512 * DT,
+    ],
+    dtype=np.float64,
+  )
+
+
+LIB = ctypes.CDLL(str(Path(__file__).with_name('libencoder' + ('.dylib' if sys.platform == 'darwin' else '.so'))))
+LIB.paired_cost.argtypes = [ARRAY, ARRAY, ctypes.c_double, ARRAY, ctypes.c_int, ctypes.c_double, ctypes.c_double, ARRAY]
+LIB.paired_cost.restype = ctypes.c_double
+LIB.paired_select.argtypes = [ARRAY, ARRAY, ctypes.c_double, ARRAY, ctypes.c_int, ARRAY, ctypes.c_int, ARRAY, ctypes.c_int, ctypes.c_int, ARRAY]
+LIB.paired_select.restype = None
+
+
+def levels(held, pref, rate, count, lsb, bound, clear):
+  reach = rate * 0.008 * count + lsb / 2
+  lo = max(round(-bound / lsb), int(np.floor((held - reach) / lsb)))
+  hi = min(round(bound / lsb), int(np.ceil((held + reach) / lsb)))
+  # Include both sides of the fast-latch clearing thresholds even when outside
+  # the reachable range: equal slew endpoints can otherwise have different flags.
+  near = np.floor(np.array([-clear, clear]) / lsb).astype(int)
+  thresholds = np.r_[near - 1, near, near + 1] * lsb
+  return np.unique(np.clip(np.r_[np.arange(lo, hi + 1) * lsb, pref, round(held / lsb) * lsb, thresholds, -bound, bound], -bound, bound))
+
+
+class PairedRelease:
+  def __init__(self, request, freeze_i=True, interaction=1.0, preserve_now=True):
+    self.request = request
+    self.freeze_i = freeze_i
+    self.interaction = interaction
+    self.preserve_now = preserve_now
+
+  def choose(self, speed_kmh, curvature, phase=0):
+    m = self.request
+    if not self.freeze_i or m.c0_i != 0:
+      raise ValueError('Joint encoder requires its nominal zero-I estimate')
+    if phase not in range(10) or speed_kmh <= 0 or not np.isfinite([speed_kmh, curvature, *state(m)]).all():
+      raise ValueError('Finite moving-vehicle inputs and scheduler phase required')
+    p = parameters(m, speed_kmh, True, self.interaction)
+    s = state(m)
+    pref = np.array(quantize(static_pair(m, curvature, speed_kmh)))
+    target = float(p[0] * pref[0] + p[1] * pref[1])
+    count = next(k for k in range(1, 3) if (phase + 8 * k) // 10 > 0)
+    c0s = levels(m.c0, pref[0], max(p[10:12]), count, 0.01, 5.11, p[14])
+    c1s = levels(m.c1, pref[1], max(p[12:14]), count, 0.0005, 0.5, p[15])
+    result = np.full(9, np.nan)
+    LIB.paired_select(s, p, target, pref, count, c0s, len(c0s), c1s, len(c1s), int(self.preserve_now), result)
+    if not np.isfinite(result).all() or abs(result[0]) > 5.11 or abs(result[1]) > 0.5:
+      raise ValueError('No finite bounded joint command')
+    return tuple(result[:2]), {
+      'cost': float(result[2]),
+      'first_state': result[3:8].copy(),
+      'count': count,
+      'pref': pref,
+      'planned_target': target,
+      'evaluations': len(c0s) * len(c1s),
+      'immediate_error_bound': result[8],
+    }

openpilot/selfdrive/controls/lib/ford_joint/inverse.py

@@ -0,0 +1,80 @@
+"""Invert the recovered angle-target stage; no torque or wheel plant model."""
+
+import copy
+import math
+
+from openpilot.selfdrive.controls.lib.ford_joint.model import clip, interp_int, bracket
+
+C0_BOUND = 5.11
+C1_BOUND = 0.5
+
+
+def invert_angle(output, speed_kmh, target_angle, angle, yaw, accel, wheelbase, ratio, accel_allowance=3.0):
+  """Invert 0x1818e6 BEFORE its separate angle-reference filter.
+
+  The angle equation is affine in acceleration within the acceleration bounds.
+  Predict only the measurement-driven states it updates this cycle. The unknown
+  assist/load input occurs downstream and does not enter this calculation.
+  """
+  if not all(math.isfinite(x) for x in (speed_kmh, target_angle, angle, yaw, accel, wheelbase, ratio)):
+    raise ValueError('Nonfinite input')
+  if speed_kmh <= 0 or wheelbase <= 0 or ratio <= 0:
+    raise ValueError('A moving vehicle with positive geometry is required')
+  c = output.cal
+  si, sf = bracket(int(speed_kmh * 256) & 0xFFFF, output.speed_bp)
+
+  def tab(a):
+    return interp_int(output.tables[a], si, sf)
+
+  v = speed_kmh / 3.6
+  yaw_acc = output.yaw_acc + c.h(0xFEF25A90) / 65536 * (v * yaw - output.yaw_acc)
+  bank = output.bank_residual + c.h(0xFEF25A88) / 65536 * (accel - v * yaw - output.bank_residual)
+  floor_v = max(v, c.b(0xFEF25AB7) / 32, 1e-6)
+  understeer = c.read(0xFEF25A9A, 'h')[0] / 16384
+  conversion = 180 / math.pi * ratio
+  bias_error = (floor_v * understeer * yaw + yaw / floor_v * wheelbase) * conversion - angle - output.angle_bias
+  bias = output.angle_bias + tab(0xFEF25CB0) / 65536 * bias_error
+  p = tab(0xFEF25CA0) / 2048
+  slope = (c.b(0xFEF25AB1) / 128 + p) * (understeer + wheelbase / floor_v**2) * conversion
+  intercept = (-p * yaw_acc * (understeer + wheelbase / floor_v**2) + bank * c.read(0xFEF25AB0, 'b')[0] / 64 * understeer) * conversion
+  intercept += bias * c.read(0xFEF25AB2, 'b')[0] / 64
+  def sign(x):
+    return int(x > 0) - int(x < 0)
+  allowance = accel_allowance + (abs(bank) if sign(yaw_acc) != sign(bank) else 0)
+  wanted_accel = (target_angle - intercept) / slope
+  bounded_accel = clip(wanted_accel, -allowance, allowance)
+  return {
+    'curvature': bounded_accel / v**2,
+    'wanted_accel': wanted_accel,
+    'bounded_accel': bounded_accel,
+    'allowance': allowance,
+    'reachable_target': intercept + slope * bounded_accel,
+    'accel_limited': abs(wanted_accel) > allowance,
+    'slope_per_curvature': slope * v**2,
+    'intercept': intercept,
+  }
+
+
+def arc_pair(curvature, speed_kmh):
+  """Current base geometry only; no current OP proportional/integral terms."""
+  half = 0.5 * 7 * curvature
+  sinc = math.sin(half) / half if half else 1.0
+  c1 = max(7.0, speed_kmh / 3.6) * curvature
+  clipped_c1 = clip(c1, -C1_BOUND, C1_BOUND)
+  c0 = 0.5 * curvature * 49 * sinc * sinc + 7 * (c1 - clipped_c1)
+  return clip(c0, -C0_BOUND, C0_BOUND), clipped_c1
+
+
+def static_pair(request, curvature, speed_kmh):
+  """Preserve the base's channel proportion, solve its static curvature sum."""
+  probe = copy.copy(request)
+  gains = probe.step(speed_kmh, request.c0, request.c1, freeze_i=True)
+  p0, p1 = arc_pair(curvature, speed_kmh)
+  total = gains['g0'] * p0 + gains['g1'] * p1
+  scale = curvature / total if total else 1.0
+  return clip(p0 * scale, -C0_BOUND, C0_BOUND), clip(p1 * scale, -C1_BOUND, C1_BOUND)
+
+
+def quantize(pair):
+  """Existing symmetric controller bounds; DBC LSBs, round nearest."""
+  return (clip(round(pair[0] / 0.01) * 0.01, -C0_BOUND, C0_BOUND), clip(round(pair[1] / 0.0005) * 0.0005, -C1_BOUND, C1_BOUND))

openpilot/selfdrive/controls/lib/ford_joint/model.py

@@ -0,0 +1,179 @@
+"""Nominal older-PSCM request recurrence for the opt-in joint encoder.
+
+The held fields, filter, slow/fast latch and zero internal I are estimates.
+Calibration is packaged numerical data, never loaded from an ECU image.
+"""
+
+import bisect
+import json
+import struct
+from pathlib import Path
+
+DT = 0.008
+
+
+class Calibration:
+  def __init__(self):
+    self.entries = json.loads(Path(__file__).with_name('calibration.json').read_text())['entries']
+
+  def read(self, address, fmt='f'):
+    item = self.entries[hex(address)]
+    if item['format'] != fmt:
+      raise ValueError(f'Unexpected calibration format: {address:x}')
+    return tuple(item['values'])
+
+  def f(self, address):
+    return self.read(address)[0]
+
+  def b(self, address):
+    return self.read(address, 'B')[0]
+
+  def h(self, address):
+    return self.read(address, 'H')[0]
+
+
+def clip(x, lo, hi):
+  return min(max(x, lo), hi)
+
+
+def slew(x, target, rate):
+  return x + clip(target - x, -rate * DT, rate * DT)
+
+
+def bracket(x, breakpoints):
+  i = max(0, bisect.bisect_right(breakpoints, x) - 1)
+  if i == len(breakpoints) - 1 or x <= breakpoints[0]:
+    return i, 0
+  return i, min(255, int((x - breakpoints[i]) * 256 / (breakpoints[i + 1] - breakpoints[i])))
+
+
+def interp(tab, idx, frac):
+  return tab[idx] if not frac else tab[idx] + (tab[idx + 1] - tab[idx]) * frac / 256
+
+
+def interp_int(tab, idx, frac):
+  if not frac:
+    return tab[idx]
+  d = tab[idx + 1] - tab[idx]
+  return tab[idx] + (1 if d >= 0 else -1) * (abs(d) * frac // 256)
+
+
+def f32(x):
+  return struct.unpack('<f', struct.pack('<f', x))[0]
+
+
+def interp_f32(tab, idx, frac):
+  return tab[idx] if not frac else f32(tab[idx] + f32(f32(tab[idx + 1] - tab[idx]) * frac) / 256)
+
+
+def bracket_f32(x, breakpoints):
+  i = max(0, bisect.bisect_right(breakpoints, x) - 1)
+  if i == len(breakpoints) - 1 or x <= breakpoints[0]:
+    return i, 0
+  fraction = f32(f32(f32(x - breakpoints[i]) / f32(breakpoints[i + 1] - breakpoints[i])) * 256)
+  return i, min(255, int(fraction))
+
+
+class MainRequest:
+  def __init__(self, cal=None, *, native_lookup=False):
+    self.cal = cal or Calibration()
+    c = self.cal
+    self.speed_bp = c.read(0xFEF25E40, '8H')
+    self.distance_bp = c.read(0xFEF25D98, '10f')
+    self.tables = {
+      a: c.read(a, fmt)
+      for a, fmt in [
+        (0xFEF258FC, '8B'),
+        (0xFEF25B78, '8f'),
+        (0xFEF25B98, '8f'),
+        (0xFEF25BB8, '8f'),
+        (0xFEF25BE8, '8f'),
+        (0xFEF25C60, '8H'),
+        (0xFEF25D70, '10f'),
+        (0xFEF25DC0, '10f'),
+        (0xFEF25DE8, '10f'),
+        (0xFEF25E20, '8H'),
+        (0xFEF25E30, '8H'),
+      ]
+    }
+    self.c0 = self.c1 = self.c2 = self.c3 = self.c0_i = self.filtered = 0.0
+    self.preview_t = c.f(0xFEF25A58)
+    self.fast = False
+    # Opt-in preserves the reproducibility of archived exploratory replays.
+    # New inverse-allocation experiments use this corrected native lookup path.
+    self.native_lookup = native_lookup
+
+  def step(self, speed_kmh, c0, c1, c2=0.0, c3=0.0, active=True, interaction=1.0, freeze_i=False, release_i=False, special_release=False, indicator=False):
+    c = self.cal
+    # 180f4e: input state, speed lookup, history-dependent slew selection.
+    lookup_speed = f32(speed_kmh) if self.native_lookup else speed_kmh
+    si, sf = bracket(int(lookup_speed * 256) & 0xFFFF, self.speed_bp)
+    self.c3 = slew(self.c3, c3, c.f(0xFEF259E8 if active else 0xFEF259EC))
+    c2_target = c2 + interp(self.tables[0xFEF25BE8], si, sf) * c3
+    self.c2 = slew(self.c2, c2_target, c.f(0xFEF259E4 if active else 0xFEF259F0))
+    if interaction < c.f(0xFEF259F4):
+      self.fast = True
+    elif abs(c0) <= c.f(0xFEF25A00) and abs(c1) <= c.f(0xFEF25A10):
+      self.fast = False
+    c1rate = c.f(0xFEF25A14 if not active else (0xFEF25A0C if self.fast else 0xFEF25A08))
+    c0rate = c.f(0xFEF25A04 if not active else (0xFEF259FC if self.fast else 0xFEF259F8))
+    self.c1 = slew(self.c1, c1, c1rate)
+    self.c0 = slew(self.c0, c0, c0rate)
+    # 181270: EDL C0/time derivative and mode shortening gains are zero.
+    target_t = clip(c.f(0xFEF25A58) - c.f(0xFEF2596C) * abs(self.c2) - (c.f(0xFEF259A0) if indicator else 0), c.f(0xFEF2599C), c.f(0xFEF25A58))
+    self.preview_t += clip(target_t - self.preview_t, c.f(0xFEF259A4) * DT, c.f(0xFEF259A8) * DT)
+    lookup_interp = interp_f32 if self.native_lookup else interp
+    distance = clip(
+      f32(f32(lookup_speed * f32(0.28)) * f32(self.preview_t)) if self.native_lookup else speed_kmh * 0.28 * self.preview_t,
+      lookup_interp(self.tables[0xFEF25BB8], si, sf),
+      lookup_interp(self.tables[0xFEF25B98], si, sf),
+    )
+    di, df = (bracket_f32 if self.native_lookup else bracket)(distance, self.distance_bp)
+    g0 = lookup_interp(self.tables[0xFEF25DE8], di, df)
+    g1 = lookup_interp(self.tables[0xFEF25DC0], di, df)
+    g3 = lookup_interp(self.tables[0xFEF25D70], di, df)
+    opposite = abs(self.c0) > c.h(0xFEF25A70) / 8192 and abs(self.c0_i) > c.h(0xFEF25A72) / 8192 and self.c0 * self.c0_i < 0
+    forced_release = special_release or opposite
+    if active and not release_i and not forced_release:
+      delta = 0 if freeze_i else clip(self.c0, c.f(0xFEF25968), c.f(0xFEF25960)) * interp(self.tables[0xFEF25B78], si, sf)
+      self.c0_i = clip(self.c0_i + delta * DT, c.f(0xFEF25964), c.f(0xFEF2595C))
+    else:
+      rate = (c.h(0xFEF25A74) if special_release else interp_int(self.tables[0xFEF25C60], si, sf)) if forced_release else c.h(0xFEF25A60)
+      self.c0_i = slew(self.c0_i, 0, rate / 512)
+    req = g3 * self.c3 + g1 * self.c1 + g0 * (self.c0 + self.c0_i) + self.c2
+
+    def contribution(lo, hi, x):
+      return clip(abs(x) - lo, 0, max(1e-6, hi - lo)) / max(1e-6, hi - lo)
+
+    v = speed_kmh * 0.28
+    heading_weight = contribution(c.b(0xFEF25AA5) / 128, c.b(0xFEF25AA6) / 128, self.c1 * v)
+    curvature_weight = contribution(c.b(0xFEF25AA1) / 64, c.b(0xFEF25AA2) / 64, self.c2 * v * v)
+    rate_weight = contribution(c.b(0xFEF25A9E) / 32, c.b(0xFEF25A9F) / 32, self.c3 * v * v * v)
+    alpha = min(
+      c.b(0xFEF25AA4) / 256,
+      (interp_int(self.tables[0xFEF258FC], si, sf) + c.b(0xFEF25AA7) * heading_weight + c.b(0xFEF25AA3) * curvature_weight + c.b(0xFEF25AA0) * rate_weight)
+      / 256,
+    )
+    self.filtered += alpha * (req - self.filtered)
+    return {
+      'held_c0': self.c0,
+      'held_c1': self.c1,
+      'held_c2': self.c2,
+      'held_c3': self.c3,
+      'integral_c0': self.c0_i,
+      'request_curvature': req,
+      'filtered_curvature': self.filtered,
+      'preview_distance': distance,
+      'preview_time': self.preview_t,
+      'fast_flag': int(self.fast),
+      'speed_index': si,
+      'speed_fraction': sf,
+      'alpha': alpha,
+      'g0': g0,
+      'g1': g1,
+      'accel_request': self.filtered * (speed_kmh / 3.6) ** 2,
+      'accel_bound': interp_int(self.tables[0xFEF25E20], si, sf) / 8192,
+      'c0_contribution': g0 * self.c0,
+      'c1_contribution': g1 * self.c1,
+      'c0_i_contribution': g0 * self.c0_i,
+    }

openpilot/selfdrive/controls/lib/ford_model_action.py

@@ -206,7 +206,11 @@ class FordModelActionController:
   neither a limit nor a repeated measurement freezes the model request.
   """
   def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False,
-               c0_proportional_gain=None, direct_path=False):
+               c0_proportional_gain=None, direct_path=False, joint_control=False):
+    self.joint_control = bool(joint_control and not direct_path)
+    if self.joint_control:
+      proportional_gain = integral_gain = c0_proportional_gain = 0.
+      c0_time_based = False
     if c0_proportional_gain is None:
       c0_proportional_gain = DIRECT_PATH_C0_PROPORTIONAL_GAIN if direct_path else C0_PROPORTIONAL_GAIN
     self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based,
@@ -216,6 +220,8 @@ class FordModelActionController:
     self.request_buffer = deque([0.] * self.request_buffer_size, maxlen=self.request_buffer_size)
     self.hypothesis = ('model-path-direct-feedback-v23' if self.direct_path else
                        'model-action-curvature-c0-feedback-v23-soft-c0-c1')
+    if self.joint_control:
+      self.hypothesis = 'model-action-joint-reference-v24'
     self.reset()
 
   def path_curvature(self, model, speed):
@@ -225,7 +231,7 @@ class FordModelActionController:
 
   def set_c0_time_based(self, enabled, *, lateral_engaged):
     """Apply a distance change only after lateral assistance is disengaged."""
-    if lateral_engaged or self.core.c0_time_based == bool(enabled):
+    if self.joint_control or lateral_engaged or self.core.c0_time_based == bool(enabled):
       return False
     self.core.c0_time_based = bool(enabled)
     self.reset('c0_distance_changed')
@@ -312,13 +318,18 @@ class FordModelActionController:
                         'feedback_error': self.core.feedback_curvature-current_curvature,
                         'driver_override': driver_override, 'pscm_limited': pscm_limited, 'pscm_status_fresh': bool(status_fresh),
                         'command': (command.path_offset, command.path_angle, 0., 0.)}
+    if self.joint_control:
+      # card receives the normal desired steering angle and owns allocation at
+      # the transmit boundary. This path carries validity, never a second PI.
+      self.diagnostics['command_stage'] = 'reference_only'
+      return FordPath(valid=True)
     return command
 
 
-def select_model_action_controller(CP, enabled, *, c0_time_based=False, direct_path=False):
+def select_model_action_controller(CP, enabled, *, c0_time_based=False, direct_path=False, joint_control=False):
   """Only opt-in Ford CAN FD vehicles override upstream curvature control."""
   compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD
   if enabled and compatible:
     return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN,
-                                     c0_time_based=c0_time_based, direct_path=direct_path)
+                                     c0_time_based=c0_time_based, direct_path=direct_path, joint_control=joint_control)
   return None

openpilot/selfdrive/ui/sunnypilot/mici/layouts/toggles.py

@@ -21,6 +21,7 @@ class TogglesLayoutMiciSP(TogglesLayoutMici):
     super()._update_toggles()
     cp = ui_state.CP
     visible = bool(cp is not None and cp.brand == 'ford' and cp.flags & FordFlags.CANFD
-                   and ui_state.params.get_bool('FordModelActionController'))
+                   and ui_state.params.get_bool('FordModelActionController') and
+                   (not ui_state.params.get_bool('FordPscmJointControl') or ui_state.params.get_bool('FordGeometryReference')))
     self._ford_c0_toggle.set_visible(visible)
     self._ford_c0_help.set_visible(visible)

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -2208,6 +2208,29 @@
               "equals": true
             }
           ]
+        },
+        {
+          "key": "FordPscmJointControl",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Coordinated C0/C1 Steering (Experimental)",
+          "description": "Coordinate steering entry and release using an estimated Ford steering-controller response.",
+          "details": "Uses the normal model-action steering target. Requires Selected-Action Path Tracking on and Model Geometry Reference off. Default off; turning this option off restores the existing selected-action controller. Turning Selected-Action Path Tracking off restores upstream Ford control. Changes apply after an offroad-to-onroad cycle. This trial uses a response reconstructed from older Ford firmware and has only been tested offline. C0 distance selection does not apply to this trial.",
+          "enablement": [
+            {
+              "type": "offroad_only"
+            },
+            {
+              "type": "param",
+              "key": "FordModelActionController",
+              "equals": true
+            },
+            {
+              "type": "param",
+              "key": "FordGeometryReference",
+              "equals": false
+            }
+          ]
         }
       ]
     },

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -29,6 +29,20 @@ sections:
     - type: param
       key: FordModelActionController
       equals: true
+  - key: FordPscmJointControl
+    widget: toggle
+    needs_onroad_cycle: true
+    title: Coordinated C0/C1 Steering (Experimental)
+    description: Coordinate steering entry and release using an estimated Ford steering-controller response.
+    details: Uses the normal model-action steering target. Requires Selected-Action Path Tracking on and Model Geometry Reference off. Default off; turning this option off restores the existing selected-action controller. Turning Selected-Action Path Tracking off restores upstream Ford control. Changes apply after an offroad-to-onroad cycle. This trial uses a response reconstructed from older Ford firmware and has only been tested offline. C0 distance selection does not apply to this trial.
+    enablement:
+    - $ref: '#/macros/offroad'
+    - type: param
+      key: FordModelActionController
+      equals: true
+    - type: param
+      key: FordGeometryReference
+      equals: false
 - id: hyundai
   title: Hyundai / Kia / Genesis Settings
   description: ''

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -307,6 +307,16 @@ class TestKnownVehicleSettings(OpenpilotTestCase):
     with tempfile.TemporaryDirectory() as path:
       assert Params(path).get_default_value("FordGeometryReference") is False
 
+  def test_ford_joint_is_default_off_and_requires_action_mode(self, schema):
+    items = _brand_items(schema["vehicle_settings"].get("ford"))
+    item = next(item for item in items if item["key"] == "FordPscmJointControl")
+    assert item["widget"] == "toggle" and item["needs_onroad_cycle"] is True
+    assert item["enablement"] == [{"type": "offroad_only"},
+                                  {"type": "param", "key": "FordModelActionController", "equals": True},
+                                  {"type": "param", "key": "FordGeometryReference", "equals": False}]
+    with tempfile.TemporaryDirectory() as path:
+      assert Params(path).get_default_value("FordPscmJointControl") is False
+
   def test_hyundai_has_longitudinal_tuning(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))}
     assert "HyundaiLongitudinalTuning" in keys