mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-15 00:03:45 +08:00
Ford: add explicit proportional C1 feedback trial
This commit is contained in:
Isaac Barham
@@ -0,0 +1,151 @@
|
||||
# Ford C1 proportional feedback trial
|
||||
|
||||
V6 adds **P = 0.25** to the selected-action controller. It responds to a steering
|
||||
shortfall immediately and subtracts demand immediately when the wheel exceeds
|
||||
the selected request. V5 accumulated correction over traveled distance. P has
|
||||
no stored correction to release when its error disappears.
|
||||
|
||||
This is an initial drive-trial gain, not an identified PSCM calibration or a
|
||||
claim of improved physical tracking. Six Lightning routes establish command
|
||||
behavior across recorded scenarios. They cannot identify the best stable gain
|
||||
without observing the vehicle responding to the changed commands.
|
||||
|
||||
## Command law
|
||||
|
||||
With curvature in inverse meters, speed in meters per second and heading in radians:
|
||||
|
||||
```text
|
||||
D = max(7 m, speed × 1 s)
|
||||
error = selected_limited_curvature - measured_steering_curvature
|
||||
base_C1 = clip(D × selected_limited_curvature, -0.5, +0.5)
|
||||
P = 0.25 × D × error
|
||||
I_increment = speed × error × fresh_measurement_elapsed_time
|
||||
C1 = amplitude_and_slew_limit(base_C1 + P + I)
|
||||
```
|
||||
|
||||
P is 25% of the heading-equivalent tracking error, not a 25% multiplier on the
|
||||
model request. At matched curvature it is zero. It is stateless and can change
|
||||
with a new request even if a steering publication repeats; repeated steering
|
||||
publications still cannot integrate I twice. Driver override and fresh PSCM
|
||||
denied/inactive states clear both feedback terms. Fresh `limit=2` inhibits
|
||||
outward I accumulation while permitting unwind; P remains available inside the
|
||||
existing combined output envelope.
|
||||
|
||||
The existing C1 amplitude limit (±0.5 rad) and slew (0.5 rad/s) apply to the sum.
|
||||
Anti-windup includes P when calculating I's available headroom. P can consume
|
||||
a slew interval that previously allowed I accumulation. The conditional I
|
||||
release rules, including [completed-unwind release](ford_unwind_catchup.md),
|
||||
remain. C0 retains the same 7 m mapping, base-heading overflow, cap and slew;
|
||||
neither P nor I spills into C0. C2/C3 stay zero. No plant, gain schedule or
|
||||
automatic gain learning is introduced.
|
||||
|
||||
Onroad selection explicitly supplies `C1_PROPORTIONAL_GAIN = 0.25`. Direct
|
||||
`FordModelActionController()` and `ModelActionController()` construction defaults
|
||||
to zero P for v5 reference/replay compatibility. The existing default-off
|
||||
Sunnylink toggle selects v6 on any Ford CAN FD. Toggle off still selects
|
||||
upstream Ford control. See [installation and selection](ford_model_action_drive_test.md).
|
||||
|
||||
## Lightning replay findings
|
||||
|
||||
Routes `112`, `113`, `114`, `115`, `b9` and `ca` supplied 677,871 control cycles.
|
||||
Each was replayed with P gains 0, 0.1, 0.25 and 0.5, paired with diagnostic
|
||||
feedback delays 0, 0.2 and 0.4 s: 12 combinations and 8,134,452 candidate updates.
|
||||
Recorded model, driver, steering and PSCM inputs stayed fixed.
|
||||
|
||||
Both command columns are replayed C1 in radians with left positive. The angle
|
||||
pair is the single recorded desired/actual wheel measurement, not a predicted
|
||||
outcome for either candidate.
|
||||
|
||||
| Example | Desired / actual angle | V5 C1 | P=0.25 C1 |
|
||||
| --- | ---: | ---: | ---: |
|
||||
| 115, 207.908 s: late left entry | 94.2° / 51.2° | +0.1685 | +0.1825 |
|
||||
| 115, 208.099 s: entry continues | 111.0° / 72.8° | +0.2105 | +0.2245 |
|
||||
| 114, 473.086 s: well-tracked bend | 57.3° / 56.3° | +0.1855 | +0.1850 |
|
||||
| 113, 481.567 s: hanging right exit | −7.6° / −94.4° | +0.0645 | +0.0875 |
|
||||
| 115, 133.595 s: completed unwind | 6.0° / 29.9° | +0.0105 | 0.0000 |
|
||||
|
||||
The completed-unwind example is excluded by the original quality/driver clean
|
||||
mask. It is useful for checking command release, not autonomous tracking
|
||||
attribution. Large-turn windows often contain interventions and require review
|
||||
of driver input before assigning a tracking result to the controller.
|
||||
|
||||
Across 2,740.44 seconds of valid, feedback-enabled, clean samples with desired
|
||||
wheel angle below 30°, the duration-weighted mean absolute C1 change is
|
||||
0.001001 rad at P=0.25, versus 0.001961 at P=0.5. Per-route 95th-percentile
|
||||
changes at P=0.25 are 0.0025–0.0070 rad; the largest ordinary-cohort change is
|
||||
0.0350 rad. Small average command changes do not establish unchanged centering
|
||||
or stability.
|
||||
|
||||
P=0.25 is an engineering choice between the tested smaller and larger responses,
|
||||
not an optimization result. At the late-entry example, adding a fixed 0.4 s
|
||||
feedback delay instead gives C1 +0.1420 rad. Across the ordinary cohort, that
|
||||
delayed P=0.25 variant changes C1 by 0.011255 rad on average. V6 therefore
|
||||
retains v5's feedback timing to isolate P. This does not identify or disprove
|
||||
the vehicle's physical delay. The diagnostic delay variants change only P and
|
||||
I integration targets; request-release decisions still use the current request.
|
||||
|
||||
## Tuning and next-drive evidence
|
||||
|
||||
Comma's [torque controller](https://github.com/commaai/openpilot/blob/master/openpilot/selfdrive/controls/lib/latcontrol_torque.py)
|
||||
separates feedforward, P and I and aligns its torque feedback reference with
|
||||
steering delay. Its [angle PID controller](https://github.com/commaai/openpilot/blob/master/openpilot/selfdrive/controls/lib/latcontrol_pid.py)
|
||||
uses desired-minus-measured steering angle directly. These different paths do
|
||||
not imply one delay setting should be copied into Ford C1.
|
||||
|
||||
Use the same discipline: explicit parameters, separate term logging, fixed
|
||||
request conditions and measured response. Comma's
|
||||
[lateral maneuver report](https://blog.comma.ai/0111release/#lateral-maneuver-report)
|
||||
uses repeatable step/sine maneuvers to assess response. C1 is a path-heading
|
||||
request to another controller, not normalized steering torque; numerical torque
|
||||
gains and torque calibration cannot be copied across.
|
||||
|
||||
For the next controlled evaluation, compare similar speeds and model requests:
|
||||
entry delay/shortfall, overshoot as the request relaxes, correction after catch-up,
|
||||
ordinary-bend centering and oscillation. Keep desired/actual tracking on original
|
||||
timestamps. Check C0/C1 caps, slew, driver input and fresh PSCM flags separately.
|
||||
More gain cannot remove hardware limits and can introduce oscillation. These
|
||||
logs all come from a Lightning; the gain is not yet validated across other
|
||||
PSCMs. No scripted maneuver mode is enabled by this change.
|
||||
|
||||
Periodic `Ford C2-free path tracking` events identify
|
||||
`hypothesis=model-action-c1-pi-v6` and expose `heading_proportional`,
|
||||
`proportional_gain`, `feedback_curvature` and `feedback_error` alongside
|
||||
`heading_feedforward`, `heading_correction`, command and release diagnostics.
|
||||
`calibration_approved=false` remains.
|
||||
|
||||
## Validation and reproduction
|
||||
|
||||
The final selected path exactly matches the sweep's P=0.25, zero-delay variant
|
||||
on all six routes, including C0/C1, P, I and activation. Zero-P/zero-delay matches
|
||||
v5 exactly on every cycle. All variants preserve C0 and activation. Another
|
||||
200,000 seeded stress cycles check PI arithmetic, anti-windup, mirror symmetry,
|
||||
driver/PSCM arbitration, resets, amplitude/slew and zero-P parity. Sweep,
|
||||
selected replay and stress total **9,012,323 Float32/CAN round trips**.
|
||||
Encoding checks do not test vehicle motion.
|
||||
|
||||
**776 tests and 9,145 subtests passed; 178 were skipped.** Coverage includes
|
||||
actual startup selection, controlsd request source/limiting, Float32 publication,
|
||||
100 Hz CAN encoding, checksums, both turn signs, integral release, Sunnylink
|
||||
persistence, toggle-off upstream behavior and Ford safety tests. Ruff,
|
||||
controller Ty and settings compilation passed. A hardware build/device boot
|
||||
and physical response tests have not been performed.
|
||||
|
||||
Use the project's Python environment and built cereal/opendbc dependencies:
|
||||
|
||||
```sh
|
||||
export PYTHONPATH=.:opendbc_repo:.cache/ford_v6/test_deps
|
||||
export PYTHONDONTWRITEBYTECODE=1
|
||||
export PARAMS_ROOT=/tmp/ford-pi-test-params
|
||||
export LOG_ROOT=/tmp/ford-pi-test-logs
|
||||
python -m tools.ford_pscm_lab.pi_replay .cache/ford_route115 --output .cache/ford_pi_sweep/route115
|
||||
python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_route115 --baseline 22d188776cb557acea459a1fca70812bdb2df46c --output .cache/ford_pi_sweep/selected115
|
||||
python -m tools.ford_pscm_lab.pi_stress --cycles 200000 --gain .25 --output .cache/ford_pi_sweep/stress.json
|
||||
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
|
||||
```
|
||||
|
||||
Repeat both replay commands for the other five extracts. The machine-readable
|
||||
[validation record](ford_c1_pi_validation.json) records counts, source hashes,
|
||||
cohort definitions and sampled command changes. Publication time proxies the
|
||||
computation clock; full SubMaster state and selected maneuver-plan publications
|
||||
are not reconstructed. Real maneuver source selection is exercised in integration
|
||||
tests. Historical controller reports retain their original version scope.
|
||||
@@ -0,0 +1,373 @@
|
||||
{
|
||||
"hypothesis": "model-action-c1-pi-v6",
|
||||
"baseline_revision": "22d188776cb557acea459a1fca70812bdb2df46c",
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"selected_gain": 0.25,
|
||||
"selected_feedback_delay_s": 0.0,
|
||||
"calibration_approved": false,
|
||||
"scope": "Frozen recorded steering, model, driver and PSCM inputs. Command checks only; no predicted wheel response or physical tracking improvement score.",
|
||||
"controller_sha256": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
|
||||
"routes": {
|
||||
"112": {
|
||||
"route": "84865544361f55cb_00000112--ec2edd4afc",
|
||||
"fingerprint": "FORD_F_150_LIGHTNING_MK1",
|
||||
"cycles": 108971,
|
||||
"candidate_updates": 1307652,
|
||||
"sweep_can_round_trips": 1307652,
|
||||
"selected_can_round_trips": 108971,
|
||||
"selected_matches_sweep_commands_p_i_valid_exactly": true,
|
||||
"zero_gain_zero_delay_matches_v5_exactly": true,
|
||||
"all_c0_and_activation_match_exactly": true,
|
||||
"ordinary_clean_seconds": 655.2228206790003,
|
||||
"ordinary_mean_abs_c1_change_rad": 0.0008885702173989647,
|
||||
"ordinary_p95_abs_c1_change_rad": 0.0025000000000000022,
|
||||
"ordinary_max_abs_c1_change_rad": 0.02400000000000002,
|
||||
"all_valid_max_abs_c1_change_rad": 0.054500000000000104,
|
||||
"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": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/angles.npz": "9429813237b4988dd99a1461d1a9bd6f6d3056693a93a5dd1ec63567d19f2aa1"
|
||||
},
|
||||
"sweep_report_sha256": "413e844d20d7591d32d95b1d48a14db11fa089800315cb899e9f5a32a3e630f5",
|
||||
"selected_report_sha256": "ab4ced7ec6b628ac4a941fb968e6e18d06f9c616f8704f1a6abd0081a4fb0e65"
|
||||
},
|
||||
"113": {
|
||||
"route": "84865544361f55cb_00000113--3947b0487c",
|
||||
"fingerprint": "FORD_F_150_LIGHTNING_MK1",
|
||||
"cycles": 49614,
|
||||
"candidate_updates": 595368,
|
||||
"sweep_can_round_trips": 595368,
|
||||
"selected_can_round_trips": 49614,
|
||||
"selected_matches_sweep_commands_p_i_valid_exactly": true,
|
||||
"zero_gain_zero_delay_matches_v5_exactly": true,
|
||||
"all_c0_and_activation_match_exactly": true,
|
||||
"ordinary_clean_seconds": 187.8263240149995,
|
||||
"ordinary_mean_abs_c1_change_rad": 0.0015004664599222238,
|
||||
"ordinary_p95_abs_c1_change_rad": 0.007000000000000006,
|
||||
"ordinary_max_abs_c1_change_rad": 0.025000000000000022,
|
||||
"all_valid_max_abs_c1_change_rad": 0.08250000000000002,
|
||||
"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": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/angles.npz": "2f280a1b5973901d878f1e36564d6e7ae6d817cac176149f2cb898ab7591d65b"
|
||||
},
|
||||
"sweep_report_sha256": "a712fdbe0602404ade075b0ccb540d5f571e5a55d37723fc2674c79b5f62252f",
|
||||
"selected_report_sha256": "0098ef5b1dd0b8b1f0b742cfb5f8e80211eb60718a08e471927c78b9a7c740b3"
|
||||
},
|
||||
"114": {
|
||||
"route": "84865544361f55cb_00000114--03902c6e04",
|
||||
"fingerprint": "FORD_F_150_LIGHTNING_MK1",
|
||||
"cycles": 61027,
|
||||
"candidate_updates": 732324,
|
||||
"sweep_can_round_trips": 732324,
|
||||
"selected_can_round_trips": 61027,
|
||||
"selected_matches_sweep_commands_p_i_valid_exactly": true,
|
||||
"zero_gain_zero_delay_matches_v5_exactly": true,
|
||||
"all_c0_and_activation_match_exactly": true,
|
||||
"ordinary_clean_seconds": 263.004525574,
|
||||
"ordinary_mean_abs_c1_change_rad": 0.0010481495632417845,
|
||||
"ordinary_p95_abs_c1_change_rad": 0.003500000000000003,
|
||||
"ordinary_max_abs_c1_change_rad": 0.010999999999999954,
|
||||
"all_valid_max_abs_c1_change_rad": 0.0655,
|
||||
"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": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/angles.npz": "63aa2ff6a21e8613519bda476690e2eda6f42cb5dcf108606bc6072c9e29408d"
|
||||
},
|
||||
"sweep_report_sha256": "138c522c4278d6827eb6192acc2052ba4832f652c3194e2ca330d3a4458a2f85",
|
||||
"selected_report_sha256": "52cdc595248505301501d815d7f6219c47e8cdcac2a059dc345498b3ef707875"
|
||||
},
|
||||
"115": {
|
||||
"route": "84865544361f55cb_00000115--899b9bf91d",
|
||||
"fingerprint": "FORD_F_150_LIGHTNING_MK1",
|
||||
"cycles": 40037,
|
||||
"candidate_updates": 480444,
|
||||
"sweep_can_round_trips": 480444,
|
||||
"selected_can_round_trips": 40037,
|
||||
"selected_matches_sweep_commands_p_i_valid_exactly": true,
|
||||
"zero_gain_zero_delay_matches_v5_exactly": true,
|
||||
"all_c0_and_activation_match_exactly": true,
|
||||
"ordinary_clean_seconds": 165.0304544679998,
|
||||
"ordinary_mean_abs_c1_change_rad": 0.0009869321832509364,
|
||||
"ordinary_p95_abs_c1_change_rad": 0.0030000000000000027,
|
||||
"ordinary_max_abs_c1_change_rad": 0.034999999999999976,
|
||||
"all_valid_max_abs_c1_change_rad": 0.09900000000000003,
|
||||
"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": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/angles.npz": "71415b0f49efbfeda495ae52bde4beb7e16f31a1f5570af515a365e756056d3b"
|
||||
},
|
||||
"sweep_report_sha256": "cdb08fede1f86427174c1291e40f63a3ac18044dfb6276e98e0bc184bbff7dc5",
|
||||
"selected_report_sha256": "92d3e720d12018599a5883ecfb3c2dd1330e22235e07b3cd0dcc9bbc0aa7c3dd"
|
||||
},
|
||||
"b9": {
|
||||
"route": "84865544361f55cb_000000b9--5da7fe66ad",
|
||||
"fingerprint": "FORD_F_150_LIGHTNING_MK1",
|
||||
"cycles": 90774,
|
||||
"candidate_updates": 1089288,
|
||||
"sweep_can_round_trips": 1089288,
|
||||
"selected_can_round_trips": 90774,
|
||||
"selected_matches_sweep_commands_p_i_valid_exactly": true,
|
||||
"zero_gain_zero_delay_matches_v5_exactly": true,
|
||||
"all_c0_and_activation_match_exactly": true,
|
||||
"ordinary_clean_seconds": 577.3471252719997,
|
||||
"ordinary_mean_abs_c1_change_rad": 0.001220959786299961,
|
||||
"ordinary_p95_abs_c1_change_rad": 0.0050000000000000044,
|
||||
"ordinary_max_abs_c1_change_rad": 0.02350000000000002,
|
||||
"all_valid_max_abs_c1_change_rad": 0.10250000000000004,
|
||||
"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": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/angles.npz": "ce16212d9b13e41993d2c8df3246ddfe21324ad0cb213310e62eee1d685b197b"
|
||||
},
|
||||
"sweep_report_sha256": "13c9b2c4c0f89ec580e9c7a70c7556b4e40873d4d98561c9edfa1c12a0855669",
|
||||
"selected_report_sha256": "733c3eb657cbae29fcb809ca42b85ed6fc03ab44665c5fe31020e266717ce53c"
|
||||
},
|
||||
"ca": {
|
||||
"route": "84865544361f55cb_000000ca--1f70b49ec6",
|
||||
"fingerprint": "FORD_F_150_LIGHTNING_MK1",
|
||||
"cycles": 327448,
|
||||
"candidate_updates": 3929376,
|
||||
"sweep_can_round_trips": 3929376,
|
||||
"selected_can_round_trips": 327448,
|
||||
"selected_matches_sweep_commands_p_i_valid_exactly": true,
|
||||
"zero_gain_zero_delay_matches_v5_exactly": true,
|
||||
"all_c0_and_activation_match_exactly": true,
|
||||
"ordinary_clean_seconds": 892.0115341569954,
|
||||
"ordinary_mean_abs_c1_change_rad": 0.0008244438245156879,
|
||||
"ordinary_p95_abs_c1_change_rad": 0.0025000000000000022,
|
||||
"ordinary_max_abs_c1_change_rad": 0.01200000000000001,
|
||||
"all_valid_max_abs_c1_change_rad": 0.05149999999999999,
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/angles.npz": "1681bda6db7dac332abf627d97e7452acd7e0faef0c196b11e55e22291cad073"
|
||||
},
|
||||
"sweep_report_sha256": "4ced4be50569d4025c9de7fb8085714deebfb6098238ab63e5a2e4c88460a94b",
|
||||
"selected_report_sha256": "db0edb05105600f211e7ff833a324887f4402dc64ddceb44ee8ddec911de7533"
|
||||
}
|
||||
},
|
||||
"examples": [
|
||||
{
|
||||
"description": "Late left entry",
|
||||
"route": "115",
|
||||
"time_s": 207.908066963,
|
||||
"desired_angle_deg": 94.1796646118164,
|
||||
"actual_angle_deg": 51.20000076293945,
|
||||
"speed_mph": 12.099885821086458,
|
||||
"strict_clean": true,
|
||||
"c0_m_left_positive": 0.96,
|
||||
"c1_rad_left_positive": {
|
||||
"v5": 0.16849999999999998,
|
||||
"p_025": 0.1825,
|
||||
"p_025_delay_04": 0.14200000000000002
|
||||
},
|
||||
"p_rad_left_positive": 0.018101743422448635,
|
||||
"i_rad_left_positive": 0.015372994845796784
|
||||
},
|
||||
{
|
||||
"description": "Entry continues",
|
||||
"route": "115",
|
||||
"time_s": 208.09921410900006,
|
||||
"desired_angle_deg": 111.01296997070312,
|
||||
"actual_angle_deg": 72.80000305175781,
|
||||
"speed_mph": 12.297558204224886,
|
||||
"strict_clean": true,
|
||||
"c0_m_left_positive": 1.13,
|
||||
"c1_rad_left_positive": {
|
||||
"v5": 0.21050000000000002,
|
||||
"p_025": 0.22450000000000003,
|
||||
"p_025_delay_04": 0.18100000000000005
|
||||
},
|
||||
"p_rad_left_positive": 0.016087211202830076,
|
||||
"i_rad_left_positive": 0.02176835685651445
|
||||
},
|
||||
{
|
||||
"description": "Completed right-turn unwind",
|
||||
"route": "115",
|
||||
"time_s": 133.59461515599992,
|
||||
"desired_angle_deg": 5.989600658416748,
|
||||
"actual_angle_deg": 29.899999618530273,
|
||||
"speed_mph": 5.890273288393662,
|
||||
"strict_clean": false,
|
||||
"c0_m_left_positive": 0.1900000000000004,
|
||||
"c1_rad_left_positive": {
|
||||
"v5": 0.010500000000000065,
|
||||
"p_025": 0.0,
|
||||
"p_025_delay_04": -0.0030000000000000027
|
||||
},
|
||||
"p_rad_left_positive": -0.010158478980883956,
|
||||
"i_rad_left_positive": -0.0015653052344988395
|
||||
},
|
||||
{
|
||||
"description": "Large left overshoot; nearby driver input",
|
||||
"route": "114",
|
||||
"time_s": 168.957705716,
|
||||
"desired_angle_deg": 278.3920593261719,
|
||||
"actual_angle_deg": 450.20001220703125,
|
||||
"speed_mph": 7.027778014508665,
|
||||
"strict_clean": true,
|
||||
"c0_m_left_positive": 5.11,
|
||||
"c1_rad_left_positive": {
|
||||
"v5": 0.34099999999999997,
|
||||
"p_025": 0.2875,
|
||||
"p_025_delay_04": 0.3125
|
||||
},
|
||||
"p_rad_left_positive": -0.07201755233108997,
|
||||
"i_rad_left_positive": -0.13527950258838367
|
||||
},
|
||||
{
|
||||
"description": "Well-tracked left bend",
|
||||
"route": "114",
|
||||
"time_s": 473.085523785,
|
||||
"desired_angle_deg": 57.32655334472656,
|
||||
"actual_angle_deg": 56.29999923706055,
|
||||
"speed_mph": 27.33261651794824,
|
||||
"strict_clean": true,
|
||||
"c0_m_left_positive": 0.6699999999999999,
|
||||
"c1_rad_left_positive": {
|
||||
"v5": 0.1855,
|
||||
"p_025": 0.18500000000000005,
|
||||
"p_025_delay_04": 0.15900000000000003
|
||||
},
|
||||
"p_rad_left_positive": 0.0007143015310955292,
|
||||
"i_rad_left_positive": 0.022011912629614844
|
||||
},
|
||||
{
|
||||
"description": "Hanging right exit",
|
||||
"route": "113",
|
||||
"time_s": 481.56693996600006,
|
||||
"desired_angle_deg": -7.566320896148682,
|
||||
"actual_angle_deg": -94.4000015258789,
|
||||
"speed_mph": 11.959057581279636,
|
||||
"strict_clean": true,
|
||||
"c0_m_left_positive": -0.5800000000000001,
|
||||
"c1_rad_left_positive": {
|
||||
"v5": 0.0645,
|
||||
"p_025": 0.08750000000000002,
|
||||
"p_025_delay_04": 0.034499999999999975
|
||||
},
|
||||
"p_rad_left_positive": 0.03597008844371885,
|
||||
"i_rad_left_positive": 0.06765882642510383
|
||||
}
|
||||
],
|
||||
"ordinary_cohort": "Existing interval-clean angle mask, valid replay and feedback enabled, absolute desired wheel angle <30 degrees.",
|
||||
"weighting": "Extracted interval duration weights for seconds and mean absolute command changes; percentiles are cycle-weighted.",
|
||||
"ordinary_clean_seconds": 2740.4427841649945,
|
||||
"ordinary_mean_abs_c1_change_by_setting_rad": [
|
||||
{
|
||||
"kp": 0.0,
|
||||
"delay_s": 0.0,
|
||||
"mean_abs_delta_rad": 0.0
|
||||
},
|
||||
{
|
||||
"kp": 0.1,
|
||||
"delay_s": 0.0,
|
||||
"mean_abs_delta_rad": 0.0003946011107110312
|
||||
},
|
||||
{
|
||||
"kp": 0.25,
|
||||
"delay_s": 0.0,
|
||||
"mean_abs_delta_rad": 0.0010009008647461168
|
||||
},
|
||||
{
|
||||
"kp": 0.5,
|
||||
"delay_s": 0.0,
|
||||
"mean_abs_delta_rad": 0.001961192031401653
|
||||
},
|
||||
{
|
||||
"kp": 0.0,
|
||||
"delay_s": 0.2,
|
||||
"mean_abs_delta_rad": 0.006131012841938232
|
||||
},
|
||||
{
|
||||
"kp": 0.1,
|
||||
"delay_s": 0.2,
|
||||
"mean_abs_delta_rad": 0.006179941022213607
|
||||
},
|
||||
{
|
||||
"kp": 0.25,
|
||||
"delay_s": 0.2,
|
||||
"mean_abs_delta_rad": 0.006305419932002006
|
||||
},
|
||||
{
|
||||
"kp": 0.5,
|
||||
"delay_s": 0.2,
|
||||
"mean_abs_delta_rad": 0.006643212063186747
|
||||
},
|
||||
{
|
||||
"kp": 0.0,
|
||||
"delay_s": 0.4,
|
||||
"mean_abs_delta_rad": 0.010964264260175235
|
||||
},
|
||||
{
|
||||
"kp": 0.1,
|
||||
"delay_s": 0.4,
|
||||
"mean_abs_delta_rad": 0.011070256870936306
|
||||
},
|
||||
{
|
||||
"kp": 0.25,
|
||||
"delay_s": 0.4,
|
||||
"mean_abs_delta_rad": 0.011255438766715855
|
||||
},
|
||||
{
|
||||
"kp": 0.5,
|
||||
"delay_s": 0.4,
|
||||
"mean_abs_delta_rad": 0.01162257561022028
|
||||
}
|
||||
],
|
||||
"totals": {
|
||||
"cycles": 677871,
|
||||
"candidate_updates": 8134452,
|
||||
"sweep_can_round_trips": 8134452,
|
||||
"selected_can_round_trips": 677871,
|
||||
"can_round_trips_including_stress": 9012323
|
||||
},
|
||||
"stress": {
|
||||
"cycles": 200000,
|
||||
"gain": 0.25,
|
||||
"seed": 20260913,
|
||||
"mirrored_updates": 200000,
|
||||
"zero_gain_exact_v5_comparisons": 200000,
|
||||
"can_round_trips": 200000,
|
||||
"baseline_revision": "22d188776cb557acea459a1fca70812bdb2df46c",
|
||||
"baseline_source_sha256": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e",
|
||||
"release_cycles": 91,
|
||||
"calibration_approved": false,
|
||||
"checks": "Independent scalar PI arithmetic, combined anti-windup, mirror symmetry, slew/amplitude, driver/PSCM gates, resets, zero-P v5 parity and CAN.",
|
||||
"scope": "Check PI arithmetic and CAN invariants without a model of vehicle response.",
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/pi_stress.py": "07f3bd99c56e2184ba3402d7b2f324506ac0b786c7a15e422e9e2392c934544a",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f"
|
||||
},
|
||||
"formatting_only_final_source_ast_identical": true,
|
||||
"final_script_sha256": "cff014beda81b0bfbaabd7219b8e2848464c5c45ac779669f80084d5be347a06"
|
||||
},
|
||||
"tests": {
|
||||
"passed": 776,
|
||||
"skipped": 178,
|
||||
"subtests_passed": 9145,
|
||||
"log_sha256": "5b9274e2557f41b630ad285b0426c916835c2997fc7f5c4fcad91aa4c6f64243",
|
||||
"ruff": "passed",
|
||||
"controller_ty": "passed",
|
||||
"settings_compiler": "passed"
|
||||
}
|
||||
}
|
||||
@@ -4,6 +4,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)
|
||||
and [completed-unwind correction release](ford_unwind_catchup.md).
|
||||
V6 adds [explicit proportional C1 feedback with P=0.25](ford_c1_pi.md), retaining
|
||||
the existing feedback timing. This is an initial drive-trial gain.
|
||||
It is selectable on **any Ford CAN FD vehicle**
|
||||
through the existing persistent, default-off Sunnylink
|
||||
toggle. Offline checks establish software behavior; physical tracking,
|
||||
@@ -21,8 +23,8 @@ 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-v5`**. They report desired and
|
||||
measured curvature, base heading, accumulated correction, applied heading,
|
||||
identify **`hypothesis=model-action-c1-pi-v6`**. They report desired and
|
||||
measured curvature, base heading, proportional and accumulated correction, applied heading,
|
||||
feedback timing and driver/PSCM gating. `carryover_release_count` counts
|
||||
conditional releases since the last controller reset; it does not control
|
||||
steering. `offset_overflow` reports the extra C0 target in meters before C0
|
||||
@@ -32,6 +34,8 @@ 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.
|
||||
`heading_proportional`, `proportional_gain`, `feedback_curvature` and
|
||||
`feedback_error` separate the new P contribution and its reference from I.
|
||||
|
||||
Turning the toggle off and completing another offroad-to-onroad cycle restores
|
||||
**upstream Ford curvature control**: 20 Hz steering messages, limited mode on
|
||||
@@ -45,9 +49,10 @@ See [toggle-off validation](ford_upstream_fallback.md).
|
||||
|
||||
`controlsd` supplies the selected, upstream-limited desired curvature and the
|
||||
measured steering-derived curvature already used in its tracking diagnostics.
|
||||
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
|
||||
Fresh steering publications advance C1 integration. P responds to the current
|
||||
error without accumulating. Repeated publications may advance P and output slew
|
||||
but cannot integrate the same elapsed interval twice.
|
||||
Driver override clears P and I. A fresh PSCM reached-limit flag stops
|
||||
extra outward accumulation while preserving unwind and base model changes.
|
||||
With fresh feedback, the controller can discard an opposing correction when
|
||||
it prevents C1 from following the direction shared by original model C0, applied C0
|
||||
@@ -69,9 +74,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 [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
|
||||
See [proportional feedback trial and validation](ford_c1_pi.md) and
|
||||
`ford_c1_pi_validation.json` for current evidence and reproduction commands.
|
||||
[Completed-unwind release](ford_unwind_catchup.md) and
|
||||
`ford_unwind_catchup_validation.json` record v5. [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
|
||||
|
||||
@@ -1,8 +1,9 @@
|
||||
"""Experimental Ford C2-free controller with measured-curvature C1 feedback.
|
||||
"""Experimental Ford C2-free controller with measured-curvature C1 PI feedback.
|
||||
|
||||
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.
|
||||
The selected experiment adds an explicit proportional heading-error term.
|
||||
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.
|
||||
@@ -19,6 +20,7 @@ from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
|
||||
|
||||
OFFSET_STATION_M = 7.0
|
||||
HEADING_TIME_S = 1.0
|
||||
C1_PROPORTIONAL_GAIN = 0.25 # Initial drive-trial gain, not a learned calibration.
|
||||
CALIBRATION_APPROVED = False
|
||||
|
||||
|
||||
@@ -60,11 +62,15 @@ class ModelActionController:
|
||||
|
||||
Feedback integrates requested minus measured curvature over traveled distance.
|
||||
Freshness, measurement cadence and driver/PSCM arbitration belong to the caller.
|
||||
Zero P is the v5 reference; onroad selection supplies the explicit trial gain.
|
||||
"""
|
||||
__slots__ = ('c0', 'c1', 'correction', 'carryover_release_count', 'last_feedback_desired', 'request_release',
|
||||
'unwind_direction', 'unwind_release')
|
||||
'unwind_direction', 'unwind_release', 'proportional_gain', 'proportional', 'feedback_curvature')
|
||||
|
||||
def __init__(self):
|
||||
def __init__(self, proportional_gain=0.):
|
||||
if not _finite(proportional_gain) or proportional_gain < 0.:
|
||||
raise ValueError('Proportional gain must be finite and nonnegative')
|
||||
self.proportional_gain = float(proportional_gain)
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
@@ -74,18 +80,26 @@ class ModelActionController:
|
||||
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.
|
||||
self.proportional = self.feedback_curvature = 0.
|
||||
|
||||
def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True,
|
||||
feedback_dt=None, feedback_enabled=True, pscm_limited=False):
|
||||
feedback_dt=None, feedback_enabled=True, pscm_limited=False, feedback_curvature=None):
|
||||
feedback_dt = dt if feedback_dt is None else feedback_dt
|
||||
if (not active or not valid or not _finite(dt, feedback_dt, current_curvature) or not .002 <= dt <= .1
|
||||
or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1.):
|
||||
feedback_curvature = desired_curvature if feedback_curvature is None else feedback_curvature
|
||||
if (not active or not valid or not _finite(dt, feedback_dt, current_curvature, feedback_curvature) or not .002 <= dt <= .1
|
||||
or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(feedback_curvature) > 1.):
|
||||
self.reset()
|
||||
return FordPath()
|
||||
target = encode_model_action(model, desired_curvature, speed)
|
||||
if not target.valid:
|
||||
self.reset()
|
||||
return FordPath()
|
||||
self.feedback_curvature = feedback_curvature
|
||||
feedback_error = feedback_curvature-current_curvature
|
||||
self.proportional = self.proportional_gain*max(OFFSET_STATION_M, speed*HEADING_TIME_S)*feedback_error if feedback_enabled else 0.
|
||||
if not _finite(self.proportional):
|
||||
self.reset()
|
||||
return FordPath()
|
||||
base_c1 = float(np.clip(target.path_angle, -.5, .5))
|
||||
# Preserve the linear path reference at 7 m when the base heading clips.
|
||||
# This is instantaneous geometry, not stored error or C1 feedback spill.
|
||||
@@ -145,7 +159,7 @@ class ModelActionController:
|
||||
and (base_c1+self.correction)*direction <= 0.):
|
||||
self.correction = 0.
|
||||
self.carryover_release_count += 1
|
||||
increment = (desired_curvature-current_curvature)*speed*feedback_dt
|
||||
increment = feedback_error*speed*feedback_dt
|
||||
# LimitReached inhibits only extra demand in the measured turn direction.
|
||||
# Opposing correction and changes to the model request remain available.
|
||||
direction = current_curvature if current_curvature else self.c1
|
||||
@@ -153,16 +167,17 @@ class ModelActionController:
|
||||
# An old opposing correction may return to zero; don't trap it below
|
||||
# the base request just because the PSCM now reports a limit.
|
||||
increment = float(np.clip(increment, min(-self.correction, 0.), max(-self.correction, 0.)))
|
||||
# Include P in the available headroom so I cannot wind up behind it.
|
||||
# Integrate only as far as this cycle's amplitude/slew envelope permits.
|
||||
# If the base moved outside that envelope, allow increments toward it;
|
||||
# never rewrite existing correction merely because the base changed.
|
||||
request = base_c1+self.correction
|
||||
request = base_c1+self.proportional+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))
|
||||
c1 = float(np.clip(base_c1+self.proportional+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.)
|
||||
|
||||
@@ -179,18 +194,20 @@ class FordModelActionController:
|
||||
clears the correction. Fresh PSCM limits only inhibit outward integration;
|
||||
neither a limit nor a repeated measurement freezes the model request.
|
||||
"""
|
||||
def __init__(self):
|
||||
self.core = ModelActionController()
|
||||
def __init__(self, proportional_gain=0.):
|
||||
self.core = ModelActionController(proportional_gain=proportional_gain)
|
||||
self.hypothesis = 'model-action-c1-pi-v6' if proportional_gain else 'model-action-c1-feedback-v5'
|
||||
self.reset()
|
||||
|
||||
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-v5',
|
||||
self.diagnostics = {'status': status, 'hypothesis': self.hypothesis,
|
||||
'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,
|
||||
reference_time, active, valid=True, driver_pressed=False, driver_torque=0., pscm_status=None):
|
||||
reference_time, active, valid=True, driver_pressed=False, driver_torque=0., pscm_status=None,
|
||||
feedback_curvature=None):
|
||||
reason = None
|
||||
if not active:
|
||||
reason = 'inactive'
|
||||
@@ -221,14 +238,15 @@ class FordModelActionController:
|
||||
or (status_fresh and pscm_status.limit == 3))
|
||||
feedback_enabled = not (driver_override or (status_fresh and (pscm_status.denied or pscm_status.lateralState != 2)))
|
||||
command = self.core.update(model, desired_curvature, current_curvature=current_curvature, speed=speed, dt=dt,
|
||||
feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited)
|
||||
feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited,
|
||||
feedback_curvature=feedback_curvature)
|
||||
if not command.valid:
|
||||
self.reset('invalid_path')
|
||||
return command
|
||||
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-v5',
|
||||
self.diagnostics = {'status': 'active', 'hypothesis': self.hypothesis,
|
||||
'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,
|
||||
@@ -236,6 +254,9 @@ class FordModelActionController:
|
||||
'heading_feedforward': base_heading,
|
||||
'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading),
|
||||
'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled,
|
||||
'heading_proportional': self.core.proportional, 'proportional_gain': self.core.proportional_gain,
|
||||
'feedback_curvature': self.core.feedback_curvature,
|
||||
'feedback_error': self.core.feedback_curvature-current_curvature,
|
||||
'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,
|
||||
@@ -248,5 +269,5 @@ def select_model_action_controller(CP, enabled):
|
||||
"""Only opt-in Ford CAN FD vehicles override upstream curvature control."""
|
||||
compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD
|
||||
if enabled and compatible:
|
||||
return FordModelActionController()
|
||||
return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN)
|
||||
return None
|
||||
|
||||
@@ -180,7 +180,9 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
|
||||
exec(call, environment)
|
||||
expected_curvature = (-1 if maneuver else 1)*.000125
|
||||
assert controls.desired_curvature == pytest.approx(expected_curvature)
|
||||
assert controls.ford_path.path_angle == pytest.approx(20.*expected_curvature)
|
||||
assert controller.core.proportional == pytest.approx(.25*20.*expected_curvature)
|
||||
assert controller.core.correction == 0. # First measurement has no elapsed feedback time.
|
||||
assert controls.ford_path.path_angle == pytest.approx((-1 if maneuver else 1)*.003)
|
||||
assert controls.ford_path.path_offset == pytest.approx(.04)
|
||||
assert cc.latActive and cc.actuators.curvature == 0.
|
||||
assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
|
||||
@@ -237,9 +239,11 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
|
||||
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main)
|
||||
frame = 0
|
||||
for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.02),
|
||||
(sign*.004, 0., 100, sign*.02), (sign*.005, 0., 100, 0.),
|
||||
(sign*.003, 0., 100, sign*.02), (0., 1.0625, 5, 0.)]:
|
||||
# A .005 rad P step consumes one slew interval before I can accumulate;
|
||||
# going from -.005 to +.005 consumes two. Matched steering removes P only.
|
||||
for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.0198),
|
||||
(sign*.004, 0., 100, sign*.0198), (sign*.005, 0., 100, 0.),
|
||||
(sign*.003, 0., 100, sign*.0196), (0., 1.0625, 5, 0.)]:
|
||||
for _ in range(count):
|
||||
now = 1.+frame*.01
|
||||
controls.curvature, cs.steeringTorque = measured, torque
|
||||
@@ -263,8 +267,12 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
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])
|
||||
frame += 1
|
||||
assert controls.ford_path_controller.core.correction == pytest.approx(expected)
|
||||
assert controls.ford_path.path_angle == pytest.approx(sign*.08+expected)
|
||||
core = controls.ford_path_controller.core
|
||||
expected_p = .25*20.*(sign*.004-measured) if torque == 0. else 0.
|
||||
assert core.proportional == pytest.approx(expected_p)
|
||||
assert core.correction == pytest.approx(expected)
|
||||
assert core.c1 == pytest.approx(sign*.08+expected_p+expected)
|
||||
assert controls.ford_path.path_angle == pytest.approx(core.c1, abs=.00025)
|
||||
assert controls.ford_path.path_offset == pytest.approx(.4)
|
||||
|
||||
|
||||
@@ -276,7 +284,7 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s
|
||||
model.action = SimpleNamespace(desiredCurvature=.004)
|
||||
cc = structs.CarControl(latActive=True)
|
||||
cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
|
||||
for frame in range(101):
|
||||
for frame in range(102):
|
||||
now = 1.+frame*.01
|
||||
controls.curvature = .004 if frame < 100 else .003
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
|
||||
@@ -288,6 +296,8 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
controller = controls.ford_path_controller
|
||||
assert controller.diagnostics['pscm_limited'] is service_valid
|
||||
assert controller.core.proportional == pytest.approx(.005)
|
||||
# First error sample spends the slew allowance on P; the second may add I.
|
||||
assert controller.core.correction == pytest.approx(0. if service_valid else .0002)
|
||||
assert cc.latActive and controls.ford_path.valid
|
||||
|
||||
@@ -337,11 +347,12 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel
|
||||
packet = next(packet for packet in packets if packet[0] == address)
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
if frame == 199:
|
||||
assert core.correction == pytest.approx(sign*(-speed*.002 if same_turn else .06))
|
||||
# P spends one or three slew intervals before the remaining I updates.
|
||||
assert core.correction == pytest.approx(sign*(-.0198 if same_turn else .0582))
|
||||
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-v5'
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v6'
|
||||
if same_turn:
|
||||
assert request_releases > 0
|
||||
assert controls.desired_curvature == pytest.approx(sign*.01)
|
||||
|
||||
@@ -0,0 +1,102 @@
|
||||
"""Explicit PI experiment semantics; no simulated PSCM response."""
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
|
||||
|
||||
|
||||
@pytest.mark.parametrize('gain', [.1, .25, .5])
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_p_responds_without_waiting_for_integral_and_disappears_at_catchup(gain, sign):
|
||||
controller = ModelActionController(proportional_gain=gain)
|
||||
controller.c1 = sign*.2
|
||||
out = controller.update(straight(), sign*.01, current_curvature=sign*.009,
|
||||
speed=20., dt=.1, feedback_dt=0.)
|
||||
assert controller.proportional == pytest.approx(sign*gain*.02)
|
||||
assert controller.correction == 0.
|
||||
assert out.path_angle == pytest.approx(sign*(.2+gain*.02))
|
||||
out = controller.update(straight(), sign*.01, current_curvature=sign*.01, speed=20., dt=.1)
|
||||
assert controller.proportional == controller.correction == 0.
|
||||
assert out.path_angle == pytest.approx(sign*.2)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_aligned_feedback_does_not_replace_current_feedforward_or_path(sign):
|
||||
controller = ModelActionController(proportional_gain=.25)
|
||||
controller.c0, controller.c1 = sign*.4, sign*.2
|
||||
out = controller.update(straight(sign*.4), sign*.01, current_curvature=sign*.008,
|
||||
feedback_curvature=sign*.008, speed=20., dt=.1)
|
||||
assert controller.proportional == controller.correction == 0.
|
||||
assert out.path_offset == pytest.approx(sign*.4)
|
||||
assert out.path_angle == pytest.approx(sign*.2)
|
||||
# Latest request is ahead of measured steering, but the delay-aligned target
|
||||
# has already been exceeded. P and I must use the explicit feedback target.
|
||||
out = controller.update(straight(sign*.4), sign*.01, current_curvature=sign*.008,
|
||||
feedback_curvature=sign*.006, speed=20., dt=.1)
|
||||
assert controller.proportional == pytest.approx(-sign*.01)
|
||||
assert sign*controller.correction < 0.
|
||||
assert sign*out.path_angle < .2
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_pi_combined_request_obeys_slew_and_does_not_wind_up_behind_p(sign):
|
||||
controller = ModelActionController(proportional_gain=.5)
|
||||
for _ in range(200):
|
||||
before = controller.c1
|
||||
out = controller.update(straight(), sign*.01, current_curvature=-sign*.1, speed=20., dt=.01)
|
||||
assert abs(controller.c1-before) <= .0050000001
|
||||
assert abs(out.path_angle) <= .50000001
|
||||
assert controller.correction == 0. # Feedforward + P alone exceeds the cap.
|
||||
assert out.path_angle == pytest.approx(sign*.5)
|
||||
for _ in range(60):
|
||||
out = controller.update(straight(), sign*.01, current_curvature=sign*.01, speed=20., dt=.01)
|
||||
assert out.path_angle == pytest.approx(sign*.2)
|
||||
assert controller.correction == controller.proportional == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('gain', [-.1, math.nan, math.inf, None, 'bad'])
|
||||
def test_invalid_gain_is_rejected(gain):
|
||||
with pytest.raises(ValueError):
|
||||
ModelActionController(proportional_gain=gain)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('feedback', [math.nan, math.inf, 'bad', 1.001])
|
||||
def test_invalid_feedback_target_clears_all_output(feedback):
|
||||
controller = ModelActionController(proportional_gain=.25)
|
||||
controller.c1, controller.correction = .2, .01
|
||||
out = controller.update(straight(), .01, current_curvature=.008, feedback_curvature=feedback, speed=20., dt=.01)
|
||||
assert out == FordPath()
|
||||
assert controller.proportional == controller.correction == controller.c1 == 0.
|
||||
|
||||
|
||||
def test_overflowing_p_cannot_escape_as_an_active_command():
|
||||
controller = ModelActionController(proportional_gain=1e308)
|
||||
out = controller.update(straight(), 1., current_curvature=-1., speed=55., dt=.01)
|
||||
assert out == FordPath()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('limited', [False, True])
|
||||
def test_driver_override_clears_both_p_and_i(limited):
|
||||
controller = ModelActionController(proportional_gain=.25)
|
||||
controller.c1, controller.correction = .2, .01
|
||||
controller.update(straight(), .01, current_curvature=.008, speed=20., dt=.01,
|
||||
feedback_enabled=False, pscm_limited=limited)
|
||||
assert controller.proportional == controller.correction == 0.
|
||||
|
||||
|
||||
def test_adapter_logs_separate_feedforward_p_i_and_explicit_feedback_target():
|
||||
controller = FordModelActionController(proportional_gain=.25)
|
||||
for i in range(30):
|
||||
now = 1.+i*.01
|
||||
controller.update(straight(), .004, current_curvature=.002, feedback_curvature=.003,
|
||||
speed=20., yaw_rate=0., now=now, measurement_time=now, model_time=now,
|
||||
reference_time=now, active=True)
|
||||
d = controller.diagnostics
|
||||
assert d['heading_feedforward'] == pytest.approx(.08)
|
||||
assert d['heading_proportional'] == pytest.approx(.005)
|
||||
assert d['proportional_gain'] == .25 and d['feedback_curvature'] == .003
|
||||
assert d['heading_correction'] > 0.
|
||||
assert d['heading_request'] == pytest.approx(d['heading_feedforward']+d['heading_proportional']+d['heading_correction'])
|
||||
@@ -9,7 +9,7 @@ import pytest
|
||||
|
||||
from opendbc.car.ford.values import CAR, FordFlags
|
||||
from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import C1_PROPORTIONAL_GAIN, FordModelActionController, select_model_action_controller
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
|
||||
|
||||
@@ -46,6 +46,8 @@ def test_actual_startup_priority(candidate, observer, fingerprint):
|
||||
selected = startup(car_params(carFingerprint=fingerprint), params=SimpleNamespace(get_bool=settings.__getitem__))
|
||||
if candidate:
|
||||
assert type(selected.ford_path_controller) is FordModelActionController
|
||||
assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .25
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v6'
|
||||
else:
|
||||
assert selected.ford_path_controller is None
|
||||
assert selected.ford_model_action == candidate
|
||||
|
||||
@@ -16,7 +16,7 @@ from types import ModuleType, SimpleNamespace
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.controls.lib import ford_model_action
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import C1_PROPORTIONAL_GAIN, FordModelActionController, ModelActionController
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency
|
||||
|
||||
|
||||
@@ -57,12 +57,13 @@ def replay(directory, output, baseline_revision=BASELINE):
|
||||
models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths]
|
||||
old, baseline_hash = original_controller(baseline_revision)
|
||||
old_has_feedback = 'current_curvature' in inspect.signature(old.update).parameters
|
||||
controller, wire_check = FordModelActionController(), WireCheck()
|
||||
controller, wire_check = FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN), 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))
|
||||
proportional = np.zeros(len(t))
|
||||
baseline_correction = np.zeros(len(t))
|
||||
feedback_dt = np.zeros(len(t))
|
||||
feedback_enabled = np.zeros(len(t), bool)
|
||||
@@ -96,6 +97,7 @@ def replay(directory, output, baseline_revision=BASELINE):
|
||||
d = controller.diagnostics
|
||||
reasons[d['status']] += 1
|
||||
correction[i] = controller.core.correction
|
||||
proportional[i] = controller.core.proportional
|
||||
baseline_correction[i] = getattr(old.core, 'correction', 0.)
|
||||
feedback_dt[i] = d.get('feedback_dt', 0.)
|
||||
feedback_enabled[i] = d.get('feedback_enabled', False)
|
||||
@@ -122,6 +124,7 @@ def replay(directory, output, baseline_revision=BASELINE):
|
||||
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_revision, 'baseline_source_sha256': baseline_hash,
|
||||
'proportional_gain': C1_PROPORTIONAL_GAIN,
|
||||
'calibration_approved': False, 'cycles': len(t), 'active_cycles': int(valid.sum()),
|
||||
'validity_matches_baseline_exactly': True, 'status_counts': dict(reasons),
|
||||
'c0_matches_baseline_exactly': bool(np.array_equal(commands[:, 0], baseline[:, 0])),
|
||||
@@ -156,7 +159,7 @@ def replay(directory, output, baseline_revision=BASELINE):
|
||||
'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, baseline_correction=baseline_correction, feedback_dt=feedback_dt,
|
||||
correction=correction, proportional=proportional, baseline_correction=baseline_correction, feedback_dt=feedback_dt,
|
||||
feedback_enabled=feedback_enabled, pscm_limited=pscm_limited, offset_overflow=offset_overflow,
|
||||
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')
|
||||
|
||||
@@ -0,0 +1,124 @@
|
||||
"""Compare explicit C1 P gains and feedback delays on fixed route measurements.
|
||||
|
||||
No PSCM plant is fitted. A change in command is not a predicted change in wheel
|
||||
angle. Feedforward uses the current selected request. Only P and elapsed-distance
|
||||
integration use the delayed request; v5's conditional I retirement is preserved.
|
||||
"""
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.controls.lib import ford_model_action
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
|
||||
from tools.ford_pscm_lab.feedback_replay import OPENDBC, original_controller
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency
|
||||
|
||||
|
||||
BASELINE = '22d188776cb557acea459a1fca70812bdb2df46c'
|
||||
GAINS = (0., .1, .25, .5)
|
||||
DELAYS = (0., .2, .4)
|
||||
|
||||
|
||||
def replay(directory, output):
|
||||
directory, output = directory.resolve(), output.resolve()
|
||||
if output == directory or directory in output.parents:
|
||||
raise ValueError('Output must be outside the source route directory')
|
||||
verify_dependency(OPENDBC)
|
||||
metadata = json.loads((directory/'metadata.json').read_text())
|
||||
with np.load(directory/'route.npz', allow_pickle=False) as z:
|
||||
r = {k: table(z, k) for k in ('controls', 'cs', 'cc', 'model', 'params', 'pscm')}
|
||||
with np.load(directory/'model_paths.npz', allow_pickle=False) as z:
|
||||
model_ns, paths = z['ns'], z['paths']
|
||||
c, t = r['controls'], r['controls']['t']
|
||||
assert all(np.all(np.diff(stream['t']) >= 0.) for stream in r.values())
|
||||
cs, pa, ps = (sample(r[k], t) for k in ('cs', 'params', 'pscm'))
|
||||
cc = sample(r['cc'], t, nearest=True)
|
||||
mi = np.clip(np.searchsorted(r['model']['ns'], c['model_ns']), 0, len(r['model']['ns'])-1)
|
||||
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]
|
||||
settings = [(gain, delay) for delay in DELAYS for gain in GAINS]
|
||||
controllers = [FordModelActionController(proportional_gain=gain) for gain, _ in settings]
|
||||
reference, reference_hash = original_controller(BASELINE)
|
||||
delayed = {}
|
||||
for delay in DELAYS:
|
||||
# Causal history lookup at the steering measurement's time. Zero delay is
|
||||
# the original v5 same-cycle request, kept exact for baseline comparison.
|
||||
ix = np.searchsorted(t, cs['t']-delay, side='right')-1
|
||||
delayed[delay] = np.where(ix >= 0, c['desired'][np.maximum(ix, 0)], c['measured']) if delay else c['desired']
|
||||
shape = (len(settings), len(t))
|
||||
commands = np.zeros((*shape, 4))
|
||||
proportional, integral, feedforward, feedback_error = (np.zeros(shape) for _ in range(4))
|
||||
enabled = np.zeros(shape, bool)
|
||||
valid = np.zeros(shape, bool)
|
||||
wire = WireCheck()
|
||||
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]
|
||||
and pa['valid'][i] and r['model']['valid'][mi[i]] and exact[i]
|
||||
and abs(cc['t'][i]-now) < .005 and 0. <= now-pa['t'][i] <= .15)
|
||||
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]))
|
||||
kwargs = {'speed': cs['speed'][i], 'yaw_rate': cs['yaw'][i], 'now': now, 'measurement_time': cs['t'][i],
|
||||
'model_time': model_time, 'reference_time': model_time, 'active': bool(cc['active'][i]), 'valid': service_valid,
|
||||
'current_curvature': c['measured'][i], 'driver_pressed': bool(cs['pressed'][i]),
|
||||
'driver_torque': cs['torque'][i], 'pscm_status': status}
|
||||
old = reference.update(model, c['desired'][i], **kwargs)
|
||||
for k, (controller, (_, delay)) in enumerate(zip(controllers, settings, strict=True)):
|
||||
command = controller.update(model, c['desired'][i], feedback_curvature=delayed[delay][i], **kwargs)
|
||||
if k == 0:
|
||||
assert command == old
|
||||
assert controller.core.correction == reference.core.correction
|
||||
commands[k, i] = command.path_offset, command.path_angle, command.curvature, command.curvature_rate
|
||||
valid[k, i] = command.valid
|
||||
d = controller.diagnostics
|
||||
proportional[k, i] = d.get('heading_proportional', 0.)
|
||||
integral[k, i] = d.get('heading_correction', 0.)
|
||||
feedforward[k, i] = d.get('heading_feedforward', 0.)
|
||||
feedback_error[k, i] = delayed[delay][i]-c['measured'][i]
|
||||
enabled[k, i] = d.get('feedback_enabled', False)
|
||||
wire.check(command)
|
||||
for k in range(len(settings)):
|
||||
field_checks(commands[k], valid[k], t)
|
||||
np.testing.assert_array_equal(valid[k], valid[0])
|
||||
np.testing.assert_array_equal(commands[k, :, 0], commands[0, :, 0])
|
||||
assert np.all(proportional[k, ~enabled[k]] == 0.)
|
||||
assert np.all(integral[k, ~enabled[k]] == 0.)
|
||||
assert np.all(abs(integral[k]) <= 1.+1e-10)
|
||||
report = {'scope': __doc__, 'baseline_revision': BASELINE, 'baseline_source_sha256': reference_hash,
|
||||
'cycles': len(t), 'candidate_updates': len(settings)*len(t), 'can_round_trips': wire.count,
|
||||
'zero_gain_zero_delay_matches_v5_exactly': True, 'all_c0_and_activation_match_exactly': True,
|
||||
'calibration_approved': False, 'settings': [],
|
||||
'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in
|
||||
(directory/'route.npz', directory/'model_paths.npz', directory/'metadata.json',
|
||||
Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())},
|
||||
'limitations': ['Recorded model, steering, driver and PSCM inputs stay fixed; no tracking improvement score.',
|
||||
'0.2/0.4 s are diagnostic timing alternatives, not fitted or validated PSCM delays.',
|
||||
'Gain sweep does not identify a physically optimal or stable gain.',
|
||||
'Publication clock proxies computation time; maneuver references are not reconstructed.']}
|
||||
for k, (gain, delay) in enumerate(settings):
|
||||
report['settings'].append({'index': k, 'kp': gain, 'delay_s': delay,
|
||||
'max_abs_c1_change_rad': float(abs(commands[k, :, 1]-commands[0, :, 1]).max()),
|
||||
'max_abs_p_rad': float(abs(proportional[k]).max()),
|
||||
'max_abs_i_rad': float(abs(integral[k]).max())})
|
||||
output.mkdir(parents=True, exist_ok=True)
|
||||
np.savez_compressed(output/'commands.npz', t=t-metadata['t0'], settings=np.array(settings), commands=commands,
|
||||
valid=valid, proportional=proportional, integral=integral, feedforward=feedforward,
|
||||
feedback_error=feedback_error, feedback_enabled=enabled,
|
||||
desired_curvature=c['desired'], measured_curvature=c['measured'],
|
||||
desired_angle=c['desired_angle'], actual_angle=c['actual_angle'], speed=cs['speed'])
|
||||
(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))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('directory', type=Path)
|
||||
parser.add_argument('--output', type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
replay(args.directory, args.output)
|
||||
@@ -0,0 +1,95 @@
|
||||
"""Check PI arithmetic and CAN invariants without a model of vehicle response."""
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.controls.lib import ford_model_action
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
|
||||
from tools.ford_pscm_lab.feedback_replay import OPENDBC, original_controller
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck, verify_dependency
|
||||
from tools.ford_pscm_lab.pi_replay import BASELINE
|
||||
|
||||
|
||||
def stress(cycles, gain, output):
|
||||
verify_dependency(OPENDBC)
|
||||
rng = np.random.default_rng(20260913)
|
||||
controller = ModelActionController(proportional_gain=gain)
|
||||
mirror = ModelActionController(proportional_gain=gain)
|
||||
zero = ModelActionController()
|
||||
original, original_hash = original_controller(BASELINE)
|
||||
wire = WireCheck()
|
||||
release_count = 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
|
||||
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)
|
||||
z = zero.update(model(offset), desired, current_curvature=measured, **args)
|
||||
old = original.core.update(model(offset), desired, current_curvature=measured, **args)
|
||||
assert z == old and zero.correction == original.core.correction
|
||||
state = np.array([controller.c0, controller.c1, controller.correction, controller.proportional])
|
||||
np.testing.assert_allclose(state, -np.array([mirror.c0, mirror.c1, mirror.correction, mirror.proportional]), atol=1e-10, rtol=0.)
|
||||
assert controller.carryover_release_count == mirror.carryover_release_count
|
||||
assert abs(controller.request_release+mirror.request_release) <= 1e-10
|
||||
assert controller.unwind_release == -mirror.unwind_release
|
||||
assert abs(controller.c0) <= 5.11+1e-10 and abs(controller.c1) <= .5+1e-10 and abs(controller.correction) <= 1.+1e-10
|
||||
if active:
|
||||
distance = max(7., speed)
|
||||
base = min(.5, max(-.5, distance*desired))
|
||||
p = gain*distance*(desired-measured) if enabled else 0.
|
||||
assert controller.proportional == p
|
||||
assert abs(controller.c0-previous[0]) <= 4.*dt+1e-10
|
||||
assert abs(controller.c1-previous[1]) <= .5*dt+1e-10
|
||||
if enabled:
|
||||
released = controller.carryover_release_count > count
|
||||
release_count += released or bool(controller.unwind_release)
|
||||
remaining = 0. if released else previous[2]-controller.unwind_release+controller.request_release
|
||||
increment = (desired-measured)*speed*feedback_dt
|
||||
direction = measured if measured else previous[1]
|
||||
if limited and increment*direction > 0.:
|
||||
increment = min(max(increment, min(-remaining, 0.)), max(-remaining, 0.))
|
||||
lower, upper = max(-.5, previous[1]-.5*dt), min(.5, previous[1]+.5*dt)
|
||||
target = base+p+remaining
|
||||
increment = min(max(increment, min(lower-target, 0.)), max(upper-target, 0.))
|
||||
assert abs(controller.correction-(remaining+increment)) <= 1e-10
|
||||
else:
|
||||
assert controller.correction == 0.
|
||||
target = min(.5, max(-.5, base+p+controller.correction))
|
||||
expected = previous[1]+min(.5*dt, max(-.5*dt, target-previous[1]))
|
||||
assert abs(controller.c1-expected) <= 1e-10
|
||||
else:
|
||||
assert np.all(state == 0.)
|
||||
assert out.curvature == out.curvature_rate == other.curvature == other.curvature_rate == 0.
|
||||
wire.check(out)
|
||||
report = {'cycles': cycles, 'gain': gain, 'seed': 20260913, 'mirrored_updates': cycles,
|
||||
'zero_gain_exact_v5_comparisons': cycles, 'can_round_trips': wire.count,
|
||||
'baseline_revision': BASELINE, 'baseline_source_sha256': original_hash,
|
||||
'release_cycles': release_count, 'calibration_approved': False,
|
||||
'checks':
|
||||
'Independent scalar PI arithmetic, combined anti-windup, mirror symmetry, slew/amplitude, driver/PSCM gates, resets, zero-P v5 parity and CAN.',
|
||||
'scope': __doc__, 'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in
|
||||
(Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())}}
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
output.write_text(json.dumps(report, indent=2)+'\n')
|
||||
print(json.dumps(report, indent=2))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--cycles', type=int, default=200_000)
|
||||
parser.add_argument('--gain', type=float, default=.25)
|
||||
parser.add_argument('--output', type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
stress(args.cycles, args.gain, args.output)
|
||||
Reference in New Issue
Block a user