diff --git a/docs/ford_feedback_delay_v21.md b/docs/ford_feedback_delay_v21.md new file mode 100644 index 0000000000..4796b3bca2 --- /dev/null +++ b/docs/ford_feedback_delay_v21.md @@ -0,0 +1,98 @@ +# Ford feedback timing follows comma's request-history pattern + +The base C0/C1 mapping continues to use the latest selected curvature. C0 P, +C1 P and C1 I now calculate error from an earlier selected curvature, using +the same one-second request buffer and frame-index expression as comma's torque +controller. The buffer advances at the 100 Hz controls rate, including while +disengaged. The outgoing base request is never buffered for later transmission. + +`controlsd` passes the same `lat_delay` value it passes to the upstream lateral +controller: `lateralDelay.lateralDelay + LAT_SMOOTH_SECONDS`. The existing extra +Ford model preview remains model-side; this change does not add it to feedback +delay or infer hardware delay from a turn's angle crossing. In the four replayed +routes the published delay was 0.1689463705 s and `LAT_SMOOTH_SECONDS` was zero. +Upstream's integer indexing selects the request 16 control cycles earlier, +nominally 0.16 s. The maximum lookback is 99 cycles, nominally 0.99 s. + +The history is in curvature units because that is this controller's feedback +quantity. This adopts upstream's timing pattern, not its torque conversion, +acceleration gain schedule, friction/jerk feedforward or low-speed integral +freeze. Ford gains, formulas, anti-windup, integration cadence, field bounds, +upstream curvature limiting and driver/PSCM arbitration remain unchanged. +Action C0 P remains 1.0; direct-path C0 P remains 0.5; C1 P=0.75 and I=1.0. +Both Ford reference modes use the new timing. + +The history starts at zero, like upstream. Ordinary disengagement clears P/I +while continuing to record requests. Existing invalid-input, timing and path +resets also clear request history, preventing invalid retained references from +surviving recovery. A nonfinite delay rejects the active command; finite delay +values use upstream's bounded frame selection. An explicit zero delay recovers +the previous command law exactly. Existing offline feedback-reference overrides +remain available; production supplies delay rather than an override. + +Diagnostics identify `v21-delayed-feedback` and report the requested delay, +nominal frame delay, delayed feedback curvature/error and latest requested +curvature separately. The reported frame delay is not a measured physical delay. + +## Verification and limitations + +The delay-tracking regression failed against the old feedback behavior: a wheel +following a request with the configured delay still received correction. It +passes with delayed feedback through entry, reversal and release, in both turn +directions and both reference modes. A separate test executes the actual +upstream torque controller's buffer-selection expressions as an oracle. + +Integration tests execute controlsd's actual delay wiring, source selection, +limiter, adapter and Float32 publication through the Ford CAN sender. A +delay-matched wheel trace generates zero P/I while the current base commands +reach CAN immediately. Persistent error still integrates; repeated measurements +do not integrate twice; driver and fresh PSCM override still clear correction +immediately. Exact suite counts are in the validation JSON. + +Four native-time route replays cover 583,599 control cycles and 58,362 CAN +pack/decode checks. The baseline is the filtered-driver controller at +`baeabfaa807c7a07baf183968e570f2c1d3fd665`. Baseline commands match the archived +v20 replay exactly on all four routes. A third controller with zero delay +matches that baseline's commands and integral on every cycle. With delay, +command validity, base heading/overflow and feedback arbitration remain equal. +All commands remain bounded with C2=C3=0. + +| Route | Low-speed C0 changes >0.25 m, v20 → v21 | Low-speed C1 changes >0.05 rad, v20 → v21 | C1 at field bound, v20 → v21 | +| --- | --- | --- | --- | +| 162 | 70 → 68 | 50 → 44 | 1.06 → 0.92 s | +| 157 | 137 → 110 | 59 → 51 | 6.52 → 5.94 s | +| 151 | 77 → 65 | 42 → 36 | 4.04 → 3.89 s | +| 149 | 197 → 159 | 111 → 95 | 16.08 → 13.23 s | + +Low speed means below 15 mph; these counts include valid driver-interaction +periods and are command-continuity measurements, not autonomous tracking scores. +Neither baseline nor candidate was driven in these recordings; both command +streams use the same recorded requests and vehicle motion. + +The change reduces transient correction. At route-162 time 302.649 s, requesting +141.4 degrees with the wheel at 87.3 degrees, replay C0 changes from -2.19 to +-1.73 m and C1 from -0.3695 to -0.3135 rad. At 307.502 s, after driver input, +opposite-direction release C0 falls from +1.52 to +1.04 m and C1 from +0.0655 +to +0.0355 rad. The base request is unchanged in each example. + +This can mean less correction during both entry and unwind. Fewer large command +steps do not prove reduced physical oscillation, maintained turn authority or +better release. These are frozen-motion replays, not a validated PSCM simulator. +The supplied delay has not been identified specifically for the combined C0/C1 +response. The next physical evaluation must distinguish these effects; this is +not a demonstrated death-wobble fix. + +Reproduce with the project's Python environment and built dependencies: + +```sh +PYTHONPATH=.:opendbc_repo:.cache/ford_geometry_deps python \ + tools/ford_pscm_lab/filtered_driver_replay.py \ + --source .cache/ford_route162/full \ + --delay-intake .cache/ford_route162/intake.npz \ + --baseline baeabfaa807c7a07baf183968e570f2c1d3fd665 \ + --output .cache/ford_feedback_delay_v21/162 +``` + +Pull and restart the software while offroad. The existing master toggle still +selects upstream Ford control when disabled. For the current action trial, +leave Model Geometry Reference and C0 one-second distance disabled. diff --git a/docs/ford_feedback_delay_v21_validation.json b/docs/ford_feedback_delay_v21_validation.json new file mode 100644 index 0000000000..bc2d79188f --- /dev/null +++ b/docs/ford_feedback_delay_v21_validation.json @@ -0,0 +1,391 @@ +{ + "baseline_commit": "baeabfaa807c7a07baf183968e570f2c1d3fd665", + "tests": { + "passed": 722, + "subtests_passed": 2 + }, + "total_cycles": 583599, + "total_wire_checks": 58362, + "code_sha256": { + "openpilot/selfdrive/controls/lib/ford_model_action.py": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d", + "openpilot/selfdrive/controls/controlsd.py": "3427212538fcb1cf4d8a44f6be62c54b31ab5ba1a3eb16d958bb58f9c52abecf", + "openpilot/selfdrive/controls/lib/latcontrol_torque.py": "83ea33d37c349b10eca994150dae2a5841ec56b7038402432205a74054c917ee", + "tools/ford_pscm_lab/filtered_driver_replay.py": "9c210b31df62cb6f41e6cf10d76e2e7ba15eefbbfcb30a8ea0244e095365543a" + }, + "method": "Same frozen recorded inputs for v20, v21 using logged lateralDelay, and v21 with zero delay. The latter must exactly match v20 at every cycle. No counterfactual wheel response is predicted.", + "limitations": "Supplied delay is not an identified C0/C1-specific response model. Lower command steps do not establish physical stability. Feedback demand is lower on rising and falling requests; entry and unwind can weaken.", + "points": [ + { + "meaning": "Clean entry shortfall", + "t": 302.64934647200005, + "desired_angle": 141.37510681152344, + "old_c0": -2.1900000000000004, + "new_c0": -1.7300000000000004, + "old_c1": -0.36950000000000005, + "new_c1": -0.3135, + "old_i": -0.05458394920016066, + "new_i": -0.02249363859059498, + "old_reference": -0.03498964384198189, + "new_reference": -0.030434442684054375 + }, + { + "meaning": "Release following driver input; command comparison only", + "t": 307.502298498, + "desired_angle": 22.443912506103516, + "old_c0": 1.52, + "new_c0": 1.04, + "old_c1": 0.0655, + "new_c1": 0.035499999999999976, + "old_i": 0.018708751669361232, + "new_i": 0.01363232660438634, + "old_reference": -0.005546241067349911, + "new_reference": -0.010331000201404095 + } + ], + "routes": { + "162": { + "cycles": 49003, + "wire_checks": 4901, + "zero_delay_matches_baseline": true, + "feedback_delay_requested_min_median_max": [ + 0.16894637048244476, + 0.16894637048244476, + 0.16894637048244476 + ], + "feedback_delay_used_min_median_max": [ + 0.16, + 0.16, + 0.16 + ], + "baseline_recorded_error_p50_p95_p99_max": { + "c0": [ + 2.3841861818141297e-09, + 5.2452087118126656e-08, + 0.39000000458657785, + 2.5800000023841863 + ], + "c1": [ + 0.001499999597668611, + 0.07550000331401825, + 0.11149999844551088, + 0.24500000381469733 + ] + }, + "latched_angle_match_fraction": 1.0, + "command_changes": { + "c0": [ + 0.019999999999999574, + 0.22000000000000064, + 0.4300000000000006, + 0.839999999999999 + ], + "c1": [ + 0.003500000000000003, + 0.02899999999999997, + 0.04049999999999998, + 0.0635 + ] + }, + "variants": { + "old": { + "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 + }, + "new": { + "feedback_switches_active": 218, + "feedback_switches_low_speed": 95, + "low_speed_c0_steps_over_025m": 68, + "low_speed_c1_steps_over_005rad": 44, + "low_speed_step_p99": { + "c0": 0.13999999999999968, + "c1": 0.018449999999999786 + }, + "raw_crossing_c0_steps_over_025m": 0, + "c0_bound_active_s": 0.0, + "c1_bound_active_s": 0.9192208439999945 + } + }, + "method": "Compare Ford feedback variants 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": "baeabfaa807c7a07baf183968e570f2c1d3fd665", + "baseline_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773", + "candidate_controller_sha256": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d", + "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", + ".cache/ford_route162/intake.npz": "9455b01e1d1d3dcba08276b382a8c6cd54ff6fb19cb417b72d0ac89cf193034b" + }, + "baseline_matches_archived_v20_commands_and_integral": true + }, + "157": { + "cycles": 76554, + "wire_checks": 7656, + "zero_delay_matches_baseline": true, + "feedback_delay_requested_min_median_max": [ + 0.0, + 0.16894637048244476, + 0.16894637048244476 + ], + "feedback_delay_used_min_median_max": [ + 0.16, + 0.16, + 0.16 + ], + "baseline_recorded_error_p50_p95_p99_max": { + "c0": [ + 0.009999999999999787, + 0.41999999010562905, + 1.3956999619007082, + 5.39999999165535 + ], + "c1": [ + 4.291534405620467e-09, + 0.03549999962002037, + 0.06600000168576836, + 0.4085000002384186 + ] + }, + "latched_angle_match_fraction": 1.0, + "command_changes": { + "c0": [ + 0.009999999999999787, + 0.21999999999999975, + 0.4900000000000002, + 1.5100000000000007 + ], + "c1": [ + 0.0030000000000000027, + 0.028999999999999915, + 0.039000000000000035, + 0.09049999999999997 + ] + }, + "variants": { + "old": { + "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 + }, + "new": { + "feedback_switches_active": 224, + "feedback_switches_low_speed": 148, + "low_speed_c0_steps_over_025m": 110, + "low_speed_c1_steps_over_005rad": 51, + "low_speed_step_p99": { + "c0": 0.16000000000000014, + "c1": 0.01750000000000007 + }, + "raw_crossing_c0_steps_over_025m": 0, + "c0_bound_active_s": 0.2999506119999751, + "c1_bound_active_s": 5.943273473999852 + } + }, + "method": "Compare Ford feedback variants 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": "baeabfaa807c7a07baf183968e570f2c1d3fd665", + "baseline_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773", + "candidate_controller_sha256": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d", + "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", + ".cache/ford_route157/intake.npz": "5dffd86fde68197727d6c6b9c5eae82320a456fa1206c4cf18e5e9c69419dde4" + }, + "baseline_matches_archived_v20_commands_and_integral": true + }, + "151": { + "cycles": 325708, + "wire_checks": 32571, + "zero_delay_matches_baseline": true, + "feedback_delay_requested_min_median_max": [ + 0.16894637048244476, + 0.16894637048244476, + 0.16894637048244476 + ], + "feedback_delay_used_min_median_max": [ + 0.16, + 0.16, + 0.16 + ], + "baseline_recorded_error_p50_p95_p99_max": { + "c0": [ + 0.010000000223516992, + 0.2000000013411043, + 1.010000009727478, + 4.550000023841858 + ], + "c1": [ + 0.004999999240040742, + 0.02900000113248826, + 0.07250000010803342, + 0.28449999523162844 + ] + }, + "latched_angle_match_fraction": 1.0, + "command_changes": { + "c0": [ + 0.009999999999999787, + 0.07999999999999999, + 0.2900000000000002, + 1.2599999999999998 + ], + "c1": [ + 0.0025000000000000022, + 0.02250000000000002, + 0.038500000000000034, + 0.07650000000000001 + ] + }, + "variants": { + "old": { + "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 + }, + "new": { + "feedback_switches_active": 259, + "feedback_switches_low_speed": 101, + "low_speed_c0_steps_over_025m": 65, + "low_speed_c1_steps_over_005rad": 36, + "low_speed_step_p99": { + "c0": 0.10000000000000053, + "c1": 0.01050000000000001 + }, + "raw_crossing_c0_steps_over_025m": 0, + "c0_bound_active_s": 0.0, + "c1_bound_active_s": 3.8897953380001127 + } + }, + "method": "Compare Ford feedback variants 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": "baeabfaa807c7a07baf183968e570f2c1d3fd665", + "baseline_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773", + "candidate_controller_sha256": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d", + "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", + ".cache/ford_route151/intake.npz": "18bbefb7738cebd0071dc987e90f74469a0c8ed456f9412324030592cafd68c9" + }, + "baseline_matches_archived_v20_commands_and_integral": true + }, + "149": { + "cycles": 132334, + "wire_checks": 13234, + "zero_delay_matches_baseline": true, + "feedback_delay_requested_min_median_max": [ + 0.16894637048244476, + 0.16894637048244476, + 0.16894637048244476 + ], + "feedback_delay_used_min_median_max": [ + 0.16, + 0.16, + 0.16 + ], + "baseline_recorded_error_p50_p95_p99_max": { + "c0": [ + 0.02999999910593054, + 0.8400000008046626, + 2.1400000276565554, + 4.560000002980233 + ], + "c1": [ + 0.015500000141560999, + 0.10499999982118613, + 0.32499999746203423, + 0.43149999833107 + ] + }, + "latched_angle_match_fraction": 1.0, + "command_changes": { + "c0": [ + 0.009999999999999787, + 0.1999999999999993, + 0.4900000000000002, + 1.3100000000000005 + ], + "c1": [ + 0.0040000000000000036, + 0.03200000000000003, + 0.04999999999999999, + 0.08850000000000002 + ] + }, + "variants": { + "old": { + "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 + }, + "new": { + "feedback_switches_active": 354, + "feedback_switches_low_speed": 171, + "low_speed_c0_steps_over_025m": 159, + "low_speed_c1_steps_over_005rad": 95, + "low_speed_step_p99": { + "c0": 0.16000000000000014, + "c1": 0.017500000000000016 + }, + "raw_crossing_c0_steps_over_025m": 0, + "c0_bound_active_s": 0.0, + "c1_bound_active_s": 13.22902962499984 + } + }, + "method": "Compare Ford feedback variants 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": "baeabfaa807c7a07baf183968e570f2c1d3fd665", + "baseline_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773", + "candidate_controller_sha256": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d", + "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", + ".cache/ford_route149/intake.npz": "503919c4f1566ef850007c59f16ce074697ba57732f21beb7b158c1ca7be3a9d" + }, + "baseline_matches_archived_v20_commands_and_integral": true + } + } +} diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 30be646c7c..5c3eeec42c 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -180,6 +180,7 @@ class Controls(ControlsExt): model_time=self.sm.logMonoTime['modelV2'] * 1e-9, reference_time=self.sm.logMonoTime[reference_service] * 1e-9, active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]), + lat_delay=lat_delay, driver_pressed=CS.steeringPressed, driver_torque=CS.steeringTorque, reference_source=reference_service, roll=lp.roll, pscm_status=self.sm['carStateSP'].fordPscmStatus if self.sm.valid['carStateSP'] else None, diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 5abc136437..c86b85261c 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -8,12 +8,14 @@ Commands use the current bounded request without an additional C0/C1 slew. The direct-path trial samples model position and heading separately, retains independent upstream request limits, and uses heading-equivalent feedback. """ +from collections import deque import math import struct import numpy as np from opendbc.car.ford.values import FordFlags +from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path @@ -28,6 +30,7 @@ DIRECT_PATH_C0_PROPORTIONAL_GAIN = 0.5 C0_RESPONSE_CONSTANT = 0.010717679293424373 C0_RESPONSE_INVERSE_SPEED_SQUARED = 0.018122981795212647 CALIBRATION_APPROVED = False +CURVATURE_REQUEST_BUFFER_SECONDS = 1.0 def _packed(value, resolution, offset): @@ -177,8 +180,9 @@ class FordModelActionController: controlsd owns upstream selection/limiting and service health. This adapter checks ages and clock order, then supplies elapsed time to the core. - Feedback advances once per fresh steering measurement; repeated samples - still use the current request. Raw model geometry is checked on every cycle. + Base commands use the latest request. Like comma's torque controller, feedback + uses a one-second request buffer indexed by lateralDelay at the 100 Hz control + cadence. Only integration waits for fresh steering measurements. CAN yaw remains a health gate, not the feedback measurement. Ford's filtered steeringPressed and fresh PSCM driver overrides clear the correction. @@ -192,8 +196,10 @@ 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-v20-filtered-driver' if self.direct_path else - 'model-action-curvature-c0-feedback-v20-filtered-driver') + self.request_buffer_size = int(CURVATURE_REQUEST_BUFFER_SECONDS / DT_CTRL) + self.request_buffer = deque([0.] * self.request_buffer_size, maxlen=self.request_buffer_size) + self.hypothesis = ('model-path-direct-feedback-v21-delayed-feedback' if self.direct_path else + 'model-action-curvature-c0-feedback-v21-delayed-feedback') self.reset() def path_curvature(self, model, speed): @@ -211,6 +217,8 @@ class FordModelActionController: def reset(self, status='inactive'): self.core.reset() + if status != 'inactive': + self.request_buffer = deque([0.] * self.request_buffer_size, maxlen=self.request_buffer_size) self.last_time = self.last_measurement_time = self.last_model_time = None self.diagnostics = {'status': status, 'hypothesis': self.hypothesis, 'c0_time_based': self.core.c0_time_based, @@ -218,13 +226,22 @@ class FordModelActionController: def update(self, model, desired_curvature, *, current_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time, active, valid=True, driver_pressed=False, driver_torque=0., pscm_status=None, - feedback_curvature=None, curvature_scale=1., reference_source='modelV2', roll=0.): + feedback_curvature=None, curvature_scale=1., reference_source='modelV2', roll=0., lat_delay=0.): + # Record on every control cycle, including disengagement, as upstream does. + # This buffer changes feedback only; it never queues the outgoing base path. + feedback_delay = 0. + if _finite(desired_curvature, lat_delay) and abs(desired_curvature) <= 1.: + self.request_buffer.append(desired_curvature) + delay_frames = int(np.clip(lat_delay / DT_CTRL + 1, 1, self.request_buffer_size)) + if feedback_curvature is None: + feedback_curvature = self.request_buffer[-delay_frames] + feedback_delay = (delay_frames - 1) * DT_CTRL reason = None if not active: reason = 'inactive' elif not valid: reason = 'invalid_service' - elif not _finite(desired_curvature, current_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time): + elif not _finite(desired_curvature, current_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time, lat_delay): reason = 'nonfinite' elif not all(-.005 <= now - timestamp <= .15 for timestamp in (measurement_time, model_time, reference_time)): reason = 'stale_input' @@ -267,6 +284,7 @@ class FordModelActionController: 'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time, 'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1, 'curvature_error': desired_curvature-current_curvature, 'feedback_dt': feedback_dt, + 'feedback_delay_requested': lat_delay, 'feedback_delay': feedback_delay, 'heading_feedforward': base_heading, 'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading), 'offset_proportional': self.core.offset_proportional, 'c0_proportional_gain': self.core.c0_proportional_gain, diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index 91078a020a..dcfc95dfe3 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-v20-filtered-driver') + self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-feedback-v21-delayed-feedback') 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_feedback_delay.py b/openpilot/selfdrive/controls/tests/test_ford_feedback_delay.py new file mode 100644 index 0000000000..025377d233 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_feedback_delay.py @@ -0,0 +1,127 @@ +"""Feedforward remains current while feedback follows comma's request history.""" +import ast +from collections import deque +import math +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, encode_model_action +from openpilot.selfdrive.controls.lib.ford_path import FordPath +from openpilot.selfdrive.controls.tests.test_ford_model_action import straight +from openpilot.selfdrive.controls.tests.test_ford_model_action_feedback import status + + +def step(controller, frame, desired, measured=0., **overrides): + now = 1.+frame*.01 + kwargs = {'current_curvature': measured, 'speed': 5., 'yaw_rate': 0., 'now': now, + 'measurement_time': now, 'model_time': now, 'reference_time': now, + 'active': True, 'lat_delay': .2} + kwargs.update(overrides) + return controller.update(straight(), desired, **kwargs) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('direct_path', [False, True]) +def test_perfectly_delayed_tracking_does_not_generate_phantom_feedback(sign, direct_path): + controller = FordModelActionController(direct_path=direct_path) + requests = [0.]*100+[sign*.004]*100+[-sign*.003]*100+[0.]*100 + for i, desired in enumerate(requests): + measured = requests[max(0, i-20)] + command = step(controller, i, desired, measured) + assert command.valid + assert controller.core.feedback_curvature == pytest.approx(measured) + assert controller.core.proportional == controller.core.offset_proportional == controller.core.correction == 0. + # Current heading reverses/releases immediately even while feedback uses the old request. + assert command.path_angle == pytest.approx(7.*desired, abs=.00025) + if not direct_path: + assert command.path_offset == pytest.approx(encode_model_action(straight(), desired, 5.).path_offset, abs=.005) + + +@pytest.mark.parametrize('delay', [-1., 0., .009, .16894637048244476, .2, .4, .99, 2.]) +def test_same_buffer_index_as_comma_torque_controller(delay): + controller = FordModelActionController() + upstream = deque([0.]*100, maxlen=100) + # Execute the actual upstream selection expressions, not a second copy of our formula. + source_path = Path(__file__).resolve().parents[1]/'lib/latcontrol_torque.py' + tree = ast.parse(source_path.read_text()) + cls = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'LatControlTorque') + method = next(n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name == 'update') + nodes = [n for n in method.body if isinstance(n, ast.Assign) and + ast.unparse(n.targets[0]) in ('delay_frames', 'expected_lateral_accel')] + assert len(nodes) == 2 + oracle = compile(ast.Module(body=nodes, type_ignores=[]), str(source_path), 'exec') + torque = SimpleNamespace(dt=.01, lat_accel_request_buffer=upstream, lat_accel_request_buffer_len=100) + for i in range(230): + desired = .005*math.sin(i*.023) + upstream.append(desired) + context = {'self': torque, 'np': np, 'lat_delay': delay} + exec(oracle, context) + expected = context['expected_lateral_accel'] + step(controller, i, desired, lat_delay=delay) + assert controller.core.feedback_curvature == expected + assert controller.diagnostics['feedback_delay'] == pytest.approx((context['delay_frames']-1)*.01) + + +def test_history_keeps_up_while_disengaged_and_delay_changes_without_another_queue(): + controller = FordModelActionController() + requests = [] + for i in range(170): + desired = .00001*i + requests.append(desired) + if i < 130: + assert step(controller, i, desired, active=False) == FordPath() + else: + delay = .1 if i < 150 else .3 + measured = requests[i-round(delay/.01)] + step(controller, i, desired, measured, lat_delay=delay) + assert controller.core.feedback_curvature == pytest.approx(measured) + assert controller.core.correction == 0. + + +def test_persistent_error_still_builds_correction_and_repeated_measurements_do_not_double_integrate(): + controller = FordModelActionController() + for i in range(150): + step(controller, i, .004, active=False) + step(controller, 150, .004, .003) + for i in range(151, 251): + step(controller, i, .004, .003) + assert controller.core.proportional > 0. and controller.core.offset_proportional > 0. + assert controller.core.correction == pytest.approx(.005) + for i in range(251, 256): + step(controller, i, .004, .003, measurement_time=3.5) + assert controller.core.correction == pytest.approx(.005) + + +@pytest.mark.parametrize('override', ['driver', 'pscm']) +def test_driver_override_still_clears_feedback_immediately_with_delayed_reference(override): + controller = FordModelActionController() + for i in range(150): + step(controller, i, .004) + assert controller.core.correction > 0. + kwargs = {'driver_pressed': True} if override == 'driver' else {'pscm_status': status(2.5, limit=3)} + command = step(controller, 150, -.004, **kwargs) + assert controller.core.proportional == controller.core.offset_proportional == controller.core.correction == 0. + assert command.path_angle < 0. + + +@pytest.mark.parametrize('delay', [math.nan, math.inf, None]) +def test_invalid_delay_rejects_command_without_raising(delay): + controller = FordModelActionController() + assert step(controller, 0, .004, lat_delay=delay) == FordPath() + assert controller.diagnostics['status'] == 'nonfinite' + + +@pytest.mark.parametrize('invalid', [{'valid': False}, {'desired': math.nan}, {'lat_delay': math.nan}, {'now': 5.}]) +def test_bad_input_or_timing_resets_request_history(invalid): + controller = FordModelActionController() + for i in range(150): + step(controller, i, .004) + options = dict(invalid) + desired = options.pop('desired', .004) + assert step(controller, 150, desired, **options) == FordPath() + assert all(value == 0. for value in controller.request_buffer) + assert step(controller, 151, 0.).valid + assert controller.core.feedback_curvature == 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 e4df523c54..4338909df8 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -145,8 +145,11 @@ def pipeline(): selection = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.sm.valid['lateralManeuverPlan']") limiter = next(n for n in body if isinstance(n, ast.Assign) and isinstance(n.value, ast.Call) and isinstance(n.value.func, ast.Name) and n.value.func.id == 'clip_curvature') + delay = next(n for n in body if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'lat_delay') + model_source = ast.parse((root/'selfdrive/modeld/modeld.py').read_text()) + smoothing = next(n for n in model_source.body if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'LAT_SMOOTH_SECONDS') branch = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.CP.brand == 'ford'") - call = compile(ast.Module(body=[selection, limiter, branch], type_ignores=[]), str(controls_file), 'exec') + call = compile(ast.Module(body=[smoothing, selection, limiter, delay, branch], type_ignores=[]), str(controls_file), 'exec') publication_file = root/'sunnypilot/selfdrive/controls/controlsd_ext.py' body = _method(publication_file, 'ControlsExt', 'state_control_ext').body publish = [n for n in body if (isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'ford_path') or @@ -163,7 +166,8 @@ class Subscriptions: self.valid = {'lateralManeuverPlan': maneuver, 'modelV2': True, 'carStateSP': True} self.logMonoTime = {'carState': 995_000_000, 'modelV2': 980_000_000, 'lateralManeuverPlan': 990_000_000} self.failed = set() - self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1)} + self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1), + 'lateralDelay': SimpleNamespace(lateralDelay=0.)} def __getitem__(self, service): return self.messages[service] @@ -291,6 +295,51 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline assert (core.offset_proportional == 0.) == (cs.steeringPressed or measured == sign*.004) +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('delay', [.16894637048244476, .2]) +def test_controlsd_uses_lateral_delay_for_feedback_and_current_base_on_can(pipeline, sign, delay): + call, publication = pipeline + controls, sm = startup(), Subscriptions(False) + controls.sm, controls.desired_curvature = sm, 0. + sm.messages['lateralDelay'].lateralDelay = delay + model = straight() + cc = structs.CarControl(latActive=True) + cs = SimpleNamespace(vEgo=5., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=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=5., vEgoRaw=5.), 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) + history = [0.]*100 + for frame, desired in enumerate([0.]*100+[sign*.004]*100+[-sign*.003]*100+[0.]*100): + now = 1.+frame*.01 + model.action = SimpleNamespace(desiredCurvature=desired) + selected, _ = clip_curvature(cs.vEgo, controls.desired_curvature, desired, 0.) + history.append(selected) + controls.curvature = history[-int(delay/.01+1)] + sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9)) + 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)}) + core = controls.ford_path_controller.core + assert core.feedback_curvature == controls.curvature + assert core.proportional == core.offset_proportional == core.correction == 0. + assert_current_request(core, selected, cs.vEgo) + 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)) + received = parser.update([round(now*1e9), packets]) + address = parser.dbc.name_to_msg['LateralMotionControl2'].address + assert address in received + wire = parser.vl['LateralMotionControl2'] + assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-encode_model_action(model, selected, cs.vEgo).path_offset, abs=.005) + assert wire['LatCtlPath_An_Actl'] == pytest.approx(-7.*selected, abs=.00025) + assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0. + 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]) + + @pytest.mark.parametrize('service_valid', [False, True]) def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, service_valid): controls, sm = startup(), Subscriptions(False) @@ -363,7 +412,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-v20-filtered-driver' + assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v21-delayed-feedback' 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_selection.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py index 10c29180a8..9c7d5d127c 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-v20-filtered-driver' + assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v21-delayed-feedback' 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 index 35676ef23e..c42fa5bdd7 100644 --- a/tools/ford_pscm_lab/filtered_driver_replay.py +++ b/tools/ford_pscm_lab/filtered_driver_replay.py @@ -1,4 +1,4 @@ -"""Compare driver-input arbitration on recorded references and frozen vehicle motion. +"""Compare Ford feedback variants 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. @@ -6,6 +6,7 @@ Both controllers receive the logged selected curvature. Recorded-command agreeme is expected only for routes driven with the selected baseline and fixed 7 m C0. """ import argparse +import ast import hashlib import json from pathlib import Path @@ -19,7 +20,7 @@ from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionCo from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table -def replay(source, destination, baseline_class, provenance): +def replay(source, destination, baseline_class, provenance, *, delay_intake=None): 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')} @@ -28,6 +29,16 @@ def replay(source, destination, baseline_class, provenance): path_ns = z['ns'] c = r['controls'] t = c['t'] + delays = np.zeros(len(t)) + if delay_intake is not None: + with np.load(delay_intake) as z: + stream = z['delay'] + # Match controlsd's lateralDelay message plus its imported modeld constant. + source_tree = ast.parse(Path('openpilot/selfdrive/modeld/modeld.py').read_text()) + smoothing = next(ast.literal_eval(n.value) for n in source_tree.body + if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'LAT_SMOOTH_SECONDS') + delays = sample({'t': stream[:, 0], 'value': stream[:, 3]}, t)['value'] + smoothing + delays[t < stream[0, 0]] = smoothing 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) @@ -38,10 +49,12 @@ def replay(source, destination, baseline_class, provenance): steerRatio=car['steer_ratio'], rotationalInertia=0.) vm = VehicleModel(cp) cores = [baseline_class(), FordModelActionController()] + zero_delay = FordModelActionController() if delay_intake is not None else None 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'] + 'recorded_c0', 'recorded_c1', 'latched_angle_error', 'model_ns', 'desired_angle', + 'desired', 'measured', 'old_reference', 'new_reference', 'feedback_delay', 'old_c0_p', 'new_c0_p', 'old_c1_p', 'new_c1_p'] 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)) @@ -54,12 +67,13 @@ def replay(source, destination, baseline_class, provenance): 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) + inputs = {'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} + for variant, core in enumerate(cores): + delay_args = {'lat_delay': delays[i]} if variant == 1 and delay_intake is not None else {} + command = core.update(models[mi[i]], c['desired'][i], **inputs, **delay_args) 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)): @@ -67,13 +81,21 @@ def replay(source, destination, baseline_class, provenance): assert core.core.correction == core.core.proportional == core.core.offset_proportional == 0. commands.append(command) assert commands[0].valid == commands[1].valid + if zero_delay is not None: + assert zero_delay.update(models[mi[i]], c['desired'][i], **inputs, lat_delay=0.) == commands[0] + assert zero_delay.core.correction == cores[0].core.correction + for key in ('heading_feedforward', 'offset_overflow', 'driver_override', 'feedback_enabled', 'pscm_limited'): + assert cores[0].diagnostics.get(key) == cores[1].diagnostics.get(key) 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]] + sent['c0'][i], sent['c1'][i], abs(cs['angle'][i]-c['actual_angle'][i]), c['model_ns'][i], c['desired_angle'][i], + c['desired'][i], c['measured'][i], cores[0].core.feedback_curvature, cores[1].core.feedback_curvature, + cores[1].diagnostics.get('feedback_delay', 0.), cores[0].core.offset_proportional, cores[1].core.offset_proportional, + cores[0].core.proportional, cores[1].core.proportional] a = dict(zip(names, rows.T, strict=True)) live = a['valid'].astype(bool) consecutive = live[1:] & live[:-1] & (np.diff(t) < .03) @@ -83,6 +105,9 @@ def replay(source, destination, baseline_class, provenance): & ~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, + 'zero_delay_matches_baseline': True if zero_delay is not None else None, + 'feedback_delay_requested_min_median_max': np.quantile(delays, [0., .5, 1.]).tolist(), + 'feedback_delay_used_min_median_max': np.quantile(a['feedback_delay'][live], [0., .5, 1.]).tolist(), '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)), @@ -104,6 +129,8 @@ def replay(source, destination, baseline_class, provenance): 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')} + if delay_intake is not None: + metrics['sources_sha256'][str(delay_intake)] = hashlib.sha256(delay_intake.read_bytes()).hexdigest() 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') @@ -115,6 +142,7 @@ if __name__ == '__main__': parser.add_argument('--source', type=Path, required=True) parser.add_argument('--output', type=Path, required=True) parser.add_argument('--baseline', default='4900c0a40c87c72000b7168a6cf4fe6dd98ea6d0') + parser.add_argument('--delay-intake', type=Path, help='intake.npz with logged lateralDelay; requires a baseline with identical gains and driver gating') 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' @@ -124,5 +152,5 @@ if __name__ == '__main__': 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) + result = replay(args.source, args.output, namespace['FordModelActionController'], provenance, delay_intake=args.delay_intake) print(json.dumps({k: v for k, v in result.items() if k != 'sources_sha256'}, indent=2))