mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-30 14:53:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ford: add proportional C0 tracking correction

Browse Source 
Add stateless C0 feedback using an offline Lightning response fit and the existing vehicle-model curvature conversion. Keep C1 P=0.75/I=1.0, the current feedforward geometry, field bounds, and upstream fallback.

Validate with 439 tests and native-time replay over ten Lightning routes: 1,170,113 cycles, 117,016 CAN round trips, and identical C1 output. Physical improvement remains an on-road trial.
This commit is contained in:
Isaac Barham
2026-09-15 20:04:11 -04:00
parent d425b3260b
commit 1336171a20
10 changed files with 1244 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/ford_c0_feedback_v15.md
+105
View File
@@ -0,0 +1,105 @@
# Ford C0 proportional feedback trial
The existing controller sends its tracking correction through C1 only. Offline
identification on the Lightning suggests that stronger C1 requests can stop
producing faster wheel movement while additional C0 may still help. This trial
adds proportional correction to C0. It does not change the selected curvature,
C0 base geometry, C1 PI calculation, transmission rate or existing opt-in selection.
## Command
With host-sign road curvature error `e = reference - measured`:
```
scale = VM.get_steer_from_curvature(1, speed, 0) / (CP.steerRatio * CP.wheelbase)
C0_response = 0.010717679293424373 + 0.018122981795212647 / max(speed, 1.34)^2
C0_P = 0.5 * e * scale / C0_response
C0 = clip(existing_bounded_C0_base + C0_P, -5.11, 5.11)
```
The response coefficients are an offline fit to isolated C0 commands on route
`84865544361f55cb/00000145--5d9f02fee7`. They express **geometric steering curvature
per metre of C0**, not road curvature or an instantaneous wheel response. The
existing vehicle model converts the error into those units; its road-roll and
steering-angle offsets cancel in the error. No plant identification, observer or
dynamic plant simulation runs on the device.
`0.5` is an explicit trial feedback gain, not a fitted optimum. Before field
clipping, the extra C0 has a fitted steady effect equal to half the current
wheel-angle error. This does not mean 50% of the C0 field. The 1.34 m/s term
holds the fitted conversion below the identification dataset's minimum speed;
it does not disable steering or add a maneuver state.
C0 correction is recomputed every update, including updates that have no fresh
integration interval. It has no integrator, request-change state, deadband, or
additional slew limit. It becomes zero at zero error and changes sign on
overshoot. Existing driver override and PSCM arbitration disable it together
with other feedback. PSCM limit-reached continues to inhibit outward C1
integration; it does not freeze either proportional command. C1 remains
P=0.75, I=1.0. C2/C3 remain zero.
The existing `FordModelActionController` Sunnylink toggle still selects this
controller for CAN FD. With the toggle off, startup selects upstream Ford
control. No new UI or lateral-maneuver changes are included.
## Offline validation
The native-time replay compares the candidate against production commit
`d425b3260b0785b22096d130702b54a2e0761c36`, with recorded measurements, requests,
model health, driver input, service timestamps and PSCM arbitration held fixed.
- Ten Lightning routes: a9, b9, 112, 113, 117, 11c, 125, 146, 149 and 151.
- 1,170,113 cycles; 117,016 in-memory Float32/CAN encode/decode checks.
- Identical C1 output, C1 integral and command validity on every compared cycle.
- All outputs finite, inside field bounds, with C2/C3 zero.
- C0 exactly matches the old command whenever its new correction is zero or
feedback is disabled.
- Across eligible samples with at least 30 degrees of wheel-angle error, the
median C0 change is 0.65 m and the 95th percentile is 1.47 m.
- Across eligible requests below 5 degrees, the median change is 0.01 m and
the 95th percentile is 0.05 m. This cohort includes turn exits with a still
turned wheel; its largest correction is consequently much larger.
- At route 151 time 306.89 s, the replay changes C0 from 0.75 to 1.68 m for
approximately 76 degrees of tracking error. Both replays send the same C1.
The test suite covers immediate correction, no accumulation, reversal, catch-up,
invalid inputs, real vehicle-model units across stiffness/speed/roll changes,
driver/PSCM arbitration, upstream fallback, and actual controlsd publication
through the 100 Hz CAN sender. The machine-readable route replay summary is
`ford_c0_feedback_v15_validation.json`.
```
PYTHONPATH=.:opendbc_repo python -m pytest -q \
openpilot/selfdrive/controls/tests/test_ford_model_action*.py \
openpilot/selfdrive/controls/tests/test_ford_path.py
PYTHONPATH=.:opendbc_repo python tools/ford_pscm_lab/c0_feedback_validate.py \
--output .cache/ford_c0_feedback_v15 --workers 4
```
The replay requires the existing local rlog extracts. Input hashes are recorded
in its validation JSON. It does not assume the truck follows modified commands.
## What is still experimental
The earlier fitted plant overpredicted one second of wheel movement by about
16 degrees in the route 151 example. It also misses the phase of a low-speed
oscillation in route 125. Independent C0/C1 response contributions are an
approximation; a shared PSCM limit could prevent the extra movement predicted
from C0.
An additional four-second fitted-plant simulation, with controller feedback
recomputed against simulated wheel angle, showed no regression for the tested
0.5 gain in its turn, unwind and near-straight cohorts. Route 149 had 27 eligible
turn windows; route 151 had no four-second turn windows surviving the strict
intervention/status mask. Future reference and speed were frozen, and the model
does not reproduce the route 125 failure faithfully. These results are a
sanity check, not validation of road tracking or an optimized gain.
Actual acceptance is better desired-versus-actual wheel tracking on turn entry,
without added oscillation, overshoot or delayed unwind. The software behavior
is validated; the physical improvement remains to be measured.
Diagnostics identify `model-action-curvature-c0-feedback-v15` and record
`offset_proportional`, `c0_proportional_gain`, and `curvature_scale` alongside
the existing heading/feedforward/integral signals.
docs/ford_c0_feedback_v15_validation.json
+856
View File
@@ -0,0 +1,856 @@
{
"scope": "Replay C0 feedback against v14 using frozen native-time Lightning measurements.\n\nThis checks software/CAN behavior, not the physical response to new commands.\nUse cached route.npz, model_paths.npz and metadata.json from the rlog extractor.\n",
"routes": [
{
"route": "112",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 108971,
"wire_round_trips": 10898,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 83590,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.28000000000000025,
1.7300000000000004
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"straight": {
"n": 43982,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.04999999999999982,
1.4699999999999998
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"turn": {
"n": 9939,
"delta_c0_m_quantiles": [
0.15999999999999925,
1.1199999999999997,
1.7300000000000004
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"large_error": {
"n": 3604,
"delta_c0_m_quantiles": [
0.73,
1.4299999999999997,
1.7300000000000004
],
"c0_old_capped": 0,
"c0_new_capped": 0
}
},
"sources": {
".cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
".cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
},
{
"route": "113",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 49614,
"wire_round_trips": 4962,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 24171,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.9400000000000004,
2.2200000000000006
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"straight": {
"n": 16815,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.05999999999999961,
1.0499999999999998
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"turn": {
"n": 2920,
"delta_c0_m_quantiles": [
0.6600000000000001,
1.8704999999999974,
2.2200000000000006
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"large_error": {
"n": 2831,
"delta_c0_m_quantiles": [
0.8699999999999997,
1.9399999999999995,
2.2200000000000006
],
"c0_old_capped": 0,
"c0_new_capped": 0
}
},
"sources": {
".cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327",
".cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f",
".cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
},
{
"route": "117",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 27301,
"wire_round_trips": 2731,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 13659,
"delta_c0_m_quantiles": [
0.03000000000000025,
0.5,
1.5900000000000007
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"straight": {
"n": 5001,
"delta_c0_m_quantiles": [
0.010000000000000675,
0.3000000000000007,
1.5899999999999999
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"turn": {
"n": 2891,
"delta_c0_m_quantiles": [
0.1200000000000001,
0.7400000000000002,
1.38
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"large_error": {
"n": 1195,
"delta_c0_m_quantiles": [
0.54,
1.5599999999999996,
1.5900000000000007
],
"c0_old_capped": 0,
"c0_new_capped": 0
}
},
"sources": {
".cache/ford_route117/route.npz": "65e4ea6c76dce73018b636479b3777116f1e58bb9813e6c1a7b79018b219140e",
".cache/ford_route117/metadata.json": "348396b3059c5f54d7938cd3acf2bb268a813b47ff47561da7a94cb75c3f3114",
".cache/ford_route117/model_paths.npz": "079f6003968287530e1ed6f2243c727e80c2ef3d16fb8a7d945d7eca744d54e7",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
},
{
"route": "11c",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 54146,
"wire_round_trips": 5415,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 36780,
"delta_c0_m_quantiles": [
0.020000000000000462,
0.3700000000000001,
2.1799999999999997
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"straight": {
"n": 16750,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.09999999999999964,
0.5
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"turn": {
"n": 2461,
"delta_c0_m_quantiles": [
0.1899999999999995,
0.8899999999999997,
2.1799999999999997
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"large_error": {
"n": 1827,
"delta_c0_m_quantiles": [
0.5599999999999996,
0.9569999999999982,
2.1799999999999997
],
"c0_old_capped": 0,
"c0_new_capped": 0
}
},
"sources": {
".cache/ford_maneuver_11c/analysis/route.npz": "1f3c37a7c7ae85ba69e9958435a9568f8244fa862f43ad1ee2a0192d1df13966",
".cache/ford_maneuver_11c/analysis/metadata.json": "2d0051d23059145988ca905e5386704def24b8080ce92541cec5e4ab501bcd0c",
".cache/ford_maneuver_11c/analysis/model_paths.npz": "0ff433da3dde1d4d7f10596cd53558ec5de9d56fbc49aec3e8afb4c9872377d5",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
},
{
"route": "125",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 41717,
"wire_round_trips": 4172,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 41107,
"delta_c0_m_quantiles": [
0.0,
0.05999999999999961,
0.40000000000000036
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"straight": {
"n": 33135,
"delta_c0_m_quantiles": [
0.0,
0.010000000000000675,
0.28000000000000025
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"large_error": {
"n": 192,
"delta_c0_m_quantiles": [
0.3200000000000003,
0.3799999999999999,
0.40000000000000036
],
"c0_old_capped": 0,
"c0_new_capped": 0
}
},
"sources": {
".cache/ford_c0_response_124_125/route125/route.npz": "8dcf304871bf5c6bf1ce206c788d7034bf5f056522faded0aa7948ff1cccbb75",
".cache/ford_c0_response_124_125/route125/metadata.json": "1f2b812d3c2cc8e5a2246098756940d6f8989a2591daf4d74b1ef4c8555e2d57",
".cache/ford_c0_response_124_125/route125/model_paths.npz": "4be9c7809dabbcb52296ba93b4743146dc941e49a32aca07f008b75757703e49",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
},
{
"route": "146",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 53229,
"wire_round_trips": 5323,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 40717,
"delta_c0_m_quantiles": [
0.0,
0.04999999999999982,
1.1300000000000003
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"straight": {
"n": 30796,
"delta_c0_m_quantiles": [
0.0,
0.02999999999999936,
0.35000000000000053
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"turn": {
"n": 585,
"delta_c0_m_quantiles": [
0.3700000000000001,
1.0300000000000002,
1.1300000000000003
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"large_error": {
"n": 298,
"delta_c0_m_quantiles": [
0.4500000000000002,
1.0614999999999999,
1.1300000000000003
],
"c0_old_capped": 0,
"c0_new_capped": 0
}
},
"sources": {
".cache/ford_model_replay_146/full/route.npz": "b01db5bdd1c3b9fb83418a298571c98e60b5eb0509800d86cdaf07bafcdc6d63",
".cache/ford_model_replay_146/full/metadata.json": "2be1254dd1eee422f3b2c2011faa91a21ab431751998525105381c2886df15ca",
".cache/ford_model_replay_146/full/model_paths.npz": "11e0e9c8ce2959aac72d2f8a9cc4f6a025480034d5cd8b59b68a41b2ae3cf4a8",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
},
{
"route": "149",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 132334,
"wire_round_trips": 13234,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 109224,
"delta_c0_m_quantiles": [
0.019999999999999574,
0.43999999999999995,
2.2800000000000002
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"straight": {
"n": 54914,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.07000000000000028,
2.04
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"turn": {
"n": 15161,
"delta_c0_m_quantiles": [
0.20999999999999996,
1.0300000000000002,
2.25
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"large_error": {
"n": 6659,
"delta_c0_m_quantiles": [
0.6399999999999997,
1.4100000000000001,
2.2800000000000002
],
"c0_old_capped": 0,
"c0_new_capped": 0
}
},
"sources": {
".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade",
".cache/ford_route149/full/metadata.json": "624fff03c25eb298661cb7b25f3dbe6d214d863f799d93635c0f4f05fc0d2b32",
".cache/ford_route149/full/model_paths.npz": "19827800b8fb5983f3d6b72fcfaf36e35a40170bb17cd7c1e49374744fb449fb",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
},
{
"route": "151",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 325708,
"wire_round_trips": 32571,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 124806,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.10000000000000053,
2.21
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"straight": {
"n": 85546,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.03999999999999915,
0.6300000000000008
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"turn": {
"n": 3715,
"delta_c0_m_quantiles": [
0.25,
1.12,
2.21
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"large_error": {
"n": 1839,
"delta_c0_m_quantiles": [
0.6200000000000001,
1.35,
2.21
],
"c0_old_capped": 0,
"c0_new_capped": 0
}
},
"sources": {
".cache/ford_route151/full/route.npz": "41a5b8bd388cf5a3d553f784542376ac9355fcdc5be4f427053d0504537babe1",
".cache/ford_route151/full/metadata.json": "937825317a0edd470c54647240b922be8f79dda5b3365ffdd61281f0aca877a1",
".cache/ford_route151/full/model_paths.npz": "980b3843cec04254280d744a5801cee1bf0f8ef371052398327ff245f9eee01b",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
},
{
"route": "a9",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 286319,
"wire_round_trips": 28632,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 142471,
"delta_c0_m_quantiles": [
0.019999999999999574,
0.15999999999999925,
1.5699999999999994
],
"c0_old_capped": 25,
"c0_new_capped": 128
},
"straight": {
"n": 99221,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.05999999999999961,
1.4800000000000004
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"turn": {
"n": 6893,
"delta_c0_m_quantiles": [
0.23000000000000043,
0.9699999999999998,
1.5600000000000005
],
"c0_old_capped": 25,
"c0_new_capped": 128
},
"large_error": {
"n": 3224,
"delta_c0_m_quantiles": [
0.6600000000000001,
1.2599999999999998,
1.5699999999999994
],
"c0_old_capped": 25,
"c0_new_capped": 128
}
},
"sources": {
".cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975",
".cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93",
".cache/ford_routea9/model_paths.npz": "7f3646ad24aa52de5982c65600813de5bbc5f7664d92a2516bbba35da6e7b240",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
},
{
"route": "b9",
"baseline": "d425b3260b0785b22096d130702b54a2e0761c36",
"baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8",
"cycles": 90774,
"wire_round_trips": 9078,
"c1_identical": true,
"c0_gain": 0.5,
"metrics": {
"all": {
"n": 81066,
"delta_c0_m_quantiles": [
0.010000000000000675,
0.3100000000000005,
2.12
],
"c0_old_capped": 0,
"c0_new_capped": 10
},
"straight": {
"n": 42647,
"delta_c0_m_quantiles": [
0.009999999999999787,
0.07000000000000028,
1.63
],
"c0_old_capped": 0,
"c0_new_capped": 0
},
"turn": {
"n": 9816,
"delta_c0_m_quantiles": [
0.1200000000000001,
0.8300000000000001,
2.12
],
"c0_old_capped": 0,
"c0_new_capped": 10
},
"large_error": {
"n": 3486,
"delta_c0_m_quantiles": [
0.6600000000000001,
1.5100000000000005,
2.12
],
"c0_old_capped": 0,
"c0_new_capped": 10
}
},
"sources": {
".cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
".cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee",
".cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad",
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca"
}
}
],
"test_summary": {
"pytest_passed": 439,
"ruff": "passed",
"git_diff_check": "passed"
},
"fit_source_sha256": "536ea20ee1bfa9366453c6d01fae6c7dd7b83388f2a71662cc12b43311767c6f",
"fitted_plant_sanity_check": {
"scope": "Approximate fitted-plant simulation with frozen future reference/speed, recomputed controller feedback, 20 Hz, four-second clean windows. Not physical tracking validation.",
"routes": {
"149": {
"0.0": {
"all": {
"windows": 515,
"tracking_rmse_deg": 1.9599623978328702,
"p95_abs_error_deg": 3.6108132362595673
},
"turn": {
"windows": 27,
"tracking_rmse_deg": 5.648897344766248,
"p95_abs_error_deg": 10.154937282895988
},
"unwind": {
"windows": 16,
"tracking_rmse_deg": 4.56296777072208,
"p95_abs_error_deg": 5.8047028901317175
},
"near_straight": {
"windows": 280,
"tracking_rmse_deg": 0.5053540034793153,
"p95_abs_error_deg": 0.9099971698933058
}
},
"0.25": {
"all": {
"windows": 515,
"tracking_rmse_deg": 1.868833791306516,
"p95_abs_error_deg": 3.4194303642888486
},
"turn": {
"windows": 27,
"tracking_rmse_deg": 5.3063969129788715,
"p95_abs_error_deg": 9.669302705935252
},
"unwind": {
"windows": 16,
"tracking_rmse_deg": 4.307554036590833,
"p95_abs_error_deg": 5.549381163721539
},
"near_straight": {
"windows": 280,
"tracking_rmse_deg": 0.4868309463403626,
"p95_abs_error_deg": 0.8696197439962221
}
},
"0.5": {
"all": {
"windows": 515,
"tracking_rmse_deg": 1.7898071883511764,
"p95_abs_error_deg": 3.260221243224674
},
"turn": {
"windows": 27,
"tracking_rmse_deg": 5.006440079078192,
"p95_abs_error_deg": 9.002861391131468
},
"unwind": {
"windows": 16,
"tracking_rmse_deg": 4.078042889798365,
"p95_abs_error_deg": 5.24331028633435
},
"near_straight": {
"windows": 280,
"tracking_rmse_deg": 0.4694670576005347,
"p95_abs_error_deg": 0.8331203606609966
}
},
"0.75": {
"all": {
"windows": 515,
"tracking_rmse_deg": 1.7203138728888623,
"p95_abs_error_deg": 3.1149598999491044
},
"turn": {
"windows": 27,
"tracking_rmse_deg": 4.743912352245749,
"p95_abs_error_deg": 8.664577551257613
},
"unwind": {
"windows": 16,
"tracking_rmse_deg": 3.8750067743533827,
"p95_abs_error_deg": 4.9966209407120425
},
"near_straight": {
"windows": 280,
"tracking_rmse_deg": 0.45412470534078503,
"p95_abs_error_deg": 0.7964604020331516
}
},
"1.0": {
"all": {
"windows": 515,
"tracking_rmse_deg": 1.6590272653809652,
"p95_abs_error_deg": 2.990976062659628
},
"turn": {
"windows": 27,
"tracking_rmse_deg": 4.507245023760021,
"p95_abs_error_deg": 8.107112731535002
},
"unwind": {
"windows": 16,
"tracking_rmse_deg": 3.693644190831072,
"p95_abs_error_deg": 4.749308802498215
},
"near_straight": {
"windows": 280,
"tracking_rmse_deg": 0.4404620597734029,
"p95_abs_error_deg": 0.7694584025732562
}
}
},
"151": {
"0.0": {
"all": {
"windows": 794,
"tracking_rmse_deg": 0.9049924838460381,
"p95_abs_error_deg": 1.496850675938314
},
"unwind": {
"windows": 7,
"tracking_rmse_deg": 1.3568942395552432,
"p95_abs_error_deg": 2.4717442023707914
},
"near_straight": {
"windows": 525,
"tracking_rmse_deg": 0.47746381422226297,
"p95_abs_error_deg": 0.9045192063209317
}
},
"0.25": {
"all": {
"windows": 794,
"tracking_rmse_deg": 0.8744021810097441,
"p95_abs_error_deg": 1.4250449905919713
},
"unwind": {
"windows": 7,
"tracking_rmse_deg": 1.2734874215389322,
"p95_abs_error_deg": 2.3213325000763567
},
"near_straight": {
"windows": 525,
"tracking_rmse_deg": 0.45648429088650827,
"p95_abs_error_deg": 0.8575351438117865
}
},
"0.5": {
"all": {
"windows": 794,
"tracking_rmse_deg": 0.8487165771252436,
"p95_abs_error_deg": 1.3633464420801624
},
"unwind": {
"windows": 7,
"tracking_rmse_deg": 1.1986282107421773,
"p95_abs_error_deg": 2.163514655425466
},
"near_straight": {
"windows": 525,
"tracking_rmse_deg": 0.4395196212749708,
"p95_abs_error_deg": 0.82723861668119
}
},
"0.75": {
"all": {
"windows": 794,
"tracking_rmse_deg": 0.8256116631456035,
"p95_abs_error_deg": 1.309241916169811
},
"unwind": {
"windows": 7,
"tracking_rmse_deg": 1.1341202040172427,
"p95_abs_error_deg": 2.0707107084230465
},
"near_straight": {
"windows": 525,
"tracking_rmse_deg": 0.42406706004679057,
"p95_abs_error_deg": 0.7946947170401512
}
},
"1.0": {
"all": {
"windows": 794,
"tracking_rmse_deg": 0.8052189413919172,
"p95_abs_error_deg": 1.2575495536727697
},
"unwind": {
"windows": 7,
"tracking_rmse_deg": 1.0739985410343649,
"p95_abs_error_deg": 1.9599553503928666
},
"near_straight": {
"windows": 525,
"tracking_rmse_deg": 0.41043500769646296,
"p95_abs_error_deg": 0.7683540262306987
}
}
},
"125": {
"0.0": {
"all": {
"windows": 346,
"tracking_rmse_deg": 1.5101438591368561,
"p95_abs_error_deg": 1.3433430124202965
},
"near_straight": {
"windows": 246,
"tracking_rmse_deg": 0.3997814518183382,
"p95_abs_error_deg": 0.8420670524669589
}
},
"0.25": {
"all": {
"windows": 346,
"tracking_rmse_deg": 1.460719401448126,
"p95_abs_error_deg": 1.2614591679946765
},
"near_straight": {
"windows": 246,
"tracking_rmse_deg": 0.377498378600397,
"p95_abs_error_deg": 0.7868969957812859
}
},
"0.5": {
"all": {
"windows": 346,
"tracking_rmse_deg": 1.4162750202728391,
"p95_abs_error_deg": 1.188472341120946
},
"near_straight": {
"windows": 246,
"tracking_rmse_deg": 0.35896062148345687,
"p95_abs_error_deg": 0.7523514432810818
}
},
"0.75": {
"all": {
"windows": 346,
"tracking_rmse_deg": 1.3766402449201502,
"p95_abs_error_deg": 1.1248966870109076
},
"near_straight": {
"windows": 246,
"tracking_rmse_deg": 0.34130621657460447,
"p95_abs_error_deg": 0.7141102879383627
}
},
"1.0": {
"all": {
"windows": 346,
"tracking_rmse_deg": 1.3414563262275077,
"p95_abs_error_deg": 1.0712299506410858
},
"near_straight": {
"windows": 246,
"tracking_rmse_deg": 0.3255065971687205,
"p95_abs_error_deg": 0.6807731116817476
}
}
}
}
}
}
openpilot/selfdrive/controls/controlsd.py
+2
View File
@@ -171,6 +171,8 @@ class Controls(ControlsExt):
reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
self.ford_path = self.ford_path_controller.update(
ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(),
# Roll/angle offset cancel in the error; retain the normal steering-angle conversion's speed and stiffness effects.
curvature_scale=self.VM.get_steer_from_curvature(1., CS.vEgo, 0.) / (self.CP.steerRatio*self.CP.wheelbase),
measurement_time=self.sm.logMonoTime['carState'] * 1e-9,
model_time=self.sm.logMonoTime['modelV2'] * 1e-9,
reference_time=self.sm.logMonoTime[reference_service] * 1e-9,
openpilot/selfdrive/controls/lib/ford_model_action.py
+32 -13
View File
@@ -1,7 +1,7 @@
"""Opt-in Ford C2-free model mapping with measured-curvature PI feedback.
C0 samples a desired-curvature arc at 7 m, optionally max(7 m, v*1s),
including base-heading overflow. C1
including base-heading overflow, plus proportional tracking correction. C1
combines the selected curvature's heading with proportional and integrated
tracking error. Reference distance and gains are explicit trial choices.
Commands use the current bounded request without an additional C0/C1 slew.
@@ -19,6 +19,10 @@ OFFSET_STATION_M = 7.0
HEADING_TIME_S = 1.0
C1_PROPORTIONAL_GAIN = 0.75 # Drive-trial gains, not a learned calibration.
C1_INTEGRAL_GAIN = 1.0
C0_PROPORTIONAL_GAIN = 0.5 # Trial fraction of the measured wheel-angle error.
# Offline Lightning fit: geometric curvature per metre of C0, with v in m/s.
C0_RESPONSE_CONSTANT = 0.010717679293424373
C0_RESPONSE_INVERSE_SPEED_SQUARED = 0.018122981795212647
CALIBRATION_APPROVED = False
@@ -63,26 +67,33 @@ class ModelActionController:
Freshness, measurement cadence and driver/PSCM arbitration belong to the caller.
"""
__slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature', 'c0_time_based')
__slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature', 'c0_time_based',
'c0_proportional_gain', 'offset_proportional')
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False):
if not _finite(proportional_gain, integral_gain) or min(proportional_gain, integral_gain) < 0.:
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False,
c0_proportional_gain=C0_PROPORTIONAL_GAIN):
if not _finite(proportional_gain, integral_gain, c0_proportional_gain) or min(proportional_gain, integral_gain, c0_proportional_gain) < 0.:
raise ValueError('PI gains must be finite and nonnegative')
self.proportional_gain, self.integral_gain = float(proportional_gain), float(integral_gain)
self.c0_proportional_gain = float(c0_proportional_gain)
self.c0_time_based = bool(c0_time_based)
self.reset()
def reset(self):
self.c0 = self.c1 = self.correction = self.proportional = self.feedback_curvature = 0.
self.offset_proportional = 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_curvature=None):
feedback_dt=None, feedback_enabled=True, pscm_limited=False, feedback_curvature=None, curvature_scale=1.):
feedback_dt = dt if feedback_dt is None else feedback_dt
reference = desired_curvature if feedback_curvature is None else feedback_curvature
if (not active or not valid or not _finite(dt, feedback_dt, current_curvature, reference) or not .002 <= dt <= .1
if (not active or not valid or not _finite(dt, feedback_dt, current_curvature, reference, curvature_scale) or not .002 <= dt <= .1
or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(reference) > 1.):
self.reset()
return FordPath()
if curvature_scale <= 0.:
self.reset()
return FordPath()
target = encode_model_action(model, desired_curvature, speed, c0_time_based=self.c0_time_based)
if not target.valid:
self.reset()
@@ -90,12 +101,16 @@ class ModelActionController:
self.feedback_curvature = reference
error = reference-current_curvature
self.proportional = self.proportional_gain*max(OFFSET_STATION_M, speed*HEADING_TIME_S)*error if feedback_enabled else 0.
if not _finite(self.proportional):
# Road-curvature error -> geometric steering error -> metres of C0.
# This is proportional only: nothing is accumulated or carried into a release.
response = C0_RESPONSE_CONSTANT+C0_RESPONSE_INVERSE_SPEED_SQUARED/max(speed, 1.34)**2
self.offset_proportional = self.c0_proportional_gain*error*curvature_scale/response if feedback_enabled else 0.
if not _finite(self.proportional, self.offset_proportional):
self.reset()
return FordPath()
base = float(np.clip(target.path_angle, -.5, .5))
offset = float(np.clip(target.path_offset+OFFSET_STATION_M*(target.path_angle-base), -5.11, 5.11))
self.c0 = offset
self.c0 = float(np.clip(offset+self.offset_proportional, -5.11, 5.11))
if feedback_enabled:
increment = self.integral_gain*error*speed*feedback_dt
if not _finite(increment):
@@ -129,9 +144,11 @@ 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, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False):
self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based)
self.hypothesis = 'model-action-curvature-c0-distance-pi-v14'
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False,
c0_proportional_gain=C0_PROPORTIONAL_GAIN):
self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based,
c0_proportional_gain=c0_proportional_gain)
self.hypothesis = 'model-action-curvature-c0-feedback-v15'
self.reset()
def set_c0_time_based(self, enabled, *, lateral_engaged):
@@ -151,7 +168,7 @@ class FordModelActionController:
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,
feedback_curvature=None):
feedback_curvature=None, curvature_scale=1.):
reason = None
if not active:
reason = 'inactive'
@@ -183,7 +200,7 @@ class FordModelActionController:
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_curvature=feedback_curvature)
feedback_curvature=feedback_curvature, curvature_scale=curvature_scale)
if not command.valid:
self.reset('invalid_path')
return command
@@ -199,6 +216,8 @@ class FordModelActionController:
'curvature_error': desired_curvature-current_curvature, 'feedback_dt': feedback_dt,
'heading_feedforward': base_heading,
'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading),
'offset_proportional': self.core.offset_proportional, 'c0_proportional_gain': self.core.c0_proportional_gain,
'curvature_scale': curvature_scale,
'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled,
'heading_proportional': self.core.proportional, 'proportional_gain': self.core.proportional_gain,
'integral_gain': self.core.integral_gain, 'feedback_curvature': self.core.feedback_curvature,
openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py
+12 -5
View File
@@ -29,7 +29,8 @@ from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import
def assert_current_request(core, desired, speed):
target = encode_model_action(straight(), desired, speed)
base = min(.5, max(-.5, target.path_angle))
assert core.c0 == pytest.approx(min(5.11, max(-5.11, target.path_offset+7.*(target.path_angle-base))))
offset = min(5.11, max(-5.11, target.path_offset+7.*(target.path_angle-base)))
assert core.c0 == pytest.approx(min(5.11, max(-5.11, offset+core.offset_proportional)))
assert core.c1 == pytest.approx(min(.5, max(-.5, base+core.proportional+core.correction)))
@@ -190,7 +191,8 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
assert controller.core.proportional == pytest.approx(.75*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)*.0045)
assert controls.ford_path.path_offset == pytest.approx(0.) # Limited curvature arc is below one C0 step.
assert controls.ford_path.path_offset == pytest.approx((-1 if maneuver else 1)*.01)
assert controller.core.offset_proportional*expected_curvature > 0.
assert cc.latActive and cc.actuators.curvature == 0.
assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
@@ -280,7 +282,9 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
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(sign*.1)
base = encode_model_action(straight(), sign*.004, cs.vEgo).path_offset
assert controls.ford_path.path_offset == pytest.approx(base+core.offset_proportional, abs=.005)
assert (core.offset_proportional == 0.) == (torque != 0. or measured == sign*.004)
@pytest.mark.parametrize('service_valid', [False, True])
@@ -355,13 +359,16 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
if frame == 199:
assert sign*core.correction < 0. if same_turn else sign*core.correction > 0.
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v14'
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v15'
if same_turn:
assert controls.desired_curvature == pytest.approx(sign*.01)
assert sign*controls.ford_path.path_angle >= speed*.01 # No old unwind correction left below the new base.
else:
assert sign*controls.ford_path.path_angle < 0.
assert controls.ford_path.path_offset == pytest.approx(sign*(.24 if same_turn else -.02))
if same_turn:
assert sign*controls.ford_path.path_offset > .24
else:
assert sign*controls.ford_path.path_offset < -.02
controls.ford_path_controller.reset()
assert core.c0 == core.c1 == core.correction == 0.
openpilot/selfdrive/controls/tests/test_ford_model_action_c0_feedback.py
+104
View File
@@ -0,0 +1,104 @@
"""C0 proportional correction semantics, independent of simulated PSCM motion."""
import math
import pytest
from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
from openpilot.selfdrive.controls.lib.ford_path import FordPath
from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
@pytest.mark.parametrize('sign', [-1., 1.])
def test_c0_helps_entry_then_releases_at_catchup_without_accumulation(sign):
controller = ModelActionController(c0_proportional_gain=.5)
matched = ModelActionController(c0_proportional_gain=0.)
kwargs = {'speed': 5., 'dt': .01, 'feedback_dt': 0., 'curvature_scale': 1.2}
base = matched.update(straight(), sign*.02, current_curvature=sign*.01, **kwargs)
first = controller.update(straight(), sign*.02, current_curvature=sign*.01, **kwargs)
assert sign*(first.path_offset-base.path_offset) > .4
assert controller.offset_proportional*sign > 0.
for _ in range(500):
held = controller.update(straight(), sign*.02, current_curvature=sign*.01, **kwargs)
assert held == first
caught = controller.update(straight(), sign*.02, current_curvature=sign*.02, **kwargs)
assert controller.offset_proportional == 0.
assert caught.path_offset == base.path_offset
overshot = controller.update(straight(), sign*.02, current_curvature=sign*.03, **kwargs)
assert sign*(overshot.path_offset-base.path_offset) < -.4
@pytest.mark.parametrize('sign', [-1., 1.])
def test_zero_target_commands_opposite_c0_and_does_not_retain_old_correction(sign):
core = ModelActionController(c0_proportional_gain=.5)
core.update(straight(), sign*.1, current_curvature=0., speed=5., dt=.01)
out = core.update(straight(), 0., current_curvature=sign*.03, speed=5., dt=.01, feedback_dt=0.)
assert sign*out.path_offset < 0.
out = core.update(straight(), 0., current_curvature=0., speed=5., dt=.01)
assert out.path_offset == core.offset_proportional == 0.
def test_new_c0_does_not_change_c1_or_its_integral_for_identical_measurements():
old, new = ModelActionController(c0_proportional_gain=0.), ModelActionController(c0_proportional_gain=.5)
for i in range(200):
desired, measured = .03*math.sin(i/13), .025*math.sin((i-5)/13)
kwargs = {'current_curvature': measured, 'speed': 12., 'dt': .01, 'feedback_enabled': i % 7 != 0, 'pscm_limited': i % 9 == 0}
a, b = [c.update(straight(), desired, **kwargs) for c in (old, new)]
assert a.path_angle == b.path_angle
assert (old.correction, old.proportional) == (new.correction, new.proportional)
assert b.curvature == b.curvature_rate == 0.
def test_c0_is_disabled_by_existing_feedback_arbitration_and_reset():
core = ModelActionController(c0_proportional_gain=.5)
core.update(straight(), .02, current_curvature=0., speed=5., dt=.01)
assert core.offset_proportional > 0.
disabled = core.update(straight(), .02, current_curvature=0., speed=5., dt=.01, feedback_enabled=False)
assert core.offset_proportional == 0.
core.reset()
assert core.c0 == core.offset_proportional == 0.
assert disabled.path_offset > 0. # Existing feedforward remains available.
@pytest.mark.parametrize('scale', [0., -1., math.nan, math.inf, None])
def test_bad_curvature_conversion_cannot_send_a_command(scale):
core = ModelActionController()
assert core.update(straight(), .02, current_curvature=0., speed=5., dt=.01, curvature_scale=scale) == FordPath()
assert core.c0 == core.offset_proportional == 0.
@pytest.mark.parametrize('gain', [-1., math.nan, math.inf, None])
def test_bad_c0_gain_is_rejected(gain):
with pytest.raises(ValueError):
ModelActionController(c0_proportional_gain=gain)
def test_c0_uses_feedback_reference_and_stays_inside_field_bounds():
core = ModelActionController(c0_proportional_gain=.5)
core.update(straight(), .03, current_curvature=.02, feedback_curvature=.02, speed=5., dt=.01)
assert core.offset_proportional == 0.
for sign in (-1., 1.):
out = core.update(straight(), sign*.9, current_curvature=-sign*.9, speed=55., dt=.01, curvature_scale=3.)
assert out.path_offset == pytest.approx(sign*5.11)
def test_c0_response_units_and_explicit_gain():
core = ModelActionController(c0_proportional_gain=.5)
core.update(straight(), .01, current_curvature=0., speed=5., dt=.01, curvature_scale=1.2)
# Fitted C0 gain is geometric curvature per metre of command, not road curvature.
response = .010717679293424373+.018122981795212647/25.
assert core.offset_proportional == pytest.approx(.5*.01*1.2/response)
@pytest.mark.parametrize('speed', [1., 5., 20., 40.])
@pytest.mark.parametrize('stiffness,ratio,roll', [(.3, 14., -.1), (1., 16.9, 0.), (2., 20., .1)])
def test_curvature_error_conversion_matches_normal_desired_wheel_angle(speed, stiffness, ratio, roll):
controls = startup()
vm, cp = controls.VM, controls.CP
vm.update_params(stiffness, ratio)
angle, angle_offset, desired = 15., 1.5, .002
measured = -vm.calc_curvature(math.radians(angle-angle_offset), speed, roll)
target = math.degrees(vm.get_steer_from_curvature(-desired, speed, roll))+angle_offset
scale = vm.get_steer_from_curvature(1., speed, 0.)/(cp.steerRatio*cp.wheelbase)
converted_error = -(desired-measured)*scale*cp.steerRatio*cp.wheelbase*180/math.pi
assert converted_error == pytest.approx(target-angle)
openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py
+1 -1
View File
@@ -17,7 +17,7 @@ def tick(controller, desired, measured, **overrides):
@pytest.mark.parametrize('sign', [-1., 1.])
def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign):
controller, matched = ModelActionController(proportional_gain=0., integral_gain=1.), ModelActionController(proportional_gain=0., integral_gain=1.)
controller, matched = (ModelActionController(proportional_gain=0., integral_gain=1., c0_proportional_gain=0.) for _ in range(2))
for _ in range(100):
tick(controller, sign*.004, sign*.004)
for _ in range(100):
openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py
+3 -2
View File
@@ -63,7 +63,7 @@ def test_extra_offset_releases_immediately_without_stored_overflow(sign):
@pytest.mark.parametrize('sign', [-1., 1.])
def test_c1_feedback_saturation_does_not_spill_correction_into_c0(sign):
controller = ModelActionController(proportional_gain=0., integral_gain=1.)
controller = ModelActionController(proportional_gain=0., integral_gain=1., c0_proportional_gain=0.)
for _ in range(200):
out = controller.update(straight(sign*.2), sign*.02, current_curvature=0., speed=20., dt=.01)
assert out.path_angle == pytest.approx(sign*.5)
@@ -78,6 +78,7 @@ def test_overflow_is_base_geometry_with_existing_feedback_gates(sign, enabled, l
for _ in range(150):
out = controller.update(straight(sign*.2), sign*.03, current_curvature=sign*.02, speed=20., dt=.01,
feedback_enabled=enabled, pscm_limited=limited)
assert out.path_offset == pytest.approx(sign*((1-math.cos(.21))/.03+.7), abs=.005)
expected = sign*((1-math.cos(.21))/.03+.7)+controller.offset_proportional
assert out.path_offset == pytest.approx(expected, abs=.005)
assert out.path_angle == pytest.approx(sign*.5)
assert controller.correction == 0.
openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py
+7 -2
View File
@@ -8,6 +8,7 @@ from types import SimpleNamespace
import pytest
from opendbc.car.ford.values import CAR, FordFlags
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType
from openpilot.selfdrive.controls.lib.ford_model_action import C1_INTEGRAL_GAIN, C1_PROPORTIONAL_GAIN, FordModelActionController, select_model_action_controller
from openpilot.selfdrive.controls.lib.ford_path import FordPath
@@ -18,7 +19,9 @@ CANFD_CARS = [car for car in CAR if car.config.flags & FordFlags.CANFD]
def car_params(**overrides):
return SimpleNamespace(**({'brand': 'ford', 'flags': FordFlags.CANFD, 'carFingerprint': 'FORD_F_150_LIGHTNING_MK1',
'carFw': []} | overrides))
'carFw': [], 'steerRatio': 16.9, 'wheelbase': 3.7, 'mass': 3084., 'rotationalInertia': 5000.,
'centerToFront': 1.628, 'steerRatioRear': 0., 'tireStiffnessFront': 378306.8125,
'tireStiffnessRear': 469877.5625} | overrides))
def startup(cp=None, params=None):
@@ -36,6 +39,7 @@ def startup(cp=None, params=None):
'select_model_action_controller': select_model_action_controller,
'cloudlog': SimpleNamespace(event=lambda *args, **kwargs: None)}
exec(compile(ast.Module(body=body[start:end+1], type_ignores=[]), str(filename), 'exec'), environment)
controls.VM = VehicleModel(controls.CP)
return controls
@@ -48,7 +52,8 @@ def test_actual_startup_priority(candidate, observer, fingerprint):
assert type(selected.ford_path_controller) is FordModelActionController
assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .75
assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == 1.
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v14'
assert selected.ford_path_controller.core.c0_proportional_gain == .5
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v15'
else:
assert selected.ford_path_controller is None
assert selected.ford_model_action == candidate
tools/ford_pscm_lab/c0_feedback_validate.py
+122
View File
@@ -0,0 +1,122 @@
"""Replay C0 feedback against v14 using frozen native-time Lightning measurements.
This checks software/CAN behavior, not the physical response to new commands.
Use cached route.npz, model_paths.npz and metadata.json from the rlog extractor.
"""
import argparse
from concurrent.futures import ProcessPoolExecutor, as_completed
import hashlib
import json
from pathlib import Path
from types import SimpleNamespace
import numpy as np
from opendbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib import ford_model_action
from tools.ford_pscm_lab.feedback_replay import original_controller
from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table
BASELINE = 'd425b3260b0785b22096d130702b54a2e0761c36'
SPECIAL = {'11c': 'ford_maneuver_11c/analysis', '125': 'ford_c0_response_124_125/route125',
'146': 'ford_model_replay_146/full', '149': 'ford_route149/full', '151': 'ford_route151/full'}
ROUTES = ['a9', 'b9', '112', '113', '117', '11c', '125', '146', '149', '151']
def replay(label, output):
source = Path('.cache')/SPECIAL.get(label, 'ford_route'+label)
meta = json.loads((source/'metadata.json').read_text())
with np.load(source/'route.npz') as z:
r = {k: table(z, k) for k in ('controls', 'cs', 'cc', 'model', 'params', 'pscm')}
with np.load(source/'model_paths.npz') as z:
model_ns, paths = z['ns'], z['paths']
reference = Path('.cache/ford_route112/metadata.json')
car = {**json.loads(reference.read_text())['car'][0], **meta['car'][0]}
assert car['fingerprint'] == 'FORD_F_150_LIGHTNING_MK1'
cp = SimpleNamespace(**{k: car[k] for k in ('mass', 'wheelbase', 'centerToFront', 'steerRatioRear',
'tireStiffnessFront', 'tireStiffnessRear')},
steerRatio=car['steer_ratio'], rotationalInertia=0.)
vm = VehicleModel(cp)
c, t = r['controls'], r['controls']['t']
assert all(np.all(np.diff(s['t']) >= 0) for s 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]
old, old_hash = original_controller(BASELINE)
new = ford_model_action.FordModelActionController()
wire = WireCheck()
names = ['t', 'valid', 'c0_old', 'c0_new', 'c1', 'offset_p', 'enabled', 'error_deg', 'target_deg', 'speed', 'curvature_scale']
rows = np.zeros((len(t), len(names)))
for i, now in enumerate(t):
model_time = r['model']['t'][mi[i]]
good_params = np.isfinite([pa['stiffness'][i], pa['steer_ratio'][i]]).all() and min(pa['stiffness'][i], pa['steer_ratio'][i]) > 0
if good_params:
vm.update_params(max(pa['stiffness'][i], .1), max(pa['steer_ratio'][i], .1))
scale = vm.get_steer_from_curvature(1., cs['speed'][i], 0.)/(cp.steerRatio*cp.wheelbase)
valid = bool(c['valid'][i] and cc['valid'][i] and cs['valid'][i] and cs['can_valid'][i] and good_params
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)
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]))
args = {'current_curvature': c['measured'][i], 'yaw_rate': cs['yaw'][i], 'speed': cs['speed'][i], 'now': now,
'measurement_time': cs['t'][i], 'model_time': model_time, 'reference_time': model_time, 'active': bool(cc['active'][i]),
'valid': valid, 'driver_pressed': bool(cs['pressed'][i]), 'driver_torque': cs['torque'][i], 'pscm_status': status}
model = models[mi[i]] if exact[i] else None
a = old.update(model, c['desired'][i], **args)
b = new.update(model, c['desired'][i], curvature_scale=scale, **args)
assert a.valid == b.valid
assert a.path_angle == b.path_angle
assert old.core.correction == new.core.correction
assert abs(b.path_offset) <= 5.1100001 and abs(b.path_angle) <= .5000001
assert b.curvature == b.curvature_rate == 0.
assert np.isfinite([b.path_offset, b.path_angle, new.core.offset_proportional]).all()
if not new.diagnostics.get('feedback_enabled', False):
assert new.core.offset_proportional == 0. and a.path_offset == b.path_offset
if new.core.offset_proportional == 0.:
assert a.path_offset == b.path_offset
error = -(c['desired'][i]-c['measured'][i])*scale*cp.steerRatio*cp.wheelbase*180/np.pi
target = vm.get_steer_from_curvature(-c['desired'][i], cs['speed'][i], pa['roll'][i])*180/np.pi+pa['angle_offset'][i]
rows[i] = [now-meta['t0'], b.valid, a.path_offset, b.path_offset, b.path_angle, new.core.offset_proportional,
new.diagnostics.get('feedback_enabled', False), error, target, cs['speed'][i], scale]
if i % 10 == 0: # Native-time integration above; CAN round trip on every tenth frame.
wire.check(b)
dest = output/label
dest.mkdir(parents=True, exist_ok=True)
np.savez_compressed(dest/'commands.npz', names=names, rows=rows)
eligible = rows[:, 1].astype(bool) & rows[:, 6].astype(bool)
groups = {'all': eligible, 'straight': eligible & (abs(rows[:, 8]) < 5),
'turn': eligible & (abs(rows[:, 8]) >= 45), 'large_error': eligible & (abs(rows[:, 7]) >= 30)}
metrics = {}
for name, mask in groups.items():
if mask.any():
delta = abs(rows[mask, 3]-rows[mask, 2])
metrics[name] = {'n': int(mask.sum()), 'delta_c0_m_quantiles': np.quantile(delta, [.5, .95, 1.]).tolist(),
'c0_old_capped': int((abs(rows[mask, 2]) >= 5.105).sum()),
'c0_new_capped': int((abs(rows[mask, 3]) >= 5.105).sum())}
files = [source/n for n in ('route.npz', 'metadata.json', 'model_paths.npz')]
files += [reference, Path(__file__), Path(ford_model_action.__file__)]
report = {'route': label, 'baseline': BASELINE, 'baseline_sha256': old_hash, 'cycles': len(t), 'wire_round_trips': wire.count,
'c1_identical': True, 'c0_gain': new.core.c0_proportional_gain, 'metrics': metrics,
'sources': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in files}}
(dest/'report.json').write_text(json.dumps(report, indent=2)+'\n')
return report
if __name__ == '__main__':
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--output', type=Path, required=True)
parser.add_argument('--routes', nargs='+', default=ROUTES, choices=ROUTES)
parser.add_argument('--workers', type=int, default=4)
args = parser.parse_args()
with ProcessPoolExecutor(max_workers=args.workers) as pool:
jobs = {pool.submit(replay, label, args.output): label for label in args.routes}
results = []
for job in as_completed(jobs):
result = job.result()
results.append(result)
print(json.dumps({k: result[k] for k in ('route', 'cycles', 'wire_round_trips', 'metrics')}), flush=True)
(args.output/'validation.json').write_text(json.dumps({'scope': __doc__, 'routes': results}, indent=2)+'\n')