diff --git a/openpilot/cereal/log.capnp b/openpilot/cereal/log.capnp index 873de64297..3a17839a97 100644 --- a/openpilot/cereal/log.capnp +++ b/openpilot/cereal/log.capnp @@ -1220,6 +1220,18 @@ 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; # added to the captured command: meters for C0, radians for C1 + speed @4 :Float32; # target m/s + + enum Channel { none @0; c0 @1; c1 @2; } + enum Phase { waiting @0; baseline @1; pulse @2; release @3; complete @4; aborted @5; } + } } struct LongitudinalPlan @0xe00b5b3eba12876c { diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 896d61bcc1..b7e37e020f 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -85,6 +85,7 @@ inline static std::unordered_map 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"}}, {"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, {"LongitudinalPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast(cereal::LongitudinalPersonality::STANDARD))}}, {"NetworkMetered", {PERSISTENT | BACKUP, BOOL}}, diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 62316f943f..43d9e530ca 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -16,6 +16,7 @@ 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, is_channel_plan, 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 @@ -58,6 +59,7 @@ 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() 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") @@ -149,7 +151,7 @@ class Controls(ControlsExt): # Steering PID loop and lateral MPC # Reset desired curvature to current to avoid violating the limits on engage - if self.sm.valid['lateralManeuverPlan']: + if self.sm.valid['lateralManeuverPlan'] and not is_channel_plan(self.sm['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 @@ -168,7 +170,8 @@ 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 self.sm.valid['lateralManeuverPlan'] else 'modelV2' + reference_service = ('lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] and + not is_channel_plan(self.sm['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, @@ -178,6 +181,22 @@ 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 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 or CS.brakePressed, + speed=CS.vEgo, angle=CS.steeringAngleDeg, curvature=self.curvature, + pscm=self.sm['carStateSP'].fordPscmStatus, + ) + if override is not None: + self.ford_path = override + self.ford_path_controller.reset('channel_test') + 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: diff --git a/openpilot/selfdrive/controls/lib/ford_channel_test.py b/openpilot/selfdrive/controls/lib/ford_channel_test.py new file mode 100644 index 0000000000..e25b143778 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/ford_channel_test.py @@ -0,0 +1,116 @@ +"""Bounded, opt-in channel isolation. This is a diagnostic, not a driving controller.""" +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' +CONFLICTS = ('LateralManeuverMode', 'LongitudinalManeuverMode', 'JoystickDebugMode') +SPEEDS = (10.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS) +AMPLITUDE = {'c0': .03, 'c1': .01} +BASELINE_S, PULSE_S, RELEASE_S = .5, .5, 2. +TOTAL_S = BASELINE_S + PULSE_S + RELEASE_S +MAX_SPEED_ERROR = .7 +MAX_BASE_C0, MAX_BASE_C1 = .03, .015 +MAX_STEERING_CHANGE = 15. +MAX_LAT_ACCEL = 1. + + +def selected(CP, params): + return bool(CP.brand == 'ford' and CP.flags & FordFlags.CANFD and params.get_bool('FordModelActionController') + and params.get_bool(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' + + +class FordChannelTest: + """Validate the 20 Hz test lease at the 100 Hz command boundary. + + Capture the normal command once during baseline. Hold the untested field + throughout, and return the tested field to that baseline for release. No PI + or live model change may alter the output of an accepted run. + """ + def __init__(self): + self.clear() + + def clear(self): + self.run_id = None + self.channel = None + self.baseline = FordPath() + self.start = self.pulse_start = self.last_time = None + self.phase = 'waiting' + 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, angle, curvature, pscm): + test = plan.fordChannelTest + channel, phase = str(test.channel), str(test.phase) + fresh = math.isfinite(now) and math.isfinite(plan_time) and -.005 <= now-plan_time <= .15 + # An explicit fresh end releases the override. A vanished/stale publisher + # cannot silently remove an active test lease and re-arm an old pulse. + if (not plan_valid or phase in ('waiting', 'complete', 'aborted') or channel == 'none') 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 test plan') + 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, angle, curvature, test.delta, test.speed, normal.path_offset, normal.path_angle)): + return self.fail('nonfinite input') + if (pscm is None or not pscm.valid or pscm.canMonoTime <= 0 or not -.005 <= now-pscm.canMonoTime*1e-9 <= .15 + or pscm.denied or pscm.limit in (2, 3) or pscm.lateralState != 2): + return self.fail('PSCM unavailable, denied or limited') + 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 channel not in AMPLITUDE or phase not in ('baseline', 'pulse', 'release') or test.runId == 0: + return self.fail('invalid test identity') + if ((phase == 'pulse' and abs(abs(test.delta)-AMPLITUDE[channel]) > 1e-6) + or (phase != 'pulse' and test.delta != 0.)): + return self.fail('invalid pulse amplitude') + if self.run_id is None: + if phase != 'baseline' or abs(normal.path_offset) > MAX_BASE_C0 or abs(normal.path_angle) > MAX_BASE_C1 or abs(curvature) > .001: + return self.fail('baseline not ready') + self.run_id, self.channel = test.runId, channel + self.baseline, self.start, self.base_angle = normal, now, angle + self.target_speed = test.speed + if (test.runId != self.run_id or 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 > TOTAL_S+.2): + return self.fail('test identity or timing changed') + stages = ('waiting', 'baseline', 'pulse', 'release') + if stages.index(phase) < stages.index(self.phase) or stages.index(phase) > stages.index(self.phase)+1: + return self.fail('invalid phase transition') + if phase == 'baseline' and now-self.start > BASELINE_S+.15: + return self.fail('baseline timeout') + if phase == 'pulse': + if self.pulse_start is None: + if now-self.start < BASELINE_S-.15: + return self.fail('pulse started early') + self.pulse_start, self.pulse_delta = now, test.delta + if now-self.pulse_start > PULSE_S+.1 or test.delta != self.pulse_delta: + return self.fail('pulse timeout or direction changed') + if phase == 'release' and (self.pulse_start is None or now-self.pulse_start < PULSE_S-.15): + return self.fail('release started early') + if abs(angle-self.base_angle) > MAX_STEERING_CHANGE or abs(curvature)*speed**2 > MAX_LAT_ACCEL: + return self.fail('steering response exceeded test envelope') + self.phase, self.last_time = phase, now + c0 = self.baseline.path_offset + (test.delta if channel == 'c0' else 0.) + c1 = self.baseline.path_angle + (test.delta if channel == 'c1' else 0.) + command = FordPath(True, c0, c1, 0., 0.) + self.diagnostics = {'status': 'active', 'run_id': self.run_id, 'channel': channel, 'phase': phase, + 'delta': test.delta, 'target_speed': test.speed, + 'baseline_c0': self.baseline.path_offset, 'baseline_c1': self.baseline.path_angle, + 'command': (c0, c1, 0., 0.)} + return command diff --git a/openpilot/selfdrive/controls/tests/test_ford_channel_test.py b/openpilot/selfdrive/controls/tests/test_ford_channel_test.py new file mode 100644 index 0000000000..9d72cdebd6 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_channel_test.py @@ -0,0 +1,299 @@ +"""Exercise pulse timing, fault handling, actual controlsd and the Ford CAN sender.""" +import math +from collections import defaultdict +from types import SimpleNamespace + +import pytest + +from opendbc.can.parser 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 +from openpilot.common.params import Params, ParamKeyFlag +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature +from openpilot.selfdrive.controls.lib.ford_channel_test import AMPLITUDE, SPEEDS, FordChannelTest, is_channel_plan +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.ford_maneuversd import Sequence, TRIALS +from openpilot.selfdrive.car.helpers import convert_carControlSP + + +def plan(channel='c0', phase='baseline', delta=0., speed=SPEEDS[0], run_id=1): + msg = log.LateralManeuverPlan.new_message() + msg.fordChannelTest = {'runId': run_id, 'channel': channel, 'phase': phase, 'delta': delta, 'speed': speed} + # Exercise real Float32 serialization. + with log.LateralManeuverPlan.from_bytes(msg.to_bytes()) as reader: + return reader.as_builder() + + +def status(now=10., **changes): + return SimpleNamespace(**({'valid': True, 'canMonoTime': round(now*1e9), 'denied': False, 'limit': 0, 'lateralState': 2} | changes)) + + +def update(test, now=10., msg=None, **changes): + args = {'plan_valid': True, 'plan_time': now, 'now': now, 'normal': FordPath(True, .01, .002, 0., 0.), 'active': True, + 'healthy': True, 'driver_input': False, 'speed': SPEEDS[0], 'angle': 0., 'curvature': 0., 'pscm': status(now)} + return test.update(msg or plan(), **(args | changes)) + + +def pulse(test): + for i in range(51): + result = update(test, 10.+i*.01, plan(phase='pulse', delta=.03) if i == 50 else plan()) + assert result.valid + return result + + +@pytest.mark.parametrize('changes', [ + {'active': False}, {'healthy': False}, {'driver_input': True}, {'speed': SPEEDS[0]+.71}, + {'speed': math.nan}, {'angle': math.inf}, {'angle': 15.01}, {'curvature': .1}, + {'normal': FordPath()}, {'normal': FordPath(True, math.nan, 0.)}, {'plan_time': 10.}, + {'pscm': None}, {'pscm': status(10.51, denied=True)}, {'pscm': status(10.51, limit=2)}, + {'pscm': status(10.51, limit=3)}, {'pscm': status(10.51, lateralState=1)}, {'pscm': status(10.)}, +]) +def test_active_faults_zero_output_and_latch(changes): + test = FordChannelTest() + pulse(test) + assert update(test, 10.51, plan(phase='pulse', delta=.03), **changes) == FordPath() + assert test.aborted + assert update(test, 10.52, plan(phase='pulse', delta=.03)) == FordPath() + assert update(test, 10.53, plan(phase='aborted'), plan_valid=False) is None + + +@pytest.mark.parametrize('msg', [ + plan('c1', 'pulse', .01), plan('c0', 'pulse', -.03), plan('c0', 'pulse', .04), + plan('c0', 'pulse', math.nan), plan('c0', 'pulse', .03, run_id=2), plan('c0', 'baseline'), + plan('c0', 'pulse', .03, speed=SPEEDS[1]), plan('c0', 'release'), +]) +def test_midpulse_identity_or_phase_changes_abort(msg): + test = FordChannelTest() + pulse(test) + assert update(test, 10.51, msg) == FordPath() + + +@pytest.mark.parametrize('phase,delta', [('pulse', .03), ('release', 0.)]) +def test_cannot_start_midrun(phase, delta): + assert update(FordChannelTest(), msg=plan(phase=phase, delta=delta)) == FordPath() + + +@pytest.mark.parametrize('phase', ['baseline', 'pulse', 'release']) +def test_frozen_phase_times_out_even_with_fresh_timestamps(phase): + test = FordChannelTest() + result = None + for i in range(350): + stage = 'baseline' if i < 50 or phase == 'baseline' else ('pulse' if i < 100 or phase == 'pulse' else 'release') + result = update(test, 10.+i*.01, plan(phase=stage, delta=.03 if stage == 'pulse' else 0.)) + if not result.valid: + break + assert result == FordPath() and test.aborted + + +def seq_update(seq, now, **changes): + return seq.update(now, **({'engaged': True, 'lat_active': True, 'healthy': True, 'ready': True, + 'driver_input': False, 'speed': TRIALS[min(seq.index, 7)].speed} | changes)) + + +def test_full_suite_with_serialized_20hz_plans_and_100hz_receiver(): + seq, test = Sequence(), FordChannelTest() + phases, counts, baselines = {}, defaultdict(int), {} + result = seq_update(seq, 10., engaged=False, lat_active=False) + msg = plan(result.trial.channel, result.phase, result.delta, result.trial.speed, result.run_id) + plan_time = 10. + for i in range(1, 6501): + now = 10.+i*.01 + if i % 5 == 0: + result = seq_update(seq, now) + msg = plan(result.trial.channel, result.phase, result.delta, result.trial.speed, result.run_id) + plan_time = now + normal = FordPath(True, .01, .002) if result.phase == 'baseline' else FordPath(True, 1., .2) + output = update(test, now, msg, plan_valid=result.active, plan_time=plan_time, normal=normal, speed=result.trial.speed) + if result.active: + assert output.valid, test.diagnostics + expected = (.01+result.delta, .002) if result.trial.channel == 'c0' else (.01, .002+result.delta) + assert (output.path_offset, output.path_angle) == pytest.approx(expected) + assert output.curvature == output.curvature_rate == 0. + phases.setdefault(result.run_id, set()).add(result.phase) + counts[result.run_id, result.phase] += 1 + if result.phase == 'baseline': + baselines[result.run_id] = output + if result.phase == 'release': + assert output == baselines[result.run_id] + else: + assert output is None + if result.text == 'Ford tests finished': + break + assert seq.index == 8 + assert len(phases) == 8 + assert all(stages == {'baseline', 'pulse', 'release'} for stages in phases.values()) + assert all(45 <= counts[run, 'pulse'] <= 55 and 195 <= counts[run, 'release'] <= 205 for run in phases) + assert {(t.speed, t.channel, t.direction) for t in TRIALS} == {(v, c, s) for v in SPEEDS for c in AMPLITUDE for s in (1, -1)} + + +@pytest.mark.parametrize('changes', [{'driver_input': True}, {'healthy': False}, {'lat_active': False}, + {'engaged': False}, {'speed': SPEEDS[1]}, {'speed': math.nan}]) +def test_sequence_requires_real_disengagement_to_retry(changes): + seq = Sequence() + assert seq_update(seq, 10.).phase == 'aborted' # restarting while engaged cannot arm + seq_update(seq, 10.05, engaged=False) + for i in range(1, 45): + result = seq_update(seq, 10.05+i*.05) + assert result.active + assert seq_update(seq, 12.3, **changes).phase == 'aborted' + for i in range(1, 20): + assert seq_update(seq, 12.3+i*.05, lat_active=False).phase == 'aborted' + seq_update(seq, 13.3, engaged=False, lat_active=False) + for i in range(1, 45): + result = seq_update(seq, 13.3+i*.05) + assert result.active and result.run_id == 2 and seq.index == 0 + + +def test_mode_selection_and_transient_parameter(tmp_path): + params = Params(str(tmp_path)) + assert not params.get_bool('FordChannelTestMode') + 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') + assert b'FordChannelTestMode' not in params.all_keys(ParamKeyFlag.BACKUP) + + +@pytest.mark.parametrize('trial', TRIALS) +def test_actual_controlsd_and_wire_isolate_release_then_abort(pipeline, trial): # noqa: F811 + call, publication = pipeline + params = SimpleNamespace(get_bool=lambda key: key in ('FordModelActionController', 'FordChannelTestMode')) + controls = startup(params=params) + sm = Subscriptions(True) + controls.sm, controls.desired_curvature, controls.curvature = sm, 0., 0. + cc = structs.CarControl(latActive=True) + cs = SimpleNamespace(vEgo=trial.speed, yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0., + gasPressed=False, brakePressed=False, steeringAngleDeg=0., cruiseState=SimpleNamespace(enabled=True)) + model = straight() + model.action = SimpleNamespace(desiredCurvature=.0003) + cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint=controls.CP.carFingerprint) + sender = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP()) + vehicle = SimpleNamespace(out=structs.CarState(vEgo=trial.speed, vEgoRaw=trial.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) + baseline = None + for i in range(151): + now = 10.+i*.01 + phase = 'baseline' if i < 50 else ('pulse' if i < 100 else 'release') + delta = trial.direction*AMPLITUDE[trial.channel] if phase == 'pulse' else 0. + sm.messages['lateralManeuverPlan'] = plan(trial.channel, phase, delta, trial.speed) + sm.logMonoTime = dict.fromkeys(sm.logMonoTime, round(now*1e9)) + pscm = sm['carStateSP'].fordPscmStatus + pscm.valid, pscm.canMonoTime, pscm.lateralState = True, round(now*1e9), 2 + cs.brakePressed = i == 150 + if i > 0: + model.action.desiredCurvature = .03 # normal PI/model requests cannot leak into the held fields + exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.), + 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda t=now: t), 'math': math}) + msg = custom.CarControlSP.new_message() + exec(publication, {'self': controls, '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'] + if baseline is None: + baseline = (wire['LatCtlPathOffst_L_Actl'], wire['LatCtlPath_An_Actl']) + actual = (wire['LatCtlPathOffst_L_Actl'], wire['LatCtlPath_An_Actl']) + if i == 150: + assert wire['LatCtl_D2_Rq'] == 0 and actual == (0., 0.) and not cc.latActive + else: + expected = (baseline[0]-delta, baseline[1]) if trial.channel == 'c0' else (baseline[0], baseline[1]-delta) + assert actual == pytest.approx(expected, abs=1e-8) + assert wire['LatCtl_D2_Rq'] == 2 + assert controls.ford_path_controller.core.correction == 0. + assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0. + + +def test_daemon_publishes_both_speeds_and_real_schema(monkeypatch): + from openpilot.cereal import messaging + from openpilot.tools.lateral_maneuvers import ford_maneuversd as daemon + from openpilot.tools.lateral_maneuvers.ford_report import ChannelRuns + + class Finished(Exception): + pass + + plans, alerts = [], [] + clock = [10.] + + class SM(dict): + def __init__(self, services): + super().__init__((s, getattr(messaging.new_message(s), s)) for s in services) + + def update(self, _timeout): + cs = self['carState'] + cs.vEgo = plans[-1].lateralManeuverPlan.fordChannelTest.speed if plans else SPEEDS[0] + cs.canValid, cs.cruiseState.enabled = True, True + self['selfdriveState'].enabled = clock[0] > 10. + self['carControl'].latActive = clock[0] > 10. + self['carControlSP'].fordLateralPath = {'enabled': True, 'valid': True} + self['carStateSP'].fordPscmStatus = {'valid': True, 'canMonoTime': round(clock[0]*1e9), 'lateralState': 2} + + def all_checks(self, _services): + return True + + class PM: + def __init__(self, _services): + pass + + def send(self, service, message): + message.logMonoTime = round(clock[0]*1e9) + (plans if service == 'lateralManeuverPlan' else alerts).append(message) + if service == 'alertDebug' and message.alertDebug.alertText1 == 'Ford tests finished': + raise Finished + + class Clock: + def __init__(self, rate): + assert rate == 20 + + def keep_time(self): + clock[0] += .05 + assert clock[0] < 75. + + monkeypatch.setattr(daemon.messaging, 'SubMaster', SM) + monkeypatch.setattr(daemon.messaging, 'PubMaster', PM) + monkeypatch.setattr(daemon, 'Ratekeeper', Clock) + monkeypatch.setattr(daemon, 'time', SimpleNamespace(monotonic=lambda: clock[0])) + monkeypatch.setattr(daemon, 'cloudlog', SimpleNamespace(event=lambda *_a, **_kw: None)) + with pytest.raises(Finished): + daemon.main() + grouped = ChannelRuns() + for msg in plans: + grouped.add(msg) + assert msg.lateralManeuverPlan.desiredCurvature == 0. # channel plans never pretend to be model references + assert len(grouped.runs) == 8 + assert all(run.outcome == 'complete' for run in grouped.runs) + active = {(m.lateralManeuverPlan.fordChannelTest.channel, round(m.lateralManeuverPlan.fordChannelTest.speed, 3)) + for m in plans if m.valid} + assert active == {(c, round(v, 3)) for c in AMPLITUDE for v in SPEEDS} + + +def test_enabled_channel_payload_is_ignored_when_toggle_off(pipeline): # noqa: F811 + controls = startup() + sm = Subscriptions(True) + sm.messages['lateralManeuverPlan'] = plan(phase='pulse', delta=.03) + sm.messages['lateralManeuverPlan'].desiredCurvature = -.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.), + 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}) + assert controls.ford_channel_test is None + assert controls.desired_curvature > 0. and controls.ford_path.path_angle > 0. + assert controls.ford_path.path_offset == 0. + assert controls.ford_path_controller.diagnostics['reference_age'] == pytest.approx(.02) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py index b9b722ba13..83e1530ef2 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -22,6 +22,7 @@ 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 is_channel_plan 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 @@ -140,7 +141,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 ast.unparse(n.test) == "self.sm.valid['lateralManeuverPlan']") + selection = next(n for n in body if isinstance(n, ast.If) and "self.sm.valid['lateralManeuverPlan']" in ast.unparse(n.test)) 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'") @@ -183,7 +184,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.), - 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)} + 'is_channel_plan': is_channel_plan, '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) @@ -227,7 +228,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.), - 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}) + 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}) assert controls.ford_path.valid == cc.latActive == (failed == 'lateralManeuverPlan' and not maneuver) @@ -256,7 +257,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.), 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda now=now: now)} exec(call, environment) msg = custom.CarControlSP.new_message() @@ -299,7 +300,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.), 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda now=now: now)}) controller = controls.ford_path_controller assert controller.diagnostics['pscm_limited'] is service_valid @@ -335,7 +336,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.), 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'is_channel_plan': is_channel_plan, '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 @@ -390,7 +391,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.), 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda now=now: now)}) assert_current_request(core, controls.desired_curvature, cs.vEgo) if frame == 129: @@ -440,7 +441,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.), 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda now=now: now)}) assert_current_request(core, controls.desired_curvature, cs.vEgo) assert core.correction == 0. @@ -497,7 +498,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.), 'clip_curvature': clip_curvature}) + 'lp': SimpleNamespace(roll=0.), 'is_channel_plan': is_channel_plan, '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}) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py index 6c501c194f..f3c46db7a0 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py @@ -11,6 +11,7 @@ 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] @@ -34,6 +35,7 @@ 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 diff --git a/openpilot/selfdrive/ui/mici/layouts/settings/developer.py b/openpilot/selfdrive/ui/mici/layouts/settings/developer.py index 2d7dc0899d..504f625cc9 100644 --- a/openpilot/selfdrive/ui/mici/layouts/settings/developer.py +++ b/openpilot/selfdrive/ui/mici/layouts/settings/developer.py @@ -1,4 +1,5 @@ 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 @@ -79,6 +80,9 @@ 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) @@ -93,6 +97,7 @@ 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, ]) @@ -104,11 +109,13 @@ 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._alpha_long_toggle) + release_blocked_toggles = (self._joystick_toggle, self._long_maneuver_toggle, self._lat_maneuver_toggle, + self._ford_channel_test_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 @@ -118,6 +125,8 @@ 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: @@ -140,6 +149,9 @@ 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: @@ -163,6 +175,7 @@ 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) @@ -170,6 +183,7 @@ 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) @@ -178,6 +192,7 @@ 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) @@ -186,6 +201,19 @@ 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) + self._ford_channel_test_toggle.set_checked(False) + + def _on_ford_channel_test(self, state: bool): + 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) diff --git a/openpilot/system/manager/process_config.py b/openpilot/system/manager/process_config.py index f776a85923..84aa1a1e09 100644 --- a/openpilot/system/manager/process_config.py +++ b/openpilot/system/manager/process_config.py @@ -8,6 +8,7 @@ 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 @@ -50,6 +51,9 @@ 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") @@ -146,6 +150,7 @@ 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), diff --git a/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md b/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md new file mode 100644 index 0000000000..4f872df354 --- /dev/null +++ b/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md @@ -0,0 +1,23 @@ +# Ford C0 / C1 comparison + +This separate diagnostic measures the wheel response to small C0 and C1 pulses at **10 and 20 mph**. It requires the Ford model-action controller and a CAN FD Ford. It makes no change to normal controller tuning. + +1. While offroad, enable **Settings → Developer → Ford C0 / C1 test** on comma four. This selects the test for the next onroad session and turns off the other maneuver/joystick modes. +2. Use a clear, straight test area with room for a small lateral deviation. Engage ACC and lateral control, then hold the speed shown on screen. Testing requires two continuous seconds of steady, nearly straight driving without steering or pedal input. It will not start if you are manually holding the accelerator. +3. At 10 mph it runs **C0 right, C0 left, C1 right, C1 left**. Repeat those four trials at 20 mph. Each trial holds the starting commands for 0.5 seconds, pulses one field for 0.5 seconds, then restores the starting commands for 2 seconds to observe release. +4. Follow the displayed phase and target speed. Normal model tracking resumes between trials and after completion. If a trial aborts, **disengage and reengage** to retry it; clearing steering input alone does not restart the pulse. Stop if the area no longer provides room for the test. +5. After “Ford tests finished,” end the route and upload all logs. The test toggle clears when the device returns offroad or the manager restarts. + +The pulse is **±0.03 m C0** or **±0.01 rad C1**, added to the captured starting command. These are small diagnostic increments, not assumed equivalent steering demands. The other field remains fixed. C2/C3 stay zero, and PI correction does not modify the test commands. Release returns to the captured baseline; it does not deliberately countersteer or promise that the wheel immediately centers. + +Driver input, lost ACC/lateral engagement, invalid or stale inputs, PSCM denial/limit status, or excessive response abort the test. The diagnostic additionally stops above 15° wheel movement from baseline or 1 m/s² measured lateral acceleration. These are abort thresholds for this test, not a model of the PSCM or changes to normal driving limits. + +Generate the report with the existing command: + +```sh +python openpilot/tools/lateral_maneuvers/generate_report.py DEVICE/ROUTE +``` + +The generator detects channel trials and plots **C0 sent, C1 sent, actual wheel movement and speed**. It measures thresholded movement onset from the changed CAN packet, peak movement, and residual angle at the end of release. It flags incomplete, intervened, limited, stale, or non-isolated trials for exclusion. Standard lateral maneuver reports keep their existing behavior. + +One run in each direction provides a first comparison, not a reliable universal plant model. Compare timing alongside response strength: a larger response can cross a movement threshold sooner even with identical delay. These short pulses do not validate sustained intersection turns. diff --git a/openpilot/tools/lateral_maneuvers/README.md b/openpilot/tools/lateral_maneuvers/README.md index 7a4c381b15..8588bd93eb 100644 --- a/openpilot/tools/lateral_maneuvers/README.md +++ b/openpilot/tools/lateral_maneuvers/README.md @@ -5,6 +5,8 @@ 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 10 and 20 mph. + ## Instructions 1. Check out a development branch such as `master` on your comma device. diff --git a/openpilot/tools/lateral_maneuvers/ford_maneuversd.py b/openpilot/tools/lateral_maneuvers/ford_maneuversd.py new file mode 100644 index 0000000000..b2078995b8 --- /dev/null +++ b/openpilot/tools/lateral_maneuvers/ford_maneuversd.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +"""Eight short Ford channel pulses. Normal model tracking runs between trials.""" +import math +import time +from dataclasses import dataclass + +from openpilot.cereal import messaging +from openpilot.common.constants import CV +from openpilot.common.realtime import Ratekeeper +from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.controls.lib.ford_channel_test import ( + AMPLITUDE, BASELINE_S, PULSE_S, TOTAL_S, SPEEDS, MAX_SPEED_ERROR, MAX_BASE_C0, MAX_BASE_C1, +) + + +@dataclass(frozen=True) +class Trial: + speed: float + channel: str + direction: int + + @property + def description(self): + # Controller coordinates are left-positive; the CAN sender negates C0/C1. + return f'{self.channel.upper()} {"left" if self.direction > 0 else "right"} {self.speed*CV.MS_TO_MPH:.0f} mph' + + +TRIALS = tuple(Trial(speed, channel, sign) for speed in SPEEDS for channel in AMPLITUDE for sign in (-1, 1)) + + +@dataclass(frozen=True) +class Output: + trial: Trial + run_id: int + phase: str + text: str + delta: float = 0. + + @property + def active(self): + return self.phase in ('baseline', 'pulse', 'release') + + +class Sequence: + def __init__(self): + self.index = self.run_id = 0 + self.start = self.ready_start = self.last_time = None + self.complete_until = None + self.need_disengage = True # also prevents a daemon restart from re-arming while engaged + self.reason = '' + + def update(self, now, *, engaged, lat_active, healthy, ready, driver_input, speed): + trial = TRIALS[min(self.index, len(TRIALS)-1)] + def out(phase, text, delta=0.): + return Output(trial, self.run_id, phase, text, delta) + healthy = healthy and math.isfinite(speed) + clock_ok = math.isfinite(now) and (self.last_time is None or 0. < now-self.last_time <= .2) + self.last_time = now + if self.index == len(TRIALS): + return out('complete', 'Ford tests finished') + if self.start is not None: + if not clock_ok or not healthy or not engaged or not lat_active or driver_input or abs(speed-trial.speed) > MAX_SPEED_ERROR: + self.start = self.ready_start = None + self.need_disengage = True + self.reason = 'Aborted: disengage to retry' + return out('aborted', self.reason) + elapsed = now-self.start + if elapsed < BASELINE_S: + return out('baseline', 'Active: holding baseline') + if elapsed < BASELINE_S+PULSE_S: + return out('pulse', 'Active: pulse', trial.direction*AMPLITUDE[trial.channel]) + if elapsed < TOTAL_S: + return out('release', 'Active: release') + self.start = None + self.complete_until = now+1. + return out('complete', 'Complete') + if self.complete_until is not None: + if now < self.complete_until: + return out('complete', 'Complete') + self.index += 1 + self.complete_until = self.ready_start = None + return out('waiting', 'Next trial') + if self.need_disengage: + if not engaged: + self.need_disengage = False + self.reason = '' + else: + return out('aborted', self.reason or 'Disengage before testing') + ready = ready and healthy and lat_active and engaged and not driver_input and abs(speed-trial.speed) <= MAX_SPEED_ERROR and clock_ok + if not ready: + self.ready_start = None + return out('waiting', f'Set {trial.speed*CV.MS_TO_MPH:.0f} mph; straight and steady') + if self.ready_start is None: + self.ready_start = now + if now-self.ready_start < 2.: + return out('waiting', 'Starting: hold straight') + self.run_id += 1 + self.start, self.ready_start = now, None + return out('baseline', 'Active: holding baseline') + + +def main(): + services = ['carState', 'carStateSP', 'carControl', 'carControlSP', 'controlsState', 'selfdriveState', + 'selfdriveStateSP', 'modelV2', 'vehicleParameters'] + sm = messaging.SubMaster(services) + pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug']) + sequence = Sequence() + rk = Ratekeeper(20) + previous = None + while True: + sm.update(0) + now = time.monotonic() + cs, cc, path = sm['carState'], sm['carControl'], sm['carControlSP'].fordLateralPath + mads = sm['selfdriveStateSP'].mads + pscm = sm['carStateSP'].fordPscmStatus + lp, controls = sm['vehicleParameters'], sm['controlsState'] + finite = all(math.isfinite(x) for x in (cs.vEgo, cs.steeringTorque, cs.steeringRateDeg, lp.roll, controls.curvature, + controls.desiredCurvature, path.pathOffset, path.pathAngle)) + healthy = (finite and sm.all_checks(services) and cs.canValid and cs.cruiseState.enabled + and not cs.steerFaultTemporary and not cs.steerFaultPermanent + and pscm.valid and pscm.canMonoTime > 0 and -.005 <= now-pscm.canMonoTime*1e-9 <= .15 + and not pscm.denied and pscm.limit not in (2, 3)) + driver = cs.steeringPressed or abs(cs.steeringTorque) > 1. or cs.gasPressed or cs.brakePressed + ready = (path.enabled and path.valid and abs(path.pathOffset) <= MAX_BASE_C0 and abs(path.pathAngle) <= MAX_BASE_C1 + and abs(controls.curvature) <= .001 and abs(controls.desiredCurvature) <= .001 + and abs(cs.steeringRateDeg) < 2. and abs(lp.roll) < .12 and pscm.lateralState == 2) + result = sequence.update(now, engaged=mads.enabled if mads.available else sm['selfdriveState'].enabled, + lat_active=cc.latActive, healthy=healthy, ready=ready, driver_input=driver, speed=cs.vEgo) + plan = messaging.new_message('lateralManeuverPlan') + plan.valid = result.active + test = plan.lateralManeuverPlan.fordChannelTest + test.runId, test.channel, test.phase = result.run_id, result.trial.channel, result.phase + test.delta, test.speed = result.delta, result.trial.speed + pm.send('lateralManeuverPlan', plan) + alert = messaging.new_message('alertDebug') + alert.valid = True + alert.alertDebug.alertText1 = result.text + alert.alertDebug.alertText2 = result.trial.description + pm.send('alertDebug', alert) + key = (result.run_id, result.phase, result.text) + if key != previous: + cloudlog.event('Ford channel test progress', run_id=result.run_id, channel=result.trial.channel, + phase=result.phase, delta=result.delta, target_speed=result.trial.speed, message=result.text) + previous = key + rk.keep_time() + + +if __name__ == '__main__': + main() diff --git a/openpilot/tools/lateral_maneuvers/ford_report.py b/openpilot/tools/lateral_maneuvers/ford_report.py new file mode 100644 index 0000000000..078ee78570 --- /dev/null +++ b/openpilot/tools/lateral_maneuvers/ford_report.py @@ -0,0 +1,213 @@ +"""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 = ['Ford C0 / C1 test', + '', + '

Ford C0 / C1 test

', f'

{escape(platform)} · {escape(route)}
Commit {escape(ID.gitCommit)}

', + '

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.

', + '

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.

'] + 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'

{escape(title)}

') + if d['problems']: + html.append(f'

Exclude from comparison: {escape(", ".join(sorted(d["problems"])))}

') + else: + html.append('

Complete; command isolation and logged validity checks passed.

') + if d['peak'] is not None: + onset = f'{d["onset"]:.3f} s' if d['onset'] is not None else 'not detected' + html.append(f'

Onset: {onset} (threshold {d["threshold"]:.2f}°) · ' + + f'Peak movement: {d["peak"]:.2f}° · End residual: {d["residual"]:+.2f}°

') + if not len(d['wheels']) or not len(d['commands']): + html.append('

Insufficient wheel or CAN data to plot.

') + 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'{escape(title)} command and wheel response') + output.write_text(''.join(html)) + return output diff --git a/openpilot/tools/lateral_maneuvers/generate_report.py b/openpilot/tools/lateral_maneuvers/generate_report.py index e26690e2da..6fd7c3bee5 100755 --- a/openpilot/tools/lateral_maneuvers/generate_report.py +++ b/openpilot/tools/lateral_maneuvers/generate_report.py @@ -18,6 +18,7 @@ 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, report as ford_channel_report def lat_accel(curvature, v): @@ -233,8 +234,10 @@ 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: @@ -248,4 +251,9 @@ if __name__ == '__main__': if active_prev: maneuvers[-1][1][-1].append(msg) - report(platform, args.route, args.description, CP, ID, maneuvers) + 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) diff --git a/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py b/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py new file mode 100644 index 0000000000..244a2524ee --- /dev/null +++ b/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py @@ -0,0 +1,111 @@ +"""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 AMPLITUDE, SPEEDS +from openpilot.tools.lateral_maneuvers.ford_report import ChannelRuns, analyze, first_sustained, report + + +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): + collector = ChannelRuns() + packer = CANPacker('ford_lincoln_base_pt') + cp = structs.CarParams(safetyConfigs=[structs.CarParams.SafetyConfig()]) + bus = CanBus(cp) + for i in range(301): + t = 10.+i*.01 + if i % 5 == 0: + p = event('lateralManeuverPlan', t) + phase = 'baseline' if i < 50 else ('pulse' if i < 100 else 'release') + if i == 300: + phase, p.valid = ('aborted' if abort else 'complete'), False + p.lateralManeuverPlan.fordChannelTest = {'runId': 1, 'channel': channel, 'phase': phase, + 'delta': direction*AMPLITUDE[channel] if phase == 'pulse' else 0., 'speed': SPEEDS[0]} + collector.add(p) + delta = direction*AMPLITUDE[channel] if 50 <= i < 100 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 < 150 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]) +def test_command_aligned_metrics(channel, direction): + cp, run = fixture(channel, direction) + 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(11.) + assert d['peak'] == pytest.approx(2.) + assert d['residual'] == pytest.approx(0.) + assert d['delta'] == pytest.approx(direction*AMPLITUDE[channel]) + + +@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