diff --git a/docs/ford_c1_request_release.md b/docs/ford_c1_request_release.md new file mode 100644 index 0000000000..cb8e3ef641 --- /dev/null +++ b/docs/ford_c1_request_release.md @@ -0,0 +1,128 @@ +# Ford changed-request correction release + +The Chestnut/Tee Time routes 112 and 113 ran `17a86842f` (C1 feedback v3). +In route 113, an increasing turn request remained below its base C1 because +negative correction from an earlier oversteer episode took time to return to +zero. The existing reversal release did not apply: requested and measured +curvature were already in the same turn direction. + +Version `model-action-c1-feedback-v4` retires a bounded amount of correction +when a changed request and measured error both oppose that correction. This +addresses software command delay. It does not establish improved wheel tracking +or fix all the recorded hanging exits. + +## Rule + +On a fresh steering measurement, evaluate the previous selected curvature and +the current selected curvature using today's existing heading reference: + +```text +distance = max(7 m, speed * 1 s) +change = clip(distance * desired, -0.5, 0.5) + - clip(distance * previous_desired, -0.5, 0.5) +error = desired - measured +``` + +Retirement requires all of: + +- Feedback enabled and a previous feedback request available. +- Heading change at least one existing C1 DBC step (0.0005 rad). +- Change and current error agree in direction. +- Stored correction opposes that direction. +- The magnitude of `distance * error` is at least the correction magnitude. + +Move the correction toward zero by at most the heading change, without crossing +zero. Then run the existing reversal release, elapsed-distance integration, +PSCM arbitration and final output slew. The mismatch requirement is an +engineering guard using the existing reference distance; it is not a fitted +PSCM response threshold or proof of stability. It protects a larger learned +correction from small target/measurement noise. There is no new tunable strength +multiplier, and the existing 1:1 feedback-strength choice remains. + +The last selected curvature adds one control state. Duplicate steering samples +do not advance this history or retire correction; the next fresh sample uses +the net request change. A speed change alone cannot cause retirement because +both requests are evaluated at the same current speed. Invalid input and +disengagement reset the history. Driver override clears correction and prevents +a pending request change from being applied later. + +C0 mapping and overflow, C2/C3 zeroing, amplitude/slew limits, sender cadence and +all input/driver/PSCM gates remain unchanged. Toggle off still selects upstream +Ford control on every platform. The existing default-off toggle selects v4 on +Ford CAN FD vehicles. `request_release` logs signed radians retired on that +cycle; periodic diagnostics do not capture every individual retirement. + +## Evidence and limits + +The regression command `python -m pytest -q -p no:cacheprovider +openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py` +initially returned **4 failed** on v3. It checks that an obsolete correction no +longer delays a changed same-direction turn or unwind after the output slew +has time to respond. Expanded cases cover small noise, matched tracking, +insufficient error, speed-only changes, clipped base requests, duplicate +measurements, override and reset. Integration tests exercise actual controlsd +selection/limiting, both model and maneuver references, Float32 publication +and Ford CAN packing. + +Frozen replay compares v4 with the deployed v3 on the same recorded model, +measurement, driver and PSCM inputs: + +| Route | Control cycles | Retirement cycles | Largest C1 difference | +| --- | ---: | ---: | ---: | +| 112 | 108,971 | 414 | 0.0235 rad | +| 113 | 49,614 | 119 | 0.0275 rad | +| Historical b9 | 90,774 | 440 | 0.0350 rad | + +Activation and C0 are identical on every replay cycle. C2/C3 remain zero. +Retirement changes subsequent correction history, so command differences can +persist after a retirement cycle. In frozen measurements the vehicle cannot +react to those differences. Command differences occur in ordinary bends too; +these tests do not establish unchanged real-world centering or stability. + +In route 113, segment 3, old opposing correction reaches zero at **3:18.630** +instead of **3:19.253**: **0.623 s earlier**. At 3:18.649, C1 magnitude is +0.319 rad instead of 0.2925 rad. The PSCM limit flag still inhibits additional +outward integration; retiring opposing correction cannot create new stored +outward demand through that gate. + +The route 113 exit at 8:01.567 has **identical C1** in this replay. Route 112's +11:40.555 overshoot changes C1 by only 0.0015 rad, slightly later in the unwind +direction on the frozen history. These are material limits: the change does +not solve those exits. C0's contribution and physical PSCM response remain +unresolved. No counterfactual wheel-angle or tracking-error score is reported. + +The combined suite passes **717 tests and 9,145 subtests**, with 178 inherited +or unsupported safety-test skips. Feedback stress and zero-error stress cover +200,000 cycles each; the latter also covers 18,138 field-boundary cases. +Together with the three route replays, these verify **667,497 Float32/CAN round +trips**, separately from the integration suite. Stress compares each step to +v3 after only the declared retirement and checks sign symmetry, bounds, slew, +resets, arbitration and correction direction. Ruff, the controller Ty check +and settings compilation pass. Numerical records are in +`ford_c1_request_release_validation.json`. + +## Reproduction + +Use the project's native Python dependencies and unchanged opendbc revision +`64aa61b9b3fd26e70a7caa915acab207ff3cd64a`. Route commands require the full-rlog +extracts (`route.npz`, `model_paths.npz`, `metadata.json`) identified by the +validation hashes. No original logs are modified. + +```sh +export PYTHONDONTWRITEBYTECODE=1 +export PYTHONPATH=.:opendbc_repo +# Optional writable roots for the tests' temporary parameter stores and logs: +export PARAMS_ROOT=/tmp/ford-v4-test-params +export LOG_ROOT=/tmp/ford-v4-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_v4/stress.json +python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260912 --opendbc-revision 64aa61b9b3fd26e70a7caa915acab207ff3cd64a --output .cache/ford_v4/zero_error.json +python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_route112 --baseline 17a86842f --output .cache/ford_v4/route112 +python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_route113 --baseline 17a86842f --output .cache/ford_v4/route113 +python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_routeb9 --baseline 17a86842f --output .cache/ford_v4/routeb9 +``` + +Publication time approximates the computation clock; full SubMaster health is +not reconstructable. These routes have no selected maneuver-plan publications; +that source is covered by integration tests. No device build, boot, installation +or physical steering test is performed offline. diff --git a/docs/ford_c1_request_release_validation.json b/docs/ford_c1_request_release_validation.json new file mode 100644 index 0000000000..92b2642339 --- /dev/null +++ b/docs/ford_c1_request_release_validation.json @@ -0,0 +1,214 @@ +{ + "hypothesis": "model-action-c1-feedback-v4", + "baseline_revision": "17a86842f97f65216443a5d89648c8ace8518758", + "scope": "Software command delay and invariants only; no counterfactual wheel motion or proven physical tracking improvement. Hanging exits remain unresolved.", + "calibration_approved": false, + "tests": { + "passed": 717, + "subtests_passed": 9145, + "skipped": 178, + "log_sha256": "1864f8618b9d788f67e57766cf7b9ab9eda8e98a2ad0caf1c668bb9936b6eb7d", + "initial_regression": "4 failures on v3; same-turn command delay tests pass on v4", + "environment": "PARAMS_ROOT and LOG_ROOT point to dedicated temporary directories; initial sandbox path failures resolved without changing tests." + }, + "feedback_stress": { + "cycles": 200000, + "mirrored_updates": 200000, + "can_round_trips": 200000, + "carryover_release_count": 165, + "baseline_revision": "17a86842f97f65216443a5d89648c8ace8518758", + "baseline_source_sha256": "167ae5a01fdd7ea014e6ad3fe9d0b6e31c67de8ba057ec5ecf18ab38fc16353f", + "request_release_cycles": 20454, + "exact_unchanged_state_and_commands_without_request_release": 179546, + "exact_v3_match_after_only_declared_retirement": 200000, + "checks": "Mirror symmetry, reset/override, amplitude, slew, correction bounds, bounded request retirement, carryover direction/confirmation, integration, PSCM limits, CAN.", + "scope": "Numerical software invariants only; no model of vehicle motion.", + "calibration_approved": false, + "controller_sha256": "5673630d31910fcfa5a3cc9a8d533b6b8fe9a76e2627bf1f67b52b3550ec7442" + }, + "zero_error_stress": { + "seed": 20260912, + "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": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "af97c665f342c66b1be2502e188c63e6f3ee106d0a0d5e80997bc3040373ff9f" + } + }, + "replays": { + "112": { + "baseline_revision": "17a86842f", + "baseline_source_sha256": "167ae5a01fdd7ea014e6ad3fe9d0b6e31c67de8ba057ec5ecf18ab38fc16353f", + "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": 414, + "request_release_seconds": 4.192443282002046, + "max_abs_request_release_rad": 0.008723706007003784, + "feedback_enabled_seconds": 840.7664582650004, + "pscm_limit_2_seconds": 14.441855805999936, + "c1_changed_cycles": 58152, + "max_abs_c1_change_rad": 0.023500000000000076, + "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": "2e9ea03d4947c7bb3a02c864e0dbc7c9bf031af51dba5e44a8aaa137c3aac6b2", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5673630d31910fcfa5a3cc9a8d533b6b8fe9a76e2627bf1f67b52b3550ec7442" + }, + "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.", + "targeted_points": [ + { + "time_s": 700.5553145380001, + "baseline_c0_c1": [ + -0.34999999999999964, + 0.010000000000000009 + ], + "candidate_c0_c1": [ + -0.34999999999999964, + 0.008500000000000008 + ] + } + ] + }, + "113": { + "baseline_revision": "17a86842f", + "baseline_source_sha256": "167ae5a01fdd7ea014e6ad3fe9d0b6e31c67de8ba057ec5ecf18ab38fc16353f", + "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.235422420998475, + "max_abs_request_release_rad": 0.009946223348379135, + "feedback_enabled_seconds": 243.3033761190004, + "pscm_limit_2_seconds": 14.003248144999816, + "c1_changed_cycles": 17825, + "max_abs_c1_change_rad": 0.027500000000000024, + "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": "2e9ea03d4947c7bb3a02c864e0dbc7c9bf031af51dba5e44a8aaa137c3aac6b2", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5673630d31910fcfa5a3cc9a8d533b6b8fe9a76e2627bf1f67b52b3550ec7442" + }, + "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.", + "old_correction_zero_s": 199.2531750590001, + "new_correction_zero_s": 198.62974550599984, + "earlier_correction_zero_s": 0.6234295530002782, + "targeted_points": [ + { + "time_s": 198.64943081699994, + "baseline_c0_c1": [ + 2.16, + 0.2925 + ], + "candidate_c0_c1": [ + 2.16, + 0.319 + ] + }, + { + "time_s": 481.56693996600006, + "baseline_c0_c1": [ + 0.5800000000000001, + -0.0645 + ], + "candidate_c0_c1": [ + 0.5800000000000001, + -0.0645 + ] + } + ] + }, + "b9": { + "baseline_revision": "17a86842f", + "baseline_source_sha256": "167ae5a01fdd7ea014e6ad3fe9d0b6e31c67de8ba057ec5ecf18ab38fc16353f", + "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": 440, + "request_release_seconds": 4.520361682000512, + "max_abs_request_release_rad": 0.017186015844345093, + "feedback_enabled_seconds": 816.0284774219999, + "pscm_limit_2_seconds": 9.308613716000167, + "c1_changed_cycles": 48629, + "max_abs_c1_change_rad": 0.03500000000000003, + "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": "cd76c6106b2e41e905b752f1638d5b3e0feaa10ec21e038268df33183a91c640", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f" + }, + "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." + } + }, + "float32_can_round_trips_excluding_integration_tests": 667497, + "checks": { + "ruff": true, + "controller_ty": true, + "settings_compilation": true, + "toggle_off_upstream_integration": true + }, + "final_source_sha256": { + "openpilot/selfdrive/controls/lib/ford_model_action.py": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f", + "openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py": "4b4a69d67eba0ff9d5db0a50a4f868c5eb5f7c8c79d70832500554febc7d28fe", + "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "7b2429a5c40e5067b8edea4c11e9cdd4c6271d09f7982e30126eb42b93425a50", + "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "5943a37f6e3865297ac543fb922a3c8b6e016c5af3eff589f518bd9615bbdb4f", + "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "df883702a847465815feb6c4fbf88130c27e69d7e3e256c9962d99a9738ee353", + "tools/ford_pscm_lab/feedback_replay.py": "cd76c6106b2e41e905b752f1638d5b3e0feaa10ec21e038268df33183a91c640" + }, + "source_note": "Controller comment/docstring cleanup followed feedback stress and routes 112/113. The recorded tested hashes are retained; b9 and zero-error stress record the final controller source. No executable controller change followed those runs." +} diff --git a/docs/ford_model_action_drive_test.md b/docs/ford_model_action_drive_test.md index 85f7cf60a6..edf2d6a0bf 100644 --- a/docs/ford_model_action_drive_test.md +++ b/docs/ford_model_action_drive_test.md @@ -20,12 +20,15 @@ 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-v3`**. They report desired and +identify **`hypothesis=model-action-c1-feedback-v4`**. 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 steering. `offset_overflow` reports the extra C0 target in meters before C0 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. Turning the toggle off and completing another offroad-to-onroad cycle restores **upstream Ford curvature control**: 20 Hz steering messages, limited mode on @@ -45,8 +48,10 @@ Driver override clears the correction. A fresh PSCM reached-limit flag stops extra outward accumulation while preserving unwind and base model changes. 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. Neutral or conflicting -C0 and matched curvature preserve the correction. Final output slew still applies. +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). 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. @@ -58,9 +63,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 [the overflow specification and validation](ford_c1_overflow.md) and -`ford_c1_overflow_validation.json` for current evidence and reproduction -commands. The carryover specification and `ford_c1_carryover_validation.json` +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 +`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 `ford_model_action_drive_test_validation.json` are historical records for the diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 90d2495f8a..16118fdd89 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -4,6 +4,7 @@ Selected only by its explicit toggle. The 7 m station and one-second scale are 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. Base heading clipped by C1 is allocated to C0 at the existing 7 m reference. """ import math @@ -54,12 +55,12 @@ def encode_model_action(model, desired_curvature, speed): class ModelActionController: - """Unquantized C0/C1 slew positions and one C1 feedback correction. + """C0/C1 slew positions, C1 correction and the last feedback request. 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') + __slots__ = ('c0', 'c1', 'correction', 'carryover_release_count', 'last_feedback_desired', 'request_release') def __init__(self): self.reset() @@ -67,6 +68,8 @@ class ModelActionController: def reset(self): self.c0 = self.c1 = self.correction = 0. 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. def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True, feedback_dt=None, feedback_enabled=True, pscm_limited=False): @@ -86,9 +89,28 @@ class ModelActionController: self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt)) lower = max(-.5, self.c1-.5*dt) upper = min(.5, self.c1+.5*dt) + self.request_release = 0. if not feedback_enabled: self.correction = 0. else: + # 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 + # step. Never cross zero or discard a steady tracking correction just + # because error changes sign. + # Require the heading mismatch over the existing reference distance to + # cover the correction: small tracking noise must not erode a useful I. + # Evaluate both requests at today's speed so speed changes alone do not + # release anything. Duplicate measurements leave this history untouched. + if feedback_dt > 0. and self.last_feedback_desired is not None: + 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.))) + self.correction += self.request_release direction = math.copysign(1., base_c1) # Release only correction that prevents C1 from requesting the direction # shared by model C0, slewed C0 and base C1, while measured steering is @@ -112,6 +134,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.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)) self.c1 += float(np.clip(c1-self.c1, -.5*dt, .5*dt)) return FordPath(True, _packed(self.c0, .01, -5.12), _packed(self.c1, .0005, -.5), 0., 0.) @@ -121,8 +145,8 @@ class FordModelActionController: """Input adapter for the opt-in selected-action controller. controlsd owns upstream selection/limiting and service health. This adapter - checks ages and clock order, then supplies elapsed time to the three-state - core. Feedback advances once per fresh steering measurement; repeated samples + checks ages and clock order, then supplies elapsed time to the core. + Feedback advances once per fresh steering measurement; repeated samples can still advance output slew. Raw model geometry is checked on every cycle. CAN yaw remains a health gate, not the feedback measurement. Driver override @@ -136,7 +160,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-v3', + self.diagnostics = {'status': status, 'hypothesis': 'model-action-c1-feedback-v4', '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, @@ -178,7 +202,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-v3', + self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c1-feedback-v4', '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, @@ -186,6 +210,7 @@ class FordModelActionController: 'heading_feedforward': base_heading, 'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading), 'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled, + 'request_release': self.core.request_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 ea867bb4c7..4ee7a1f231 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-v3') + self.assertEqual(record['hypothesis'], 'model-action-c1-feedback-v4') 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 1637154608..4aa888029c 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action.py @@ -42,12 +42,13 @@ 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_three_control_states_are_sufficient_for_every_next_output(): +def test_four_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 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 b74249d94e..241eb000be 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -294,31 +294,35 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s @pytest.mark.parametrize('sign', [-1., 1.]) @pytest.mark.parametrize('maneuver', [False, True]) -def test_carryover_release_through_selected_limited_request_and_actual_can(pipeline, sign, maneuver): +@pytest.mark.parametrize('same_turn', [False, True]) +def test_carryover_release_through_selected_limited_request_and_actual_can(pipeline, sign, maneuver, same_turn): call, publication = pipeline controls, sm = startup(), Subscriptions(maneuver) controls.sm, controls.desired_curvature = sm, sign*.004 core = controls.ford_path_controller.core cc = structs.CarControl(latActive=True) - cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.) + speed = 10. if same_turn else 20. + cs = SimpleNamespace(vEgo=speed, 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=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int), + vehicle = SimpleNamespace(out=structs.CarState(vEgo=speed, vEgoRaw=speed), 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) + request_releases = 0 for frame in range(280): now = 1.+frame*.01 - desired = sign*(.004 if frame < 200 else -.001) - model = straight(sign*(.2 if frame < 200 else -.2)) + desired = sign*(.004 if frame < 200 else .01 if same_turn else -.001) + model = straight(sign*(.2 if frame < 200 or same_turn else -.2)) model.action = SimpleNamespace(desiredCurvature=-desired if maneuver else desired) sm.messages['lateralManeuverPlan'].desiredCurvature = desired - controls.curvature = sign*(.004 if frame < 100 else .001 if frame < 200 else .003) + controls.curvature = sign*(.004 if frame < 100 else .006 if same_turn else .001 if frame < 200 else .003) sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9), lateralManeuverPlan=round(now*1e9)) before = core.c0, core.c1 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 + request_releases += core.request_release != 0. 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)) @@ -333,13 +337,18 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel 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]) if frame == 199: - assert core.correction == pytest.approx(sign*.06) + assert core.correction == pytest.approx(sign*(-speed*.002 if same_turn else .06)) assert core.carryover_release_count == 0 - assert core.carryover_release_count == 1 - assert controls.ford_path_controller.diagnostics['carryover_release_count'] == 1 - assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-feedback-v3' - assert sign*controls.ford_path.path_angle < 0. - assert controls.ford_path.path_offset == pytest.approx(-sign*.2) + 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' + if same_turn: + assert request_releases > 0 + assert controls.desired_curvature == pytest.approx(sign*.01) + assert sign*controls.ford_path.path_angle >= speed*.01 # No old unwind correction left below the new base. + else: + assert sign*controls.ford_path.path_angle < 0. + assert controls.ford_path.path_offset == pytest.approx(sign*(.2 if same_turn else -.2)) controls.ford_path_controller.reset() assert core.carryover_release_count == 0 diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py new file mode 100644 index 0000000000..b6a811f78c --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py @@ -0,0 +1,129 @@ +"""Changed requests should not wait for obsolete C1 correction to integrate away. + +These exercise command delay, not a simulated PSCM or predicted wheel angle. +""" +import pytest + +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController +from openpilot.selfdrive.controls.tests.test_ford_model_action import straight + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('unwind', [False, True]) +def test_changed_request_retires_opposing_correction_within_output_slew(sign, unwind): + controller = ModelActionController() + desired = sign*.035 + model = straight(sign*.4) + for _ in range(100): + controller.update(model, desired, current_curvature=desired, speed=4., dt=.01) + # Build old unwind (or turn-in) correction while retaining the same request. + old_error = sign*(.01 if unwind else -.01) + for _ in range(50): + controller.update(model, desired, current_curvature=desired-old_error, speed=4., dt=.01) + assert controller.correction == pytest.approx(old_error*2.) + # The model now moves in the opposite direction to the stored correction. + # Actual steering is on the other side of the new target: the old correction + # is delaying exactly the response now needed. Both remain in the same turn. + changed = desired-old_error + before = controller.c1 + for _ in range(20): + out = controller.update(model, changed, current_curvature=desired, speed=4., dt=.01) + assert abs(controller.c1-before) <= .100000001 + assert out.path_offset == pytest.approx(sign*.4) + assert (out.path_angle-7.*changed)*old_error <= 1e-10, 'Old correction still opposes the changed model request' + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('case', ['unchanged', 'matched', 'still_short', 'helpful', 'small_error', 'small_change', 'speed_only', 'clipped_base']) +def test_request_release_preserves_correction_without_confirming_evidence(sign, case): + controller = ModelActionController() + previous, desired, measured, correction, speed = .035, .025, .035, .02, 4. + if case == 'unchanged': + desired = previous + measured = .05 + elif case == 'matched': + measured = desired + elif case == 'still_short': + measured = .015 + elif case == 'helpful': + correction = -.02 + elif case == 'small_error': + measured = desired+.00001 + elif case == 'small_change': + desired, measured = previous-.00001, .05 + elif case == 'speed_only': + desired, measured, speed = previous, .05, 10. + elif case == 'clipped_base': + previous, desired, measured = .1, .09, .11 + controller.c0, controller.c1, controller.correction = sign*.4, sign*.2, sign*correction + controller.last_feedback_desired = sign*previous + controller.update(straight(sign*.4), sign*desired, current_curvature=sign*measured, speed=speed, dt=.01) + assert controller.request_release == 0. + assert abs(controller.correction-sign*correction) <= abs(desired-measured)*speed*.01+1e-10 + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_small_request_change_cannot_erase_the_entire_correction(sign): + controller = ModelActionController() + controller.c0, controller.c1, controller.correction = sign*.4, sign*.265, sign*.02 + controller.last_feedback_desired = sign*.035 + controller.update(straight(sign*.4), sign*.034, current_curvature=sign*.05, speed=4., dt=.01) + assert sign*controller.correction > .01 # Most of the original 0.02 rad must survive. + assert 0. < -sign*controller.request_release <= .0070000001 + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_small_target_and_measurement_noise_cannot_erase_steady_correction(sign): + controller = ModelActionController() + controller.c0, controller.c1, controller.correction = sign*.4, sign*.265, sign*.02 + controller.last_feedback_desired = sign*.035 + for i in range(1000): + desired = sign*(.035+(.00001 if i % 2 else -.00001)) + measured = sign*.035 + controller.update(straight(sign*.4), desired, current_curvature=measured, speed=4., dt=.01) + assert controller.request_release == 0. + assert controller.correction == pytest.approx(sign*.02) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('limited', [False, True]) +def test_retiring_correction_at_pscm_limit_never_builds_extra_turn_demand(sign, limited): + controller = ModelActionController() + controller.c0, controller.c1, controller.correction = sign*.4, sign*.225, -sign*.02 + controller.last_feedback_desired = sign*.035 + controller.update(straight(sign*.4), sign*.045, current_curvature=sign*.035, speed=4., dt=.01, pscm_limited=limited) + assert controller.correction == 0. # Slew/limit still prevents a new outward increment here. + assert controller.request_release == pytest.approx(sign*.02) + assert controller.c1 == pytest.approx(sign*.23) + + +def test_adapter_retires_request_once_per_fresh_measurement_and_reset_clears_history(): + controller = FordModelActionController() + def tick(now, desired, measured, stamp, **kwargs): + return controller.update(straight(.4), desired, current_curvature=measured, speed=4., yaw_rate=0., now=now, + measurement_time=stamp, model_time=now, reference_time=now, active=True, **kwargs) + for i in range(150): + now = 1.+i*.01 + tick(now, .035, .035 if i < 100 else .025, now) + before = controller.core.correction + tick(2.50, .025, .035, 2.49) + assert controller.core.correction == before + assert controller.core.last_feedback_desired == .035 + assert controller.diagnostics['request_release'] == 0. + tick(2.51, .025, .035, 2.51) + assert controller.diagnostics['request_release'] == pytest.approx(-before) + assert controller.core.last_feedback_desired == .025 + tick(2.52, .025, .035, 2.51) + assert controller.diagnostics['request_release'] == 0. + tick(2.53, .025, .035, 2.53, valid=False) + assert controller.core.last_feedback_desired is None + assert controller.core.request_release == 0. + + +def test_driver_override_clears_correction_and_cannot_leave_a_pending_release(): + controller = ModelActionController() + controller.correction, controller.last_feedback_desired = .02, .035 + controller.update(straight(.4), .025, current_curvature=.035, speed=4., dt=.01, feedback_enabled=False) + assert controller.correction == controller.request_release == 0. + controller.update(straight(.4), .025, current_curvature=.035, speed=4., dt=.01) + assert controller.request_release == 0. diff --git a/tools/ford_pscm_lab/feedback_replay.py b/tools/ford_pscm_lab/feedback_replay.py index 43f246aca4..bd060e7459 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_V2 = '959ae3d6e76c479f48e081c060b0f3569a6f15f4' +FEEDBACK_V3 = '17a86842f97f65216443a5d89648c8ace8518758' OPENDBC = '64aa61b9b3fd26e70a7caa915acab207ff3cd64a' @@ -68,6 +68,7 @@ def replay(directory, output, baseline_revision=BASELINE): feedback_enabled = np.zeros(len(t), bool) pscm_limited = np.zeros(len(t), bool) offset_overflow = np.zeros(len(t)) + request_release = np.zeros(len(t)) reasons = Counter() releases = [] for i, now in enumerate(t): @@ -97,6 +98,7 @@ def replay(directory, output, baseline_revision=BASELINE): feedback_enabled[i] = d.get('feedback_enabled', False) pscm_limited[i] = d.get('pscm_limited', False) offset_overflow[i] = d.get('offset_overflow', 0.) + request_release[i] = d.get('request_release', 0.) 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']), @@ -117,6 +119,9 @@ def replay(directory, output, baseline_revision=BASELINE): 'max_abs_c0_change_m': float(abs(commands[:, 0]-baseline[:, 0]).max()), 'offset_overflow_seconds': float(weight[offset_overflow != 0.].sum()), 'max_abs_offset_overflow_target_m': float(abs(offset_overflow).max()), + '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()), 'carryover_releases': releases, 'feedback_enabled_seconds': float(weight[feedback_enabled].sum()), 'pscm_limit_2_seconds': float(weight[pscm_limited & valid].sum()), @@ -139,7 +144,8 @@ def replay(directory, output, baseline_revision=BASELINE): output.mkdir(parents=True, exist_ok=True) 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) + feedback_enabled=feedback_enabled, pscm_limited=pscm_limited, offset_overflow=offset_overflow, + request_release=request_release) (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)) @@ -149,15 +155,17 @@ def stress(cycles, output): verify_dependency(OPENDBC) rng = np.random.default_rng(20260909) controller, mirror, wire = ModelActionController(), ModelActionController(), WireCheck() - old, baseline_hash = original_controller(FEEDBACK_V2) + old, baseline_hash = original_controller(FEEDBACK_V3) releases = 0 - unchanged_without_overflow = 0 + request_releases = 0 + unchanged_without_request_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.) active, enabled, limited = i % 211 != 0, i % 97 != 0, i % 7 == 0 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_count = controller.carryover_release_count args = {'speed': speed, 'dt': dt, 'feedback_dt': feedback_dt, 'active': active, 'feedback_enabled': enabled, 'pscm_limited': limited} @@ -168,25 +176,33 @@ 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 + # After just the declared retirement, the entire remaining command law + # must match the previously deployed implementation exactly. + old.core.correction += retired 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 - baseline_released = old.core.carryover_release_count > previous_count - raw_heading = max(7., speed)*desired - overflow = raw_heading-float(np.clip(raw_heading, -.5, .5)) - if not active or overflow == 0.: - assert out == baseline_out - assert (controller.c0, controller.c1, controller.correction) == (old.core.c0, old.core.c1, old.core.correction) - unchanged_without_overflow += 1 - if active: - expected_c0 = previous[0]+float(np.clip(np.clip(offset+7.*overflow, -5.11, 5.11)-previous[0], -4.*dt, 4.*dt)) - assert abs(controller.c0-expected_c0) <= 1e-10 - if released == baseline_released: - assert controller.c1 == old.core.c1 and controller.correction == old.core.correction + 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 + if retired: + 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 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(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 @@ -196,7 +212,7 @@ 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]) + delta = controller.correction-(0. if released else previous[2]+retired) 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 @@ -211,9 +227,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_V2, 'baseline_source_sha256': baseline_hash, - 'exact_unchanged_state_and_commands_without_overflow': unchanged_without_overflow, - 'checks': 'Mirror symmetry, reset/override, amplitude, slew, correction bounds, carryover direction/confirmation, integration, PSCM limits, CAN.', + 'baseline_revision': FEEDBACK_V3, 'baseline_source_sha256': baseline_hash, + '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, + '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()} output.parent.mkdir(parents=True, exist_ok=True)