diff --git a/docs/ford_geometry_reference.md b/docs/ford_geometry_reference.md new file mode 100644 index 0000000000..fc5ee04f01 --- /dev/null +++ b/docs/ford_geometry_reference.md @@ -0,0 +1,82 @@ +# Ford model geometry reference trial + +`FordGeometryReference` changes the steering reference used by the existing +Ford C0/C1 feedback controller. It is default off and requires +`FordModelActionController` and a Ford CAN FD vehicle. + +In Sunnylink's Ford settings, enable **Model Geometry Reference (Experimental)** +while offroad, then cycle offroad to onroad. Keep **Selected-Action Path Tracking** +enabled. Disable only the geometry toggle to compare with the previous model +action. Disable the controller toggle to restore upstream Ford control. + +## Reference calculation + +After modeld publishes the plan into the message builder, use its orientation +and orientation rate with the existing `get_curvature_from_plan` function: + +``` +curvature = 2 * heading_at_preview / (max(speed, 1) * preview) + - initial_heading_rate / max(speed, 1) +``` + +The existing function stabilizes previews shorter than 0.3 seconds using the +0.3-second heading. The experiment uses the exact `lat_action_t` already supplied +to inference, including learned/fixed delay, model-specific smoothing allowance, +the Ford speed-dependent 0.4-second preview addition, and 75 ms frame/action +timing compensation. CTMV2 on the recent route has roughly 0.744 seconds of +preview below 15 mph. This is a time preview, not direct sampling at seven metres. + +Apply the same model-specific lateral smoothing and standstill hold as the +original action, once per published model frame. Use the published plan itself, +without a separate Planplus multiplier. Both `modelV2.action` and +`drivingModelData.action` receive the selected curvature. Preserve the original +action's independent smoothing history and its acceleration/stop fields. + +controlsd still applies its existing curvature/acceleration/jerk limits and +lateral maneuver override. Its desired curvature and desired steering angle now +describe the selected reference. The unchanged controller converts that same +reference into C0/C1 and closes the loop on measured steering. C0 distance, +C1 distance, P/I gains, driver arbitration, PSCM limit behavior, CAN cadence, +and field bounds are unchanged; C2/C3 stay zero. + +Malformed/nonfinite/incomplete orientation data retains the original action +for that frame and resets geometry smoothing history. Existing model validity +and freshness gates still apply. There is no runtime mode swap during engagement; +the toggle is read at modeld startup. + +## Logging + +`modelDataV2SP.fordGeometryReference` records enabled/valid, associated model +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. + +## Offline validation + +The tests cover signs/units across speed and preview, smoothing, release and +reversal, standstill, invalid geometry, message serialization, preservation of +the original action/longitudinal control, selection through actual controlsd +code, desired-angle logging, maneuver priority, and upstream fallback. + +Three Lightning routes (149, 151, 157; CTMV2 and Tee Time) were replayed through +the current reference selection, upstream limiter, and controller using frozen +recorded steering/driver/PSCM measurements. Native controller cadence is retained; +every tenth command gets a CAN pack/decode check. Reference timing uses recorded +delay and the speed of the first control sample consuming each model. That speed +can differ slightly from modeld's original inference input. This is not a neural +replay or a prediction of wheel motion under modified commands. + +Results and input hashes: `ford_geometry_reference_validation.json`. + +``` +PYTHONPATH=.:opendbc_repo python tools/ford_pscm_lab/geometry_reference_validate.py \ + --output .cache/ford_geometry +``` + +Geometry is not uniformly earlier or stronger. The route 157 right-turn example +requests a large angle earlier; the problematic segment 9 left initially asks +for less and later asks for more. It hits command field bounds more often in +the frozen-measurement replay. Straight cohorts are defined by the original +action, so they also include disagreements about turn entry/exit. No claim of +improved physical tracking or stability follows from these offline results. diff --git a/docs/ford_geometry_reference_validation.json b/docs/ford_geometry_reference_validation.json new file mode 100644 index 0000000000..bcb7d809ea --- /dev/null +++ b/docs/ford_geometry_reference_validation.json @@ -0,0 +1,212 @@ +{ + "scope": "Replay the geometry reference and current C0/C1 controller on frozen route measurements.\n\nRequires cached full/route.npz, full/metadata.json, intake.npz and intake.json.\nThis is command validation, not a simulation of the truck following new requests.\n", + "routes": [ + { + "route": "157", + "model": "CTMV2", + "cycles": 76554, + "model_frames": 14892, + "fallback_frames": 0, + "wire_round_trips": 15312, + "metrics": { + "all": { + "samples": 44971, + "angle_reference_difference_deg_p50_p95_max": [ + 0.7096582083526739, + 37.323791775018584, + 473.2113566966026 + ], + "action_c0_bound_samples": 0, + "geometry_c0_bound_samples": 26, + "action_c1_bound_samples": 433, + "geometry_c1_bound_samples": 1393 + }, + "straight": { + "samples": 29296, + "angle_reference_difference_deg_p50_p95_max": [ + 0.3907433332260141, + 10.800464754067871, + 172.34883032860316 + ], + "action_c0_bound_samples": 0, + "geometry_c0_bound_samples": 0, + "action_c1_bound_samples": 0, + "geometry_c1_bound_samples": 0 + }, + "turn": { + "samples": 5773, + "angle_reference_difference_deg_p50_p95_max": [ + 18.222567807080637, + 104.0235491521133, + 473.2113566966026 + ], + "action_c0_bound_samples": 0, + "geometry_c0_bound_samples": 26, + "action_c1_bound_samples": 433, + "geometry_c1_bound_samples": 1393 + } + }, + "sources_sha256": { + ".cache/ford_route157/full/route.npz": "e2e2573f904aa11ee9e10450e7f5b965d475657b61127e827a67eadcd6b857fb", + ".cache/ford_route157/full/metadata.json": "f2d72c9a877ed6694e4da6831841113dc0c05987e2c76cca51f358d12411b235", + ".cache/ford_route157/intake.npz": "5dffd86fde68197727d6c6b9c5eae82320a456fa1206c4cf18e5e9c69419dde4", + ".cache/ford_route157/intake.json": "1d82c7f87d42229b96771bce3dd43c6b1651aa4fb8bb0ca3b1886d5a1a1c1a3c", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/geometry_reference_validate.py": "84dae09c158f5aaa3b88b5141545cd16829022437027d3d86c60a0acf7d78f56", + "openpilot/sunnypilot/modeld_v2/ford_geometry.py": "9a51519533b7e4a6b5aa5ff58ad5b646afb2267c943d5a790e86d8b20324d84b", + "openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "149", + "model": "CTMV2", + "cycles": 132334, + "model_frames": 26104, + "fallback_frames": 0, + "wire_round_trips": 26468, + "metrics": { + "all": { + "samples": 109224, + "angle_reference_difference_deg_p50_p95_max": [ + 1.0809205342304384, + 21.80654129599715, + 180.71838950537807 + ], + "action_c0_bound_samples": 0, + "geometry_c0_bound_samples": 142, + "action_c1_bound_samples": 845, + "geometry_c1_bound_samples": 2430 + }, + "straight": { + "samples": 67435, + "angle_reference_difference_deg_p50_p95_max": [ + 0.6422363701463203, + 6.246569735165232, + 62.050131143674285 + ], + "action_c0_bound_samples": 0, + "geometry_c0_bound_samples": 0, + "action_c1_bound_samples": 0, + "geometry_c1_bound_samples": 0 + }, + "turn": { + "samples": 15161, + "angle_reference_difference_deg_p50_p95_max": [ + 6.8859610951543, + 87.51133149734092, + 180.71838950537807 + ], + "action_c0_bound_samples": 0, + "geometry_c0_bound_samples": 142, + "action_c1_bound_samples": 845, + "geometry_c1_bound_samples": 2430 + } + }, + "sources_sha256": { + ".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade", + ".cache/ford_route149/full/metadata.json": "624fff03c25eb298661cb7b25f3dbe6d214d863f799d93635c0f4f05fc0d2b32", + ".cache/ford_route149/intake.npz": "503919c4f1566ef850007c59f16ce074697ba57732f21beb7b158c1ca7be3a9d", + ".cache/ford_route149/intake.json": "80f8c4ba69b4259ed308e0db110123063c6917357d26b49c9b6585695c9b24ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/geometry_reference_validate.py": "84dae09c158f5aaa3b88b5141545cd16829022437027d3d86c60a0acf7d78f56", + "openpilot/sunnypilot/modeld_v2/ford_geometry.py": "9a51519533b7e4a6b5aa5ff58ad5b646afb2267c943d5a790e86d8b20324d84b", + "openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "151", + "model": "TT", + "cycles": 325708, + "model_frames": 64837, + "fallback_frames": 0, + "wire_round_trips": 65142, + "metrics": { + "all": { + "samples": 124806, + "angle_reference_difference_deg_p50_p95_max": [ + 0.6014079626170221, + 8.687564671100564, + 233.5485828527104 + ], + "action_c0_bound_samples": 0, + "geometry_c0_bound_samples": 6, + "action_c1_bound_samples": 162, + "geometry_c1_bound_samples": 588 + }, + "straight": { + "samples": 105881, + "angle_reference_difference_deg_p50_p95_max": [ + 0.5210065989727145, + 4.103656620119354, + 143.66919002099516 + ], + "action_c0_bound_samples": 0, + "geometry_c0_bound_samples": 0, + "action_c1_bound_samples": 0, + "geometry_c1_bound_samples": 0 + }, + "turn": { + "samples": 3715, + "angle_reference_difference_deg_p50_p95_max": [ + 9.423421334227854, + 66.85200116463068, + 233.5485828527104 + ], + "action_c0_bound_samples": 0, + "geometry_c0_bound_samples": 6, + "action_c1_bound_samples": 162, + "geometry_c1_bound_samples": 588 + } + }, + "sources_sha256": { + ".cache/ford_route151/full/route.npz": "41a5b8bd388cf5a3d553f784542376ac9355fcdc5be4f427053d0504537babe1", + ".cache/ford_route151/full/metadata.json": "937825317a0edd470c54647240b922be8f79dda5b3365ffdd61281f0aca877a1", + ".cache/ford_route151/intake.npz": "18bbefb7738cebd0071dc987e90f74469a0c8ed456f9412324030592cafd68c9", + ".cache/ford_route151/intake.json": "0cfe327b5889f2603d902d2a01e915bf9a27b7a312137111d892a5e8540e03aa", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/geometry_reference_validate.py": "84dae09c158f5aaa3b88b5141545cd16829022437027d3d86c60a0acf7d78f56", + "openpilot/sunnypilot/modeld_v2/ford_geometry.py": "9a51519533b7e4a6b5aa5ff58ad5b646afb2267c943d5a790e86d8b20324d84b", + "openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + } + ], + "tests_passed": 605, + "route157_timing": [ + { + "event": 9, + "action": { + "entry_75deg": 509.71654137700006, + "release_below_25deg": 515.628442615 + }, + "geometry": { + "entry_75deg": 509.26702728900005, + "release_below_25deg": 515.7717539590001 + } + }, + { + "event": 11, + "action": { + "entry_75deg": 595.0217840170001, + "release_below_25deg": 601.3182963449999 + }, + "geometry": { + "entry_75deg": 595.2721063070001, + "release_below_25deg": 601.874657156 + } + }, + { + "event": 16, + "action": { + "entry_75deg": 700.319324972, + "release_below_25deg": 708.5712472329999 + }, + "geometry": { + "entry_75deg": 700.2793086730001, + "release_below_25deg": 709.1140404539999 + } + } + ], + "totals": { + "cycles": 534596, + "model_frames": 105833, + "fallback_frames": 0, + "wire_round_trips": 106922 + } +} diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index de82110b75..e3b3e4bcb3 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -482,6 +482,18 @@ struct ModelDataV2SP @0xa1680744031fdb2d { laneTurnDirection @0 :TurnDirection; leftLaneChangeEdgeBlock @1 :Bool; rightLaneChangeEdgeBlock @2 :Bool; + fordGeometryReference @3 :FordGeometryReference; + + struct FordGeometryReference { + enabled @0 :Bool; + valid @1 :Bool; # False while enabled means invalid geometry; original action is retained. + modelMonoTime @2 :UInt64; + actionDesiredCurvature @3 :Float32; # Original model action, with its own unchanged smoothing history. + rawCurvature @4 :Float32; + selectedCurvature @5 :Float32; # Published modelV2.action, before controlsd's normal limits/maneuver override. + previewSeconds @6 :Float32; + smoothSeconds @7 :Float32; + } enum TurnDirection { none @0; diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 896d61bcc1..7e4f5d99aa 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -240,6 +240,7 @@ inline static std::unordered_map keys = { {"FordPscmObserver", {PERSISTENT | BACKUP, BOOL, "0"}}, {"FordModelActionController", {PERSISTENT | BACKUP, BOOL, "0"}}, {"FordC0TimeBased", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"FordGeometryReference", {PERSISTENT | BACKUP, BOOL, "0"}}, {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}}, {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}}, {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/openpilot/sunnypilot/modeld_v2/ford_geometry.py b/openpilot/sunnypilot/modeld_v2/ford_geometry.py new file mode 100644 index 0000000000..a05b517dd3 --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/ford_geometry.py @@ -0,0 +1,56 @@ +"""Optional Ford reference from the published plan, using modeld's own timing.""" +import numpy as np + +from opendbc.car.ford.values import FordFlags +from openpilot.selfdrive.controls.lib.drive_helpers import MIN_STABLE_DELAY, get_curvature_from_plan, smooth_value + + +def select_ford_geometry_reference(CP, controller_enabled, geometry_enabled): + if controller_enabled and geometry_enabled and CP.brand == 'ford' and CP.flags & FordFlags.CANFD: + return FordGeometryReference() + return None + + +class FordGeometryReference: + def __init__(self): + self.previous = 0. + + def update(self, yaws, yaw_rates, times, *, speed, preview, smooth_seconds, smoothing_enabled): + # Reject an incomplete/nonfinite plan instead of extrapolating beyond it. + yaws, yaw_rates, times = (np.asarray(x) for x in (yaws, yaw_rates, times)) + if (yaws.ndim != 1 or yaw_rates.shape != yaws.shape or times.shape != yaws.shape or len(times) < 2 + or not np.isfinite([speed, preview, smooth_seconds]).all() or speed < 0 or preview <= 0 or smooth_seconds < 0 + or not all(np.isfinite(x).all() for x in (yaws, yaw_rates, times)) + or times[0] != 0 or not np.all(np.diff(times) > 0) or times[-1] < max(preview, MIN_STABLE_DELAY)): + self.previous = 0. + return None + raw = float(get_curvature_from_plan(yaws, yaw_rates, times, speed, preview)) + selected = raw + if smoothing_enabled: + selected = float(smooth_value(raw, self.previous, smooth_seconds)) if speed > .3 else self.previous + if not np.isfinite([raw, selected]).all() or max(abs(raw), abs(selected)) > float(np.finfo(np.float32).max): + self.previous = 0. + return None + self.previous = selected + return raw, selected + + def apply(self, model_msg, driving_msg, sp_msg, *, speed, preview, smooth_seconds, smoothing_enabled): + """Keep the original action separately; both published actions use the selected reference. + + The caller retains its original prev_action, so this experiment cannot feed + geometry into the learned action's smoothing or change longitudinal control. + """ + model = model_msg.modelV2 + ref = sp_msg.modelDataV2SP.fordGeometryReference + ref.enabled = True + ref.modelMonoTime = model_msg.logMonoTime + ref.actionDesiredCurvature = model.action.desiredCurvature + ref.previewSeconds = preview + ref.smoothSeconds = smooth_seconds if smoothing_enabled else 0. + result = self.update(model.orientation.z, model.orientationRate.z, model.orientation.t, + speed=speed, preview=preview, smooth_seconds=smooth_seconds, smoothing_enabled=smoothing_enabled) + ref.valid = result is not None + if result is not None: + ref.rawCurvature, model.action.desiredCurvature = result + driving_msg.drivingModelData.action.desiredCurvature = model.action.desiredCurvature + ref.selectedCurvature = model.action.desiredCurvature diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 3ddaccc578..0f2abdde32 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -51,6 +51,7 @@ from openpilot.sunnypilot.modeld_v2.compile_modeld import (derive_frame_skip, ma make_supercombo_input_queues, nv12_copy_size, WARP_INPUTS, POLICY_INPUTS) from openpilot.sunnypilot.livedelay.helpers import get_ford_delay_offset, get_lat_delay +from openpilot.sunnypilot.modeld_v2.ford_geometry import select_ford_geometry_reference from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase from openpilot.sunnypilot.modeld_v2.helpers import load_oob from openpilot.sunnypilot.models.helpers import get_active_bundle @@ -415,6 +416,9 @@ def main(demo=False): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("modeld got CarParams: %s", CP.brand) ford_model_action = params.get_bool("FordModelActionController") + ford_geometry = select_ford_geometry_reference(CP, ford_model_action, params.get_bool("FordGeometryReference")) + if CP.brand == 'ford': + cloudlog.event('Ford model reference selected', source='geometry' if ford_geometry is not None else 'action') # TODO Move smooth seconds to action function long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS @@ -546,6 +550,11 @@ def main(demo=False): frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, meta_constants) modelv2_send.modelV2.big = model.chestnut + if ford_geometry is not None: + ford_geometry.apply(modelv2_send, drivingdata_send, mdv2sp_send, speed=v_ego, preview=lat_action_t, + smooth_seconds=model.LAT_SMOOTH_SECONDS, + smoothing_enabled=model.generation is not None and model.generation >= 10) + desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] diff --git a/openpilot/sunnypilot/modeld_v2/tests/test_ford_geometry.py b/openpilot/sunnypilot/modeld_v2/tests/test_ford_geometry.py new file mode 100644 index 0000000000..4e4abf5c35 --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/tests/test_ford_geometry.py @@ -0,0 +1,171 @@ +import ast +import itertools +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from opendbc.car.ford.values import FordFlags +from openpilot.cereal import log +from openpilot.common.params import Params, ParamKeyFlag +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_curvature_from_plan, smooth_value +from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from openpilot.selfdrive.controls.tests.test_ford_model_action_adapter import Subscriptions, pipeline # noqa: F401 +from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import car_params, startup +from openpilot.sunnypilot.modeld_v2.constants import ModelConstants +from openpilot.sunnypilot.modeld_v2.ford_geometry import FordGeometryReference, select_ford_geometry_reference + + +TIMES = np.array(ModelConstants.T_IDXS) + + +def update(ref, curvature=.02, speed=5., **overrides): + kwargs = {'yaws': TIMES*speed*curvature, 'yaw_rates': np.full(len(TIMES), speed*curvature), 'times': TIMES, + 'speed': speed, 'preview': .743946, 'smooth_seconds': .1, 'smoothing_enabled': True} + return ref.update(**(kwargs | overrides)) + + +@pytest.mark.parametrize('speed', [1., 5., 15., 30.]) +@pytest.mark.parametrize('preview', [.1, .3, .743946, 1.2]) +@pytest.mark.parametrize('curvature', [-.04, 0., .04]) +def test_constant_radius_sign_and_units(speed, preview, curvature): + raw, selected = update(FordGeometryReference(), curvature, speed, preview=preview, smoothing_enabled=False) + assert raw == pytest.approx(curvature) + assert selected == raw + + +def test_entry_reversal_and_release_use_existing_model_smoothing_once_per_frame(): + ref = FordGeometryReference() + previous = 0. + for requested in [.02]*20+[-.02]*20+[0.]*40: + raw, selected = update(ref, requested) + previous = smooth_value(requested, previous, .1) + assert raw == pytest.approx(requested) + assert selected == pytest.approx(previous) + assert abs(selected) < 1e-9 + + +def test_standstill_hold_and_model_specific_smoothing(): + ref = FordGeometryReference() + _, moving = update(ref) + for speed in [0., .1, .3]: + assert update(ref, -.02, speed)[1] == moving + assert update(ref, -.02, 5., smooth_seconds=0.)[1] == pytest.approx(-.02) + + +@pytest.mark.parametrize('changes', [ + {'yaws': []}, {'yaw_rates': []}, {'times': TIMES[:-1]}, {'times': TIMES[::-1]}, + {'times': np.ones(33)}, {'times': TIMES+1}, {'yaws': np.ones((33, 1))}, + {'yaws': np.full(33, np.nan)}, {'yaw_rates': np.full(33, np.inf)}, + {'yaws': np.full(33, 1e40)}, + {'speed': np.nan}, {'speed': -1.}, {'preview': 0.}, {'preview': np.inf}, {'preview': 11.}, {'smooth_seconds': -1.}, +]) +def test_invalid_geometry_resets_history(changes): + ref = FordGeometryReference() + update(ref) + assert update(ref, **changes) is None + assert ref.previous == 0. + assert update(ref) == update(FordGeometryReference()) + + +@pytest.mark.parametrize('controller,geometry', list(itertools.product([False, True], repeat=2))) +@pytest.mark.parametrize('brand,flags', [('ford', FordFlags.CANFD), ('ford', FordFlags.CANFD | 8), ('ford', 0), ('tesla', 1)]) +def test_opt_in_cannot_change_other_vehicles_or_upstream(controller, geometry, brand, flags): + ref = select_ford_geometry_reference(SimpleNamespace(brand=brand, flags=flags), controller, geometry) + assert (ref is not None) == bool(controller and geometry and brand == 'ford' and flags & FordFlags.CANFD) + + +def messages(): + m, d, sp = [log.Event.new_message(**{key: {}}) for key in ('modelV2', 'drivingModelData', 'modelDataV2SP')] + m.logMonoTime = 980_000_000 + action = log.ModelDataV2.Action(desiredCurvature=-.003, desiredAcceleration=.4, shouldStop=True) + m.modelV2.action = d.drivingModelData.action = action + m.modelV2.orientation.z = (TIMES*5.*.02).tolist() + m.modelV2.orientation.t = TIMES.tolist() + m.modelV2.orientationRate.z = np.full(33, .1).tolist() + m.modelV2.position.x = (TIMES*5.).tolist() + m.modelV2.position.y = np.zeros(33).tolist() + return m, d, sp, action + + +def apply(ref, m, d, sp): + ref.apply(m, d, sp, speed=5., preview=.743946, smooth_seconds=.1, smoothing_enabled=True) + + +@pytest.mark.parametrize('invalid', [False, True]) +def test_publication_preserves_original_history_and_longitudinal_and_serializes_provenance(invalid): + m, d, sp, original = messages() + before = original.to_dict() + if invalid: + m.modelV2.orientationRate.z = [] + apply(FordGeometryReference(), m, d, sp) + assert original.to_dict() == before # prev_action held by modeld must remain the learned action. + assert m.modelV2.action.to_dict() == d.drivingModelData.action.to_dict() + assert m.modelV2.action.desiredAcceleration == original.desiredAcceleration + assert m.modelV2.action.shouldStop == original.shouldStop + with log.Event.from_bytes(sp.to_bytes()) as decoded: + ref = decoded.modelDataV2SP.fordGeometryReference + assert ref.enabled and ref.valid == (not invalid) + assert ref.modelMonoTime == m.logMonoTime + assert ref.actionDesiredCurvature == original.desiredCurvature + assert ref.selectedCurvature == m.modelV2.action.desiredCurvature + if invalid: + assert ref.selectedCurvature == original.desiredCurvature + else: + assert ref.selectedCurvature > 0. and ref.actionDesiredCurvature < 0. + + +@pytest.mark.parametrize('maneuver', [False, True]) +def test_published_geometry_flows_through_actual_controlsd_selection_feedback_and_angle_target(pipeline, maneuver): # noqa: F811 + from opendbc.car import structs + + m, d, sp, _ = messages() + apply(FordGeometryReference(), m, d, sp) + controls = startup() + controls.sm, controls.desired_curvature, controls.curvature = Subscriptions(maneuver), 0., 0. + cc = structs.CarControl(latActive=True) + cs = SimpleNamespace(vEgo=5., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0., steeringAngleDeg=0.) + exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': m.modelV2, + 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}) + # Geometry asks right; original action and maneuver both ask left. The unchanged + # upstream jerk limit permits 0.002 curvature in this first 10 ms step. + expected = -.002 if maneuver else .002 + assert controls.desired_curvature == pytest.approx(expected) + assert controls.ford_path.path_offset*expected > 0. + assert controls.ford_path.path_angle*expected > 0. + lac = SimpleNamespace(use_steer_limited_by_safety=False, _check_saturation=lambda *args: False) + _, angle, angle_log = LatControlAngle.update(lac, True, cs, controls.VM, SimpleNamespace(roll=0., angleOffsetDeg=0.), + False, controls.desired_curvature, None, False, .743946) + assert angle*expected < 0. and angle_log.steeringAngleDesiredDeg == pytest.approx(angle) + + +def test_new_setting_default_off_persists_without_enabling_controller(tmp_path): + params = Params(str(tmp_path)) + assert params.get_default_value('FordGeometryReference') is False + params.put_bool('FordGeometryReference', True, block=True) + assert b'FordGeometryReference' in params.all_keys(ParamKeyFlag.BACKUP) + assert startup(params=params).ford_path_controller is None + assert select_ford_geometry_reference(car_params(), params.get_bool('FordModelActionController'), True) is None + params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START) + assert params.get_bool('FordGeometryReference') + + +@pytest.mark.parametrize('enabled', [False, True]) +def test_actual_modeld_hook_uses_exact_timing_after_original_action_history_is_saved(enabled): + path = Path(__file__).resolve().parents[1]/'modeld.py' + tree = ast.parse(path.read_text()) + hook = next(n for n in ast.walk(tree) if isinstance(n, ast.If) and ast.unparse(n.test) == 'ford_geometry is not None') + m, d, sp, _ = messages() + ref = FordGeometryReference() if enabled else None + before = m.modelV2.to_dict(), d.drivingModelData.to_dict(), sp.modelDataV2SP.to_dict() + env = {'ford_geometry': ref, 'modelv2_send': m, 'drivingdata_send': d, 'mdv2sp_send': sp, 'v_ego': 5., 'lat_action_t': .743946, + 'model': SimpleNamespace(LAT_SMOOTH_SECONDS=.1, generation=12)} + exec(compile(ast.Module(body=[hook], type_ignores=[]), str(path), 'exec'), env) + if not enabled: + assert before == (m.modelV2.to_dict(), d.drivingModelData.to_dict(), sp.modelDataV2SP.to_dict()) + return + expected = smooth_value(get_curvature_from_plan(list(m.modelV2.orientation.z), list(m.modelV2.orientationRate.z), + TIMES, 5., .743946), 0., .1) + assert m.modelV2.action.desiredCurvature == pytest.approx(expected) + assert sp.modelDataV2SP.fordGeometryReference.previewSeconds == pytest.approx(.743946) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index b6a6514b89..3b98819a90 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2190,6 +2190,24 @@ "type": "offroad_only" } ] + }, + { + "key": "FordGeometryReference", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Model Geometry Reference (Experimental)", + "description": "Use model heading and heading rate as the steering reference for the Ford C0/C1 controller.", + "details": "On uses the published model path to calculate desired curvature at the model's existing preview time, with the same output smoothing. Off uses the original model action. Both use the existing C0/C1 mapping, steering feedback, and limits. Requires Selected-Action Path Tracking; otherwise upstream Ford control remains selected. The original action and selected reference are both logged. Default off. Changes apply after an offroad-to-onroad cycle.", + "enablement": [ + { + "type": "offroad_only" + }, + { + "type": "param", + "key": "FordModelActionController", + "equals": true + } + ] } ] }, diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index 057581d467..b3ad499f76 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -18,6 +18,17 @@ sections: details: Derives path offset and heading from the same selected desired curvature and adjusts the heading request using the difference between requested and measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone. enablement: - $ref: '#/macros/offroad' + - key: FordGeometryReference + widget: toggle + needs_onroad_cycle: true + title: Model Geometry Reference (Experimental) + description: Use model heading and heading rate as the steering reference for the Ford C0/C1 controller. + details: On uses the published model path to calculate desired curvature at the model's existing preview time, with the same output smoothing. Off uses the original model action. Both use the existing C0/C1 mapping, steering feedback, and limits. Requires Selected-Action Path Tracking; otherwise upstream Ford control remains selected. The original action and selected reference are both logged. Default off. Changes apply after an offroad-to-onroad cycle. + enablement: + - $ref: '#/macros/offroad' + - type: param + key: FordModelActionController + equals: true - id: hyundai title: Hyundai / Kia / Genesis Settings description: '' diff --git a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py index 45502cd766..84fc0be89f 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -299,6 +299,14 @@ class TestKnownVehicleSettings(OpenpilotTestCase): assert "FordSharedPathController" not in keys assert "FordPscmObserver" not in keys + def test_ford_geometry_is_a_separate_cycle_only_opt_in(self, schema): + items = _brand_items(schema["vehicle_settings"].get("ford")) + item = next(item for item in items if item["key"] == "FordGeometryReference") + assert item["widget"] == "toggle" and item["needs_onroad_cycle"] is True + assert item["enablement"] == [{"type": "offroad_only"}, {"type": "param", "key": "FordModelActionController", "equals": True}] + with tempfile.TemporaryDirectory() as path: + assert Params(path).get_default_value("FordGeometryReference") is False + def test_hyundai_has_longitudinal_tuning(self, schema): keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))} assert "HyundaiLongitudinalTuning" in keys diff --git a/tools/ford_pscm_lab/geometry_reference_validate.py b/tools/ford_pscm_lab/geometry_reference_validate.py new file mode 100644 index 0000000000..04c454d51c --- /dev/null +++ b/tools/ford_pscm_lab/geometry_reference_validate.py @@ -0,0 +1,135 @@ +"""Replay the geometry reference and current C0/C1 controller on frozen route measurements. + +Requires cached full/route.npz, full/metadata.json, intake.npz and intake.json. +This is command validation, not a simulation of the truck following new requests. +""" +import argparse +from concurrent.futures import ProcessPoolExecutor, as_completed +import hashlib +import json +from pathlib import Path +from types import SimpleNamespace + +import numpy as np + +from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController +from openpilot.sunnypilot.modeld_v2.constants import ModelConstants +from openpilot.sunnypilot.modeld_v2.ford_geometry import FordGeometryReference +from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table + + +def replay(label, output): + source = Path('.cache')/f'ford_route{label}' + meta = json.loads((source/'full/metadata.json').read_text()) + intake = json.loads((source/'intake.json').read_text()) + with np.load(source/'full/route.npz') as z: + r = {k: table(z, k) for k in ('controls', 'cs', 'cc', 'params', 'pscm')} + with np.load(source/'intake.npz') as z: + m, paths, delays = z['model'], z['plans'], z['delay'] + c, t = r['controls'], r['controls']['t'] + cs, pa, ps = [sample(r[k], t) for k in ('cs', 'params', 'pscm')] + cc = sample(r['cc'], t, nearest=True) + mi = np.clip(np.searchsorted(m[:, 3], c['model_ns']), 0, len(m)-1) + exact = m[mi, 3] == c['model_ns'] + # Approximation: first control sample consuming each model, not the unavailable + # exact carState latched at inference start. No future model points are used. + ci = np.clip(np.searchsorted(c['model_ns'], m[:, 3]), 0, len(t)-1) + speed = np.maximum(cs['speed'][ci], 0.) # modeld clamps the filtered speed at standstill. + delay = delays[np.clip(np.searchsorted(delays[:, 0], m[:, 0], side='right')-1, 0, len(delays)-1), 3] + settings = intake['settings'][0]['params'] + assert settings['FordC0TimeBased'] == '0' and settings['LagdToggle'] == '1' + assert np.all(m[:, 6] == 1), 'This replay expects one big model throughout the route' + bundle = next(b for b in intake['bundles'].values() if b['internalName'] == settings['ModelManager_ActiveBundleChestnut']) + tau = float(next(o['value'] for o in bundle['overrides'] if o['key'] == 'lat')) + preview = delay+tau+.075+.4*np.clip((30-speed/.44704)/15, 0, 1) + reference = FordGeometryReference() + geometry = np.empty(len(m)) + fallback = 0 + for j, p in enumerate(paths): + result = reference.update(p[2], p[3], ModelConstants.T_IDXS, speed=speed[j], preview=preview[j], + smooth_seconds=tau, smoothing_enabled=bundle['generation'] >= 10) + geometry[j] = np.float32(m[j, 7] if result is None else result[1]) + fallback += result is None + car = {**json.loads(Path('.cache/ford_route112/metadata.json').read_text())['car'][0], **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 = [FordModelActionController(), FordModelActionController()] + models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths] + selected = [0., 0.] + wires = [WireCheck(), WireCheck()] + names = ['t', 'valid', 'feedback', 'speed', 'driver', 'actual_angle', 'action_angle', 'geometry_angle', + 'action_c0', 'geometry_c0', 'action_c1', 'geometry_c1'] + rows = np.empty((len(t), len(names))) + for i, now in enumerate(t): + good_params = np.isfinite([pa['stiffness'][i], pa['steer_ratio'][i]]).all() and min(pa['stiffness'][i], pa['steer_ratio'][i]) > 0 + if good_params: + 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) + valid = bool(c['valid'][i] and cc['valid'][i] and cs['valid'][i] and cs['can_valid'][i] and good_params + and pa['valid'][i] and m[mi[i], 2] and exact[i] and abs(cc['t'][i]-now) < .005 + and 0 <= now-pa['t'][i] <= .15) + 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]) + targets = [m[mi[i], 7], geometry[mi[i]]] + angles, commands = [], [] + for j, core in enumerate(cores): + selected[j], _ = clip_curvature(cs['speed'][i], selected[j], targets[j] if active else c['measured'][i], pa['roll'][i]) + command = core.update(models[mi[i]], selected[j], current_curvature=c['measured'][i], yaw_rate=cs['yaw'][i], + speed=cs['speed'][i], now=now, measurement_time=cs['t'][i], model_time=m[mi[i], 0], + reference_time=m[mi[i], 0], active=active, valid=valid, curvature_scale=scale, + driver_pressed=bool(cs['pressed'][i]), driver_torque=cs['torque'][i], pscm_status=status) + assert abs(command.path_offset) <= 5.1100001 and abs(command.path_angle) <= .5000001 + assert command.curvature == command.curvature_rate == 0. + assert np.isfinite([command.path_offset, command.path_angle]).all() + if not core.diagnostics.get('feedback_enabled', False): + assert core.core.offset_proportional == core.core.correction == 0. + if i % 10 == 0: + wires[j].check(command) + commands.append(command) + angles.append(vm.get_steer_from_curvature(-selected[j], cs['speed'][i], pa['roll'][i])*180/np.pi+pa['angle_offset'][i]) + assert commands[0].valid == commands[1].valid + a, b = commands + rows[i] = [now-meta['t0'], b.valid, cores[1].diagnostics.get('feedback_enabled', False), cs['speed'][i], + bool(cs['pressed'][i] or abs(cs['torque'][i]) > 1. or status.limit == 3), cs['angle'][i], *angles, + a.path_offset, b.path_offset, a.path_angle, b.path_angle] + eligible = rows[:, 1].astype(bool) & rows[:, 2].astype(bool) + metrics = {} + for name, mask in {'all': eligible, 'straight': eligible & (abs(rows[:, 6]) < 10), + 'turn': eligible & (abs(rows[:, 6]) >= 45)}.items(): + if not mask.any(): + continue + metrics[name] = {'samples': int(mask.sum()), + 'angle_reference_difference_deg_p50_p95_max': np.quantile(abs(rows[mask, 7]-rows[mask, 6]), [.5, .95, 1.]).tolist(), + 'action_c0_bound_samples': int((abs(rows[mask, 8]) >= 5.105).sum()), + 'geometry_c0_bound_samples': int((abs(rows[mask, 9]) >= 5.105).sum()), + 'action_c1_bound_samples': int((abs(rows[mask, 10]) >= .49975).sum()), + 'geometry_c1_bound_samples': int((abs(rows[mask, 11]) >= .49975).sum())} + sources = [source/'full/route.npz', source/'full/metadata.json', source/'intake.npz', source/'intake.json', Path(__file__), + Path('openpilot/sunnypilot/modeld_v2/ford_geometry.py'), Path('openpilot/selfdrive/controls/lib/ford_model_action.py')] + report = {'route': label, 'model': bundle['internalName'], 'cycles': len(t), 'model_frames': len(m), 'fallback_frames': fallback, + 'wire_round_trips': sum(w.count for w in wires), 'metrics': metrics, + 'sources_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in sources}} + dest = output/label + dest.mkdir(parents=True, exist_ok=True) + np.savez_compressed(dest/'commands.npz', names=names, rows=rows) + (dest/'report.json').write_text(json.dumps(report, indent=2)+'\n') + return report + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--output', type=Path, required=True) + parser.add_argument('--routes', nargs='+', default=['149', '151', '157']) + args = parser.parse_args() + results = [] + with ProcessPoolExecutor(max_workers=3) as pool: + for job in as_completed([pool.submit(replay, route, args.output) for route in args.routes]): + result = job.result() + results.append(result) + print(json.dumps({k: v for k, v in result.items() if k != 'sources_sha256'}), flush=True) + (args.output/'validation.json').write_text(json.dumps({'scope': __doc__, 'routes': results}, indent=2)+'\n')