Ford: predict path offset from calibrated vehicle motion

Use fresh calibrated turn rate for the existing 150 ms C0 pose forecast.
Retain selected-curvature prediction when motion or calibration is unavailable,
and report the active pose source in controller diagnostics. C1, coefficient
limits, slew rates, C2/C3 zero, and the existing Sunnylink toggle are unchanged.

Validated 340,757 recorded cycles against the reviewed offline candidate,
681,514 CAN round trips, and the Ford/controller/Sunnylink test suite.
This is an experimental drive candidate, not a proven road-tracking fix.

Assisted-by: OpenAI Codex

openpilot/selfdrive/controls/controlsd.py

@@ -171,12 +171,21 @@ class Controls(ControlsExt):
     if self.CP.brand == "ford":
       ford_model = model_v2 if self.sm.valid['modelV2'] else None
       if self.ford_model_action:
+        assert isinstance(self.ford_path_controller, FordModelActionController)
         reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
+        now = time.monotonic()
+        motion = self.sm['deviceMotion']
+        pose_valid = (self.calibrated_pose is not None and self.pose_calibrator.calib_valid and
+                      self.sm.all_checks(['deviceMotion', 'extrinsicsCalibration']) and
+                      motion.angularVelocityDevice.valid and motion.sensorsOK and motion.inputsOK and
+                      -.005 <= now - self.sm.logMonoTime['extrinsicsCalibration'] * 1e-9 <= 1.)
         self.ford_path = self.ford_path_controller.update(
-          ford_model, self.desired_curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(),
+          ford_model, self.desired_curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=now,
           measurement_time=self.sm.logMonoTime['carState'] * 1e-9,
           model_time=self.sm.logMonoTime['modelV2'] * 1e-9,
           reference_time=self.sm.logMonoTime[reference_service] * 1e-9,
+          pose_yaw_rate=self.calibrated_pose.angular_velocity.z if pose_valid else None,
+          pose_time=self.sm.logMonoTime['deviceMotion'] * 1e-9, pose_valid=pose_valid,
           active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]),
         )
         if not self.ford_path.valid:
@@ -188,6 +197,7 @@ class Controls(ControlsExt):
                          measured_curvature=self.curvature,
                          **self.ford_path_controller.diagnostics)
       elif self.ford_pscm_observer:
+        assert isinstance(self.ford_path_controller, FordPscmObserverPathController)
         self.ford_path = self.ford_path_controller.update(ford_model, self.desired_curvature,
                                                           current_curvature=self.curvature, v_ego=CS.vEgo,
                                                           v_ego_raw=CS.vEgoRaw, active=CC.latActive)

openpilot/selfdrive/controls/lib/ford_model_action.py

@@ -18,8 +18,8 @@ CALIBRATION_APPROVED = False
 PREDICTION_TIME_S = .15  # geometric preview, not an identified actuator delay
 
 
-def _predict_offset(path, c0, desired_curvature, speed):
-  """Read the same path from a predicted pose along the selected curvature.
+def _predict_offset(path, c0, pose_curvature, speed):
+  """Read the same path from a predicted constant-curvature vehicle pose.
 
   A matched constant-radius path retains its offset. Developing/flattening
   bends can move the target earlier. The core retains field limits and slew.
@@ -31,7 +31,7 @@ def _predict_offset(path, c0, desired_curvature, speed):
     return c0
   x = float(np.interp(OFFSET_STATION_M+distance, station, longitudinal))
   y = float(np.interp(OFFSET_STATION_M+distance, station, lateral))
-  rotation = desired_curvature*distance
+  rotation = pose_curvature*distance
   # (1-cos(rotation))/curvature, evaluated without cancellation or division by zero.
   translation = distance*math.sin(rotation/2)*float(np.sinc(rotation/(2*math.pi)))
   predicted = math.cos(rotation)*y-math.sin(rotation)*x+translation
@@ -53,11 +53,14 @@ def _finite(*values):
     return False
 
 
-def encode_model_action(model, desired_curvature, speed):
+def encode_model_action(model, desired_curvature, speed, *, pose_yaw_rate=None):
   """Encode predicted y(7) and max(7, v*1s)*selected limited curvature.
 
   Preserve the reviewed core's endpoint hold when the path ends before 7 m.
   This samples the available geometry; it does not extrapolate an unseen path.
+  Calibrated measured yaw predicts the vehicle pose when available; otherwise
+  retain the selected-curvature prediction. This is geometric yaw feedback,
+  whose sensitivity depends on the existing preview time and path distance.
   """
   if not _finite(desired_curvature, speed) or not .3 <= speed <= 55 or abs(desired_curvature) > 1:
     return FordPath()
@@ -69,7 +72,10 @@ def encode_model_action(model, desired_curvature, speed):
     return FordPath()
   station, _, lateral, _ = path
   c0 = float(np.interp(min(OFFSET_STATION_M, station[-1]), station, lateral))
-  c0 = _predict_offset(path, c0, desired_curvature, speed)
+  pose_curvature = desired_curvature
+  if pose_yaw_rate is not None and _finite(pose_yaw_rate) and abs(pose_yaw_rate) <= 3:
+    pose_curvature = pose_yaw_rate / speed
+  c0 = _predict_offset(path, c0, pose_curvature, speed)
   c1 = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature
   return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
 
@@ -77,8 +83,9 @@ def encode_model_action(model, desired_curvature, speed):
 class ModelActionController:
   """Only two control states: unquantized, independently slewed C0 and C1.
 
-  Freshness and engagement belong to the caller. Measured yaw is checked for
-  input health only; it never changes valid offset or heading targets.
+  Freshness and engagement belong to the caller. Raw Ford yaw checks input
+  health only. A separate calibrated yaw input can change the offset forecast;
+  heading always follows the selected limited curvature.
   """
   __slots__ = ('c0', 'c1')
 
@@ -88,12 +95,12 @@ class ModelActionController:
   def reset(self):
     self.c0 = self.c1 = 0.
 
-  def update(self, model, desired_curvature, *, speed, dt, yaw_rate=0., active=True, valid=True):
-    # Retain the existing input-health gate without yaw feedback.
+  def update(self, model, desired_curvature, *, speed, dt, yaw_rate=0., active=True, valid=True, pose_yaw_rate=None):
+    # Raw Ford yaw remains an input-health check, not a pose measurement.
     if not active or not valid or not _finite(dt, yaw_rate) or not .002 <= dt <= .1 or abs(yaw_rate) > 3:
       self.reset()
       return FordPath()
-    target = encode_model_action(model, desired_curvature, speed)
+    target = encode_model_action(model, desired_curvature, speed, pose_yaw_rate=pose_yaw_rate)
     if not target.valid:
       self.reset()
       return FordPath()
@@ -109,12 +116,13 @@ class FordModelActionController:
 
   controlsd owns upstream selection/limiting and service health. This adapter
   checks ages and clock order, then supplies elapsed time to the two-state
-  core. Its timestamps and diagnostics never affect the targets. Raw model
-  geometry is checked on every cycle, even at a repeated model timestamp.
+  core. Pose age gates measured-motion use; diagnostics do not feed back into
+  the core. Raw model geometry is checked even at a repeated model timestamp.
 
-  Host-coordinate yaw supplies diagnostics and input-health checks. Engagement
-  and downstream driver arbitration still apply. PSCM status and driver torque
-  are not control-law inputs.
+  Raw Ford yaw supplies diagnostics and input-health checks. Fresh, healthy
+  calibrated motion supplies pose prediction; unavailable motion falls back
+  to the existing requested-pose forecast without resetting the slew states.
+  PSCM status and driver torque are not control-law inputs.
   """
   def __init__(self):
     self.core = ModelActionController()
@@ -123,11 +131,11 @@ 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-action-c0-c1-prediction-v5',
+    self.diagnostics = {'status': status, 'hypothesis': 'model-action-measured-pose-v6',
                         '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,
-             active, valid=True):
+             active, valid=True, pose_yaw_rate=None, pose_time=None, pose_valid=False):
     reason = None
     if not active:
       reason = 'inactive'
@@ -149,14 +157,21 @@ class FordModelActionController:
     ):
       self.reset('timing_reset')
       return FordPath()
-    command = self.core.update(model, desired_curvature, speed=speed, dt=dt, yaw_rate=yaw_rate)
+    pose_age = now - pose_time if _finite(pose_time) else None
+    if not _finite(pose_age):
+      pose_age = None
+    use_pose = (pose_valid and pose_yaw_rate is not None and pose_age is not None and
+                _finite(pose_yaw_rate) and abs(pose_yaw_rate) <= 3 and -.005 <= pose_age <= .15)
+    command = self.core.update(model, desired_curvature, speed=speed, dt=dt, yaw_rate=yaw_rate,
+                               pose_yaw_rate=pose_yaw_rate if use_pose else None)
     if not command.valid:
       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-action-c0-c1-prediction-v5',
+    self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-measured-pose-v6',
                         'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature,
-                        'yaw_rate': yaw_rate,
+                        'yaw_rate': yaw_rate, 'pose_source': 'measured' if use_pose else 'requested',
+                        'pose_yaw_rate': pose_yaw_rate if use_pose else None, 'pose_age': pose_age,
                         '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

@@ -49,11 +49,16 @@ class TestFordControlsLogging(unittest.TestCase):
     controller = FordModelActionController()
     for active, valid in ((False, True), (True, True), (True, False)):
       controller.update(circle(.01), .005, yaw_rate=.05, speed=20., now=1.,
-                        measurement_time=1., model_time=1., reference_time=1., active=active, valid=valid)
+                        measurement_time=1., model_time=1., reference_time=1., active=active, valid=valid,
+                        pose_yaw_rate=.04, pose_time=.98, pose_valid=True)
       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-action-c0-c1-prediction-v5')
+      self.assertEqual(record['hypothesis'], 'model-action-measured-pose-v6')
       self.assertIs(record['calibration_approved'], False)
       self.assertEqual(record['command'][2:], [0., 0.])
       self.assertEqual(record['status'], controller.diagnostics['status'])
+      if active and valid:
+        self.assertEqual(record['pose_source'], 'measured')
+        self.assertAlmostEqual(record['pose_yaw_rate'], .04)
+        self.assertAlmostEqual(record['pose_age'], .02)

openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py

@@ -150,9 +150,11 @@ class Subscriptions:
 
   def __init__(self, maneuver):
     self.valid = {'lateralManeuverPlan': maneuver, 'modelV2': True}
-    self.logMonoTime = {'carState': 995_000_000, 'modelV2': 980_000_000, 'lateralManeuverPlan': 990_000_000}
+    self.logMonoTime = {'carState': 995_000_000, 'modelV2': 980_000_000, 'lateralManeuverPlan': 990_000_000,
+                       'deviceMotion': 980_000_000, 'extrinsicsCalibration': 750_000_000}
     self.failed = set()
-    self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1)}
+    self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1),
+                     'deviceMotion': SimpleNamespace(angularVelocityDevice=SimpleNamespace(valid=True), sensorsOK=True, inputsOK=True)}
 
   def __getitem__(self, service):
     return self.messages[service]
@@ -164,11 +166,15 @@ class Subscriptions:
 @pytest.mark.parametrize('maneuver', [False, True])
 @pytest.mark.parametrize('host_yaw', [.0072, .3])
 @pytest.mark.parametrize('initial_curvature', [0., .005])
-def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver, host_yaw, initial_curvature):
+@pytest.mark.parametrize('calibrated_yaw', [None, -.05, .05])
+def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver, host_yaw, initial_curvature, calibrated_yaw):
   call, publication = pipeline
   sm = Subscriptions(maneuver)
   controls = startup()
   controller = controls.ford_path_controller
+  if calibrated_yaw is not None:
+    controls.pose_calibrator.calib_valid = True
+    controls.calibrated_pose = SimpleNamespace(angular_velocity=SimpleNamespace(z=calibrated_yaw))
   initial_curvature *= -1 if maneuver else 1
   controls.sm, controls.desired_curvature, controls.curvature = sm, initial_curvature, 0.
   if initial_curvature:
@@ -178,7 +184,8 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
   model.action = SimpleNamespace(desiredCurvature=.1)
   cc = structs.CarControl(latActive=True)
   cs = SimpleNamespace(vEgo=20., yawRate=-host_yaw, canValid=True, steeringPressed=False, steeringTorque=0.)
-  environment = {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
+  environment = {'FordModelActionController': FordModelActionController, 'self': controls, 'CS': cs, 'CC': cc,
+                     'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
                      'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}
   exec(call, environment)
   expected_curvature = initial_curvature+(-1 if maneuver else 1)*.000125
@@ -186,9 +193,12 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
   assert controls.ford_path.path_angle == pytest.approx(20.*expected_curvature)
   expected_offset = .04
   if initial_curvature:
-    expected_offset = .44 if maneuver else .36
+    grows = maneuver if calibrated_yaw is None else calibrated_yaw < 0.
+    expected_offset = .44 if grows else .36
   assert controls.ford_path.path_offset == pytest.approx(expected_offset)
   assert controller.diagnostics['yaw_rate'] == host_yaw
+  assert controller.diagnostics['pose_source'] == ('requested' if calibrated_yaw is None else 'measured')
+  assert controller.diagnostics['pose_yaw_rate'] == calibrated_yaw
   assert cc.latActive and cc.actuators.curvature == 0.
   assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
 
@@ -224,6 +234,85 @@ def test_actual_controlsd_service_gates(pipeline, maneuver, failed):
   cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
   model = straight()
   model.action = SimpleNamespace(desiredCurvature=.1)
-  exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
+  exec(pipeline[0], {'FordModelActionController': FordModelActionController, 'self': controls, 'CS': cs, 'CC': cc,
+                     'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
                          'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
   assert controls.ford_path.valid == cc.latActive == (failed == 'lateralManeuverPlan' and not maneuver)
+
+
+@pytest.mark.parametrize('fault', ['missing_pose', 'uncalibrated', 'motion_service', 'calibration_service', 'yaw_invalid',
+                                 'sensors_invalid', 'inputs_invalid', 'stale_calibration', 'future_calibration',
+                                 'stale_motion', 'future_motion', 'nan_yaw', 'infinite_yaw', 'yaw_range'])
+def test_actual_controlsd_unhealthy_pose_retains_requested_pose_and_engagement(pipeline, fault):
+  sm = Subscriptions(False)
+  controls = startup()
+  controls.sm, controls.desired_curvature, controls.curvature = sm, .01, 0.
+  controls.pose_calibrator.calib_valid = True
+  controls.calibrated_pose = SimpleNamespace(angular_velocity=SimpleNamespace(z=.05))
+  motion = sm.messages['deviceMotion']
+  if fault == 'missing_pose':
+    controls.calibrated_pose = None
+  elif fault == 'uncalibrated':
+    controls.pose_calibrator.calib_valid = False
+  elif fault == 'motion_service':
+    sm.failed.add('deviceMotion')
+  elif fault == 'calibration_service':
+    sm.failed.add('extrinsicsCalibration')
+  elif fault == 'yaw_invalid':
+    motion.angularVelocityDevice.valid = False
+  elif fault == 'sensors_invalid':
+    motion.sensorsOK = False
+  elif fault == 'inputs_invalid':
+    motion.inputsOK = False
+  elif fault == 'stale_calibration':
+    sm.logMonoTime['extrinsicsCalibration'] = -1_000_000
+  elif fault == 'future_calibration':
+    sm.logMonoTime['extrinsicsCalibration'] = 1_006_000_000
+  elif fault == 'stale_motion':
+    sm.logMonoTime['deviceMotion'] = 849_000_000
+  elif fault == 'future_motion':
+    sm.logMonoTime['deviceMotion'] = 1_006_000_000
+  elif fault == 'nan_yaw':
+    controls.calibrated_pose.angular_velocity.z = math.nan
+  elif fault == 'infinite_yaw':
+    controls.calibrated_pose.angular_velocity.z = math.inf
+  elif fault == 'yaw_range':
+    controls.calibrated_pose.angular_velocity.z = 3.01
+  controller = controls.ford_path_controller
+  controller.core.c0, controller.core.c1 = .4, .2
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=20., yawRate=-.3, canValid=True, steeringPressed=False, steeringTorque=0.)
+  model = straight(.4)
+  model.action = SimpleNamespace(desiredCurvature=.01)
+  exec(pipeline[0], {'FordModelActionController': FordModelActionController, 'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
+                    'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
+  assert cc.latActive and controls.ford_path.valid
+  assert controls.ford_path.path_offset == pytest.approx(.36)
+  assert controls.ford_path.path_angle == pytest.approx(.195)  # Upstream acceleration limiting still applies.
+  assert controller.diagnostics['pose_source'] == 'requested'
+  assert controller.diagnostics['pose_yaw_rate'] is None
+  json.dumps(controller.diagnostics, allow_nan=False)
+
+
+@pytest.mark.parametrize('pose_time', [None, math.nan, math.inf, -math.inf, 'bad', .849, 1.006])
+def test_invalid_pose_timestamp_uses_fallback_without_a_reset(pose_time):
+  controller = FordModelActionController()
+  controller.core.c0, controller.core.c1 = .4, .2
+  out = update(controller, pose_yaw_rate=.05, pose_time=pose_time, pose_valid=True)
+  assert out.path_offset == pytest.approx(.36)
+  assert out.path_angle == pytest.approx(.2)
+  assert controller.diagnostics['pose_source'] == 'requested'
+  json.dumps(controller.diagnostics, allow_nan=False)
+
+
+def test_pose_fallback_and_recovery_preserve_slew_states():
+  controller = FordModelActionController()
+  controller.core.c0, controller.core.c1 = .4, .2
+  states = []
+  for i, healthy in enumerate((True, False, True)):
+    now = 1.+i*.01
+    out = update(controller, now, pose_yaw_rate=-.1, pose_time=now, pose_valid=healthy)
+    states.append(controller.core.c0)
+    assert out.path_angle == pytest.approx(.2)
+    assert controller.diagnostics['pose_source'] == ('measured' if healthy else 'requested')
+  assert states == pytest.approx([.44, .4, .44])

openpilot/selfdrive/controls/tests/test_ford_model_action_measured_pose.py

@@ -0,0 +1,89 @@
+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
+from openpilot.selfdrive.controls.lib.ford_path import _model_path
+
+
+def circle(k, length=70.0):
+  s = np.linspace(0.0, length, 1401)
+  if k:
+    x, y = np.sin(k * s) / k, (1 - np.cos(k * s)) / k
+  else:
+    x, y = s, np.zeros_like(s)
+  return SimpleNamespace(position=SimpleNamespace(x=x, y=y), orientation=SimpleNamespace(z=k * s))
+
+
+@pytest.mark.parametrize('speed', [0.3, 4.0, 15.0, 30.0, 55.0])
+@pytest.mark.parametrize('curvature', [-0.08, -0.005, 0.0, 0.005, 0.08])
+def test_measured_pose_equals_requested_pose_when_tracking(speed, curvature):
+  m = circle(curvature)
+  # Values over the measurement's health range fall back to the same baseline.
+  assert encode_model_action(m, curvature, speed, pose_yaw_rate=speed * curvature) == encode_model_action(m, curvature, speed)
+
+
+@pytest.mark.parametrize('measured_curvature', [-0.02, 0.0, 0.004, 0.008, 0.02])
+@pytest.mark.parametrize('speed', [4.0, 15.0, 30.0])
+def test_offset_is_exact_rigid_transform_using_measured_pose(speed, measured_curvature):
+  m = circle(0.008)
+  out = encode_model_action(m, 0.008, speed, pose_yaw_rate=speed * measured_curvature)
+  s, x, y, _ = _model_path(m)
+  d = min(speed * 0.15, max(0.0, s[-1] - 7.0))
+  angle = measured_curvature * d
+  vehicle_x = math.sin(angle) / measured_curvature if measured_curvature else d
+  vehicle_y = (1 - math.cos(angle)) / measured_curvature if measured_curvature else 0.0
+  target_x = np.interp(7.0 + d, s, x)
+  target_y = np.interp(7.0 + d, s, y)
+  expected = -math.sin(angle) * (target_x - vehicle_x) + math.cos(angle) * (target_y - vehicle_y)
+  assert out.path_offset == pytest.approx(expected, abs=1e-12)
+  assert out.path_angle == encode_model_action(m, 0.008, speed).path_angle
+
+
+@pytest.mark.parametrize('invalid', [None, float('nan'), float('inf'), -float('inf'), 3.01, -3.01])
+def test_unavailable_measurement_retains_baseline_target(invalid):
+  m = circle(0.01)
+  assert encode_model_action(m, 0.01, 15.0, pose_yaw_rate=invalid) == encode_model_action(m, 0.01, 15.0)
+
+
+def test_undertracking_adds_offset_and_overtracking_releases_offset():
+  m = circle(0.01)
+  base = encode_model_action(m, 0.01, 15.0)
+  under = encode_model_action(m, 0.01, 15.0, pose_yaw_rate=0.10)
+  over = encode_model_action(m, 0.01, 15.0, pose_yaw_rate=0.20)
+  assert under.path_offset > base.path_offset > over.path_offset
+  assert under.path_angle == base.path_angle == over.path_angle
+
+
+def test_two_state_core_keeps_caps_slew_and_reset():
+  c = ModelActionController()
+  old = (0.0, 0.0)
+  for _ in range(200):
+    out = c.update(circle(0.2), 0.2, speed=20.0, dt=0.01, pose_yaw_rate=0.1)
+    assert out.valid and out.curvature == out.curvature_rate == 0.0
+    assert abs(c.c0 - old[0]) <= 0.0400000001
+    assert abs(c.c1 - old[1]) <= 0.0050000001
+    assert abs(out.path_offset) <= 5.11 and abs(out.path_angle) <= 0.5
+    old = c.c0, c.c1
+  assert c.__slots__ == ('c0', 'c1')
+  assert not c.update(circle(0.01), 0.01, speed=15.0, dt=0.01, active=False, pose_yaw_rate=0.1).valid
+  assert (c.c0, c.c1) == (0.0, 0.0)
+
+
+def test_measurement_fallback_and_recovery_keep_slew_and_selected_c1():
+  c = ModelActionController()
+  original = ModelActionController()
+  old = (0.0, 0.0)
+  for index in range(180):
+    curvature = 0.02 if index < 90 else -0.015
+    measured = 0.05 if index < 50 else None if index < 100 else -0.03
+    model = circle(curvature)
+    out = c.update(model, curvature, speed=15.0, dt=0.01, pose_yaw_rate=measured)
+    ref = original.update(model, curvature, speed=15.0, dt=0.01)
+    assert out.path_angle == ref.path_angle
+    assert abs(c.c0 - old[0]) <= 0.0400000001
+    assert abs(c.c1 - old[1]) <= 0.0050000001
+    assert out.curvature == out.curvature_rate == 0.0
+    old = c.c0, c.c1

openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py

@@ -27,7 +27,8 @@ def startup(cp=None, params=None):
   end = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_path')
   if params is None:
     params = SimpleNamespace(get_bool=lambda key: key == 'FordModelActionController')
-  controls = SimpleNamespace(CP=cp or car_params(), params=params)
+  controls = SimpleNamespace(CP=cp or car_params(), params=params, calibrated_pose=None,
+                             pose_calibrator=SimpleNamespace(calib_valid=False))
   environment = {'self': controls, 'FordFlags': FordFlags, 'FordPath': FordPath,
                  'FordPathController': FordPathController, 'FordPscmObserverPathController': FordPscmObserverPathController,
                  'FordModelActionController': FordModelActionController,

openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py

@@ -1,4 +1,4 @@
-"""Measured yaw gates input health but cannot attenuate path demand."""
+"""Raw Ford yaw gates input health but cannot change path demand."""
 import math
 
 import pytest

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -2184,7 +2184,7 @@
           "needs_onroad_cycle": true,
           "title": "Selected-Action Path Tracking (Experimental)",
           "description": "Follow the selected steering plan with nearby model-path centering on the Ford CAN FD F-150 Lightning.",
-          "details": "Uses a short prediction of the nearby model path to respond as bends develop, plus a heading request based on selected planned curvature. The predicted offset uses the full geometric request within the existing command limits and rate limits. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. 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.",
+          "details": "Uses measured vehicle motion to predict the nearby model path, plus a heading request based on selected planned curvature. If the motion measurement is unavailable, prediction uses the requested turn rate. The existing command limits and rate limits remain in effect. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. 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"