Ford: add selectable model geometry reference

This commit is contained in:
Isaac Barham
2026-09-15 22:08:20 -04:00
parent 1336171a20
commit 18ded03800
11 changed files with 715 additions and 0 deletions
+82
View File
@@ -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.
@@ -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
}
}
+12
View File
@@ -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;
+1
View File
@@ -240,6 +240,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},
@@ -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
+9
View File
@@ -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]
@@ -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)
@@ -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
}
]
}
]
},
@@ -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: ''
@@ -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
@@ -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')