Ford: coordinate endpoint buildup and retain stop requests through speed undershoot

docs/ford_joint_equal_arrival_trial.md

@@ -0,0 +1,39 @@
+# Ford coordinated allocation trial
+
+The existing `FordModelActionController` + `FordPscmJointControl` gate now uses
+the recovered normal C0/C1 slew rates to choose its steady endpoint split:
+
+```
+t = desired_internal_curvature / (g0 * c0_rate + g1 * c1_rate)
+c0 = t * c0_rate
+c1 = t * c1_rate
+```
+
+When a field clips, allocate its missing contribution to the other field's
+remaining capacity. The existing dynamic selector still handles held state,
+heading-dependent filtering, target preview and release. No new gain, control
+mode, search horizon or native-library ABI is added. DBC bounds and C2=C3=0 remain.
+The normal upstream controller is unchanged with the existing master toggle off.
+
+This is the exact endpoint policy tested against ML3V-14D003-BD /
+ML34-14D007-EDL instructions. At 18 mph, the approximately 204-degree internal
+target settles within 2.5 degrees in 2.864 rather than 3.384 seconds. The first
+90% of its rise is unchanged. The approximately 25-degree release takes .624
+rather than .456 seconds to settle: this is a known trial tradeoff, not a proven
+improvement in all driving. These are internal target results, not wheel-motion
+or closed-loop smoothness predictions. Actual steering feedback must assess both
+large turns and ordinary correction/release.
+
+For stopping, controlsd and card share the same normalization: a filtered speed
+between -0.3 and 0 m/s counts as zero only when raw speed is in [0, 0.3) m/s and
+reverse is not selected. This prevents small speed-filter undershoot from
+invalidating and clearing the last active packet. Larger/inconsistent negative
+speeds, reverse, stale inputs, faults, override and disengagement still release.
+No global CarState speed or PSCM speed message is altered. Retaining the request
+does not bypass the firmware's separate near-zero-speed output gate.
+
+Validation: 926 Ford tests and two subtests passed; lint and diff checks passed.
+The shipped encoder reproduces all 9,000 saved candidate command samples from
+the native-verified step/release fixture (maximum difference 8.89e-16). Native
+instruction validation and the underlying fixtures are recorded in the local
+PSCM authority report. Diagnostics include `allocation: equal-arrival`.

openpilot/selfdrive/car/ford_joint_control.py

@@ -7,6 +7,7 @@ It estimates older-firmware state; it cannot observe ECU RAM or prove acceptance
 import math
 
 from opendbc.car.ford.values import FordFlags
+from openpilot.selfdrive.controls.lib.ford_path import joint_control_speed
 
 # Small steering-wheel trim around the nominal inverse, in degrees. These are
 # trial tuning values, not recovered PSCM constants. Entry assist is separately gated.
@@ -102,18 +103,19 @@ class FordJointControl:
     self.advance(now)
     path = CC_SP.fordLateralPath
     target = float(CC.actuators.steeringAngleDeg)
+    speed_ms = joint_control_speed(CS)
     # controlsd publishes calibrated car-frame motion and gates the path on its
     # health/freshness. Raw Ford CAN yaw has a zero offset on the audited truck.
     # Calibrated Z is opposite the CAN/pinion sign used by the recovered model.
     yaw = -float(CC.angularVelocity[2]) if len(CC.angularVelocity) == 3 else math.nan
-    finite = all(math.isfinite(v) for v in (CS.vEgo, CS.steeringAngleDeg, CS.yawRate, yaw, target))
+    finite = all(math.isfinite(v) for v in (speed_ms, CS.steeringAngleDeg, CS.yawRate, yaw, target))
     if finite:
       # Ford does not populate CarState.steeringRateDeg. Derive motion from the
       # measured angle; 0.1 s filtering suppresses its 0.1-degree quantization.
       if 0.0 < dt <= 0.1:
         rate = (CS.steeringAngleDeg - self.measurement[1]) / dt
         self.wheel_rate += dt / (0.1 + dt) * (rate - self.wheel_rate)
-      self.measurement = (max(0.0, CS.vEgo * 3.6), CS.steeringAngleDeg, yaw)
+      self.measurement = (max(0.0, speed_ms * 3.6), CS.steeringAngleDeg, yaw)
     else:
       self.wheel_rate = 0.0
     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
@@ -124,7 +126,7 @@ class FordJointControl:
       fresh
       and CS.canValid
       and finite
-      and 0.0 <= CS.vEgo <= 55
+      and 0.0 <= speed_ms <= 55
       and abs(CS.yawRate) <= 3
       and abs(yaw) <= 3
       and path.enabled
@@ -142,7 +144,7 @@ class FordJointControl:
       self.requested_rate = 0.0
     command = (0.0, 0.0)
     details = {}
-    stop_hold = active and CS.vEgo < 0.3
+    stop_hold = active and speed_ms < 0.3
     if stop_hold:
       # The moving inverse divides by v**2. At a stop retain the last actual
       # active packet, not an inactive/zero request or a fictitious speed.
@@ -251,6 +253,7 @@ class FordJointControl:
       'driver_override': override,
       'driver_pressed': bool(CS.steeringPressed),
       'stop_hold': bool(stop_hold),
+      'allocation': 'equal-arrival',
       '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

@@ -0,0 +1,49 @@
+"""A field reaching its bound must not discard a jointly reachable target."""
+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.model import MainRequest
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_joint_encoder_preserves_large_reachable_target(sign):
+  request = MainRequest(native_lookup=True)
+  speed = 15 * 1.609344
+  inverse = invert_angle(AngleModel(request.cal), speed, sign * 280., 0., 0., 0., 3.7, 16.9)
+  assert not inverse['accel_limited']
+  probe = MainRequest(native_lookup=True).step(speed, 0., 0., freeze_i=True)
+  c0, c1 = static_pair(request, inverse['curvature'], speed)
+  assert abs(c1) <= C1_BOUND
+  assert abs(c0) <= C0_BOUND
+  assert probe['g0'] * c0 + probe['g1'] * c1 == pytest.approx(inverse['curvature'], abs=1e-12)
+
+  _, info = PairedRelease(request).choose(speed, inverse['curvature'])
+  # The planned target is the CAN-quantized steady pair, not next-tick wheel motion.
+  quantization_error = probe['g0'] * .005 + probe['g1'] * .00025
+  assert abs(info['planned_target'] - inverse['curvature']) <= quantization_error + 1e-12
+
+
+@pytest.mark.parametrize('speed', [8., 15., 19.312128, 24.14016, 28.968192, 50., 100., 180.])
+@pytest.mark.parametrize('fraction', [-2., -1., -.8, -.3, 0., .3, .8, 1., 2.])
+def test_static_allocation_uses_available_combined_field_range(speed, fraction):
+  request = MainRequest(native_lookup=True)
+  probe = request.step(speed, 0., 0., freeze_i=True)
+  g0, g1 = probe['g0'], probe['g1']
+  capacity = g0 * C0_BOUND + g1 * C1_BOUND
+  c0, c1 = static_pair(request, fraction * capacity, speed)
+  assert abs(c0) <= C0_BOUND and abs(c1) <= C1_BOUND
+  expected = max(-capacity, min(capacity, fraction * capacity))
+  assert g0 * c0 + g1 * c1 == pytest.approx(expected, abs=1e-12)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_unsaturated_pair_reaches_both_endpoints_together(sign):
+  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)
+  c0, c1 = static_pair(request, inverse['curvature'], speed)
+  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'])

openpilot/selfdrive/car/tests/test_ford_joint_control.py

@@ -133,6 +133,28 @@ def test_stop_repeats_transmitted_request_and_resume_keeps_target(sign):
   assert not p.joint.fault
 
 
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_stop_speed_filter_undershoot_keeps_actual_transmitted_hold(sign):
+  p = Pipeline()
+  p.cs.gearShifter = structs.CarState.GearShifter.drive
+  for i in range(30):
+    p.tick(1+i*.01, sign*120.)
+  sent = p.joint.sent
+  assert sent[2] and any(abs(v) > .01 for v in sent[:2])
+  p.cs.vEgoRaw, p.cs.standstill = 0., True
+  for i, speed in enumerate([.01, 0., -.01, -.04, -.01, 0., .01, .29]):
+    p.cs.vEgo = speed
+    assert p.tick(1.3+i*.01, sign*120.).latActive
+    assert p.joint.sent == sent
+    assert p.joint.diagnostics['stop_hold']
+  p.cs.vEgo = p.cs.vEgoRaw = .3
+  assert p.tick(1.38, sign*120.).latActive
+  assert p.joint.sent[0]*sign > 0 and p.joint.sent[1]*sign > 0
+  p.cs.gearShifter = structs.CarState.GearShifter.reverse
+  assert not p.tick(1.39, sign*120.).latActive
+  assert p.joint.sent == (0., 0., False)
+
+
 @pytest.mark.parametrize('failure', ['disengage', 'stale', 'can', 'steering_fault', 'override', 'denied'])
 def test_stopped_hold_still_releases_and_does_not_resurrect_old_command(failure):
   p = Pipeline()
@@ -451,16 +473,18 @@ 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), 5.6554469893989605),
-  (20., 3., .4, .03, False, -.03, 2, (2.98, .399), 151.49476621872228),
+  (20., 0., 0., 0., False, .03, 0, (.03, .002), 4.719272529061328),
+  (20., 3., .4, .03, False, -.03, 2, (2.98, .399), 142.8506441410823),
   (40., 3., .4, .03, True, 0., 0, (2.95, .395), 12.167867914611671),
-  (6., 5.11, .5, .1, False, -.1, 0, (5.08, .498), 1391.5462859829215),
-  (80., -5.11, -.5, -.1, True, .03, 2, (-5.08, -.4975), 34.51646821335084),
+  (6., 5.11, .5, .1, False, -.1, 0, (5.08, .498), 1371.556512359005),
+  (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),
 ])
-def test_optimized_selection_preserves_v24_commands(speed, c0, c1, filtered, fast, target, phase, pair, cost):
+def test_optimized_selection_matches_frozen_cases(speed, c0, c1, filtered, fast, target, phase, pair, cost):
   # 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.
   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/controlsd.py

@@ -180,7 +180,10 @@ class Controls(ControlsExt):
         reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
         now = time.monotonic()
         yaw_rate, motion_valid = -CS.yawRate, True
+        ford_speed = CS.vEgo
         if self.ford_path_controller.joint_control:
+          from openpilot.selfdrive.controls.lib.ford_path import joint_control_speed
+          ford_speed = joint_control_speed(CS)
           # card's inverse uses the calibrated yaw published in carControl.
           # Never keep steering from a stale pose or fall back to raw CAN yaw.
           motion = self.sm['deviceMotion']
@@ -190,7 +193,7 @@ class Controls(ControlsExt):
                           and -0.005 <= now - self.sm.logMonoTime['deviceMotion'] * 1e-9 <= 0.15)
           yaw_rate = float(self.calibrated_pose.angular_velocity.xyz[2]) if motion_valid else math.nan
         self.ford_path = self.ford_path_controller.update(
-          ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=yaw_rate, speed=CS.vEgo, now=now,
+          ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=yaw_rate, speed=ford_speed, now=now,
           # Roll/angle offset cancel in the error; retain the normal steering-angle conversion's speed and stiffness effects.
           curvature_scale=self.VM.get_steer_from_curvature(1., CS.vEgo, 0.) / (self.CP.steerRatio*self.CP.wheelbase),
           measurement_time=self.sm.logMonoTime['carState'] * 1e-9,

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

@@ -55,25 +55,24 @@ def invert_angle(output, speed_kmh, target_angle, angle, yaw, accel, wheelbase,
   }
 
 
-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, *, gains=None):
-  """Preserve the base's channel proportion, solve its static curvature sum."""
+  """Allocate equal nominal buildup times, 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']
-  p0, p1 = arc_pair(curvature, speed_kmh)
-  total = gains[0] * p0 + gains[1] * p1
-  scale = curvature / total if total else 1.0
-  return clip(p0 * scale, -C0_BOUND, C0_BOUND), clip(p1 * scale, -C1_BOUND, C1_BOUND)
+  # Recovered normal held-input rates; the dynamic selector still accounts for
+  # current held states, the fast latch, heading-dependent filter and release.
+  r0, r1 = request.cal.f(0xFEF259F8), request.cal.f(0xFEF25A08)
+  duration = curvature / (gains[0] * r0 + gains[1] * r1)
+  p0, p1 = duration * r0, duration * r1
+  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.
+    if gains[0]:
+      c0 = clip((curvature - gains[1] * c1) / gains[0], -C0_BOUND, C0_BOUND)
+    if gains[1]:
+      c1 = clip((curvature - gains[0] * c0) / gains[1], -C1_BOUND, C1_BOUND)
+  return c0, c1
 
 
 def quantize(pair):

openpilot/selfdrive/controls/lib/ford_path.py

@@ -5,6 +5,7 @@ import math
 import numpy as np
 
 from opendbc.car.ford.values import CarControllerParams
+from opendbc.car.structs import CarState
 
 
 DBC_OFFSET = (-5.12, 5.11)
@@ -41,6 +42,15 @@ class FordPath:
   curvature_rate: float = 0.0
 
 
+def joint_control_speed(CS):
+  """Treat small speed-filter undershoot as stopped only near raw standstill."""
+  if CS.gearShifter == CarState.GearShifter.reverse:
+    return math.nan
+  if -0.3 <= CS.vEgo < 0.0 and 0.0 <= CS.vEgoRaw < 0.3:
+    return 0.0
+  return CS.vEgo
+
+
 @dataclass(frozen=True)
 class FordPscmState:
   path_offset: float = 0.0

openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py

@@ -269,7 +269,8 @@ def test_joint_calibrated_motion_gate_only_affects_joint_mode(pipeline, joint, f
     controls.calibrated_pose.angular_velocity.xyz[2] = math.nan if failure == 'nan' else 3.1
 
   cc = structs.CarControl(latActive=True)
-  cs = SimpleNamespace(vEgo=20., yawRate=-.028, canValid=True, steeringPressed=False, steeringTorque=0.)
+  cs = SimpleNamespace(vEgo=20., vEgoRaw=20., gearShifter=structs.CarState.GearShifter.drive,
+                       yawRate=-.028, canValid=True, steeringPressed=False, steeringTorque=0.)
   model = straight()
   model.action = SimpleNamespace(desiredCurvature=.001)
   exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
@@ -281,6 +282,34 @@ def test_joint_calibrated_motion_gate_only_affects_joint_mode(pipeline, joint, f
   assert msg.fordLateralPath.valid == cc.latActive
 
 
+@pytest.mark.parametrize('speed,raw,reverse,expected', [
+  (-.01, 0., False, True), (-.29, .1, False, True), (0., 0., False, True),
+  (-.31, 0., False, False), (-.01, 1., False, False), (-.01, math.nan, False, False),
+  (-.01, 0., True, False), (.1, .1, True, False),
+])
+def test_joint_controlsd_stop_speed_undershoot(pipeline, speed, raw, reverse, expected):
+  settings = {'FordModelActionController', 'FordPscmJointControl'}
+  controls = startup(params=SimpleNamespace(get_bool=lambda key: key in settings))
+  sm = Subscriptions(False)
+  controls.sm, controls.desired_curvature, controls.curvature = sm, 0., 0.
+  controls.calibrated_pose = SimpleNamespace(angular_velocity=SimpleNamespace(xyz=[0., 0., 0.]))
+  controls.pose_calibrator = SimpleNamespace(calib_valid=True)
+  sm.messages['deviceMotion'] = SimpleNamespace(inputsOK=True, sensorsOK=True, angularVelocityDevice=SimpleNamespace(valid=True))
+  sm.logMonoTime['deviceMotion'] = 980_000_000
+  cs = structs.CarState(vEgo=speed, vEgoRaw=raw, canValid=True, standstill=True,
+                        gearShifter=structs.CarState.GearShifter.reverse if reverse else structs.CarState.GearShifter.drive)
+  cc = structs.CarControl(latActive=True)
+  model = straight()
+  model.action = SimpleNamespace(desiredCurvature=.001)
+  exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
+                    'lp': SimpleNamespace(roll=0.), 'math': math, 'clip_curvature': clip_curvature,
+                    'time': SimpleNamespace(monotonic=lambda: 1.)})
+  assert cc.latActive == controls.ford_path.valid == expected
+  msg = custom.CarControlSP.new_message()
+  exec(pipeline[1], {'self': controls, 'CC_SP': msg})
+  assert msg.fordLateralPath.valid == cc.latActive
+
+
 @pytest.mark.parametrize('sign', [-1., 1.])
 def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline, sign):
   call, publication = pipeline