mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-15 02:13:43 +08:00
Ford: retire opposing C1 correction as the selected request changes
This commit is contained in:
Isaac Barham
@@ -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.
|
||||
@@ -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."
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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.)}
|
||||
|
||||
@@ -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'])
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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.
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user