diff --git a/docs/ford_model_action_drive_test.md b/docs/ford_model_action_drive_test.md index 852c2a4ad7..541049d698 100644 --- a/docs/ford_model_action_drive_test.md +++ b/docs/ford_model_action_drive_test.md @@ -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` diff --git a/docs/ford_model_action_measured_pose_validation.json b/docs/ford_model_action_measured_pose_validation.json new file mode 100644 index 0000000000..b42944fd5f --- /dev/null +++ b/docs/ford_model_action_measured_pose_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." + ] +} diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index fe2f466fc3..8b7bf69b6b 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -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: diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py index 750e3bf359..e62ce1b7aa 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -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) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index e6c5d7e5b2..855942d7de 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -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