Ford: hold joint endpoint C0 at the supervisor saturation

The equal-arrival endpoint held 2-4 m of C0 in large turns. In the reference
ML3V/ML34 calibration the path supervisor's C0 term clip(0.5*C0, +-0.5)
saturates at 1.0 m, so that extra C0 added primary curvature but no
turn-direction allowance, while on release the supervisor kept excluding
zero demand until held C0 slewed (1.5 m/s) back below about 0.41 m.

Cap the static endpoint's C0 at 1.0 m and give the rest to C1, returning to
C0 only at C1's bound. The paired selector still builds with both fields,
then trades C0 for C1 at constant curvature before a release is needed.
Endpoints whose equal-arrival C0 is at most 1.0 m are unchanged.

Offline evidence on reference-firmware instructions (no wheel, assist or
vehicle model): zero demand excluded after release 1.48/1.88/2.17 s ->
0.39/0.39/0.48 s for large 15/20/25 km/h steps with an unchanged 50%/90%
rise; replayed routes 194-196 cut forced old-direction output after large
releases by 50-75%; packets away from large turns are identical. Three
frozen selector costs are refreshed; their selected packets are unchanged.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

docs/ford_joint_supervisor_endpoint.md

@@ -0,0 +1,38 @@
+# Ford joint endpoint: hold C0 at the supervisor saturation
+
+The equal-arrival endpoint (`C0 = 1.5 t`, `C1 = 0.1 t`) holds 2-4 m of C0 in large
+turns. In the reference ML3V-14D003-BD / ML34-14D007-EDL calibration the path
+supervisor centre is
+
+```
+S(v) * (clip(0.5 * held_C0, +-0.5) + clip(10 * held_C1, +-0.349609))
+```
+
+with width `0.399994 * S + 5` below 40 km/h. Its C0 term saturates at 1.0 m, so
+held C0 above that adds primary curvature but no turn-direction allowance. On
+release, zero demand stays excluded until held C0 slews (1.5 m/s) below about
+0.41 m while C1 is saturated, so extra C0 adds (C0 - 0.41) / 1.5 s of forced
+old-direction output.
+
+`static_pair` therefore caps its equal-arrival C0 at 1.0 m and gives the rest to
+C1; the remainder returns to C0 only when C1 reaches its bound. Endpoints with
+equal-arrival C0 of at most 1.0 m are unchanged. The paired selector, dynamics,
+quantization, preview, gates, stop hold and DBC bounds are unchanged; it still
+builds with both fields, then trades C0 for C1 at constant curvature.
+
+Offline evidence (reference firmware, not the live RL38 calibration; no wheel,
+assist or vehicle model):
+
+- Native zero-demand supervisor probe, controlled 15/20/25 km/h large steps:
+  zero excluded after release 1.48/1.88/2.17 s -> 0.39/0.39/0.48 s. The first
+  50%/90% of the rise is unchanged; the last 1% can settle up to 0.5 s later.
+- Native probe on replayed routes 194/195/196 packets: forced old-direction
+  output in the 3 s after large releases 24.9 -> 6.4, 19.6 -> 8.4,
+  15.0 -> 3.6, 16.6 -> 8.3, 11.0 -> 4.4 unit*s (one driver-involved release
+  9.6 -> 9.2).
+- 81,340 replay updates per policy: with no |target| >= 30 deg within 10 s,
+  packets are identical on 194/196 and 98.9% identical on 195.
+
+These are firmware-stage predictions. A physical A/B must show whether the wheel
+releases large turns earlier; diagnostics report `allocation:
+equal-arrival-supervisor-c0`.

openpilot/selfdrive/car/ford_joint_control.py

@@ -253,7 +253,7 @@ class FordJointControl:
       'driver_override': override,
       'driver_pressed': bool(CS.steeringPressed),
       'stop_hold': bool(stop_hold),
-      'allocation': 'equal-arrival',
+      'allocation': 'equal-arrival-supervisor-c0',
       'yaw_source': 'calibrated_pose',
       'yaw_rate': yaw if math.isfinite(yaw) else None,
       'can_yaw_rate': float(CS.yawRate) if math.isfinite(CS.yawRate) else None,

openpilot/selfdrive/car/tests/test_ford_joint_allocation.py

@@ -3,7 +3,7 @@ import pytest
 
 from openpilot.selfdrive.controls.lib.ford_joint.angle import AngleModel
 from openpilot.selfdrive.controls.lib.ford_joint.encoder import PairedRelease
-from openpilot.selfdrive.controls.lib.ford_joint.inverse import C0_BOUND, C1_BOUND, invert_angle, static_pair
+from openpilot.selfdrive.controls.lib.ford_joint.inverse import C0_BOUND, C0_SUPERVISOR_SATURATION, C1_BOUND, invert_angle, static_pair
 from openpilot.selfdrive.controls.lib.ford_joint.model import MainRequest
 
 
@@ -40,10 +40,64 @@ def test_static_allocation_uses_available_combined_field_range(speed, fraction):
 
 @pytest.mark.parametrize('sign', [-1., 1.])
 def test_unsaturated_pair_reaches_both_endpoints_together(sign):
+  # Below the supervisor's C0 saturation the endpoint is still equal arrival.
   request = MainRequest(native_lookup=True)
   speed = 15 * 1.609344
-  inverse = invert_angle(AngleModel(request.cal), speed, sign * 180., 0., 0., 0., 3.7, 16.9)
+  inverse = invert_angle(AngleModel(request.cal), speed, sign * 40., 0., 0., 0., 3.7, 16.9)
   c0, c1 = static_pair(request, inverse['curvature'], speed)
+  assert abs(c0) < C0_SUPERVISOR_SATURATION
   assert c0 / request.cal.f(0xFEF259F8) == pytest.approx(c1 / request.cal.f(0xFEF25A08))
   probe = request.step(speed, 0., 0., freeze_i=True)
   assert probe['g0'] * c0 + probe['g1'] * c1 == pytest.approx(inverse['curvature'])
+
+
+@pytest.mark.parametrize('speed', [8., 15., 19.312128, 24.14016, 28.968192, 50., 100., 180.])
+@pytest.mark.parametrize('fraction', [.05, .2, .4, .6, .8, .95, 1.])
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_endpoint_holds_c0_at_supervisor_saturation(speed, fraction, sign):
+  request = MainRequest(native_lookup=True)
+  probe = request.step(speed, 0., 0., freeze_i=True)
+  g0, g1 = probe['g0'], probe['g1']
+  curvature = sign * fraction * (g0 * C0_BOUND + g1 * C1_BOUND)
+  c0, c1 = static_pair(request, curvature, speed)
+  assert g0 * c0 + g1 * c1 == pytest.approx(curvature, abs=1e-12)
+  # Held C0 exceeds the supervisor saturation only when C1 alone cannot carry the rest.
+  assert abs(c0) <= C0_SUPERVISOR_SATURATION + 1e-12 or abs(c1) == C1_BOUND
+  assert static_pair(request, -curvature, speed) == pytest.approx((-c0, -c1), abs=1e-15)
+
+
+def _equal_arrival(request, curvature, speed_kmh, *, gains):
+  r0, r1 = request.cal.f(0xFEF259F8), request.cal.f(0xFEF25A08)
+  duration = curvature / (gains[0] * r0 + gains[1] * r1)
+  return duration * r0, duration * r1
+
+
+@pytest.mark.parametrize('speed,curvature', [(15., .06), (20., .05), (25., .04)])
+def test_capped_endpoint_keeps_entry_and_parks_c0_before_release(monkeypatch, speed, curvature):
+  """Production selector, quantized packets: same rise, less held C0 at the end of the hold."""
+  from openpilot.selfdrive.controls.lib.ford_joint import encoder
+  from openpilot.selfdrive.controls.lib.ford_joint.inverse import quantize
+
+  def run():
+    m = MainRequest(native_lookup=True)
+    selector = PairedRelease(m)
+    sent, phase, rise = (0., 0.), 0., None
+    for i in range(500):  # 5 s step-and-hold at 100 Hz
+      phase += .01
+      ticks = int((phase + 1e-12) / .008)
+      phase -= ticks * .008
+      for _ in range(ticks):
+        m.step(speed, *sent, freeze_i=True)
+      if rise is None and m.filtered >= .9 * curvature:
+        rise = i * .01
+      count = max(1, min(2, int((phase + .01 + 1e-12) / .008)))
+      command, _ = selector.choose(speed, curvature, phase=0 if count == 2 else 2)
+      sent = quantize(command)
+    return rise, m.c0, m.filtered
+
+  capped = run()
+  monkeypatch.setattr(encoder, 'static_pair', _equal_arrival)
+  equal = run()
+  assert capped[0] == pytest.approx(equal[0], abs=.02)
+  assert capped[2] == pytest.approx(curvature, rel=.01)
+  assert abs(capped[1]) <= C0_SUPERVISOR_SATURATION + .15 < abs(equal[1])

openpilot/selfdrive/car/tests/test_ford_joint_control.py

@@ -473,10 +473,10 @@ def test_candidate_does_not_mutate_state_and_native_step_matches_python():
 
 
 @pytest.mark.parametrize('speed,c0,c1,filtered,fast,target,phase,pair,cost', [
-  (20., 0., 0., 0., False, .03, 0, (.03, .002), 4.719272529061328),
-  (20., 3., .4, .03, False, -.03, 2, (2.98, .399), 142.8506441410823),
+  (20., 0., 0., 0., False, .03, 0, (.03, .002), 5.206693787207076),
+  (20., 3., .4, .03, False, -.03, 2, (2.98, .399), 149.0975016923494),
   (40., 3., .4, .03, True, 0., 0, (2.95, .395), 12.167867914611671),
-  (6., 5.11, .5, .1, False, -.1, 0, (5.08, .498), 1371.556512359005),
+  (6., 5.11, .5, .1, False, -.1, 0, (5.08, .498), 1423.8735141422555),
   (80., -5.11, -.5, -.1, True, .03, 2, (-5.08, -.4975), 39.42744549117561),
   (40., 3., -.4, 0., False, 0., 2, (3.02, -.399), 6.184073685045232),
 ])
@@ -484,7 +484,8 @@ def test_optimized_selection_matches_frozen_cases(speed, c0, c1, filtered, fast,
   # Frozen outputs from 528ed3615 before pruning/caching the full-return search.
   # Cover entry, reversal, release, saturation, cancellation and both tick counts.
   # Costs refreshed for the equal-buildup endpoint and residual allocation at
-  # field bounds. All six immediate selected packets remain unchanged.
+  # field bounds, then for the supervisor-saturation C0 endpoint (its return
+  # path differs). All six immediate selected packets remain unchanged.
   m = MainRequest(native_lookup=True)
   m.c0, m.c1, m.filtered, m.fast = c0, c1, filtered, fast
   command, info = PairedRelease(m).choose(speed, target, phase)

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

@@ -7,6 +7,11 @@ from openpilot.selfdrive.controls.lib.ford_joint.model import clip, interp_int,
 
 C0_BOUND = 5.11
 C1_BOUND = 0.5
+# Reference-calibration supervisor centre term clip(0.5 * held C0, +-0.5) saturates
+# here. Held C0 beyond it adds primary curvature but no turn-direction supervisor
+# allowance, and on release the supervisor keeps excluding zero demand until held
+# C0 slews (1.5 m/s) back below about 0.41 m.
+C0_SUPERVISOR_SATURATION = 1.0
 
 
 def invert_angle(output, speed_kmh, target_angle, angle, yaw, accel, wheelbase, ratio, accel_allowance=3.0):
@@ -56,7 +61,8 @@ def invert_angle(output, speed_kmh, target_angle, angle, yaw, accel, wheelbase,
 
 
 def static_pair(request, curvature, speed_kmh, *, gains=None):
-  """Allocate equal nominal buildup times, then use remaining field capacity."""
+  """Allocate equal nominal buildup times, hold C0 at most at the supervisor's C0
+  saturation, then use remaining field capacity."""
   if gains is None:
     probe = copy.copy(request).step(speed_kmh, request.c0, request.c1, freeze_i=True)
     gains = probe['g0'], probe['g1']
@@ -65,6 +71,11 @@ def static_pair(request, curvature, speed_kmh, *, gains=None):
   r0, r1 = request.cal.f(0xFEF259F8), request.cal.f(0xFEF25A08)
   duration = curvature / (gains[0] * r0 + gains[1] * r1)
   p0, p1 = duration * r0, duration * r1
+  if abs(p0) > C0_SUPERVISOR_SATURATION and gains[1]:
+    # Only the held endpoint moves: the selector still builds with both fields,
+    # then trades C0 for C1 at constant curvature before a release is needed.
+    p0 = math.copysign(C0_SUPERVISOR_SATURATION, p0)
+    p1 = (curvature - gains[0] * p0) / gains[1]
   c0, c1 = clip(p0, -C0_BOUND, C0_BOUND), clip(p1, -C1_BOUND, C1_BOUND)
   if abs(p0) > C0_BOUND or abs(p1) > C1_BOUND:
     # Clipping one field must not silently lower a target the pair can encode.