diff --git a/openpilot/cereal/log.capnp b/openpilot/cereal/log.capnp index 3a17839a97..a4537a4d48 100644 --- a/openpilot/cereal/log.capnp +++ b/openpilot/cereal/log.capnp @@ -1226,11 +1226,11 @@ struct LateralManeuverPlan { runId @0 :UInt32; channel @1 :Channel; phase @2 :Phase; - delta @3 :Float32; # added to the captured command: meters for C0, radians for C1 + delta @3 :Float32; # legacy raw-pulse increment; zero for normal channel maneuvers 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; } + enum Phase { waiting @0; baseline @1; pulse @2; release @3; complete @4; aborted @5; maneuver @6; } } } diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 43d9e530ca..b48e9153da 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -16,7 +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.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 @@ -151,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'] and not is_channel_plan(self.sm['lateralManeuverPlan']): + if use_maneuver_reference(self.sm['lateralManeuverPlan'], self.sm.valid['lateralManeuverPlan'], self.ford_channel_test is not None): 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 @@ -170,8 +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'] and - not is_channel_plan(self.sm['lateralManeuverPlan']) else 'modelV2') + reference_service = ('lateralManeuverPlan' if use_maneuver_reference(self.sm['lateralManeuverPlan'], + self.sm.valid['lateralManeuverPlan'], self.ford_channel_test is not None) 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, @@ -188,12 +188,10 @@ class Controls(ControlsExt): 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, + speed=CS.vEgo, ) 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) diff --git a/openpilot/selfdrive/controls/lib/ford_channel_test.py b/openpilot/selfdrive/controls/lib/ford_channel_test.py index 3f5f735980..a6bc90838a 100644 --- a/openpilot/selfdrive/controls/lib/ford_channel_test.py +++ b/openpilot/selfdrive/controls/lib/ford_channel_test.py @@ -1,4 +1,4 @@ -"""Bounded, opt-in channel isolation. This is a diagnostic, not a driving controller.""" +"""Opt-in output isolation for the normal lateral maneuver controller path.""" import math from opendbc.car.ford.values import FordFlags @@ -8,14 +8,8 @@ from openpilot.selfdrive.controls.lib.ford_path import FordPath PARAM = 'FordChannelTestMode' CONFLICTS = ('LateralManeuverMode', 'LongitudinalManeuverMode', 'JoystickDebugMode') SPEEDS = (15.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS) -# 25% of the controller's symmetric field limits, rounded to the CAN steps. -AMPLITUDE = {'c0': round(.25*5.11, 2), 'c1': .25*.5} -BASELINE_S, PULSE_S, RELEASE_S = .5, 1., 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. +MAX_RUN_S = 4. # 2.6 s nominal, with room for the accepted 16–24 Hz model cadence def selected(CP, params): @@ -28,22 +22,25 @@ def is_channel_plan(plan): return test is not None and test.channel != 'none' -class FordChannelTest: - """Validate the 20 Hz test lease at the 100 Hz command boundary. +def is_channel_maneuver(plan): + return is_channel_plan(plan) and plan.fordChannelTest.phase == 'maneuver' - 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 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): 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.run_id = self.channel = self.start = self.last_time = None self.aborted = False self.diagnostics = {} @@ -52,66 +49,32 @@ class FordChannelTest: 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): + def update(self, plan, *, plan_valid, plan_time, now, normal, active, healthy, driver_input, speed): 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): + 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 test plan') + return self.fail('stale maneuver plan') + if not is_channel_maneuver(plan) or test.runId == 0 or test.delta != 0.: + 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, angle, curvature, test.delta, test.speed, normal.path_offset, normal.path_angle)): + 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 (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.)} + 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, + 'command': (command.path_offset, command.path_angle, 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 index 319c99c234..0e306220a7 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_channel_test.py +++ b/openpilot/selfdrive/controls/tests/test_ford_channel_test.py @@ -1,160 +1,74 @@ -"""Exercise pulse timing, fault handling, actual controlsd and the Ford CAN sender.""" +"""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.parser import CANParser +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 +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 AMPLITUDE, SPEEDS, FordChannelTest, is_channel_plan +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.ford_maneuversd import Sequence, TRIALS -from openpilot.selfdrive.car.helpers import convert_carControlSP +from openpilot.tools.lateral_maneuvers import lateral_maneuversd as daemon -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. +def plan(channel='c0', phase='maneuver', speed=SPEEDS[0], run_id=1, curvature=.01): + msg = log.LateralManeuverPlan.new_message(desiredCurvature=curvature) + msg.fordChannelTest = {'runId': run_id, 'channel': channel, 'phase': phase, 'speed': speed} 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)} + 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)) -def pulse(test): - for i in range(51): - result = update(test, 10.+i*.01, plan(phase='pulse', delta=AMPLITUDE['c0']) 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.)}, + {'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() - pulse(test) - assert update(test, 10.51, plan(phase='pulse', delta=AMPLITUDE['c0']), **changes) == FordPath() - assert test.aborted - assert update(test, 10.52, plan(phase='pulse', delta=AMPLITUDE['c0'])) == FordPath() - assert update(test, 10.53, plan(phase='aborted'), plan_valid=False) is None + 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('c1', 'pulse', AMPLITUDE['c1']), plan('c0', 'pulse', -AMPLITUDE['c0']), plan('c0', 'pulse', AMPLITUDE['c0']+.01), - plan('c0', 'pulse', math.nan), plan('c0', 'pulse', AMPLITUDE['c0'], run_id=2), plan('c0', 'baseline'), - plan('c0', 'pulse', AMPLITUDE['c0'], speed=SPEEDS[1]), plan('c0', 'release'), -]) -def test_midpulse_identity_or_phase_changes_abort(msg): +@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() - pulse(test) - assert update(test, 10.51, msg) == FordPath() + update(test) + assert update(test, 10.01, msg) == FordPath() -@pytest.mark.parametrize('phase,delta', [('pulse', AMPLITUDE['c0']), ('release', 0.)]) -def test_cannot_start_midrun(phase, delta): - assert update(FordChannelTest(), msg=plan(phase=phase, delta=delta)) == FordPath() +def test_legacy_raw_pulse_cannot_actuate(): + msg = plan(phase='pulse') + msg.fordChannelTest.delta = 1.28 + assert update(FordChannelTest(), msg=msg) == FordPath() -@pytest.mark.parametrize('phase', ['baseline', 'pulse', 'release']) -def test_frozen_phase_times_out_even_with_fresh_timestamps(phase): +def test_fresh_but_frozen_maneuver_times_out(): test = FordChannelTest() - result = None - for i in range(400): - stage = 'baseline' if i < 50 or phase == 'baseline' else ('pulse' if i < 150 or phase == 'pulse' else 'release') - result = update(test, 10.+i*.01, plan(phase=stage, delta=AMPLITUDE['c0'] 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 AMPLITUDE == {'c0': 1.28, 'c1': .125} - assert all(95 <= counts[run, 'pulse'] <= 105 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)} - assert [t.description for t in TRIALS] == [f'{channel} {direction} {mph} mph' - for mph in (15, 20) for channel in ('C0', 'C1') for direction in ('right', 'left')] - - -@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 + 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)) - 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) @@ -169,143 +83,155 @@ def test_mode_selection_and_transient_parameter(tmp_path): 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) -@pytest.mark.parametrize('abort_kind,abort_frame', [('brake', 200), ('angle', 75), ('pscm', 75)]) -def test_actual_controlsd_and_wire_isolate_release_then_abort(pipeline, trial, abort_kind, abort_frame): # noqa: F811 +@pytest.mark.parametrize('channel', ['c0', 'c1']) +@pytest.mark.parametrize('speed', SPEEDS) +@pytest.mark.parametrize('limited', [False, True]) +def test_real_injection_and_wire_match_normal_controller_on_selected_channel(pipeline, channel, speed, limited): # 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) + normal = startup() + isolated = startup(params=SimpleNamespace(get_bool=lambda key: key in ('FordModelActionController', 'FordChannelTestMode'))) + 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=trial.speed, vEgoRaw=trial.speed), acc_tja_status_stock_values=defaultdict(int), + 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) - baseline = None - for i in range(abort_frame+1): + 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=False, brakePressed=False, cruiseState=SimpleNamespace(enabled=True)) + for i in range(251): now = 10.+i*.01 - phase = 'baseline' if i < 50 else ('pulse' if i < 150 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 = abort_kind == 'brake' and i == abort_frame - cs.steeringAngleDeg = 15.01 if abort_kind == 'angle' and i == abort_frame else 0. - pscm.limit = 2 if abort_kind == 'pscm' and i == abort_frame else 0 - 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}) + request = (.5 if i < 100 else -.5)/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) + 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': controls, 'CC_SP': msg}) + 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'] - if baseline is None: - baseline = (wire['LatCtlPathOffst_L_Actl'], wire['LatCtlPath_An_Actl']) - actual = (wire['LatCtlPathOffst_L_Actl'], wire['LatCtlPath_An_Actl']) - if i == abort_frame: - 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['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: + assert isolated.desired_curvature < 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 - +def run_daemon(monkeypatch, interrupt=False): class Finished(Exception): pass - plans, alerts = [], [] + plans, alerts, checks = [], [], [] clock = [10.] - - subscribers = [] + 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} - subscribers.append(self) - def update(self, _timeout): + 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 - self.events['selfdriveState'].selfdriveState.enabled = clock[0] > 10. - self.events['carControl'].carControl.latActive = clock[0] > 10. - self.events['carControlSP'].carControlSP.fordLateralPath = {'enabled': True, 'valid': True} - self.events['carStateSP'].carStateSP.fordPscmStatus = {'valid': True, 'canMonoTime': round(clock[0]*1e9), 'lateralState': 2} + 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) - # SubMaster conflates 100 Hz services to the daemon's 20 Hz polling rate. - # Keep its real frequency/alive/valid checks instead of assuming health. 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, 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': + 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 - class Clock: - def __init__(self, rate): - assert rate == 20 + 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 - def keep_time(self): - clock[0] += .05 - assert clock[0] < 75., f'Test never completed; frequency checks: {subscribers[0].freq_ok}' - 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() +@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: - 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} + 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 -def test_enabled_channel_payload_is_ignored_when_toggle_off(pipeline): # noqa: F811 +@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='pulse', delta=AMPLITUDE['c0']) - sm.messages['lateralManeuverPlan'].desiredCurvature = -.1 + 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.), - 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}) - assert controls.ford_channel_test is None + '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. - 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 83e1530ef2..395b259826 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -22,7 +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.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 @@ -184,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.), - 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)} + 'use_maneuver_reference': use_maneuver_reference, '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 +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.), - 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}) + 'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}) assert controls.ford_path.valid == cc.latActive == (failed == 'lateralManeuverPlan' and not maneuver) @@ -257,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.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda now=now: now)} exec(call, environment) msg = custom.CarControlSP.new_message() @@ -300,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.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, '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 +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.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, '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 +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.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, '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 +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.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature, + 'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, '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 +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.), 'is_channel_plan': is_channel_plan, 'clip_curvature': clip_curvature}) + 'lp': SimpleNamespace(roll=0.), 'use_maneuver_reference': use_maneuver_reference, '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/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md b/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md index c1597634a9..bd9fb13fac 100644 --- a/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md +++ b/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md @@ -1,23 +1,23 @@ -# Ford C0 / C1 comparison +# Ford lateral maneuvers, one output channel at a time -This separate diagnostic measures the wheel response to C0 and C1 pulses at **15 and 20 mph**. It requires the Ford model-action controller and a CAN FD Ford. It makes no change to normal controller tuning. +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. -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 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 15 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 1 second, 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 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. -The pulse is **25% of each field's maximum magnitude**: **±1.28 m C0** (25% of 5.11 m, rounded to the 1 cm CAN step) or **±0.125 rad C1** (25% of 0.5 rad), added to the captured starting command. These are path fields, not wheel angles or percentages of steering torque. The amplitudes and duration were increased after the initial 0.03 m / 0.01 rad, 0.5-second probes produced little reported wheel movement. Equal percentages are not assumed to produce equivalent steering. 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. +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**. -Driver input, lost ACC/lateral engagement, invalid or stale inputs, PSCM denial/limit status, or excessive response abort the test. The diagnostic additionally stops requesting test control above 15° wheel movement from baseline or 1 m/s² measured lateral acceleration. A pulse may therefore abort before its full second. These thresholds trigger an abort after measured movement; they do not guarantee the physical response cannot overshoot them, model the PSCM, or change normal driving limits. +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. -Generate the report with the existing command: +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 ``` -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. +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. diff --git a/openpilot/tools/lateral_maneuvers/ford_maneuversd.py b/openpilot/tools/lateral_maneuvers/ford_maneuversd.py index fffe576c3e..92f3bd889d 100644 --- a/openpilot/tools/lateral_maneuvers/ford_maneuversd.py +++ b/openpilot/tools/lateral_maneuvers/ford_maneuversd.py @@ -1,149 +1,7 @@ #!/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, frequency=20) - 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() +"""Run the existing lateral maneuver suite through one Ford output at a time.""" +from openpilot.tools.lateral_maneuvers.lateral_maneuversd import main if __name__ == '__main__': - main() + main(ford_channels=True) diff --git a/openpilot/tools/lateral_maneuvers/ford_report.py b/openpilot/tools/lateral_maneuvers/ford_report.py index 078ee78570..a45e2049a3 100644 --- a/openpilot/tools/lateral_maneuvers/ford_report.py +++ b/openpilot/tools/lateral_maneuvers/ford_report.py @@ -211,3 +211,35 @@ def report(platform, route, CP, ID, runs, output_dir=None): html.append(f'{escape(title)} command and wheel response') 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 diff --git a/openpilot/tools/lateral_maneuvers/generate_report.py b/openpilot/tools/lateral_maneuvers/generate_report.py index 6fd7c3bee5..e6ba28afed 100755 --- a/openpilot/tools/lateral_maneuvers/generate_report.py +++ b/openpilot/tools/lateral_maneuvers/generate_report.py @@ -14,11 +14,12 @@ 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, report as ford_channel_report +from openpilot.tools.lateral_maneuvers.ford_report import ChannelRuns, channel_commands, report as ford_channel_report def lat_accel(curvature, v): @@ -59,33 +60,42 @@ 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 + 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 - 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_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_lateralPlan = [(t - t_lateralPlan[0]) / 1e9 for t in t_lateralPlan] - t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput] + t_carOutput = [(t - (origin if channel else 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) + maneuver_valid = all(latActive) and not any(cs.steeringPressed for cs in carState) and not command_problems _open = 'open' if maneuver_valid else '' title = f'Run #{int(run)+1}' + (' (invalid maneuver!)' if not maneuver_valid else '') builder.append(f"

{title}

\n") + if channel: + builder.append(f'

Normal maneuver target through {channel.upper()} only; normal controller feedback remains active.

') + if command_problems: + builder.append(f'

CAN validation: {", ".join(sorted(command_problems))}

') 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_ego, strict=False)) + for lp, v in zip(lateralPlan, v_plan, strict=False)) threshold = amplitude * 0.5 builder.append('

50% peak') - for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False): + for t, cs, v in zip(t_controlsState, controlsState, v_controls, strict=False): actual = lat_accel(cs.curvature, v) - baseline_accel if abs(actual) > threshold: builder.append(f', crossed in {t:.3f}s') @@ -99,10 +109,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_ego[0]) - baseline_accel)] - for i in range(1, min(len(lateralPlan), len(v_ego))): + action_targets = [(0, lat_accel(lateralPlan[0].desiredCurvature, v_plan[0]) - baseline_accel)] + for i in range(1, min(len(lateralPlan), len(v_plan))): if abs(lateralPlan[i].desiredCurvature - lateralPlan[i - 1].desiredCurvature) > 0.001: - desired = lat_accel(lateralPlan[i].desiredCurvature, v_ego[i]) - baseline_accel + desired = lat_accel(lateralPlan[i].desiredCurvature, v_plan[i]) - baseline_accel action_targets.append((i, desired)) for j, (start_i, act_target) in enumerate(action_targets): @@ -111,7 +121,7 @@ def report(platform, route, _description, CP, ID, maneuvers): builder.append(f'

aTarget: {round(act_target, 1)} m/s^2') prev_crossed = False - for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False): + for t, cs, v in zip(t_controlsState, controlsState, v_controls, strict=False): if not (start_time <= t <= end_time): continue actual_accel = lat_accel(cs.curvature, v) - baseline_accel @@ -131,19 +141,19 @@ def report(platform, route, _description, CP, ID, maneuvers): target_cross_times.setdefault(description, []) plt.rcParams['font.size'] = 40 - fig = plt.figure(figsize=(30, 40)) - ax = fig.subplots(5, 1, sharex=True, gridspec_kw={'height_ratios': [5, 5, 3, 3, 3]}) + fig = plt.figure(figsize=(30, 50 if channel else 40)) + ax = fig.subplots(7 if channel else 5, 1, sharex=True, gridspec_kw={'height_ratios': [5, 5, 3, 3, 3] + ([3, 3] if channel else [])}) 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_ego, strict=False)] + desired_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_plan, 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_ego, strict=False)] + actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controlsState, v_controls, 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: @@ -161,6 +171,17 @@ 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}) 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) diff --git a/openpilot/tools/lateral_maneuvers/lateral_maneuversd.py b/openpilot/tools/lateral_maneuvers/lateral_maneuversd.py index ed07c896f9..7d3ee8375f 100755 --- a/openpilot/tools/lateral_maneuvers/lateral_maneuversd.py +++ b/openpilot/tools/lateral_maneuvers/lateral_maneuversd.py @@ -9,6 +9,7 @@ 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 @@ -20,6 +21,7 @@ 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 @@ -100,21 +102,34 @@ MANEUVERS = [ ] -def main(): +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): params = Params() cloudlog.info("lateral_maneuversd is waiting for CarParams") messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2') + services = ['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'] + if ford_channels: + services += ['carStateSP', 'vehicleParameters'] + sm = messaging.SubMaster(services, poll='modelV2') pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug']) - maneuvers = iter(MANEUVERS) + maneuvers = iter(channel_maneuvers() if ford_channels else 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() @@ -138,16 +153,19 @@ def main(): elif maneuver is not None: # any driver input aborts the maneuver CS = sm['carState'] - if CS.steeringPressed or CS.gasPressed: + 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: aborted_cnt = int(1.0 / DT_MDL) - abort_reason = ('steering pressed' if CS.steeringPressed else 'gas pressed').ljust(20) + 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) 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, curvature, roll) + accel = maneuver.get_accel(v_ego, sm['carControl'].latActive and not (ford_channels and (invalid or aborted)), curvature, roll) if maneuver._run_completed: complete_cnt = int(1.0 / DT_MDL) @@ -191,9 +209,18 @@ def main(): pm.send('alertDebug', alert_msg) - plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0 + plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0 and (not ford_channels or not maneuver.finished) 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: diff --git a/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py b/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py index 4611104583..3c9eaec073 100644 --- a/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py +++ b/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py @@ -7,10 +7,13 @@ 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.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 @@ -114,3 +117,75 @@ def test_incomplete_missing_and_bad_checksum(): 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']) +def test_normal_channel_report_uses_real_targets_and_decoded_output(channel, 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} + 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)) + 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 + assert captured == [['Lateral Accel (m/s^2)', 'Wheel angle (deg)', 'Velocity (mph)', 'Jerk (m/s^3)', 'Roll (deg)', 'C0 sent (m)', 'C1 sent (rad)']] + 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]