mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-10-01 03:53:45 +08:00
Ford: use filtered driver input for feedback continuity
This commit is contained in:
Isaac Barham
@@ -0,0 +1,75 @@
|
||||
# Ford feedback: use filtered driver input
|
||||
|
||||
Route 162 ran action mode at C0 P=1.0. Raw torque crossings above 1 Nm
|
||||
repeatedly removed C0/C1 feedback even while Ford's `steeringPressed` remained
|
||||
false. The controller duplicated the torque threshold without Ford's existing
|
||||
filter, so short crossings discarded the integral and abruptly removed P.
|
||||
|
||||
The adapter now uses `steeringPressed` for this decision. The existing Ford
|
||||
threshold and filter are unchanged. From a zero counter, sustained torque
|
||||
crosses that filter on the sixth sample. Fresh PSCM driver override (limit=3)
|
||||
and invalid torque still clear feedback immediately. PSCM denial/inactive
|
||||
status, freshness checks and input validity retain their previous behavior.
|
||||
Clearing feedback removes correction; the base path request remains.
|
||||
|
||||
This applies to action and direct-path modes. Gains, reference selection,
|
||||
distances, bounds, integration/unwind math, upstream request limits and CAN
|
||||
cadence are unchanged. Action C0 P remains 1.0; direct-path C0 P remains 0.5.
|
||||
Diagnostic names end in `v20-filtered-driver` so the next drive can confirm the
|
||||
new arbitration actually ran.
|
||||
|
||||
## Validation
|
||||
|
||||
The short-torque-pulse regression failed against the old adapter before the
|
||||
change. Afterward, the Ford controller, integration/CAN, geometry and Sunnylink
|
||||
suites passed: 658 tests and 2 subtests. Tests exercise the actual shared Ford
|
||||
filter, both torque signs, both reference modes, immediate PSCM override and
|
||||
invalid torque. Ruff and `git diff --check` passed.
|
||||
|
||||
Native-time replay compares the new adapter with
|
||||
`4900c0a40c87c72000b7168a6cf4fe6dd98ea6d0`, supplying identical recorded selected
|
||||
curvature, vehicle measurements, driver flags and PSCM status. Four routes
|
||||
cover 583,599 control cycles and 58,362 CAN pack/decode checks. Every command
|
||||
stays in its field bounds with C2=C3=0, and filtered driver input or fresh PSCM
|
||||
override clears all feedback.
|
||||
|
||||
Route 162's baseline reproduces recorded C0/C1 to Float32 precision (maximum
|
||||
errors 1.15e-7 m and 1.48e-8 rad). Older routes were driven with earlier gains;
|
||||
their comparisons below are two counterfactual command streams on the same
|
||||
recorded motion, not a reproduction of those older deployed controllers.
|
||||
|
||||
| Route | C0 changes >0.25 m below 15 mph, old → new | C1 changes >0.05 rad below 15 mph, old → new | C1 at field bound, old → new |
|
||||
| --- | --- | --- | --- |
|
||||
| 162 | 152 → 70 | 83 → 50 | 0.97 → 1.06 s |
|
||||
| 157 | 229 → 137 | 105 → 59 | 5.83 → 6.52 s |
|
||||
| 151 | 142 → 77 | 86 → 42 | 3.39 → 4.04 s |
|
||||
| 149 | 375 → 197 | 226 → 111 | 10.44 → 16.08 s |
|
||||
|
||||
For route 162, feedback on/off transitions fall from 538 to 218. At raw-torque
|
||||
threshold crossings with unchanged model frame, nearly unchanged target and
|
||||
no filtered driver/PSCM override, large C0 changes fall from 99 to zero.
|
||||
Remaining transitions include legitimate driver input and PSCM status changes.
|
||||
|
||||
Retained correction changes C1 after a brief torque crossing has ended. This
|
||||
can increase holding and time at the field limit, especially on route 149.
|
||||
Replay proves command continuity and override behavior on frozen measurements;
|
||||
it cannot establish improved physical tracking, stability or unwinding. Route
|
||||
162 also had lag without any torque-triggered reset, which this change alone
|
||||
does not explain.
|
||||
|
||||
Detailed metrics, source hashes and controller provenance are in
|
||||
`ford_filtered_driver_v20_validation.json`. Reproduce a route with:
|
||||
|
||||
```sh
|
||||
PYTHONPATH=.:opendbc_repo:.cache/ford_geometry_deps \
|
||||
/Users/ibpersonal/dev/sunnypilot/.venv/bin/python \
|
||||
tools/ford_pscm_lab/filtered_driver_replay.py \
|
||||
--source .cache/ford_route162/full --output .cache/ford_filtered_driver_v20/162
|
||||
```
|
||||
|
||||
## Deployment
|
||||
|
||||
Pull and restart the updated software while offroad. Keep Selected-Action Path
|
||||
Tracking enabled, Model Geometry Reference disabled and C0 one-second distance
|
||||
disabled for the current action/fixed-7-m trial. Turning the master controller
|
||||
toggle off continues to select upstream Ford control.
|
||||
@@ -0,0 +1,308 @@
|
||||
{
|
||||
"baseline_commit": "4900c0a40c87c72000b7168a6cf4fe6dd98ea6d0",
|
||||
"tests": {
|
||||
"passed": 658,
|
||||
"subtests_passed": 2
|
||||
},
|
||||
"total_cycles": 583599,
|
||||
"total_wire_checks": 58362,
|
||||
"limitations": "Frozen recorded motion; no predicted wheel response. Only route 162 ran the baseline gain/version. Older-route baseline differences from recorded commands are expected.",
|
||||
"routes": {
|
||||
"162": {
|
||||
"cycles": 49003,
|
||||
"wire_checks": 4901,
|
||||
"baseline_recorded_error_p50_p95_p99_max": {
|
||||
"c0": [
|
||||
2.3841861818141297e-09,
|
||||
3.33786012163273e-08,
|
||||
5.7220459037665705e-08,
|
||||
1.1444091807533141e-07
|
||||
],
|
||||
"c1": [
|
||||
4.6193593394860955e-10,
|
||||
7.152557435219364e-09,
|
||||
1.2874603272372553e-08,
|
||||
1.478195188475695e-08
|
||||
]
|
||||
},
|
||||
"latched_angle_match_fraction": 1.0,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.3899999999999997,
|
||||
2.58
|
||||
],
|
||||
"c1": [
|
||||
0.0014999999999999458,
|
||||
0.07550000000000001,
|
||||
0.11149999999999999,
|
||||
0.2450000000000001
|
||||
]
|
||||
},
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 538,
|
||||
"feedback_switches_low_speed": 219,
|
||||
"low_speed_c0_steps_over_025m": 152,
|
||||
"low_speed_c1_steps_over_005rad": 83,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.5400000000000005,
|
||||
"c1": 0.033499999999999974
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 99,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 0.9656279909999625
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 218,
|
||||
"feedback_switches_low_speed": 95,
|
||||
"low_speed_c0_steps_over_025m": 70,
|
||||
"low_speed_c1_steps_over_005rad": 50,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.16000000000000014,
|
||||
"c1": 0.019949999999999843
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 1.0580744659999937
|
||||
}
|
||||
},
|
||||
"method": "Compare driver-input arbitration on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\nBoth controllers receive the logged selected curvature. Recorded-command agreement\nis expected only for routes driven with the selected baseline and fixed 7 m C0.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "4900c0a40c87c72000b7168a6cf4fe6dd98ea6d0",
|
||||
"baseline_controller_sha256": "d5841c1f6a4368de20ff14de89a48d7dce737c6a41d79628ccb4f8b19f28f91b",
|
||||
"candidate_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
".cache/ford_route162/full/route.npz": "a725f9ca309b90a2d750d51a9b9d5b459d99119a54ab048631bc0fce07348af8",
|
||||
".cache/ford_route162/full/model_paths.npz": "cffe310b122d9ae26a53ee504608156b4242a6bba5f681898d5b661951ac5ce5",
|
||||
".cache/ford_route162/full/metadata.json": "ef3ac1caca0eaec31f7140a0743bd3b98885ce143cb0f97a8eee615b33943d7f"
|
||||
},
|
||||
"valid_active_seconds": 319.0762965260001
|
||||
},
|
||||
"157": {
|
||||
"cycles": 76554,
|
||||
"wire_checks": 7656,
|
||||
"baseline_recorded_error_p50_p95_p99_max": {
|
||||
"c0": [
|
||||
0.009999999999999787,
|
||||
0.3699999867081644,
|
||||
1.1900000762939458,
|
||||
2.7099999666213987
|
||||
],
|
||||
"c1": [
|
||||
2.5331975406217566e-10,
|
||||
7.152557379708213e-09,
|
||||
1.3113021890553966e-08,
|
||||
1.478195188475695e-08
|
||||
]
|
||||
},
|
||||
"latched_angle_match_fraction": 1.0,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.18569999999999706,
|
||||
5.4
|
||||
],
|
||||
"c1": [
|
||||
0.0,
|
||||
0.035499999999999976,
|
||||
0.066,
|
||||
0.40850000000000003
|
||||
]
|
||||
},
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 484,
|
||||
"feedback_switches_low_speed": 312,
|
||||
"low_speed_c0_steps_over_025m": 229,
|
||||
"low_speed_c1_steps_over_005rad": 105,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.3100000000000005,
|
||||
"c1": 0.028000000000000025
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 99,
|
||||
"c0_bound_active_s": 0.25064877399995567,
|
||||
"c1_bound_active_s": 5.832844185000681
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 224,
|
||||
"feedback_switches_low_speed": 148,
|
||||
"low_speed_c0_steps_over_025m": 137,
|
||||
"low_speed_c1_steps_over_005rad": 59,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.20000000000000018,
|
||||
"c1": 0.022499999999999964
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.2999506119999751,
|
||||
"c1_bound_active_s": 6.521285587999955
|
||||
}
|
||||
},
|
||||
"method": "Compare driver-input arbitration on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\nBoth controllers receive the logged selected curvature. Recorded-command agreement\nis expected only for routes driven with the selected baseline and fixed 7 m C0.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "4900c0a40c87c72000b7168a6cf4fe6dd98ea6d0",
|
||||
"baseline_controller_sha256": "d5841c1f6a4368de20ff14de89a48d7dce737c6a41d79628ccb4f8b19f28f91b",
|
||||
"candidate_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
".cache/ford_route157/full/route.npz": "e2e2573f904aa11ee9e10450e7f5b965d475657b61127e827a67eadcd6b857fb",
|
||||
".cache/ford_route157/full/model_paths.npz": "25fc2526e092fde5ceb5aab63a5aa4f73a2c989a3263cedb0a26faf8da61c47b",
|
||||
".cache/ford_route157/full/metadata.json": "f2d72c9a877ed6694e4da6831841113dc0c05987e2c76cca51f358d12411b235"
|
||||
},
|
||||
"valid_active_seconds": 527.940320743
|
||||
},
|
||||
"151": {
|
||||
"cycles": 325708,
|
||||
"wire_checks": 32571,
|
||||
"baseline_recorded_error_p50_p95_p99_max": {
|
||||
"c0": [
|
||||
0.010000000223516992,
|
||||
0.18000000834465002,
|
||||
0.9099999795913697,
|
||||
4.419999957084657
|
||||
],
|
||||
"c1": [
|
||||
0.002999999523162822,
|
||||
0.016500000025611382,
|
||||
0.04399999969661239,
|
||||
0.09699999737739562
|
||||
]
|
||||
},
|
||||
"latched_angle_match_fraction": 1.0,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
4.55
|
||||
],
|
||||
"c1": [
|
||||
0.0,
|
||||
0.020500000000000018,
|
||||
0.050000000000000044,
|
||||
0.28450000000000003
|
||||
]
|
||||
},
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 727,
|
||||
"feedback_switches_low_speed": 225,
|
||||
"low_speed_c0_steps_over_025m": 142,
|
||||
"low_speed_c1_steps_over_005rad": 86,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.17999999999999972,
|
||||
"c1": 0.016500000000000015
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 80,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 3.3938434989995585
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 259,
|
||||
"feedback_switches_low_speed": 101,
|
||||
"low_speed_c0_steps_over_025m": 77,
|
||||
"low_speed_c1_steps_over_005rad": 42,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.13999999999999968,
|
||||
"c1": 0.014499999999999957
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.029249633000290487,
|
||||
"c1_bound_active_s": 4.042117826999856
|
||||
}
|
||||
},
|
||||
"method": "Compare driver-input arbitration on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\nBoth controllers receive the logged selected curvature. Recorded-command agreement\nis expected only for routes driven with the selected baseline and fixed 7 m C0.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "4900c0a40c87c72000b7168a6cf4fe6dd98ea6d0",
|
||||
"baseline_controller_sha256": "d5841c1f6a4368de20ff14de89a48d7dce737c6a41d79628ccb4f8b19f28f91b",
|
||||
"candidate_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
".cache/ford_route151/full/route.npz": "41a5b8bd388cf5a3d553f784542376ac9355fcdc5be4f427053d0504537babe1",
|
||||
".cache/ford_route151/full/model_paths.npz": "980b3843cec04254280d744a5801cee1bf0f8ef371052398327ff245f9eee01b",
|
||||
".cache/ford_route151/full/metadata.json": "937825317a0edd470c54647240b922be8f79dda5b3365ffdd61281f0aca877a1"
|
||||
},
|
||||
"valid_active_seconds": 1355.493226190003
|
||||
},
|
||||
"149": {
|
||||
"cycles": 132334,
|
||||
"wire_checks": 13234,
|
||||
"baseline_recorded_error_p50_p95_p99_max": {
|
||||
"c0": [
|
||||
0.020000001788138988,
|
||||
0.7800000047683717,
|
||||
2.070000008535385,
|
||||
4.560000002980233
|
||||
],
|
||||
"c1": [
|
||||
0.005500000000000005,
|
||||
0.048999999627471036,
|
||||
0.09199999963641169,
|
||||
0.1995000131130219
|
||||
]
|
||||
},
|
||||
"latched_angle_match_fraction": 1.0,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.10000000000000053,
|
||||
4.499999999999999
|
||||
],
|
||||
"c1": [
|
||||
0.0,
|
||||
0.07950000000000002,
|
||||
0.3125,
|
||||
0.4315
|
||||
]
|
||||
},
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 1024,
|
||||
"feedback_switches_low_speed": 393,
|
||||
"low_speed_c0_steps_over_025m": 375,
|
||||
"low_speed_c1_steps_over_005rad": 226,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.44079999999998143,
|
||||
"c1": 0.031500000000000035
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 217,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 10.436320454000196
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 354,
|
||||
"feedback_switches_low_speed": 171,
|
||||
"low_speed_c0_steps_over_025m": 197,
|
||||
"low_speed_c1_steps_over_005rad": 111,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.1999999999999993,
|
||||
"c1": 0.02150000000000002
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 16.08094779300012
|
||||
}
|
||||
},
|
||||
"method": "Compare driver-input arbitration on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\nBoth controllers receive the logged selected curvature. Recorded-command agreement\nis expected only for routes driven with the selected baseline and fixed 7 m C0.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "4900c0a40c87c72000b7168a6cf4fe6dd98ea6d0",
|
||||
"baseline_controller_sha256": "d5841c1f6a4368de20ff14de89a48d7dce737c6a41d79628ccb4f8b19f28f91b",
|
||||
"candidate_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade",
|
||||
".cache/ford_route149/full/model_paths.npz": "19827800b8fb5983f3d6b72fcfaf36e35a40170bb17cd7c1e49374744fb449fb",
|
||||
".cache/ford_route149/full/metadata.json": "624fff03c25eb298661cb7b25f3dbe6d214d863f799d93635c0f4f05fc0d2b32"
|
||||
},
|
||||
"valid_active_seconds": 1247.577060015
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -13,7 +13,7 @@ import struct
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.ford.values import CarControllerParams, FordFlags
|
||||
from opendbc.car.ford.values import FordFlags
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
|
||||
|
||||
@@ -180,8 +180,9 @@ class FordModelActionController:
|
||||
Feedback advances once per fresh steering measurement; repeated samples
|
||||
still use the current request. Raw model geometry is checked on every cycle.
|
||||
|
||||
CAN yaw remains a health gate, not the feedback measurement. Driver override
|
||||
clears the correction. Fresh PSCM limits only inhibit outward integration;
|
||||
CAN yaw remains a health gate, not the feedback measurement. Ford's filtered
|
||||
steeringPressed and fresh PSCM driver overrides clear the correction.
|
||||
Fresh PSCM limits only inhibit outward integration;
|
||||
neither a limit nor a repeated measurement freezes the model request.
|
||||
"""
|
||||
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False,
|
||||
@@ -191,7 +192,8 @@ class FordModelActionController:
|
||||
self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based,
|
||||
c0_proportional_gain=c0_proportional_gain)
|
||||
self.direct_path = bool(direct_path)
|
||||
self.hypothesis = 'model-path-direct-feedback-v17' if self.direct_path else 'model-action-curvature-c0-feedback-v19'
|
||||
self.hypothesis = ('model-path-direct-feedback-v20-filtered-driver' if self.direct_path else
|
||||
'model-action-curvature-c0-feedback-v20-filtered-driver')
|
||||
self.reset()
|
||||
|
||||
def path_curvature(self, model, speed):
|
||||
@@ -242,8 +244,9 @@ class FordModelActionController:
|
||||
status_fresh = (pscm_status is not None and pscm_status.valid and pscm_status.canMonoTime > 0
|
||||
and -.005 <= now-pscm_status.canMonoTime*1e-9 <= .15)
|
||||
pscm_limited = bool(status_fresh and pscm_status.limit == 2)
|
||||
# CarState already filters Ford's torque threshold into steeringPressed.
|
||||
# Rechecking raw torque here bypasses that filter and abruptly clears P/I.
|
||||
driver_override = bool(driver_pressed or not _finite(driver_torque)
|
||||
or abs(driver_torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
|
||||
or (status_fresh and pscm_status.limit == 3))
|
||||
feedback_enabled = not (driver_override or (status_fresh and (pscm_status.denied or pscm_status.lateralState != 2)))
|
||||
direct_path = self.direct_path and reference_source == 'modelV2'
|
||||
|
||||
@@ -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-curvature-c0-feedback-v19')
|
||||
self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-feedback-v20-filtered-driver')
|
||||
self.assertIs(record['calibration_approved'], False)
|
||||
self.assertEqual(record['command'][2:], [0., 0.])
|
||||
self.assertEqual(record['status'], controller.diagnostics['status'])
|
||||
|
||||
@@ -90,7 +90,7 @@ def test_both_base_channels_retain_upstream_request_limits_on_entry_reversal_and
|
||||
assert abs(command.path_offset) < .01 and abs(command.path_angle) < .001
|
||||
|
||||
|
||||
def test_feedback_tracks_path_heading_and_raw_torque_override_is_still_the_driven_baseline():
|
||||
def test_feedback_tracks_path_heading_and_filtered_driver_input_clears_correction():
|
||||
controller = FordModelActionController(direct_path=True)
|
||||
model = circle(.01)
|
||||
desired = 0.
|
||||
@@ -101,7 +101,9 @@ def test_feedback_tracks_path_heading_and_raw_torque_override_is_still_the_drive
|
||||
desired, _ = step(controller, model, 2., desired, measured=desired)
|
||||
assert controller.core.proportional == pytest.approx(0.)
|
||||
assert controller.core.correction == pytest.approx(retained)
|
||||
step(controller, model, 2.01, desired, driver_torque=1.0625)
|
||||
desired, _ = step(controller, model, 2.01, desired, driver_torque=1.0625)
|
||||
assert controller.diagnostics['feedback_enabled']
|
||||
step(controller, model, 2.02, desired, driver_pressed=True, driver_torque=1.0625)
|
||||
assert controller.core.correction == controller.core.proportional == controller.core.offset_proportional == 0.
|
||||
|
||||
|
||||
|
||||
@@ -17,6 +17,7 @@ from opendbc.car import Bus, structs
|
||||
from opendbc.car.ford.carcontroller import CarController
|
||||
from opendbc.car.ford.fordcan import calculate_lat_ctl2_checksum
|
||||
from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.selfdrive.car.helpers import convert_carControlSP
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
@@ -242,6 +243,7 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
model.action = SimpleNamespace(desiredCurvature=sign*.004)
|
||||
cc = structs.CarControl(latActive=True)
|
||||
cs = SimpleNamespace(vEgo=20., yawRate=.2, canValid=True, steeringPressed=False, steeringTorque=0.)
|
||||
driver_filter = SimpleNamespace(steering_pressed_cnt=0)
|
||||
cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1')
|
||||
downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
|
||||
vehicle = SimpleNamespace(out=structs.CarState(vEgo=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int),
|
||||
@@ -251,11 +253,13 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
# Every fresh error sample integrates within amplitude headroom.
|
||||
# Matched steering removes P and preserves I.
|
||||
for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.02),
|
||||
(sign*.004, 0., 100, sign*.02), (sign*.005, 0., 100, 0.),
|
||||
(sign*.003, 0., 100, sign*.02), (0., 1.0625, 5, 0.)]:
|
||||
(sign*.004, 0., 100, sign*.02), (sign*.004, 1.0625, 2, sign*.02),
|
||||
(sign*.005, 0., 100, 0.), (sign*.003, 0., 100, sign*.02), (0., 1.0625, 12, 0.)]:
|
||||
for _ in range(count):
|
||||
now = 1.+frame*.01
|
||||
controls.curvature, cs.steeringTorque = measured, torque
|
||||
cs.steeringPressed = CarStateBase.update_steering_pressed(
|
||||
driver_filter, abs(torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
|
||||
environment = {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
@@ -277,14 +281,14 @@ 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 = .75*20.*(sign*.004-measured) if torque == 0. else 0.
|
||||
expected_p = .75*20.*(sign*.004-measured) if not cs.steeringPressed else 0.
|
||||
assert core.proportional == pytest.approx(expected_p)
|
||||
assert core.correction == pytest.approx(expected)
|
||||
assert core.c1 == pytest.approx(sign*.08+expected_p+expected)
|
||||
assert controls.ford_path.path_angle == pytest.approx(core.c1, abs=.00025)
|
||||
base = encode_model_action(straight(), sign*.004, cs.vEgo).path_offset
|
||||
assert controls.ford_path.path_offset == pytest.approx(base+core.offset_proportional, abs=.005)
|
||||
assert (core.offset_proportional == 0.) == (torque != 0. or measured == sign*.004)
|
||||
assert (core.offset_proportional == 0.) == (cs.steeringPressed or measured == sign*.004)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('service_valid', [False, True])
|
||||
@@ -359,7 +363,7 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
if frame == 199:
|
||||
assert sign*core.correction < 0. if same_turn else sign*core.correction > 0.
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v19'
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v20-filtered-driver'
|
||||
if same_turn:
|
||||
assert controls.desired_curvature == pytest.approx(sign*.01)
|
||||
assert sign*controls.ford_path.path_angle >= speed*.01 # No old unwind correction left below the new base.
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
"""Exercise Ford's actual driver-input filter together with path feedback."""
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car.ford.values import CarControllerParams
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action_feedback import adapter_tick, status
|
||||
|
||||
|
||||
def pressed(state, torque):
|
||||
# Ford CarState.update uses this shared filter with minimum count 5.
|
||||
return CarStateBase.update_steering_pressed(state, abs(torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('direct_path', [False, True])
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_twenty_ms_torque_spike_does_not_drop_feedback_before_ford_detects_driver_input(sign, direct_path):
|
||||
state = SimpleNamespace(steering_pressed_cnt=0)
|
||||
controller, steady = (FordModelActionController(direct_path=direct_path) for _ in range(2))
|
||||
# Route 15a at 196.19 s: two 1.0625 Nm samples, steeringPressed remained false.
|
||||
torques = [0.]*50+[sign*1.0625]*2+[0.]*20
|
||||
for i, torque in enumerate(torques):
|
||||
now = 1.+i*.01
|
||||
driver = pressed(state, torque)
|
||||
assert not driver
|
||||
actual = adapter_tick(controller, now, speed=3.2, driver_pressed=driver, driver_torque=torque)
|
||||
expected = adapter_tick(steady, now, speed=3.2)
|
||||
assert actual == expected
|
||||
assert controller.core.correction == steady.core.correction
|
||||
assert controller.diagnostics['feedback_enabled']
|
||||
assert controller.core.correction > 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('direct_path', [False, True])
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_sustained_driver_input_clears_feedback_on_the_first_filtered_pressed_sample(sign, direct_path):
|
||||
state = SimpleNamespace(steering_pressed_cnt=0)
|
||||
controller = FordModelActionController(direct_path=direct_path)
|
||||
for i in range(50):
|
||||
adapter_tick(controller, 1.+i*.01, speed=3.2)
|
||||
assert controller.core.correction > 0.
|
||||
detected = False
|
||||
for i in range(12):
|
||||
torque = sign*1.0625
|
||||
driver = pressed(state, torque)
|
||||
adapter_tick(controller, 1.5+i*.01, speed=3.2, driver_pressed=driver, driver_torque=torque)
|
||||
assert controller.diagnostics['feedback_enabled'] == (not driver)
|
||||
if driver:
|
||||
detected = True
|
||||
assert controller.core.correction == controller.core.proportional == controller.core.offset_proportional == 0.
|
||||
assert detected
|
||||
|
||||
|
||||
@pytest.mark.parametrize('direct_path', [False, True])
|
||||
def test_pscm_override_remains_immediate_before_driver_filter_triggers(direct_path):
|
||||
controller = FordModelActionController(direct_path=direct_path)
|
||||
for i in range(50):
|
||||
adapter_tick(controller, 1.+i*.01, speed=3.2)
|
||||
adapter_tick(controller, 1.5, speed=3.2, driver_pressed=False, driver_torque=1.0625,
|
||||
pscm_status=status(1.5, limit=3))
|
||||
assert not controller.diagnostics['feedback_enabled']
|
||||
assert controller.core.correction == controller.core.proportional == controller.core.offset_proportional == 0.
|
||||
@@ -139,8 +139,8 @@ def test_repeated_steering_samples_do_not_reintegrate_error():
|
||||
assert controller.diagnostics['feedback_dt'] == pytest.approx(.06)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('overrides', [{'driver_pressed': True}, {'driver_torque': 1.01},
|
||||
{'driver_torque': -1.01}, {'driver_torque': math.nan},
|
||||
@pytest.mark.parametrize('overrides', [{'driver_pressed': True}, {'driver_torque': math.nan},
|
||||
{'driver_torque': math.inf}, {'driver_torque': None},
|
||||
{'pscm_status': status(2.01, limit=3)},
|
||||
{'pscm_status': status(2.01, denied=True)},
|
||||
{'pscm_status': status(2.01, lateralState=1)}])
|
||||
|
||||
@@ -53,7 +53,7 @@ def test_actual_startup_priority(candidate, observer, fingerprint):
|
||||
assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .75
|
||||
assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == 1.
|
||||
assert selected.ford_path_controller.core.c0_proportional_gain == 1.
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v19'
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v20-filtered-driver'
|
||||
else:
|
||||
assert selected.ford_path_controller is None
|
||||
assert selected.ford_model_action == candidate
|
||||
|
||||
@@ -0,0 +1,128 @@
|
||||
"""Compare driver-input arbitration on recorded references and frozen vehicle motion.
|
||||
|
||||
Input: extract.py route.npz/model_paths.npz/metadata.json directories. This checks
|
||||
command continuity and overrides; it does not predict a changed wheel response.
|
||||
Both controllers receive the logged selected curvature. Recorded-command agreement
|
||||
is expected only for routes driven with the selected baseline and fixed 7 m C0.
|
||||
"""
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
import subprocess
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table
|
||||
|
||||
|
||||
def replay(source, destination, baseline_class, provenance):
|
||||
meta = json.loads((source/'metadata.json').read_text())
|
||||
with np.load(source/'route.npz') as z:
|
||||
r = {k: table(z, k) for k in ('controls', 'cs', 'cc', 'path', 'params', 'pscm', 'model')}
|
||||
with np.load(source/'model_paths.npz') as z:
|
||||
paths = z['paths']
|
||||
path_ns = z['ns']
|
||||
c = r['controls']
|
||||
t = c['t']
|
||||
cs, pa, ps = [sample(r[k], t) for k in ('cs', 'params', 'pscm')]
|
||||
cc, sent = [sample(r[k], t, nearest=True) for k in ('cc', 'path')]
|
||||
mi = np.clip(np.searchsorted(path_ns, c['model_ns']), 0, len(path_ns)-1)
|
||||
exact = path_ns[mi] == c['model_ns']
|
||||
models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths]
|
||||
car = meta['car'][0]
|
||||
cp = SimpleNamespace(**{k: car[k] for k in ('mass', 'wheelbase', 'centerToFront', 'steerRatioRear', 'tireStiffnessFront', 'tireStiffnessRear')},
|
||||
steerRatio=car['steer_ratio'], rotationalInertia=0.)
|
||||
vm = VehicleModel(cp)
|
||||
cores = [baseline_class(), FordModelActionController()]
|
||||
wire = WireCheck()
|
||||
names = ['t', 'active', 'valid', 'speed', 'pressed', 'raw_torque', 'pscm_override', 'limit',
|
||||
'old_c0', 'new_c0', 'old_c1', 'new_c1', 'old_i', 'new_i', 'old_feedback', 'new_feedback',
|
||||
'recorded_c0', 'recorded_c1', 'latched_angle_error', 'model_ns', 'desired_angle']
|
||||
rows = np.zeros((len(t), len(names)))
|
||||
for i, now in enumerate(t):
|
||||
vm.update_params(max(pa['stiffness'][i], .1), max(pa['steer_ratio'][i], .1))
|
||||
scale = vm.get_steer_from_curvature(1., cs['speed'][i], 0.)/(cp.steerRatio*cp.wheelbase)
|
||||
error = c['desired'][i]-c['measured'][i]
|
||||
if abs(error) > 1e-5:
|
||||
scale = -np.radians(c['desired_angle'][i]-c['actual_angle'][i])/error/(cp.steerRatio*cp.wheelbase)
|
||||
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]))
|
||||
active = bool(cc['active'][i])
|
||||
valid = bool(c['valid'][i] and cs['valid'][i] and cs['can_valid'][i] and pa['valid'][i] and exact[i])
|
||||
commands = []
|
||||
for core in cores:
|
||||
command = core.update(models[mi[i]], c['desired'][i], current_curvature=c['measured'][i],
|
||||
speed=cs['speed'][i], yaw_rate=cs['yaw'][i], now=now, measurement_time=cs['t'][i],
|
||||
model_time=c['model_ns'][i]*1e-9, reference_time=c['model_ns'][i]*1e-9,
|
||||
active=active, valid=valid, driver_pressed=bool(cs['pressed'][i]), driver_torque=cs['torque'][i],
|
||||
pscm_status=status, curvature_scale=scale)
|
||||
assert abs(command.path_offset) <= 5.1100001 and abs(command.path_angle) <= .5000001
|
||||
assert command.curvature == command.curvature_rate == 0.
|
||||
if command.valid and (cs['pressed'][i] or (status.valid and -.005 <= now-ps['stamp'][i] <= .15 and status.limit == 3)):
|
||||
assert not core.diagnostics['feedback_enabled']
|
||||
assert core.core.correction == core.core.proportional == core.core.offset_proportional == 0.
|
||||
commands.append(command)
|
||||
assert commands[0].valid == commands[1].valid
|
||||
if i % 10 == 0:
|
||||
wire.check(commands[1])
|
||||
old, new = commands
|
||||
rows[i] = [now-meta['t0'], active, new.valid, cs['speed'][i], cs['pressed'][i], cs['torque'][i], status.limit == 3, status.limit == 2,
|
||||
old.path_offset, new.path_offset, old.path_angle, new.path_angle, cores[0].core.correction, cores[1].core.correction,
|
||||
cores[0].diagnostics.get('feedback_enabled', False), cores[1].diagnostics.get('feedback_enabled', False),
|
||||
sent['c0'][i], sent['c1'][i], abs(cs['angle'][i]-c['actual_angle'][i]), c['model_ns'][i], c['desired_angle'][i]]
|
||||
a = dict(zip(names, rows.T, strict=True))
|
||||
live = a['valid'].astype(bool)
|
||||
consecutive = live[1:] & live[:-1] & (np.diff(t) < .03)
|
||||
low = consecutive & (a['speed'][1:] < 15*.44704)
|
||||
raw_crossing = (consecutive & (abs(a['raw_torque'][:-1]) <= 1.) & (abs(a['raw_torque'][1:]) > 1.)
|
||||
& ~a['pressed'][:-1].astype(bool) & ~a['pressed'][1:].astype(bool)
|
||||
& ~a['pscm_override'][:-1].astype(bool) & ~a['pscm_override'][1:].astype(bool)
|
||||
& (np.diff(a['model_ns']) == 0) & (abs(np.diff(a['desired_angle'])) < .1))
|
||||
metrics = {'cycles': len(t), 'wire_checks': wire.count,
|
||||
'baseline_recorded_error_p50_p95_p99_max': {
|
||||
k: np.quantile(abs(a['old_'+k][live]-a['recorded_'+k][live]), [.5, .95, .99, 1]).tolist() for k in ('c0', 'c1')},
|
||||
'latched_angle_match_fraction': float(np.mean(a['latched_angle_error'][live] < 1e-4)),
|
||||
'command_changes': {k: np.quantile(abs(a['new_'+k][live]-a['old_'+k][live]), [.5, .95, .99, 1]).tolist() for k in ('c0', 'c1')},
|
||||
'variants': {}}
|
||||
for name in ('old', 'new'):
|
||||
metrics['variants'][name] = {
|
||||
'feedback_switches_active': int(np.count_nonzero(np.diff(a[name+'_feedback'])[consecutive])),
|
||||
'feedback_switches_low_speed': int(np.count_nonzero(np.diff(a[name+'_feedback'])[low])),
|
||||
'low_speed_c0_steps_over_025m': int(np.count_nonzero(abs(np.diff(a[name+'_c0'])[low]) > .250001)),
|
||||
'low_speed_c1_steps_over_005rad': int(np.count_nonzero(abs(np.diff(a[name+'_c1'])[low]) > .050001)),
|
||||
'low_speed_step_p99': {k: float(np.quantile(abs(np.diff(a[name+'_'+k])[low]), .99)) for k in ('c0', 'c1')},
|
||||
'raw_crossing_c0_steps_over_025m': int(np.count_nonzero(abs(np.diff(a[name+'_c0'])[raw_crossing]) > .250001)),
|
||||
'c0_bound_active_s': float(np.sum(np.minimum(np.diff(t, append=t[-1]+.01), .03)[live & (abs(a[name+'_c0']) >= 5.105)])),
|
||||
'c1_bound_active_s': float(np.sum(np.minimum(np.diff(t, append=t[-1]+.01), .03)[live & (abs(a[name+'_c1']) >= .49975)])),
|
||||
}
|
||||
# Baseline agreement is reported rather than hidden: control publication time
|
||||
# approximates the core's call time, and retained-message service checks are unavailable.
|
||||
metrics['method'] = __doc__
|
||||
metrics['provenance'] = provenance
|
||||
metrics['sources_sha256'] = {str(source/k): hashlib.sha256((source/k).read_bytes()).hexdigest() for k in ('route.npz', 'model_paths.npz', 'metadata.json')}
|
||||
destination.mkdir(parents=True, exist_ok=True)
|
||||
np.savez_compressed(destination/'commands.npz', names=names, rows=rows)
|
||||
(destination/'report.json').write_text(json.dumps(metrics, indent=2)+'\n')
|
||||
return metrics
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--source', type=Path, required=True)
|
||||
parser.add_argument('--output', type=Path, required=True)
|
||||
parser.add_argument('--baseline', default='4900c0a40c87c72000b7168a6cf4fe6dd98ea6d0')
|
||||
args = parser.parse_args()
|
||||
revision = subprocess.check_output(['git', 'rev-parse', f'{args.baseline}^{{commit}}'], text=True).strip()
|
||||
controller_path = 'openpilot/selfdrive/controls/lib/ford_model_action.py'
|
||||
code = subprocess.check_output(['git', 'show', f'{revision}:{controller_path}'], text=True)
|
||||
namespace = {'__name__': 'recorded_ford_controller'}
|
||||
exec(compile(code, '<recorded_ford_controller>', 'exec'), namespace)
|
||||
provenance = {'baseline_commit': revision, 'baseline_controller_sha256': hashlib.sha256(code.encode()).hexdigest(),
|
||||
'candidate_controller_sha256': hashlib.sha256(Path(controller_path).read_bytes()).hexdigest(),
|
||||
'fixed_c0_distance_m': 7.}
|
||||
result = replay(args.source, args.output, namespace['FordModelActionController'], provenance)
|
||||
print(json.dumps({k: v for k, v in result.items() if k != 'sources_sha256'}, indent=2))
|
||||
Reference in New Issue
Block a user