Ford: experiment with earlier C1 release from model path shape

Bound the model heading request toward zero using terminal spatial
curvature at the existing preview point. Preserve C0, command limits,
slew, 20 Hz cadence, and the existing Sunnylink selection.

This changes requests before some turn peaks as well as during unwind.
Document that authority tradeoff and distinguish command-level evidence
from unverified physical PSCM release.

Validation: Ford controller/sender/safety tests, Sunnylink schema tests,
independent geometry and replay tests, 28,138 stress CAN round trips,
and 35,775 recorded cycles with 71,550 baseline/candidate CAN checks.

Assisted-by: OpenAI Codex

docs/ford_model_action_drive_test.md

@@ -1,21 +1,22 @@
 # Ford model-point drive-test branch
 
 The default-off **Selected-Action Path Tracking (Experimental)** Sunnylink toggle
-now selects the v7 model-point candidate on the **Ford CAN FD F-150 Lightning**.
+now selects the C1 early-release candidate on the **Ford CAN FD F-150 Lightning**.
 The stored key remains `FordModelActionController`; an already enabled setting
 selects this revision after updating and completing an offroad-to-onroad cycle.
 
 The controller reads **model lateral position and model heading at the same
 point**. Start with the model's predicted distance at one second, enforce the
 existing seven-metre minimum, and hold the available endpoint when necessary.
-C0 is that point's lateral position in metres; C1 is its unwrapped heading in
-radians. C2 and C3 stay zero. The 150 ms yaw-based forecast and the reconstruction
-of heading from selected curvature are removed.
+C0 is that point's lateral position in metres. C1 starts from its unwrapped
+heading and is bounded toward zero using the model's terminal spatial curvature
+at the same point. This asks for earlier release when the path straightens
+ahead. C2 and C3 stay zero. See [the exact rule and its authority tradeoff](ford_model_release.md).
 
 This point choice is an engineering guess, not an identified Ford reference or
 PSCM calibration. `calibration_approved=false`: offline tests do not establish
 physical tracking, turn-exit behavior, or stability across different PSCMs.
-See [the model-point decision and validation](ford_model_points.md).
+The [v7 model-point decision and validation](ford_model_points.md) is historical.
 
 ## Select and restore
 
@@ -29,9 +30,10 @@ See [the model-point decision and validation](ford_model_points.md).
 
 The startup event `Ford path controller selected` reports
 `FordModelActionController`. Periodic `Ford C2-free path tracking` events report
-`hypothesis=model-pose-one-second-v7`, `pose_source=model`, `preview_time_s=1.0`
+`hypothesis=model-pose-terminal-c1-v1`, `pose_source=model`, `preview_time_s=1.0`
 and `minimum_station_m=7.0`, plus input ages, slew state and the command tuple.
-Selected desired curvature is still logged, but no longer constructs C0/C1.
+Active diagnostics also report `c1_release=terminal_spatial_curvature`.
+Selected desired curvature is still logged, but does not construct C0/C1.
 
 Turning the toggle off and completing another offroad-to-onroad cycle restores
 **PSCM Coefficient Observer** if selected, otherwise the original Ford path
@@ -53,7 +55,7 @@ candidate at 20 Hz. Float32 publication, host-to-wire negation, packing and
 Panda safety are unchanged. The opendbc pin remains
 `87ca78e6e641eefb2d654f260a6ab08df3058bd5`.
 
-Normal operation uses the model's point directly. The upstream scalar curvature
+Normal operation uses the model point with the one-sided C1 release bound. The upstream scalar curvature
 and its clipping still exist for logging/other controllers, but no longer bound
 this candidate's heading target. Its C0/C1 field caps and slew still apply;
 passing Panda TX checks does not establish an actual vehicle acceleration bound.
@@ -73,7 +75,7 @@ python -m pytest -q openpilot/selfdrive/controls/tests/test_ford*.py tools/ford_
 python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260908 --opendbc-revision 87ca78e6e641eefb2d654f260a6ab08df3058bd5 --output .cache/ford_model_points/stress.json
 ```
 
-Historical v1–v6 validation files retain their original source hashes and apply
+Historical v1–v7 validation files retain their original source hashes and apply
 to those revisions. In particular, `ford_model_action_measured_pose_validation.json`
 describes v6, not the current model-point mapping. The hardware build and device
 boot are not performed by these offline checks. Pushing a branch does not update

docs/ford_model_points.md

@@ -1,5 +1,9 @@
 # Ford model-point candidate v7
 
+This is the historical baseline at `4bd841ecc`. The driving branch now uses
+the [C1 early-release experiment](ford_model_release.md). Results below remain
+bound to their original sources and do not validate the newer request mapping.
+
 ## Decision
 
 Use the model's own position and orientation at one shared path point. This

docs/ford_model_release.md

@@ -0,0 +1,101 @@
+# Ford C1 early-release experiment
+
+The a9 recording contains holds followed by rapid steering release while C0/C1
+requests were already decreasing. Several occur without a PSCM limit flag or
+detected driver input. The experiment changes C1 alone to ask for less turning
+when the model's path straightens ahead. It does not establish which internal
+PSCM mechanism caused those holds.
+
+## Request mapping
+
+Keep the v7 sample station `s`: model arc distance at one second, with the
+existing seven-metre minimum and endpoint hold. Let `psi` be unwrapped model
+heading at that station and `psi0` its initial heading. Using the same model
+segment enclosing that point:
+
+```
+terminal_curvature = change in heading / change in arc distance
+release_heading = psi0 + s * terminal_curvature
+C0_target = model lateral position at s
+C1_target = release_heading bounded between zero and psi
+C2 = C3 = 0
+```
+
+The C1 interval respects the sign of `psi`. The correction cannot amplify the
+raw heading target or manufacture an opposite-direction target. A real model
+sign reversal continues through the normal slew. At an exact model knot the
+incoming segment supplies the slope. A duplicate selected station, zero span,
+or nonfinite derived slope retains the original heading.
+
+Heading linear in the sampled cumulative arc distance, and constant heading,
+retain the original mapping. Arc distance uses model-point chords, so even a
+true circle can differ slightly under uneven sampling; exact preservation is
+a statement about the discrete heading slope, not every sampled physical arc.
+When the terminal curvature is gentler than the average over the preview, C1
+can decrease sooner; it can reach zero even while the future pose still has a
+nonzero heading. C0 continues requesting that future lateral position.
+
+There is no temporal turn-mode latch, new strength multiplier, measured-yaw
+correction, fitted PSCM model, or foreign firmware command cap. Only the two
+existing C0/C1 slew positions persist. Limits remain ±5.11 m / ±0.5 rad and
+4 m/s / 0.5 rad/s; transmission remains 20 Hz.
+
+## Physical question and tradeoff
+
+The question is whether unloading C1 sooner makes the measured wheel release
+sooner, without unacceptable loss of turning authority. Constant discrete
+heading slope is preserved; not every real turn-in is guaranteed unchanged. A model
+point can already lie on the straight after a corner while the truck still
+needs heading change to reach it. This rule can then remove useful C1. C0 and
+the PSCM's physical response determine the result, which passive replay cannot
+establish.
+
+The prior ML3V firmware's internal contribution limits are not treated as
+Lightning limits. `LimitReached` is not used as a release trigger. No claim is
+made that this fixes the a9 hang, tracking error, or stability across PSCMs.
+`calibration_approved=false` remains.
+
+## Recorded command comparison
+
+The a9 comparison uses 7,199 original-clock models and 35,775 control cycles
+from six recorded segments. Candidate C0 is exactly equal to baseline C0 at
+raw target, publication, and sampled send times. Raw C1 never exceeds the
+original heading magnitude or invents a reversal. Independent slew histories
+can let the candidate reach a genuine opposite-direction target sooner, so
+published C1 magnitude is not universally smaller on reversal.
+
+Entry and plateau changes are included, rather than scoring only exits:
+
+| Recorded window | Baseline mean absolute C1 | Candidate mean absolute C1 |
+| --- | ---: | ---: |
+| Right entry, 38:47–38:50 | 0.13643 rad | 0.12561 rad |
+| Large left entry, 40:01–40:05 | unchanged | unchanged |
+| Right plateau, 45:26–45:28.4 | 0.11442 rad | 0.10506 rad |
+| Left entry, 45:34–45:36 | 0.16580 rad | 0.15962 rad |
+
+C1 crosses a common 0.1 rad level about 0.80–0.85 seconds earlier in three
+selected exits. The level is a comparison marker, not a PSCM limit. On the
+right exits near 38:51 and 45:29, it stops requesting the original turn
+direction 0.454 and 0.356 seconds earlier. These are proposed command times
+on frozen inputs, not measured improvements in wheel release.
+
+The baseline replay agrees with recorded publication within one wire quantum
+after cache-boundary warmup; logged publication times do not provide every
+internal controller-entry time. Original clocks, eligibility, and that
+reconstruction tolerance are retained in the validation record.
+
+## Selection and verification
+
+Use the existing default-off Sunnylink **Selected-Action Path Tracking
+(Experimental)** toggle (`FordModelActionController`). An already enabled
+setting selects this revision after updating and an offroad-to-onroad cycle.
+The diagnostic identity is `model-pose-terminal-c1-v1`; active diagnostics also
+report `c1_release=terminal_spatial_curvature`. Turning the toggle off restores
+the previous nonexperimental selection after another offroad-to-onroad cycle.
+
+See the [drive-test guide](ford_model_action_drive_test.md) and the
+[validation record](ford_model_release_validation.json). Unit/integration checks
+cover command construction, bounds, reset, sender cadence, and encoding. Route
+comparisons report changes in commands, including entry attenuation; they do
+not predict wheel motion under the changed commands. The next recording must
+compare desired versus actual steering and release timing on the actual PSCM.

openpilot/selfdrive/controls/lib/ford_model_action.py

@@ -1,4 +1,4 @@
-"""Experimental Ford C2-free controller using one shared model-path point.
+"""Experimental Ford model-point controller with earlier C1 release.
 
 The one-second preview and seven-metre minimum are engineering choices,
 not identified Ford reference points or PSCM calibration.
@@ -31,11 +31,14 @@ def _finite(*values):
 
 
 def encode_model_action(model, desired_curvature, speed):
-  """Sample model lateral position and heading at the same arc station.
+  """Sample a model pose and unload C1 when its path is straightening ahead.
 
   Use the model's distance at one second, with a seven-metre minimum and an
-  endpoint hold. Selected curvature remains a health/diagnostic input; it does
-  not reconstruct heading or rotate the model geometry in this experiment.
+  endpoint hold. Terminal spatial curvature bounds the heading contribution
+  toward zero. A constant heading slope in sampled arc distance retains C1;
+  decreasing curvature can release C1 before that heading returns to zero.
+  This is an experimental request mapping, not an identified PSCM response.
+  Selected curvature remains a health/diagnostic input only.
   """
   if not _finite(desired_curvature, speed) or not .3 <= speed <= 55 or abs(desired_curvature) > 1:
     return FordPath()
@@ -54,6 +57,17 @@ def encode_model_action(model, desired_curvature, speed):
   sample_station = min(station[-1], max(MIN_STATION_M, float(np.interp(PREVIEW_TIME_S, times, station))))
   c0 = float(np.interp(sample_station, station, lateral))
   c1 = float(np.interp(sample_station, station, heading))
+  # Use the same enclosing model segment as the pose interpolation. At an
+  # exact knot use its incoming segment; an ambiguous duplicate keeps C1.
+  upper = max(1, int(np.searchsorted(station, sample_station, side='left')))
+  span = station[upper] - station[upper - 1]
+  duplicate = (upper + 1 < len(station) and station[upper] == sample_station == station[upper + 1])
+  if span > 0. and not duplicate:
+    terminal_heading = heading[0] + sample_station * ((heading[upper] - heading[upper - 1]) / span)
+    if _finite(terminal_heading):
+      # One-sided: never amplify C1 or invent a reversal ahead of the model.
+      direction = math.copysign(1., c1)
+      c1 = direction * float(np.clip(direction * terminal_heading, 0., abs(c1)))
   return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
 
 
@@ -97,7 +111,7 @@ class FordModelActionController:
   def reset(self, status='inactive'):
     self.core.reset()
     self.last_time = self.last_measurement_time = self.last_model_time = None
-    self.diagnostics = {'status': status, 'hypothesis': 'model-pose-one-second-v7',
+    self.diagnostics = {'status': status, 'hypothesis': 'model-pose-terminal-c1-v1',
                         'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)}
 
   def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time,
@@ -130,10 +144,11 @@ class FordModelActionController:
       self.reset('invalid_path')
       return command
     self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time
-    self.diagnostics = {'status': 'active', 'hypothesis': 'model-pose-one-second-v7',
+    self.diagnostics = {'status': 'active', 'hypothesis': 'model-pose-terminal-c1-v1',
                         'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature,
                         'yaw_rate': yaw_rate, 'pose_source': 'model',
                         'preview_time_s': PREVIEW_TIME_S, 'minimum_station_m': MIN_STATION_M,
+                        'c1_release': 'terminal_spatial_curvature',
                         'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time,
                         'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1,
                         'command': (command.path_offset, command.path_angle, 0., 0.)}

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase):
       controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.005, curvature=.0025,
                                  sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
       record = self.emit_controls_event('Ford C2-free path tracking', controls)
-      self.assertEqual(record['hypothesis'], 'model-pose-one-second-v7')
+      self.assertEqual(record['hypothesis'], 'model-pose-terminal-c1-v1')
       self.assertIs(record['calibration_approved'], False)
       self.assertEqual(record['command'][2:], [0., 0.])
       self.assertEqual(record['status'], controller.diagnostics['status'])
@@ -61,3 +61,4 @@ class TestFordControlsLogging(unittest.TestCase):
         self.assertEqual(record['pose_source'], 'model')
         self.assertEqual(record['preview_time_s'], 1.)
         self.assertEqual(record['minimum_station_m'], 7.)
+        self.assertEqual(record['c1_release'], 'terminal_spatial_curvature')

openpilot/selfdrive/controls/tests/test_ford_model_points.py

@@ -49,6 +49,8 @@ def test_orientation_and_position_must_describe_the_same_times():
   assert not encode_model_action(m, .01, 20.).valid
 
 
-def test_model_heading_unwraps_before_interpolation():
+def test_model_heading_unwraps_before_interpolation_and_terminal_slope():
   m = model_points([0., .5, 1.5], [0., 10., 30.], [0., 0., 0.], [3., 3.1, -3.1])
-  assert encode_model_action(m, 0., 20.).path_angle == pytest.approx(np.pi)
+  # Heading at 20 m is pi, but the terminal slope is gentler than the
+  # average slope. Its release bound is 3 + 20 * ((2*pi - 3.1) - 3.1)/20.
+  assert encode_model_action(m, 0., 20.).path_angle == pytest.approx(2*np.pi - 3.2)

openpilot/selfdrive/controls/tests/test_ford_model_release.py

@@ -0,0 +1,148 @@
+"""C1 release geometry checks; none predicts a PSCM or vehicle response."""
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, encode_model_action
+
+
+def model_path(headings, *, origin=0., offset=.3, sign=1.):
+  # Known chord lengths make spatial heading profiles independent of speed.
+  station = (0., 6., 18., 30.)
+  heading = [sign*(origin+value) for value in headings]
+  x, y = [0.], [sign*offset]
+  for i in range(1, len(station)):
+    direction = (heading[i-1]+heading[i])/2
+    distance = station[i]-station[i-1]
+    x.append(x[-1]+distance*math.cos(direction))
+    y.append(y[-1]+distance*math.sin(direction))
+  times = [0., .5, 1.5, 2.5]
+  return SimpleNamespace(position=SimpleNamespace(t=times, x=x, y=y),
+                         orientation=SimpleNamespace(t=times, z=heading))
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('origin', [0., .15])
+def test_constant_spatial_curvature_preserves_original_heading(sign, origin):
+  model = model_path([0., .12, .36, .60], origin=origin, sign=sign)
+  out = encode_model_action(model, 0., 20.)
+  # The selected point is halfway from 6 m to 18 m: psi = psi0 + .02*12.
+  assert out.valid
+  assert out.path_angle == pytest.approx(sign*(origin+.24))
+  assert out.curvature == out.curvature_rate == 0.
+
+
+@pytest.mark.parametrize('heading', [-.3, .3])
+def test_constant_heading_line_keeps_its_nonzero_heading(heading):
+  out = encode_model_action(model_path([0.]*4, origin=heading), 0., 20.)
+  assert out.valid
+  assert out.path_angle == pytest.approx(heading)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_increasing_spatial_curvature_cannot_amplify_c1(sign):
+  # Selected heading .24; terminal curvature .03/m would yield .36 rad.
+  model = model_path([0., .06, .42, .90], sign=sign)
+  out = encode_model_action(model, 0., 20.)
+  assert out.path_angle == pytest.approx(sign*.24)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('origin', [0., .15])
+def test_decreasing_spatial_curvature_unloads_c1_without_erasing_origin(sign, origin):
+  # Selected heading is psi0+.36; terminal curvature .02/m gives psi0+.24.
+  model = model_path([0., .24, .48, .54], origin=origin, sign=sign)
+  out = encode_model_action(model, 0., 20.)
+  assert out.path_angle == pytest.approx(sign*(origin+.24))
+  assert abs(out.path_angle) < origin+.36
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_opposite_terminal_curvature_releases_without_inventing_a_reversal(sign):
+  model = model_path([0., .30, .10, -.30], sign=sign)
+  out = encode_model_action(model, 0., 20.)
+  # The selected model heading is still sign*.20, though its slope has reversed.
+  assert out.valid
+  assert out.path_angle == 0.
+
+
+def test_exact_model_knot_uses_incoming_segment():
+  model = SimpleNamespace(position=SimpleNamespace(t=[0., .5, 1., 2.], x=[0., 6., 12., 24.], y=[.3]*4),
+                          orientation=SimpleNamespace(t=[0., .5, 1., 2.], z=[0., .24, .48, .48]))
+  assert encode_model_action(model, 0., 20.).path_angle == pytest.approx(.48)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_duplicate_selected_station_retains_original_c1(sign):
+  times = [0., .5, 1., 2.]
+  model = SimpleNamespace(position=SimpleNamespace(t=times, x=[0., 7., 7., 14.], y=[sign*.4]*4),
+                          orientation=SimpleNamespace(t=times, z=[0., sign*.3, sign*.2, sign*.2]))
+  out = encode_model_action(model, 0., 20.)
+  assert out.valid
+  assert out.path_offset == sign*.4
+  assert out.path_angle == pytest.approx(sign*.2)
+
+
+def test_actual_model_reversal_uses_existing_slew_and_reaches_opposite_c1():
+  controller = ModelActionController()
+  positive = model_path([0., .12, .36, .60])
+  negative = model_path([0., .12, .36, .60], sign=-1.)
+  for _ in range(50):
+    controller.update(positive, 0., speed=20., dt=.01)
+  before = controller.c1
+  first = controller.update(negative, 0., speed=20., dt=.01)
+  assert controller.c1 == pytest.approx(before-.005)
+  assert first.path_angle > 0.
+  for _ in range(100):
+    final = controller.update(negative, 0., speed=20., dt=.01)
+  assert final.path_angle == pytest.approx(-.24)
+
+
+def test_release_preserves_c0_exactly_through_target_slew_and_packing():
+  actual, reference = ModelActionController(), ModelActionController()
+  for i in range(240):
+    model = model_path([0., .24, .48, .54], offset=8.*math.sin(i*.04), sign=1. if i < 120 else -1.)
+    # C0's reference has byte-for-byte identical position and clocks, but no C1.
+    zero_heading = SimpleNamespace(position=model.position,
+                                   orientation=SimpleNamespace(t=model.orientation.t, z=[0.]*4))
+    target = encode_model_action(model, 0., 20.)
+    c0_target = encode_model_action(zero_heading, 0., 20.)
+    assert target.path_offset == c0_target.path_offset
+    output = actual.update(model, 0., speed=20., dt=.01)
+    c0_output = reference.update(zero_heading, 0., speed=20., dt=.01)
+    assert actual.c0 == reference.c0
+    assert output.path_offset == c0_output.path_offset
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_large_geometry_keeps_existing_field_caps_and_zero_c2_c3(sign):
+  controller = ModelActionController()
+  model = model_path([0.]*4, origin=.8, offset=20., sign=sign)
+  for _ in range(150):
+    out = controller.update(model, 0., speed=20., dt=.01)
+    assert out.valid
+    assert abs(out.path_offset) <= 5.11+1e-12
+    assert abs(out.path_angle) <= .5+1e-12
+    assert out.curvature == out.curvature_rate == 0.
+  assert out.path_offset == pytest.approx(sign*5.11)
+  assert out.path_angle == pytest.approx(sign*.5)
+
+
+@pytest.mark.parametrize('field', ['x', 'y', 'z'])
+def test_nonfinite_geometry_cannot_publish_a_release_request(field):
+  model = model_path([0., .24, .48, .54])
+  values = getattr(model.orientation if field == 'z' else model.position, field)
+  values[2] = math.nan
+  assert not encode_model_action(model, 0., 20.).valid
+
+
+def test_unrepresentable_terminal_slope_falls_back_to_finite_original_heading():
+  times = [0., .5, 1.]
+  model = SimpleNamespace(position=SimpleNamespace(t=times, x=[0., 5e-320, 1e-319], y=[0.]*3),
+                          orientation=SimpleNamespace(t=times, z=[0., .2, .4]))
+  out = encode_model_action(model, 0., 20.)
+  assert out.valid
+  assert np.isfinite(out.path_angle)
+  assert out.path_angle == pytest.approx(.4)

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -2183,8 +2183,8 @@
           "widget": "toggle",
           "needs_onroad_cycle": true,
           "title": "Selected-Action Path Tracking (Experimental)",
-          "description": "Follow model position and heading from one shared path point on the Ford CAN FD F-150 Lightning.",
-          "details": "Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; this model-point experiment is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
+          "description": "Follow the model path with experimental earlier steering release on the Ford CAN FD F-150 Lightning.",
+          "details": "Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Reduces the heading request when the model path straightens ahead; this may also reduce turning authority. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; earlier physical release is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
           "enablement": [
             {
               "type": "offroad_only"

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

@@ -14,8 +14,8 @@ sections:
     widget: toggle
     needs_onroad_cycle: true
     title: Selected-Action Path Tracking (Experimental)
-    description: Follow model position and heading from one shared path point on the Ford CAN FD F-150 Lightning.
-    details: Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; this model-point experiment is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
+    description: Follow the model path with experimental earlier steering release on the Ford CAN FD F-150 Lightning.
+    details: Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Reduces the heading request when the model path straightens ahead; this may also reduce turning authority. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; earlier physical release is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
     enablement:
     - $ref: '#/macros/offroad'
   - key: FordPscmObserver

tools/ford_pscm_lab/model_release_replay.py

@@ -0,0 +1,222 @@
+"""Compare C1 release commands on timestamp-preserving recorded model extracts.
+
+The model cache contains ns and points shaped (models,4,points): x,y,heading,t.
+Its shared t must be copied from original model position/orientation clocks after
+verifying that those clocks agree; this tool never synthesizes missing clocks.
+Recorded motion and eligibility are frozen. Output compares commands, never
+predicts steering or establishes whether physical unwind would improve.
+"""
+import argparse
+import hashlib
+import json
+from pathlib import Path
+from types import SimpleNamespace
+
+import numpy as np
+
+from openpilot.selfdrive.controls.lib import ford_model_action
+from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, load_controller, sample, table
+
+BASELINE_REVISION = '4bd841eccd116f535a2ee99b920a631d1718f0cc'
+WARMUP_S = 3.
+
+
+def read_models(filename):
+  with np.load(filename, allow_pickle=False) as raw:
+    if 'points' not in raw or 'ns' not in raw:
+      raise ValueError('Model cache requires ns and original x,y,heading,t arrays')
+    points, ns = raw['points'], raw['ns']
+  if points.ndim != 3 or points.shape[1] != 4 or points.shape[2] < 2 or not len(points):
+    raise ValueError('Model cache requires four arrays per model, including original timestamps')
+  if ns.shape != (len(points),) or not np.issubdtype(ns.dtype, np.integer) or np.any(np.diff(ns) <= 0):
+    raise ValueError('Model ns must be strictly increasing integer timestamps')
+  if not np.isfinite(points).all() or np.any(points[:, 3, 0] != 0.) or np.any(np.diff(points[:, 3], axis=1) <= 0.):
+    raise ValueError('Model points and original timestamps must be finite, with times starting at zero and increasing')
+  models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1], t=p[3]),
+                            orientation=SimpleNamespace(z=p[2], t=p[3])) for p in points]
+  return ns, models
+
+
+def crossings(t, values, valid, threshold, quantum, *, direction):
+  below = direction * values <= threshold + quantum + 1e-9
+  selected = below & valid
+  first = np.flatnonzero(selected)
+  crossed = np.flatnonzero(selected[1:] & valid[:-1] & ~below[:-1]) + 1
+  return {'threshold': threshold, 'quantum_tolerance': quantum, 'original_turn_direction': direction,
+          'starts_at_or_below': bool(selected[0]),
+          'first_at_or_below_s': float(t[first[0]]) if len(first) else None,
+          'first_downcross_s': float(t[crossed[0]]) if len(crossed) else None}
+
+
+def compare_window(t, old, new, valid, label, lo, hi):
+  keep = (t >= lo) & (t <= hi)
+  if not keep.any():
+    raise ValueError(f'No exact consumed models for window {label}')
+  t, old, new, valid = t[keep], old[keep], new[keep], valid[keep]
+  if not valid.any():
+    raise ValueError(f'No eligible commands in window {label}')
+  delta = abs(old[:, 1]) - abs(new[:, 1])
+  fields = {}
+  for column, name, thresholds, quantum in [(0, 'c0', [.5, .05, 0.], .01), (1, 'c1', [.1, .035, 0.], .0005)]:
+    peak = old[valid, column][np.argmax(abs(old[valid, column]))]
+    direction = -1. if peak < 0. else 1.
+    fields[name] = [{'baseline': crossings(t, old[:, column], valid, threshold, quantum, direction=direction),
+                     'candidate': crossings(t, new[:, column], valid, threshold, quantum, direction=direction)} for threshold in thresholds]
+  return {'window_s': [lo, hi], 'cycles': len(t), 'eligible_cycles': int(valid.sum()),
+          'baseline_mean_abs_c1': float(np.mean(abs(old[valid, 1]))),
+          'candidate_mean_abs_c1': float(np.mean(abs(new[valid, 1]))),
+          'c1_reduced_more_than_one_quantum_percent': float(100*np.mean(delta[valid] > .0005001)),
+          'c1_increased_more_than_one_quantum_percent': float(100*np.mean(delta[valid] < -.0005001)),
+          'largest_c1_magnitude_reduction_rad': float(np.max(delta[valid])),
+          'exact_c0_equality': bool(np.array_equal(old[:, 0], new[:, 0])), 'published_crossings': fields}
+
+
+def run(route_extract, models_file, metadata_file, output, windows):
+  inputs = [route_extract.resolve(), models_file.resolve(), metadata_file.resolve()]
+  output = output.resolve()
+  commands_output = output.with_suffix('.npz')
+  if output in inputs or commands_output in inputs:
+    raise ValueError('Output must not overwrite an input extract')
+  ns, models = read_models(models_file)
+  metadata = json.loads(metadata_file.read_text())
+  if not metadata['identities'] or any(identity['commit'] != BASELINE_REVISION or identity['dirty'] for identity in metadata['identities']):
+    raise ValueError('Recorded route must use the pinned baseline revision')
+  with np.load(route_extract, allow_pickle=False) as raw:
+    streams = {name: table(raw, name) for name in ('controls', 'cs', 'path', 'wire')}
+  controls = streams['controls']
+  mi = np.searchsorted(ns, controls['model_ns'])
+  exact = (mi < len(ns)) & (ns[np.minimum(mi, len(ns)-1)] == controls['model_ns'])
+  indices = np.flatnonzero(exact)
+  if not len(indices):
+    raise ValueError('No extracted model matches a consumed model timestamp')
+  mi = mi[indices]
+  controls = {key: values[indices] for key, values in controls.items()}
+  now = controls['t']
+  if np.any(np.diff(now) <= 0):
+    raise ValueError('Control timestamps must increase strictly')
+  cs = sample(streams['cs'], now)
+  recorded = sample(streams['path'], now, nearest=True)
+  if np.any(abs(recorded['t'] - now) >= .005):
+    raise ValueError('Recorded command publication is not from the same control cycle')
+  active = recorded['active'].astype(bool)
+  archived = load_controller(BASELINE_REVISION)
+  old_core, new_core = archived.ModelActionController(), ford_model_action.ModelActionController()
+  old = np.zeros((len(now), 4))
+  new = np.zeros_like(old)
+  valid = np.zeros(len(now), dtype=bool)
+  settled = np.zeros(len(now), dtype=bool)
+  old_targets, new_targets = [], []
+  for model in models:
+    a = archived.encode_model_action(model, 0., 10.)
+    b = ford_model_action.encode_model_action(model, 0., 10.)
+    if not a.valid or not b.valid:
+      raise ValueError('Model cache contains a path rejected by one of the encoders')
+    old_targets.append([a.path_offset, a.path_angle])
+    new_targets.append([b.path_offset, b.path_angle])
+  old_targets, new_targets = np.array(old_targets), np.array(new_targets)
+  np.testing.assert_array_equal(old_targets[:, 0], new_targets[:, 0])
+  assert (abs(new_targets[:, 1]) <= abs(old_targets[:, 1]) + 1e-12).all()
+  assert (old_targets[:, 1] * new_targets[:, 1] >= 0.).all()
+  wire = WireCheck()
+  span_start = now[0]
+  previous_valid = False
+  for i, timestamp in enumerate(now):
+    if i == 0 or indices[i] != indices[i-1]+1 or timestamp-now[i-1] > .1:
+      old_core.reset()
+      new_core.reset()
+      previous_valid = False
+      span_start = timestamp
+    dt = timestamp-now[i-1] if previous_valid else .01
+    kwargs = {'speed': cs['speed'][i], 'yaw_rate': cs['yaw'][i], 'dt': dt, 'active': bool(active[i])}
+    a = old_core.update(models[mi[i]], controls['desired'][i], **kwargs)
+    b = new_core.update(models[mi[i]], controls['desired'][i], **kwargs)
+    assert a.valid == b.valid
+    old[i] = a.path_offset, a.path_angle, a.curvature, a.curvature_rate
+    new[i] = b.path_offset, b.path_angle, b.curvature, b.curvature_rate
+    valid[i] = a.valid
+    settled[i] = timestamp - span_start >= WARMUP_S
+    wire.check(a)
+    wire.check(b)
+    previous_valid = a.valid
+  np.testing.assert_array_equal(valid, active)
+  np.testing.assert_array_equal(old[:, 0], new[:, 0])
+  for command in (old, new):
+    field_checks(command, valid, now)
+  baseline_recorded = np.column_stack([recorded['c0'], recorded['c1']])
+  baseline_delta = abs(old[:, :2].astype(np.float32).astype(float) - baseline_recorded)
+  if not settled.any() or not valid[settled].any():
+    raise ValueError('No eligible commands remain after the three-second cache-boundary warmup')
+  assert (baseline_delta[settled] <= [.010001, .0005001]).all(), 'Baseline differs from recorded output by more than one DBC quantum'
+  elapsed = now - metadata['t0']
+  window_reports = {}
+  for label, lo, hi in windows:
+    keep = (elapsed >= lo) & (elapsed <= hi)
+    if not keep.any() or not settled[keep].all():
+      raise ValueError(f'Window {label} lacks complete cache-boundary warmup')
+    window_reports[label] = compare_window(elapsed, old, new, valid, label, lo, hi)
+  # Keep the recorded 20 Hz transmission schedule. This samples the proposed
+  # publication; it does not simulate a different sender or vehicle response.
+  wire_t = streams['wire']['t']
+  wi = np.searchsorted(now, wire_t, side='right')-1
+  in_scope = (wi >= 0) & (wire_t-now[np.maximum(wi, 0)] <= .02)
+  wi = wi[in_scope]
+  scheduled_t = wire_t[in_scope]
+  scheduled_old, scheduled_new = old[wi], new[wi]
+  np.testing.assert_array_equal(scheduled_old[:, 0], scheduled_new[:, 0])
+  output.parent.mkdir(parents=True, exist_ok=True)
+  np.savez_compressed(commands_output, t=elapsed, baseline=old, candidate=new, valid=valid, settled=settled,
+                      recorded=baseline_recorded, model_ns=controls['model_ns'], model_cache_index=mi,
+                      raw_baseline=old_targets[mi], raw_candidate=new_targets[mi],
+                      scheduled_t=scheduled_t-metadata['t0'], scheduled_baseline=scheduled_old, scheduled_candidate=scheduled_new)
+  sources = [Path(__file__), *inputs, Path(ford_model_action.__file__),
+             Path(__file__).with_name('model_action_replay.py')]
+  report = {'scope': 'Command-only comparison on exact consumed recorded models with frozen recorded eligibility and motion; no physical steering prediction.',
+            'baseline_revision': BASELINE_REVISION, 'baseline_source_sha256': archived.source_sha256,
+            'clock_source': 'Cache row3 contains original shared model position/orientation times; omitted clocks are rejected.',
+            'models': len(models), 'control_cycles': len(now), 'eligible_cycles': int(valid.sum()),
+            'three_second_cache_boundary_warmup_cycles': int((~settled).sum()),
+            'baseline_matches_recorded_within_one_quantum_after_warmup': True,
+            'baseline_timing_limit': ('Replay uses logged control-message times; internal core-entry times are not logged on every cycle. ' +
+                                      'Baseline agreement is therefore checked within one DBC quantum, not asserted bit-for-bit.'),
+            'baseline_nonidentical_published_values_after_warmup': int(np.count_nonzero(baseline_delta[settled])),
+            'baseline_max_recorded_error_after_warmup': np.max(baseline_delta[settled], axis=0).tolist(),
+            'exact_c0_equality_raw_published_and_scheduled': True,
+            'raw_c1_never_amplifies_or_reverses': True,
+            'float32_CAN_roundtrips': wire.count,
+            'finite_caps_slew_zero_c2_c3_checks': True,
+            'recorded_schedule_samples': len(scheduled_t),
+            'recorded_median_CAN_period_s': float(np.median(np.diff(streams['wire']['t']))),
+            'scheduled_command_scope': ('Latest proposed publication at each original send timestamp, restricted to <=20ms old; ' +
+                                        'existing sender behavior is unchanged.'),
+            'crossing_method': ('First command into the original turn at/below a diagnostic threshold plus one DBC quantum. ' +
+                                'Direction is the sign of the largest absolute valid baseline value per field/window and is shared by both versions; ' +
+                                'opposite-sign commands count as release. Downcross requires an immediately preceding above-threshold valid command. ' +
+                                'The 0.035 rad level is a comparison marker, not an established hardware limit. No physical-response score.'),
+            'windows': window_reports,
+            'source_sha256': {str(path.resolve()): hashlib.sha256(path.read_bytes()).hexdigest() for path in sources},
+            'command_artifact': str(commands_output)}
+  output.write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
+  return report
+
+
+def main():
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument('--route-extract', type=Path, required=True)
+  parser.add_argument('--models', type=Path, required=True)
+  parser.add_argument('--metadata', type=Path, required=True)
+  parser.add_argument('--output', type=Path, required=True)
+  parser.add_argument('--window', action='append', default=[], metavar='LABEL:START:END')
+  args = parser.parse_args()
+  windows = []
+  for value in args.window:
+    label, lo, hi = value.split(':')
+    lo, hi = float(lo), float(hi)
+    if not np.isfinite([lo, hi]).all() or hi <= lo:
+      parser.error('Window end must be finite and later than its start')
+    windows.append((label, lo, hi))
+  report = run(args.route_extract, args.models, args.metadata, args.output, windows)
+  print(json.dumps({key: value for key, value in report.items() if key not in ('windows', 'source_sha256')}, indent=2))
+
+
+if __name__ == '__main__':
+  main()

tools/ford_pscm_lab/test_model_release_replay.py

@@ -0,0 +1,73 @@
+import numpy as np
+import pytest
+
+from tools.ford_pscm_lab.model_release_replay import compare_window, crossings, read_models, run
+
+
+def original_models():
+  return np.array([[[0., 7., 14.], [0., .1, .2], [0., .02, .04], [0., 1., 2.]]])
+
+
+def test_model_clock_roundtrip(tmp_path):
+  filename = tmp_path/'models.npz'
+  points = original_models()
+  np.savez(filename, ns=np.array([123], dtype=np.int64), points=points)
+  ns, models = read_models(filename)
+  np.testing.assert_array_equal(ns, [123])
+  np.testing.assert_array_equal(models[0].position.t, points[0, 3])
+  np.testing.assert_array_equal(models[0].orientation.t, points[0, 3])
+  np.testing.assert_array_equal(models[0].orientation.z, points[0, 2])
+
+
+@pytest.mark.parametrize('times', [[1., 2., 3.], [0., 1., 1.], [0., 2., 1.], [0., np.nan, 2.]])
+def test_reject_bad_original_model_clocks(tmp_path, times):
+  points = original_models()
+  points[0, 3] = times
+  filename = tmp_path/'models.npz'
+  np.savez(filename, ns=np.array([123]), points=points)
+  with pytest.raises(ValueError, match='timestamps'):
+    read_models(filename)
+
+
+def test_run_rejects_missing_clocks_before_comparison(tmp_path):
+  filename = tmp_path/'models.npz'
+  np.savez(filename, ns=np.array([123]), points=original_models()[:, :3])
+  with pytest.raises(ValueError, match='original timestamps'):
+    run(tmp_path/'route.npz', filename, tmp_path/'metadata.json', tmp_path/'report.json', [])
+  assert not (tmp_path/'report.json').exists()
+
+
+def test_reject_noninteger_model_stamp(tmp_path):
+  filename = tmp_path/'models.npz'
+  np.savez(filename, ns=np.array([123.5]), points=original_models())
+  with pytest.raises(ValueError, match='integer timestamps'):
+    read_models(filename)
+
+
+def test_downcross_respects_quantum_and_invalid_cycles():
+  stamps = np.arange(6.)
+  # Invalid zero cannot count as release; threshold plus one quantum can.
+  values = np.array([.2, 0., .2, .1005, .1, 0.])
+  actual = crossings(stamps, values, np.array([True, False, True, True, True, True]), .1, .0005, direction=1.)
+  assert not actual['starts_at_or_below']
+  assert actual['first_at_or_below_s'] == actual['first_downcross_s'] == 3.
+  no_cross = crossings(stamps, np.full(6, .1006), np.ones(6, dtype=bool), .1, .0005, direction=1.)
+  assert no_cross['first_at_or_below_s'] is no_cross['first_downcross_s'] is None
+
+
+@pytest.mark.parametrize('direction', [-1., 1.])
+def test_sign_jump_counts_as_release(direction):
+  values = direction * np.array([.002, -.003, -.01])
+  actual = crossings(np.arange(3.), values, np.ones(3, dtype=bool), 0., .0005, direction=direction)
+  assert actual['first_at_or_below_s'] == actual['first_downcross_s'] == 1.
+
+
+def test_comparison_uses_baseline_direction_for_both_versions():
+  old, new = np.zeros((3, 4)), np.zeros((3, 4))
+  old[:, 1] = [.1, .05, -.01]
+  new[:, 1] = [.02, -.04, -.04]
+  result = compare_window(np.arange(3.), old, new, np.ones(3, dtype=bool), 'turn', 0., 2.)
+  zero = result['published_crossings']['c1'][-1]
+  assert zero['baseline']['original_turn_direction'] == zero['candidate']['original_turn_direction'] == 1.
+  assert zero['baseline']['first_downcross_s'] == 2.
+  assert zero['candidate']['first_downcross_s'] == 1.