diff --git a/docs/ford_filtered_driver_v20.md b/docs/ford_filtered_driver_v20.md new file mode 100644 index 0000000000..91d3751bb2 --- /dev/null +++ b/docs/ford_filtered_driver_v20.md @@ -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. diff --git a/docs/ford_filtered_driver_v20_validation.json b/docs/ford_filtered_driver_v20_validation.json new file mode 100644 index 0000000000..b006fa8c0f --- /dev/null +++ b/docs/ford_filtered_driver_v20_validation.json @@ -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 + } + } +} diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 02da792340..5abc136437 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -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' diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index 347a4273d2..91078a020a 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -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']) diff --git a/openpilot/selfdrive/controls/tests/test_ford_direct_path.py b/openpilot/selfdrive/controls/tests/test_ford_direct_path.py index 60328f72d5..b19617de99 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_direct_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_direct_path.py @@ -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. diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py index ceb44175f3..e4df523c54 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -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. diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_driver_input.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_driver_input.py new file mode 100644 index 0000000000..81a934a27a --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_driver_input.py @@ -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. diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py index a6ba145dda..ebb3ac1594 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py @@ -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)}]) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py index 77a53a4655..10c29180a8 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py @@ -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 diff --git a/tools/ford_pscm_lab/filtered_driver_replay.py b/tools/ford_pscm_lab/filtered_driver_replay.py new file mode 100644 index 0000000000..35676ef23e --- /dev/null +++ b/tools/ford_pscm_lab/filtered_driver_replay.py @@ -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, '', '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))