diff --git a/docs/ford_filtered_driver.md b/docs/ford_filtered_driver.md new file mode 100644 index 0000000000..68e8cdab9d --- /dev/null +++ b/docs/ford_filtered_driver.md @@ -0,0 +1,96 @@ +# Ford feedback continuity trial + +The v16 experimental controller uses Ford CarState's existing filtered +`steeringPressed` signal to detect driver input. It no longer independently +checks the same 1 Nm threshold on each raw torque sample. Fresh PSCM driver +override (`limit == 3`) and nonfinite torque still immediately clear feedback. +Filtered driver input also still clears both P and I; nothing changes in Ford's +CarState filter or the vehicle's disengagement logic. + +The full rlogs from `84865544361f55cb/0000015a--ef86556819` and +`84865544361f55cb/0000015b--22b70521d4` contain short torque crossings with +`steeringPressed == false`. At 15a +196.19 s, two samples reach 1.0625 Nm. +The old duplicate check drops proportional correction and clears the integral, +then restores proportional correction when torque falls. During the large left +in 15b, similar pulses at +168.06, +168.14, and +168.28 s repeatedly interrupt +correction. The logs establish these interruptions, not whether each pulse was +deliberate driver input. v16 follows the same driver-input classification already +used elsewhere in controls, instead of maintaining a second interpretation. + +## Scope + +This affects the opted-in Ford C0/C1 controller, with either action or geometry +as its reference. Disabling `FordModelActionController` still selects upstream +Ford control. No new toggle is needed. Diagnostics identify +`model-action-curvature-c0-feedback-v16-filtered-driver`. + +Reference selection, preview, smoothing, gains, integration, command bounds, +PSCM limit handling, and message timing are unchanged. This is a feedback +continuity change, not a wheel-speed limiter or a general cure for abrupt +low-speed motion. + +## Validation + +The regression uses the actual shared `CarStateBase.update_steering_pressed` +filter with Ford's count of 5. Two-sample pulses in either direction preserve +feedback; sustained input clears it on the first filtered pressed sample. +PSCM driver override remains immediate. Before the change, 4 of the 5 new +tests failed; afterwards all 5 passed. Integration tests exercise publication +through controlsd and the real CAN sender. The related controller, path, and +geometry suites pass: **534 tests**. + +The paired replay uses native control cadence and identical recorded +references, steering, speed, driver state, and PSCM status. Baseline is commit +`18ded0380045c29520cf1f690688e48ebad043d8`, with fixed 7 m C0. It replays +44,145 control cycles and checks CAN serialization of 4,415 candidate commands. +All commands remain finite and within field bounds; C2/C3 remain zero. Every +recorded filtered driver input or fresh PSCM override clears feedback. + +Counts below use consecutive valid active commands below 15 mph, with sample +spacing below 30 ms (normally 10 ms). A "step" is an adjacent command change, +not a measured wheel movement or an inferred comfort threshold. + +| Metric | 15a old → new | 15b old → new | Combined old → new | +|---|---:|---:|---:| +| Feedback enabled/disabled transitions | 61 → 25 | 107 → 40 | 168 → 65 | +| C0 steps larger than 0.25 m | 52 → 21 | 85 → 39 | 137 → 60 | +| C1 steps larger than 0.05 rad | 36 → 18 | 32 → 17 | 68 → 35 | + +The baseline matches recorded C1 within Float32 precision on both routes. +Maximum C0 mismatch is Float32 rounding on 15a and one 0.01 m command quantum +on 15b. Logged publication time approximates the core call time, and the replay +cannot reproduce every retained-message service-health check. These limitations +are reported with the replay results, rather than assuming exact reproduction. + +Preserving I can also sustain stronger commands. Time at the C1 field bound +increases from **2.57 to 3.81 s** on 15a and **6.70 to 7.12 s** on 15b. +The candidate's wheel response and unwind cannot be inferred from the recorded +motion: the physical vehicle did not receive these candidate commands. + +Reproduce for each extracted route directory: + +```sh +PYTHONPATH=.:opendbc_repo python tools/ford_pscm_lab/filtered_driver_replay.py \ + --source .cache/ford_route15a/rlog_full \ + --output .cache/ford_filtered_driver/15a +``` + +Input and controller hashes and detailed results are in +`ford_filtered_driver_validation.json`. + +## Why not add preview at the same time? + +The geometry and learned action are separate model outputs; code does not +require them to agree. Geometry converts previewed heading and initial heading +rate to curvature. For fixed heading perturbations, sensitivity grows as speed +falls. This can magnify frame-to-frame plan revisions at low speed; it does not +prove the resulting plan is incorrect. The model's training objectives cannot +be established from inference code alone. + +A separate frozen-plan check added 0.1 or 0.2 s of geometry preview. Selected +turn exits relaxed earlier, but some low-speed reference changes got steeper. +For example, on 15b the maximum target-angle change over 0.2 s rose from about +352 degrees/s to 386 and 429 degrees/s. These are reference changes, not wheel +rates. Extra preview is therefore not included in this feedback continuity +trial. Remaining sharp requests and geometric exit rebounds need to be judged +separately from command interruptions. diff --git a/docs/ford_filtered_driver_validation.json b/docs/ford_filtered_driver_validation.json new file mode 100644 index 0000000000..73740a3e6b --- /dev/null +++ b/docs/ford_filtered_driver_validation.json @@ -0,0 +1,140 @@ +{ + "15a": { + "cycles": 21867, + "wire_checks": 2187, + "baseline_recorded_error_p50_p95_p99_max": { + "c0": [ + 4.172324707951702e-09, + 5.2452087118126656e-08, + 8.583068833445395e-08, + 1.1444091807533141e-07 + ], + "c1": [ + 8.791685157660822e-10, + 1.1682510403510094e-08, + 1.4305114759416426e-08, + 1.478195188475695e-08 + ] + }, + "latched_angle_match_fraction": 1.0, + "command_changes": { + "c0": [ + 0.0, + 0.0, + 0.541800000000003, + 1.6400000000000006 + ], + "c1": [ + 0.0, + 0.1825, + 0.20749999999999996, + 0.21100000000000008 + ] + }, + "variants": { + "old": { + "feedback_switches_active": 112, + "feedback_switches_low_speed": 61, + "low_speed_c0_steps_over_025m": 52, + "low_speed_c1_steps_over_005rad": 36, + "low_speed_step_p99": { + "c0": 0.4996000000000004, + "c1": 0.05668000000000003 + }, + "c1_bound_active_s": 2.5702606670000137 + }, + "new": { + "feedback_switches_active": 50, + "feedback_switches_low_speed": 25, + "low_speed_c0_steps_over_025m": 21, + "low_speed_c1_steps_over_005rad": 18, + "low_speed_step_p99": { + "c0": 0.15480000000000044, + "c1": 0.02622000000000008 + }, + "c1_bound_active_s": 3.8107828889998814 + } + }, + "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.\n", + "provenance": { + "baseline_commit": "18ded0380045c29520cf1f690688e48ebad043d8", + "baseline_controller_sha256": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca", + "candidate_controller_sha256": "c1132b826a37e6b600ef75683ab8f0a8c784c668472f69f08e217a0a91cca60a", + "fixed_c0_distance_m": 7.0 + }, + "sources_sha256": { + ".cache/ford_route15a/rlog_full/route.npz": "057ce69ce028b2c9c1b42ab8110f35c888a6c398ddcc4156e74afc98c83d119d", + ".cache/ford_route15a/rlog_full/model_paths.npz": "ef820d9df14afa308bf46f95463b636a599ff24fd08bffaff1ae671b3932c24a", + ".cache/ford_route15a/rlog_full/metadata.json": "bea47c7f0c0b583964ced3484fe814ec9a7a24abbfe6272e70f9dde12580a756" + } + }, + "15b": { + "cycles": 22278, + "wire_checks": 2228, + "baseline_recorded_error_p50_p95_p99_max": { + "c0": [ + 6.70552502413102e-10, + 3.814697269177714e-08, + 1.0490417512443173e-07, + 0.009999947547912669 + ], + "c1": [ + 1.9371515502797365e-10, + 5.4836273299940785e-09, + 1.2159347528850617e-08, + 1.478195188475695e-08 + ] + }, + "latched_angle_match_fraction": 0.9999321895978843, + "command_changes": { + "c0": [ + 0.0, + 0.0, + 0.2600000000000003, + 1.740000000000001 + ], + "c1": [ + 0.0020000000000000018, + 0.012999999999999994, + 0.07999999999999996, + 0.15400000000000003 + ] + }, + "variants": { + "old": { + "feedback_switches_active": 238, + "feedback_switches_low_speed": 107, + "low_speed_c0_steps_over_025m": 85, + "low_speed_c1_steps_over_005rad": 32, + "low_speed_step_p99": { + "c0": 0.36999999999999944, + "c1": 0.03771999999999984 + }, + "c1_bound_active_s": 6.701652654999748 + }, + "new": { + "feedback_switches_active": 82, + "feedback_switches_low_speed": 40, + "low_speed_c0_steps_over_025m": 39, + "low_speed_c1_steps_over_005rad": 17, + "low_speed_step_p99": { + "c0": 0.22999999999999904, + "c1": 0.03050000000000002 + }, + "c1_bound_active_s": 7.123324506999893 + } + }, + "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.\n", + "provenance": { + "baseline_commit": "18ded0380045c29520cf1f690688e48ebad043d8", + "baseline_controller_sha256": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca", + "candidate_controller_sha256": "c1132b826a37e6b600ef75683ab8f0a8c784c668472f69f08e217a0a91cca60a", + "fixed_c0_distance_m": 7.0 + }, + "sources_sha256": { + ".cache/ford_route15b/rlog_full/route.npz": "f95e1c16a4677fee0ab57002d714e22716bf11873c308280553312fa9d18aab0", + ".cache/ford_route15b/rlog_full/model_paths.npz": "a90be55943ce0009afbf38ef2035b3fff4de8ab00ea266636247bcc5a5012eae", + ".cache/ford_route15b/rlog_full/metadata.json": "545ef8d6740e57237f35b2ca684111fc2d774e83aa1d6c8a5a0fff1b71270e85" + } + } +} diff --git a/docs/ford_geometry_reference.md b/docs/ford_geometry_reference.md index fc5ee04f01..fa2470be81 100644 --- a/docs/ford_geometry_reference.md +++ b/docs/ford_geometry_reference.md @@ -50,7 +50,10 @@ the toggle is read at modeld startup. publication time, original action curvature, raw geometric curvature, selected curvature, preview, and smoothing time. `valid=false` while enabled identifies fallback. A startup log also identifies action or geometry mode. The existing -Ford diagnostics continue to identify the unchanged v15 feedback controller. +Ford diagnostics identified the unchanged v15 feedback controller in the initial +geometry trial. The subsequent [v16 feedback continuity trial](ford_filtered_driver.md) +uses Ford's filtered driver-input signal and leaves this reference calculation +unchanged. ## Offline validation diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index a62365456a..2428c47594 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -11,7 +11,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.ford_path import FordPath, _model_path @@ -140,15 +140,16 @@ 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, c0_proportional_gain=C0_PROPORTIONAL_GAIN): self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based, c0_proportional_gain=c0_proportional_gain) - self.hypothesis = 'model-action-curvature-c0-feedback-v15' + self.hypothesis = 'model-action-curvature-c0-feedback-v16-filtered-driver' self.reset() def set_c0_time_based(self, enabled, *, lateral_engaged): @@ -194,8 +195,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 noisy torque signal into steeringPressed. + # Rechecking its raw threshold here bypasses that filter and chatters 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))) command = self.core.update(model, desired_curvature, current_curvature=current_curvature, speed=speed, dt=dt, 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 d6205c960f..2f8ef72278 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-v15' + assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v16-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..f366e35d30 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_driver_input.py @@ -0,0 +1,61 @@ +"""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('sign', [-1., 1.]) +def test_twenty_ms_torque_spike_does_not_drop_feedback_before_ford_detects_driver_input(sign): + state = SimpleNamespace(steering_pressed_cnt=0) + controller, steady = FordModelActionController(), FordModelActionController() + # 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('sign', [-1., 1.]) +def test_sustained_driver_input_clears_feedback_on_the_first_filtered_pressed_sample(sign): + state = SimpleNamespace(steering_pressed_cnt=0) + controller = FordModelActionController() + 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 + + +def test_pscm_override_remains_immediate_before_driver_filter_triggers(): + controller = FordModelActionController() + 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..7533b7008c 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': -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 9dfb36c5b4..2ceaa18ec7 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 == .5 - assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v15' + assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v16-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..8d8647a4b4 --- /dev/null +++ b/tools/ford_pscm_lab/filtered_driver_replay.py @@ -0,0 +1,120 @@ +"""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. +""" +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'] + 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])] + 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) + 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')}, + '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='18ded0380045c29520cf1f690688e48ebad043d8') + 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))