mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-10-01 03:53:45 +08:00
Ford: remove diagnostic tests and restore stock maneuver tools
Revert the six Ford channel-test commits and discard the unfinished keyboard standstill changes. Restore the original lateral maneuver and joystick tools, removing test toggles, parameters, schema additions, reports, and control overrides. Preserve the driving controller and C0 distance toggle.
Validation: 232 Ford controller tests passed; tracked tree matches de23ac008.
This commit is contained in:
Isaac Barham
@@ -1220,21 +1220,6 @@ struct DriverAssistance {
|
||||
|
||||
struct LateralManeuverPlan {
|
||||
desiredCurvature @0 :Float32; # 1/m
|
||||
fordChannelTest @1 :FordChannelTest;
|
||||
|
||||
struct FordChannelTest {
|
||||
runId @0 :UInt32;
|
||||
channel @1 :Channel;
|
||||
phase @2 :Phase;
|
||||
delta @3 :Float32; # legacy raw-pulse increment; zero for normal channel maneuvers
|
||||
speed @4 :Float32; # target m/s
|
||||
keyboardRequestId @5 :UInt64; # nonzero for a manually triggered normal-controller step
|
||||
keyboardPhase @6 :Phase; # baseline/pulse/release inside phase=maneuver
|
||||
targetAccel @7 :Float32; # signed keyboard step amplitude, m/s^2 (not a raw field increment)
|
||||
|
||||
enum Channel { none @0; c0 @1; c1 @2; }
|
||||
enum Phase { waiting @0; baseline @1; pulse @2; release @3; complete @4; aborted @5; maneuver @6; }
|
||||
}
|
||||
}
|
||||
|
||||
struct LongitudinalPlan @0xe00b5b3eba12876c {
|
||||
@@ -2134,16 +2119,6 @@ struct Joystick {
|
||||
# convenient for debug and live tuning
|
||||
axes @0: List(Float32);
|
||||
buttons @1: List(Bool);
|
||||
fordKeyboard @2 :FordKeyboard;
|
||||
|
||||
struct FordKeyboard {
|
||||
requestId @0 :UInt64; # changes only for a new action; heartbeats repeat the same request
|
||||
channel @1 :LateralManeuverPlan.FordChannelTest.Channel;
|
||||
speed @2 :Float32;
|
||||
accel @3 :Float32;
|
||||
action @4 :Action;
|
||||
enum Action { idle @0; left @1; right @2; cancel @3; }
|
||||
}
|
||||
}
|
||||
|
||||
struct DriverStateV2 {
|
||||
|
||||
@@ -85,8 +85,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"LiveTorqueParameters", {PERSISTENT | DONT_LOG, BYTES}},
|
||||
{"LocationFilterInitialState", {PERSISTENT, BYTES}},
|
||||
{"LateralManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
|
||||
{"FordChannelTestMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0"}},
|
||||
{"FordChannelKeyboardMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0"}},
|
||||
{"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
|
||||
{"LongitudinalPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPersonality::STANDARD))}},
|
||||
{"NetworkMetered", {PERSISTENT | BACKUP, BOOL}},
|
||||
|
||||
@@ -16,7 +16,6 @@ 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
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import FordChannelTest, use_maneuver_reference, selected as ford_channel_test_selected
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
|
||||
@@ -59,8 +58,6 @@ class Controls(ControlsExt):
|
||||
self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"),
|
||||
c0_time_based=self.params.get_bool("FordC0TimeBased"))
|
||||
self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController)
|
||||
self.ford_channel_test = (FordChannelTest(keyboard=self.params.get_bool('FordChannelKeyboardMode'))
|
||||
if ford_channel_test_selected(self.CP, self.params) else None)
|
||||
if self.CP.brand == "ford":
|
||||
cloudlog.event("Ford path controller selected",
|
||||
controller=type(self.ford_path_controller).__name__ if self.ford_model_action else "upstream")
|
||||
@@ -152,7 +149,7 @@ class Controls(ControlsExt):
|
||||
|
||||
# Steering PID loop and lateral MPC
|
||||
# Reset desired curvature to current to avoid violating the limits on engage
|
||||
if use_maneuver_reference(self.sm['lateralManeuverPlan'], self.sm.valid['lateralManeuverPlan'], self.ford_channel_test is not None):
|
||||
if self.sm.valid['lateralManeuverPlan']:
|
||||
new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature if CC.latActive else self.curvature
|
||||
else:
|
||||
new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
|
||||
@@ -171,8 +168,7 @@ class Controls(ControlsExt):
|
||||
if self.CP.brand == "ford":
|
||||
ford_model = model_v2 if self.sm.valid['modelV2'] else None
|
||||
if self.ford_model_action:
|
||||
reference_service = ('lateralManeuverPlan' if use_maneuver_reference(self.sm['lateralManeuverPlan'],
|
||||
self.sm.valid['lateralManeuverPlan'], self.ford_channel_test is not None) else 'modelV2')
|
||||
reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
|
||||
self.ford_path = self.ford_path_controller.update(
|
||||
ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(),
|
||||
measurement_time=self.sm.logMonoTime['carState'] * 1e-9,
|
||||
@@ -182,22 +178,6 @@ class Controls(ControlsExt):
|
||||
driver_pressed=CS.steeringPressed, driver_torque=CS.steeringTorque,
|
||||
pscm_status=self.sm['carStateSP'].fordPscmStatus if self.sm.valid['carStateSP'] else None,
|
||||
)
|
||||
if self.ford_channel_test is not None:
|
||||
override = self.ford_channel_test.update(
|
||||
self.sm['lateralManeuverPlan'], plan_valid=self.sm.valid['lateralManeuverPlan'],
|
||||
plan_time=self.sm.logMonoTime['lateralManeuverPlan']*1e-9, now=time.monotonic(),
|
||||
normal=self.ford_path, active=CC.latActive,
|
||||
healthy=(CS.cruiseState.enabled or self.ford_channel_test.keyboard)
|
||||
and self.sm.all_checks(['carStateSP', 'carState', 'vehicleParameters', 'modelV2']),
|
||||
driver_input=CS.steeringPressed or not math.isfinite(CS.steeringTorque) or abs(CS.steeringTorque) > 1.
|
||||
or (CS.gasPressed and not self.ford_channel_test.keyboard) or CS.brakePressed,
|
||||
speed=CS.vEgo,
|
||||
)
|
||||
if override is not None:
|
||||
self.ford_path = override
|
||||
self.ford_path_controller.diagnostics['command'] = (override.path_offset, override.path_angle, 0., 0.)
|
||||
if self.sm.frame % 20 == 0:
|
||||
cloudlog.event('Ford channel test command', **self.ford_channel_test.diagnostics)
|
||||
if not self.ford_path.valid:
|
||||
CC.latActive = False
|
||||
if self.sm.frame % 20 == 0:
|
||||
|
||||
@@ -1,84 +0,0 @@
|
||||
"""Opt-in output isolation for the normal lateral maneuver controller path."""
|
||||
import math
|
||||
|
||||
from opendbc.car.ford.values import FordFlags
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
|
||||
PARAM = 'FordChannelTestMode'
|
||||
KEYBOARD_PARAM = 'FordChannelKeyboardMode'
|
||||
CONFLICTS = ('LateralManeuverMode', 'LongitudinalManeuverMode', 'JoystickDebugMode')
|
||||
SPEEDS = (15.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS)
|
||||
MAX_SPEED_ERROR = .7
|
||||
MAX_RUN_S = 4. # normal suite ~2.6 s, keyboard step 3 s; allow scheduling/cadence variation
|
||||
|
||||
|
||||
def selected(CP, params):
|
||||
return bool(CP.brand == 'ford' and CP.flags & FordFlags.CANFD and params.get_bool('FordModelActionController')
|
||||
and (params.get_bool(PARAM) or params.get_bool(KEYBOARD_PARAM)) and not any(params.get_bool(key) for key in CONFLICTS))
|
||||
|
||||
|
||||
def is_channel_plan(plan):
|
||||
test = getattr(plan, 'fordChannelTest', None)
|
||||
return test is not None and test.channel != 'none'
|
||||
|
||||
|
||||
def is_channel_maneuver(plan):
|
||||
return is_channel_plan(plan) and plan.fordChannelTest.phase == 'maneuver'
|
||||
|
||||
|
||||
def use_maneuver_reference(plan, valid, channel_test_enabled):
|
||||
return valid and (not is_channel_plan(plan) or (channel_test_enabled and is_channel_maneuver(plan)))
|
||||
|
||||
|
||||
class FordChannelTest:
|
||||
"""Check the maneuver lease, then select one already-computed controller field.
|
||||
|
||||
The normal curvature limiter, model mapping and PI feedback run unchanged.
|
||||
This class supplies no waveform, captured command, gain or feedback reset.
|
||||
"""
|
||||
def __init__(self, keyboard=False):
|
||||
self.keyboard = keyboard
|
||||
self.clear()
|
||||
|
||||
def clear(self):
|
||||
self.run_id = self.channel = self.start = self.last_time = None
|
||||
self.aborted = False
|
||||
self.diagnostics = {}
|
||||
|
||||
def fail(self, reason):
|
||||
self.aborted = True
|
||||
self.diagnostics.update(status='aborted', reason=reason, command=(0., 0., 0., 0.))
|
||||
return FordPath()
|
||||
|
||||
def update(self, plan, *, plan_valid, plan_time, now, normal, active, healthy, driver_input, speed):
|
||||
test = plan.fordChannelTest
|
||||
fresh = math.isfinite(now) and math.isfinite(plan_time) and -.005 <= now-plan_time <= .15
|
||||
if not plan_valid and (fresh or self.run_id is None):
|
||||
self.clear()
|
||||
return None
|
||||
if self.aborted:
|
||||
return FordPath()
|
||||
if not fresh or not plan_valid:
|
||||
return self.fail('stale maneuver plan')
|
||||
if not is_channel_maneuver(plan) or test.runId == 0 or test.delta != 0. or bool(test.keyboardRequestId) != self.keyboard:
|
||||
return self.fail('invalid maneuver identity')
|
||||
if not active or not healthy or not normal.valid or driver_input:
|
||||
return self.fail('driver input, disengagement or invalid service')
|
||||
if not all(math.isfinite(x) for x in (speed, test.speed, plan.desiredCurvature, normal.path_offset, normal.path_angle)):
|
||||
return self.fail('nonfinite input')
|
||||
if not any(abs(test.speed-v) < 1e-5 for v in SPEEDS) or abs(speed-test.speed) > MAX_SPEED_ERROR:
|
||||
return self.fail('speed out of range')
|
||||
if self.run_id is None:
|
||||
self.run_id, self.channel, self.start, self.target_speed = test.runId, str(test.channel), now, test.speed
|
||||
if (test.runId != self.run_id or test.channel != self.channel or test.speed != self.target_speed
|
||||
or (self.last_time is not None and not .002 <= now-self.last_time <= .1) or now-self.start > MAX_RUN_S):
|
||||
return self.fail('maneuver identity or timing changed')
|
||||
self.last_time = now
|
||||
command = FordPath(True, normal.path_offset if self.channel == 'c0' else 0., normal.path_angle if self.channel == 'c1' else 0., 0., 0.)
|
||||
self.diagnostics = {'status': 'active', 'run_id': self.run_id, 'channel': self.channel,
|
||||
'desired_curvature': plan.desiredCurvature, 'target_speed': test.speed,
|
||||
'keyboard_request_id': test.keyboardRequestId, 'keyboard_phase': str(test.keyboardPhase),
|
||||
'target_accel': test.targetAccel,
|
||||
'command': (command.path_offset, command.path_angle, 0., 0.)}
|
||||
return command
|
||||
@@ -1,245 +0,0 @@
|
||||
"""Compare normal maneuver injection and controller output with channel isolation."""
|
||||
import math
|
||||
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.values import FordFlags
|
||||
from openpilot.cereal import custom, log, messaging
|
||||
from openpilot.common.params import Params, ParamKeyFlag
|
||||
from openpilot.selfdrive.car.helpers import convert_carControlSP
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import FordChannelTest, SPEEDS, use_maneuver_reference
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
|
||||
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.tools.lateral_maneuvers import lateral_maneuversd as daemon
|
||||
|
||||
|
||||
def plan(channel='c0', phase='maneuver', speed=SPEEDS[0], run_id=1, curvature=.01, keyboard_request_id=0):
|
||||
msg = log.LateralManeuverPlan.new_message(desiredCurvature=curvature)
|
||||
msg.fordChannelTest = {'runId': run_id, 'channel': channel, 'phase': phase, 'speed': speed, 'keyboardRequestId': keyboard_request_id}
|
||||
with log.LateralManeuverPlan.from_bytes(msg.to_bytes()) as reader:
|
||||
return reader.as_builder()
|
||||
|
||||
|
||||
def update(test, now=10., msg=None, **changes):
|
||||
args = {'plan_valid': True, 'plan_time': now, 'now': now, 'normal': FordPath(True, .24, .08),
|
||||
'active': True, 'healthy': True, 'driver_input': False, 'speed': SPEEDS[0]}
|
||||
return test.update(msg or plan(), **(args | changes))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('changes', [
|
||||
{'active': False}, {'healthy': False}, {'driver_input': True}, {'speed': SPEEDS[0]+.71}, {'speed': math.nan},
|
||||
{'normal': FordPath()}, {'normal': FordPath(True, math.nan, 0.)}, {'plan_time': 9.8},
|
||||
])
|
||||
def test_active_faults_zero_output_and_latch(changes):
|
||||
test = FordChannelTest()
|
||||
assert update(test).valid
|
||||
assert update(test, 10.01, **changes) == FordPath()
|
||||
assert update(test, 10.02) == FordPath()
|
||||
assert update(test, 10.03, plan_valid=False) is None
|
||||
assert update(test, 10.04, plan(run_id=2)).valid
|
||||
|
||||
|
||||
@pytest.mark.parametrize('msg', [plan(channel='c1'), plan(run_id=2), plan(speed=SPEEDS[1]), plan(phase='pulse')])
|
||||
def test_midrun_identity_changes_abort(msg):
|
||||
test = FordChannelTest()
|
||||
update(test)
|
||||
assert update(test, 10.01, msg) == FordPath()
|
||||
|
||||
|
||||
def test_legacy_raw_pulse_cannot_actuate():
|
||||
msg = plan(phase='pulse')
|
||||
msg.fordChannelTest.delta = 1.28
|
||||
assert update(FordChannelTest(), msg=msg) == FordPath()
|
||||
|
||||
|
||||
def test_fresh_but_frozen_maneuver_times_out():
|
||||
test = FordChannelTest()
|
||||
for i in range(401):
|
||||
assert update(test, 10.+i*.01).valid
|
||||
assert update(test, 14.01) == FordPath()
|
||||
|
||||
|
||||
def test_mode_selection_and_transient_parameter(tmp_path):
|
||||
params = Params(str(tmp_path))
|
||||
params.put_bool('FordModelActionController', True, block=True)
|
||||
assert startup(params=params).ford_channel_test is None
|
||||
params.put_bool('FordChannelTestMode', True, block=True)
|
||||
assert isinstance(startup(params=params).ford_channel_test, FordChannelTest)
|
||||
for key in ('FordModelActionController', 'LateralManeuverMode', 'LongitudinalManeuverMode', 'JoystickDebugMode'):
|
||||
params.put_bool(key, key != 'FordModelActionController', block=True)
|
||||
assert startup(params=params).ford_channel_test is None
|
||||
params.put_bool(key, key == 'FordModelActionController', block=True)
|
||||
for cp in (car_params(brand='toyota'), car_params(flags=0)):
|
||||
assert startup(cp, params).ford_channel_test is None
|
||||
for flag in (ParamKeyFlag.CLEAR_ON_MANAGER_START, ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION):
|
||||
params.put_bool('FordChannelTestMode', True, block=True)
|
||||
params.clear_all(flag)
|
||||
assert not params.get_bool('FordChannelTestMode')
|
||||
|
||||
|
||||
@pytest.mark.parametrize('channel', ['c0', 'c1'])
|
||||
@pytest.mark.parametrize('speed', SPEEDS)
|
||||
@pytest.mark.parametrize('limited', [False, True])
|
||||
@pytest.mark.parametrize('keyboard', [False, True])
|
||||
def test_real_injection_and_wire_match_normal_controller_on_selected_channel(pipeline, channel, speed, limited, keyboard): # noqa: F811
|
||||
call, publication = pipeline
|
||||
normal = startup()
|
||||
mode = 'FordChannelKeyboardMode' if keyboard else 'FordChannelTestMode'
|
||||
isolated = startup(params=SimpleNamespace(get_bool=lambda key: key in ('FordModelActionController', mode)))
|
||||
controllers = (normal, isolated)
|
||||
cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint=normal.CP.carFingerprint)
|
||||
sender = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
|
||||
vehicle = SimpleNamespace(out=structs.CarState(vEgo=speed, vEgoRaw=speed), 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)], sender.CAN.main)
|
||||
for control in controllers:
|
||||
control.sm, control.desired_curvature, control.curvature = Subscriptions(True), 0., 0.
|
||||
model = straight()
|
||||
model.action = SimpleNamespace(desiredCurvature=-.15) # opposite model must lose to the actual maneuver injection
|
||||
cs = SimpleNamespace(vEgo=speed, yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.,
|
||||
gasPressed=keyboard, brakePressed=False, cruiseState=SimpleNamespace(enabled=not keyboard))
|
||||
for i in range(251):
|
||||
now = 10.+i*.01
|
||||
request = (.5 if i < 100 else -.5)/speed**2
|
||||
if keyboard:
|
||||
request = (.5 if 50 <= i < 150 else 0.)/speed**2
|
||||
for control in controllers:
|
||||
sm = control.sm
|
||||
sm.messages['lateralManeuverPlan'] = plan(channel=channel if control is isolated else 'none', curvature=request, speed=speed,
|
||||
keyboard_request_id=1 if keyboard and control is isolated else 0)
|
||||
sm.logMonoTime = dict.fromkeys(sm.logMonoTime, round(now*1e9))
|
||||
status = sm['carStateSP'].fordPscmStatus
|
||||
status.valid, status.canMonoTime, status.lateralState, status.limit = True, round(now*1e9), 2, 2 if limited else 0
|
||||
cc = structs.CarControl(latActive=True)
|
||||
cs.brakePressed = i == 250
|
||||
exec(call, {'self': control, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
|
||||
'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda t=now: t), 'math': math})
|
||||
assert isolated.desired_curvature == normal.desired_curvature
|
||||
assert isolated.ford_path_controller.core.correction == normal.ford_path_controller.core.correction
|
||||
assert isolated.ford_path_controller.core.proportional == normal.ford_path_controller.core.proportional
|
||||
if i != 250:
|
||||
assert isolated.ford_path == FordPath(True, normal.ford_path.path_offset if channel == 'c0' else 0.,
|
||||
normal.ford_path.path_angle if channel == 'c1' else 0.)
|
||||
assert isolated.ford_path_controller.diagnostics['reference_age'] == pytest.approx(0.)
|
||||
msg = custom.CarControlSP.new_message()
|
||||
exec(publication, {'self': isolated, 'CC_SP': msg})
|
||||
_, packets = sender.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9))
|
||||
parser.update([round(now*1e9), packets])
|
||||
wire = parser.vl['LateralMotionControl2']
|
||||
assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-isolated.ford_path.path_offset, abs=1e-7)
|
||||
assert wire['LatCtlPath_An_Actl'] == pytest.approx(-isolated.ford_path.path_angle, abs=1e-7)
|
||||
assert wire['LatCtl_D2_Rq'] == (0 if i == 250 else 2)
|
||||
assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
|
||||
if i == 50:
|
||||
assert isolated.desired_curvature > 0.
|
||||
if i == 200:
|
||||
if keyboard:
|
||||
assert isolated.desired_curvature == 0.
|
||||
else:
|
||||
assert isolated.desired_curvature < 0.
|
||||
|
||||
|
||||
def run_daemon(monkeypatch, interrupt=False):
|
||||
class Finished(Exception):
|
||||
pass
|
||||
|
||||
plans, alerts, checks = [], [], []
|
||||
clock = [10.]
|
||||
pulse_samples = [0]
|
||||
cp = daemon.car.CarParams.new_message().to_bytes()
|
||||
|
||||
class SM(messaging.SubMaster):
|
||||
def __init__(self, services, **kwargs):
|
||||
super().__init__(services, **kwargs)
|
||||
self.events = {s: messaging.new_message(s) for s in services}
|
||||
|
||||
def update(self):
|
||||
clock[0] += .05
|
||||
assert clock[0] < 400., 'Suite did not finish'
|
||||
cs = self.events['carState'].carState
|
||||
cs.vEgo = plans[-1].lateralManeuverPlan.fordChannelTest.speed if plans else SPEEDS[0]
|
||||
cs.canValid, cs.cruiseState.enabled = True, True
|
||||
cs.steeringPressed = interrupt and pulse_samples[0] == 5
|
||||
self.events['carControl'].carControl.latActive = True
|
||||
self.events['carControl'].carControl.orientationNED = [0., 0., 0.]
|
||||
self.events['selfdriveState'].selfdriveState.enabled = True
|
||||
for msg in self.events.values():
|
||||
msg.valid, msg.logMonoTime = True, round(clock[0]*1e9)
|
||||
self.update_msgs(clock[0], [m.as_reader() for m in self.events.values()])
|
||||
checks.append(self.all_checks())
|
||||
if cs.steeringPressed:
|
||||
pulse_samples[0] += 1
|
||||
|
||||
class PM:
|
||||
def __init__(self, _services):
|
||||
pass
|
||||
|
||||
def send(self, service, msg):
|
||||
msg.logMonoTime = round(clock[0]*1e9)
|
||||
(plans if service == 'lateralManeuverPlan' else alerts).append(msg)
|
||||
if service == 'lateralManeuverPlan' and msg.valid:
|
||||
pulse_samples[0] += 1
|
||||
if service == 'alertDebug' and msg.alertDebug.alertText1 == 'Maneuvers Finished':
|
||||
raise Finished
|
||||
|
||||
with monkeypatch.context() as mp:
|
||||
mp.setattr(daemon.messaging, 'SubMaster', SM)
|
||||
mp.setattr(daemon.messaging, 'PubMaster', PM)
|
||||
mp.setattr(daemon, 'Params', lambda: SimpleNamespace(get=lambda *_a, **_kw: cp))
|
||||
with pytest.raises(Finished):
|
||||
daemon.main(ford_channels=True)
|
||||
assert all(checks[2:])
|
||||
return plans, alerts
|
||||
|
||||
|
||||
@pytest.mark.parametrize('interrupt', [False, True])
|
||||
def test_actual_daemon_reuses_all_normal_waveforms_repeats_and_retry(monkeypatch, interrupt):
|
||||
plans, alerts = run_daemon(monkeypatch, interrupt)
|
||||
groups = defaultdict(list)
|
||||
for msg in plans:
|
||||
if msg.valid:
|
||||
groups[msg.lateralManeuverPlan.fordChannelTest.runId].append(msg.lateralManeuverPlan)
|
||||
# 16 maneuvers, each performed 3 times. An interrupted attempt is retried.
|
||||
assert len(groups) == 48+interrupt
|
||||
assert sum(a.alertDebug.alertText1 == 'Complete' for a in alerts) == 48
|
||||
expected_runs = []
|
||||
for maneuver in daemon.channel_maneuvers():
|
||||
for _ in range(3):
|
||||
reference = daemon.Maneuver(maneuver.description, maneuver.actions, initial_speed=maneuver.initial_speed)
|
||||
expected = []
|
||||
while not reference.finished:
|
||||
value = reference.get_accel(maneuver.initial_speed, True, 0., 0.)
|
||||
if reference.active and not reference._run_completed:
|
||||
expected.append(value/maneuver.initial_speed**2)
|
||||
expected_runs.append((maneuver.channel, maneuver.initial_speed, expected))
|
||||
completed = list(groups.values())[1:] if interrupt else list(groups.values())
|
||||
for actual, (channel, speed, expected) in zip(completed, expected_runs, strict=True):
|
||||
assert [p.desiredCurvature for p in actual] == pytest.approx(expected, abs=1e-7)
|
||||
assert all(p.fordChannelTest.channel == channel and p.fordChannelTest.speed == pytest.approx(speed) for p in actual)
|
||||
assert all(p.fordChannelTest.delta == 0. and p.fordChannelTest.phase == 'maneuver' for p in actual)
|
||||
if interrupt:
|
||||
assert len(next(iter(groups.values()))) == 5
|
||||
|
||||
|
||||
@pytest.mark.parametrize('phase', ['maneuver', 'pulse'])
|
||||
def test_channel_payload_is_ignored_when_toggle_off(pipeline, phase): # noqa: F811
|
||||
controls = startup()
|
||||
sm = Subscriptions(True)
|
||||
sm.messages['lateralManeuverPlan'] = plan(phase=phase, curvature=-.1)
|
||||
controls.sm, controls.desired_curvature, controls.curvature = sm, 0., 0.
|
||||
model = straight()
|
||||
model.action = SimpleNamespace(desiredCurvature=.1)
|
||||
cc = structs.CarControl(latActive=True)
|
||||
cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
|
||||
exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
|
||||
'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
|
||||
assert controls.desired_curvature > 0. and controls.ford_path.path_angle > 0.
|
||||
@@ -22,7 +22,6 @@ from openpilot.selfdrive.car.helpers import convert_carControlSP
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, encode_model_action
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import use_maneuver_reference
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action import circle, straight
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import CANFD_CARS, car_params, startup
|
||||
|
||||
@@ -141,7 +140,7 @@ def pipeline():
|
||||
controls_file = root/'selfdrive/controls/controlsd.py'
|
||||
body = _method(controls_file, 'Controls', 'state_control').body
|
||||
# Execute the actual source choice, upstream limiter and Ford integration.
|
||||
selection = next(n for n in body if isinstance(n, ast.If) and "self.sm.valid['lateralManeuverPlan']" in ast.unparse(n.test))
|
||||
selection = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.sm.valid['lateralManeuverPlan']")
|
||||
limiter = next(n for n in body if isinstance(n, ast.Assign) and isinstance(n.value, ast.Call) and
|
||||
isinstance(n.value.func, ast.Name) and n.value.func.id == 'clip_curvature')
|
||||
branch = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.CP.brand == 'ford'")
|
||||
@@ -184,7 +183,7 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
|
||||
cc = structs.CarControl(latActive=True)
|
||||
cs = SimpleNamespace(vEgo=20., yawRate=-.0072, canValid=True, steeringPressed=False, steeringTorque=0.)
|
||||
environment = {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
|
||||
'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}
|
||||
'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}
|
||||
exec(call, environment)
|
||||
expected_curvature = (-1 if maneuver else 1)*.000125
|
||||
assert controls.desired_curvature == pytest.approx(expected_curvature)
|
||||
@@ -228,7 +227,7 @@ def test_actual_controlsd_service_gates(pipeline, maneuver, failed):
|
||||
model = straight()
|
||||
model.action = SimpleNamespace(desiredCurvature=.1)
|
||||
exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
|
||||
'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
|
||||
'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
|
||||
assert controls.ford_path.valid == cc.latActive == (failed == 'lateralManeuverPlan' and not maneuver)
|
||||
|
||||
|
||||
@@ -257,7 +256,7 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
controls.curvature, cs.steeringTorque = measured, torque
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
|
||||
environment = {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)}
|
||||
exec(call, environment)
|
||||
msg = custom.CarControlSP.new_message()
|
||||
@@ -300,7 +299,7 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s
|
||||
status = sm['carStateSP'].fordPscmStatus
|
||||
status.valid, status.canMonoTime, status.limit, status.lateralState = True, round(now*1e9), 2, 2
|
||||
exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
controller = controls.ford_path_controller
|
||||
assert controller.diagnostics['pscm_limited'] is service_valid
|
||||
@@ -336,7 +335,7 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9), lateralManeuverPlan=round(now*1e9))
|
||||
before = core.c0, core.c1, core.correction
|
||||
exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
assert_current_request(core, controls.desired_curvature, cs.vEgo)
|
||||
increment = .25*speed*(controls.desired_curvature-controls.curvature)*.01
|
||||
@@ -391,7 +390,7 @@ def test_unwind_and_catchup_through_selected_request_and_actual_can(pipeline, si
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9), lateralManeuverPlan=round(now*1e9))
|
||||
before = core.c0, core.c1, core.correction
|
||||
exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
assert_current_request(core, controls.desired_curvature, cs.vEgo)
|
||||
if frame == 129:
|
||||
@@ -441,7 +440,7 @@ def test_heading_overflow_and_release_through_actual_can(pipeline, sign, fingerp
|
||||
controls.curvature = clip_curvature(cs.vEgo, controls.desired_curvature, desired, 0.)[0]
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
|
||||
exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
assert_current_request(core, controls.desired_curvature, cs.vEgo)
|
||||
assert core.correction == 0.
|
||||
@@ -498,7 +497,7 @@ def test_toggle_off_preserves_upstream_actuators_and_can(pipeline, fingerprint,
|
||||
now = 1.+frame*.01
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
|
||||
exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature})
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature})
|
||||
assert cc.actuators.curvature == before and cc.latActive == active
|
||||
msg = custom.CarControlSP.new_message()
|
||||
exec(publication, {'self': controls, 'CC_SP': msg})
|
||||
|
||||
@@ -11,7 +11,6 @@ from opendbc.car.ford.values import CAR, FordFlags
|
||||
from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import C1_INTEGRAL_GAIN, C1_PROPORTIONAL_GAIN, FordModelActionController, select_model_action_controller
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import FordChannelTest, selected as ford_channel_test_selected
|
||||
|
||||
|
||||
CANFD_CARS = [car for car in CAR if car.config.flags & FordFlags.CANFD]
|
||||
@@ -35,7 +34,6 @@ def startup(cp=None, params=None):
|
||||
environment = {'self': controls, 'FordFlags': FordFlags, 'FordPath': FordPath,
|
||||
'FordModelActionController': FordModelActionController,
|
||||
'select_model_action_controller': select_model_action_controller,
|
||||
'FordChannelTest': FordChannelTest, 'ford_channel_test_selected': ford_channel_test_selected,
|
||||
'cloudlog': SimpleNamespace(event=lambda *args, **kwargs: None)}
|
||||
exec(compile(ast.Module(body=body[start:end+1], type_ignores=[]), str(filename), 'exec'), environment)
|
||||
return controls
|
||||
|
||||
@@ -167,7 +167,6 @@ class DeveloperLayout(Widget):
|
||||
self._params.put_bool("SshEnabled", state, block=True)
|
||||
|
||||
def _on_joystick_debug_mode(self, state: bool):
|
||||
self._params.put_bool('FordChannelKeyboardMode', False, block=True)
|
||||
self._params.put_bool("JoystickDebugMode", state, block=True)
|
||||
self._params.put_bool("LongitudinalManeuverMode", False, block=True)
|
||||
self._long_maneuver_toggle.action_item.set_state(False)
|
||||
@@ -175,7 +174,6 @@ class DeveloperLayout(Widget):
|
||||
self._lat_maneuver_toggle.action_item.set_state(False)
|
||||
|
||||
def _on_long_maneuver_mode(self, state: bool):
|
||||
self._params.put_bool('FordChannelKeyboardMode', False, block=True)
|
||||
self._params.put_bool("LongitudinalManeuverMode", state, block=True)
|
||||
self._params.put_bool("JoystickDebugMode", False, block=True)
|
||||
self._joystick_toggle.action_item.set_state(False)
|
||||
@@ -183,7 +181,6 @@ class DeveloperLayout(Widget):
|
||||
self._lat_maneuver_toggle.action_item.set_state(False)
|
||||
|
||||
def _on_lat_maneuver_mode(self, state: bool):
|
||||
self._params.put_bool('FordChannelKeyboardMode', False, block=True)
|
||||
self._params.put_bool("LateralManeuverMode", state, block=True)
|
||||
self._params.put_bool("ExperimentalMode", False, block=True)
|
||||
self._params.put_bool("JoystickDebugMode", False, block=True)
|
||||
|
||||
@@ -1,5 +1,4 @@
|
||||
from collections.abc import Callable
|
||||
from opendbc.car.ford.values import FordFlags
|
||||
from openpilot.common.time_helpers import system_time_valid
|
||||
from openpilot.system.ui.widgets.scroller import NavScroller
|
||||
from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigToggle, BigParamControl, BigCircleParamControl, GreyBigButton
|
||||
@@ -80,9 +79,6 @@ class DeveloperLayoutMici(NavScroller):
|
||||
self._lat_maneuver_toggle = BigToggle("lateral maneuver mode",
|
||||
initial_state=ui_state.params.get_bool("LateralManeuverMode"),
|
||||
toggle_callback=self._on_lat_maneuver_mode)
|
||||
self._ford_channel_test_toggle = BigToggle("Ford C0 / C1 test",
|
||||
initial_state=ui_state.params.get_bool("FordChannelTestMode"),
|
||||
toggle_callback=self._on_ford_channel_test)
|
||||
self._alpha_long_toggle = BigToggle("alpha longitudinal",
|
||||
initial_state=ui_state.params.get_bool("AlphaLongitudinalEnabled"),
|
||||
toggle_callback=self._on_alpha_long_enabled)
|
||||
@@ -97,7 +93,6 @@ class DeveloperLayoutMici(NavScroller):
|
||||
self._joystick_toggle,
|
||||
self._long_maneuver_toggle,
|
||||
self._lat_maneuver_toggle,
|
||||
self._ford_channel_test_toggle,
|
||||
self._alpha_long_toggle,
|
||||
self._debug_mode_toggle,
|
||||
])
|
||||
@@ -109,13 +104,11 @@ class DeveloperLayoutMici(NavScroller):
|
||||
("JoystickDebugMode", self._joystick_toggle),
|
||||
("LongitudinalManeuverMode", self._long_maneuver_toggle),
|
||||
("LateralManeuverMode", self._lat_maneuver_toggle),
|
||||
("FordChannelTestMode", self._ford_channel_test_toggle),
|
||||
("AlphaLongitudinalEnabled", self._alpha_long_toggle),
|
||||
("ShowDebugInfo", self._debug_mode_toggle),
|
||||
)
|
||||
onroad_blocked_toggles = (self._adb_toggle, self._joystick_toggle)
|
||||
release_blocked_toggles = (self._joystick_toggle, self._long_maneuver_toggle, self._lat_maneuver_toggle,
|
||||
self._ford_channel_test_toggle, self._alpha_long_toggle)
|
||||
release_blocked_toggles = (self._joystick_toggle, self._long_maneuver_toggle, self._lat_maneuver_toggle, self._alpha_long_toggle)
|
||||
engaged_blocked_toggles = (self._long_maneuver_toggle, self._lat_maneuver_toggle, self._alpha_long_toggle)
|
||||
|
||||
# Hide non-release toggles on release builds
|
||||
@@ -125,8 +118,6 @@ class DeveloperLayoutMici(NavScroller):
|
||||
# Disable toggles that require offroad
|
||||
for item in onroad_blocked_toggles:
|
||||
item.set_enabled(lambda: ui_state.is_offroad())
|
||||
self._ford_channel_test_toggle.set_enabled(lambda: ui_state.is_offroad())
|
||||
self._ford_channel_test_toggle.set_visible(False) # revealed after CarParams gating on show
|
||||
|
||||
# Disable toggles that require not engaged
|
||||
for item in engaged_blocked_toggles:
|
||||
@@ -149,9 +140,6 @@ class DeveloperLayoutMici(NavScroller):
|
||||
|
||||
def _update_toggles(self):
|
||||
ui_state.update_params()
|
||||
cp = ui_state.CP
|
||||
self._ford_channel_test_toggle.set_visible(bool(cp is not None and cp.brand == 'ford' and cp.flags & FordFlags.CANFD
|
||||
and not ui_state.is_release and ui_state.params.get_bool('FordModelActionController')))
|
||||
|
||||
# CP gating
|
||||
if ui_state.CP is not None:
|
||||
@@ -175,7 +163,6 @@ class DeveloperLayoutMici(NavScroller):
|
||||
item.set_checked(ui_state.params.get_bool(key))
|
||||
|
||||
def _on_joystick_debug_mode(self, state: bool):
|
||||
self._clear_ford_channel_test()
|
||||
ui_state.params.put_bool("JoystickDebugMode", state, block=True)
|
||||
ui_state.params.put_bool("LongitudinalManeuverMode", False, block=True)
|
||||
self._long_maneuver_toggle.set_checked(False)
|
||||
@@ -183,7 +170,6 @@ class DeveloperLayoutMici(NavScroller):
|
||||
self._lat_maneuver_toggle.set_checked(False)
|
||||
|
||||
def _on_long_maneuver_mode(self, state: bool):
|
||||
self._clear_ford_channel_test()
|
||||
ui_state.params.put_bool("LongitudinalManeuverMode", state, block=True)
|
||||
ui_state.params.put_bool("JoystickDebugMode", False, block=True)
|
||||
self._joystick_toggle.set_checked(False)
|
||||
@@ -192,7 +178,6 @@ class DeveloperLayoutMici(NavScroller):
|
||||
restart_needed_callback()
|
||||
|
||||
def _on_lat_maneuver_mode(self, state: bool):
|
||||
self._clear_ford_channel_test()
|
||||
ui_state.params.put_bool("LateralManeuverMode", state, block=True)
|
||||
ui_state.params.put_bool("ExperimentalMode", False, block=True)
|
||||
ui_state.params.put_bool("JoystickDebugMode", False, block=True)
|
||||
@@ -201,21 +186,6 @@ class DeveloperLayoutMici(NavScroller):
|
||||
self._long_maneuver_toggle.set_checked(False)
|
||||
restart_needed_callback()
|
||||
|
||||
def _clear_ford_channel_test(self):
|
||||
ui_state.params.put_bool('FordChannelTestMode', False, block=True)
|
||||
ui_state.params.put_bool('FordChannelKeyboardMode', False, block=True)
|
||||
self._ford_channel_test_toggle.set_checked(False)
|
||||
|
||||
def _on_ford_channel_test(self, state: bool):
|
||||
ui_state.params.put_bool('FordChannelKeyboardMode', False, block=True)
|
||||
ui_state.params.put_bool('FordChannelTestMode', state, block=True)
|
||||
for key, toggle in (('LateralManeuverMode', self._lat_maneuver_toggle),
|
||||
('LongitudinalManeuverMode', self._long_maneuver_toggle), ('JoystickDebugMode', self._joystick_toggle)):
|
||||
ui_state.params.put_bool(key, False, block=True)
|
||||
toggle.set_checked(False)
|
||||
ui_state.params.put_bool('ExperimentalMode', False, block=True)
|
||||
restart_needed_callback()
|
||||
|
||||
def _on_alpha_long_enabled(self, state: bool):
|
||||
def do_toggle(_state: bool):
|
||||
ui_state.params.put_bool("AlphaLongitudinalEnabled", _state, block=True)
|
||||
|
||||
@@ -8,7 +8,6 @@ from openpilot.common.params import Params
|
||||
from openpilot.common.hardware import PC, COMMA_HARDWARE
|
||||
from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import selected as ford_channel_test_selected
|
||||
|
||||
from openpilot.sunnypilot.mapd.mapd_manager import MAPD_PATH
|
||||
|
||||
@@ -51,9 +50,6 @@ def long_maneuver(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
def lat_maneuver(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and params.get_bool("LateralManeuverMode")
|
||||
|
||||
def ford_channel_test(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and ford_channel_test_selected(CP, params)
|
||||
|
||||
def not_long_maneuver(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and not params.get_bool("LongitudinalManeuverMode")
|
||||
|
||||
@@ -150,7 +146,6 @@ procs = [
|
||||
PythonProcess("plannerd", "openpilot.selfdrive.controls.plannerd", not_long_maneuver),
|
||||
PythonProcess("maneuversd", "openpilot.tools.longitudinal_maneuvers.maneuversd", long_maneuver),
|
||||
PythonProcess("lateral_maneuversd", "openpilot.tools.lateral_maneuvers.lateral_maneuversd", lat_maneuver),
|
||||
PythonProcess("ford_maneuversd", "openpilot.tools.lateral_maneuvers.ford_maneuversd", ford_channel_test),
|
||||
PythonProcess("radard", "openpilot.selfdrive.controls.radard", only_onroad),
|
||||
PythonProcess("hardwared", "openpilot.system.hardware.hardwared", always_run),
|
||||
PythonProcess("modem", "openpilot.common.hardware.comma.modem", always_run, enabled=COMMA_HARDWARE),
|
||||
|
||||
@@ -5,9 +5,6 @@
|
||||
With joystick_control, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard.
|
||||
joystick_control uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks.
|
||||
|
||||
For the custom Ford C0/C1 controller, use the [Ford keyboard channel test](../lateral_maneuvers/FORD_CHANNEL_TEST.md#keyboard-triggered-steps-acc-or-mads).
|
||||
It preserves the normal controller and ACC/MADS, unlike stock Joystick Debug Mode below.
|
||||
|
||||
## Usage
|
||||
|
||||
The car must be off, and openpilot must be offroad before starting `joystick_control`.
|
||||
|
||||
@@ -1,137 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Keyboard frontend for Ford isolated-channel tests. Keeps controlsd and ACC/MADS running."""
|
||||
import argparse
|
||||
import math
|
||||
import time
|
||||
|
||||
from opendbc.car.ford.values import FordFlags
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.cereal import log, messaging
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import CONFLICTS, KEYBOARD_PARAM, PARAM, SPEEDS
|
||||
from openpilot.tools.lateral_maneuvers.ford_keyboard import MIN_ACCEL, MAX_ACCEL, ACCEL_INCREMENT
|
||||
from openpilot.tools.lib.kbhit import KBHit
|
||||
|
||||
HELP = '1 C0 | 2 C1 | V 15/20 mph | +/- strength | A left step | D right step | R cancel | Q quit'
|
||||
|
||||
|
||||
class KeyboardControl:
|
||||
def __init__(self, speed=15, accel=.5):
|
||||
if speed not in (15, 20) or not math.isfinite(accel) or not MIN_ACCEL <= accel <= MAX_ACCEL:
|
||||
raise ValueError(f'Use 15/20 mph and {MIN_ACCEL}–{MAX_ACCEL} m/s²')
|
||||
self.request = log.Joystick.FordKeyboard.new_message(channel='c0', speed=speed*CV.MPH_TO_MS, accel=accel, action='idle')
|
||||
self.last_key, self.last_key_time = '', -math.inf
|
||||
|
||||
def action(self, action, now):
|
||||
self.request.requestId = max(self.request.requestId+1, int(now*1e9))
|
||||
self.request.action = action
|
||||
|
||||
def key(self, key, now):
|
||||
key = key.lower()
|
||||
repeated = key == self.last_key and now-self.last_key_time < .5
|
||||
self.last_key, self.last_key_time = key, now
|
||||
if key in ('a', 'd'):
|
||||
if not repeated:
|
||||
self.action('left' if key == 'a' else 'right', now)
|
||||
elif key in ('r', 'q'):
|
||||
self.action('cancel', now)
|
||||
else:
|
||||
self.request.action = 'idle'
|
||||
if key in ('1', '2'):
|
||||
self.request.channel = 'c0' if key == '1' else 'c1'
|
||||
elif key == 'v':
|
||||
self.request.speed = SPEEDS[1] if abs(self.request.speed-SPEEDS[0]) < 1e-5 else SPEEDS[0]
|
||||
elif key in ('+', '=', '-', '_'):
|
||||
increment = ACCEL_INCREMENT if key in ('+', '=') else -ACCEL_INCREMENT
|
||||
self.request.accel = min(MAX_ACCEL, max(MIN_ACCEL, self.request.accel+increment))
|
||||
return key != 'q'
|
||||
|
||||
def message(self):
|
||||
msg = messaging.new_message('testJoystick')
|
||||
msg.valid = True
|
||||
# Normal joystick consumers receive no gas/brake or steering-axis request.
|
||||
msg.testJoystick.axes = [0., 0.]
|
||||
msg.testJoystick.fordKeyboard = self.request
|
||||
return msg
|
||||
|
||||
|
||||
def enable(params):
|
||||
offroad = params.get_bool('IsOffroad')
|
||||
if not offroad and (not params.get_bool(KEYBOARD_PARAM) or any(params.get_bool(k) for k in (*CONFLICTS, PARAM))):
|
||||
raise ValueError('Start this tool while the car is off and openpilot is offroad.')
|
||||
if not params.get_bool('FordModelActionController'):
|
||||
raise ValueError('Enable the Ford model-action controller first.')
|
||||
raw = params.get('CarParamsPersistent')
|
||||
if not raw:
|
||||
raise ValueError('Drive once to identify the car before enabling this test.')
|
||||
cp = messaging.log_from_bytes(raw, car.CarParams)
|
||||
if cp.brand != 'ford' or not cp.flags & FordFlags.CANFD:
|
||||
raise ValueError('This test requires a CAN FD Ford.')
|
||||
if not offroad:
|
||||
return # Reconnect to an already-selected keyboard mode without changing any driving mode.
|
||||
for key in (*CONFLICTS, PARAM):
|
||||
params.put_bool(key, False, block=True)
|
||||
params.put_bool(KEYBOARD_PARAM, True, block=True)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--speed', type=int, choices=(15, 20), default=15, help='target mph (default: 15)')
|
||||
parser.add_argument('--accel', type=float, default=.5, help=f'step strength in m/s², {MIN_ACCEL}–{MAX_ACCEL} (default: 0.5)')
|
||||
args = parser.parse_args()
|
||||
params = Params()
|
||||
try:
|
||||
control = KeyboardControl(args.speed, args.accel)
|
||||
kb = KBHit()
|
||||
enable(params)
|
||||
except (ValueError, OSError) as exc:
|
||||
parser.exit(1, f'{exc}\n')
|
||||
pm = messaging.PubMaster(['testJoystick'])
|
||||
sm = messaging.SubMaster(['alertDebug', 'carState', 'carStateSP', 'carControlSP'])
|
||||
rk = Ratekeeper(20, print_delay_threshold=None)
|
||||
print(f'\nFord keyboard test connected. Use ACC or MADS with manual throttle.\n{HELP}\n', flush=True)
|
||||
print('Strength is a maneuver target, not a percentage of C0/C1. R cancels the test; normal lateral control remains active.', flush=True)
|
||||
running = True
|
||||
try:
|
||||
while running:
|
||||
# Bound input work so pasted text cannot stall the heartbeat/watchdog.
|
||||
for _ in range(32):
|
||||
if not kb.kbhit():
|
||||
break
|
||||
key = kb.getch()
|
||||
if not key:
|
||||
running = False
|
||||
break
|
||||
running = control.key(key, time.monotonic())
|
||||
if not running:
|
||||
break
|
||||
if not params.get_bool(KEYBOARD_PARAM):
|
||||
print('\nKeyboard test mode cleared; exiting.', flush=True)
|
||||
break
|
||||
pm.send('testJoystick', control.message())
|
||||
sm.update(0)
|
||||
if rk.frame % 10 == 0:
|
||||
req, cs = control.request, sm['carState']
|
||||
path, pscm = sm['carControlSP'].fordLateralPath, sm['carStateSP'].fordPscmStatus
|
||||
telemetry = (f'C0 {path.pathOffset:+.2f}m C1 {path.pathAngle:+.4f}rad | wheel {cs.steeringAngleDeg:+.1f}° | limit {pscm.limit}'
|
||||
if sm.all_alive() and sm.all_valid() else 'Waiting for live vehicle data')
|
||||
print(f'{str(req.channel).upper()} | {req.speed*CV.MS_TO_MPH:.0f}mph | {req.accel:.2f}m/s² | '
|
||||
+ f'{sm["alertDebug"].alertText1} | {telemetry}', flush=True)
|
||||
rk.keep_time()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
control.action('cancel', time.monotonic())
|
||||
pm.send('testJoystick', control.message())
|
||||
kb.set_normal_term()
|
||||
# Keep the daemon alive onroad so it can explicitly publish an inactive plan;
|
||||
# otherwise controlsd would only see the last, now-stale active plan.
|
||||
if params.get_bool('IsOffroad'):
|
||||
params.put_bool(KEYBOARD_PARAM, False, block=True)
|
||||
print('\nKeyboard disconnected; no further steps will start.', flush=True)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,54 +0,0 @@
|
||||
# Ford lateral maneuvers, one output channel at a time
|
||||
|
||||
Enable **Settings → Developer → Ford C0 / C1 test** on comma four while offroad. The Ford model-action controller must be enabled on a CAN FD Ford. This runs the existing lateral maneuver tool with a single output channel selected.
|
||||
|
||||
The suite uses the **normal lateral-acceleration targets**, converted to desired curvature by the normal maneuver tool. That request goes through the existing curvature/jerk limits and the normal Ford controller. **C0-only sends its normal C0 output with C1 zero. C1-only sends its normal C1 output, including P/I feedback, with C0 zero.** C2/C3 stay zero. The diagnostic does not replace the controller mapping, freeze a starting command, reset feedback, or inject a percentage of the field range.
|
||||
|
||||
At **15 mph**, it runs the standard step right, step left, 0.5 Hz sine and jitter through C0, then through C1. It repeats the same sequence at **20 mph**. Each maneuver retains the standard three runs: **48 completed runs total**.
|
||||
|
||||
The step includes the normal opposite-direction command; the sine and jitter keep the existing action arrays and timing. These tests therefore exercise turn-in and reversal through the real controller. Equal desired acceleration does not guarantee that either channel alone can achieve it. This measures the controller and vehicle together with one output selected, not the PSCM channel in isolation from controller feedback.
|
||||
|
||||
Use the same clear, straight test area as the regular lateral maneuver suite. Set ACC to the displayed speed and engage lateral control. The normal maneuver readiness/countdown checks apply. Driver input, disengagement or invalid inputs stop the attempted run. **Completed runs remain completed; the interrupted run retries when the standard readiness conditions recover.** After steering intervention alone, another disengagement is not required. Brake/ACC disengagement requires reengaging ACC and lateral control.
|
||||
|
||||
The former pulse-specific 15° wheel and 1 m/s² aborts are gone. They would cut off normal maneuvers. Normal controller limits, vehicle fault handling, field bounds, driver intervention and a stale-plan watchdog remain. PSCM limit-reached uses the normal controller behavior, including its integral anti-windup, rather than terminating the maneuver.
|
||||
|
||||
When “Maneuvers Finished” appears, finish the route and upload all logs. Going offroad or restarting clears the test toggle and resets suite progress.
|
||||
|
||||
Generate the report as usual:
|
||||
|
||||
```sh
|
||||
python openpilot/tools/lateral_maneuvers/generate_report.py DEVICE/ROUTE
|
||||
```
|
||||
|
||||
Channel runs use the standard lateral maneuver report, grouped by maneuver, speed and channel. It shows desired versus measured lateral acceleration, actual wheel angle, speed/jerk/roll, and decoded C0/C1 CAN commands. The report checks that the unused field remained zero. Archived raw-pulse routes still use their original report.
|
||||
|
||||
## Keyboard-triggered steps (ACC or MADS)
|
||||
|
||||
A laptop keyboard over SSH is enough. This mode keeps normal `controlsd` running, including ACC/MADS and the tested Ford formulas. **Do not enable stock Joystick Debug Mode:** that replaces `controlsd` and bypasses this controller. The keyboard tool selects its own transient mode and disables the automatic maneuver modes.
|
||||
|
||||
With the car off and openpilot offroad, SSH into the comma and run from `/data/openpilot`:
|
||||
|
||||
```sh
|
||||
python openpilot/tools/joystick/ford_keyboard_control.py --speed 15
|
||||
```
|
||||
|
||||
Leave that terminal connected, start the car, and engage lateral control. You may use **ACC**, or **MADS with manual accelerator input**. Hold the selected speed, 15 or 20 mph, within ±0.7 m/s (about ±1.6 mph). Braking, steering intervention, loss of lateral engagement, invalid data, or leaving the speed window aborts the current run. MADS is engaged normally; this tool does not enable it or override its engagement rules. Use a passenger to operate the keyboard while you drive in the test area.
|
||||
|
||||
| Key | Action |
|
||||
| --- | --- |
|
||||
| `1` / `2` | Select C0-only / C1-only for the next step |
|
||||
| `V` | Select 15 / 20 mph for the next step |
|
||||
| `+` / `-` | Adjust the next target by 0.25 m/s²; default 0.5, range 0.25–3.0 |
|
||||
| `A` / `D` | Trigger one left / right step when the screen says Ready |
|
||||
| `R` | Cancel the test and return to normal lateral control |
|
||||
| `Q` or Ctrl-C | Cancel and exit the keyboard tool |
|
||||
|
||||
The strength is a **lateral-acceleration target**, not a percentage of the C0/C1 field range. The upper setting matches the existing normal lateral-acceleration limit; normal curvature/jerk limits and field bounds still apply. For a different starting strength, add `--accel 1.0` to the command. Settings changed during a run apply only to the next run.
|
||||
|
||||
Each keypress starts one **0.5 s baseline → 1.0 s step → 1.5 s release** sequence. The reference is the captured starting curvature plus the requested acceleration divided by current speed squared. Release returns the **target** to that baseline; it does not forcibly zero C1's normal P/I correction. The unused channel and C2/C3 remain zero throughout the sequence. Afterward, normal model following resumes with both channels. The terminal displays computed C0/C1, measured wheel angle, and PSCM limit status; the route also records actual transmitted CAN commands.
|
||||
|
||||
The tool requires two seconds of stable, straight driving before accepting a trigger. A keypress while unready or busy is discarded, not queued. **It never automatically starts another run or retries an interrupted one.** Losing the keyboard heartbeat for more than 0.2 s aborts the run; the normal controller also retains its independent stale-plan/run-duration checks. `R` and `Q` cancel only the test, not MADS or ACC. Disengage lateral control normally if you want it off. The keyboard mode clears when the car goes offroad or manager restarts.
|
||||
|
||||
If SSH disconnects or you exit the keyboard program, reconnect and run the same command. Reconnecting onroad is accepted only when keyboard mode is already selected; it does not enable a new driving mode. An interrupted step stays aborted and needs a fresh trigger.
|
||||
|
||||
Use the same report command above after uploading the route. Keyboard runs add a PSCM limit-status plot and markers for the baseline/step/release transitions. These test the real controller with one output selected; C1 feedback and the vehicle response are both part of the measurement.
|
||||
@@ -5,8 +5,6 @@
|
||||
|
||||
Test your vehicle's lateral control tuning with this tool. The tool will test the vehicle's ability to follow a few lateral maneuvers and includes a tool to generate a report from the route.
|
||||
|
||||
Ford CAN FD with the model-action controller also has a separate [C0 / C1 comparison](FORD_CHANNEL_TEST.md) at 15 and 20 mph.
|
||||
|
||||
## Instructions
|
||||
|
||||
1. Check out a development branch such as `master` on your comma device.
|
||||
|
||||
@@ -1,133 +0,0 @@
|
||||
"""Manually triggered curvature steps through the normal Ford channel test path."""
|
||||
import math
|
||||
import time
|
||||
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL, Ratekeeper
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MAX_LATERAL_ACCEL_NO_ROLL, MIN_SPEED
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import SPEEDS, MAX_SPEED_ERROR
|
||||
from openpilot.tools.lateral_maneuvers.lateral_maneuversd import MAX_CURV, MAX_ROLL, TIMER
|
||||
|
||||
INPUT_TIMEOUT = .2
|
||||
ACCEL_INCREMENT = .25
|
||||
MIN_ACCEL, MAX_ACCEL = ACCEL_INCREMENT, MAX_LATERAL_ACCEL_NO_ROLL
|
||||
BASELINE_S, STEP_S, RELEASE_S = .5, 1., 1.5
|
||||
|
||||
|
||||
class KeyboardManeuver:
|
||||
def __init__(self):
|
||||
self.last_request_id = 0
|
||||
self.ready_since = self.start = self.last_update = None
|
||||
self.request_id = self.run_id = 0
|
||||
self.channel, self.speed, self.accel, self.baseline = 'none', SPEEDS[0], 0., 0.
|
||||
self.phase, self.status = 'waiting', 'Waiting for keyboard'
|
||||
|
||||
@property
|
||||
def active(self):
|
||||
return self.start is not None
|
||||
|
||||
def stop(self, status, phase='aborted'):
|
||||
self.start = self.ready_since = None
|
||||
self.phase, self.status = phase, status
|
||||
|
||||
def update(self, now, request, *, input_time, input_valid, healthy, ready, speed, curvature):
|
||||
# Consume every new request, including ones received while busy/unready/stale.
|
||||
# A rejected keypress must never become a delayed automatic start.
|
||||
new_request = request.requestId > self.last_request_id
|
||||
self.last_request_id = max(request.requestId, self.last_request_id)
|
||||
valid_input = (input_valid and math.isfinite(now) and math.isfinite(input_time)
|
||||
and -.005 <= now-input_time <= INPUT_TIMEOUT
|
||||
and request.channel in ('c0', 'c1') and request.action in ('idle', 'left', 'right', 'cancel')
|
||||
and math.isfinite(request.speed) and any(abs(request.speed-v) < 1e-5 for v in SPEEDS)
|
||||
and math.isfinite(request.accel) and MIN_ACCEL-1e-6 <= request.accel <= MAX_ACCEL+1e-6)
|
||||
continuous = self.last_update is None or 0. < now-self.last_update <= .15
|
||||
self.last_update = now
|
||||
if not valid_input or not healthy or not continuous or not all(math.isfinite(v) for v in (speed, curvature)):
|
||||
self.stop('Aborted: input/data lost' if self.active else 'Waiting for keyboard and active lateral control')
|
||||
return
|
||||
if new_request and request.action == 'cancel':
|
||||
self.stop('Aborted: keyboard cancel')
|
||||
return
|
||||
if self.active:
|
||||
if abs(speed-self.speed) > MAX_SPEED_ERROR:
|
||||
self.stop('Aborted: speed out of range')
|
||||
else:
|
||||
elapsed = now-self.start
|
||||
if elapsed >= BASELINE_S+STEP_S+RELEASE_S:
|
||||
self.stop('Complete', 'complete')
|
||||
else:
|
||||
self.phase = 'baseline' if elapsed < BASELINE_S else ('pulse' if elapsed < BASELINE_S+STEP_S else 'release')
|
||||
self.status = f'Active {self.channel.upper()} {self.phase}'
|
||||
return
|
||||
if not ready or abs(speed-request.speed) > MAX_SPEED_ERROR:
|
||||
self.ready_since = None
|
||||
self.status = f'Set {request.speed*CV.MS_TO_MPH:.0f} mph; straight and steady'
|
||||
else:
|
||||
if self.ready_since is None:
|
||||
self.ready_since = now
|
||||
self.status = 'Ready: A left / D right' if now-self.ready_since >= TIMER else 'Waiting: steady for 2 seconds'
|
||||
if new_request and request.action in ('left', 'right'):
|
||||
if self.ready_since is None or now-self.ready_since < TIMER:
|
||||
self.status = 'Not ready; press A/D again when ready'
|
||||
return
|
||||
self.start = now
|
||||
self.run_id += 1
|
||||
self.request_id = request.requestId
|
||||
self.channel, self.speed = str(request.channel), request.speed
|
||||
# Ford's desired-curvature coordinates are right-positive, like normal lat man.
|
||||
self.accel = request.accel * (-1 if request.action == 'left' else 1)
|
||||
self.baseline = curvature
|
||||
self.phase, self.status = 'baseline', f'Active {self.channel.upper()} baseline'
|
||||
|
||||
def plan(self, speed):
|
||||
msg = messaging.new_message('lateralManeuverPlan')
|
||||
msg.valid = self.active
|
||||
plan = msg.lateralManeuverPlan
|
||||
if self.active:
|
||||
plan.desiredCurvature = self.baseline + (self.accel if self.phase == 'pulse' else 0.) / max(speed, MIN_SPEED)**2
|
||||
plan.fordChannelTest = {'runId': self.run_id, 'channel': self.channel, 'speed': self.speed,
|
||||
'phase': 'maneuver' if self.active else self.phase, 'delta': 0.,
|
||||
'keyboardRequestId': self.request_id, 'keyboardPhase': self.phase, 'targetAccel': self.accel}
|
||||
return msg
|
||||
|
||||
@property
|
||||
def description(self):
|
||||
direction = 'right' if self.accel > 0 else 'left'
|
||||
return f'step {direction} {abs(self.accel):.2f}m/s² {self.speed*CV.MS_TO_MPH:.0f}mph {self.channel.upper()} keyboard'
|
||||
|
||||
|
||||
def main():
|
||||
services = ['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2', 'carStateSP', 'vehicleParameters']
|
||||
sm = messaging.SubMaster(services+['testJoystick'], frequency=1./DT_MDL)
|
||||
pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug'])
|
||||
test = KeyboardManeuver()
|
||||
rk = Ratekeeper(1./DT_MDL, print_delay_threshold=None)
|
||||
previous = None
|
||||
while True:
|
||||
sm.update(0)
|
||||
cs, cc, status = sm['carState'], sm['carControl'], sm['carStateSP'].fordPscmStatus
|
||||
now = time.monotonic()
|
||||
healthy = (sm.all_checks(services) and cs.canValid and cc.latActive
|
||||
and not (cs.steerFaultTemporary or cs.steerFaultPermanent or cs.steeringPressed or cs.brakePressed)
|
||||
and math.isfinite(cs.steeringTorque) and abs(cs.steeringTorque) <= 1.)
|
||||
curvature, roll = sm['controlsState'].desiredCurvature, sm['vehicleParameters'].roll
|
||||
ready = (abs(curvature) < MAX_CURV and abs(sm['controlsState'].curvature) < MAX_CURV and abs(roll) < MAX_ROLL
|
||||
and status.valid and -.005 <= now-status.canMonoTime*1e-9 <= .15 and status.lateralState == 2
|
||||
and not status.denied and status.limit != 3)
|
||||
test.update(now, sm['testJoystick'].fordKeyboard, input_time=sm.logMonoTime['testJoystick']*1e-9,
|
||||
input_valid=sm.valid['testJoystick'], healthy=healthy, ready=ready, speed=cs.vEgo, curvature=curvature)
|
||||
alert = messaging.new_message('alertDebug')
|
||||
alert.valid = True
|
||||
alert.alertDebug.alertText1 = test.status
|
||||
alert.alertDebug.alertText2 = test.description if test.run_id else 'Ford keyboard C0 / C1 test'
|
||||
pm.send('alertDebug', alert)
|
||||
pm.send('lateralManeuverPlan', test.plan(cs.vEgo))
|
||||
identity = (test.run_id, test.phase, test.status)
|
||||
if identity != previous:
|
||||
cloudlog.event('Ford keyboard test', run_id=test.run_id, request_id=test.request_id, phase=test.phase, status=test.status,
|
||||
channel=test.channel, target_accel=test.accel, speed=cs.vEgo, wheel_angle=cs.steeringAngleDeg,
|
||||
pscm_limit=status.limit)
|
||||
previous = identity
|
||||
rk.keep_time()
|
||||
@@ -1,17 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run the existing lateral maneuver suite through one Ford output at a time."""
|
||||
from openpilot.tools.lateral_maneuvers.lateral_maneuversd import main as suite_main
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import KEYBOARD_PARAM
|
||||
from openpilot.tools.lateral_maneuvers.ford_keyboard import main as keyboard_main
|
||||
|
||||
|
||||
def main():
|
||||
if Params().get_bool(KEYBOARD_PARAM):
|
||||
keyboard_main()
|
||||
else:
|
||||
suite_main(ford_channels=True)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,245 +0,0 @@
|
||||
"""Report raw Ford channel pulses without treating them as curvature targets."""
|
||||
import base64
|
||||
import io
|
||||
from dataclasses import dataclass, field
|
||||
from html import escape
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from opendbc.can import CANParser
|
||||
from opendbc.car.ford.fordcan import CanBus, calculate_lat_ctl2_checksum
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import is_channel_plan
|
||||
|
||||
SERVICES = {'lateralManeuverPlan', 'carControl', 'carState', 'carStateSP', 'sendcan'}
|
||||
|
||||
|
||||
@dataclass
|
||||
class Run:
|
||||
run_id: int
|
||||
channel: str
|
||||
speed: float
|
||||
messages: list = field(default_factory=list)
|
||||
outcome: str = 'incomplete'
|
||||
|
||||
|
||||
class ChannelRuns:
|
||||
def __init__(self):
|
||||
self.runs = []
|
||||
self.current = None
|
||||
|
||||
def add(self, msg):
|
||||
kind = msg.which()
|
||||
if kind == 'lateralManeuverPlan' and is_channel_plan(msg.lateralManeuverPlan):
|
||||
test = msg.lateralManeuverPlan.fordChannelTest
|
||||
active = msg.valid and str(test.phase) in ('baseline', 'pulse', 'release')
|
||||
if active and (self.current is None or self.current.run_id != test.runId):
|
||||
self.current = Run(test.runId, str(test.channel), test.speed)
|
||||
self.runs.append(self.current)
|
||||
if not active and self.current is not None:
|
||||
self.current.outcome = str(test.phase)
|
||||
self.current.messages.append(msg)
|
||||
self.current = None
|
||||
if self.current is not None and kind in SERVICES:
|
||||
self.current.messages.append(msg)
|
||||
|
||||
|
||||
def first_sustained(t, mask, duration=.05):
|
||||
start = None
|
||||
previous = None
|
||||
for stamp, passed in zip(t, mask, strict=True):
|
||||
if not passed or (previous is not None and stamp-previous > .05):
|
||||
start = None
|
||||
if passed:
|
||||
if start is None:
|
||||
start = stamp
|
||||
if stamp-start >= duration:
|
||||
return float(start)
|
||||
previous = stamp
|
||||
return None
|
||||
|
||||
|
||||
def analyze(run, CP):
|
||||
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], CanBus(CP).main)
|
||||
address = parser.dbc.name_to_msg['LateralMotionControl2'].address
|
||||
plans, wheels, commands = [], [], []
|
||||
problems = set()
|
||||
if run.outcome != 'complete':
|
||||
problems.add(run.outcome)
|
||||
for msg in sorted(run.messages, key=lambda m: m.logMonoTime):
|
||||
t, kind = msg.logMonoTime*1e-9, msg.which()
|
||||
if kind != 'lateralManeuverPlan' and not msg.valid:
|
||||
problems.add(f'invalid {kind}')
|
||||
if kind == 'lateralManeuverPlan':
|
||||
p = msg.lateralManeuverPlan.fordChannelTest
|
||||
plans.append((t, str(p.phase), p.delta))
|
||||
elif kind == 'carState':
|
||||
cs = msg.carState
|
||||
wheels.append((t, cs.steeringAngleDeg, cs.vEgo*CV.MS_TO_MPH))
|
||||
if cs.steeringPressed or abs(cs.steeringTorque) > 1. or cs.gasPressed or cs.brakePressed:
|
||||
problems.add('driver input')
|
||||
if not cs.canValid or cs.steerFaultTemporary or cs.steerFaultPermanent or not cs.cruiseState.enabled:
|
||||
problems.add('vehicle fault')
|
||||
if abs(cs.vEgo-run.speed) > .7:
|
||||
problems.add('speed out of range')
|
||||
elif kind == 'carControl' and not msg.carControl.latActive:
|
||||
problems.add('lateral inactive')
|
||||
elif kind == 'carStateSP':
|
||||
p = msg.carStateSP.fordPscmStatus
|
||||
if not p.valid or not -.005 <= t-p.canMonoTime*1e-9 <= .15 or p.denied or p.limit in (2, 3) or p.lateralState != 2:
|
||||
problems.add('PSCM invalid, denied or limited')
|
||||
elif kind == 'sendcan':
|
||||
for packet in msg.sendcan:
|
||||
if packet.address == address and packet.src == CanBus(CP).main:
|
||||
parser.update([msg.logMonoTime, [(packet.address, packet.dat, packet.src)]])
|
||||
wire = parser.vl['LateralMotionControl2']
|
||||
commands.append((t, -wire['LatCtlPathOffst_L_Actl'], -wire['LatCtlPath_An_Actl']))
|
||||
if wire['LatCtl_D2_Rq'] != 2 or wire['LatCtlCurv_No_Actl'] != 0. or wire['LatCtlCrv_NoRate2_Actl'] != 0.:
|
||||
problems.add('unexpected CAN mode or C2/C3')
|
||||
if wire['LatCtlPath_No_Cs'] != calculate_lat_ctl2_checksum(int(wire['LatCtl_D2_Rq']), int(wire['LatCtlPath_No_Cnt']), packet.dat):
|
||||
problems.add('CAN checksum')
|
||||
seen = {m.which() for m in run.messages}
|
||||
if missing := SERVICES-seen:
|
||||
problems.add('missing '+', '.join(sorted(missing)))
|
||||
pulse = next((t for t, phase, _ in plans if phase == 'pulse'), None)
|
||||
release = next((t for t, phase, _ in plans if phase == 'release'), None)
|
||||
if not plans or plans[0][1] != 'baseline' or pulse is None or release is None:
|
||||
problems.add('missing test phases')
|
||||
data = {'problems': problems, 'plans': plans, 'wheels': np.array(wheels), 'commands': np.array(commands), 'pulse': pulse, 'release': release,
|
||||
'onset': None, 'peak': None, 'residual': None, 'wire_on': None, 'wire_off': None, 'threshold': None, 'delta': None}
|
||||
if len(wheels) < 10 or len(commands) < 10:
|
||||
problems.add('insufficient wheel or CAN samples')
|
||||
if pulse is None or release is None or len(wheels) < 10 or len(commands) < 10:
|
||||
return data
|
||||
w, c = data['wheels'], data['commands']
|
||||
if not np.isfinite(w).all() or not np.isfinite(c).all():
|
||||
problems.add('nonfinite data')
|
||||
return data
|
||||
for name, samples in (('wheel', w), ('CAN', c)):
|
||||
if np.max(np.diff(samples[:, 0])) > .1:
|
||||
problems.add(f'{name} data gap')
|
||||
bw, bc = w[(w[:, 0] >= pulse-.2) & (w[:, 0] < pulse)], c[(c[:, 0] >= pulse-.2) & (c[:, 0] < pulse)]
|
||||
if len(bw) < 3 or len(bc) < 3:
|
||||
problems.add('insufficient baseline')
|
||||
return data
|
||||
base_angle, base_command = np.median(bw[:, 1]), np.median(bc[:, 1:], axis=0)
|
||||
selected = 1 if run.channel == 'c0' else 2
|
||||
other = 2 if selected == 1 else 1
|
||||
held = c[c[:, 0] >= plans[0][0]+.06] # allow the initial plan to reach controlsd and the sender
|
||||
if not len(held) or np.max(np.abs(held[:, other]-base_command[other-1])) > (1e-5 if other == 2 else 1e-4):
|
||||
problems.add('other field changed')
|
||||
delta = next(d for _, p, d in plans if p == 'pulse')
|
||||
tolerance = .00026 if selected == 2 else .0051
|
||||
pulse_samples = c[(c[:, 0] >= pulse) & (c[:, 0] < release)]
|
||||
matching = pulse_samples[np.abs(pulse_samples[:, selected]-base_command[selected-1]-delta) < tolerance]
|
||||
if not len(matching):
|
||||
problems.add('pulse absent from CAN')
|
||||
return data
|
||||
on = matching[0, 0]
|
||||
release_samples = c[c[:, 0] >= release]
|
||||
returned = release_samples[np.abs(release_samples[:, selected]-base_command[selected-1]) < tolerance]
|
||||
off = returned[0, 0] if len(returned) else None
|
||||
if off is None:
|
||||
problems.add('release absent from CAN')
|
||||
# Ignore the one-cycle boundary uncertainty; verify the rest of each phase.
|
||||
for start, end, expected in ((pulse+.06, release, base_command[selected-1]+delta),
|
||||
(release+.06, c[-1, 0]+.001, base_command[selected-1])):
|
||||
samples = c[(c[:, 0] >= start) & (c[:, 0] < end)]
|
||||
if not len(samples) or np.max(np.abs(samples[:, selected]-expected)) > tolerance:
|
||||
problems.add('selected field not held')
|
||||
response = w[w[:, 0] >= on]
|
||||
change = response[:, 1]-base_angle
|
||||
threshold = max(.5, 3.*float(np.ptp(bw[:, 1])))
|
||||
onset = first_sustained(response[:, 0], np.abs(change) >= threshold)
|
||||
tail = response[response[:, 0] >= response[-1, 0]-.2]
|
||||
data.update(onset=None if onset is None else onset-on, peak=float(np.max(np.abs(change))),
|
||||
residual=float(np.mean(tail[:, 1])-base_angle), wire_on=on, wire_off=off, threshold=threshold, delta=delta,
|
||||
base_angle=base_angle)
|
||||
return data
|
||||
|
||||
|
||||
def report(platform, route, CP, ID, runs, output_dir=None):
|
||||
output_dir = Path(output_dir) if output_dir else Path(__file__).parent/'lateral_reports'
|
||||
output_dir.mkdir(parents=True, exist_ok=True)
|
||||
output = output_dir/f'{platform}_{route.replace("/", "_").replace("|", "_")}_ford_channels.html'
|
||||
html = ['<!doctype html><meta charset="utf-8"><title>Ford C0 / C1 test</title>',
|
||||
'<style>body{font:17px system-ui;max-width:1050px;margin:40px auto;padding:0 24px;color:#18232e}img{width:100%}.bad{color:#a21c24}</style>',
|
||||
'<h1>Ford C0 / C1 test</h1>', f'<p>{escape(platform)} · {escape(route)}<br>Commit {escape(ID.gitCommit)}</p>',
|
||||
'<p>Each pulse holds the other field constant. All plots use left-positive coordinates. '
|
||||
+ 'C0 and C1 pulse sizes are not assumed to produce equal steering. These measurements describe these trials; '
|
||||
+ 'they do not establish a universal PSCM delay or predict intersection turns.</p>',
|
||||
'<p>Onset means wheel movement from baseline exceeding max(0.5°, 3 × baseline range) for 50 ms, '
|
||||
+ 'timed from the first changed CAN command. Peak covers the pulse and release. Residual is the last 200 ms '
|
||||
+ 'of wheel angle minus baseline. A larger response can cross the onset threshold earlier without having less delay.</p>']
|
||||
for index, run in enumerate(runs, 1):
|
||||
d = analyze(run, CP)
|
||||
direction = ('left' if d['delta'] > 0 else 'right') if d['delta'] is not None else 'unknown direction'
|
||||
title = f'{index}. {run.channel.upper()} {direction} · {run.speed*CV.MS_TO_MPH:.0f} mph'
|
||||
html.append(f'<h2>{escape(title)}</h2>')
|
||||
if d['problems']:
|
||||
html.append(f'<p class="bad">Exclude from comparison: {escape(", ".join(sorted(d["problems"])))}</p>')
|
||||
else:
|
||||
html.append('<p>Complete; command isolation and logged validity checks passed.</p>')
|
||||
if d['peak'] is not None:
|
||||
onset = f'{d["onset"]:.3f} s' if d['onset'] is not None else 'not detected'
|
||||
html.append(f'<p>Onset: <b>{onset}</b> (threshold {d["threshold"]:.2f}°) · '
|
||||
+ f'Peak movement: <b>{d["peak"]:.2f}°</b> · End residual: <b>{d["residual"]:+.2f}°</b></p>')
|
||||
if not len(d['wheels']) or not len(d['commands']):
|
||||
html.append('<p>Insufficient wheel or CAN data to plot.</p>')
|
||||
continue
|
||||
origin = d['wire_on'] if d['wire_on'] is not None else d['commands'][0, 0]
|
||||
fig, axes = plt.subplots(4, 1, figsize=(11, 9), sharex=True, layout='constrained')
|
||||
c, w = d['commands'], d['wheels']
|
||||
axes[0].step(c[:, 0]-origin, c[:, 1], where='post', label='C0 sent', color='#007ea7')
|
||||
axes[1].step(c[:, 0]-origin, c[:, 2], where='post', label='C1 sent', color='#ba5d07')
|
||||
axes[2].plot(w[:, 0]-origin, w[:, 1]-d.get('base_angle', w[0, 1]), label='Wheel movement', color='#7d3fa2')
|
||||
axes[3].plot(w[:, 0]-origin, w[:, 2], label='Speed', color='#007453')
|
||||
for ax, label in zip(axes, ('C0 (m)', 'C1 (rad)', 'Wheel Δ (°)', 'Speed (mph)'), strict=True):
|
||||
ax.set_ylabel(label)
|
||||
ax.grid(alpha=.2)
|
||||
ax.legend(loc='upper right')
|
||||
if d['wire_on'] is not None:
|
||||
ax.axvline(0., color='#444', linestyle='--')
|
||||
if d['wire_off'] is not None:
|
||||
ax.axvline(d['wire_off']-origin, color='#444', linestyle=':')
|
||||
axes[-1].set_xlabel('Seconds from CAN pulse start · dotted line = CAN return to baseline')
|
||||
buf = io.BytesIO()
|
||||
fig.savefig(buf, format='png', dpi=120)
|
||||
plt.close(fig)
|
||||
html.append(f'<img alt="{escape(title)} command and wheel response" src="data:image/png;base64,{base64.b64encode(buf.getvalue()).decode()}">')
|
||||
output.write_text(''.join(html))
|
||||
return output
|
||||
|
||||
|
||||
def channel_commands(msgs, CP, channel, start_time):
|
||||
"""Decode the actual output for normal maneuver reports, in left-positive units."""
|
||||
bus = CanBus(CP).main
|
||||
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], bus)
|
||||
address = parser.dbc.name_to_msg['LateralMotionControl2'].address
|
||||
samples, problems = [], set()
|
||||
for msg in msgs:
|
||||
if msg.which() != 'sendcan':
|
||||
continue
|
||||
for packet in msg.sendcan:
|
||||
if packet.address != address or packet.src != bus:
|
||||
continue
|
||||
parser.update([msg.logMonoTime, [(packet.address, packet.dat, packet.src)]])
|
||||
wire = parser.vl['LateralMotionControl2']
|
||||
t = (msg.logMonoTime-start_time)*1e-9
|
||||
samples.append((t, -wire['LatCtlPathOffst_L_Actl'], -wire['LatCtlPath_An_Actl']))
|
||||
if t >= .06: # initial plan delivery to the controller/sender
|
||||
if not msg.valid or wire['LatCtl_D2_Rq'] != 2:
|
||||
problems.add('inactive/invalid CAN output')
|
||||
other = wire['LatCtlPath_An_Actl'] if channel == 'c0' else wire['LatCtlPathOffst_L_Actl']
|
||||
if abs(other) > 1e-7 or wire['LatCtlCurv_No_Actl'] != 0. or wire['LatCtlCrv_NoRate2_Actl'] != 0.:
|
||||
problems.add('channel isolation failed')
|
||||
if wire['LatCtlPath_No_Cs'] != calculate_lat_ctl2_checksum(int(wire['LatCtl_D2_Rq']), int(wire['LatCtlPath_No_Cnt']), packet.dat):
|
||||
problems.add('CAN checksum')
|
||||
data = np.asarray(samples).reshape(-1, 3)
|
||||
if len(data) < 2:
|
||||
problems.add('missing CAN output')
|
||||
elif np.max(np.diff(data[:, 0])) > .1:
|
||||
problems.add('CAN data gap')
|
||||
return data, problems
|
||||
@@ -14,12 +14,10 @@ from openpilot.common.utils import tabulate
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LP_FILTER_CUTOFF_HZ
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import is_channel_maneuver
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.tools.longitudinal_maneuvers.generate_report import format_car_params
|
||||
from openpilot.tools.lateral_maneuvers.ford_report import ChannelRuns, channel_commands, report as ford_channel_report
|
||||
|
||||
|
||||
def lat_accel(curvature, v):
|
||||
@@ -60,47 +58,33 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
t_controlsState, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True)
|
||||
t_lateralPlan, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid], strict=True)
|
||||
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
|
||||
channel = str(lateralPlan[0].fordChannelTest.channel) if is_channel_maneuver(lateralPlan[0]) else None
|
||||
keyboard = bool(channel and lateralPlan[0].fordChannelTest.keyboardRequestId)
|
||||
origin = t_lateralPlan[0]
|
||||
commands, command_problems = channel_commands(msgs, CP, channel, origin) if channel else (None, set())
|
||||
|
||||
# make time relative seconds
|
||||
t_carControl = [(t - (origin if channel else t_carControl[0])) / 1e9 for t in t_carControl]
|
||||
t_carState = [(t - (origin if channel else t_carState[0])) / 1e9 for t in t_carState]
|
||||
t_controlsState = [(t - (origin if channel else t_controlsState[0])) / 1e9 for t in t_controlsState]
|
||||
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
|
||||
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
|
||||
t_controlsState = [(t - t_controlsState[0]) / 1e9 for t in t_controlsState]
|
||||
t_lateralPlan = [(t - t_lateralPlan[0]) / 1e9 for t in t_lateralPlan]
|
||||
t_carOutput = [(t - (origin if channel else t_carOutput[0])) / 1e9 for t in t_carOutput]
|
||||
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
|
||||
|
||||
# maneuver validity
|
||||
latActive = [m.latActive for m in carControl]
|
||||
maneuver_valid = all(latActive) and not any(cs.steeringPressed for cs in carState) and not command_problems
|
||||
maneuver_valid = all(latActive) and not any(cs.steeringPressed for cs in carState)
|
||||
|
||||
_open = 'open' if maneuver_valid else ''
|
||||
title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')
|
||||
|
||||
builder.append(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
|
||||
if channel:
|
||||
builder.append(f'<p>Normal maneuver target through {channel.upper()} only; normal controller feedback remains active.</p>')
|
||||
if keyboard:
|
||||
builder.append('<p>Keyboard-triggered step: baseline, one-second target, then return to the baseline target. '
|
||||
+ 'C1 feedback can remain nonzero during release. Accelerator input is allowed with MADS. '
|
||||
+ 'The PSCM limit plot records limitReached=2; this does not by itself abort the step.</p>')
|
||||
if command_problems:
|
||||
builder.append(f'<p>CAN validation: {", ".join(sorted(command_problems))}</p>')
|
||||
|
||||
baseline_accel = lat_accel(controlsState[0].curvature, carState[0].vEgo)
|
||||
v_ego = [m.vEgo for m in carState]
|
||||
v_plan = np.interp(t_lateralPlan, t_carState, v_ego) if channel else v_ego
|
||||
v_controls = np.interp(t_controlsState, t_carState, v_ego) if channel else v_ego
|
||||
cross_markers = []
|
||||
|
||||
if description.startswith(('sine', 'jitter')):
|
||||
amplitude = max(abs(lat_accel(lp.desiredCurvature, v) - baseline_accel)
|
||||
for lp, v in zip(lateralPlan, v_plan, strict=False))
|
||||
for lp, v in zip(lateralPlan, v_ego, strict=False))
|
||||
threshold = amplitude * 0.5
|
||||
builder.append('<h3 style="font-weight: normal">50% peak')
|
||||
for t, cs, v in zip(t_controlsState, controlsState, v_controls, strict=False):
|
||||
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
|
||||
actual = lat_accel(cs.curvature, v) - baseline_accel
|
||||
if abs(actual) > threshold:
|
||||
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
|
||||
@@ -114,10 +98,10 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
if maneuver_valid:
|
||||
target_cross_times.setdefault(description, [])
|
||||
else:
|
||||
action_targets = [(0, lat_accel(lateralPlan[0].desiredCurvature, v_plan[0]) - baseline_accel)]
|
||||
for i in range(1, min(len(lateralPlan), len(v_plan))):
|
||||
action_targets = [(0, lat_accel(lateralPlan[0].desiredCurvature, v_ego[0]) - baseline_accel)]
|
||||
for i in range(1, min(len(lateralPlan), len(v_ego))):
|
||||
if abs(lateralPlan[i].desiredCurvature - lateralPlan[i - 1].desiredCurvature) > 0.001:
|
||||
desired = lat_accel(lateralPlan[i].desiredCurvature, v_plan[i]) - baseline_accel
|
||||
desired = lat_accel(lateralPlan[i].desiredCurvature, v_ego[i]) - baseline_accel
|
||||
action_targets.append((i, desired))
|
||||
|
||||
for j, (start_i, act_target) in enumerate(action_targets):
|
||||
@@ -126,7 +110,7 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
|
||||
builder.append(f'<h3 style="font-weight: normal">aTarget: {round(act_target, 1)} m/s^2')
|
||||
prev_crossed = False
|
||||
for t, cs, v in zip(t_controlsState, controlsState, v_controls, strict=False):
|
||||
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
|
||||
if not (start_time <= t <= end_time):
|
||||
continue
|
||||
actual_accel = lat_accel(cs.curvature, v) - baseline_accel
|
||||
@@ -146,20 +130,19 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
target_cross_times.setdefault(description, [])
|
||||
|
||||
plt.rcParams['font.size'] = 40
|
||||
fig = plt.figure(figsize=(30, 55 if keyboard else (50 if channel else 40)))
|
||||
ratios = [5, 5, 3, 3, 3] + ([3, 3] if channel else []) + ([2] if keyboard else [])
|
||||
ax = fig.subplots(len(ratios), 1, sharex=True, gridspec_kw={'height_ratios': ratios})
|
||||
fig = plt.figure(figsize=(30, 40))
|
||||
ax = fig.subplots(5, 1, sharex=True, gridspec_kw={'height_ratios': [5, 5, 3, 3, 3]})
|
||||
|
||||
ax[0].grid(linewidth=4)
|
||||
desired_label = 'lateralManeuverPlan.desiredCurvature * vEgo^2'
|
||||
desired_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_plan, strict=False)]
|
||||
desired_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_ego, strict=False)]
|
||||
if description.startswith(('sine', 'jitter')):
|
||||
ax[0].plot(t_lateralPlan[:len(desired_lat_accel)], desired_lat_accel, 'C1', label=desired_label, linewidth=6)
|
||||
else:
|
||||
t_desired = [t_lateralPlan[0]] + t_lateralPlan[:len(desired_lat_accel)]
|
||||
desired_lat_accel = [baseline_accel] + desired_lat_accel
|
||||
ax[0].step(t_desired, desired_lat_accel, 'C1', label=desired_label, linewidth=6, where='post')
|
||||
actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controlsState, v_controls, strict=False)]
|
||||
actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controlsState, v_ego, strict=False)]
|
||||
ax[0].plot(t_controlsState[:len(actual_lat_accel)], actual_lat_accel, 'g', label='controlsState.curvature * vEgo^2', linewidth=6)
|
||||
ax[0].set_ylabel('Lateral Accel (m/s^2)')
|
||||
for ct, cv in cross_markers:
|
||||
@@ -177,35 +160,6 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
ax[1].plot(t_carOutput, [getattr(m.actuatorsOutput, steer_field) for m in carOutput], 'g', label=f'carOutput.actuatorsOutput.{steer_field}', linewidth=6)
|
||||
ax[1].set_ylabel(steer_ylabel)
|
||||
ax[1].legend(prop={'size': 30})
|
||||
if channel:
|
||||
ax[1].clear()
|
||||
ax[1].grid(linewidth=4)
|
||||
ax[1].plot(t_carState, [cs.steeringAngleDeg for cs in carState], 'g', label='Actual wheel angle', linewidth=6)
|
||||
ax[1].set_ylabel('Wheel angle (deg)')
|
||||
ax[1].legend(prop={'size': 30})
|
||||
for idx, label in ((1, 'C0 sent (m)'), (2, 'C1 sent (rad)')):
|
||||
ax[4+idx].step(commands[:, 0], commands[:, idx], where='post', linewidth=6, label=label)
|
||||
ax[4+idx].set_ylabel(label)
|
||||
ax[4+idx].grid(linewidth=4)
|
||||
ax[4+idx].legend(prop={'size': 30})
|
||||
if keyboard:
|
||||
pscm = [(float((m.logMonoTime-origin)*1e-9), m.carStateSP.fordPscmStatus.limit)
|
||||
for m in msgs if m.which() == 'carStateSP' and m.valid and m.carStateSP.fordPscmStatus.valid]
|
||||
if pscm:
|
||||
times, limits = zip(*pscm, strict=True)
|
||||
ax[7].step(times, limits, where='post', linewidth=6, label='PSCM limit status')
|
||||
else:
|
||||
ax[7].text(.05, .5, 'No valid PSCM status logged', transform=ax[7].transAxes)
|
||||
ax[7].set_yticks([0, 1, 2, 3])
|
||||
ax[7].set_ylabel('PSCM limit\n2 = reached')
|
||||
ax[7].grid(linewidth=4)
|
||||
previous_phase = None
|
||||
for t, plan in zip(t_lateralPlan, lateralPlan, strict=True):
|
||||
phase = str(plan.fordChannelTest.keyboardPhase)
|
||||
if phase != previous_phase:
|
||||
for axis in ax:
|
||||
axis.axvline(t, color='#777777', linestyle='--', linewidth=2)
|
||||
previous_phase = phase
|
||||
|
||||
ax[2].grid(linewidth=4)
|
||||
ax[2].plot(t_carState, [v * CV.MS_TO_MPH for v in v_ego], label='carState.vEgo', linewidth=6)
|
||||
@@ -279,10 +233,8 @@ if __name__ == '__main__':
|
||||
maneuvers: list[tuple[str, list[list]]] = []
|
||||
active_prev = False
|
||||
description_prev = None
|
||||
channel_runs = ChannelRuns()
|
||||
|
||||
for msg in lr:
|
||||
channel_runs.add(msg)
|
||||
if msg.which() == 'alertDebug':
|
||||
active = 'Active' in msg.alertDebug.alertText1 or msg.alertDebug.alertText1 == 'Complete'
|
||||
if active and not active_prev:
|
||||
@@ -296,9 +248,4 @@ if __name__ == '__main__':
|
||||
if active_prev:
|
||||
maneuvers[-1][1][-1].append(msg)
|
||||
|
||||
if channel_runs.runs:
|
||||
output = ford_channel_report(platform, args.route, CP, ID, channel_runs.runs)
|
||||
print(f'Opening Ford channel report: {output}')
|
||||
webbrowser.open_new_tab(str(output))
|
||||
else:
|
||||
report(platform, args.route, args.description, CP, ID, maneuvers)
|
||||
report(platform, args.route, args.description, CP, ID, maneuvers)
|
||||
|
||||
@@ -9,7 +9,6 @@ from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import SPEEDS
|
||||
from openpilot.tools.longitudinal_maneuvers.maneuversd import Action, Maneuver as _Maneuver
|
||||
|
||||
# thresholds for starting maneuvers
|
||||
@@ -21,7 +20,6 @@ TIMER = 2.0 # sec stable conditions before starting maneuver
|
||||
@dataclass
|
||||
class Maneuver(_Maneuver):
|
||||
_baseline_curvature: float = 0.0
|
||||
channel: str = 'none'
|
||||
|
||||
def get_accel(self, v_ego: float, lat_active: bool, curvature: float, roll: float) -> float:
|
||||
self._run_completed = False
|
||||
@@ -102,34 +100,21 @@ MANEUVERS = [
|
||||
]
|
||||
|
||||
|
||||
def channel_maneuvers():
|
||||
# Reuse the normal action arrays, timing, repeat count and readiness logic.
|
||||
templates = [m for m in MANEUVERS if m.initial_speed == MANEUVERS[0].initial_speed]
|
||||
return [Maneuver(f'{m.description.rsplit(" ", 1)[0]} {speed*CV.MS_TO_MPH:.0f}mph {channel.upper()}',
|
||||
m.actions, repeat=m.repeat, initial_speed=speed, channel=channel)
|
||||
for speed in SPEEDS for channel in ('c0', 'c1') for m in templates]
|
||||
|
||||
|
||||
def main(ford_channels=False):
|
||||
def main():
|
||||
params = Params()
|
||||
cloudlog.info("lateral_maneuversd is waiting for CarParams")
|
||||
messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
|
||||
services = ['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2']
|
||||
if ford_channels:
|
||||
services += ['carStateSP', 'vehicleParameters']
|
||||
sm = messaging.SubMaster(services, poll='modelV2')
|
||||
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2')
|
||||
pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug'])
|
||||
|
||||
maneuvers = iter(channel_maneuvers() if ford_channels else MANEUVERS)
|
||||
maneuvers = iter(MANEUVERS)
|
||||
maneuver = None
|
||||
complete_cnt = 0
|
||||
aborted_cnt = 0
|
||||
abort_reason = ''
|
||||
display_holdoff = 0
|
||||
prev_text = ''
|
||||
run_id = 0
|
||||
was_active = False
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
@@ -153,19 +138,16 @@ def main(ford_channels=False):
|
||||
elif maneuver is not None:
|
||||
# any driver input aborts the maneuver
|
||||
CS = sm['carState']
|
||||
invalid = ford_channels and (not sm.all_checks() or not CS.canValid or not CS.cruiseState.enabled
|
||||
or not sm['carControl'].latActive or CS.steerFaultTemporary or CS.steerFaultPermanent)
|
||||
if CS.steeringPressed or CS.gasPressed or (ford_channels and CS.brakePressed) or invalid:
|
||||
if CS.steeringPressed or CS.gasPressed:
|
||||
aborted_cnt = int(1.0 / DT_MDL)
|
||||
abort_reason = ('Waiting: engage ACC/lateral; valid data' if invalid else
|
||||
('steering pressed' if CS.steeringPressed else ('brake pressed' if CS.brakePressed else 'gas pressed'))).ljust(20)
|
||||
abort_reason = ('steering pressed' if CS.steeringPressed else 'gas pressed').ljust(20)
|
||||
aborted = aborted_cnt > 0
|
||||
speed_out_of_range = maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV
|
||||
if aborted or speed_out_of_range:
|
||||
maneuver.reset()
|
||||
|
||||
roll = sm['carControl'].orientationNED[0] if len(sm['carControl'].orientationNED) == 3 else 0.0
|
||||
accel = maneuver.get_accel(v_ego, sm['carControl'].latActive and not (ford_channels and (invalid or aborted)), curvature, roll)
|
||||
accel = maneuver.get_accel(v_ego, sm['carControl'].latActive, curvature, roll)
|
||||
|
||||
if maneuver._run_completed:
|
||||
complete_cnt = int(1.0 / DT_MDL)
|
||||
@@ -209,18 +191,9 @@ def main(ford_channels=False):
|
||||
|
||||
pm.send('alertDebug', alert_msg)
|
||||
|
||||
plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0 and (not ford_channels or not maneuver.finished)
|
||||
plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0
|
||||
if plan_send.valid:
|
||||
plan_send.lateralManeuverPlan.desiredCurvature = maneuver._baseline_curvature + accel / max(v_ego, MIN_SPEED) ** 2
|
||||
if ford_channels:
|
||||
if plan_send.valid and not was_active:
|
||||
run_id += 1
|
||||
test = plan_send.lateralManeuverPlan.fordChannelTest
|
||||
test.runId = run_id
|
||||
test.channel = maneuver.channel if maneuver is not None else 'none'
|
||||
test.speed = maneuver.initial_speed if maneuver is not None else 0.
|
||||
test.phase = 'maneuver' if plan_send.valid else ('complete' if complete_cnt > 0 else 'waiting')
|
||||
was_active = plan_send.valid
|
||||
pm.send('lateralManeuverPlan', plan_send)
|
||||
|
||||
if maneuver is not None and maneuver.finished and complete_cnt == 0:
|
||||
|
||||
@@ -1,237 +0,0 @@
|
||||
"""Offline keyboard, maneuver, and real controller/CAN checks; no hardware actuation."""
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.cereal import log, messaging
|
||||
from openpilot.common.params import Params, ParamKeyFlag
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import KEYBOARD_PARAM, PARAM, SPEEDS, FordChannelTest
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
|
||||
from openpilot.tools.joystick.ford_keyboard_control import KeyboardControl, enable
|
||||
from openpilot.tools.lateral_maneuvers import ford_keyboard as daemon
|
||||
from openpilot.tools.lateral_maneuvers.ford_keyboard import KeyboardManeuver, BASELINE_S, STEP_S, RELEASE_S
|
||||
|
||||
|
||||
def request(**kw):
|
||||
return log.Joystick.FordKeyboard.new_message(**({'requestId': 0, 'channel': 'c0', 'speed': SPEEDS[0],
|
||||
'accel': .5, 'action': 'idle'} | kw))
|
||||
|
||||
|
||||
def step(test, t, req, **kw):
|
||||
test.update(t, req, **({'input_time': t, 'input_valid': True, 'healthy': True, 'ready': True,
|
||||
'speed': req.speed, 'curvature': .001} | kw))
|
||||
return test.plan(kw.get('speed', req.speed))
|
||||
|
||||
|
||||
def ready(test, req, start=10.):
|
||||
for i in range(42):
|
||||
step(test, start+i*.05, req)
|
||||
assert test.status == 'Ready: A left / D right'
|
||||
return start+2.10
|
||||
|
||||
|
||||
@pytest.mark.parametrize('channel', ['c0', 'c1'])
|
||||
@pytest.mark.parametrize('speed', SPEEDS)
|
||||
@pytest.mark.parametrize('direction,sign', [('left', -1), ('right', 1)])
|
||||
@pytest.mark.parametrize('amplitude', [.25, .5, 3.])
|
||||
def test_timed_step_uses_normal_curvature_then_returns_to_baseline(channel, speed, direction, sign, amplitude):
|
||||
test = KeyboardManeuver()
|
||||
req = request(channel=channel, speed=speed, accel=amplitude)
|
||||
start = ready(test, req)
|
||||
req.requestId, req.action = 1, direction
|
||||
plans = [step(test, start+i*.05, req) for i in range(61)]
|
||||
assert plans[0].valid and not plans[-1].valid
|
||||
assert plans[-1].lateralManeuverPlan.fordChannelTest.phase == 'complete'
|
||||
phases = [str(p.lateralManeuverPlan.fordChannelTest.keyboardPhase) for p in plans[:-1]]
|
||||
assert {'baseline', 'pulse', 'release'} == set(phases)
|
||||
assert sum(x == 'pulse' for x in phases)*.05 == pytest.approx(STEP_S, abs=.05)
|
||||
for p in plans[:-1]:
|
||||
plan = p.lateralManeuverPlan
|
||||
expected = .001+(sign*amplitude/speed**2 if plan.fordChannelTest.keyboardPhase == 'pulse' else 0.)
|
||||
assert plan.desiredCurvature == pytest.approx(expected)
|
||||
assert plan.fordChannelTest.phase == 'maneuver' and plan.fordChannelTest.delta == 0.
|
||||
assert plan.fordChannelTest.keyboardRequestId == 1
|
||||
assert plan.fordChannelTest.targetAccel == pytest.approx(sign*amplitude)
|
||||
for i in range(1, 81):
|
||||
step(test, start+3.+i*.05, req)
|
||||
assert not test.active # holding the last trigger never repeats a step
|
||||
|
||||
|
||||
@pytest.mark.parametrize('fault', [
|
||||
{'input_valid': False}, {'input_time': 1.}, {'input_time': 100.}, {'healthy': False},
|
||||
{'speed': SPEEDS[0]+.71}, {'speed': math.nan}, {'curvature': math.nan},
|
||||
])
|
||||
def test_abort_never_queues_or_retries_old_request(fault):
|
||||
test, req = KeyboardManeuver(), request()
|
||||
start = ready(test, req)
|
||||
req.requestId, req.action = 1, 'left'
|
||||
assert step(test, start, req).valid
|
||||
assert not step(test, start+.05, req, **fault).valid
|
||||
for i in range(1, 61):
|
||||
assert not step(test, start+.05+i*.05, req).valid
|
||||
req.requestId = 2
|
||||
assert step(test, start+3.10, req).valid
|
||||
|
||||
|
||||
def test_unready_and_busy_keypresses_are_consumed_not_queued():
|
||||
test, req = KeyboardManeuver(), request(requestId=1, action='right')
|
||||
start = ready(test, req)
|
||||
assert not test.active
|
||||
req.requestId = 2
|
||||
assert step(test, start, req).valid
|
||||
req.requestId, req.action, req.channel, req.accel = 3, 'left', 'c1', 3.
|
||||
for i in range(1, 121):
|
||||
p = step(test, start+i*.05, req)
|
||||
if p.valid:
|
||||
assert p.lateralManeuverPlan.fordChannelTest.channel == 'c0'
|
||||
assert p.lateralManeuverPlan.fordChannelTest.targetAccel == .5
|
||||
assert test.run_id == 1 and not test.active
|
||||
|
||||
|
||||
@pytest.mark.parametrize('changes', [{'channel': 'none'}, {'accel': math.nan}, {'accel': 3.25}, {'accel': 0.}, {'speed': 0.}])
|
||||
def test_bad_input_cannot_trigger(changes):
|
||||
test, req = KeyboardManeuver(), request()
|
||||
start = ready(test, req)
|
||||
req.requestId, req.action = 1, 'right'
|
||||
for k, v in changes.items():
|
||||
setattr(req, k, v)
|
||||
assert not step(test, start, req).valid
|
||||
|
||||
|
||||
def test_frozen_time_clock_gap_and_cancel_abort():
|
||||
for delta in [0., -.1, .151]:
|
||||
test, req = KeyboardManeuver(), request()
|
||||
start = ready(test, req)
|
||||
req.requestId, req.action = 1, 'right'
|
||||
step(test, start, req)
|
||||
assert not step(test, start+delta, req).valid
|
||||
test, req = KeyboardManeuver(), request()
|
||||
start = ready(test, req)
|
||||
req.requestId, req.action = 1, 'right'
|
||||
step(test, start, req)
|
||||
req.requestId, req.action = 2, 'cancel'
|
||||
assert not step(test, start+.05, req).valid
|
||||
assert test.phase == 'aborted'
|
||||
|
||||
|
||||
def test_keyboard_mapping_heartbeat_repeat_suppression_and_cancel():
|
||||
control = KeyboardControl()
|
||||
for key in ('2', '+', 'v'):
|
||||
assert control.key(key, 10.)
|
||||
control.key('a', 11.)
|
||||
p = control.message().testJoystick
|
||||
assert list(p.axes) == [0., 0.]
|
||||
assert p.fordKeyboard.channel == 'c1' and p.fordKeyboard.action == 'left'
|
||||
assert p.fordKeyboard.accel == .75 and p.fordKeyboard.speed == pytest.approx(SPEEDS[1])
|
||||
first = p.fordKeyboard.requestId
|
||||
for i in range(1, 101):
|
||||
control.key('a', 11.+i*.05)
|
||||
assert control.message().testJoystick.fordKeyboard.requestId == first
|
||||
control.key('a', 17.)
|
||||
assert control.request.requestId > first
|
||||
control.key('r', 18.)
|
||||
assert control.request.action == 'cancel'
|
||||
assert not control.key('q', 19.)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('kwargs', [{'speed': 10}, {'accel': math.nan}, {'accel': math.inf}, {'accel': 0.}, {'accel': 3.1}])
|
||||
def test_cli_rejects_invalid_settings(kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
KeyboardControl(**kwargs)
|
||||
|
||||
|
||||
def test_offroad_setup_and_mode_lifecycle_use_real_params(tmp_path):
|
||||
params = Params(str(tmp_path))
|
||||
cp = car.CarParams.new_message(brand='ford', flags=1)
|
||||
params.put('CarParamsPersistent', cp.to_bytes(), block=True)
|
||||
params.put_bool('FordModelActionController', True, block=True)
|
||||
with pytest.raises(ValueError, match='offroad'):
|
||||
enable(params)
|
||||
params.put_bool('IsOffroad', True, block=True)
|
||||
params.put_bool('JoystickDebugMode', True, block=True)
|
||||
params.put_bool(PARAM, True, block=True)
|
||||
enable(params)
|
||||
assert params.get_bool(KEYBOARD_PARAM) and not params.get_bool(PARAM) and not params.get_bool('JoystickDebugMode')
|
||||
assert startup(params=params).ford_channel_test.keyboard
|
||||
params.put_bool('IsOffroad', False, block=True)
|
||||
enable(params) # reconnect to the selected mode, without an ignition cycle
|
||||
params.put_bool(PARAM, True, block=True)
|
||||
with pytest.raises(ValueError, match='offroad'):
|
||||
enable(params)
|
||||
assert params.get_bool(PARAM) # a refused onroad mode change has no side effects
|
||||
params.put_bool(PARAM, False, block=True)
|
||||
for flag in (ParamKeyFlag.CLEAR_ON_MANAGER_START, ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION):
|
||||
params.put_bool(KEYBOARD_PARAM, True, block=True)
|
||||
params.clear_all(flag)
|
||||
assert not params.get_bool(KEYBOARD_PARAM)
|
||||
for brand, flags in [('toyota', 1), ('ford', 0)]:
|
||||
params.put_bool('IsOffroad', True, block=True)
|
||||
params.put('CarParamsPersistent', car.CarParams.new_message(brand=brand, flags=flags).to_bytes(), block=True)
|
||||
with pytest.raises(ValueError, match='CAN FD Ford'):
|
||||
enable(params)
|
||||
|
||||
|
||||
def test_keyboard_plan_requires_keyboard_mode():
|
||||
test, req = KeyboardManeuver(), request()
|
||||
start = ready(test, req)
|
||||
req.requestId, req.action = 1, 'right'
|
||||
p = step(test, start, req).lateralManeuverPlan
|
||||
from openpilot.selfdrive.controls.tests.test_ford_channel_test import update
|
||||
assert not update(FordChannelTest(), msg=p).valid
|
||||
assert update(FordChannelTest(keyboard=True), msg=p).valid
|
||||
|
||||
|
||||
@pytest.mark.parametrize('keyboard', [False, True])
|
||||
def test_manager_imported_entrypoint_selects_the_correct_daemon(monkeypatch, keyboard):
|
||||
# Manager imports the module and calls main(); it does not execute __main__.
|
||||
from openpilot.tools.lateral_maneuvers import ford_maneuversd
|
||||
calls = []
|
||||
monkeypatch.setattr(ford_maneuversd, 'Params', lambda: SimpleNamespace(get_bool=lambda _key: keyboard))
|
||||
monkeypatch.setattr(ford_maneuversd, 'keyboard_main', lambda: calls.append('keyboard'))
|
||||
monkeypatch.setattr(ford_maneuversd, 'suite_main', lambda **kw: calls.append(kw))
|
||||
ford_maneuversd.main()
|
||||
assert calls == (['keyboard'] if keyboard else [{'ford_channels': True}])
|
||||
|
||||
|
||||
def test_real_daemon_allows_mads_manual_throttle_and_publishes_complete_run(monkeypatch):
|
||||
class Finished(Exception):
|
||||
pass
|
||||
clock, plans = [10.], []
|
||||
class SM(messaging.SubMaster):
|
||||
def __init__(self, services, **kwargs):
|
||||
super().__init__(services, **kwargs)
|
||||
self.events = {s: messaging.new_message(s) for s in services}
|
||||
|
||||
def update(self, _timeout):
|
||||
clock[0] += .05
|
||||
assert clock[0] < 20
|
||||
cs = self.events['carState'].carState
|
||||
cs.vEgo, cs.canValid, cs.gasPressed, cs.cruiseState.enabled = SPEEDS[0], True, True, False
|
||||
self.events['carControl'].carControl.latActive = True # MADS, selfdriveState.enabled remains false
|
||||
self.events['carStateSP'].carStateSP.fordPscmStatus = {'valid': True, 'canMonoTime': round(clock[0]*1e9), 'lateralState': 2}
|
||||
self.events['testJoystick'].testJoystick.fordKeyboard = request(requestId=1 if clock[0] > 12.5 else 0, action='right')
|
||||
for msg in self.events.values():
|
||||
msg.valid, msg.logMonoTime = True, round(clock[0]*1e9)
|
||||
self.update_msgs(clock[0], [m.as_reader() for m in self.events.values()])
|
||||
|
||||
class PM:
|
||||
def __init__(self, _services):
|
||||
pass
|
||||
|
||||
def send(self, service, msg):
|
||||
if service == 'lateralManeuverPlan':
|
||||
plans.append(msg)
|
||||
if msg.lateralManeuverPlan.fordChannelTest.phase == 'complete':
|
||||
raise Finished
|
||||
|
||||
monkeypatch.setattr(daemon.messaging, 'SubMaster', SM)
|
||||
monkeypatch.setattr(daemon.messaging, 'PubMaster', PM)
|
||||
monkeypatch.setattr(daemon, 'time', SimpleNamespace(monotonic=lambda: clock[0]))
|
||||
monkeypatch.setattr(daemon, 'Ratekeeper', lambda *_a, **_kw: SimpleNamespace(keep_time=lambda: None))
|
||||
with pytest.raises(Finished):
|
||||
daemon.main()
|
||||
active = [p for p in plans if p.valid]
|
||||
assert len(active)*.05 == pytest.approx(BASELINE_S+STEP_S+RELEASE_S, abs=.05)
|
||||
assert {str(p.lateralManeuverPlan.fordChannelTest.keyboardPhase) for p in active} == {'baseline', 'pulse', 'release'}
|
||||
@@ -1,201 +0,0 @@
|
||||
"""Synthetic log tests validate report math, not the truck's physical response."""
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.ford.fordcan import CanBus, create_lat_ctl2_msg
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.selfdrive.controls.lib.ford_channel_test import SPEEDS
|
||||
from openpilot.tools.lateral_maneuvers.ford_report import ChannelRuns, analyze, first_sustained, report
|
||||
|
||||
|
||||
# Archived raw-pulse routes remain readable after replacing the diagnostic.
|
||||
AMPLITUDE = {'c0': 1.28, 'c1': .125}
|
||||
|
||||
def event(kind, t):
|
||||
m = messaging.new_message(kind, size=1 if kind == 'sendcan' else None)
|
||||
m.logMonoTime, m.valid = round(t*1e9), True
|
||||
return m
|
||||
|
||||
|
||||
def fixture(channel='c0', direction=1, other_changes=False, response=True, abort=False, pulse_frames=100, amplitude=None):
|
||||
collector = ChannelRuns()
|
||||
packer = CANPacker('ford_lincoln_base_pt')
|
||||
cp = structs.CarParams(safetyConfigs=[structs.CarParams.SafetyConfig()])
|
||||
bus = CanBus(cp)
|
||||
amplitude = AMPLITUDE[channel] if amplitude is None else amplitude
|
||||
release_frame, complete_frame = 50+pulse_frames, 250+pulse_frames
|
||||
for i in range(complete_frame+1):
|
||||
t = 10.+i*.01
|
||||
if i % 5 == 0:
|
||||
p = event('lateralManeuverPlan', t)
|
||||
phase = 'baseline' if i < 50 else ('pulse' if i < release_frame else 'release')
|
||||
if i == complete_frame:
|
||||
phase, p.valid = ('aborted' if abort else 'complete'), False
|
||||
p.lateralManeuverPlan.fordChannelTest = {'runId': 1, 'channel': channel, 'phase': phase,
|
||||
'delta': direction*amplitude if phase == 'pulse' else 0., 'speed': SPEEDS[0]}
|
||||
collector.add(p)
|
||||
delta = direction*amplitude if 50 <= i < release_frame else 0.
|
||||
c0, c1 = (.01+delta, .002) if channel == 'c0' else (.01, .002+delta)
|
||||
if other_changes and 70 <= i < 90:
|
||||
if channel == 'c0':
|
||||
c1 += .001
|
||||
else:
|
||||
c0 += .01
|
||||
address, data, src = create_lat_ctl2_msg(packer, bus, 2, -c0, -c1, 0., 0., i % 16)
|
||||
m = event('sendcan', t)
|
||||
m.sendcan[0].address, m.sendcan[0].dat, m.sendcan[0].src = address, data, src
|
||||
collector.add(m)
|
||||
m = event('carState', t)
|
||||
m.carState.canValid, m.carState.vEgo = True, SPEEDS[0]
|
||||
m.carState.cruiseState.enabled = True
|
||||
m.carState.steeringAngleDeg = (direction*2. if response and 70 <= i < release_frame+50 else 0.)
|
||||
collector.add(m)
|
||||
m = event('carControl', t)
|
||||
m.carControl.latActive = True
|
||||
collector.add(m)
|
||||
m = event('carStateSP', t)
|
||||
m.carStateSP.fordPscmStatus = {'valid': True, 'canMonoTime': round(t*1e9), 'lateralState': 2}
|
||||
collector.add(m)
|
||||
assert len(collector.runs) == 1
|
||||
return cp, collector.runs[0]
|
||||
|
||||
|
||||
@pytest.mark.parametrize('channel', ['c0', 'c1'])
|
||||
@pytest.mark.parametrize('direction', [-1, 1])
|
||||
@pytest.mark.parametrize('legacy', [False, True])
|
||||
def test_command_aligned_metrics(channel, direction, legacy):
|
||||
amplitude = {'c0': .03, 'c1': .01}[channel] if legacy else AMPLITUDE[channel]
|
||||
pulse_frames = 50 if legacy else 100
|
||||
cp, run = fixture(channel, direction, pulse_frames=pulse_frames, amplitude=amplitude)
|
||||
d = analyze(run, cp)
|
||||
assert not d['problems']
|
||||
assert d['onset'] == pytest.approx(.2)
|
||||
assert d['wire_on'] == pytest.approx(10.5)
|
||||
assert d['wire_off'] == pytest.approx(10.5+pulse_frames*.01)
|
||||
assert d['peak'] == pytest.approx(2.)
|
||||
assert d['residual'] == pytest.approx(0.)
|
||||
assert d['delta'] == pytest.approx(direction*amplitude)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('channel', ['c0', 'c1'])
|
||||
def test_contaminated_field_excluded(channel):
|
||||
cp, run = fixture(channel, other_changes=True)
|
||||
assert 'other field changed' in analyze(run, cp)['problems']
|
||||
|
||||
|
||||
def test_no_movement_is_not_reported_as_zero_delay():
|
||||
cp, run = fixture(response=False)
|
||||
d = analyze(run, cp)
|
||||
assert d['onset'] is None and d['peak'] == 0.
|
||||
assert not d['problems']
|
||||
|
||||
|
||||
def test_abort_remains_visible_in_report(tmp_path):
|
||||
cp, run = fixture(abort=True)
|
||||
assert 'aborted' in analyze(run, cp)['problems']
|
||||
output = report('FORD', 'device/route', cp, SimpleNamespace(gitCommit='synthetic'), [run], tmp_path)
|
||||
html = output.read_text()
|
||||
assert 'Exclude from comparison: aborted' in html
|
||||
assert 'data:image/png;base64,' in html
|
||||
|
||||
|
||||
def test_incomplete_missing_and_bad_checksum():
|
||||
cp, run = fixture()
|
||||
run.outcome = 'incomplete'
|
||||
run.messages = [m for m in run.messages if m.which() != 'carStateSP']
|
||||
m = next(m for m in run.messages if m.which() == 'sendcan')
|
||||
data = bytearray(m.sendcan[0].dat)
|
||||
data[1] ^= 1
|
||||
m.sendcan[0].dat = bytes(data)
|
||||
problems = analyze(run, cp)['problems']
|
||||
assert {'incomplete', 'missing carStateSP', 'CAN checksum'} <= problems
|
||||
|
||||
|
||||
def test_single_sample_and_gap_cannot_count_as_onset():
|
||||
assert first_sustained([0., .01, .02, .03], [False, True, False, False]) is None
|
||||
assert first_sustained([0., .5], [True, True]) is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize('channel', ['c0', 'c1'])
|
||||
@pytest.mark.parametrize('keyboard', [False, True])
|
||||
def test_normal_channel_report_uses_real_targets_and_decoded_output(channel, keyboard, tmp_path, monkeypatch):
|
||||
from pathlib import Path
|
||||
from openpilot.tools.lateral_maneuvers import generate_report as generator
|
||||
from openpilot.tools.lateral_maneuvers.ford_report import channel_commands
|
||||
|
||||
cp = structs.CarParams(carFingerprint='FORD_SYNTHETIC_REPORT_TEST', steerControlType=structs.CarParams.SteerControlType.curvature,
|
||||
safetyConfigs=[structs.CarParams.SafetyConfig()])
|
||||
packer, bus = CANPacker('ford_lincoln_base_pt'), CanBus(cp)
|
||||
messages = []
|
||||
speed = SPEEDS[0]
|
||||
for i in range(250):
|
||||
t = 10.+i*.01
|
||||
curvature = (.5 if i < 105 else -.5)/speed**2
|
||||
if i % 5 == 0:
|
||||
m = event('lateralManeuverPlan', t)
|
||||
m.lateralManeuverPlan.desiredCurvature = curvature
|
||||
m.lateralManeuverPlan.fordChannelTest = {'runId': 1, 'channel': channel, 'phase': 'maneuver', 'speed': speed,
|
||||
'keyboardRequestId': 1 if keyboard else 0,
|
||||
'keyboardPhase': 'pulse' if i < 105 else 'release'}
|
||||
messages.append(m)
|
||||
m = event('carState', t)
|
||||
m.carState.vEgo, m.carState.steeringAngleDeg = speed, 16. if i < 110 else -16.
|
||||
messages.append(m)
|
||||
m = event('controlsState', t)
|
||||
m.controlsState.curvature = curvature*.8 if i > 10 else 0.
|
||||
m.controlsState.desiredCurvature = curvature
|
||||
messages.append(m)
|
||||
m = event('carControl', t)
|
||||
m.carControl.latActive = True
|
||||
m.carControl.orientationNED = [0., 0., 0.]
|
||||
messages.append(m)
|
||||
messages.append(event('carOutput', t))
|
||||
if keyboard:
|
||||
m = event('carStateSP', t)
|
||||
m.carStateSP.fordPscmStatus = {'valid': True, 'canMonoTime': m.logMonoTime, 'lateralState': 2, 'limit': 2 if 70 < i < 90 else 0}
|
||||
messages.append(m)
|
||||
c0, c1 = (24.5*curvature, 0.) if channel == 'c0' else (0., speed*curvature)
|
||||
address, data, src = create_lat_ctl2_msg(packer, bus, 2, -c0, -c1, 0., 0., i % 16)
|
||||
m = event('sendcan', t)
|
||||
m.sendcan[0].address, m.sendcan[0].dat, m.sendcan[0].src = address, data, src
|
||||
messages.append(m)
|
||||
m = event('alertDebug', 12.5)
|
||||
m.alertDebug.alertText1 = 'Complete'
|
||||
messages.append(m)
|
||||
commands, problems = channel_commands(messages, cp, channel, 10_000_000_000)
|
||||
assert not problems
|
||||
legacy_collector = ChannelRuns()
|
||||
for msg in messages:
|
||||
legacy_collector.add(msg)
|
||||
assert not legacy_collector.runs # CLI dispatches these targets to the normal report
|
||||
assert (commands[:, 2 if channel == 'c0' else 1] == 0.).all()
|
||||
captured = []
|
||||
savefig = generator.plt.Figure.savefig
|
||||
|
||||
def capture_figure(fig, *args, **kwargs):
|
||||
captured.append([axis.get_ylabel() for axis in fig.axes])
|
||||
# Render the real figure at preview resolution to keep this check fast.
|
||||
savefig(fig, *args, **(kwargs | {'dpi': 40}))
|
||||
|
||||
monkeypatch.setattr(generator.plt.Figure, 'savefig', capture_figure)
|
||||
opened = []
|
||||
monkeypatch.setattr(generator.webbrowser, 'open_new_tab', opened.append)
|
||||
monkeypatch.setattr(generator, '__file__', str(tmp_path/'generate_report.py'))
|
||||
generator.report('FORD_SYNTHETIC_REPORT_TEST', f'synthetic-{channel}', None, cp,
|
||||
SimpleNamespace(gitCommit='synthetic only', gitBranch='test', gitRemote='local'),
|
||||
[(f'step right 15mph {channel.upper()}', [messages])])
|
||||
output = Path(opened[0])
|
||||
html = output.read_text()
|
||||
output.rename(tmp_path/output.name)
|
||||
assert f'Normal maneuver target through {channel.upper()} only' in html
|
||||
assert 'invalid maneuver!' not in html
|
||||
expected = ['Lateral Accel (m/s^2)', 'Wheel angle (deg)', 'Velocity (mph)', 'Jerk (m/s^3)', 'Roll (deg)', 'C0 sent (m)', 'C1 sent (rad)']
|
||||
assert captured == [expected + (['PSCM limit\n2 = reached'] if keyboard else [])]
|
||||
if keyboard:
|
||||
assert 'Keyboard-triggered step' in html and 'Accelerator input is allowed with MADS' in html
|
||||
assert 'data:image/webp;base64,' in html
|
||||
# A live nonzero unused field must invalidate the isolated-channel measurement.
|
||||
assert 'channel isolation failed' in channel_commands(messages, cp, 'c1' if channel == 'c0' else 'c0', 10_000_000_000)[1]
|
||||
Reference in New Issue
Block a user