Ford: validate pose state freshness and finish v6 deployment metadata

Check both motion publication and embedded filter-state ages before using calibrated yaw. Rebuild the pose when calibration changes between motion samples. Four regression cases demonstrate stale, future, missing-state fallback and calibration-only pose refresh. Keep the Sunnylink YAML source synchronized with its generated schema and update the drive guide and source-bound validation record for v6. Validation: 427 tests and 26 subtests pass; type, lint and settings compiler checks pass. All 340,757 recorded cycles still match the reviewed candidate, with 681,514 CAN round trips. Physical steering improvement remains unverified. Assisted-by: OpenAI Codex
2026-09-09 15:53:43 +08:00 · 2026-09-08 11:45:34 -04:00
parent d4f6403746
commit c70a9ee84b
5 changed files with 169 additions and 13 deletions
@@ -1,9 +1,12 @@
 # Ford selected-action drive-test branch

 The candidate is selectable on the **Ford CAN FD F-150 Lightning** behind
-its own persistent, default-off Sunnylink toggle. Version 5 retains full geometric
-path prediction and [removes yaw damping](ford_model_action_no_yaw_damping.md).
-Measured yaw is used only for input-health checks and diagnostics. Input gates are unchanged.
+its own persistent, default-off Sunnylink toggle. Version 6 uses calibrated
+measured turn rate for C0's existing 150 ms vehicle-pose forecast. C1 still uses
+selected, upstream-limited curvature. Raw Ford yaw remains an input-health and
+diagnostic signal; calibrated motion now affects C0. This is yaw feedback whose
+sensitivity depends on the existing preview time and path distance. There is no
+fitted PSCM plant model, added strength multiplier, new filter or extra core state.
 `calibration_approved=false`: offline checks do not establish physical tracking,
 turn-exit behavior or closed-loop stability.

@@ -20,7 +23,10 @@ turn-exit behavior or closed-loop stability.

 The startup log event `Ford path controller selected` should report
 `FordModelActionController`. Periodic `Ford C2-free path tracking` events
-identify `hypothesis=model-action-c0-c1-prediction-v5` and report host yaw and the command tuple.
+identify `hypothesis=model-action-measured-pose-v6`. Active events report
+`pose_source=measured` when fresh calibrated motion is used, otherwise
+`pose_source=requested`. They also include `pose_yaw_rate`, `pose_age`, raw host
+`yaw_rate`, selected curvature and the command tuple.

 Turning the new toggle off and completing another offroad-to-onroad cycle
 restores **PSCM Coefficient Observer** if selected, otherwise the original
@@ -41,6 +47,17 @@ existing Ford call path, selected upstream-limited curvature, service gates,
 invalid-output disengagement, Float32 publication and downstream CAN builder.
 C2 and C3 stay zero. No opendbc pointer or Panda safety change is included.

+Measured-pose use requires healthy `deviceMotion` and `extrinsicsCalibration`
+services, calibrated extrinsics no older than 1 s, valid angular velocity and
+sensor/input flags, and finite calibrated yaw within ±3 rad/s. Both the motion
+publication and its embedded filter-state timestamp must be no older than
+150 ms; all three age checks allow at most 5 ms future skew. Calibration-only
+updates rebuild the cached pose before the candidate uses it.
+Unavailable, unhealthy or stale optional motion falls back to the v5
+requested-curvature forecast without resetting C0/C1 slew. Existing base-input
+failures still invalidate the command. C1, the ±5.11 m / ±0.5 rad field caps,
+4 m/s / 0.5 rad/s slew rates, packing and two core states are unchanged.
+
 Sunnylink publishes the toggle through its generated settings schema and
 writes the registered Boolean through the existing parameter endpoint. The
 offroad UI rule and `needs_onroad_cycle` metadata describe when it can be
@@ -54,9 +71,22 @@ returns separate strings owned by the parameter handle. Regression tests
 check distinct registered keys across flags, and toggle tests check its
 persistence and backup registration using the rebuilt native library.

-The current validation record is `ford_model_action_no_yaw_damping_validation.json`;
-the [v5 notes](ford_model_action_no_yaw_damping.md) explain damping removal and its
-scope. `ford_model_action_full_prediction_validation.json` archives v4 checks;
+The current validation record is `ford_model_action_measured_pose_validation.json`.
+Its runtime replay exactly matches the reviewed offline candidate across
+340,757 recorded cycles and 681,514 Float32/CAN round trips. C1 and eligibility
+match v5 on all four extracts. The replay checks frozen recorded inputs; it does
+not simulate how the vehicle would respond to different commands.
+
+The candidate adds C0 during the recorded weak-bend turn-rate shortfalls and
+reduces it during the older overshoot example. Tight-turn C1 saturation remains.
+On the 70.24-second sustained quiet-path cohort, C0 amplitude RMS decreases but
+per-cycle change RMS rises about 28%; physical centering and tracking still need
+evaluation. All four routes came from the same truck, so cross-PSCM performance
+has not been demonstrated.
+
+`ford_model_action_no_yaw_damping_validation.json` archives v5 checks;
+the [v5 notes](ford_model_action_no_yaw_damping.md) explain the prior damping removal.
+`ford_model_action_full_prediction_validation.json` archives v4 checks;
 the [full-prediction notes](ford_model_action_full_prediction.md) explain cap removal
 and remaining physical uncertainty. `ford_model_action_prediction_validation.json`
 archives the capped v3 evaluation. `ford_model_action_damping_validation.json`
@@ -0,0 +1,94 @@
+{
+  "version": "model-action-measured-pose-v6",
+  "base_commit": "72e9d94f62487d07e9b7c5cb17d5988d5ddfbedb",
+  "opendbc_revision": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+  "scope": "Offline runtime command replay on frozen recorded calibrated yaw, geometry and vehicle inputs; separate controlsd integration tests exercise health/freshness, calibration updates, publication and CAN. No counterfactual vehicle simulation or road-validation claim.",
+  "calibration_approved": false,
+  "source_sha256": {
+    "openpilot/selfdrive/controls/controlsd.py": "c12e8d9be37e21f27a2132974b0efe98f3e681fd07e12c263e897ad67a44871b",
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "326539389b04034932db41ca2c67794779403b2c0e97f408a81a9f5734114899",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "2a55779b1a32f4dc3317b568365efbadc1f6aab06a0b3398514be891025d5e2d",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "0e131ca17380a5a4fe37754659f77aeda550f5e3c113edb27b576673e57bf6b4",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_measured_pose.py": "4d81efa0f09350559ff4290fd2ec11e744be221623fd414de85adfe2fecb4289",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "4e87ff347046d8de7248b9272a0393dd5d12e10df5430f00540b675331d7fe58",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py": "063b066aeddf0410dc208fa0017a58cda78effb1ca33c2af8b53422dbdad06f3",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "6f8900aa642d7f3883b003fd9244f29997343808fc22119c5ca953c3e1946fde",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "c8f7cc4c44e39027fecb52efc61dd5f3902e030193b10ac2f2c36c51cc6d4010"
+  },
+  "validation": {
+    "pytest": {
+      "passed": 427,
+      "subtests_passed": 26,
+      "skipped": 0,
+      "command": "python -m pytest -q openpilot/selfdrive/controls/tests/test_ford*.py tools/ford_pscm_lab opendbc_repo/opendbc/car/ford/tests/test_ford.py openpilot/sunnypilot/sunnylink/tests",
+      "output_sha256": "ed2ecdf280dadc52a834b2dfaa777e64505f7763d1a271849dadc729a86abb5d"
+    },
+    "type_check": "ty check openpilot/selfdrive/controls/lib/ford_model_action.py openpilot/selfdrive/controls/controlsd.py: pass",
+    "ruff_changed_python": "pass",
+    "settings_compiler_check": "pass",
+    "diff_check": "pass",
+    "regression_red_green": "49 measured-pose tests and 4 state-freshness/calibration tests failed before their implementation and passed afterward"
+  },
+  "replay": {
+    "cycles": 340757,
+    "wire_round_trips": 681514,
+    "command_matches_reviewed_candidate": true,
+    "baseline_matches_archived_v5": true,
+    "c1_eligibility_unchanged": true,
+    "checks": [
+      "finite values",
+      "caps",
+      "independent slew",
+      "inactive reset",
+      "Float32/CAN packing",
+      "zero C2 and C3",
+      "publication and filter-state freshness"
+    ],
+    "routes": {
+      "a2": {
+        "cycles": 71111,
+        "wire_round_trips": 142222,
+        "runtime_matches_reviewed_lab_candidate": true,
+        "baseline_matches_archive": true,
+        "identical_c1_eligibility": true
+      },
+      "a0": {
+        "cycles": 114582,
+        "wire_round_trips": 229164,
+        "runtime_matches_reviewed_lab_candidate": true,
+        "baseline_matches_archive": true,
+        "identical_c1_eligibility": true
+      },
+      "9b": {
+        "cycles": 71396,
+        "wire_round_trips": 142792,
+        "runtime_matches_reviewed_lab_candidate": true,
+        "baseline_matches_archive": true,
+        "identical_c1_eligibility": true
+      },
+      "9e": {
+        "cycles": 83668,
+        "wire_round_trips": 167336,
+        "runtime_matches_reviewed_lab_candidate": true,
+        "baseline_matches_archive": true,
+        "identical_c1_eligibility": true
+      }
+    },
+    "report_sha256": {
+      "a2_pose_report.json": "35d709991507ac7c10fd3148fd61dcf33a9606f97cd134d2bd94cee999f9cbe1",
+      "a0_pose_report.json": "1e34fbcdb14d04b23ce755ddd2e66fa6ab12377ab925693474e9f719b6339530",
+      "9b_pose_report.json": "e601662de44a354247f3943c233ca14ce5c449dbb4289ab9564ced21d524f02b",
+      "9e_pose_report.json": "fb0ee49e7aab65d45b61251aa83ad2247cd90e91f6c077f86d2f8474af7013e4"
+    },
+    "reproduce": "PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=.:opendbc_repo python .cache/ford_v6/replay.py a2 a0 9b 9e",
+    "reproduction_requirements": "Original route extracts in the local .cache directories and the matching scripts/reports in the ford-v6 artifact. Replay scripts and private recordings are not shipped on the device."
+  },
+  "limitations": [
+    "All route extracts came from one truck; cross-PSCM behavior is unverified.",
+    "C0 command changes are not percentages of physical steering improvement.",
+    "The older overshoot example has nearby driver influence.",
+    "The same C1 cap remains active during tight-turn peak misses.",
+    "Quiet-path per-cycle C0-change RMS increases about 28%; physical centering is unverified.",
+    "Hardware build, device boot and physical driving were not performed."
+  ]
+}
@@ -87,7 +87,7 @@ class Controls(ControlsExt):
    self.sm.update(15)
    if self.sm.updated["extrinsicsCalibration"]:
      self.pose_calibrator.feed_extrinsics_calibration(self.sm['extrinsicsCalibration'])
-    if self.sm.updated["deviceMotion"]:
+    if self.sm.updated["deviceMotion"] or (self.ford_model_action and self.sm.updated["extrinsicsCalibration"]):
      device_motion = Pose.from_device_motion(self.sm['deviceMotion'])
      self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_motion)

@@ -178,6 +178,7 @@ class Controls(ControlsExt):
        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['deviceMotion'] * 1e-9 <= .15 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=now,
@@ -185,7 +186,7 @@ class Controls(ControlsExt):
          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,
+          pose_time=motion.timestamp * 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:
@@ -16,13 +16,14 @@ from opendbc.can import CANParser
 from opendbc.car import Bus, structs
 from opendbc.car.ford.carcontroller import CarController
 from opendbc.car.ford.values import FordFlags
-from openpilot.cereal import custom
+from openpilot.cereal import custom, log
 from openpilot.selfdrive.car.helpers import convert_carControlSP
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
 from openpilot.selfdrive.controls.lib.ford_path import FordPath
 from openpilot.selfdrive.controls.tests.test_ford_model_action import circle, straight
 from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
+from openpilot.selfdrive.locationd.helpers import Pose, PoseCalibrator


 def update(controller, now=1., **overrides):
@@ -154,7 +155,8 @@ class Subscriptions:
                       'deviceMotion': 980_000_000, 'extrinsicsCalibration': 750_000_000}
    self.failed = set()
    self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1),
-                     'deviceMotion': SimpleNamespace(angularVelocityDevice=SimpleNamespace(valid=True), sensorsOK=True, inputsOK=True)}
+                     'deviceMotion': SimpleNamespace(angularVelocityDevice=SimpleNamespace(valid=True), sensorsOK=True, inputsOK=True,
+                                                     timestamp=970_000_000)}

  def __getitem__(self, service):
    return self.messages[service]
@@ -242,7 +244,8 @@ def test_actual_controlsd_service_gates(pipeline, maneuver, failed):

@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'])
+                                 'stale_motion', 'future_motion', 'stale_estimate', 'future_estimate', 'missing_estimate',
+                                 'nan_yaw', 'infinite_yaw', 'yaw_range'])
 def test_actual_controlsd_unhealthy_pose_retains_requested_pose_and_engagement(pipeline, fault):
  sm = Subscriptions(False)
  controls = startup()
@@ -272,6 +275,12 @@ def test_actual_controlsd_unhealthy_pose_retains_requested_pose_and_engagement(p
    sm.logMonoTime['deviceMotion'] = 849_000_000
  elif fault == 'future_motion':
    sm.logMonoTime['deviceMotion'] = 1_006_000_000
+  elif fault == 'stale_estimate':
+    motion.timestamp = 849_000_000
+  elif fault == 'future_estimate':
+    motion.timestamp = 1_006_000_000
+  elif fault == 'missing_estimate':
+    motion.timestamp = 0
  elif fault == 'nan_yaw':
    controls.calibrated_pose.angular_velocity.z = math.nan
  elif fault == 'infinite_yaw':
@@ -316,3 +325,25 @@ def test_pose_fallback_and_recovery_preserve_slew_states():
    assert out.path_angle == pytest.approx(.2)
    assert controller.diagnostics['pose_source'] == ('measured' if healthy else 'requested')
  assert states == pytest.approx([.44, .4, .44])
+
+
+def test_calibration_only_update_refreshes_pose_before_ford_uses_it():
+  controls = startup()
+  controls.pose_calibrator = PoseCalibrator()
+  motion = log.DeviceMotion.new_message()
+  motion.angularVelocityDevice.x = .5
+  motion.angularVelocityDevice.z = .1
+  controls.calibrated_pose = controls.pose_calibrator.build_calibrated_pose(Pose.from_device_motion(motion))
+  sm = Subscriptions(False)
+  sm.updated = {'extrinsicsCalibration': True, 'deviceMotion': False}
+  sm.update = lambda timeout: None
+  sm.messages.update(deviceMotion=motion, extrinsicsCalibration=log.ExtrinsicsCalibration.new_message(
+    rpyCalib=[0., .2, 0.], calStatus='calibrated'))
+  controls.sm = sm
+  controls_file = Path(__file__).resolve().parents[3]/'selfdrive/controls/controlsd.py'
+  method = _method(controls_file, 'Controls', 'update')
+  environment = {'Pose': Pose}
+  exec(compile(ast.Module(body=[method], type_ignores=[]), str(controls_file), 'exec'), environment)
+  environment['update'](controls)
+  assert controls.pose_calibrator.calib_valid
+  assert controls.calibrated_pose.angular_velocity.z == pytest.approx(math.sin(.2)*.5+math.cos(.2)*.1)
@@ -15,7 +15,7 @@ sections:
    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:
    - $ref: '#/macros/offroad'
  - key: FordPscmObserver