mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-15 00:03:45 +08:00
Ford: simplify C1 feedback with continuous PI unwind
This commit is contained in:
Isaac Barham
@@ -0,0 +1,82 @@
|
||||
# Ford continuous PI drive trial (v7)
|
||||
|
||||
The selected controller follows the current, upstream-limited desired curvature
|
||||
with P=0.50 and I=0.25. It replaces v6's request-change, C0-confirmation and
|
||||
unwind-catchup release rules. C0 mapping, heading overflow allocation, input
|
||||
health gates, driver/PSCM arbitration and final output limits remain unchanged.
|
||||
Only C0, C1 and I carry control history. C2/C3 stay zero.
|
||||
|
||||
```text
|
||||
D = max(7 m, speed × 1 s)
|
||||
error = selected desired curvature − measured steering-derived curvature
|
||||
base = clip(D × desired curvature, −0.5, +0.5)
|
||||
P = 0.50 × D × error
|
||||
increment = 0.25 × speed × error × fresh steering measurement interval
|
||||
C1 target = clip(base + P + I, −0.5, +0.5)
|
||||
```
|
||||
|
||||
After PSCM outward-accumulation arbitration, the increment first cancels up to
|
||||
its own magnitude of opposing I. It cannot cross zero in that step. Any remainder
|
||||
is bounded by the combined command's amplitude/slew headroom. C1 output still
|
||||
slews at 0.5 rad/s. This allows old I to unwind even when the output is already
|
||||
slewing. It adds no timer, request history, turn detection or position threshold.
|
||||
|
||||
P doubles relative to v6, and I builds at one quarter the previous rate for an
|
||||
identical error and fresh measurement interval. Lower I also unwinds more slowly
|
||||
for an identical existing state and error; its replay benefit comes mainly from
|
||||
storing less correction. Zero error removes P and holds I. Persistent tracking
|
||||
bias may need that holding correction. There is no claim that all I is unwanted.
|
||||
|
||||
## Evidence and limitations
|
||||
|
||||
Six offline settings were compared across fourteen routes and 1,578,250 source
|
||||
cycles before selecting this candidate. The production selector and adapter now
|
||||
exactly reproduce the selected P=0.50/I=0.25 archived commands, validity, P, I,
|
||||
feedforward, C0 overflow and feedback/PSCM gates on every cycle of all fourteen
|
||||
routes. These comprise Lightning 112–117, a0, a2, a5, a9, b8, b9, ca and Raptor 02.
|
||||
The integration replay performs 1,578,250 actual Float32/CAN round trips.
|
||||
|
||||
A further 20,000 randomized cycles with mirrored and independent C0 paths perform
|
||||
60,000 CAN checks on production, checking independent scalar arithmetic,
|
||||
C0-independent feedback, symmetry, reset, amplitude and slew behavior.
|
||||
The Ford, Sunnylink, params, sender and safety suite passes 679 tests and
|
||||
9,145 subtests; 178 are skipped by the platform test suite. Removed maneuver
|
||||
heuristic tests are replaced with continuous-error, cancellation, freshness,
|
||||
three-state reproduction and actual controlsd-to-CAN entry/exit checks.
|
||||
Historical untracked offline experiment tests are outside this deployment suite.
|
||||
|
||||
At a previously reviewed route-115 exit (133.595 s), the original small PI
|
||||
controller requested +0.1090 rad C1; this trial requests +0.0175. That sample
|
||||
includes driver context. Reviewed large entries remain similar, but commands
|
||||
are not identical everywhere. In a previously well-tracked route-116 bend
|
||||
(112 s), C1 falls from +0.1530 to +0.1355 rad. Lower I could weaken a persistent
|
||||
bend, while higher P can increase response to measurement fluctuations.
|
||||
|
||||
Recorded wheel motion remains fixed in replay. These checks establish software
|
||||
behavior and exact integration of the candidate; they do not establish improved
|
||||
physical tracking or stability. Gains remain experimental, not an identified
|
||||
universal PSCM calibration. No device build, boot or new drive is claimed.
|
||||
|
||||
## Selection and reproduction
|
||||
|
||||
Use the existing default-off Sunnylink **Selected-Action Path Tracking
|
||||
(Experimental)** toggle on any Ford CAN FD, followed by a real offroad-to-onroad
|
||||
cycle. Logs identify `model-action-c1-pi-v7`, `proportional_gain=0.5`,
|
||||
`integral_gain=0.25`. Toggle-off selects upstream Ford control. See the
|
||||
[drive instructions](ford_model_action_drive_test.md).
|
||||
|
||||
With the built cereal/opendbc environment and archived local extracts:
|
||||
|
||||
```sh
|
||||
export PYTHONPATH=.:opendbc_repo:.cache/ford_v6/test_deps
|
||||
export PYTHONDONTWRITEBYTECODE=1
|
||||
export PARAMS_ROOT=/tmp/ford-v7-params
|
||||
export LOG_ROOT=/tmp/ford-v7-logs
|
||||
python -m tools.ford_pscm_lab.minimal_pi_validate --output .cache/ford_minimal_tuning/production --workers 4
|
||||
python -m tools.ford_pscm_lab.minimal_pi_production_stress --cycles 20000 --output .cache/ford_minimal_tuning/production_stress.json
|
||||
```
|
||||
|
||||
The validation JSON records route and source hashes. The archived six-setting
|
||||
sweep is local evidence, not a checked-in dataset. Historical v5/v6 lab tools
|
||||
load their pinned controller revisions so their baseline comparisons retain
|
||||
their original meaning after production changes.
|
||||
@@ -0,0 +1,332 @@
|
||||
{
|
||||
"scope": "Production integration of the selected continuous PI drive trial; fixed recorded motion, not a physical tracking prediction.",
|
||||
"parent_revision": "5e6993aab",
|
||||
"previous_controller_revision": "bf00bc691def830e1beb15363d05416714c1dc42",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"control_history": [
|
||||
"c0",
|
||||
"c1",
|
||||
"correction"
|
||||
],
|
||||
"physical_lines": {
|
||||
"module": 201,
|
||||
"core_class": 58,
|
||||
"core_update": 42
|
||||
},
|
||||
"production_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
|
||||
"route_cycles": 1578250,
|
||||
"production_can_round_trips": 1638250,
|
||||
"tests": {
|
||||
"passed": 679,
|
||||
"skipped": 178,
|
||||
"subtests_passed": 9145,
|
||||
"log_sha256": "2bc3dfd7ba08d4985d3dea98f235eb1fc99b23e5bbb3927e823a2439c8e9a481",
|
||||
"scope": "Ford controls, tracked PSCM lab tests, PSCM status, Sunnylink, params, Ford car and safety suites; historical untracked experiments excluded."
|
||||
},
|
||||
"ruff": "pass",
|
||||
"ty_production": "pass",
|
||||
"routes": [
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "112",
|
||||
"cycles": 108971,
|
||||
"can_round_trips": 108971,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"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/.cache/ford_minimal_tuning/sweep/112/report.json": "d8cbcf8b676a67a90310390611da31783b4e025a0a965239ef4672caa47aae56",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/112/commands.npz": "8d91f449c7f8741ce465ec4326949a9d9527ed6b5de7a526ad7d30212f16c469",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "113",
|
||||
"cycles": 49614,
|
||||
"can_round_trips": 49614,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"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/.cache/ford_minimal_tuning/sweep/113/report.json": "e2dfeba5d3fe8a1147f82bce9ddf2ad3f14271bda35fac1639d383f2a0189228",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/113/commands.npz": "3890c802072dd2d38b4cded5f9f80310d1f86eb8a8f3598138235a0e2cc16460",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "114",
|
||||
"cycles": 61027,
|
||||
"can_round_trips": 61027,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"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/.cache/ford_minimal_tuning/sweep/114/report.json": "eb630bb5f35a43c8902e15a97b53d83db3dc3cfa843a14d09cb8a1e6dab89289",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/114/commands.npz": "899033d0c9eeffe7929aa1289bfbc0e7da5608833b03989ce10a87d975344d3d",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "115",
|
||||
"cycles": 40037,
|
||||
"can_round_trips": 40037,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"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/.cache/ford_minimal_tuning/sweep/115/report.json": "58cee499da3191808aedbd79a9497d9c1553f49007f24543c44bc2b58ff971fa",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/115/commands.npz": "09c2da33dabbc0072c61578224cd5ab27ed44690394315350c1c33708b0a67bc",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "116",
|
||||
"cycles": 68793,
|
||||
"can_round_trips": 68793,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/route.npz": "13df1d49edd0c92bd4a4236c766ee76baa6dad477824b9de4b79927c98410394",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/model_paths.npz": "85d1112b5e89340e8a955b7fe4040b3135cf1c91a54ff756826c6119cc9cc01d",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/metadata.json": "d52fe63d013fa546524ad9522a26943d9372730d397d92d75dfd9d75c63ce766",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/116/report.json": "01d2e608a53f9fd805baee853d18acc63b5506319e0f0ceeeb3e59a92bae0385",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/116/commands.npz": "2186a1a1fea2ea98390fb90c9df1288d321cd165f70e3aef0c268cbbd2fd8c95",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "117",
|
||||
"cycles": 27301,
|
||||
"can_round_trips": 27301,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/route.npz": "65e4ea6c76dce73018b636479b3777116f1e58bb9813e6c1a7b79018b219140e",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/model_paths.npz": "079f6003968287530e1ed6f2243c727e80c2ef3d16fb8a7d945d7eca744d54e7",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/metadata.json": "348396b3059c5f54d7938cd3acf2bb268a813b47ff47561da7a94cb75c3f3114",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/117/report.json": "80efcf48f018d3a887e8b9fdb36f67d9c5d9a57b09f117c8fdecd4d5556ff80b",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/117/commands.npz": "c2056b5d9faf765cb4713f73e9c4937bb24c9eeb1d4dac0d21e141322d52609e",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "a0",
|
||||
"cycles": 114582,
|
||||
"can_round_trips": 114582,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/route.npz": "01ce8b0361c3154d07d5449525a650baa2cc261675095cb1f04ec9e37a9174d3",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/model_paths.npz": "5d0c69d10a6d13882b47d950ae64c4d98f7ca2b59eab27e4e9557eef3bc4917b",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/metadata.json": "00496a11afbee475e9b5d66838a4d1663a3c0063cd562b4d1408cce6d71e586e",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a0/report.json": "5f3ca84b700e019cc72201d9e4ff85c42fbd15e9a3850434caf1bac688b332d8",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a0/commands.npz": "5c52d587ba45446117677686aabc6af81d5d2eacab5c04b02b53fe6fb95e36d0",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "a2",
|
||||
"cycles": 71111,
|
||||
"can_round_trips": 71111,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/route.npz": "af4128d278be6be0042f72a02255ddbec96a8a5aa435b9852c8202e4a5cb39e5",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/model_paths.npz": "5a5a7c4fc30c7b3bc7f2413daa8b11fe86b69dd3249e8df2d35b0a0636a49717",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/metadata.json": "cfecd240216e409a1e2591c6c2e6f0bad002ecd2a50c8f6dbf22a8f9174708da",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a2/report.json": "de61021f0c13af16e98688b17aadae9d61c99d20be82329b7cc235836defa41a",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a2/commands.npz": "fd263ab667b97905d840751577179ad75472421104cf20663e3ee51bf32dd2ee",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "a5",
|
||||
"cycles": 38961,
|
||||
"can_round_trips": 38961,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/route.npz": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/model_paths.npz": "72e014b844322df6318f6af588c27175ce68d9a37bda62b70eafca5bdfe6dfe4",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/metadata.json": "c533b06d1b7c598330b36b117bdb3624221bc4193ded8354246d5c86f36b2628",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a5/report.json": "dc6c05043c5caef6bf12b12eff69ef53d213fe8606f59865859d2c475e0aa3ee",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a5/commands.npz": "45f551b32a354a65bff585b81a89091abc5edad406b6ca100a8dcabc50c5dbe3",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "a9",
|
||||
"cycles": 286319,
|
||||
"can_round_trips": 286319,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/model_paths.npz": "7f3646ad24aa52de5982c65600813de5bbc5f7664d92a2516bbba35da6e7b240",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a9/report.json": "17c41415b9fb9fcd8d72835347a205113c32d32d955c0de68850a9764968b3e9",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a9/commands.npz": "27af5f6fb8f6e08f31d54e24c90430d30c72d31b1448bc1fbcb31f35804afe8a",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "b8",
|
||||
"cycles": 160431,
|
||||
"can_round_trips": 160431,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b8/report.json": "5c6b52222abbb95147dc756a34e62ada97c42aa550b952fd6ae7b26d8d9639fa",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b8/commands.npz": "9c158eff98009bcb039a4e7c29b97b0d3a9c67d61160a4e4766a176174de52d7",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "b9",
|
||||
"cycles": 90774,
|
||||
"can_round_trips": 90774,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"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/.cache/ford_minimal_tuning/sweep/b9/report.json": "9eeda0c24ede7a55ef8a924e0ebe0bcb0dea7a9612457af0c5aff898a828c195",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b9/commands.npz": "2dcc0865591c988d8a08671311a9631f3c4aa3ded4e90ee6f76d28b1654809ad",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "ca",
|
||||
"cycles": 327448,
|
||||
"can_round_trips": 327448,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"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/.cache/ford_minimal_tuning/sweep/ca/report.json": "025f2f49f68ec2164a10474a4c6ec0eb1b6022990e135fc38134a052a8710dff",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/ca/commands.npz": "cbe9f63955a409495f57466dc5811d3eed65af2cf096ee060d924437488c3f76",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
{
|
||||
"scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
|
||||
"route": "raptor02",
|
||||
"cycles": 132881,
|
||||
"can_round_trips": 132881,
|
||||
"production_matches_archived_trial_exactly": true,
|
||||
"opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
|
||||
"hypothesis": "model-action-c1-pi-v7",
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/route.npz": "8a4cfe53994988d053b47d9caadf21ebd064fd20e598fa05ddc2da245bd0e980",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/model_paths.npz": "d5dc6f72d91472b8f2fb1add1680cdaea1e122774ddd6d8519fd7673daf71bb5",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/metadata.json": "9602e08efc2bd784133837c4156c075bb89bad3bedd6b309f15e884ed20e338a",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/raptor02/report.json": "2c2dda2a5e4e8a764a10852cca3e5caf319b62e3d1d841ef9e1b8c11be77ebf6",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/raptor02/commands.npz": "e0440d0a4035b975a9c4a47d85335b5b0f0903fcd00dd7e7c858856deffd1249",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
}
|
||||
],
|
||||
"production_stress": {
|
||||
"cycles": 20000,
|
||||
"kp": 0.5,
|
||||
"ki": 0.25,
|
||||
"controller_updates": 60000,
|
||||
"can_round_trips": 60000,
|
||||
"seed": 20260913,
|
||||
"independent_c0_comparisons": 20000,
|
||||
"checks": "Independent scalar PI/unwind-first/anti-windup arithmetic, mirror symmetry, exact C0 independence, limits, slew, resets, CAN.",
|
||||
"source_sha256": {
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_production_stress.py": "fa9058826512498be1938797efba25666df832080fb7ad6ff18788ee4bda333d",
|
||||
"/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
|
||||
}
|
||||
},
|
||||
"limits": [
|
||||
"No hardware build, device boot or new physical drive performed.",
|
||||
"Matched offline commands do not establish improved tracking or closed-loop stability.",
|
||||
"Higher P can amplify measurement fluctuations; lower I can take longer to correct persistent error.",
|
||||
"Toggle defaults off and off selects upstream Ford control; on applies to any Ford CAN FD."
|
||||
]
|
||||
}
|
||||
@@ -1,11 +1,9 @@
|
||||
# Ford selected-action drive-test branch
|
||||
|
||||
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.
|
||||
The current v7 experiment uses [continuous C1 PI feedback](ford_c1_minimal_pi.md)
|
||||
with **P=0.50 and I=0.25**, replacing the earlier conditional-release rules.
|
||||
Only C0, C1 and accumulated error carry control history. The restored model
|
||||
mapping and [base C1 overflow allocation to C0](ford_c1_overflow.md) remain.
|
||||
It is selectable on **any Ford CAN FD vehicle**
|
||||
through the existing persistent, default-off Sunnylink
|
||||
toggle. Offline checks establish software behavior; physical tracking,
|
||||
@@ -23,19 +21,12 @@ 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-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
|
||||
amplitude and slew limits. `calibration_approved=false` remains.
|
||||
`request_release` reports correction retired on the current cycle when a changed
|
||||
request and sufficiently large measured error agree. Periodic logs can miss
|
||||
individual retirement cycles.
|
||||
`unwind_direction` remembers an unfinished unwind; `unwind_release` reports
|
||||
correction retired when a confirmed unwind catches the selected curvature.
|
||||
`heading_proportional`, `proportional_gain`, `feedback_curvature` and
|
||||
`feedback_error` separate the new P contribution and its reference from I.
|
||||
identify **`hypothesis=model-action-c1-pi-v7`**. They report desired and measured
|
||||
curvature, base heading, proportional and accumulated correction, applied heading,
|
||||
feedback timing and driver/PSCM gating. `proportional_gain=0.5` and
|
||||
`integral_gain=0.25` identify the trial. `offset_overflow` reports the extra C0
|
||||
target in meters before C0 amplitude and slew limits. `calibration_approved=false`
|
||||
remains. The retired request/unwind/reversal diagnostic fields are removed.
|
||||
|
||||
Turning the toggle off and completing another offroad-to-onroad cycle restores
|
||||
**upstream Ford curvature control**: 20 Hz steering messages, limited mode on
|
||||
@@ -54,15 +45,11 @@ 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
|
||||
and base C1, while measured curvature is still opposite. A separate bounded
|
||||
release now handles changed requests within the same turn when measured error
|
||||
also opposes the old correction. V5 additionally retires dominant unwind memory
|
||||
at catch-up when the selected request reaches zero/new-side curvature and both
|
||||
C0 requests confirm that side. Steady requests cannot arm this release, and
|
||||
catching an old-side bend retains correction. Final output slew still applies.
|
||||
See [completed-unwind release](ford_unwind_catchup.md).
|
||||
With fresh feedback, the part of the error increment that cancels existing I
|
||||
is applied before the ordinary accumulation clamp. Any remainder must fit the
|
||||
combined feedforward/P/I amplitude and slew envelope. There is no C0 confirmation
|
||||
threshold or remembered turn direction. Zero error removes P and holds I; it
|
||||
does not trigger a release. Final command limits still apply.
|
||||
|
||||
C0 starts with the original 7 m model-path mapping. When the raw base heading
|
||||
exceeds ±0.5 rad, C0 additionally receives 7 m times the clipped-away heading.
|
||||
@@ -74,8 +61,9 @@ 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 [proportional feedback trial and validation](ford_c1_pi.md) and
|
||||
`ford_c1_pi_validation.json` for current evidence and reproduction commands.
|
||||
See [continuous PI trial and validation](ford_c1_minimal_pi.md) and
|
||||
`ford_c1_minimal_pi_validation.json` for current evidence and reproduction commands.
|
||||
[Proportional feedback](ford_c1_pi.md) and its validation JSON record v6.
|
||||
[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
|
||||
|
||||
@@ -1,13 +1,8 @@
|
||||
"""Experimental Ford C2-free controller with measured-curvature C1 PI feedback.
|
||||
"""Opt-in Ford C2-free model mapping with measured-curvature 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.
|
||||
Base heading clipped by C1 is allocated to C0 at the existing 7 m reference.
|
||||
C0 samples the model at 7 m, including clipped base-heading overflow. C1
|
||||
combines the selected curvature's heading with proportional and integrated
|
||||
tracking error. Reference distance and gains are explicit trial choices.
|
||||
"""
|
||||
import math
|
||||
import struct
|
||||
@@ -20,7 +15,8 @@ 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.
|
||||
C1_PROPORTIONAL_GAIN = 0.50 # Drive-trial gains, not a learned calibration.
|
||||
C1_INTEGRAL_GAIN = 0.25
|
||||
CALIBRATION_APPROVED = False
|
||||
|
||||
|
||||
@@ -58,127 +54,62 @@ def encode_model_action(model, desired_curvature, speed):
|
||||
|
||||
|
||||
class ModelActionController:
|
||||
"""C0/C1 slew, C1 correction, last feedback request and unwind direction.
|
||||
"""Only C0 slew, C1 slew and integrated tracking error carry control history.
|
||||
|
||||
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', 'proportional_gain', 'proportional', 'feedback_curvature')
|
||||
__slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature')
|
||||
|
||||
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)
|
||||
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN):
|
||||
if not _finite(proportional_gain, integral_gain) or min(proportional_gain, integral_gain) < 0.:
|
||||
raise ValueError('PI gains must be finite and nonnegative')
|
||||
self.proportional_gain, self.integral_gain = float(proportional_gain), float(integral_gain)
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.c0 = self.c1 = self.correction = 0.
|
||||
self.carryover_release_count = 0 # Diagnostic only; never feeds the command law.
|
||||
self.last_feedback_desired = None
|
||||
self.request_release = 0. # Diagnostic radians retired on this cycle.
|
||||
self.unwind_direction = 0.
|
||||
self.unwind_release = 0. # Diagnostic only; final output still obeys slew.
|
||||
self.proportional = self.feedback_curvature = 0.
|
||||
self.c0 = self.c1 = self.correction = 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_curvature=None):
|
||||
feedback_dt = dt if feedback_dt is None else feedback_dt
|
||||
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.):
|
||||
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
|
||||
or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(reference) > 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.
|
||||
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):
|
||||
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.
|
||||
c0 = float(np.clip(target.path_offset + OFFSET_STATION_M*(target.path_angle-base_c1), -5.11, 5.11))
|
||||
self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt))
|
||||
lower = max(-.5, self.c1-.5*dt)
|
||||
upper = min(.5, self.c1+.5*dt)
|
||||
self.request_release = 0.
|
||||
self.unwind_release = 0.
|
||||
if not feedback_enabled:
|
||||
self.correction = 0.
|
||||
self.unwind_direction = 0.
|
||||
else:
|
||||
error = desired_curvature-current_curvature
|
||||
if feedback_dt > 0.:
|
||||
# Remember an unwind only when a relaxing request and excess measured
|
||||
# steering call for leaving the old turn. This may precede accumulation
|
||||
# while the output is still slewing. A steady request cannot arm it.
|
||||
previous = self.last_feedback_desired
|
||||
if (previous is not None and (desired_curvature-previous)*previous < 0.
|
||||
and current_curvature*previous > 0. and error*previous < 0.):
|
||||
self.unwind_direction = math.copysign(1., error)
|
||||
direction = self.unwind_direction
|
||||
if direction and error*direction <= 0.:
|
||||
# Steering has caught the request. Retire dominant unwind memory only
|
||||
# after the selected request crosses zero and both path offsets also
|
||||
# confirm the new side. Catching a steady old-side bend retains I.
|
||||
if (desired_curvature*direction >= 0. and self.correction*direction > abs(base_c1)
|
||||
and min(target.path_offset*direction, self.c0*direction) >= .01):
|
||||
self.unwind_release = self.correction
|
||||
self.correction = 0.
|
||||
self.unwind_direction = 0.
|
||||
# A changed target can make old correction counterproductive before
|
||||
# steering reverses. Retire at most the heading change, and only when
|
||||
# fresh measured error calls for that same change, by at least a C1 DBC
|
||||
# step. Never cross zero or discard a steady tracking correction just
|
||||
# because error changes sign.
|
||||
# Require the heading mismatch over the existing reference distance to
|
||||
# cover the correction: small tracking noise must not erode a useful I.
|
||||
# Evaluate both requests at today's speed so speed changes alone do not
|
||||
# release anything. Duplicate measurements leave this history untouched.
|
||||
if feedback_dt > 0. and self.last_feedback_desired is not None:
|
||||
distance = max(OFFSET_STATION_M, speed*HEADING_TIME_S)
|
||||
previous_base = float(np.clip(distance*self.last_feedback_desired, -.5, .5))
|
||||
change = base_c1-previous_base
|
||||
if (abs(change) >= .0005 and change*error > 0. and change*self.correction < 0.
|
||||
and abs(distance*error) >= abs(self.correction)):
|
||||
self.request_release = float(np.clip(change, min(-self.correction, 0.), max(-self.correction, 0.)))
|
||||
self.correction += self.request_release
|
||||
direction = math.copysign(1., base_c1)
|
||||
# Release only correction that prevents C1 from requesting the direction
|
||||
# shared by model C0, slewed C0 and base C1, while measured steering is
|
||||
# still opposite. One DBC step confirms each request is nonzero. Matched
|
||||
# steering, neutral/conflicting centering and duplicate samples retain I.
|
||||
if (feedback_dt > 0. and abs(base_c1) >= .0005 and current_curvature*direction < 0.
|
||||
and min(target.path_offset*direction, self.c0*direction) >= .01
|
||||
and (base_c1+self.correction)*direction <= 0.):
|
||||
self.correction = 0.
|
||||
self.carryover_release_count += 1
|
||||
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.
|
||||
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 += float(np.clip(offset-self.c0, -4.*dt, 4.*dt))
|
||||
lower, upper = max(-.5, self.c1-.5*dt), min(.5, self.c1+.5*dt)
|
||||
if feedback_enabled:
|
||||
increment = self.integral_gain*error*speed*feedback_dt
|
||||
if not _finite(increment):
|
||||
self.reset()
|
||||
return FordPath()
|
||||
direction = current_curvature if current_curvature else self.c1
|
||||
if pscm_limited and increment*direction > 0.:
|
||||
# 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.proportional+self.correction
|
||||
# Retire existing I before limiting new accumulation; never cross zero
|
||||
# through this step. The final command still obeys amplitude and slew.
|
||||
relief = float(np.clip(increment, min(-self.correction, 0.), max(-self.correction, 0.)))
|
||||
self.correction += relief
|
||||
increment -= relief
|
||||
request = base+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.proportional+self.correction, -.5, .5))
|
||||
self.c1 += float(np.clip(c1-self.c1, -.5*dt, .5*dt))
|
||||
else:
|
||||
self.correction = 0.
|
||||
request = float(np.clip(base+self.proportional+self.correction, -.5, .5))
|
||||
self.c1 += float(np.clip(request-self.c1, -.5*dt, .5*dt))
|
||||
return FordPath(True, _packed(self.c0, .01, -5.12), _packed(self.c1, .0005, -.5), 0., 0.)
|
||||
|
||||
|
||||
@@ -194,9 +125,9 @@ 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=0.):
|
||||
self.core = ModelActionController(proportional_gain=proportional_gain)
|
||||
self.hypothesis = 'model-action-c1-pi-v6' if proportional_gain else 'model-action-c1-feedback-v5'
|
||||
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN):
|
||||
self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain)
|
||||
self.hypothesis = 'model-action-c1-pi-v7'
|
||||
self.reset()
|
||||
|
||||
def reset(self, status='inactive'):
|
||||
@@ -255,11 +186,8 @@ class FordModelActionController:
|
||||
'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,
|
||||
'integral_gain': self.core.integral_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,
|
||||
'driver_override': driver_override, 'pscm_limited': pscm_limited, 'pscm_status_fresh': bool(status_fresh),
|
||||
'command': (command.path_offset, command.path_angle, 0., 0.)}
|
||||
return command
|
||||
@@ -269,5 +197,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(proportional_gain=C1_PROPORTIONAL_GAIN)
|
||||
return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN)
|
||||
return None
|
||||
|
||||
@@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase):
|
||||
controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.03, curvature=.015,
|
||||
sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
|
||||
record = self.emit_controls_event('Ford C2-free path tracking', controls)
|
||||
self.assertEqual(record['hypothesis'], 'model-action-c1-feedback-v5')
|
||||
self.assertEqual(record['hypothesis'], 'model-action-c1-pi-v7')
|
||||
self.assertIs(record['calibration_approved'], False)
|
||||
self.assertEqual(record['command'][2:], [0., 0.])
|
||||
self.assertEqual(record['status'], controller.diagnostics['status'])
|
||||
|
||||
@@ -42,14 +42,12 @@ def test_centering_information_is_independent_of_action_and_not_scaled_with_spee
|
||||
assert target.path_angle == pytest.approx(sign*.2) # No 10 m cap at highway speed.
|
||||
|
||||
|
||||
def test_five_control_states_are_sufficient_for_every_next_output():
|
||||
def test_three_control_states_are_sufficient_for_every_next_output():
|
||||
controller = ModelActionController()
|
||||
assert not hasattr(controller, '__dict__')
|
||||
for i in range(300):
|
||||
copied = ModelActionController()
|
||||
copied.c0, copied.c1, copied.correction = controller.c0, controller.c1, controller.correction
|
||||
copied.last_feedback_desired = controller.last_feedback_desired
|
||||
copied.unwind_direction = controller.unwind_direction
|
||||
model = straight(.2*math.sin(i*.1))
|
||||
kwargs = {'speed': 20., 'dt': .01}
|
||||
desired = .005*math.cos(i*.03)
|
||||
|
||||
@@ -180,9 +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 controller.core.proportional == pytest.approx(.25*20.*expected_curvature)
|
||||
assert controller.core.proportional == pytest.approx(.5*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_angle == pytest.approx((-1 if maneuver else 1)*.0035)
|
||||
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)
|
||||
@@ -239,11 +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
|
||||
# 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.)]:
|
||||
# P spends output slew headroom before new I accumulates. Existing I can
|
||||
# unwind throughout that slew. Matched steering removes P and preserves I.
|
||||
for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.0049),
|
||||
(sign*.004, 0., 100, sign*.0049), (sign*.005, 0., 100, -sign*.0001),
|
||||
(sign*.003, 0., 100, sign*.0048), (0., 1.0625, 5, 0.)]:
|
||||
for _ in range(count):
|
||||
now = 1.+frame*.01
|
||||
controls.curvature, cs.steeringTorque = measured, torque
|
||||
@@ -268,7 +268,7 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
frame += 1
|
||||
core = controls.ford_path_controller.core
|
||||
expected_p = .25*20.*(sign*.004-measured) if torque == 0. else 0.
|
||||
expected_p = .5*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)
|
||||
@@ -284,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(102):
|
||||
for frame in range(103):
|
||||
now = 1.+frame*.01
|
||||
controls.curvature = .004 if frame < 100 else .003
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
|
||||
@@ -296,16 +296,16 @@ 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 controller.core.proportional == pytest.approx(.01)
|
||||
# Two error samples spend the slew allowance on P; the third may add I.
|
||||
assert controller.core.correction == pytest.approx(0. if service_valid else .00005)
|
||||
assert cc.latActive and controls.ford_path.valid
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('maneuver', [False, True])
|
||||
@pytest.mark.parametrize('same_turn', [False, True])
|
||||
def test_carryover_release_through_selected_limited_request_and_actual_can(pipeline, sign, maneuver, same_turn):
|
||||
def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(pipeline, sign, maneuver, same_turn):
|
||||
call, publication = pipeline
|
||||
controls, sm = startup(), Subscriptions(maneuver)
|
||||
controls.sm, controls.desired_curvature = sm, sign*.004
|
||||
@@ -318,7 +318,6 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel
|
||||
vehicle = SimpleNamespace(out=structs.CarState(vEgo=speed, vEgoRaw=speed), acc_tja_status_stock_values=defaultdict(int),
|
||||
lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
|
||||
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main)
|
||||
request_releases = 0
|
||||
for frame in range(280):
|
||||
now = 1.+frame*.01
|
||||
desired = sign*(.004 if frame < 200 else .01 if same_turn else -.001)
|
||||
@@ -327,12 +326,13 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel
|
||||
sm.messages['lateralManeuverPlan'].desiredCurvature = desired
|
||||
controls.curvature = sign*(.004 if frame < 100 else .006 if same_turn else .001 if frame < 200 else .003)
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9), lateralManeuverPlan=round(now*1e9))
|
||||
before = core.c0, core.c1
|
||||
before = core.c0, core.c1, core.correction
|
||||
exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
assert abs(core.c0-before[0]) <= .0400000001 and abs(core.c1-before[1]) <= .0050000001
|
||||
request_releases += core.request_release != 0.
|
||||
increment = .25*speed*(controls.desired_curvature-controls.curvature)*.01
|
||||
assert abs(core.correction-before[2]) <= abs(increment)+1e-10
|
||||
msg = custom.CarControlSP.new_message()
|
||||
exec(publication, {'self': controls, 'CC_SP': msg})
|
||||
_, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9))
|
||||
@@ -347,26 +347,21 @@ 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:
|
||||
# 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-pi-v6'
|
||||
assert sign*core.correction < 0. if same_turn else sign*core.correction > 0.
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v7'
|
||||
if same_turn:
|
||||
assert request_releases > 0
|
||||
assert controls.desired_curvature == pytest.approx(sign*.01)
|
||||
assert sign*controls.ford_path.path_angle >= speed*.01 # No old unwind correction left below the new base.
|
||||
else:
|
||||
assert sign*controls.ford_path.path_angle < 0.
|
||||
assert controls.ford_path.path_offset == pytest.approx(sign*(.2 if same_turn else -.2))
|
||||
controls.ford_path_controller.reset()
|
||||
assert core.carryover_release_count == 0
|
||||
assert core.c0 == core.c1 == core.correction == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('maneuver', [False, True])
|
||||
def test_completed_unwind_release_through_selected_request_and_actual_can(pipeline, sign, maneuver):
|
||||
def test_unwind_and_catchup_through_selected_request_and_actual_can(pipeline, sign, maneuver):
|
||||
call, publication = pipeline
|
||||
controls, sm = startup(), Subscriptions(maneuver)
|
||||
controls.sm, controls.desired_curvature = sm, -sign*.02
|
||||
@@ -378,7 +373,6 @@ def test_completed_unwind_release_through_selected_request_and_actual_can(pipeli
|
||||
vehicle = SimpleNamespace(out=structs.CarState(vEgo=4., vEgoRaw=4.), acc_tja_status_stock_values=defaultdict(int),
|
||||
lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
|
||||
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main)
|
||||
release_count = 0
|
||||
for frame in range(180):
|
||||
now = 1.+frame*.01
|
||||
desired = -sign*(.02 if frame < 30 else .01 if frame < 80 else 0.)
|
||||
@@ -392,12 +386,12 @@ def test_completed_unwind_release_through_selected_request_and_actual_can(pipeli
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
assert abs(core.c0-before[0]) <= .0400000001 and abs(core.c1-before[1]) <= .0050000001
|
||||
release_count += core.unwind_release != 0.
|
||||
if frame == 129:
|
||||
assert sign*core.correction > .04 and core.unwind_direction == sign
|
||||
assert 0. < sign*core.correction < .02
|
||||
if frame == 130:
|
||||
assert core.unwind_release == before[2] and core.correction == 0.
|
||||
assert core.c1 == pytest.approx(before[1]-sign*.005)
|
||||
# Zero measured error removes P, but does not arbitrarily erase I.
|
||||
assert core.correction == before[2]
|
||||
assert core.proportional == 0.
|
||||
msg = custom.CarControlSP.new_message()
|
||||
exec(publication, {'self': controls, 'CC_SP': msg})
|
||||
_, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9))
|
||||
@@ -411,8 +405,8 @@ def test_completed_unwind_release_through_selected_request_and_actual_can(pipeli
|
||||
assert wire['LatCtl_D2_Rq'] == 2 and wire['LatCtlPath_No_Cnt'] == frame % 16
|
||||
packet = next(packet for packet in packets if packet[0] == address)
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
assert release_count == 1
|
||||
assert controls.ford_path.path_angle == core.correction == 0.
|
||||
assert controls.ford_path.path_angle == pytest.approx(core.correction, abs=.00025)
|
||||
assert 0. < sign*core.correction < .02
|
||||
assert controls.ford_path.path_offset == pytest.approx(sign*.05)
|
||||
|
||||
|
||||
@@ -438,7 +432,7 @@ def test_heading_overflow_and_release_through_actual_can(pipeline, sign, fingerp
|
||||
# Match the selected request after its real upstream limiter, isolating base allocation.
|
||||
controls.curvature = clip_curvature(cs.vEgo, controls.desired_curvature, desired, 0.)[0]
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
|
||||
before = core.c0, core.c1
|
||||
before = core.c0, core.c1, core.correction
|
||||
exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
|
||||
@@ -15,117 +15,9 @@ def tick(controller, desired, measured, **overrides):
|
||||
return controller.update(straight(.4), desired, **kwargs)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_old_turn_correction_does_not_keep_c1_in_old_direction_after_reversal(sign):
|
||||
controller = ModelActionController()
|
||||
# Build an actual correction through feedback, rather than injecting a state.
|
||||
for _ in range(100):
|
||||
controller.update(straight(), sign*.004, current_curvature=sign*.004, speed=20., dt=.01)
|
||||
for _ in range(100):
|
||||
controller.update(straight(), sign*.004, current_curvature=sign*.001, speed=20., dt=.01)
|
||||
assert controller.correction == pytest.approx(sign*.06)
|
||||
# The request has reversed, but measured steering still points into the old
|
||||
# turn. C0 also confirms the new direction. The existing slew permits
|
||||
# crossing zero within these 0.4 seconds.
|
||||
for _ in range(40):
|
||||
before = controller.c1
|
||||
out = controller.update(straight(-sign*.2), -sign*.001, current_curvature=sign*.003, speed=20., dt=.01)
|
||||
assert abs(controller.c1-before) <= .0050000001
|
||||
assert sign*out.path_angle < 0., 'Stored old-turn correction still overrides the new C1 direction'
|
||||
assert out.path_offset == pytest.approx(-sign*.2)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('limited', [False, True])
|
||||
def test_carryover_release_uses_fresh_feedback_and_preserves_final_slew(sign, limited):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = -sign*.2, sign*.03, sign*.05
|
||||
kwargs = {'speed': 20., 'dt': .01, 'current_curvature': sign*.003, 'pscm_limited': limited}
|
||||
controller.update(straight(-sign*.2), -sign*.001, feedback_dt=0., **kwargs)
|
||||
assert controller.correction == pytest.approx(sign*.05)
|
||||
before = controller.c1
|
||||
out = controller.update(straight(-sign*.2), -sign*.001, **kwargs)
|
||||
assert controller.correction == 0.
|
||||
assert controller.carryover_release_count == 1
|
||||
assert out.path_angle == pytest.approx(before-sign*.005)
|
||||
for _ in range(30):
|
||||
controller.update(straight(-sign*.2), -sign*.001, **kwargs)
|
||||
assert controller.carryover_release_count == 1
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('case', ['matched', 'same_turn', 'already_turning_new_way', 'c0_opposes', 'c0_neutral',
|
||||
'c0_sub_resolution', 'target_c0_opposes', 'c1_sub_resolution',
|
||||
'neutral_request', 'neutral_measurement', 'correction_helps', 'correction_not_dominant'])
|
||||
def test_carryover_release_preserves_steady_correction_and_ambiguous_requests(sign, case):
|
||||
controller = ModelActionController()
|
||||
offset, desired, measured, correction = -.2, -.001, .003, .05
|
||||
if case == 'matched':
|
||||
measured = desired
|
||||
elif case == 'same_turn':
|
||||
measured = -.0005
|
||||
elif case == 'already_turning_new_way':
|
||||
measured = -.003
|
||||
elif case == 'c0_opposes':
|
||||
offset = .2
|
||||
elif case == 'c0_neutral':
|
||||
offset = 0.
|
||||
elif case == 'c0_sub_resolution':
|
||||
offset = -.001
|
||||
elif case == 'c1_sub_resolution':
|
||||
desired = -.000001
|
||||
elif case == 'neutral_request':
|
||||
desired = 0.
|
||||
elif case == 'neutral_measurement':
|
||||
measured = 0.
|
||||
elif case == 'correction_helps':
|
||||
correction = -.05
|
||||
elif case == 'correction_not_dominant':
|
||||
correction = .01
|
||||
controller.c0, controller.c1, controller.correction = sign*offset, sign*(20.*desired+correction), sign*correction
|
||||
if case == 'target_c0_opposes':
|
||||
offset = .2 # The old slewed C0 alone is not enough to confirm the request.
|
||||
controller.update(straight(sign*offset), sign*desired, current_curvature=sign*measured, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == 0
|
||||
assert abs(controller.correction-sign*correction) <= abs(desired-measured)*20.*.01+1e-10
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_carryover_release_waits_for_c0_to_finish_opposing_the_new_request(sign):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = sign*.2, sign*.03, sign*.05
|
||||
for _ in range(4):
|
||||
controller.update(straight(-sign*.2), -sign*.001, current_curvature=sign*.003, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == 0
|
||||
for _ in range(3):
|
||||
controller.update(straight(-sign*.2), -sign*.001, current_curvature=sign*.003, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == 1
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_steady_opposing_correction_is_preserved_through_small_error_crossings(sign):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = -sign*.2, sign*.03, sign*.05
|
||||
for i in range(200):
|
||||
measured = sign*(-.001+(-1 if i % 2 else 1)*.000001)
|
||||
controller.update(straight(-sign*.2), -sign*.001, current_curvature=measured, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == 0
|
||||
assert controller.correction == pytest.approx(sign*.05)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('offset', [.009999, .01, .010001])
|
||||
@pytest.mark.parametrize('heading', [.0004999, .0005, .0005001])
|
||||
def test_carryover_direction_confirmation_uses_existing_dbc_steps(sign, offset, heading):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = -sign*offset, sign*.03, sign*.05
|
||||
controller.update(straight(-sign*offset), -sign*heading/20., current_curvature=sign*.003, speed=20., dt=.01)
|
||||
assert controller.carryover_release_count == int(offset >= .01 and heading >= .0005)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign):
|
||||
controller, matched = ModelActionController(), ModelActionController()
|
||||
controller, matched = ModelActionController(proportional_gain=0., integral_gain=1.), ModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
for _ in range(100):
|
||||
tick(controller, sign*.004, sign*.004)
|
||||
for _ in range(100):
|
||||
@@ -147,7 +39,7 @@ def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign):
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_amplitude_and_slew_limits_do_not_store_unavailable_feedback(sign):
|
||||
controller = ModelActionController()
|
||||
controller = ModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
# The unchanged model request is already ahead of the output slew.
|
||||
for _ in range(10):
|
||||
tick(controller, sign*.01, 0.)
|
||||
@@ -169,7 +61,7 @@ def test_amplitude_and_slew_limits_do_not_store_unavailable_feedback(sign):
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_pscm_limit_only_blocks_feedback_further_into_measured_turn(sign):
|
||||
controller = ModelActionController()
|
||||
controller = ModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
for _ in range(100):
|
||||
tick(controller, sign*.004, sign*.004)
|
||||
for _ in range(100):
|
||||
@@ -186,7 +78,7 @@ def test_pscm_limit_only_blocks_feedback_further_into_measured_turn(sign):
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_pscm_limit_cannot_trap_old_correction_below_the_model_request(sign):
|
||||
controller = ModelActionController()
|
||||
controller = ModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
controller.correction = -sign*.02
|
||||
controller.c1 = sign*.06
|
||||
for _ in range(200):
|
||||
@@ -196,7 +88,7 @@ def test_pscm_limit_cannot_trap_old_correction_below_the_model_request(sign):
|
||||
|
||||
|
||||
def test_driver_intervention_clears_feedback_through_existing_output_slew():
|
||||
controller = ModelActionController()
|
||||
controller = ModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
for _ in range(100):
|
||||
tick(controller, .004, .004)
|
||||
for _ in range(100):
|
||||
@@ -216,7 +108,7 @@ def test_driver_intervention_clears_feedback_through_existing_output_slew():
|
||||
('current_curvature', 1.01), ('feedback_dt', math.nan),
|
||||
('feedback_dt', -.001), ('feedback_dt', .151), ('active', False)])
|
||||
def test_bad_feedback_inputs_and_disengagement_clear_every_control_state(field, value):
|
||||
controller = ModelActionController()
|
||||
controller = ModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
controller.correction = .03
|
||||
out = tick(controller, .004, .003, **{field: value})
|
||||
assert out == FordPath()
|
||||
@@ -237,7 +129,7 @@ def status(now, **overrides):
|
||||
|
||||
|
||||
def test_repeated_steering_samples_only_advance_output_slew():
|
||||
controller = FordModelActionController()
|
||||
controller = FordModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
for i in range(100):
|
||||
adapter_tick(controller, 1.+i*.01, current_curvature=.004)
|
||||
before = controller.core.correction
|
||||
@@ -255,7 +147,7 @@ def test_repeated_steering_samples_only_advance_output_slew():
|
||||
{'pscm_status': status(2.01, denied=True)},
|
||||
{'pscm_status': status(2.01, lateralState=1)}])
|
||||
def test_adapter_clears_feedback_when_driver_or_pscm_overrides(overrides):
|
||||
controller = FordModelActionController()
|
||||
controller = FordModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
for i in range(101):
|
||||
adapter_tick(controller, 1.+i*.01)
|
||||
assert controller.core.correction > 0.
|
||||
@@ -268,7 +160,7 @@ def test_adapter_clears_feedback_when_driver_or_pscm_overrides(overrides):
|
||||
({'canMonoTime': 0}, False), ({'canMonoTime': 1_800_000_000}, False),
|
||||
({'canMonoTime': 2_020_000_000}, False), ({'limit': 1}, False)])
|
||||
def test_only_fresh_reached_pscm_limit_blocks_outward_integration(overrides, limited):
|
||||
controller = FordModelActionController()
|
||||
controller = FordModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
for i in range(100):
|
||||
adapter_tick(controller, 1.+i*.01, current_curvature=.004)
|
||||
adapter_tick(controller, 2., pscm_status=status(2., **{'limit': 2, **overrides}))
|
||||
@@ -279,7 +171,7 @@ def test_only_fresh_reached_pscm_limit_blocks_outward_integration(overrides, lim
|
||||
def test_measurement_cadence_preserves_elapsed_distance_integration():
|
||||
results = []
|
||||
for period in (1, 2, 5):
|
||||
controller = FordModelActionController()
|
||||
controller = FordModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
for i in range(101):
|
||||
now = 1.+i*.01
|
||||
adapter_tick(controller, now, current_curvature=.004)
|
||||
|
||||
@@ -59,7 +59,7 @@ def test_extra_offset_releases_at_existing_slew_without_stored_overflow(sign):
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_c1_feedback_saturation_does_not_spill_correction_into_c0(sign):
|
||||
controller = ModelActionController()
|
||||
controller = ModelActionController(proportional_gain=0., integral_gain=1.)
|
||||
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)
|
||||
@@ -77,14 +77,3 @@ def test_overflow_is_base_geometry_with_existing_feedback_gates(sign, enabled, l
|
||||
assert out.path_offset == pytest.approx(sign*.9)
|
||||
assert out.path_angle == pytest.approx(sign*.5)
|
||||
assert controller.correction == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_overflow_cannot_replace_model_centering_confirmation_for_carryover_release(sign):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = sign*.6, -sign*.1, -sign*.6
|
||||
for _ in range(20):
|
||||
controller.update(straight(-sign*.1), sign*.03, current_curvature=-sign*.002, speed=20., dt=.01)
|
||||
assert sign*controller.c0 > 0. # Overflow agrees with heading; model centering still opposes it.
|
||||
assert controller.carryover_release_count == 0
|
||||
assert sign*controller.correction < -.4
|
||||
|
||||
@@ -1,129 +0,0 @@
|
||||
"""Changed requests should not wait for obsolete C1 correction to integrate away.
|
||||
|
||||
These exercise command delay, not a simulated PSCM or predicted wheel angle.
|
||||
"""
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('unwind', [False, True])
|
||||
def test_changed_request_retires_opposing_correction_within_output_slew(sign, unwind):
|
||||
controller = ModelActionController()
|
||||
desired = sign*.035
|
||||
model = straight(sign*.4)
|
||||
for _ in range(100):
|
||||
controller.update(model, desired, current_curvature=desired, speed=4., dt=.01)
|
||||
# Build old unwind (or turn-in) correction while retaining the same request.
|
||||
old_error = sign*(.01 if unwind else -.01)
|
||||
for _ in range(50):
|
||||
controller.update(model, desired, current_curvature=desired-old_error, speed=4., dt=.01)
|
||||
assert controller.correction == pytest.approx(old_error*2.)
|
||||
# The model now moves in the opposite direction to the stored correction.
|
||||
# Actual steering is on the other side of the new target: the old correction
|
||||
# is delaying exactly the response now needed. Both remain in the same turn.
|
||||
changed = desired-old_error
|
||||
before = controller.c1
|
||||
for _ in range(20):
|
||||
out = controller.update(model, changed, current_curvature=desired, speed=4., dt=.01)
|
||||
assert abs(controller.c1-before) <= .100000001
|
||||
assert out.path_offset == pytest.approx(sign*.4)
|
||||
assert (out.path_angle-7.*changed)*old_error <= 1e-10, 'Old correction still opposes the changed model request'
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('case', ['unchanged', 'matched', 'still_short', 'helpful', 'small_error', 'small_change', 'speed_only', 'clipped_base'])
|
||||
def test_request_release_preserves_correction_without_confirming_evidence(sign, case):
|
||||
controller = ModelActionController()
|
||||
previous, desired, measured, correction, speed = .035, .025, .035, .02, 4.
|
||||
if case == 'unchanged':
|
||||
desired = previous
|
||||
measured = .05
|
||||
elif case == 'matched':
|
||||
measured = desired
|
||||
elif case == 'still_short':
|
||||
measured = .015
|
||||
elif case == 'helpful':
|
||||
correction = -.02
|
||||
elif case == 'small_error':
|
||||
measured = desired+.00001
|
||||
elif case == 'small_change':
|
||||
desired, measured = previous-.00001, .05
|
||||
elif case == 'speed_only':
|
||||
desired, measured, speed = previous, .05, 10.
|
||||
elif case == 'clipped_base':
|
||||
previous, desired, measured = .1, .09, .11
|
||||
controller.c0, controller.c1, controller.correction = sign*.4, sign*.2, sign*correction
|
||||
controller.last_feedback_desired = sign*previous
|
||||
controller.update(straight(sign*.4), sign*desired, current_curvature=sign*measured, speed=speed, dt=.01)
|
||||
assert controller.request_release == 0.
|
||||
assert abs(controller.correction-sign*correction) <= abs(desired-measured)*speed*.01+1e-10
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_small_request_change_cannot_erase_the_entire_correction(sign):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = sign*.4, sign*.265, sign*.02
|
||||
controller.last_feedback_desired = sign*.035
|
||||
controller.update(straight(sign*.4), sign*.034, current_curvature=sign*.05, speed=4., dt=.01)
|
||||
assert sign*controller.correction > .01 # Most of the original 0.02 rad must survive.
|
||||
assert 0. < -sign*controller.request_release <= .0070000001
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_small_target_and_measurement_noise_cannot_erase_steady_correction(sign):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = sign*.4, sign*.265, sign*.02
|
||||
controller.last_feedback_desired = sign*.035
|
||||
for i in range(1000):
|
||||
desired = sign*(.035+(.00001 if i % 2 else -.00001))
|
||||
measured = sign*.035
|
||||
controller.update(straight(sign*.4), desired, current_curvature=measured, speed=4., dt=.01)
|
||||
assert controller.request_release == 0.
|
||||
assert controller.correction == pytest.approx(sign*.02)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('limited', [False, True])
|
||||
def test_retiring_correction_at_pscm_limit_never_builds_extra_turn_demand(sign, limited):
|
||||
controller = ModelActionController()
|
||||
controller.c0, controller.c1, controller.correction = sign*.4, sign*.225, -sign*.02
|
||||
controller.last_feedback_desired = sign*.035
|
||||
controller.update(straight(sign*.4), sign*.045, current_curvature=sign*.035, speed=4., dt=.01, pscm_limited=limited)
|
||||
assert controller.correction == 0. # Slew/limit still prevents a new outward increment here.
|
||||
assert controller.request_release == pytest.approx(sign*.02)
|
||||
assert controller.c1 == pytest.approx(sign*.23)
|
||||
|
||||
|
||||
def test_adapter_retires_request_once_per_fresh_measurement_and_reset_clears_history():
|
||||
controller = FordModelActionController()
|
||||
def tick(now, desired, measured, stamp, **kwargs):
|
||||
return controller.update(straight(.4), desired, current_curvature=measured, speed=4., yaw_rate=0., now=now,
|
||||
measurement_time=stamp, model_time=now, reference_time=now, active=True, **kwargs)
|
||||
for i in range(150):
|
||||
now = 1.+i*.01
|
||||
tick(now, .035, .035 if i < 100 else .025, now)
|
||||
before = controller.core.correction
|
||||
tick(2.50, .025, .035, 2.49)
|
||||
assert controller.core.correction == before
|
||||
assert controller.core.last_feedback_desired == .035
|
||||
assert controller.diagnostics['request_release'] == 0.
|
||||
tick(2.51, .025, .035, 2.51)
|
||||
assert controller.diagnostics['request_release'] == pytest.approx(-before)
|
||||
assert controller.core.last_feedback_desired == .025
|
||||
tick(2.52, .025, .035, 2.51)
|
||||
assert controller.diagnostics['request_release'] == 0.
|
||||
tick(2.53, .025, .035, 2.53, valid=False)
|
||||
assert controller.core.last_feedback_desired is None
|
||||
assert controller.core.request_release == 0.
|
||||
|
||||
|
||||
def test_driver_override_clears_correction_and_cannot_leave_a_pending_release():
|
||||
controller = ModelActionController()
|
||||
controller.correction, controller.last_feedback_desired = .02, .035
|
||||
controller.update(straight(.4), .025, current_curvature=.035, speed=4., dt=.01, feedback_enabled=False)
|
||||
assert controller.correction == controller.request_release == 0.
|
||||
controller.update(straight(.4), .025, current_curvature=.035, speed=4., dt=.01)
|
||||
assert controller.request_release == 0.
|
||||
@@ -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 C1_PROPORTIONAL_GAIN, FordModelActionController, select_model_action_controller
|
||||
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
|
||||
|
||||
|
||||
@@ -46,8 +46,9 @@ 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'
|
||||
assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .50
|
||||
assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == .25
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v7'
|
||||
else:
|
||||
assert selected.ford_path_controller is None
|
||||
assert selected.ford_model_action == candidate
|
||||
|
||||
@@ -1,127 +1,78 @@
|
||||
"""Unwind correction must not become a new turn after a confirmed catch-up.
|
||||
import math
|
||||
|
||||
These reproduce controller memory, not the PSCM's physical steering response.
|
||||
"""
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
|
||||
|
||||
|
||||
def unwind(controller, sign):
|
||||
for _ in range(30):
|
||||
controller.update(straight(-sign*.5), -sign*.02, current_curvature=-sign*.02, speed=4., dt=.01)
|
||||
# Selected curvature relaxes, but the wheel is still too far into the old
|
||||
# turn. Feedback builds the same opposite-direction correction seen in 115.
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_existing_integral_can_unwind_while_output_slews(sign):
|
||||
core = ModelActionController(.5, .25)
|
||||
core.c1, core.correction = sign*.07, sign*.03
|
||||
core.update(straight(), sign*.002, current_curvature=sign*.004, speed=20., dt=.01)
|
||||
assert core.correction == pytest.approx(sign*.0299)
|
||||
assert core.c1 == pytest.approx(sign*.065)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_unwinding_cannot_charge_opposite_correction_through_a_slew_limit(sign):
|
||||
core = ModelActionController(.5, .25)
|
||||
core.c1, core.correction = sign*.07, sign*.03
|
||||
core.update(straight(), 0., current_curvature=sign*.5, speed=20., dt=.01, feedback_dt=.15)
|
||||
assert core.correction == 0.
|
||||
assert core.c1 == pytest.approx(sign*.065)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('ki', [0., .25, .5, 1.])
|
||||
def test_integral_gain_scales_fresh_error_only(ki):
|
||||
core = ModelActionController(0., ki)
|
||||
core.c1 = .04
|
||||
core.update(straight(), .002, current_curvature=.001, speed=20., dt=.01)
|
||||
assert core.correction == pytest.approx(ki*.0002)
|
||||
before = core.correction
|
||||
core.update(straight(), 0., current_curvature=.001, speed=20., dt=.01, feedback_dt=0.)
|
||||
assert core.correction == before
|
||||
|
||||
|
||||
def test_zero_error_does_not_erase_holding_correction():
|
||||
core = ModelActionController(.5, .25)
|
||||
core.c1, core.correction = .14, .1
|
||||
for _ in range(50):
|
||||
controller.update(straight(-sign*.5), -sign*.01, current_curvature=-sign*.04, speed=4., dt=.01)
|
||||
assert sign*controller.correction > .04
|
||||
assert controller.unwind_direction == sign
|
||||
for _ in range(30):
|
||||
controller.update(straight(sign*.05), 0., current_curvature=-sign*.01, speed=4., dt=.01)
|
||||
assert sign*controller.correction > .04
|
||||
core.update(straight(), .002, current_curvature=.002, speed=20., dt=.01)
|
||||
assert core.correction == .1
|
||||
|
||||
|
||||
@pytest.mark.parametrize('ki', [-1., math.inf, math.nan])
|
||||
def test_invalid_integral_gain_is_rejected(ki):
|
||||
with pytest.raises(ValueError):
|
||||
ModelActionController(.25, ki)
|
||||
|
||||
|
||||
def test_overflowing_integral_increment_resets():
|
||||
core = ModelActionController(.25, 1e308)
|
||||
assert not core.update(straight(), 1., current_curvature=0., speed=55., dt=.01).valid
|
||||
assert core.c0 == core.c1 == core.correction == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('requested', [0., .0001])
|
||||
def test_completed_unwind_does_not_keep_requesting_a_new_turn(sign, requested):
|
||||
controller = ModelActionController()
|
||||
unwind(controller, sign)
|
||||
before = controller.c1
|
||||
# The model path now confirms the unwind direction and actual steering has
|
||||
# caught the near-zero/new-direction request. Allow the original output slew
|
||||
# to finish; the old unwind correction must no longer prop up C1.
|
||||
for _ in range(20):
|
||||
out = controller.update(straight(sign*.05), sign*requested, current_curvature=sign*requested, speed=4., dt=.01)
|
||||
assert abs(controller.c1-before) <= .100000001
|
||||
assert out.path_offset == pytest.approx(sign*.05)
|
||||
assert controller.correction == 0., 'Stored unwind correction remains after catch-up'
|
||||
assert abs(out.path_angle-7.*sign*requested) <= .000500001
|
||||
assert controller.unwind_direction == 0.
|
||||
def test_centering_cannot_gate_continuous_heading_correction(sign):
|
||||
commands = []
|
||||
for offset in (-.1, -.010001, -.01, -.009999, 0., .009999, .01, .010001, .1):
|
||||
core = ModelActionController()
|
||||
core.c0, core.c1, core.correction = offset, sign*.07, sign*.03
|
||||
out = core.update(straight(offset), sign*.002, current_curvature=sign*.004, speed=20., dt=.01)
|
||||
commands.append((out.path_angle, core.correction))
|
||||
assert len(set(commands)) == 1
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('case', ['not_caught', 'old_side_request', 'model_c0_old', 'model_c0_neutral', 'slewed_c0_old',
|
||||
'correction_not_dominant', 'opposite_correction', 'no_unwind', 'duplicate'])
|
||||
def test_unwind_release_requires_catchup_and_confirmed_new_direction(sign, case):
|
||||
controller = ModelActionController()
|
||||
unwind(controller, sign)
|
||||
desired = measured = 0.
|
||||
offset, feedback_dt = sign*.05, .01
|
||||
if case == 'not_caught':
|
||||
measured = -sign*.005
|
||||
elif case == 'old_side_request':
|
||||
desired = measured = -sign*.001
|
||||
elif case == 'model_c0_old':
|
||||
offset = -sign*.05
|
||||
elif case == 'model_c0_neutral':
|
||||
offset = 0.
|
||||
elif case == 'slewed_c0_old':
|
||||
controller.c0 = -sign*.5
|
||||
elif case == 'correction_not_dominant':
|
||||
desired = measured = sign*.02
|
||||
elif case == 'opposite_correction':
|
||||
controller.correction = -sign*.01
|
||||
elif case == 'no_unwind':
|
||||
controller.unwind_direction = 0.
|
||||
elif case == 'duplicate':
|
||||
feedback_dt = 0.
|
||||
before = controller.correction
|
||||
controller.update(straight(offset), desired, current_curvature=measured, speed=4., dt=.01, feedback_dt=feedback_dt)
|
||||
assert controller.unwind_release == 0.
|
||||
assert abs(controller.correction-before) <= abs(desired-measured)*4.*feedback_dt+1e-10
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('desired', [0., .004])
|
||||
def test_steady_tracking_correction_survives_matched_steering_and_noise(sign, desired):
|
||||
controller = ModelActionController()
|
||||
controller.correction, controller.c1, controller.c0 = sign*.05, sign*(7.*desired+.05), sign*.05
|
||||
for i in range(300):
|
||||
measured = sign*(desired+(1 if i % 2 else -1)*.000001)
|
||||
controller.update(straight(sign*.05), sign*desired, current_curvature=measured, speed=4., dt=.01)
|
||||
assert controller.unwind_direction == controller.unwind_release == 0.
|
||||
assert controller.correction == pytest.approx(sign*.05)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('limited', [False, True])
|
||||
def test_catchup_release_uses_existing_slew_and_is_consumed_once(limited):
|
||||
controller = ModelActionController()
|
||||
unwind(controller, 1.)
|
||||
before, correction = controller.c1, controller.correction
|
||||
controller.update(straight(.05), 0., current_curvature=.001, speed=4., dt=.01, pscm_limited=limited)
|
||||
assert controller.unwind_release == correction
|
||||
assert controller.c1 == pytest.approx(before-.005)
|
||||
assert controller.correction <= 0. # Remaining feedback can only correct the overshoot.
|
||||
assert controller.unwind_direction == 0.
|
||||
controller.update(straight(.05), 0., current_curvature=.001, speed=4., dt=.01)
|
||||
assert controller.unwind_release == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('override', ['driver', 'invalid', 'inactive'])
|
||||
def test_unwind_history_is_cleared_by_override_and_reset(override):
|
||||
controller = ModelActionController()
|
||||
unwind(controller, 1.)
|
||||
kwargs = {'driver': {'feedback_enabled': False}, 'invalid': {'valid': False}, 'inactive': {'active': False}}[override]
|
||||
controller.update(straight(.05), 0., current_curvature=0., speed=4., dt=.01, **kwargs)
|
||||
assert controller.unwind_direction == controller.unwind_release == controller.correction == 0.
|
||||
|
||||
|
||||
def test_duplicate_adapter_measurement_cannot_consume_unwind_until_fresh_catchup():
|
||||
controller = FordModelActionController()
|
||||
for frame in range(130):
|
||||
now = 1.+frame*.01
|
||||
desired = -.02 if frame < 30 else -.01 if frame < 80 else 0.
|
||||
measured = -.02 if frame < 30 else -.04 if frame < 80 else -.01
|
||||
controller.update(straight(-.5 if frame < 80 else .05), desired, current_curvature=measured,
|
||||
speed=4., yaw_rate=0., now=now, measurement_time=now, model_time=now, reference_time=now, active=True)
|
||||
assert controller.core.unwind_direction == 1.
|
||||
before = controller.core.correction
|
||||
for now, stamp in [(2.30, 2.29), (2.31, 2.31), (2.32, 2.31)]:
|
||||
controller.update(straight(.05), 0., current_curvature=0., speed=4., yaw_rate=0., now=now,
|
||||
measurement_time=stamp, model_time=now, reference_time=now, active=True)
|
||||
if now == 2.30:
|
||||
assert controller.core.correction == before and controller.core.unwind_direction == 1.
|
||||
else:
|
||||
assert controller.core.correction == controller.core.unwind_direction == 0.
|
||||
assert controller.diagnostics['unwind_release'] == (before if now == 2.31 else 0.)
|
||||
def test_duplicate_measurements_cannot_retire_integral(sign, limited):
|
||||
core = ModelActionController()
|
||||
core.c1, core.correction = sign*.07, sign*.03
|
||||
core.update(straight(), -sign*.002, current_curvature=sign*.004, speed=20., dt=.01,
|
||||
feedback_dt=0., pscm_limited=limited)
|
||||
assert core.correction == sign*.03
|
||||
assert core.proportional == pytest.approx(-sign*.06)
|
||||
assert core.c1 == pytest.approx(sign*.065)
|
||||
|
||||
@@ -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 C1_PROPORTIONAL_GAIN, FordModelActionController, ModelActionController
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import C1_INTEGRAL_GAIN, C1_PROPORTIONAL_GAIN, FordModelActionController
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency
|
||||
|
||||
|
||||
@@ -89,7 +89,7 @@ def replay(directory, output, baseline_revision=BASELINE):
|
||||
feedback = {'current_curvature': c['measured'][i], 'driver_pressed': bool(cs['pressed'][i]),
|
||||
'driver_torque': cs['torque'][i], 'pscm_status': status}
|
||||
previous = old.update(model, c['desired'][i], **common, **(feedback if old_has_feedback else {}))
|
||||
previous_count, previous_correction = controller.core.carryover_release_count, controller.core.correction
|
||||
previous_count, previous_correction = getattr(controller.core, 'carryover_release_count', 0), controller.core.correction
|
||||
command = controller.update(model, c['desired'][i], **feedback, **common)
|
||||
for destination, result in ((baseline, previous), (commands, command)):
|
||||
destination[i] = result.path_offset, result.path_angle, result.curvature, result.curvature_rate
|
||||
@@ -111,7 +111,7 @@ def replay(directory, output, baseline_revision=BASELINE):
|
||||
'correction_after_rad': float(correction[i]), 'base_c1_rad': float(d['heading_feedforward']),
|
||||
'desired_angle_deg': float(c['desired_angle'][i]), 'actual_angle_deg': float(c['actual_angle'][i]),
|
||||
'baseline_c0_c1': baseline[i, :2].tolist(), 'candidate_c0_c1': commands[i, :2].tolist()})
|
||||
if controller.core.carryover_release_count > previous_count:
|
||||
if getattr(controller.core, 'carryover_release_count', 0) > previous_count:
|
||||
releases.append({'time_s': float(now-metadata['t0']), 'correction_before_rad': float(previous_correction),
|
||||
'correction_after_rad': float(correction[i]), 'base_c1_rad': float(d['heading_feedforward']),
|
||||
'desired_angle_deg': float(c['desired_angle'][i]), 'actual_angle_deg': float(c['actual_angle'][i]),
|
||||
@@ -124,7 +124,8 @@ 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,
|
||||
'proportional_gain': C1_PROPORTIONAL_GAIN, 'integral_gain': C1_INTEGRAL_GAIN,
|
||||
'hypothesis': controller.hypothesis,
|
||||
'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])),
|
||||
@@ -170,7 +171,10 @@ def replay(directory, output, baseline_revision=BASELINE):
|
||||
def stress(cycles, output):
|
||||
verify_dependency(OPENDBC)
|
||||
rng = np.random.default_rng(20260913)
|
||||
controller, mirror, wire = ModelActionController(), ModelActionController(), WireCheck()
|
||||
# This historical stress compares v5's conditional release with v4.
|
||||
candidate_revision = '22d188776cb557acea459a1fca70812bdb2df46c'
|
||||
candidate, candidate_hash = original_controller(candidate_revision)
|
||||
controller, mirror, wire = candidate.core, type(candidate.core)(), WireCheck()
|
||||
old, baseline_hash = original_controller(FEEDBACK_V4)
|
||||
releases = 0
|
||||
request_releases = 0
|
||||
@@ -263,10 +267,11 @@ def stress(cycles, output):
|
||||
'request_release_cycles': request_releases,
|
||||
'unwind_release_cycles': unwind_releases,
|
||||
'exact_unchanged_state_and_commands_without_unwind_release': unchanged_without_unwind_release,
|
||||
'candidate_revision': candidate_revision, 'candidate_source_sha256': candidate_hash,
|
||||
'exact_v4_match_after_only_declared_retirement': cycles,
|
||||
'checks': 'Symmetry, resets, amplitude/slew, bounded retirement, carryover confirmation, integration, PSCM limits, CAN.',
|
||||
'scope': 'Numerical software invariants only; no model of vehicle motion.', 'calibration_approved': False,
|
||||
'controller_sha256': hashlib.sha256(Path(ford_model_action.__file__).read_bytes()).hexdigest()}
|
||||
'controller_sha256': candidate_hash}
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
output.write_text(json.dumps(report, indent=2)+'\n')
|
||||
print(json.dumps(report, indent=2))
|
||||
|
||||
@@ -0,0 +1,83 @@
|
||||
"""Independent arithmetic and C0-independence stress for production PI."""
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.controls.lib import ford_model_action
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
|
||||
|
||||
|
||||
def stress(cycles, output):
|
||||
wire = WireCheck()
|
||||
kp, ki = .5, .25
|
||||
rng = np.random.default_rng(20260913)
|
||||
core, mirror, other = (ModelActionController() for _ in range(3))
|
||||
for i in range(cycles):
|
||||
desired, measured = rng.uniform(-.1, .1, 2)
|
||||
speed, dt, offset, alternate = rng.uniform(.3, 55.), rng.uniform(.002, .1), *rng.uniform(-8., 8., 2)
|
||||
active, enabled, limited = i % 211 != 0, i % 97 != 0, i % 7 == 0
|
||||
feedback_dt = 0. if i % 5 == 0 else rng.uniform(.002, .15)
|
||||
before = core.c0, core.c1, core.correction
|
||||
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.]))
|
||||
|
||||
outputs = [core.update(model(offset), desired, current_curvature=measured, **args),
|
||||
mirror.update(model(-offset), -desired, current_curvature=-measured, **args),
|
||||
other.update(model(alternate), desired, current_curvature=measured, **args)]
|
||||
state = np.array([core.c0, core.c1, core.correction, core.proportional])
|
||||
np.testing.assert_allclose(state, -np.array([mirror.c0, mirror.c1, mirror.correction, mirror.proportional]), atol=1e-10, rtol=0.)
|
||||
assert (core.c1, core.correction, core.proportional) == (other.c1, other.correction, other.proportional)
|
||||
if active:
|
||||
distance = max(7., speed)
|
||||
base = min(.5, max(-.5, distance*desired))
|
||||
target_c0 = min(5.11, max(-5.11, offset+7.*(distance*desired-base)))
|
||||
assert abs(core.c0-(before[0]+min(4.*dt, max(-4.*dt, target_c0-before[0])))) <= 1e-10
|
||||
p = kp*distance*(desired-measured) if enabled else 0.
|
||||
integral = 0.
|
||||
if enabled:
|
||||
increment = ki*(desired-measured)*speed*feedback_dt
|
||||
direction = measured if measured else before[1]
|
||||
if limited and increment*direction > 0.:
|
||||
increment = min(max(increment, min(-before[2], 0.)), max(-before[2], 0.))
|
||||
integral = before[2]
|
||||
if increment*integral < 0.:
|
||||
cancel = math.copysign(min(abs(increment), abs(integral)), increment)
|
||||
integral += cancel
|
||||
increment -= cancel
|
||||
lower, upper = max(-.5, before[1]-.5*dt), min(.5, before[1]+.5*dt)
|
||||
request = base+p+integral
|
||||
integral += min(max(increment, min(lower-request, 0.)), max(upper-request, 0.))
|
||||
assert core.proportional == p and abs(core.correction-integral) <= 1e-10
|
||||
request = min(.5, max(-.5, base+p+integral))
|
||||
assert abs(core.c1-(before[1]+min(.5*dt, max(-.5*dt, request-before[1])))) <= 1e-10
|
||||
assert abs(core.c0) <= 5.11+1e-10 and abs(core.c1) <= .5+1e-10 and abs(core.correction) <= 1.+1e-10
|
||||
else:
|
||||
assert np.all(state == 0.)
|
||||
for command in outputs:
|
||||
assert command.curvature == command.curvature_rate == 0.
|
||||
wire.check(command)
|
||||
result = {'cycles': cycles, 'kp': kp, 'ki': ki, 'controller_updates': 3*cycles,
|
||||
'can_round_trips': wire.count, 'seed': 20260913, 'independent_c0_comparisons': cycles,
|
||||
'checks': 'Independent scalar PI/unwind-first/anti-windup arithmetic, mirror symmetry, exact C0 independence, limits, slew, resets, CAN.',
|
||||
'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(result, indent=2)+'\n')
|
||||
print(json.dumps(result))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--cycles', type=int, default=20_000)
|
||||
parser.add_argument('--output', type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
stress(args.cycles, args.output)
|
||||
@@ -0,0 +1,109 @@
|
||||
"""Verify production commands against the archived P=.50/I=.25 offline candidate.
|
||||
|
||||
Fixed recorded motion verifies integration parity, not physical tracking.
|
||||
"""
|
||||
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 openpilot.selfdrive.controls.lib import ford_model_action
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import select_model_action_controller
|
||||
from opendbc.car.ford.values import FordFlags
|
||||
from tools.ford_pscm_lab.feedback_replay import OPENDBC
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency
|
||||
|
||||
|
||||
ROUTES = ('112', '113', '114', '115', '116', '117', 'a0', 'a2', 'a5', 'a9', 'b8', 'b9', 'ca', 'raptor02')
|
||||
SELECTED = 4
|
||||
|
||||
|
||||
def replay(label, output):
|
||||
directory = Path('.cache/ford_raptor_route02' if label == 'raptor02' else f'.cache/ford_route{label}').resolve()
|
||||
previous = Path('.cache/ford_minimal_tuning/sweep')/label
|
||||
output = (output/label).resolve()
|
||||
if output == directory or directory in output.parents or output == previous.resolve():
|
||||
raise ValueError('Output must preserve previous experiments and input extracts')
|
||||
verify_dependency(OPENDBC)
|
||||
archived_report = json.loads((previous/'report.json').read_text())
|
||||
assert archived_report['settings'][SELECTED] == ['unwind_p50_i25', .5, .25, True]
|
||||
for filename in ('route.npz', 'model_paths.npz', 'metadata.json'):
|
||||
path = directory/filename
|
||||
assert hashlib.sha256(path.read_bytes()).hexdigest() == archived_report['source_sha256'][str(path)]
|
||||
archived = dict(np.load(previous/'commands.npz', allow_pickle=False))
|
||||
metadata = json.loads((directory/'metadata.json').read_text())
|
||||
with np.load(directory/'route.npz', allow_pickle=False) as raw:
|
||||
r = {key: table(raw, key) for key in ('controls', 'cs', 'cc', 'model', 'params', 'pscm')}
|
||||
if 'maneuver' in raw and len(raw['maneuver']):
|
||||
raise ValueError('Maneuver reference reconstruction is not available in this sweep')
|
||||
with np.load(directory/'model_paths.npz', allow_pickle=False) as raw:
|
||||
model_ns, paths = raw['ns'], raw['paths']
|
||||
c, t = r['controls'], r['controls']['t']
|
||||
assert all(np.all(np.diff(stream['t']) >= 0.) for stream in r.values())
|
||||
np.testing.assert_allclose(archived['t'], t-metadata['t0'], rtol=0., atol=1e-8)
|
||||
cs, pa, ps = (sample(r[key], t) for key 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]
|
||||
cp = SimpleNamespace(brand='ford', flags=int(FordFlags.CANFD))
|
||||
assert select_model_action_controller(cp, False) is None
|
||||
controller = select_model_action_controller(cp, True)
|
||||
assert controller is not None
|
||||
assert (controller.core.proportional_gain, controller.core.integral_gain) == (.5, .25)
|
||||
shape = (len(t),)
|
||||
commands = np.zeros((*shape, 4))
|
||||
valid = np.zeros(shape, bool)
|
||||
names = ('heading_proportional', 'heading_correction', 'heading_feedforward', 'feedback_enabled', 'offset_overflow', 'pscm_limited')
|
||||
diagnostics = {name: np.zeros(shape) for name in names}
|
||||
wire = WireCheck()
|
||||
for i, now in enumerate(t):
|
||||
model_time = r['model']['t'][mi[i]]
|
||||
services_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': services_valid,
|
||||
'current_curvature': c['measured'][i], 'driver_pressed': bool(cs['pressed'][i]),
|
||||
'driver_torque': cs['torque'][i], 'pscm_status': status}
|
||||
command = controller.update(model, c['desired'][i], **kwargs)
|
||||
commands[i] = command.path_offset, command.path_angle, command.curvature, command.curvature_rate
|
||||
valid[i] = command.valid
|
||||
for name in names:
|
||||
diagnostics[name][i] = controller.diagnostics.get(name, 0.)
|
||||
wire.check(command)
|
||||
field_checks(commands, valid, t)
|
||||
np.testing.assert_array_equal(commands, archived['commands'][SELECTED])
|
||||
np.testing.assert_array_equal(valid, archived['valid'][SELECTED])
|
||||
for name in names:
|
||||
np.testing.assert_array_equal(diagnostics[name], archived[name][SELECTED])
|
||||
output.mkdir(parents=True, exist_ok=True)
|
||||
sources = (directory/'route.npz', directory/'model_paths.npz', directory/'metadata.json',
|
||||
previous/'report.json', previous/'commands.npz', Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())
|
||||
result = {'scope': __doc__, 'route': label, 'cycles': len(t), 'can_round_trips': wire.count,
|
||||
'production_matches_archived_trial_exactly': True, 'opendbc_revision': OPENDBC,
|
||||
'hypothesis': controller.hypothesis, 'kp': .5, 'ki': .25,
|
||||
'source_sha256': {str(path.resolve()): hashlib.sha256(path.read_bytes()).hexdigest() for path in sources}}
|
||||
(output/'report.json').write_text(json.dumps(result, indent=2, allow_nan=False)+'\n')
|
||||
return result
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--routes', nargs='+', choices=ROUTES, default=ROUTES)
|
||||
parser.add_argument('--output', type=Path, required=True)
|
||||
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}
|
||||
for job in as_completed(jobs):
|
||||
result = job.result()
|
||||
print(json.dumps({'completed': jobs[job], 'cycles': result['cycles'], 'can_checks': result['can_round_trips']}), flush=True)
|
||||
@@ -12,13 +12,12 @@ 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'
|
||||
CANDIDATE = 'bf00bc691def830e1beb15363d05416714c1dc42'
|
||||
GAINS = (0., .1, .25, .5)
|
||||
DELAYS = (0., .2, .4)
|
||||
|
||||
@@ -42,7 +41,8 @@ def replay(directory, output):
|
||||
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]
|
||||
candidate, candidate_hash = original_controller(CANDIDATE)
|
||||
controllers = [type(candidate)(proportional_gain=gain) for gain, _ in settings]
|
||||
reference, reference_hash = original_controller(BASELINE)
|
||||
delayed = {}
|
||||
for delay in DELAYS:
|
||||
@@ -91,12 +91,13 @@ def replay(directory, output):
|
||||
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,
|
||||
'candidate_revision': CANDIDATE, 'candidate_source_sha256': candidate_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())},
|
||||
Path(__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.',
|
||||
|
||||
@@ -7,19 +7,18 @@ 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
|
||||
from tools.ford_pscm_lab.pi_replay import BASELINE, CANDIDATE
|
||||
|
||||
|
||||
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()
|
||||
candidate, candidate_hash = original_controller(CANDIDATE)
|
||||
controller = type(candidate.core)(proportional_gain=gain)
|
||||
mirror = type(candidate.core)(proportional_gain=gain)
|
||||
zero = type(candidate.core)()
|
||||
original, original_hash = original_controller(BASELINE)
|
||||
wire = WireCheck()
|
||||
release_count = 0
|
||||
@@ -75,12 +74,13 @@ def stress(cycles, gain, output):
|
||||
wire.check(out)
|
||||
report = {'cycles': cycles, 'gain': gain, 'seed': 20260913, 'mirrored_updates': cycles,
|
||||
'zero_gain_exact_v5_comparisons': cycles, 'can_round_trips': wire.count,
|
||||
'candidate_revision': CANDIDATE, 'candidate_source_sha256': candidate_hash,
|
||||
'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())}}
|
||||
(Path(__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))
|
||||
|
||||
Reference in New Issue
Block a user