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