From 22d188776cb557acea459a1fca70812bdb2df46c Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Sun, 13 Sep 2026 00:45:55 -0400 Subject: [PATCH] Ford: release completed unwind correction after catch-up --- docs/ford_model_action_drive_test.md | 21 +- docs/ford_unwind_catchup.md | 122 ++++++ docs/ford_unwind_catchup_validation.json | 346 ++++++++++++++++++ .../controls/lib/ford_model_action.py | 37 +- .../tests/test_ford_controlsd_logging.py | 2 +- .../controls/tests/test_ford_model_action.py | 3 +- .../tests/test_ford_model_action_adapter.py | 54 ++- .../tests/test_ford_model_action_unwind.py | 127 +++++++ tools/ford_pscm_lab/feedback_replay.py | 68 +++- 9 files changed, 746 insertions(+), 34 deletions(-) create mode 100644 docs/ford_unwind_catchup.md create mode 100644 docs/ford_unwind_catchup_validation.json create mode 100644 openpilot/selfdrive/controls/tests/test_ford_model_action_unwind.py diff --git a/docs/ford_model_action_drive_test.md b/docs/ford_model_action_drive_test.md index edf2d6a0bf..40003e41ba 100644 --- a/docs/ford_model_action_drive_test.md +++ b/docs/ford_model_action_drive_test.md @@ -2,7 +2,8 @@ The current experiment adds [measured-curvature C1 feedback](ford_c1_feedback.md) and [conditional correction release](ford_c1_carryover.md) to the restored -original v1 mapping, with [base C1 overflow allocated to C0](ford_c1_overflow.md). +original v1 mapping, with [base C1 overflow allocated to C0](ford_c1_overflow.md) +and [completed-unwind correction release](ford_unwind_catchup.md). It is selectable on **any Ford CAN FD vehicle** through the existing persistent, default-off Sunnylink toggle. Offline checks establish software behavior; physical tracking, @@ -20,7 +21,7 @@ turn-exit behavior and closed-loop stability remain unvalidated. The startup event `Ford path controller selected` should report `FordModelActionController`. Periodic `Ford C2-free path tracking` events -identify **`hypothesis=model-action-c1-feedback-v4`**. They report desired and +identify **`hypothesis=model-action-c1-feedback-v5`**. They report desired and measured curvature, base heading, accumulated correction, applied heading, feedback timing and driver/PSCM gating. `carryover_release_count` counts conditional releases since the last controller reset; it does not control @@ -29,6 +30,8 @@ amplitude and slew limits. `calibration_approved=false` remains. `request_release` reports correction retired on the current cycle when a changed request and sufficiently large measured error agree. Periodic logs can miss individual retirement cycles. +`unwind_direction` remembers an unfinished unwind; `unwind_release` reports +correction retired when a confirmed unwind catches the selected curvature. Turning the toggle off and completing another offroad-to-onroad cycle restores **upstream Ford curvature control**: 20 Hz steering messages, limited mode on @@ -50,8 +53,11 @@ With fresh feedback, the controller can discard an opposing correction when it prevents C1 from following the direction shared by original model C0, applied C0 and base C1, while measured curvature is still opposite. A separate bounded release now handles changed requests within the same turn when measured error -also opposes the old correction. Matched curvature preserves correction; final -output slew still applies. See the [current release rule](ford_c1_request_release.md). +also opposes the old correction. V5 additionally retires dominant unwind memory +at catch-up when the selected request reaches zero/new-side curvature and both +C0 requests confirm that side. Steady requests cannot arm this release, and +catching an old-side bend retains correction. Final output slew still applies. +See [completed-unwind release](ford_unwind_catchup.md). C0 starts with the original 7 m model-path mapping. When the raw base heading exceeds ±0.5 rad, C0 additionally receives 7 m times the clipped-away heading. @@ -63,9 +69,10 @@ place. An explicit selection flag distinguishes upstream mode from an invalid experimental command; invalid experimental input cannot switch to upstream. The opendbc sender restores upstream behavior when that flag is false. -See [changed-request release and validation](ford_c1_request_release.md) and -`ford_c1_request_release_validation.json` for current evidence and reproduction -commands. The [overflow specification](ford_c1_overflow.md) and +See [completed-unwind release and validation](ford_unwind_catchup.md) and +`ford_unwind_catchup_validation.json` for current evidence and reproduction +commands. [Changed-request release](ford_c1_request_release.md) and its +validation JSON record v4. The [overflow specification](ford_c1_overflow.md) and `ford_c1_overflow_validation.json` record v3. The carryover specification and `ford_c1_carryover_validation.json` record the previous experiment. `ford_c1_feedback_validation.json` records the initial feedback version at `5fbb583e5`. `ford_model_action_validation.json` and diff --git a/docs/ford_unwind_catchup.md b/docs/ford_unwind_catchup.md new file mode 100644 index 0000000000..b00e1d82d2 --- /dev/null +++ b/docs/ford_unwind_catchup.md @@ -0,0 +1,122 @@ +# Ford completed-unwind correction release + +Version `model-action-c1-feedback-v5` releases dominant C1 correction after a +confirmed unwind reaches the selected curvature. This fixes stored correction +continuing to request a new turn after its original unwind is complete. + +Route 115 (`codex-last2`) ran v4, `6df5eabb7`. Near 2:13.2, desired and actual +steering were both near zero after a right turn, but C1 still requested about +0.1165 rad left. About 0.114 rad was accumulated unwind correction. V4's +changed-request release did not apply: the small new left request was increasing +while the measured error called for less left steering. The earlier reversal +release also did not apply because measured and requested curvature were +already on the same side. + +## Rule + +On a fresh steering measurement, remember an unwind direction when the selected +curvature relaxes toward zero (or crosses it), measured curvature remains on +the previous side, and measured error calls for leaving that old turn. + +When measured error reaches or passes zero in that unwind direction, release +the stored correction only if all of these agree: + +- The selected curvature has reached zero or crossed into the unwind direction. +- Correction points in that direction and exceeds the magnitude of base C1. +- Original model C0 and applied C0 both confirm that direction by at least the + existing 0.01 m DBC step. + +Consume the unwind marker at this first catch-up, even if the other conditions +prevent release. A steady old-side bend, neutral/conflicting C0, or a correction +smaller than base C1 retains its correction. Steady requests cannot arm the +marker. Duplicate steering publications cannot arm or consume it. Driver/PSCM +feedback inhibition and controller resets clear it; correction reversing +direction also clears it. + +After retirement, the v4 command law still runs: bounded changed-request +release, reversal release, measured-error integration, PSCM arbitration, and +final C1 slew. Removing stored correction therefore does not jump the output. +There is one additional control state, `unwind_direction`; `unwind_release` +only reports the signed correction retired on the current cycle. Periodic +diagnostics may miss individual release cycles. + +This is a conditional correction-reset policy, not a PSCM plant model. It adds +no strength multiplier. The existing 1:1 feedback choice, C0 mapping/overflow, +C2/C3 zeroing, amplitude/slew limits, sender cadence and input gates remain. +Toggle off still selects upstream Ford control; toggle on selects the experiment +on Ford CAN FD platforms. See [selection and restore](ford_model_action_drive_test.md). + +## Exact exit replay + +Frozen route 115 measurements trigger one release at **2:13.203501**. The +selected steering angle is 0.469 degrees left and measured angle is 0.500 degrees +left. The controller retires 0.114026 rad of left unwind correction. Its first +C1 output moves from 0.1165 to 0.1110 rad left, respecting the original slew. + +At **2:13.594615**, old C1 is **0.1240 rad left**, versus **0.0105 rad left** in +v5. C0 is identical. The earlier unwind (2:09 through 2:13.2), comparison turn +(3:20 through 3:34), and comparison bend (5:33 through 5:45) have identical +commands throughout their windows. + +The recorded wheel motion stays fixed in this replay. It does not predict a +new steering angle, prove stability, or establish that the full overshoot is +fixed. This maneuver also includes driver input and a changing C0 request; +neither its whole swing nor every hanging exit can be attributed to stored I. + +## Validation + +Four targeted regressions failed on v4 because correction persisted after +catch-up; all now pass. Expanded tests cover both directions, fresh/duplicate +feedback, catch-up confirmation, steady tracking/noise, old-side bends, C0 +agreement, dominant correction, reset/override, limit-reached behavior and slew. +Integration exercises actual controlsd request selection/limiting for model +and maneuver sources, Float32 publication, and Ford CAN packing/checksums. + +The combined suite passes **753 tests and 9,145 subtests**, with **178 inherited +or unsupported safety-test skips**. Random feedback stress covers 200,000 cycles +and their mirrors. Each cycle exactly matches v4 after only the declared new +retirement; 59 cycles retire correction. Independent zero-error checks cover +200,000 cycles and 18,138 field-boundary cases. Ruff, controller Ty and settings +compilation checks pass. + +| Frozen route | Cycles | New releases | Changed C1 cycles | Largest C1 difference | +| --- | ---: | ---: | ---: | ---: | +| 114 | 61,027 | 4 | 2,849 | 0.0150 rad | +| 115 | 40,037 | 1 | 384 | 0.1140 rad | +| 112 | 108,971 | 14 | 30,036 | 0.0720 rad | +| 113 | 49,614 | 1 | 951 | 0.0050 rad | +| Historical b9 | 90,774 | 16 | 7,013 | 0.1435 rad | + +All routes compare v5 against v4 with the same recorded inputs. Activation and +C0 match exactly on all 350,423 cycles. Releases also occur at smaller exits; +changed history can affect subsequent ordinary bends. These results do not +establish unchanged physical centering. Route 114's large segment-2 overshoot +does not trigger this new release, so it remains a separate unresolved case. + +The five replays and two stress runs verify **768,561 Float32/CAN round trips**, +separately from the integration suite. Exact source hashes and numerical +results are in `ford_unwind_catchup_validation.json`. + +## Reproduction + +Use the native project Python dependencies and unchanged opendbc revision +`64aa61b9b3fd26e70a7caa915acab207ff3cd64a`. Route replay requires the full-rlog +extracts identified by validation hashes; the original logs are not modified. + +```sh +export PYTHONDONTWRITEBYTECODE=1 +export PYTHONPATH=.:opendbc_repo +export PARAMS_ROOT=/tmp/ford-v5-test-params +export LOG_ROOT=/tmp/ford-v5-test-logs +python -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/sunnypilot/sunnylink/tests openpilot/common/tests/test_params.py opendbc_repo/opendbc/car/ford/tests/test_ford.py opendbc_repo/opendbc/safety/tests/test_ford.py +python -m tools.ford_pscm_lab.feedback_replay stress --cycles 200000 --output .cache/ford_unwind_catchup/stress.json +python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260913 --opendbc-revision 64aa61b9b3fd26e70a7caa915acab207ff3cd64a --output .cache/ford_unwind_catchup/zero_error.json +for route in 114 115 112 113 b9; do + python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_route${route} --baseline 6df5eabb7e7f6bc4e206644d5ca9069df820124e --output .cache/ford_unwind_catchup/route${route} +done +``` + +Publication time approximates the computation clock; full SubMaster health is +not reconstructable. These route replays do not reconstruct selected maneuver +messages; integration tests cover that source. No device build, boot, +installation or physical steering test is performed offline. diff --git a/docs/ford_unwind_catchup_validation.json b/docs/ford_unwind_catchup_validation.json new file mode 100644 index 0000000000..9696cf66a4 --- /dev/null +++ b/docs/ford_unwind_catchup_validation.json @@ -0,0 +1,346 @@ +{ + "hypothesis": "model-action-c1-feedback-v5", + "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e", + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "scope": "Software command release and numerical invariants only; no counterfactual steering motion, physical stability or improved tracking claim.", + "calibration_approved": false, + "tests": { + "passed": 753, + "subtests_passed": 9145, + "skipped": 178, + "log_sha256": "64c683622cc91125e32cb0d78f4a5340b8d58fa149d90b06361629394489731d", + "initial_regression": "4 failed on v4: stored unwind correction remains after catch-up; all pass on v5", + "regression_log_sha256": "0ced2a0686cccba3c321c58749a679843a3179fa57078a6b030d20f0f9ae33e2" + }, + "feedback_stress": { + "cycles": 200000, + "mirrored_updates": 200000, + "can_round_trips": 200000, + "carryover_release_count": 181, + "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e", + "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f", + "seed": 20260913, + "request_release_cycles": 20214, + "unwind_release_cycles": 59, + "exact_unchanged_state_and_commands_without_unwind_release": 199941, + "exact_v4_match_after_only_declared_retirement": 200000, + "checks": "Symmetry, resets, amplitude/slew, bounded retirement, carryover confirmation, integration, PSCM limits, CAN.", + "scope": "Numerical software invariants only; no model of vehicle motion.", + "calibration_approved": false, + "controller_sha256": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e" + }, + "zero_error_stress": { + "seed": 20260913, + "random_cycles": 200000, + "mirrored_core_updates": 200000, + "invalid_or_inactive_resets": 3537, + "field_boundary_cases": 18138, + "float32_can_round_trips": 218138, + "analytic_targets_scalar_slew_and_mirror_checks_pass": true, + "direct_raw_float32_packing_matches_host_output": true, + "max_continuous_step_c0_c1": [ + 0.4000000000000019, + 0.05000000000000002 + ], + "calibration_approved": false, + "scope": "Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.", + "opendbc_import_head": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/stress_model_action.py": "cec2619285dd41274562ac035ee8ea0a389269a0c4ef1b62efa6252ad1a714aa", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "af97c665f342c66b1be2502e188c63e6f3ee106d0a0d5e80997bc3040373ff9f" + } + }, + "replays": { + "114": { + "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e", + "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f", + "calibration_approved": false, + "cycles": 61027, + "active_cycles": 48419, + "validity_matches_baseline_exactly": true, + "status_counts": { + "inactive": 12608, + "active": 48419 + }, + "c0_matches_baseline_exactly": true, + "c0_changed_cycles": 0, + "max_abs_c0_change_m": 0.0, + "offset_overflow_seconds": 6.548916987999917, + "max_abs_offset_overflow_target_m": 1.233181856572628, + "request_release_cycles": 288, + "request_release_seconds": 2.98796386799998, + "max_abs_request_release_rad": 0.006280367394214892, + "unwind_release_cycles": 4, + "max_abs_unwind_release_rad": 0.01457856219656457, + "feedback_enabled_seconds": 438.3550780740002, + "pscm_limit_2_seconds": 8.604224759000118, + "c1_changed_cycles": 2849, + "max_abs_c1_change_rad": 0.015000000000000013, + "max_abs_correction_rad": 0.2921633626620207, + "can_round_trips": 61027, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e" + }, + "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.", + "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.", + "report_sha256": "9a9d7a6c92ec2cc1e19dc6b6e628e402cbe3f6344e09ccbee1c5c76deedebc7e", + "commands_sha256": "fc1a5249cc9a15e3369f177a88781377c327e27b681e1028ad26063cb70167a5" + }, + "115": { + "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e", + "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f", + "calibration_approved": false, + "cycles": 40037, + "active_cycles": 33976, + "validity_matches_baseline_exactly": true, + "status_counts": { + "inactive": 6061, + "active": 33976 + }, + "c0_matches_baseline_exactly": true, + "c0_changed_cycles": 0, + "max_abs_c0_change_m": 0.0, + "offset_overflow_seconds": 2.095635206999873, + "max_abs_offset_overflow_target_m": 0.6360401958227158, + "request_release_cycles": 181, + "request_release_seconds": 1.7931359259980582, + "max_abs_request_release_rad": 0.005393094571379859, + "unwind_release_cycles": 1, + "max_abs_unwind_release_rad": 0.11402585053291069, + "unwind_releases": [ + { + "time_s": 133.203500567, + "retired_rad": -0.11402585053291069, + "correction_after_rad": 0.0, + "base_c1_rad": -0.0025691102200653404, + "desired_angle_deg": 0.46853354573249817, + "actual_angle_deg": 0.5, + "baseline_c0_c1": [ + -0.04999999999999982, + -0.11650000000000005 + ], + "candidate_c0_c1": [ + -0.04999999999999982, + -0.11099999999999999 + ] + } + ], + "feedback_enabled_seconds": 306.2414766659987, + "pscm_limit_2_seconds": 6.4724571650002645, + "c1_changed_cycles": 384, + "max_abs_c1_change_rad": 0.1140000000000001, + "max_abs_correction_rad": 0.13572457044904282, + "can_round_trips": 40037, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e" + }, + "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.", + "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.", + "report_sha256": "e3e4183e14ce20b2d3b0938028a5a4ab3200f72dd46e898174751aa94912ad84", + "commands_sha256": "ba9b6411a432430bb1380f63fe011ba91235b393f3785c17d2904657c95d27d9" + }, + "112": { + "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e", + "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f", + "calibration_approved": false, + "cycles": 108971, + "active_cycles": 91414, + "validity_matches_baseline_exactly": true, + "status_counts": { + "inactive": 17557, + "active": 91414 + }, + "c0_matches_baseline_exactly": true, + "c0_changed_cycles": 0, + "max_abs_c0_change_m": 0.0, + "offset_overflow_seconds": 1.43945091800002, + "max_abs_offset_overflow_target_m": 0.5727187991142273, + "request_release_cycles": 381, + "request_release_seconds": 3.8611647240012985, + "max_abs_request_release_rad": 0.008723706007003784, + "unwind_release_cycles": 14, + "max_abs_unwind_release_rad": 0.07267236868778636, + "feedback_enabled_seconds": 840.7664582650004, + "pscm_limit_2_seconds": 14.441855805999936, + "c1_changed_cycles": 30036, + "max_abs_c1_change_rad": 0.07200000000000006, + "max_abs_correction_rad": 0.205313389369823, + "can_round_trips": 108971, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e" + }, + "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.", + "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.", + "report_sha256": "aaa1d819cf344de668ea92eb58714a6ae537054a0feadc05a0fa15097a57c553", + "commands_sha256": "9f191699d19af8f123e9245a77827a2eca27d4b3191f9acddf9389edbda0ef24" + }, + "113": { + "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e", + "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f", + "calibration_approved": false, + "cycles": 49614, + "active_cycles": 27207, + "validity_matches_baseline_exactly": true, + "status_counts": { + "inactive": 22407, + "active": 27207 + }, + "c0_matches_baseline_exactly": true, + "c0_changed_cycles": 0, + "max_abs_c0_change_m": 0.0, + "offset_overflow_seconds": 2.7607263189997866, + "max_abs_offset_overflow_target_m": 0.6065444126725197, + "request_release_cycles": 119, + "request_release_seconds": 1.2281319819985583, + "max_abs_request_release_rad": 0.009946223348379135, + "unwind_release_cycles": 1, + "max_abs_unwind_release_rad": 0.005007614799767142, + "feedback_enabled_seconds": 243.3033761190004, + "pscm_limit_2_seconds": 14.003248144999816, + "c1_changed_cycles": 951, + "max_abs_c1_change_rad": 0.00500000000000006, + "max_abs_correction_rad": 0.16966817302181283, + "can_round_trips": 49614, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e" + }, + "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.", + "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.", + "report_sha256": "032aaf4b2dd3a54264330c9c6367ad4a63f49d779ce9acd4c3c1d355d731d545", + "commands_sha256": "c1d4e59718400d21d724d502e2befa315db56802a7622d3d68e8b3af7daea319" + }, + "b9": { + "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e", + "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f", + "calibration_approved": false, + "cycles": 90774, + "active_cycles": 86474, + "validity_matches_baseline_exactly": true, + "status_counts": { + "inactive": 4300, + "active": 86474 + }, + "c0_matches_baseline_exactly": true, + "c0_changed_cycles": 0, + "max_abs_c0_change_m": 0.0, + "offset_overflow_seconds": 5.703841178999909, + "max_abs_offset_overflow_target_m": 4.280821338295937, + "request_release_cycles": 416, + "request_release_seconds": 4.241454379000558, + "max_abs_request_release_rad": 0.017186015844345093, + "unwind_release_cycles": 16, + "max_abs_unwind_release_rad": 0.15648483206475247, + "feedback_enabled_seconds": 816.0284774219999, + "pscm_limit_2_seconds": 9.308613716000167, + "c1_changed_cycles": 7013, + "max_abs_c1_change_rad": 0.14349999999999996, + "max_abs_correction_rad": 0.18457476562660308, + "can_round_trips": 90774, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e" + }, + "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.", + "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.", + "report_sha256": "d66308cefae9956d2b67b8d9ceb83804054e838c70e84999ba758ab1cbf21d5d", + "commands_sha256": "406275e9c230a2975fd8c9d0824d3a902283d3a5cb597328026578db66c2cb6b" + } + }, + "checks": { + "ruff": true, + "controller_ty": true, + "settings_compilation": true + }, + "limitations": [ + "Recorded driver input and PSCM response remain fixed during replay.", + "No device build, boot or physical test performed.", + "Releases occur at smaller exits too; unchanged real-world centering is not established.", + "Route 114 segment-2 overshoot does not trigger this release." + ], + "float32_can_round_trips_excluding_integration_tests": 768561, + "exit_points_left_positive": [ + { + "time_s": 131.19208205699988, + "baseline_c1_rad": -0.10949999999999999, + "candidate_c1_rad": -0.10949999999999999, + "baseline_correction_rad": 0.04102452737994574, + "candidate_correction_rad": 0.04102452737994574 + }, + { + "time_s": 133.19194825599993, + "baseline_c1_rad": 0.11650000000000005, + "candidate_c1_rad": 0.11650000000000005, + "baseline_correction_rad": 0.11402585053291069, + "candidate_correction_rad": 0.11402585053291069 + }, + { + "time_s": 133.203500567, + "baseline_c1_rad": 0.11650000000000005, + "candidate_c1_rad": 0.11099999999999999, + "baseline_correction_rad": 0.1140256728569634, + "candidate_correction_rad": -0.0 + }, + { + "time_s": 133.40571050699987, + "baseline_c1_rad": 0.118, + "candidate_c1_rad": 0.009500000000000064, + "baseline_correction_rad": 0.11367557589137142, + "candidate_correction_rad": -0.0 + }, + { + "time_s": 133.59461515599992, + "baseline_c1_rad": 0.124, + "candidate_c1_rad": 0.010500000000000065, + "baseline_correction_rad": 0.1120761218192909, + "candidate_correction_rad": -0.0015653052344988395 + }, + { + "time_s": 207.90007524899988, + "baseline_c1_rad": 0.16449999999999998, + "candidate_c1_rad": 0.16449999999999998, + "baseline_correction_rad": 0.016763380618580685, + "candidate_correction_rad": 0.016763380618580685 + }, + { + "time_s": 338.168560822, + "baseline_c1_rad": -0.173, + "candidate_c1_rad": -0.173, + "baseline_correction_rad": -0.024831306563109386, + "candidate_correction_rad": -0.024831306563109386 + } + ], + "identical_route115_command_windows_s": [ + [ + 129.0, + 133.2 + ], + [ + 200.0, + 214.0 + ], + [ + 333.0, + 345.0 + ] + ] +} diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 16118fdd89..173891fb8c 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -5,6 +5,7 @@ engineering choices. Feeding integrated heading mismatch into C1 at 1:1 is an explicit feedback-strength choice, not an identified PSCM model or calibration. Opposed correction may be released when both path commands confirm the turn. Changed requests retire opposing correction only while measured error agrees. +Completed, direction-confirmed unwinds release dominant old correction. Base heading clipped by C1 is allocated to C0 at the existing 7 m reference. """ import math @@ -55,12 +56,13 @@ def encode_model_action(model, desired_curvature, speed): class ModelActionController: - """C0/C1 slew positions, C1 correction and the last feedback request. + """C0/C1 slew, C1 correction, last feedback request and unwind direction. Feedback integrates requested minus measured curvature over traveled distance. Freshness, measurement cadence and driver/PSCM arbitration belong to the caller. """ - __slots__ = ('c0', 'c1', 'correction', 'carryover_release_count', 'last_feedback_desired', 'request_release') + __slots__ = ('c0', 'c1', 'correction', 'carryover_release_count', 'last_feedback_desired', 'request_release', + 'unwind_direction', 'unwind_release') def __init__(self): self.reset() @@ -70,6 +72,8 @@ class ModelActionController: self.carryover_release_count = 0 # Diagnostic only; never feeds the command law. self.last_feedback_desired = None self.request_release = 0. # Diagnostic radians retired on this cycle. + self.unwind_direction = 0. + self.unwind_release = 0. # Diagnostic only; final output still obeys slew. def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True, feedback_dt=None, feedback_enabled=True, pscm_limited=False): @@ -90,9 +94,30 @@ class ModelActionController: lower = max(-.5, self.c1-.5*dt) upper = min(.5, self.c1+.5*dt) self.request_release = 0. + self.unwind_release = 0. if not feedback_enabled: self.correction = 0. + self.unwind_direction = 0. else: + error = desired_curvature-current_curvature + if feedback_dt > 0.: + # Remember an unwind only when a relaxing request and excess measured + # steering call for leaving the old turn. This may precede accumulation + # while the output is still slewing. A steady request cannot arm it. + previous = self.last_feedback_desired + if (previous is not None and (desired_curvature-previous)*previous < 0. + and current_curvature*previous > 0. and error*previous < 0.): + self.unwind_direction = math.copysign(1., error) + direction = self.unwind_direction + if direction and error*direction <= 0.: + # Steering has caught the request. Retire dominant unwind memory only + # after the selected request crosses zero and both path offsets also + # confirm the new side. Catching a steady old-side bend retains I. + if (desired_curvature*direction >= 0. and self.correction*direction > abs(base_c1) + and min(target.path_offset*direction, self.c0*direction) >= .01): + self.unwind_release = self.correction + self.correction = 0. + self.unwind_direction = 0. # A changed target can make old correction counterproductive before # steering reverses. Retire at most the heading change, and only when # fresh measured error calls for that same change, by at least a C1 DBC @@ -106,7 +131,6 @@ class ModelActionController: distance = max(OFFSET_STATION_M, speed*HEADING_TIME_S) previous_base = float(np.clip(distance*self.last_feedback_desired, -.5, .5)) change = base_c1-previous_base - error = desired_curvature-current_curvature if (abs(change) >= .0005 and change*error > 0. and change*self.correction < 0. and abs(distance*error) >= abs(self.correction)): self.request_release = float(np.clip(change, min(-self.correction, 0.), max(-self.correction, 0.))) @@ -134,6 +158,8 @@ class ModelActionController: # never rewrite existing correction merely because the base changed. request = base_c1+self.correction self.correction += float(np.clip(increment, min(lower-request, 0.), max(upper-request, 0.))) + if self.correction*self.unwind_direction < 0.: + self.unwind_direction = 0. if self.last_feedback_desired is None or feedback_dt > 0. or not feedback_enabled: self.last_feedback_desired = desired_curvature c1 = float(np.clip(base_c1+self.correction, -.5, .5)) @@ -160,7 +186,7 @@ class FordModelActionController: def reset(self, status='inactive'): self.core.reset() self.last_time = self.last_measurement_time = self.last_model_time = None - self.diagnostics = {'status': status, 'hypothesis': 'model-action-c1-feedback-v4', + self.diagnostics = {'status': status, 'hypothesis': 'model-action-c1-feedback-v5', 'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)} def update(self, model, desired_curvature, *, current_curvature, yaw_rate, speed, now, measurement_time, model_time, @@ -202,7 +228,7 @@ class FordModelActionController: self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time raw_heading = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature base_heading = float(np.clip(raw_heading, -.5, .5)) - self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c1-feedback-v4', + self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c1-feedback-v5', 'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature, 'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time, 'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1, @@ -211,6 +237,7 @@ class FordModelActionController: 'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading), 'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled, 'request_release': self.core.request_release, + 'unwind_direction': self.core.unwind_direction, 'unwind_release': self.core.unwind_release, 'carryover_release_count': self.core.carryover_release_count, 'driver_override': driver_override, 'pscm_limited': pscm_limited, 'pscm_status_fresh': bool(status_fresh), 'command': (command.path_offset, command.path_angle, 0., 0.)} diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index 4ee7a1f231..f462a6e571 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase): controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.03, curvature=.015, sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000})) record = self.emit_controls_event('Ford C2-free path tracking', controls) - self.assertEqual(record['hypothesis'], 'model-action-c1-feedback-v4') + self.assertEqual(record['hypothesis'], 'model-action-c1-feedback-v5') self.assertIs(record['calibration_approved'], False) self.assertEqual(record['command'][2:], [0., 0.]) self.assertEqual(record['status'], controller.diagnostics['status']) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action.py b/openpilot/selfdrive/controls/tests/test_ford_model_action.py index 4aa888029c..a0111bf211 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action.py @@ -42,13 +42,14 @@ def test_centering_information_is_independent_of_action_and_not_scaled_with_spee assert target.path_angle == pytest.approx(sign*.2) # No 10 m cap at highway speed. -def test_four_control_states_are_sufficient_for_every_next_output(): +def test_five_control_states_are_sufficient_for_every_next_output(): controller = ModelActionController() assert not hasattr(controller, '__dict__') for i in range(300): copied = ModelActionController() copied.c0, copied.c1, copied.correction = controller.c0, controller.c1, controller.correction copied.last_feedback_desired = controller.last_feedback_desired + copied.unwind_direction = controller.unwind_direction model = straight(.2*math.sin(i*.1)) kwargs = {'speed': 20., 'dt': .01} desired = .005*math.cos(i*.03) 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 241eb000be..38b704f92b 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -341,7 +341,7 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel assert core.carryover_release_count == 0 assert core.carryover_release_count == (0 if same_turn else 1) assert controls.ford_path_controller.diagnostics['carryover_release_count'] == core.carryover_release_count - assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-feedback-v4' + assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-feedback-v5' if same_turn: assert request_releases > 0 assert controls.desired_curvature == pytest.approx(sign*.01) @@ -353,6 +353,58 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel assert core.carryover_release_count == 0 +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('maneuver', [False, True]) +def test_completed_unwind_release_through_selected_request_and_actual_can(pipeline, sign, maneuver): + call, publication = pipeline + controls, sm = startup(), Subscriptions(maneuver) + controls.sm, controls.desired_curvature = sm, -sign*.02 + core = controls.ford_path_controller.core + cc = structs.CarControl(latActive=True) + cs = SimpleNamespace(vEgo=4., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.) + cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1') + downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP()) + vehicle = SimpleNamespace(out=structs.CarState(vEgo=4., vEgoRaw=4.), acc_tja_status_stock_values=defaultdict(int), + lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int)) + parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main) + release_count = 0 + for frame in range(180): + now = 1.+frame*.01 + desired = -sign*(.02 if frame < 30 else .01 if frame < 80 else 0.) + controls.curvature = -sign*(.02 if frame < 30 else .04 if frame < 80 else .01 if frame < 130 else 0.) + model = straight(sign*(-.5 if frame < 80 else .05)) + model.action = SimpleNamespace(desiredCurvature=-desired if maneuver else desired) + sm.messages['lateralManeuverPlan'].desiredCurvature = desired + sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9), lateralManeuverPlan=round(now*1e9)) + before = core.c0, core.c1, core.correction + exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, + 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, + 'time': SimpleNamespace(monotonic=lambda now=now: now)}) + assert abs(core.c0-before[0]) <= .0400000001 and abs(core.c1-before[1]) <= .0050000001 + release_count += core.unwind_release != 0. + if frame == 129: + assert sign*core.correction > .04 and core.unwind_direction == sign + if frame == 130: + assert core.unwind_release == before[2] and core.correction == 0. + assert core.c1 == pytest.approx(before[1]-sign*.005) + msg = custom.CarControlSP.new_message() + exec(publication, {'self': controls, 'CC_SP': msg}) + _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9)) + received = parser.update([round(now*1e9), packets]) + address = parser.dbc.name_to_msg['LateralMotionControl2'].address + assert address in received + wire = parser.vl['LateralMotionControl2'] + assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle) + assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset) + assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0. + assert wire['LatCtl_D2_Rq'] == 2 and wire['LatCtlPath_No_Cnt'] == frame % 16 + packet = next(packet for packet in packets if packet[0] == address) + assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1]) + assert release_count == 1 + assert controls.ford_path.path_angle == core.correction == 0. + assert controls.ford_path.path_offset == pytest.approx(sign*.05) + + @pytest.mark.parametrize('sign', [-1., 1.]) @pytest.mark.parametrize('fingerprint', CANFD_CARS) def test_heading_overflow_and_release_through_actual_can(pipeline, sign, fingerprint): diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_unwind.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_unwind.py new file mode 100644 index 0000000000..1417a03266 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_unwind.py @@ -0,0 +1,127 @@ +"""Unwind correction must not become a new turn after a confirmed catch-up. + +These reproduce controller memory, not the PSCM's physical steering response. +""" +import pytest + +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController +from openpilot.selfdrive.controls.tests.test_ford_model_action import straight + + +def unwind(controller, sign): + for _ in range(30): + controller.update(straight(-sign*.5), -sign*.02, current_curvature=-sign*.02, speed=4., dt=.01) + # Selected curvature relaxes, but the wheel is still too far into the old + # turn. Feedback builds the same opposite-direction correction seen in 115. + for _ in range(50): + controller.update(straight(-sign*.5), -sign*.01, current_curvature=-sign*.04, speed=4., dt=.01) + assert sign*controller.correction > .04 + assert controller.unwind_direction == sign + for _ in range(30): + controller.update(straight(sign*.05), 0., current_curvature=-sign*.01, speed=4., dt=.01) + assert sign*controller.correction > .04 + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('requested', [0., .0001]) +def test_completed_unwind_does_not_keep_requesting_a_new_turn(sign, requested): + controller = ModelActionController() + unwind(controller, sign) + before = controller.c1 + # The model path now confirms the unwind direction and actual steering has + # caught the near-zero/new-direction request. Allow the original output slew + # to finish; the old unwind correction must no longer prop up C1. + for _ in range(20): + out = controller.update(straight(sign*.05), sign*requested, current_curvature=sign*requested, speed=4., dt=.01) + assert abs(controller.c1-before) <= .100000001 + assert out.path_offset == pytest.approx(sign*.05) + assert controller.correction == 0., 'Stored unwind correction remains after catch-up' + assert abs(out.path_angle-7.*sign*requested) <= .000500001 + assert controller.unwind_direction == 0. + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('case', ['not_caught', 'old_side_request', 'model_c0_old', 'model_c0_neutral', 'slewed_c0_old', + 'correction_not_dominant', 'opposite_correction', 'no_unwind', 'duplicate']) +def test_unwind_release_requires_catchup_and_confirmed_new_direction(sign, case): + controller = ModelActionController() + unwind(controller, sign) + desired = measured = 0. + offset, feedback_dt = sign*.05, .01 + if case == 'not_caught': + measured = -sign*.005 + elif case == 'old_side_request': + desired = measured = -sign*.001 + elif case == 'model_c0_old': + offset = -sign*.05 + elif case == 'model_c0_neutral': + offset = 0. + elif case == 'slewed_c0_old': + controller.c0 = -sign*.5 + elif case == 'correction_not_dominant': + desired = measured = sign*.02 + elif case == 'opposite_correction': + controller.correction = -sign*.01 + elif case == 'no_unwind': + controller.unwind_direction = 0. + elif case == 'duplicate': + feedback_dt = 0. + before = controller.correction + controller.update(straight(offset), desired, current_curvature=measured, speed=4., dt=.01, feedback_dt=feedback_dt) + assert controller.unwind_release == 0. + assert abs(controller.correction-before) <= abs(desired-measured)*4.*feedback_dt+1e-10 + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('desired', [0., .004]) +def test_steady_tracking_correction_survives_matched_steering_and_noise(sign, desired): + controller = ModelActionController() + controller.correction, controller.c1, controller.c0 = sign*.05, sign*(7.*desired+.05), sign*.05 + for i in range(300): + measured = sign*(desired+(1 if i % 2 else -1)*.000001) + controller.update(straight(sign*.05), sign*desired, current_curvature=measured, speed=4., dt=.01) + assert controller.unwind_direction == controller.unwind_release == 0. + assert controller.correction == pytest.approx(sign*.05) + + +@pytest.mark.parametrize('limited', [False, True]) +def test_catchup_release_uses_existing_slew_and_is_consumed_once(limited): + controller = ModelActionController() + unwind(controller, 1.) + before, correction = controller.c1, controller.correction + controller.update(straight(.05), 0., current_curvature=.001, speed=4., dt=.01, pscm_limited=limited) + assert controller.unwind_release == correction + assert controller.c1 == pytest.approx(before-.005) + assert controller.correction <= 0. # Remaining feedback can only correct the overshoot. + assert controller.unwind_direction == 0. + controller.update(straight(.05), 0., current_curvature=.001, speed=4., dt=.01) + assert controller.unwind_release == 0. + + +@pytest.mark.parametrize('override', ['driver', 'invalid', 'inactive']) +def test_unwind_history_is_cleared_by_override_and_reset(override): + controller = ModelActionController() + unwind(controller, 1.) + kwargs = {'driver': {'feedback_enabled': False}, 'invalid': {'valid': False}, 'inactive': {'active': False}}[override] + controller.update(straight(.05), 0., current_curvature=0., speed=4., dt=.01, **kwargs) + assert controller.unwind_direction == controller.unwind_release == controller.correction == 0. + + +def test_duplicate_adapter_measurement_cannot_consume_unwind_until_fresh_catchup(): + controller = FordModelActionController() + for frame in range(130): + now = 1.+frame*.01 + desired = -.02 if frame < 30 else -.01 if frame < 80 else 0. + measured = -.02 if frame < 30 else -.04 if frame < 80 else -.01 + controller.update(straight(-.5 if frame < 80 else .05), desired, current_curvature=measured, + speed=4., yaw_rate=0., now=now, measurement_time=now, model_time=now, reference_time=now, active=True) + assert controller.core.unwind_direction == 1. + before = controller.core.correction + for now, stamp in [(2.30, 2.29), (2.31, 2.31), (2.32, 2.31)]: + controller.update(straight(.05), 0., current_curvature=0., speed=4., yaw_rate=0., now=now, + measurement_time=stamp, model_time=now, reference_time=now, active=True) + if now == 2.30: + assert controller.core.correction == before and controller.core.unwind_direction == 1. + else: + assert controller.core.correction == controller.core.unwind_direction == 0. + assert controller.diagnostics['unwind_release'] == (before if now == 2.31 else 0.) diff --git a/tools/ford_pscm_lab/feedback_replay.py b/tools/ford_pscm_lab/feedback_replay.py index bd060e7459..7f1896b24f 100644 --- a/tools/ford_pscm_lab/feedback_replay.py +++ b/tools/ford_pscm_lab/feedback_replay.py @@ -21,7 +21,7 @@ from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sam BASELINE = 'a7d70e2b0890184636827351e4789d866f2a7c97' -FEEDBACK_V3 = '17a86842f97f65216443a5d89648c8ace8518758' +FEEDBACK_V4 = '6df5eabb7e7f6bc4e206644d5ca9069df820124e' OPENDBC = '64aa61b9b3fd26e70a7caa915acab207ff3cd64a' @@ -69,8 +69,11 @@ def replay(directory, output, baseline_revision=BASELINE): pscm_limited = np.zeros(len(t), bool) offset_overflow = np.zeros(len(t)) request_release = np.zeros(len(t)) + unwind_release = np.zeros(len(t)) + unwind_direction = np.zeros(len(t)) reasons = Counter() releases = [] + unwind_releases = [] for i, now in enumerate(t): model_time = r['model']['t'][mi[i]] service_valid = bool(c['valid'][i] and cc['valid'][i] and cs['valid'][i] and cs['can_valid'][i] @@ -99,6 +102,13 @@ def replay(directory, output, baseline_revision=BASELINE): pscm_limited[i] = d.get('pscm_limited', False) offset_overflow[i] = d.get('offset_overflow', 0.) request_release[i] = d.get('request_release', 0.) + unwind_release[i] = d.get('unwind_release', 0.) + unwind_direction[i] = d.get('unwind_direction', 0.) + if unwind_release[i]: + unwind_releases.append({'time_s': float(now-metadata['t0']), 'retired_rad': float(unwind_release[i]), + 'correction_after_rad': float(correction[i]), 'base_c1_rad': float(d['heading_feedforward']), + 'desired_angle_deg': float(c['desired_angle'][i]), 'actual_angle_deg': float(c['actual_angle'][i]), + 'baseline_c0_c1': baseline[i, :2].tolist(), 'candidate_c0_c1': commands[i, :2].tolist()}) if controller.core.carryover_release_count > previous_count: releases.append({'time_s': float(now-metadata['t0']), 'correction_before_rad': float(previous_correction), 'correction_after_rad': float(correction[i]), 'base_c1_rad': float(d['heading_feedforward']), @@ -122,6 +132,9 @@ def replay(directory, output, baseline_revision=BASELINE): 'request_release_cycles': int(np.count_nonzero(request_release)), 'request_release_seconds': float(weight[request_release != 0.].sum()), 'max_abs_request_release_rad': float(abs(request_release).max()), + 'unwind_release_cycles': int(np.count_nonzero(unwind_release)), + 'max_abs_unwind_release_rad': float(abs(unwind_release).max()), + 'unwind_releases': unwind_releases, 'carryover_releases': releases, 'feedback_enabled_seconds': float(weight[feedback_enabled].sum()), 'pscm_limit_2_seconds': float(weight[pscm_limited & valid].sum()), @@ -145,7 +158,7 @@ def replay(directory, output, baseline_revision=BASELINE): np.savez_compressed(output/'commands.npz', t=t-metadata['t0'], baseline=baseline, candidate=commands, valid=valid, correction=correction, baseline_correction=baseline_correction, feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited, offset_overflow=offset_overflow, - request_release=request_release) + request_release=request_release, unwind_release=unwind_release, unwind_direction=unwind_direction) (output/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n') print(json.dumps({k: v for k, v in report.items() if k not in ('source_sha256', 'carryover_releases')} | {'carryover_release_count': len(releases)}, indent=2)) @@ -153,12 +166,13 @@ def replay(directory, output, baseline_revision=BASELINE): def stress(cycles, output): verify_dependency(OPENDBC) - rng = np.random.default_rng(20260909) + rng = np.random.default_rng(20260913) controller, mirror, wire = ModelActionController(), ModelActionController(), WireCheck() - old, baseline_hash = original_controller(FEEDBACK_V3) + old, baseline_hash = original_controller(FEEDBACK_V4) releases = 0 request_releases = 0 - unchanged_without_request_release = 0 + unwind_releases = 0 + unchanged_without_unwind_release = 0 for i in range(cycles): desired, measured = rng.uniform(-.1, .1, 2) speed, dt, offset = rng.uniform(.3, 55.), rng.uniform(.002, .1), rng.uniform(-8., 8.) @@ -166,6 +180,7 @@ def stress(cycles, output): feedback_dt = 0. if i % 5 == 0 else rng.uniform(.002, .15) previous = controller.c0, controller.c1, controller.correction previous_desired = controller.last_feedback_desired + previous_unwind = controller.unwind_direction previous_count = controller.carryover_release_count args = {'speed': speed, 'dt': dt, 'feedback_dt': feedback_dt, 'active': active, 'feedback_enabled': enabled, 'pscm_limited': limited} @@ -176,33 +191,47 @@ def stress(cycles, output): # Clone the pre-update state to isolate this cycle's policy from the # different history that a previous release would otherwise create. old.core.c0, old.core.c1, old.core.correction = previous - retired = controller.request_release + retired = controller.unwind_release # After just the declared retirement, the entire remaining command law # must match the previously deployed implementation exactly. - old.core.correction += retired + old.core.correction -= retired + old.core.last_feedback_desired = previous_desired old.core.carryover_release_count = previous_count baseline_out = old.core.update(model(offset), desired, current_curvature=measured, **args) released = controller.carryover_release_count > previous_count assert out == baseline_out assert (controller.c0, controller.c1, controller.correction) == (old.core.c0, old.core.c1, old.core.correction) assert controller.carryover_release_count == old.core.carryover_release_count + assert controller.request_release == old.core.request_release if retired: + unwind_releases += 1 + assert active and enabled and feedback_dt > 0. and previous_unwind != 0. + assert retired == previous[2] + assert (desired-measured)*previous_unwind <= 0. and desired*previous_unwind >= 0. + assert retired*previous_unwind > abs(np.clip(max(7., speed)*desired, -.5, .5)) + assert min(offset*previous_unwind, controller.c0*previous_unwind) >= .01 + assert controller.unwind_direction == 0. + else: + unchanged_without_unwind_release += 1 + remaining = previous[2]-retired + request_release = controller.request_release + if request_release: request_releases += 1 assert active and enabled and feedback_dt > 0. and previous_desired is not None distance = max(7., speed) change = np.clip(distance*desired, -.5, .5)-np.clip(distance*previous_desired, -.5, .5) assert abs(change) >= .0005 - assert change*(desired-measured) > 0. and retired*previous[2] < 0. - assert abs(retired) <= min(abs(change), abs(previous[2]))+1e-10 - assert abs(distance*(desired-measured)) >= abs(previous[2]) - assert (previous[2]+retired)*previous[2] >= -1e-10 - else: - unchanged_without_request_release += 1 + assert change*(desired-measured) > 0. and request_release*remaining < 0. + assert abs(request_release) <= min(abs(change), abs(remaining))+1e-10 + assert abs(distance*(desired-measured)) >= abs(remaining) + assert (remaining+request_release)*remaining >= -1e-10 state = controller.c0, controller.c1, controller.correction mirrored = mirror.c0, mirror.c1, mirror.correction np.testing.assert_allclose(state, -np.array(mirrored), rtol=0., atol=1e-10) assert controller.carryover_release_count == mirror.carryover_release_count - assert abs(retired+mirror.request_release) <= 1e-10 + assert abs(request_release+mirror.request_release) <= 1e-10 + assert retired == -mirror.unwind_release + assert controller.unwind_direction == -mirror.unwind_direction assert abs(controller.c0) <= 5.11+1e-10 and abs(controller.c1) <= .5+1e-10 and abs(controller.correction) <= 1.+1e-10 if active: assert abs(controller.c0-previous[0]) <= 4.*dt+1e-10 @@ -212,10 +241,10 @@ def stress(cycles, output): if released: assert feedback_dt > 0. and desired*measured < 0. and previous[2]*desired < 0. assert offset*desired > 0. and controller.c0*desired > 0. - delta = controller.correction-(0. if released else previous[2]+retired) + delta = controller.correction-(0. if released else remaining+request_release) request = (desired-measured)*speed*feedback_dt assert delta*request >= -1e-10 and abs(delta) <= abs(request)+1e-10 - assert (controller.correction-previous[2])*request >= -1e-10 + assert (controller.correction-remaining)*request >= -1e-10 if limited and request*(measured if measured else previous[1]) > 0.: assert abs(controller.correction) <= abs(previous[2])+1e-10 assert controller.correction*previous[2] >= -1e-10 @@ -227,10 +256,11 @@ def stress(cycles, output): wire.check(out) report = {'cycles': cycles, 'mirrored_updates': cycles, 'can_round_trips': wire.count, 'carryover_release_count': releases, - 'baseline_revision': FEEDBACK_V3, 'baseline_source_sha256': baseline_hash, + 'baseline_revision': FEEDBACK_V4, 'baseline_source_sha256': baseline_hash, 'seed': 20260913, 'request_release_cycles': request_releases, - 'exact_unchanged_state_and_commands_without_request_release': unchanged_without_request_release, - 'exact_v3_match_after_only_declared_retirement': cycles, + 'unwind_release_cycles': unwind_releases, + 'exact_unchanged_state_and_commands_without_unwind_release': unchanged_without_unwind_release, + 'exact_v4_match_after_only_declared_retirement': cycles, 'checks': 'Symmetry, resets, amplitude/slew, bounded retirement, carryover confirmation, integration, PSCM limits, CAN.', 'scope': 'Numerical software invariants only; no model of vehicle motion.', 'calibration_approved': False, 'controller_sha256': hashlib.sha256(Path(ford_model_action.__file__).read_bytes()).hexdigest()}