mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-09 22:23:46 +08:00
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:
@@ -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 v1–v6 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"
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: c21a901370...87ca78e6e6
@@ -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):
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user