mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-11 06:53:46 +08:00
Ford: release conflicting C1 correction when the path agrees
An accumulated correction can outweigh a new C1 request while measured curvature still points the other way. Release that correction only with fresh feedback and agreement from both target and slewed C0. Keep steady-target correction, the existing integral strength, output limits and arbitration. Record releases in the v2 diagnostic identity. The change adds one release condition and a diagnostic counter; the command law still has three states. Validation: 567 tests and 9,146 subtests pass, with 178 inherited or unsupported skips. Randomized and b8/b9 replay checks cover 669,343 Float32/CAN round trips. Activation and C0 match the previous controller exactly. Replay verifies command behavior only; no physical response or stability claim is made.
This commit is contained in:
Isaac Barham
@@ -0,0 +1,102 @@
|
||||
# Ford C1 correction carryover experiment
|
||||
|
||||
The feedback controller at `5fbb583e5` can retain a correction from an earlier
|
||||
turn that outweighs the new base C1. The measured curvature can already be
|
||||
opposite the desired curvature, yet total C1 continues to request the old
|
||||
direction while the integral works back toward zero.
|
||||
|
||||
This experiment keeps the existing 1:1 feedback strength and adds a conditional
|
||||
reset of that correction. It is a command-policy experiment, not a demonstrated
|
||||
improvement in physical steering response.
|
||||
|
||||
## Release rule
|
||||
|
||||
All of the following must be true on a valid, active cycle:
|
||||
|
||||
- Feedback is enabled and a fresh steering publication advances measurement time.
|
||||
- Base C1 is nonzero by at least one DBC step (0.0005 rad).
|
||||
- Both target C0 and the slewed C0 request agree with base C1's direction,
|
||||
by at least one DBC step (0.01 m).
|
||||
- Measured steering-derived curvature points opposite the desired curvature.
|
||||
- The accumulated correction prevents total C1 from requesting the base direction:
|
||||
the sum of base C1 and correction is zero or opposite base C1.
|
||||
|
||||
The stored correction is then set to zero before the usual feedback increment.
|
||||
The final C1 command still passes through its existing ±0.5 rad amplitude and
|
||||
0.5 rad/s slew limits. The reset cannot directly jump the transmitted command.
|
||||
The DBC steps reject requests smaller than one representable step; they are
|
||||
not new strength multipliers. This reset policy is itself an engineering choice.
|
||||
|
||||
There is no reset simply because steering error crosses zero, or because C1
|
||||
and its correction have opposite signs. Matched curvature, neutral/conflicting
|
||||
C0, a correction that does not outweigh base C1, and repeated measurements all
|
||||
preserve normal integration. The condition can apply to small steering
|
||||
corrections as well as large turns; it has no turn-size or speed threshold.
|
||||
|
||||
No previous-turn direction or timer is stored. Agreement between current path
|
||||
requests and disagreement with measured curvature are the confirmation. This
|
||||
does not establish which part of the combined C0/C1 request a PSCM physically
|
||||
needs. In particular, when C0 still points into the previous turn, this rule
|
||||
deliberately leaves the integral alone.
|
||||
|
||||
## Preserved behavior and diagnostics
|
||||
|
||||
C0's 7 m mapping, its limits, the base C1 mapping, upstream curvature limiting,
|
||||
the original integral strength, driver/PSCM arbitration, C2=C3=0 and the 100 Hz
|
||||
sender are unchanged. No fitted PSCM model, proportional term or gain schedule
|
||||
is added. There are still three values used by the command law: C0, C1 and
|
||||
the correction. A diagnostic-only `carryover_release_count` is added and resets
|
||||
with the controller. It is included in the existing periodic diagnostic event.
|
||||
|
||||
The same default-off Sunnylink toggle selects this version. Its diagnostic
|
||||
identity is `model-action-c1-feedback-v2`. See the [drive-test instructions](ford_model_action_drive_test.md).
|
||||
|
||||
## Offline evidence
|
||||
|
||||
The two mirrored command-regression tests failed before the change. After
|
||||
building correction through actual feedback, the old controller still requested
|
||||
the old C1 direction 0.4 s into a reversal. Both tests now pass with the original
|
||||
output slew. Additional tests cover holding a steady curve, small error
|
||||
crossings, neutral and conflicting C0, representable command boundaries,
|
||||
freshness, driver override and PSCM limits. Integration tests execute the actual
|
||||
controlsd selection and upstream limiter, Float32 publication and Ford CAN
|
||||
builder, using both model and maneuver-plan requests and both turn directions.
|
||||
|
||||
The combined suite passes **567 tests and 9,146 subtests**, with the same 178
|
||||
inherited/unsupported safety-test skips as the original feedback validation.
|
||||
The randomized checks include mirrored inputs, zero-error compatibility, and
|
||||
comparison against the exact previous controller from cloned pre-update states.
|
||||
|
||||
Frozen b8 replay triggers 11 releases; b9 triggers 14. Activation and C0 match
|
||||
the previous controller exactly on every reconstructed cycle. In b9, most
|
||||
releases concern small corrections; one follows the large turn around 13:28.
|
||||
The C0/C1 disagreement at 14:36 is preserved. Numerical details and source
|
||||
hashes are in `ford_c1_carryover_validation.json`.
|
||||
|
||||
At the release around 13:28, the candidate C1 crosses into the requested
|
||||
direction 0.255 s earlier than the previous controller on identical frozen
|
||||
inputs. This is a command zero-crossing comparison, not a measured improvement
|
||||
in the truck's steering response. The lab checks total 669,343 Float32/CAN
|
||||
round trips, in addition to the integration tests.
|
||||
|
||||
Replay preserves recorded model requests and measured motion. A difference
|
||||
between candidate and baseline commands can persist because the recorded
|
||||
steering does not respond to the changed command. Replay cannot predict wheel
|
||||
angles, centering, oscillation, or how much earlier the vehicle would unwind.
|
||||
No device build, boot, installation or physical validation was performed.
|
||||
|
||||
## Reproduction
|
||||
|
||||
Use the branch's native dependencies and pinned opendbc revision
|
||||
`c21a9013700734dd20b09e05aa68329ad8cc20f9`. The route commands require the existing
|
||||
full-rlog b8/b9 extracts and the baseline Git revision. Run:
|
||||
|
||||
```sh
|
||||
export PYTHONDONTWRITEBYTECODE=1
|
||||
export PYTHONPATH=.:opendbc_repo
|
||||
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_c1_carryover/stress.json
|
||||
python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output .cache/ford_c1_carryover/zero_error_stress.json
|
||||
python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_routeb8 --baseline 5fbb583e592d30de266f8160a5d6b9c620c97f56 --output .cache/ford_c1_carryover/routeb8
|
||||
python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_routeb9 --baseline 5fbb583e592d30de266f8160a5d6b9c620c97f56 --output .cache/ford_c1_carryover/routeb9
|
||||
```
|
||||
@@ -0,0 +1,177 @@
|
||||
{
|
||||
"created_at_utc": "2026-09-10T14:00:37.853762+00:00",
|
||||
"scope": "Conditional release of accumulated C1 correction; offline command behavior only, no predicted vehicle response.",
|
||||
"baseline_commit": "5fbb583e592d30de266f8160a5d6b9c620c97f56",
|
||||
"baseline_source_sha256": "4499defbb7fc5ddf5029ca42c549f0935b0758b08818c5bf0490fb52221f9a34",
|
||||
"deployment_target": {
|
||||
"repository": "sunnypilot/sunnypilot",
|
||||
"branch": "hiimisaac-dev"
|
||||
},
|
||||
"hypothesis": "model-action-c1-feedback-v2",
|
||||
"calibration_approved": false,
|
||||
"toggle": {
|
||||
"key": "FordModelActionController",
|
||||
"default_enabled": false,
|
||||
"activation": "Existing controlsd startup selection"
|
||||
},
|
||||
"release_rule": "Fresh enabled feedback; target and slewed C0 agree with base C1 by >= one DBC step; measured curvature is opposite; stored correction makes total C1 zero or opposite base. Clear correction, then apply original integration and output slew.",
|
||||
"engineering_choices": "Conditional reset policy, using existing DBC steps (0.01 m, 0.0005 rad) to confirm nonzero commands. Original 1:1 integral strength is unchanged.",
|
||||
"preserved": [
|
||||
"C0 mapping and limits",
|
||||
"Base C1 mapping",
|
||||
"Original integral strength",
|
||||
"Final C1 amplitude and slew limits",
|
||||
"Driver and PSCM arbitration",
|
||||
"Upstream selection and limiting",
|
||||
"100 Hz sender",
|
||||
"C2=C3=0"
|
||||
],
|
||||
"panda_safety_changed": false,
|
||||
"opendbc_submodule_changed": false,
|
||||
"opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
|
||||
"controller_size": {
|
||||
"total_lines": 194,
|
||||
"code_lines_excluding_blanks_comments_docstrings": 131,
|
||||
"core_command_state_values": 3,
|
||||
"core_diagnostic_counters": 1
|
||||
},
|
||||
"tests": {
|
||||
"combined_suite": "567 passed, 178 skipped, 9146 subtests passed in 6.45s",
|
||||
"safety_skips": "Same 178 inherited or unsupported variants recorded in ford_c1_feedback_validation.json.",
|
||||
"regression": "Two mirrored carryover command tests fail on the exact baseline class and pass in the candidate suite.",
|
||||
"ruff_changed_python": "pass",
|
||||
"ty_controller": "pass",
|
||||
"settings_compiler_check": "pass",
|
||||
"carryover_controlsd_to_can_frames": 1120,
|
||||
"existing_feedback_controlsd_to_can_frames": 1010,
|
||||
"integration_scope": "Actual source selection, upstream limiting, controller, Float32 publication, Ford sender, both plan sources and signs, all counters and checksums."
|
||||
},
|
||||
"routes": {
|
||||
"b8": {
|
||||
"cycles": 160431,
|
||||
"active_cycles": 68217,
|
||||
"validity_and_c0_match_baseline_exactly": true,
|
||||
"c1_changed_cycles": 6065,
|
||||
"max_abs_c1_change_rad": 0.09250000000000003,
|
||||
"can_round_trips": 160431,
|
||||
"timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
|
||||
"reference_limit": "Uses exact consumed model publication as reference; b8 and b9 have no maneuver-plan messages.",
|
||||
"carryover_release_count": 11,
|
||||
"input_sha256": {
|
||||
"route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b",
|
||||
"model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615",
|
||||
"metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c"
|
||||
}
|
||||
},
|
||||
"b9": {
|
||||
"cycles": 90774,
|
||||
"active_cycles": 86474,
|
||||
"validity_and_c0_match_baseline_exactly": true,
|
||||
"c1_changed_cycles": 15208,
|
||||
"max_abs_c1_change_rad": 0.10400000000000004,
|
||||
"can_round_trips": 90774,
|
||||
"timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
|
||||
"reference_limit": "Uses exact consumed model publication as reference; b8 and b9 have no maneuver-plan messages.",
|
||||
"carryover_release_count": 14,
|
||||
"input_sha256": {
|
||||
"route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
|
||||
"model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
|
||||
"metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee"
|
||||
}
|
||||
}
|
||||
},
|
||||
"command_timing_example": {
|
||||
"event": {
|
||||
"time_s": 808.286646083,
|
||||
"correction_before_rad": -0.13089810321135922,
|
||||
"correction_after_rad": 0.0,
|
||||
"base_c1_rad": 0.06412824021622576,
|
||||
"desired_angle_deg": -24.17155647277832,
|
||||
"actual_angle_deg": -0.30000001192092896,
|
||||
"speed_m_s": 11.804088592529297,
|
||||
"baseline_c0_c1": [
|
||||
0.15000000000000036,
|
||||
-0.06600000000000006
|
||||
],
|
||||
"candidate_c0_c1": [
|
||||
0.15000000000000036,
|
||||
-0.062000000000000055
|
||||
]
|
||||
},
|
||||
"scope": "Command zero crossing on identical frozen recorded inputs; not wheel response.",
|
||||
"baseline_c1_rightward_at_s": 808.67241324,
|
||||
"candidate_c1_rightward_at_s": 808.4169884780001,
|
||||
"command_crossing_advance_s": 0.25542476199984776
|
||||
},
|
||||
"feedback_stress": {
|
||||
"cycles": 200000,
|
||||
"mirrored_updates": 200000,
|
||||
"can_round_trips": 200000,
|
||||
"carryover_release_count": 946,
|
||||
"baseline_revision": "5fbb583e592d30de266f8160a5d6b9c620c97f56",
|
||||
"baseline_source_sha256": "4499defbb7fc5ddf5029ca42c549f0935b0758b08818c5bf0490fb52221f9a34",
|
||||
"exact_unchanged_state_and_commands_without_release": 199054,
|
||||
"checks": "Mirror symmetry, reset/override, amplitude, slew, correction bounds, carryover direction/confirmation, integration, PSCM limits, CAN.",
|
||||
"scope": "Numerical software invariants only; no model of vehicle motion.",
|
||||
"calibration_approved": false,
|
||||
"controller_sha256": "6f40a05977253987a2c96e74c8c18d912367ed1e55630558ed7b28d52576e552"
|
||||
},
|
||||
"zero_error_stress": {
|
||||
"seed": 20260907,
|
||||
"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.40000000000000147,
|
||||
0.05000000000000002
|
||||
],
|
||||
"calibration_approved": false,
|
||||
"scope": "Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.",
|
||||
"opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
|
||||
},
|
||||
"total_lab_float32_can_round_trips": 669343,
|
||||
"source_sha256": {
|
||||
"openpilot/selfdrive/controls/lib/ford_model_action.py": "6f40a05977253987a2c96e74c8c18d912367ed1e55630558ed7b28d52576e552",
|
||||
"openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py": "04935fb941a795cb243870a4c03f7073c68147b01da3cedf2872476aa5fb798e",
|
||||
"openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "e2e98d3a0a531235abd032fc4d3564796613ad51ff6ba22a230c48e36f6f6848",
|
||||
"openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "90fb42ce580f0085e349467086a2eef28c2512c671755d0771f6331d30b19035",
|
||||
"tools/ford_pscm_lab/feedback_replay.py": "9bf145fbff6ed685aec2c0e5d0584e2dc7ff831021f15110f939e8a94c93280b",
|
||||
"tools/ford_pscm_lab/stress_model_action.py": "0b25188edf2b248ebe741173ce02ce75bd59f1f39fd5bd909d41a3dca2294aa8",
|
||||
"tools/ford_pscm_lab/model_action_replay.py": "af97c665f342c66b1be2502e188c63e6f3ee106d0a0d5e80997bc3040373ff9f",
|
||||
"docs/ford_c1_carryover.md": "e8d08375963efc6d1ae6ce503bb580ba00cfa90adb71c941d56fe6e3701d5cbb",
|
||||
"docs/ford_c1_feedback.md": "1b440a03082e5cec264a1d6693833ed0a7e07b6c6e2122f8e4455c5971121b57",
|
||||
"docs/ford_model_action_drive_test.md": "7ac5ca0faf9690a23e7058d09b55f23e21e2ba74666001cacb56b67e8d8b4376",
|
||||
"openpilot/selfdrive/controls/controlsd.py": "2b7e246f00bccce3a2bb9f6f44009ca77690cadb8527cd2bdfe855e9ad72ad1e",
|
||||
"opendbc_repo/opendbc/car/ford/carcontroller.py": "b2d327a1833fb1f0d09ee17f54c9c8d45517fa29beb04a4543cfbf1b43f1a65e",
|
||||
"opendbc_repo/opendbc/safety/modes/ford.h": "1d9d996292d6697ab4f02d55fae348d6aca1df94a07f7bdae48b68971b91afe7",
|
||||
"openpilot/sunnypilot/sunnylink/settings_ui.json": "7d38f315a7c5ce6d46d01a06f7eaddd4933f85639e5325ff71fdce22866ef401"
|
||||
},
|
||||
"artifact_sha256": {
|
||||
".cache/ford_c1_carryover/tests.txt": "cd65146df93632e4a2c1e086781e1da4673db7d038c7e673124f227efefbb567",
|
||||
".cache/ford_c1_carryover/baseline_regression.txt": "4469873f95ccf45a376a27fed05be3bdad5f808af7ceca472c6e2cb9d973eb7f",
|
||||
".cache/ford_c1_carryover/stress.json": "7a8d20d7abd7cf444f55b7316e8bedbed0fbe8e9587e09f90d5b1946c3c2e98c",
|
||||
".cache/ford_c1_carryover/zero_error_stress.json": "09e64eaac35df4ec324b41fabdc8baf91931106ac98b89c1ca71f4c8bf8796a4",
|
||||
".cache/ford_c1_carryover/timing.json": "0d18ed4164803adedbbd660fe024f4c28de8eb7921caa60659346ff386d3847f",
|
||||
".cache/ford_c1_carryover/routeb8/report.json": "d2c6f767cef29a74e292b6a16263d2da13b8c302e4653e419b0e232e1aaf762e",
|
||||
".cache/ford_c1_carryover/routeb8/commands.npz": "89b5c3474940b61afce060111c27fd9bad9e24d703c59fca61adf4ce10473df3",
|
||||
".cache/ford_c1_carryover/routeb9/report.json": "e3ff7bfa70e770eca763b125c283fd8a1d509ef1b6e7f26a81c398aca89a87da",
|
||||
".cache/ford_c1_carryover/routeb9/commands.npz": "e4f5f341146e2897a479baf222d678fd16352c8da931876a2471c3719faf9edf"
|
||||
},
|
||||
"test_environment": {
|
||||
"python": "/Users/ibpersonal/dev/sunnypilot/.venv/bin/python",
|
||||
"PYTHONPATH": ".:opendbc_repo:.cache/ford_v6/test_deps",
|
||||
"PYTHONDONTWRITEBYTECODE": "1",
|
||||
"LOG_ROOT": "/private/tmp/ford-carryover-logs",
|
||||
"PARAMS_ROOT": "/private/tmp/ford-carryover-params"
|
||||
},
|
||||
"limitations": [
|
||||
"Frozen replay preserves recorded requests and measured motion; changed commands do not establish changed wheel angles, centering or stability.",
|
||||
"C0/C1 agreement is a reset-policy choice, not an identified relationship between PSCM input and wheel angle.",
|
||||
"The rule can release small corrections and does not promise unchanged centering during transients.",
|
||||
"No device build, boot, installation or physical validation was performed."
|
||||
]
|
||||
}
|
||||
@@ -1,5 +1,10 @@
|
||||
# Ford C1 feedback experiment
|
||||
|
||||
This document records the original feedback change at `5fbb583e5`. The current
|
||||
version retains its feedback law and adds [conditional carryover release](ford_c1_carryover.md).
|
||||
The validation counts below describe the original change; current results are
|
||||
recorded in `ford_c1_carryover_validation.json`.
|
||||
|
||||
The restored original v1 can leave a steering error while C0 and C1 still have
|
||||
room. Its command law does not directly correct measured steering error. This
|
||||
experiment keeps that mapping and adds one accumulated C1 correction:
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
# Ford selected-action drive-test branch
|
||||
|
||||
The current experiment adds [measured-curvature C1 feedback](ford_c1_feedback.md)
|
||||
to the restored original v1 mapping. It is selectable on the **Ford CAN FD
|
||||
and [conditional correction release](ford_c1_carryover.md) to the restored
|
||||
original v1 mapping. It is selectable on the **Ford CAN FD
|
||||
F-150 Lightning** through the existing persistent, default-off Sunnylink
|
||||
toggle. Offline checks establish software behavior; physical tracking,
|
||||
turn-exit behavior and closed-loop stability remain unvalidated.
|
||||
@@ -18,9 +19,11 @@ 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-v1`**. They report desired and
|
||||
identify **`hypothesis=model-action-c1-feedback-v2`**. They report desired and
|
||||
measured curvature, base heading, accumulated correction, applied heading,
|
||||
feedback timing and driver/PSCM gating. `calibration_approved=false` remains.
|
||||
feedback timing and driver/PSCM gating. `carryover_release_count` counts
|
||||
conditional releases since the last controller reset; it does not control
|
||||
steering. `calibration_approved=false` remains.
|
||||
|
||||
Turning the toggle off and completing another offroad-to-onroad cycle restores
|
||||
**PSCM Coefficient Observer** if selected, otherwise the original Ford path
|
||||
@@ -36,14 +39,19 @@ Fresh steering publications advance C1 feedback. Repeated publications may
|
||||
advance output slew but cannot integrate the same elapsed interval twice.
|
||||
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 target 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.
|
||||
|
||||
C0 retains the original 7 m model-path mapping. C2 and C3 remain zero. The
|
||||
existing output limits, 100 Hz sender, Float32 publication and CAN builder
|
||||
remain in place. No opendbc submodule or Panda safety change is required.
|
||||
|
||||
See [the feedback specification and validation](ford_c1_feedback.md) and
|
||||
`ford_c1_feedback_validation.json` for the current evidence and reproduction
|
||||
commands. `ford_model_action_validation.json` and
|
||||
See [the carryover specification and validation](ford_c1_carryover.md) and
|
||||
`ford_c1_carryover_validation.json` for current evidence and reproduction
|
||||
commands. `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
|
||||
original offline candidate and its first wiring, respectively; their counts
|
||||
and coverage are not claims about the feedback version.
|
||||
|
||||
@@ -3,6 +3,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.
|
||||
"""
|
||||
import math
|
||||
import struct
|
||||
@@ -57,13 +58,14 @@ class ModelActionController:
|
||||
Feedback integrates requested minus measured curvature over traveled distance.
|
||||
Freshness, measurement cadence and driver/PSCM arbitration belong to the caller.
|
||||
"""
|
||||
__slots__ = ('c0', 'c1', 'correction')
|
||||
__slots__ = ('c0', 'c1', 'correction', 'carryover_release_count')
|
||||
|
||||
def __init__(self):
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.c0 = self.c1 = self.correction = 0.
|
||||
self.carryover_release_count = 0 # Diagnostic only; never feeds the command law.
|
||||
|
||||
def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True,
|
||||
feedback_dt=None, feedback_enabled=True, pscm_limited=False):
|
||||
@@ -77,12 +79,23 @@ class ModelActionController:
|
||||
self.reset()
|
||||
return FordPath()
|
||||
c0 = float(np.clip(target.path_offset, -5.11, 5.11))
|
||||
self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt))
|
||||
base_c1 = float(np.clip(target.path_angle, -.5, .5))
|
||||
lower = max(-.5, self.c1-.5*dt)
|
||||
upper = min(.5, self.c1+.5*dt)
|
||||
if not feedback_enabled:
|
||||
self.correction = 0.
|
||||
else:
|
||||
direction = math.copysign(1., base_c1)
|
||||
# Release only correction that prevents C1 from requesting the direction
|
||||
# shared by target C0, slewed C0 and base C1, while measured steering is
|
||||
# still opposite. One DBC step confirms each request is nonzero. Matched
|
||||
# steering, neutral/conflicting centering and duplicate samples retain I.
|
||||
if (feedback_dt > 0. and abs(base_c1) >= .0005 and current_curvature*direction < 0.
|
||||
and min(c0*direction, self.c0*direction) >= .01
|
||||
and (base_c1+self.correction)*direction <= 0.):
|
||||
self.correction = 0.
|
||||
self.carryover_release_count += 1
|
||||
increment = (desired_curvature-current_curvature)*speed*feedback_dt
|
||||
# LimitReached inhibits only extra demand in the measured turn direction.
|
||||
# Opposing correction and changes to the model request remain available.
|
||||
@@ -96,7 +109,6 @@ 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.)))
|
||||
self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt))
|
||||
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,7 +133,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-v1',
|
||||
self.diagnostics = {'status': status, 'hypothesis': 'model-action-c1-feedback-v2',
|
||||
'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,
|
||||
@@ -161,13 +173,14 @@ class FordModelActionController:
|
||||
self.reset('invalid_path')
|
||||
return command
|
||||
self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time
|
||||
self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c1-feedback-v1',
|
||||
self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c1-feedback-v2',
|
||||
'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,
|
||||
'curvature_error': desired_curvature-current_curvature, 'feedback_dt': feedback_dt,
|
||||
'heading_feedforward': float(np.clip(max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature, -.5, .5)),
|
||||
'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled,
|
||||
'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.)}
|
||||
return command
|
||||
|
||||
@@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase):
|
||||
controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.005, curvature=.0025,
|
||||
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-v1')
|
||||
self.assertEqual(record['hypothesis'], 'model-action-c1-feedback-v2')
|
||||
self.assertIs(record['calibration_approved'], False)
|
||||
self.assertEqual(record['command'][2:], [0., 0.])
|
||||
self.assertEqual(record['status'], controller.diagnostics['status'])
|
||||
|
||||
@@ -289,3 +289,55 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s
|
||||
assert controller.diagnostics['pscm_limited'] is service_valid
|
||||
assert controller.core.correction == pytest.approx(0. if service_valid else .0002)
|
||||
assert cc.latActive and controls.ford_path.valid
|
||||
|
||||
|
||||
@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):
|
||||
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.)
|
||||
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),
|
||||
lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
|
||||
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main)
|
||||
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))
|
||||
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)
|
||||
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
|
||||
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])
|
||||
if frame == 199:
|
||||
assert core.correction == pytest.approx(sign*.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-v2'
|
||||
assert sign*controls.ford_path.path_angle < 0.
|
||||
assert controls.ford_path.path_offset == pytest.approx(-sign*.2)
|
||||
controls.ford_path_controller.reset()
|
||||
assert core.carryover_release_count == 0
|
||||
|
||||
@@ -15,6 +15,114 @@ def tick(controller, desired, measured, **overrides):
|
||||
return controller.update(straight(.4), desired, **kwargs)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_old_turn_correction_does_not_keep_c1_in_old_direction_after_reversal(sign):
|
||||
controller = ModelActionController()
|
||||
# Build an actual correction through feedback, rather than injecting a state.
|
||||
for _ in range(100):
|
||||
controller.update(straight(), sign*.004, current_curvature=sign*.004, speed=20., dt=.01)
|
||||
for _ in range(100):
|
||||
controller.update(straight(), sign*.004, current_curvature=sign*.001, speed=20., dt=.01)
|
||||
assert controller.correction == pytest.approx(sign*.06)
|
||||
# The request has reversed, but measured steering still points into the old
|
||||
# turn. C0 also confirms the new direction. The existing slew permits
|
||||
# crossing zero within these 0.4 seconds.
|
||||
for _ in range(40):
|
||||
before = controller.c1
|
||||
out = controller.update(straight(-sign*.2), -sign*.001, current_curvature=sign*.003, speed=20., dt=.01)
|
||||
assert abs(controller.c1-before) <= .0050000001
|
||||
assert sign*out.path_angle < 0., 'Stored old-turn correction still overrides the new C1 direction'
|
||||
assert out.path_offset == pytest.approx(-sign*.2)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('limited', [False, True])
|
||||
def test_carryover_release_uses_fresh_feedback_and_preserves_final_slew(sign, limited):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = -sign*.2, sign*.03, sign*.05
|
||||
kwargs = {'speed': 20., 'dt': .01, 'current_curvature': sign*.003, 'pscm_limited': limited}
|
||||
controller.update(straight(-sign*.2), -sign*.001, feedback_dt=0., **kwargs)
|
||||
assert controller.correction == pytest.approx(sign*.05)
|
||||
before = controller.c1
|
||||
out = controller.update(straight(-sign*.2), -sign*.001, **kwargs)
|
||||
assert controller.correction == 0.
|
||||
assert controller.carryover_release_count == 1
|
||||
assert out.path_angle == pytest.approx(before-sign*.005)
|
||||
for _ in range(30):
|
||||
controller.update(straight(-sign*.2), -sign*.001, **kwargs)
|
||||
assert controller.carryover_release_count == 1
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('case', ['matched', 'same_turn', 'already_turning_new_way', 'c0_opposes', 'c0_neutral',
|
||||
'c0_sub_resolution', 'target_c0_opposes', 'c1_sub_resolution',
|
||||
'neutral_request', 'neutral_measurement', 'correction_helps', 'correction_not_dominant'])
|
||||
def test_carryover_release_preserves_steady_correction_and_ambiguous_requests(sign, case):
|
||||
controller = ModelActionController()
|
||||
offset, desired, measured, correction = -.2, -.001, .003, .05
|
||||
if case == 'matched':
|
||||
measured = desired
|
||||
elif case == 'same_turn':
|
||||
measured = -.0005
|
||||
elif case == 'already_turning_new_way':
|
||||
measured = -.003
|
||||
elif case == 'c0_opposes':
|
||||
offset = .2
|
||||
elif case == 'c0_neutral':
|
||||
offset = 0.
|
||||
elif case == 'c0_sub_resolution':
|
||||
offset = -.001
|
||||
elif case == 'c1_sub_resolution':
|
||||
desired = -.000001
|
||||
elif case == 'neutral_request':
|
||||
desired = 0.
|
||||
elif case == 'neutral_measurement':
|
||||
measured = 0.
|
||||
elif case == 'correction_helps':
|
||||
correction = -.05
|
||||
elif case == 'correction_not_dominant':
|
||||
correction = .01
|
||||
controller.c0, controller.c1, controller.correction = sign*offset, sign*(20.*desired+correction), sign*correction
|
||||
if case == 'target_c0_opposes':
|
||||
offset = .2 # The old slewed C0 alone is not enough to confirm the request.
|
||||
controller.update(straight(sign*offset), sign*desired, current_curvature=sign*measured, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == 0
|
||||
assert abs(controller.correction-sign*correction) <= abs(desired-measured)*20.*.01+1e-10
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_carryover_release_waits_for_c0_to_finish_opposing_the_new_request(sign):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = sign*.2, sign*.03, sign*.05
|
||||
for _ in range(4):
|
||||
controller.update(straight(-sign*.2), -sign*.001, current_curvature=sign*.003, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == 0
|
||||
for _ in range(3):
|
||||
controller.update(straight(-sign*.2), -sign*.001, current_curvature=sign*.003, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == 1
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_steady_opposing_correction_is_preserved_through_small_error_crossings(sign):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = -sign*.2, sign*.03, sign*.05
|
||||
for i in range(200):
|
||||
measured = sign*(-.001+(-1 if i % 2 else 1)*.000001)
|
||||
controller.update(straight(-sign*.2), -sign*.001, current_curvature=measured, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == 0
|
||||
assert controller.correction == pytest.approx(sign*.05)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('offset', [.009999, .01, .010001])
|
||||
@pytest.mark.parametrize('heading', [.0004999, .0005, .0005001])
|
||||
def test_carryover_direction_confirmation_uses_existing_dbc_steps(sign, offset, heading):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = -sign*offset, sign*.03, sign*.05
|
||||
controller.update(straight(-sign*offset), -sign*heading/20., current_curvature=sign*.003, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == int(offset >= .01 and heading >= .0005)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign):
|
||||
controller, matched = ModelActionController(), ModelActionController()
|
||||
|
||||
@@ -7,6 +7,7 @@ fixed. This verifies software behavior, not counterfactual physical tracking.
|
||||
import argparse
|
||||
from collections import Counter
|
||||
import hashlib
|
||||
import inspect
|
||||
import json
|
||||
from pathlib import Path
|
||||
import subprocess
|
||||
@@ -20,19 +21,20 @@ from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sam
|
||||
|
||||
|
||||
BASELINE = 'a7d70e2b0890184636827351e4789d866f2a7c97'
|
||||
FEEDBACK_V1 = '5fbb583e592d30de266f8160a5d6b9c620c97f56'
|
||||
OPENDBC = 'c21a9013700734dd20b09e05aa68329ad8cc20f9'
|
||||
|
||||
|
||||
def original_controller():
|
||||
def original_controller(revision=BASELINE):
|
||||
root = Path(__file__).resolve().parents[2]
|
||||
source = subprocess.check_output(['git', '-C', str(root), 'show',
|
||||
f'{BASELINE}:openpilot/selfdrive/controls/lib/ford_model_action.py'], text=True)
|
||||
f'{revision}:openpilot/selfdrive/controls/lib/ford_model_action.py'], text=True)
|
||||
module = ModuleType('ford_original_v1')
|
||||
exec(compile(source, f'{BASELINE}:ford_model_action.py', 'exec'), module.__dict__)
|
||||
exec(compile(source, f'{revision}:ford_model_action.py', 'exec'), module.__dict__)
|
||||
return module.FordModelActionController(), hashlib.sha256(source.encode()).hexdigest()
|
||||
|
||||
|
||||
def replay(directory, output):
|
||||
def replay(directory, output, baseline_revision=BASELINE):
|
||||
directory, output = directory.resolve(), output.resolve()
|
||||
if output == directory or directory in output.parents:
|
||||
raise ValueError('Output must be outside the source route directory')
|
||||
@@ -53,17 +55,20 @@ def replay(directory, output):
|
||||
exact = r['model']['ns'][mi] == c['model_ns']
|
||||
np.testing.assert_array_equal(model_ns, r['model']['ns'])
|
||||
models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths]
|
||||
old, baseline_hash = original_controller()
|
||||
old, baseline_hash = original_controller(baseline_revision)
|
||||
old_has_feedback = 'current_curvature' in inspect.signature(old.update).parameters
|
||||
controller, wire_check = FordModelActionController(), WireCheck()
|
||||
baseline = np.zeros((len(t), 4))
|
||||
commands = np.zeros_like(baseline)
|
||||
old_valid = np.zeros(len(t), bool)
|
||||
valid = np.zeros(len(t), bool)
|
||||
correction = np.zeros(len(t))
|
||||
baseline_correction = np.zeros(len(t))
|
||||
feedback_dt = np.zeros(len(t))
|
||||
feedback_enabled = np.zeros(len(t), bool)
|
||||
pscm_limited = np.zeros(len(t), bool)
|
||||
reasons = Counter()
|
||||
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]
|
||||
@@ -75,18 +80,27 @@ def replay(directory, output):
|
||||
model = models[mi[i]] if exact[i] else None
|
||||
status = SimpleNamespace(valid=bool(ps['valid'][i] and ps['status_valid'][i]), canMonoTime=round(ps['stamp'][i]*1e9),
|
||||
limit=int(ps['limit'][i]), lateralState=int(ps['lateral_state'][i]), denied=bool(ps['denied'][i]))
|
||||
previous = old.update(model, c['desired'][i], **common)
|
||||
command = controller.update(model, c['desired'][i], current_curvature=c['measured'][i],
|
||||
driver_pressed=bool(cs['pressed'][i]), driver_torque=cs['torque'][i], pscm_status=status, **common)
|
||||
feedback = {'current_curvature': c['measured'][i], 'driver_pressed': bool(cs['pressed'][i]),
|
||||
'driver_torque': cs['torque'][i], 'pscm_status': status}
|
||||
previous = old.update(model, c['desired'][i], **common, **(feedback if old_has_feedback else {}))
|
||||
previous_count, previous_correction = controller.core.carryover_release_count, controller.core.correction
|
||||
command = controller.update(model, c['desired'][i], **feedback, **common)
|
||||
for destination, result in ((baseline, previous), (commands, command)):
|
||||
destination[i] = result.path_offset, result.path_angle, result.curvature, result.curvature_rate
|
||||
old_valid[i], valid[i] = previous.valid, command.valid
|
||||
d = controller.diagnostics
|
||||
reasons[d['status']] += 1
|
||||
correction[i] = controller.core.correction
|
||||
baseline_correction[i] = getattr(old.core, 'correction', 0.)
|
||||
feedback_dt[i] = d.get('feedback_dt', 0.)
|
||||
feedback_enabled[i] = d.get('feedback_enabled', False)
|
||||
pscm_limited[i] = d.get('pscm_limited', False)
|
||||
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']),
|
||||
'desired_angle_deg': float(c['desired_angle'][i]), 'actual_angle_deg': float(c['actual_angle'][i]),
|
||||
'speed_m_s': float(cs['speed'][i]), 'baseline_c0_c1': baseline[i, :2].tolist(),
|
||||
'candidate_c0_c1': commands[i, :2].tolist()})
|
||||
wire_check.check(command)
|
||||
field_checks(commands, valid, t)
|
||||
np.testing.assert_array_equal(valid, old_valid)
|
||||
@@ -94,9 +108,10 @@ def replay(directory, output):
|
||||
assert np.all(correction[~feedback_enabled] == 0.)
|
||||
assert np.all(abs(correction) <= 1.+1e-10)
|
||||
weight = np.minimum(np.diff(t, append=t[-1]+.01), .03)
|
||||
report = {'scope': __doc__, 'baseline_revision': BASELINE, 'baseline_source_sha256': baseline_hash,
|
||||
report = {'scope': __doc__, 'baseline_revision': baseline_revision, 'baseline_source_sha256': baseline_hash,
|
||||
'calibration_approved': False, 'cycles': len(t), 'active_cycles': int(valid.sum()),
|
||||
'validity_and_c0_match_original_v1_exactly': True, 'status_counts': dict(reasons),
|
||||
'validity_and_c0_match_baseline_exactly': True, 'status_counts': dict(reasons),
|
||||
'carryover_releases': releases,
|
||||
'feedback_enabled_seconds': float(weight[feedback_enabled].sum()),
|
||||
'pscm_limit_2_seconds': float(weight[pscm_limited & valid].sum()),
|
||||
'c1_changed_cycles': int((abs(commands[:, 1]-baseline[:, 1]) > 1e-8).sum()),
|
||||
@@ -106,47 +121,71 @@ def replay(directory, output):
|
||||
(directory/'route.npz', directory/'model_paths.npz', directory/'metadata.json',
|
||||
Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())},
|
||||
'timing_limit': 'Controls publication time proxies the computation clock; full SubMaster checks are unavailable.',
|
||||
'reference_limit': 'Uses exact consumed model publication as reference; the b8 route has no maneuver-plan messages.'}
|
||||
'reference_limit': 'Uses exact consumed model publication as reference; b8 and b9 have no maneuver-plan messages.'}
|
||||
report['example_points'] = []
|
||||
for seconds in (130.937, 235.396, 236.250, 252.493, 674.430, 1534.519, 1562.507):
|
||||
for seconds in (130.937, 235.396, 236.250, 252.493, 330.651, 674.430, 808.408, 876.419, 1534.519, 1562.507):
|
||||
if seconds > t[-1]-metadata['t0']:
|
||||
continue
|
||||
i = int(np.argmin(abs(t-metadata['t0']-seconds)))
|
||||
report['example_points'].append({'time_s': float(t[i]-metadata['t0']), 'old_c0_c1': baseline[i, :2].tolist(),
|
||||
'candidate_c0_c1': commands[i, :2].tolist(), 'correction_rad': float(correction[i]),
|
||||
'feedback_enabled': bool(feedback_enabled[i]), 'pscm_limited': bool(pscm_limited[i])})
|
||||
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, feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited)
|
||||
correction=correction, baseline_correction=baseline_correction, feedback_dt=feedback_dt,
|
||||
feedback_enabled=feedback_enabled, pscm_limited=pscm_limited)
|
||||
(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 != 'source_sha256'}, indent=2))
|
||||
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))
|
||||
|
||||
|
||||
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_V1)
|
||||
releases = 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_count = controller.carryover_release_count
|
||||
args = {'speed': speed, 'dt': dt, 'feedback_dt': feedback_dt, 'active': active,
|
||||
'feedback_enabled': enabled, 'pscm_limited': limited}
|
||||
def model(y):
|
||||
return SimpleNamespace(position=SimpleNamespace(x=[0., 20.], y=[y, y]), orientation=SimpleNamespace(z=[0., 0.]))
|
||||
out = controller.update(model(offset), desired, current_curvature=measured, **args)
|
||||
other = mirror.update(model(-offset), -desired, current_curvature=-measured, **args)
|
||||
# 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
|
||||
baseline_out = old.core.update(model(offset), desired, current_curvature=measured, **args)
|
||||
released = controller.carryover_release_count > previous_count
|
||||
if not released:
|
||||
assert out == baseline_out
|
||||
assert (controller.c0, controller.c1, controller.correction) == (old.core.c0, old.core.c1, old.core.correction)
|
||||
else:
|
||||
assert out.path_offset == baseline_out.path_offset
|
||||
assert (controller.c1-old.core.c1)*(desired-measured) >= -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(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
|
||||
assert abs(controller.c1-previous[1]) <= .5*dt+1e-10
|
||||
if enabled:
|
||||
delta = controller.correction-previous[2]
|
||||
releases += released
|
||||
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])
|
||||
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
|
||||
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
|
||||
@@ -157,7 +196,10 @@ def stress(cycles, output):
|
||||
assert out.curvature == out.curvature_rate == other.curvature == other.curvature_rate == 0.
|
||||
wire.check(out)
|
||||
report = {'cycles': cycles, 'mirrored_updates': cycles, 'can_round_trips': wire.count,
|
||||
'checks': 'Mirror symmetry, reset/override, amplitude, slew, correction bounds, integration direction/size, PSCM anti-windup, CAN fields.',
|
||||
'carryover_release_count': releases,
|
||||
'baseline_revision': FEEDBACK_V1, 'baseline_source_sha256': baseline_hash,
|
||||
'exact_unchanged_state_and_commands_without_release': cycles-releases,
|
||||
'checks': 'Mirror symmetry, reset/override, amplitude, slew, correction bounds, carryover direction/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)
|
||||
@@ -171,11 +213,12 @@ if __name__ == '__main__':
|
||||
route = sub.add_parser('route')
|
||||
route.add_argument('directory', type=Path)
|
||||
route.add_argument('--output', type=Path, required=True)
|
||||
route.add_argument('--baseline', default=BASELINE, help='Git revision of the original mapping or a feedback controller')
|
||||
random = sub.add_parser('stress')
|
||||
random.add_argument('--cycles', type=int, default=200_000)
|
||||
random.add_argument('--output', type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
if args.mode == 'route':
|
||||
replay(args.directory, args.output)
|
||||
replay(args.directory, args.output, args.baseline)
|
||||
else:
|
||||
stress(args.cycles, args.output)
|
||||
|
||||
Reference in New Issue
Block a user