Ford: recover opposing heading bias during turn release

Allow release recovery only when fresh measured yaw undertracks both aligned current and delayed requests and PSCM limit is below 2. Unwind the opposing bias toward zero using current yaw error and existing antiwindup; preserve C0, base geometry, gains, rates, and safety guards.

Add mirrored unit checks and a sanitized recorded turn-exit regression. Validate with 142 tests and 97 subtests, full-route frozen-input replay, large-turn retention, CAN packing, diagnostics, and generated Sunnylink schema checks. Physical improvement remains unvalidated.

docs/ford_virtual_angle_experiment.md

@@ -1,15 +1,18 @@
 # Ford C2-free model-pose tracking with measured feedback
 
-Hypothesis `model-pose-c0-c1-feedback-v6` restores the existing allocator's
-model-path C0/C1 demand for large turns when model geometry and selected
-curvature agree. The remaining selected curvature becomes C0/C1 centering
-and turn demand; C2/C3 stay zero. Selected desired curvature remains the
-measured-yaw feedback target, even when model geometry supplies the base.
+Hypothesis `model-pose-c0-c1-feedback-v7` retains v6's model-pose C0/C1 base
+and adds narrow recovery of an opposing C1 bias during release. C0, model
+geometry, blending, gains, rates and output bounds are unchanged. Recovery
+can remove retained opposing bias but cannot create bias beyond zero or
+relax the PSCM LimitReached growth restriction. C2/C3 remain zero, and
+selected desired curvature remains the measured-yaw feedback target.
 
 This is an experimental outer controller around the multivariable PSCM.
 Its geometry does not define a calibrated C0/C1-to-wheel mapping or an angle
 servo. Command replay cannot establish the truck's response, closed-loop
 stability, or an overshoot improvement.
+The v7 recovery change has offline validation only; its physical response
+is unproven.
 
 ## Evidence and scope
 
@@ -74,7 +77,7 @@ The existing pose encoder retains its quantization and field-allocation rules.
 The residual-curvature lift is geometric, not a claim of EPS equivalence to C2.
 
 The inherited pose encoder allocates heading overflow using its asymmetric
-limits (+0.5235/−0.5 rad), before v6 applies the symmetric final ±0.5 rad
+limits (+0.5235/−0.5 rad), before the symmetric final ±0.5 rad
 heading bound. On clipped tails, this can leave mirrored C0 requests differing
 by up to 0.0235 rad × 7 m = 0.1645 m. The favorable comparison anchors lie
 below that heading cap; full model-base odd symmetry is not claimed.
@@ -120,8 +123,26 @@ both the delayed and current selected yaw requests in the base's direction,
 and total heading must still have the base's sign. Exceeding only an older,
 smaller request during turn-in does not qualify. The accepted increment may
 only reduce that existing total toward zero; it cannot grow the request or
-carry it through zero. Other error directions remain frozen, and existing
-host field and slew limits still apply.
+carry it through zero. Existing host field and slew limits still apply.
+
+The new release-recovery exception requires fresh valid PSCM status with
+limit below 2, retained bias opposing the base, and both current and delayed
+requests aligned with that base. Measured turning must be below both requests
+in their direction. It then uses the current yaw deficit × the existing
+feedback gain × measurement interval to unwind only the opposing bias toward
+zero. The increment is clipped so recovery cannot cross zero bias or create
+demand beyond the existing base. Common host anti-windup still limits what
+can be accepted. All other constrained cases remain frozen; PSCM limit 2
+never permits this request-increasing recovery.
+The no-new-bias restriction applies only to `release_recovery`. Once release
+ends, ordinary eligible integration can add correction beyond the base as
+before; its existing limits and guards are unchanged.
+
+`release_recovery` and `feedback_recovery_active=true` indicate that the
+recovery branch actually changed bias on that update. If host anti-windup
+blocks the entire increment, the status remains `host_limit` and the flag is
+false. Recovery is evaluated only on fresh measurements; the flag is false
+on repeated-measurement updates and after reset.
 
 Diagnostics distinguish `release_backoff` and `pscm_backoff`; a release takes
 precedence when both conditions apply. While `feedback_backoff_active` is
@@ -169,9 +190,9 @@ Missing PSCM status disables feedback, not an otherwise valid base request.
 
 Vehicle → Ford → **C2-Free Path Tracking (Experimental)** retains the
 `FordVirtualAngleController` key, default-off setting and offroad/onroad cycle
-requirement. Enabled selects v6 on Ford CAN FD `FORD_F_150_LIGHTNING_MK1`
+requirement. Enabled selects v7 on Ford CAN FD `FORD_F_150_LIGHTNING_MK1`
 regardless of missing or different EPS firmware-query results. Other platforms
-retain their existing controller. V6 takes priority over PSCM Coefficient
+retain their existing controller. V7 takes priority over PSCM Coefficient
 Observer while selected; disabling and cycling offroad/onroad restores the
 previous selection. Controller selection does not force lateral engagement.
 
@@ -180,7 +201,7 @@ is not validation of other firmware. No live device setting is changed.
 
 ## Diagnostics and verification
 
-The 5 Hz `Ford C2-free path tracking` event keeps its name and identifies v6.
+The 5 Hz `Ford C2-free path tracking` event keeps its name and identifies v7.
 `model_offset_base` / `model_heading_base` report the already weighted and
 encoded model contribution; `curvature_offset_base` / `curvature_heading_base`
 report the residual-curvature contribution. `model_share` and `base_guard`
@@ -192,6 +213,11 @@ The event retains source timestamps, measured curvature/yaw, final commands,
 slew scales, feedback bias/status/history, raw torque and PSCM status/age.
 `feedback_backoff_active` records the persistent heading ceiling, including
 cycles whose feedback status is `no_new_measurement`.
+`feedback_recovery_active` records an accepted recovery increment on this
+update only; it does not persist between measurements.
+`feedback_yaw_error` retains its delayed-reference meaning. Recovery instead
+uses current error, reconstructed from logged `desired_curvature`,
+synchronized car-state speed and `yaw_rate`; those two errors can differ.
 During backoff, `heading_target` can be lower in the request direction than
 the bounded sum of `heading_base` and `heading_bias`, because the temporary
 ceiling is not part of the stored bias.
@@ -202,7 +228,9 @@ Validation must cover large recorded maneuvers, flat-model centering, both
 turn directions, model/action disagreement, share transitions, release and
 reversal, release/limit backoff without growth or zero crossing, status/driver
 resets, reference causality, bounds, slew and CAN packing with C2/C3 zero.
+Recovery checks cover both directions, stopping at zero bias, repeated
+measurements, current-and-delayed agreement, and rejection at PSCM limit 2.
 Old v3/v4 command-equality expectations do not define
-v6 success. Historical v5 replay results remain historical observations.
+v7 success. Historical v5/v6 replay results remain historical observations.
 Replay fixes recorded motion and planner outputs, so enabled vehicle logs
 are still required to assess tracking error, oscillation and interventions.

openpilot/selfdrive/controls/lib/ford_virtual_angle.py

@@ -73,7 +73,8 @@ class HeadingFeedback:
     self.last_measurement_time = self.last_pscm_time = None
     self.backoff_active = False
     self.diagnostics = {'heading_bias': 0., 'feedback_status': status, 'feedback_reference_time': None,
-                        'feedback_reference_curvature': None, 'feedback_yaw_error': None, 'feedback_backoff_active': False}
+                        'feedback_reference_curvature': None, 'feedback_yaw_error': None,
+                        'feedback_backoff_active': False, 'feedback_recovery_active': False}
 
   def update(self, base, desired, *, yaw_rate, speed, now, measurement_time, dt, previous_command, heading_horizon, driver_override, pscm_status):
     reason = ('missing_pscm' if pscm_status is None else pscm_status.invalid_reason(now))
@@ -99,6 +100,7 @@ class HeadingFeedback:
       self.history.popleft()
 
     status = 'no_new_measurement'
+    recovery_active = False
     reference_time = reference_curvature = yaw_error = None
     if measurement_time != self.last_measurement_time:
       self.backoff_active = False
@@ -122,11 +124,20 @@ class HeadingFeedback:
         # measured turning must also exceed the current selected action.
         current_yaw_error = speed * desired - yaw_rate
         backoff = constrained and yaw_error * base < 0. and current_yaw_error * base < 0. and heading_before * base > 0.
-        if constrained and not backoff:
+        recovering = (releasing and pscm_status.limit < 2 and self.bias * base < 0. and
+                      desired * base > 0. and reference_curvature * base > 0. and yaw_error * base > 0. and current_yaw_error * base > 0.)
+        if constrained and not (backoff or recovering):
           status = 'release' if releasing else 'pscm_limit'
         else:
+          bias_before = self.bias
           increment = self.tuning.feedback_gain * yaw_error * measurement_dt
-          if backoff:
+          if recovering:
+            # Once both references show a shortfall, unwind a previous opposing
+            # correction during release. Use the current, smaller deficit and
+            # stop at zero bias; recovery cannot create demand beyond the base.
+            increment = float(np.clip(self.tuning.feedback_gain * current_yaw_error * measurement_dt,
+                                      min(0., -self.bias), max(0., -self.bias)))
+          elif backoff:
             # A release/limit may still reduce an excessive same-direction
             # heading request. It cannot grow that request or cross through
             # zero. This does not identify the PSCM's limiting mechanism or
@@ -149,6 +160,9 @@ class HeadingFeedback:
           if backoff:
             self.backoff_active = True
             status = 'release_backoff' if releasing else 'pscm_backoff'
+          elif recovering and self.bias != bias_before:
+            recovery_active = True
+            status = 'release_recovery'
     self.bias = float(np.clip(self.bias, -.5 - base, .5 - base))
     target = float(np.clip(base + self.bias, -.5, .5))
     if self.backoff_active:
@@ -160,7 +174,7 @@ class HeadingFeedback:
       target = float(np.clip(target, -ceiling if base < 0. else 0., ceiling if base > 0. else 0.))
     self.diagnostics = {'heading_bias': self.bias, 'feedback_status': status, 'feedback_reference_time': reference_time,
                         'feedback_reference_curvature': reference_curvature, 'feedback_yaw_error': yaw_error,
-                        'feedback_backoff_active': self.backoff_active}
+                        'feedback_backoff_active': self.backoff_active, 'feedback_recovery_active': recovery_active}
     return target
 
 
@@ -244,7 +258,7 @@ class FordVirtualAngleController:
     self.last_measurement_time = None
     self.curvature_history = deque()
     self.offset_request = self.heading_request = 0.0
-    self.diagnostics = {'status': 'inactive', 'hypothesis': 'model-pose-c0-c1-feedback-v6', 'command': (0., 0., 0., 0.),
+    self.diagnostics = {'status': 'inactive', 'hypothesis': 'model-pose-c0-c1-feedback-v7', 'command': (0., 0., 0., 0.),
                         **self.feedback.diagnostics}
 
   def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time,
@@ -313,7 +327,7 @@ class FordVirtualAngleController:
     offset = _packed(self.offset_request, .01, -5.12)
     heading = _packed(self.heading_request, .0005, -.5)
     self.command = FordPath(True, offset, heading, 0., 0.)
-    self.diagnostics = {'status': 'driver_override' if driver_override else 'active', 'hypothesis': 'model-pose-c0-c1-feedback-v6',
+    self.diagnostics = {'status': 'driver_override' if driver_override else 'active', 'hypothesis': 'model-pose-c0-c1-feedback-v7',
                         'desired_curvature': desired_curvature, 'offset_target': target_offset, 'heading_target': target_heading,
                         'model_offset_base': model_base.path_offset, 'model_heading_base': model_base.path_angle,
                         'curvature_offset_base': curvature_offset, 'curvature_heading_base': curvature_heading,

openpilot/selfdrive/controls/tests/fixtures/ford_heading_recovery_requests.json

@@ -0,0 +1,40 @@
+{
+  "description": "Signal-only v6 turn-exit recovery regression; no location, device identity, or predicted new vehicle response.",
+  "recorded_controller_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05",
+  "baseline_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05",
+  "response_delay": 0.20000000298023224,
+  "samples": 9134,
+  "models": 1843,
+  "fixture_sha256": "41d5e3efcee03a9e02fcaf7bf456c050c6a671a5b7f7fc9706ddf4d27bad71b8",
+  "windows": [
+    {
+      "name": "overturn_then_underturn",
+      "range_s": [
+        19.99615067150053,
+        29.48070058550053
+      ],
+      "samples": 521
+    },
+    {
+      "name": "well_tracked_curve_a",
+      "range_s": [
+        56.19474309950053,
+        63.68808603150053
+      ],
+      "samples": 729
+    },
+    {
+      "name": "well_tracked_curve_b",
+      "range_s": [
+        101.19780691350051,
+        116.33885445450052
+      ],
+      "samples": 810
+    }
+  ],
+  "selection": "One previously identified overturn-then-underturn event and two previously reported well-tracked curves; selected before recovery implementation.",
+  "mask": "Whole t-0.5 through t+0.65 interval active, valid, fresh, unpressed, raw driver torque magnitude <=1 Nm; requested |curvature|*speed\u00b2 >=.5 m/s\u00b2.",
+  "timing": "Exact consumed model publication; causal CAN/PSCM at estimated control computation time. Subtract observed median computation-to-publication delay; unsampled tick timing remains approximate.",
+  "context": "At least 20 seconds prior context or the available start, extended before the latest observed reset. Overlapping episodes are merged.",
+  "coordinates": "Times are local elapsed seconds; models contain only relative position.x/y and orientation.z arrays."
+}

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -62,9 +62,10 @@ class TestFordControlsLogging(unittest.TestCase):
       self.assertEqual(record['reference_service'], 'modelV2')
       self.assertEqual(record['reference_mono_time'], 123456789)
       self.assertEqual(record['status'], controller.diagnostics['status'])
-      self.assertEqual(record['hypothesis'], 'model-pose-c0-c1-feedback-v6')
+      self.assertEqual(record['hypothesis'], 'model-pose-c0-c1-feedback-v7')
       self.assertEqual(record['command'], list(controller.diagnostics['command']))
       self.assertIs(record['feedback_backoff_active'], False)
+      self.assertIs(record['feedback_recovery_active'], False)
       if active and valid:
         self.assertEqual(record['response_delay'], 0.2)
         self.assertEqual(record['desired_curvature'], 0.01)
@@ -123,6 +124,27 @@ class TestFordControlsLogging(unittest.TestCase):
         # The output ceiling is observable separately from the stored integral.
         self.assertLess(record['heading_target'], record['heading_base'] + record['heading_bias'])
 
+  def test_periodic_diagnostics_log_recovery_only_on_accepted_fresh_updates(self):
+    for limit in (0, 2):
+      with self.subTest(pscm_limit=limit):
+        controller = FordVirtualAngleController()
+        for i in range(50):
+          now = 1. + i * .01
+          controller.update(circle(.02), .02, yaw_rate=.3, speed=10., now=now, measurement_time=now,
+                            model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False))
+        self.assertLess(controller.diagnostics['heading_bias'], 0.)
+        first_status = 'release_recovery' if limit == 0 else 'release'
+        for now, expected_status in ((1.5, first_status), (1.51, 'no_new_measurement')):
+          controller.update(circle(.02), .01, yaw_rate=.03, speed=10., now=now, measurement_time=1.5,
+                            model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, limit, 2, False))
+          controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.01, curvature=.003,
+                                     sm=SimpleNamespace(logMonoTime={'modelV2': int(now * 1e9), 'carState': 1_500_000_000}))
+          record = self.emit_controls_event('Ford C2-free path tracking', controls)
+          self.assertEqual(record['feedback_status'], expected_status)
+          self.assertIs(record['feedback_recovery_active'], limit == 0 and now == 1.5)
+          self.assertIs(record['feedback_backoff_active'], False)
+          self.assertEqual(record['heading_bias'], controller.diagnostics['heading_bias'])
+
   def test_actual_ford_branch_uses_selected_reference_and_disables_invalid_output(self):
     source_path = Path(__file__).resolve().parents[1] / 'controlsd.py'
     source = ast.parse(source_path.read_text())

openpilot/selfdrive/controls/tests/test_ford_heading_recovery.py

@@ -0,0 +1,135 @@
+import unittest
+
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import HeadingFeedback, PathTuning, PscmStatus
+
+
+def update(feedback, sign, now, *, base=.2, desired=.02, yaw=.1, previous=.2, measurement=None, limit=0, **overrides):
+  inputs = {'yaw_rate': sign * yaw, 'speed': 10., 'now': now, 'measurement_time': now if measurement is None else measurement,
+            'dt': .01, 'previous_command': sign * previous, 'heading_horizon': 10., 'driver_override': False,
+            'pscm_status': PscmStatus(now, 2, limit, 2, False)}
+  inputs.update(overrides)
+  return feedback.update(sign * base, sign * desired, **inputs)
+
+
+def acquired_correction(sign, yaw=.4, desired=.03):
+  feedback = HeadingFeedback(.2, PathTuning())
+  previous = .3
+  for i in range(60):
+    target = update(feedback, sign, i * .01, base=.3, desired=desired, yaw=yaw, previous=previous)
+    previous = sign * target
+  return feedback, previous
+
+
+class TestFordHeadingRecovery(unittest.TestCase):
+  def test_release_recovers_opposing_bias_using_current_error(self):
+    for sign in (-1, 1):
+      with self.subTest(sign=sign):
+        feedback, previous = acquired_correction(sign)
+        retained = feedback.bias * .2 / .3
+        self.assertLess(sign * retained, -.01)
+        target = update(feedback, sign, .6, previous=previous)
+        # Current request needs 0.2 rad/s, delayed request 0.3 rad/s, measured
+        # yaw is 0.1 rad/s. Use the smaller current deficit, not the old turn.
+        self.assertAlmostEqual(sign * (feedback.bias - retained), .1 * .01)
+        self.assertLessEqual(sign * feedback.bias, 0.)
+        self.assertLessEqual(sign * target, .2)
+        self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
+        self.assertTrue(feedback.diagnostics['feedback_recovery_active'])
+
+  def test_recovery_stops_at_zero_bias_with_batched_measurement(self):
+    for sign in (-1, 1):
+      with self.subTest(sign=sign):
+        feedback, previous = acquired_correction(sign, yaw=.301)
+        self.assertLess(sign * feedback.bias, 0.)
+        target = update(feedback, sign, .65, previous=previous, yaw=0.)
+        self.assertAlmostEqual(feedback.bias, 0.)
+        self.assertAlmostEqual(sign * target, .2)
+        self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
+        # The request is still releasing, but zero bias cannot become boost.
+        target = update(feedback, sign, .66, yaw=0.)
+        self.assertAlmostEqual(feedback.bias, 0.)
+        self.assertAlmostEqual(sign * target, .2)
+        self.assertEqual(feedback.diagnostics['feedback_status'], 'release')
+        self.assertFalse(feedback.diagnostics['feedback_recovery_active'])
+
+  def test_opposing_delayed_request_blocks_recovery_even_with_both_positive_errors(self):
+    for sign in (-1, 1):
+      feedback, previous = acquired_correction(sign, yaw=-.2, desired=-.03)
+      retained = feedback.bias * .2 / .3
+      self.assertLess(sign * retained, 0.)
+      update(feedback, sign, .6, previous=previous, yaw=-.5)
+      self.assertGreater(sign * feedback.diagnostics['feedback_yaw_error'], 0.)
+      self.assertAlmostEqual(feedback.bias, retained)
+      self.assertEqual(feedback.diagnostics['feedback_status'], 'release')
+      self.assertFalse(feedback.diagnostics['feedback_recovery_active'])
+
+  def test_new_base_cannot_fabricate_recovery_beyond_available_slew(self):
+    for sign in (-1, 1):
+      for partial in (False, True):
+        with self.subTest(sign=sign, partial=partial):
+          feedback, previous = acquired_correction(sign)
+          bias = feedback.bias
+          before = .4 + sign * bias
+          if partial:
+            previous = before - .0045  # Only .0005 rad of the .001 recovery is deliverable.
+          target = update(feedback, sign, .6, base=.4, previous=previous)
+          self.assertAlmostEqual(sign * (feedback.bias - bias), .0005 if partial else 0.)
+          self.assertLessEqual(sign * target, .4)
+          self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery' if partial else 'host_limit')
+          self.assertEqual(feedback.diagnostics['feedback_recovery_active'], partial)
+
+  def test_recovery_requires_both_undertracking_errors_and_no_eps_limit(self):
+    for sign in (-1, 1):
+      for overrides in ({'yaw': .25}, {'yaw': .2}, {'limit': 2}, {'desired': 0.}, {'desired': -.02}):
+        with self.subTest(sign=sign, overrides=overrides):
+          feedback, previous = acquired_correction(sign)
+          retained = feedback.bias * .2 / .3
+          update(feedback, sign, .6, previous=previous, **overrides)
+          self.assertAlmostEqual(feedback.bias, retained)
+          self.assertNotEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
+
+  def test_release_does_not_recover_a_same_direction_bias(self):
+    for sign in (-1, 1):
+      feedback, previous = acquired_correction(sign, yaw=.2)
+      retained = feedback.bias * .2 / .3
+      self.assertGreater(sign * retained, 0.)
+      update(feedback, sign, .6, previous=previous)
+      self.assertAlmostEqual(feedback.bias, retained)
+      self.assertEqual(feedback.diagnostics['feedback_status'], 'release')
+
+  def test_fresh_recovery_clears_backoff_without_reusing_measurements(self):
+    for sign in (-1, 1):
+      with self.subTest(sign=sign):
+        feedback, previous = acquired_correction(sign)
+        target = update(feedback, sign, .6, previous=previous, yaw=.4)
+        self.assertTrue(feedback.backoff_active)
+        bias = feedback.bias
+        # The new current request alone cannot recover from an old observation.
+        repeated = update(feedback, sign, .61, measurement=.6, previous=sign * target, yaw=.4)
+        self.assertEqual(feedback.bias, bias)
+        self.assertTrue(feedback.backoff_active)
+        self.assertLessEqual(sign * repeated, sign * target)
+        update(feedback, sign, .62, previous=sign * repeated)
+        self.assertGreater(sign * feedback.bias, sign * bias)
+        self.assertFalse(feedback.backoff_active)
+        self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
+        bias = feedback.bias
+        update(feedback, sign, .63, measurement=.62)
+        self.assertEqual(feedback.bias, bias)
+        self.assertEqual(feedback.diagnostics['feedback_status'], 'no_new_measurement')
+        self.assertFalse(feedback.diagnostics['feedback_recovery_active'])
+
+  def test_driver_or_missing_status_clears_the_correction(self):
+    for sign in (-1, 1):
+      for overrides in ({'driver_override': True}, {'pscm_status': None},
+                        {'pscm_status': PscmStatus(.3, 2, 0, 2, False)}):
+        with self.subTest(sign=sign, overrides=overrides):
+          feedback, previous = acquired_correction(sign)
+          target = update(feedback, sign, .6, previous=previous, **overrides)
+          self.assertEqual(feedback.bias, 0.)
+          self.assertAlmostEqual(sign * target, .2)
+          self.assertNotEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
+
+
+if __name__ == '__main__':
+  unittest.main()

openpilot/selfdrive/controls/tests/test_ford_heading_recovery_routes.py

@@ -0,0 +1,118 @@
+import hashlib
+import json
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, PscmStatus
+
+
+class TestFordHeadingRecoveryRoutes(unittest.TestCase):
+  @classmethod
+  def setUpClass(cls):
+    cls.fixture = Path(__file__).parent / 'fixtures/ford_heading_recovery_requests.npz'
+    cls.metadata = json.loads(cls.fixture.with_suffix('.json').read_text())
+    cls.data = dict(np.load(cls.fixture))
+    d = cls.data
+    models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in d['models']]
+    commands, gates, rows = [], [], []
+    previous_episode = None
+    for i, now in enumerate(d['t']):
+      if d['episode'][i] != previous_episode:
+        controller = FordVirtualAngleController(response_delay=cls.metadata['response_delay'])
+        previous_episode = d['episode'][i]
+      eps = PscmStatus(float(d['pscm_timestamp'][i]), int(d['pscm_lateral_state'][i]), int(d['pscm_limit'][i]),
+                        int(d['pscm_capability'][i]), bool(d['pscm_denied'][i]), bool(d['pscm_valid'][i]))
+      prior_bias, prior_base = controller.feedback.bias, controller.feedback.previous_base
+      path = controller.update(models[d['model_index'][i]], d['desired_curvature'][i], yaw_rate=d['yaw_rate'][i], speed=d['speed'][i],
+                               now=now, measurement_time=d['measurement_time'][i], model_time=d['model_time'][i],
+                               reference_time=d['reference_time'][i], active=bool(d['active'][i]), valid=bool(d['valid'][i]),
+                               steering_pressed=bool(d['pressed'][i]), steering_torque=d['steering_torque'][i], pscm_status=eps)
+      row = dict(controller.diagnostics)
+      base = row.get('heading_base', 0.)
+      retained_bias = prior_bias if prior_base is not None and prior_base * base >= 0. else 0.
+      if prior_base and prior_base * base >= 0.:
+        retained_bias *= min(1., abs(base / prior_base))
+      row['bias_before_update'] = retained_bias
+      commands.append((path.path_offset, path.path_angle, path.curvature, path.curvature_rate))
+      gates.append(path.valid)
+      rows.append(row)
+    cls.commands, cls.gates = np.array(commands), np.array(gates)
+    cls.status = np.array([row['feedback_status'] for row in rows])
+    cls.recovering = np.array([row.get('feedback_recovery_active', False) for row in rows])
+    cls.backoff = np.array([row.get('feedback_backoff_active', False) for row in rows])
+    for key in ('heading_base', 'heading_target', 'heading_bias', 'bias_before_update', 'feedback_reference_curvature', 'feedback_yaw_error'):
+      setattr(cls, key, np.array([row.get(key, np.nan) for row in rows], dtype=float))
+
+  def test_fixture_hash_and_original_controller_provenance(self):
+    self.assertEqual(hashlib.sha256(self.fixture.read_bytes()).hexdigest(), self.metadata['fixture_sha256'])
+    self.assertEqual(self.metadata['baseline_revision'], '61dac4977bf9c36504398e8a4959dfed79cf6f05')
+    self.assertEqual(len(self.metadata['windows']), 3)
+    self.assertGreater(int(self.data['evidence'].sum()), 1000)
+
+  def test_recorded_turn_exit_releases_opposing_correction(self):
+    d = self.data
+    # Select the captured command problem from the old policy, not from the
+    # candidate result: release was freezing an opposing bias while both
+    # current and delayed requests still exceeded measured turning.
+    base, bias = d['baseline_heading_base'], d['baseline_heading_bias']
+    current_error = d['desired_curvature'] * d['speed'] - d['yaw_rate']
+    delayed = d['baseline_feedback_reference_curvature']
+    mask = (d['window_masks'][:, 0] & (d['baseline_status'] == 'release') & (bias * base < 0.) &
+            (current_error * base > 0.) & (d['baseline_feedback_yaw_error'] * base > 0.) &
+            (d['desired_curvature'] * base > 0.) & (delayed * base > 0.) & (d['pscm_limit'] < 2))
+    self.assertGreater(int(mask.sum()), 100)
+    along_turn = np.sign(d['desired_curvature'][mask])
+    increase = (self.commands[mask, 1] - d['baseline_commands'][mask, 1]) * along_turn
+    self.assertGreater(float(np.median(increase)), .005)
+    self.assertGreater(int((self.recovering & mask).sum()), 25)
+    self.assertLess(float(np.median(abs(self.heading_bias[mask]))), float(np.median(abs(bias[mask]))) - .005)
+
+  def test_recovery_only_cancels_bias_with_both_requests_undertracked(self):
+    d, mask = self.data, self.recovering
+    self.assertGreater(int(mask.sum()), 25)
+    self.assertTrue((self.status[mask] == 'release_recovery').all())
+    self.assertTrue((d['pscm_limit'][mask] < 2).all())
+    self.assertTrue((d['pscm_valid'][mask] & self.gates[mask] & ~d['pressed'][mask]).all())
+    self.assertTrue((abs(d['steering_torque'][mask]) <= 1.).all())
+    self.assertFalse(self.backoff[mask].any())
+    self.assertTrue((self.bias_before_update[mask] * self.heading_base[mask] < 0.).all())
+    self.assertTrue((self.feedback_yaw_error[mask] * self.heading_base[mask] > 0.).all())
+    current_error = d['speed'] * d['desired_curvature'] - d['yaw_rate']
+    self.assertTrue((current_error[mask] * self.heading_base[mask] > 0.).all())
+    self.assertTrue((d['desired_curvature'][mask] * self.heading_base[mask] > 0.).all())
+    self.assertTrue((self.feedback_reference_curvature[mask] * self.heading_base[mask] > 0.).all())
+    self.assertTrue((abs(self.heading_bias[mask]) < abs(self.bias_before_update[mask])).all())
+    self.assertTrue((self.heading_bias[mask] * self.bias_before_update[mask] >= -1e-12).all())
+    self.assertTrue((abs(self.heading_target[mask]) <= abs(self.heading_base[mask]) + 1e-12).all())
+
+  def test_c0_base_and_output_validity_are_unchanged(self):
+    evidence = self.data['evidence']
+    np.testing.assert_array_equal(self.commands[evidence, 0], self.data['baseline_commands'][evidence, 0])
+    np.testing.assert_array_equal(self.heading_base[evidence], self.data['baseline_heading_base'][evidence])
+    np.testing.assert_array_equal(self.gates[evidence], self.data['baseline_valid'][evidence])
+
+  def test_well_tracked_curves_keep_command_scale(self):
+    for index in (1, 2):
+      with self.subTest(window=self.metadata['windows'][index]['name']):
+        mask = self.data['window_masks'][:, index]
+        old, new = self.data['baseline_commands'][mask, 1], self.commands[mask, 1]
+        # This bounds collateral command change; it cannot guarantee the same
+        # future vehicle response on a drive with the candidate installed.
+        self.assertGreaterEqual(float(np.median(abs(new))), .95 * float(np.median(abs(old))))
+        self.assertLess(float(np.quantile(abs(new - old), .9)), .02)
+
+  def test_all_fixture_commands_respect_field_and_rate_limits(self):
+    d = self.data
+    np.testing.assert_array_equal(self.commands[:, 2:], 0.)
+    self.assertTrue(np.isfinite(self.commands).all())
+    self.assertTrue((abs(self.commands[:, :2]) <= [5.110000001, .500000001]).all())
+    continuous = (d['episode'][1:] == d['episode'][:-1]) & self.gates[1:] & self.gates[:-1]
+    allowed = np.diff(d['t'])[:, None] * [4., .5] + [.01, .0005] + np.array([1e-8, 1e-8])
+    self.assertTrue((abs(np.diff(self.commands[:, :2], axis=0))[continuous] <= allowed[continuous]).all())
+
+
+if __name__ == '__main__':
+  unittest.main()

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -2184,7 +2184,7 @@
           "needs_onroad_cycle": true,
           "title": "C2-Free Path Tracking (Experimental)",
           "description": "Follow large turns from the model path while retaining planned-curvature centering on the F-150 Lightning with C2 off.",
-          "details": "Uses the existing controller's model-path geometry for large turns when the model and planned curvature agree. Smaller or opposing requests use planned curvature for centering. A bounded measured-turning correction requires fresh, valid steering-controller status and clears during driver override. During turn release or a reported steering limit, that correction can only reduce the existing turn request toward zero. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; other vehicles retain their existing controller. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
+          "details": "Uses the existing controller's model-path geometry for large turns when the model and planned curvature agree. Smaller or opposing requests use planned curvature for centering. A bounded measured-turning correction requires fresh, valid steering-controller status and clears during driver override. During turn release, a previous opposing correction can unwind when turning falls below both recent and current requests. When the steering controller reports a reached limit, the correction can only reduce the existing turn request toward zero. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; other vehicles retain their existing controller. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. 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

@@ -15,7 +15,7 @@ sections:
     needs_onroad_cycle: true
     title: C2-Free Path Tracking (Experimental)
     description: Follow large turns from the model path while retaining planned-curvature centering on the F-150 Lightning with C2 off.
-    details: Uses the existing controller's model-path geometry for large turns when the model and planned curvature agree. Smaller or opposing requests use planned curvature for centering. A bounded measured-turning correction requires fresh, valid steering-controller status and clears during driver override. During turn release or a reported steering limit, that correction can only reduce the existing turn request toward zero. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; other vehicles retain their existing controller. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
+    details: Uses the existing controller's model-path geometry for large turns when the model and planned curvature agree. Smaller or opposing requests use planned curvature for centering. A bounded measured-turning correction requires fresh, valid steering-controller status and clears during driver override. During turn release, a previous opposing correction can unwind when turning falls below both recent and current requests. When the steering controller reports a reached limit, the correction can only reduce the existing turn request toward zero. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; other vehicles retain their existing controller. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
     enablement:
     - $ref: '#/macros/offroad'
   - key: FordPscmObserver