Ford: gate 20Hz selected-action cadence with shared startup selection

Snapshot the existing Sunnylink toggle into CarParamsSP so controlsd and the CAN sender agree. Preserve the v6 C0/C1 calculation at 100Hz and Panda safety unchanged. Add actual sender timing, packing, selection, and safety checks; record the a5 frozen-input replay. Physical tracking improvement remains unverified.

Assisted-by: OpenAI Codex
This commit is contained in:
Isaac Barham
2026-09-08 12:56:33 -04:00
parent c70a9ee84b
commit e1cd61166c
9 changed files with 270 additions and 16 deletions
+60
View File
@@ -0,0 +1,60 @@
# Selected-action 20 Hz cadence experiment
The selected-action controller now sends LateralMotionControl2 every fifth
100 Hz update (20 Hz, nominally 50 ms). Each send uses the latest published
C0/C1. The v6 controller module is byte-identical to `c70a9ee84`: its geometry,
150 ms forecast, caps, two states, and 4 m/s / 0.5 rad/s slew are unchanged.
C2 and C3 remain zero. This tests transport cadence, not a new strength gain.
The existing default-off `FordModelActionController` Sunnylink setting is
snapshotted by `card` into `CarParamsSP.flags` on the supported CAN FD Lightning.
Both controller selection and send cadence use that snapshot. An onroad setting
write cannot switch either one. Complete an offroad-to-onroad cycle after updating.
The diagnostic hypothesis remains `model-action-measured-pose-v6`; the build
commit, CarParamsSP flag, and measured send cadence distinguish this experiment.
With the toggle off, existing CAN FD controllers retain 100 Hz transmission.
Legacy CAN stays at 20 Hz. Panda safety is byte-identical to `c21a9013`.
The sender retains Panda's existing per-message C2 slew bound even on the
20 Hz path; this controller does not use C2. No safety limit is relaxed.
Counters advance once per transmitted request, including wrap from 15 to 0.
Invalid paths zero the next scheduled request; disengagement sends mode 0 on
that request, without a new ramp-out sequence. Relative to a 100 Hz sender,
a change can wait up to four more control ticks (nominally 40 ms).
## Evidence and limits
On route `84865544361f55cb_000000a5--d0f935d323`, the camera's observed inactive
LMC2 stream ran at 19.993 Hz (7,952 messages, median 50.051 ms). Our sender ran
at 99.321 Hz (38,496 messages, median 9.938 ms). The camera data does not establish
the factory's active-mode cadence. All supplied v1v6 drives already used
100 Hz; cadence has not been established as the cause of weak tracking.
The [transport replay record](ford_model_action_cadence_validation.json) covers
38,496 recorded send cycles at each of five possible scheduling phases:
192,480 sender updates and 38,496 transmitted requests in total. Every emitted
C0/C1 exactly matched the corresponding recorded request; C2/C3 stayed zero;
mode, counter and checksum checks passed. Every request passed the unchanged,
compiled Panda TX hook with controls eligibility set from the recorded mode.
This tests TX bounds, not a full Panda RX watchdog, vehicle response, or device boot.
Targeted tests additionally cover exact send intervals, latest-sample delivery,
counter wrap, every disengagement/invalid-input phase, unchanged fallback cadence,
100 Hz core slew, and a shared selection snapshot surviving serialization and
subsequent stored-toggle changes. The broader offline run passed 689 tests and
9,146 subtests; 178 inherited safety cases were skipped as inapplicable.
Ruff and typechecking of the touched production modules passed.
No physical tracking improvement is claimed. The next drive must establish
whether the lower cadence helps ordinary bends and turn exits, while checking
for added turn-in delay. Offline replay cannot predict that closed-loop response.
## Reproduce
Initialize the pinned submodule and use the project's built Python/native environment:
```sh
export PYTHONDONTWRITEBYTECODE=1
export PYTHONPATH=.:opendbc_repo
python -m pytest -q openpilot/selfdrive/controls/tests/test_ford*.py tools/ford_pscm_lab opendbc_repo/opendbc/car/ford/tests/test_ford.py openpilot/sunnypilot/sunnylink/tests openpilot/sunnypilot/mads/tests openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/common/tests/test_params.py opendbc_repo/opendbc/safety/tests/test_ford.py
```
@@ -0,0 +1,45 @@
{
"scope": "Frozen a5 publications through actual 20Hz CarController and unchanged compiled Panda TX hook, all five scheduling phases. No physical response simulation.",
"route": "84865544361f55cb_000000a5--d0f935d323",
"baseline_root": "c70a9ee84bbf6db9a687d63ef674ed4db9c13e3a",
"baseline_opendbc": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
"input_sha256": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
"phases": [
{
"phase": 0,
"input_cycles": 38496,
"sent_and_accepted": 7700
},
{
"phase": 1,
"input_cycles": 38496,
"sent_and_accepted": 7699
},
{
"phase": 2,
"input_cycles": 38496,
"sent_and_accepted": 7699
},
{
"phase": 3,
"input_cycles": 38496,
"sent_and_accepted": 7699
},
{
"phase": 4,
"input_cycles": 38496,
"sent_and_accepted": 7699
}
],
"core_byte_identical": true,
"ford_safety_byte_identical": true,
"source_sha256": {
"openpilot/selfdrive/controls/lib/ford_model_action.py": "326539389b04034932db41ca2c67794779403b2c0e97f408a81a9f5734114899",
"openpilot/selfdrive/controls/controlsd.py": "002d57a0b5b6e4e3a04789bee20b9b175a71d6893dd06074cebca2738f3986da",
"openpilot/sunnypilot/mads/helpers.py": "24970993d37242fe8a0457bac118e2c265c4c32427dc930c0001de12e5f204dd",
"opendbc_repo/opendbc/car/ford/carcontroller.py": "591b0d8455d256f7504cbd2a6a11fe1a54ba6ad3e12ac16a6952dbd92274f303",
"opendbc_repo/opendbc/car/ford/values.py": "edadaacc13581642917d1fba473ef3efc90d7145dbe801915f860747a4294046",
"opendbc_repo/opendbc/safety/modes/ford.h": "1d9d996292d6697ab4f02d55fae348d6aca1df94a07f7bdae48b68971b91afe7",
".cache/ford_cadence/replay.py": "7227f0b4f130be0e5b28f517416ffbf281f0b33085b8391f66814aa531c3e514"
}
}
+12 -5
View File
@@ -10,6 +10,11 @@ fitted PSCM plant model, added strength multiplier, new filter or extra core sta
`calibration_approved=false`: offline checks do not establish physical tracking,
turn-exit behavior or closed-loop stability.
The current cadence experiment transmits this controller's LMC2 requests at
**20 Hz (50 ms)** while its calculation remains at **100 Hz**. See
[cadence validation](ford_model_action_cadence.md). The v6 control law and
per-second C0/C1 slew are unchanged; this is not a demonstrated tracking fix.
## Select and restore
1. Install branch `hiimisaac-dev` from
@@ -17,8 +22,9 @@ turn-exit behavior or closed-loop stability.
Allow its build to finish before changing the setting.
2. While offroad, open Sunnylink device settings → Vehicle → Ford and enable
**Selected-Action Path Tracking (Experimental)** (`FordModelActionController`).
3. Complete a real offroad-to-onroad cycle. Selection occurs when `controlsd`
starts; changing a stored toggle or disengaging alone cannot swap an active
3. Complete a real offroad-to-onroad cycle. `card` snapshots the toggle into
`CarParamsSP`; the sender and `controlsd` share that selection. Changing a
stored toggle or disengaging alone cannot swap an active
controller. Initial physical evaluation remains controlled testing.
The startup log event `Ford path controller selected` should report
@@ -45,7 +51,8 @@ in Git history and the archived validation documents.
`Controls.__init__` selects the candidate once at startup. It shares the
existing Ford call path, selected upstream-limited curvature, service gates,
invalid-output disengagement, Float32 publication and downstream CAN builder.
C2 and C3 stay zero. No opendbc pointer or Panda safety change is included.
C2 and C3 stay zero. The cadence experiment updates the opendbc sender and
submodule pointer. Panda safety is unchanged.
Measured-pose use requires healthy `deviceMotion` and `extrinsicsCalibration`
services, calibrated extrinsics no older than 1 s, valid angular velocity and
@@ -101,14 +108,14 @@ and mutation probes remain recorded separately in
## Reproduce deployment checks
Initialize the branch's exact opendbc submodule (`c21a9013700734dd20b09e05aa68329ad8cc20f9`)
Initialize the branch's pinned opendbc submodule with `git submodule update --init opendbc_repo`
and build the native Params library from this branch before testing.
```sh
export PYTHONDONTWRITEBYTECODE=1
export PYTHONPATH=.:opendbc_repo
python -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/sunnypilot/sunnylink/tests openpilot/common/tests/test_params.py opendbc_repo/opendbc/car/ford/tests/test_ford.py
python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output .cache/ford_model_action_drive_test/stress.json
python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision "$(git -C opendbc_repo rev-parse HEAD)" --output .cache/ford_model_action_drive_test/stress.json
```
The full hardware build and device boot are not performed by these offline
+2 -2
View File
@@ -12,7 +12,7 @@ from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority
from openpilot.common.swaglog import cloudlog
from opendbc.car.car_helpers import interfaces
from opendbc.car.ford.values import FordFlags
from opendbc.car.ford.values import FordFlags, FordFlagsSP
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, select_model_action_controller
@@ -59,7 +59,7 @@ class Controls(ControlsExt):
self.ford_pscm_observer = (self.CP.brand == "ford" and self.CP.flags & FordFlags.CANFD and
self.params.get_bool("FordPscmObserver"))
self.ford_path_controller = FordPscmObserverPathController() if self.ford_pscm_observer else FordPathController()
self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"),
self.ford_path_controller = select_model_action_controller(self.CP, bool(self.CP_SP.flags & FordFlagsSP.MODEL_ACTION),
self.ford_path_controller)
self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController)
if self.CP.brand == "ford":
@@ -205,10 +205,10 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1')
downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, controls.CP_SP)
vehicle = SimpleNamespace(out=structs.CarState(vEgo=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int),
lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main)
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 20)], downstream.CAN.main)
for i, fail in enumerate((False, True)):
if fail:
sm.failed.add('modelV2')
@@ -216,8 +216,12 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
assert not cc.latActive and controls.ford_path == FordPath()
msg = custom.CarControlSP.new_message()
exec(publication, {'self': controls, 'CC_SP': msg})
_, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, (i+1)*10_000_000)
parser.update([(i+1)*10_000_000, packets])
for tick in range(5 if fail else 1):
now_nanos = (i + tick + 1) * 10_000_000
_, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, now_nanos)
lateral = [p for p in packets if p[0] == 0x3d6]
assert len(lateral) == int(not fail or tick == 4)
parser.update([now_nanos, packets])
wire = parser.vl['LateralMotionControl2']
assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset)
assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle)
@@ -0,0 +1,108 @@
"""Exercise 100Hz calculation and 20Hz transmission through the real CAN sender."""
from collections import defaultdict
from types import SimpleNamespace
import pytest
from opendbc.can import CANParser
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 FordFlags, FordFlagsSP, FordSafetyFlags
from opendbc.safety.tests.libsafety import libsafety_py
from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
def sender(canfd=True, selected=True):
cp = structs.CarParams(flags=int(FordFlags.CANFD) if canfd else 0, carFingerprint='FORD_F_150_LIGHTNING_MK1',
safetyConfigs=[structs.CarParams.SafetyConfig()])
cp_sp = structs.CarParamsSP(flags=int(FordFlagsSP.MODEL_ACTION) if selected else 0)
controller = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, cp_sp)
cs = SimpleNamespace(out=structs.CarState(vEgo=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int),
lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
return controller, cs
@pytest.mark.parametrize('canfd,selected,step', [(True, True, 5), (True, False, 1), (False, True, 5), (False, False, 5)])
def test_send_intervals_latest_sample_counter_and_checksum(canfd, selected, step):
controller, cs = sender(canfd, selected)
cc, sp = structs.CarControl(latActive=True), structs.CarControlSP()
sp.fordLateralPath.valid = True
name = 'LateralMotionControl2' if canfd else 'LateralMotionControl'
address = 0x3d6 if canfd else 0x3d3
parser = CANParser('ford_lincoln_base_pt', [(name, 0)], controller.CAN.main)
sent = []
for frame in range(1000):
sp.fordLateralPath.pathOffset = (frame % 101 - 50) * .01
sp.fordLateralPath.pathAngle = (frame % 101 - 50) * .0005
_, packets = controller.update(cc.as_reader(), sp, cs, frame * 10_000_000)
lateral = [p for p in packets if p[0] == address]
assert len(lateral) == int(frame % step == 0)
if not lateral:
continue
sent.append(frame)
parser.update([frame * 10_000_000, lateral])
wire = parser.vl[name]
assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-sp.fordLateralPath.pathOffset)
assert wire['LatCtlPath_An_Actl'] == pytest.approx(-sp.fordLateralPath.pathAngle)
if canfd:
counter = (len(sent) - 1) % 16
assert wire['LatCtlPath_No_Cnt'] == counter
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, counter, lateral[0][1])
assert sent == list(range(0, 1000, step))
@pytest.mark.parametrize('failure_frame', range(1, 6))
@pytest.mark.parametrize('disengage', [False, True])
def test_next_scheduled_frame_clears_invalid_or_inactive_path(failure_frame, disengage):
controller, cs = sender()
cc, sp = structs.CarControl(latActive=True), structs.CarControlSP()
sp.fordLateralPath.valid = True
sp.fordLateralPath.pathOffset, sp.fordLateralPath.pathAngle = .4, .1
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 0)], controller.CAN.main)
for frame in range(11):
if frame == failure_frame:
if disengage:
cc.latActive = False
else:
sp.fordLateralPath.valid = False
_, packets = controller.update(cc.as_reader(), sp, cs, frame * 10_000_000)
lateral = [p for p in packets if p[0] == 0x3d6]
assert len(lateral) == int(frame % 5 == 0)
if lateral and frame >= failure_frame:
parser.update([frame * 10_000_000, lateral])
wire = parser.vl['LateralMotionControl2']
assert wire['LatCtl_D2_Rq'] == (0 if disengage else 2)
assert all(wire[k] == 0. for k in ('LatCtlPathOffst_L_Actl', 'LatCtlPath_An_Actl', 'LatCtlCurv_No_Actl', 'LatCtlCrv_NoRate2_Actl'))
def test_core_slew_per_second_and_actual_panda_acceptance():
controller, cs = sender()
core = ModelActionController()
cc, sp = structs.CarControl(latActive=True), structs.CarControlSP()
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 0)], controller.CAN.main)
safety = libsafety_py.libsafety
assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, FordSafetyFlags.CANFD) == 0
safety.init_tests()
safety.set_controls_allowed(True)
frames = []
for frame in range(100):
command = core.update(straight(10.), .1, speed=20., dt=.01, pose_yaw_rate=0.)
assert core.c0 == pytest.approx((frame + 1) * .04)
assert core.c1 == pytest.approx((frame + 1) * .005)
sp.fordLateralPath.valid = command.valid
sp.fordLateralPath.pathOffset, sp.fordLateralPath.pathAngle = command.path_offset, command.path_angle
_, packets = controller.update(cc.as_reader(), sp, cs, frame * 10_000_000)
for address, data, bus in packets:
if address != 0x3d6:
continue
frames.append(frame)
safety.set_timer(frame * 10_000)
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data))
parser.update([frame * 10_000_000, [(address, data, bus)]])
wire = parser.vl['LateralMotionControl2']
assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-command.path_offset)
assert wire['LatCtlPath_An_Actl'] == pytest.approx(-command.path_angle)
assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
assert frames == list(range(0, 100, 5))
@@ -7,10 +7,14 @@ from types import SimpleNamespace
import pytest
from opendbc.car.ford.values import FordFlags
from opendbc.car import structs
from opendbc.car.ford.values import FordFlags, FordFlagsSP
from openpilot.cereal import custom
from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType
from openpilot.selfdrive.car.helpers import convert_to_capnp
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller
from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController
from openpilot.sunnypilot.mads.helpers import set_car_specific_params
def car_params(**overrides):
@@ -18,7 +22,7 @@ def car_params(**overrides):
'carFw': []} | overrides))
def startup(cp=None, params=None):
def startup(cp=None, params=None, cp_sp=None):
filename = Path(__file__).resolve().parents[1]/'controlsd.py'
tree = ast.parse(filename.read_text())
cls = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'Controls')
@@ -27,9 +31,15 @@ def startup(cp=None, params=None):
end = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_path')
if params is None:
params = SimpleNamespace(get_bool=lambda key: key == 'FordModelActionController')
controls = SimpleNamespace(CP=cp or car_params(), params=params, calibrated_pose=None,
cp = cp or car_params()
# card snapshots the toggle into CarParamsSP before controlsd starts.
if cp_sp is None:
cp_sp = structs.CarParamsSP()
if cp.brand == 'ford':
set_car_specific_params(cp, cp_sp, params)
controls = SimpleNamespace(CP=cp, CP_SP=cp_sp, params=params, calibrated_pose=None,
pose_calibrator=SimpleNamespace(calib_valid=False))
environment = {'self': controls, 'FordFlags': FordFlags, 'FordPath': FordPath,
environment = {'self': controls, 'FordFlags': FordFlags, 'FordFlagsSP': FordFlagsSP, 'FordPath': FordPath,
'FordPathController': FordPathController, 'FordPscmObserverPathController': FordPscmObserverPathController,
'FordModelActionController': FordModelActionController,
'select_model_action_controller': select_model_action_controller,
@@ -46,9 +56,23 @@ def test_actual_startup_priority(candidate, observer):
expected = FordModelActionController if candidate else previous
assert type(selected.ford_path_controller) is expected
assert selected.ford_model_action == candidate
assert bool(selected.CP_SP.flags & FordFlagsSP.MODEL_ACTION) == candidate
assert selected.ford_path == FordPath()
@pytest.mark.parametrize('selected', [False, True])
def test_controller_and_sender_share_card_snapshot_when_stored_toggle_changes(selected):
cp, cp_sp = car_params(), structs.CarParamsSP(flags=128)
set_car_specific_params(cp, cp_sp, SimpleNamespace(get_bool=lambda key: selected))
with custom.CarParamsSP.from_bytes(convert_to_capnp(cp_sp).to_bytes()) as snapshot:
controls = startup(cp, SimpleNamespace(get_bool=lambda key: not selected), snapshot)
assert controls.ford_model_action == selected
assert bool(controls.CP_SP.flags & FordFlagsSP.MODEL_ACTION) == selected
assert controls.CP_SP.flags & 128
set_car_specific_params(cp, cp_sp, SimpleNamespace(get_bool=lambda key: False))
assert cp_sp.flags == 128
@pytest.mark.parametrize('overrides', [{'brand': 'tesla'}, {'flags': 0}, {'carFingerprint': 'FORD_F_150_MK14'}])
@pytest.mark.parametrize('observer', [False, True])
def test_other_vehicles_keep_their_previous_selection(overrides, observer):
+6
View File
@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
from openpilot.common.params import Params
from opendbc.car import structs
from opendbc.car.ford.values import FordFlags, FordFlagsSP
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP
from opendbc.sunnypilot.car.tesla.values import MadsScreenButtonType, TeslaFlagsSP
@@ -54,6 +55,11 @@ def set_alternative_experience(CP: structs.CarParams, CP_SP: structs.CarParamsSP
def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
if CP.brand == "ford":
CP_SP.flags &= ~FordFlagsSP.MODEL_ACTION.value
if CP.flags & FordFlags.CANFD and CP.carFingerprint == "FORD_F_150_LIGHTNING_MK1" and params.get_bool("FordModelActionController"):
CP_SP.flags |= FordFlagsSP.MODEL_ACTION.value
if CP.brand == "hyundai":
# TODO-SP: This should be separated from MADS module for future implementations
# Use "HyundaiLongitudinalMainCruiseToggleable" param