diff --git a/openpilot/cereal/log.capnp b/openpilot/cereal/log.capnp index a4537a4d48..d60758e112 100644 --- a/openpilot/cereal/log.capnp +++ b/openpilot/cereal/log.capnp @@ -1228,6 +1228,9 @@ struct LateralManeuverPlan { phase @2 :Phase; delta @3 :Float32; # legacy raw-pulse increment; zero for normal channel maneuvers speed @4 :Float32; # target m/s + keyboardRequestId @5 :UInt64; # nonzero for a manually triggered normal-controller step + keyboardPhase @6 :Phase; # baseline/pulse/release inside phase=maneuver + targetAccel @7 :Float32; # signed keyboard step amplitude, m/s^2 (not a raw field increment) enum Channel { none @0; c0 @1; c1 @2; } enum Phase { waiting @0; baseline @1; pulse @2; release @3; complete @4; aborted @5; maneuver @6; } @@ -2131,6 +2134,16 @@ struct Joystick { # convenient for debug and live tuning axes @0: List(Float32); buttons @1: List(Bool); + fordKeyboard @2 :FordKeyboard; + + struct FordKeyboard { + requestId @0 :UInt64; # changes only for a new action; heartbeats repeat the same request + channel @1 :LateralManeuverPlan.FordChannelTest.Channel; + speed @2 :Float32; + accel @3 :Float32; + action @4 :Action; + enum Action { idle @0; left @1; right @2; cancel @3; } + } } struct DriverStateV2 { diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index b7e37e020f..9e1f2cc746 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -86,6 +86,7 @@ inline static std::unordered_map keys = { {"LocationFilterInitialState", {PERSISTENT, BYTES}}, {"LateralManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, {"FordChannelTestMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0"}}, + {"FordChannelKeyboardMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0"}}, {"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, {"LongitudinalPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast(cereal::LongitudinalPersonality::STANDARD))}}, {"NetworkMetered", {PERSISTENT | BACKUP, BOOL}}, diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index b48e9153da..e9e35dfe5b 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -59,7 +59,8 @@ 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 + self.ford_channel_test = (FordChannelTest(keyboard=self.params.get_bool('FordChannelKeyboardMode')) + if ford_channel_test_selected(self.CP, self.params) else None) if self.CP.brand == "ford": cloudlog.event("Ford path controller selected", controller=type(self.ford_path_controller).__name__ if self.ford_model_action else "upstream") @@ -186,8 +187,10 @@ class Controls(ControlsExt): 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, + healthy=(CS.cruiseState.enabled or self.ford_channel_test.keyboard) + and self.sm.all_checks(['carStateSP', 'carState', 'vehicleParameters', 'modelV2']), + driver_input=CS.steeringPressed or not math.isfinite(CS.steeringTorque) or abs(CS.steeringTorque) > 1. + or (CS.gasPressed and not self.ford_channel_test.keyboard) or CS.brakePressed, speed=CS.vEgo, ) if override is not None: diff --git a/openpilot/selfdrive/controls/lib/ford_channel_test.py b/openpilot/selfdrive/controls/lib/ford_channel_test.py index a6bc90838a..86751286f3 100644 --- a/openpilot/selfdrive/controls/lib/ford_channel_test.py +++ b/openpilot/selfdrive/controls/lib/ford_channel_test.py @@ -6,15 +6,16 @@ from openpilot.common.constants import CV from openpilot.selfdrive.controls.lib.ford_path import FordPath PARAM = 'FordChannelTestMode' +KEYBOARD_PARAM = 'FordChannelKeyboardMode' CONFLICTS = ('LateralManeuverMode', 'LongitudinalManeuverMode', 'JoystickDebugMode') SPEEDS = (15.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS) MAX_SPEED_ERROR = .7 -MAX_RUN_S = 4. # 2.6 s nominal, with room for the accepted 16–24 Hz model cadence +MAX_RUN_S = 4. # normal suite ~2.6 s, keyboard step 3 s; allow scheduling/cadence variation def selected(CP, params): return bool(CP.brand == 'ford' and CP.flags & FordFlags.CANFD and params.get_bool('FordModelActionController') - and params.get_bool(PARAM) and not any(params.get_bool(key) for key in CONFLICTS)) + and (params.get_bool(PARAM) or params.get_bool(KEYBOARD_PARAM)) and not any(params.get_bool(key) for key in CONFLICTS)) def is_channel_plan(plan): @@ -36,7 +37,8 @@ class FordChannelTest: 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): + def __init__(self, keyboard=False): + self.keyboard = keyboard self.clear() def clear(self): @@ -59,7 +61,7 @@ class FordChannelTest: return FordPath() if not fresh or not plan_valid: return self.fail('stale maneuver plan') - if not is_channel_maneuver(plan) or test.runId == 0 or test.delta != 0.: + if not is_channel_maneuver(plan) or test.runId == 0 or test.delta != 0. or bool(test.keyboardRequestId) != self.keyboard: return self.fail('invalid maneuver identity') if not active or not healthy or not normal.valid or driver_input: return self.fail('driver input, disengagement or invalid service') @@ -76,5 +78,7 @@ class FordChannelTest: command = FordPath(True, normal.path_offset if self.channel == 'c0' else 0., normal.path_angle if self.channel == 'c1' else 0., 0., 0.) self.diagnostics = {'status': 'active', 'run_id': self.run_id, 'channel': self.channel, 'desired_curvature': plan.desiredCurvature, 'target_speed': test.speed, + 'keyboard_request_id': test.keyboardRequestId, 'keyboard_phase': str(test.keyboardPhase), + 'target_accel': test.targetAccel, 'command': (command.path_offset, command.path_angle, 0., 0.)} return command diff --git a/openpilot/selfdrive/controls/tests/test_ford_channel_test.py b/openpilot/selfdrive/controls/tests/test_ford_channel_test.py index 0e306220a7..132dbf6e2f 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_channel_test.py +++ b/openpilot/selfdrive/controls/tests/test_ford_channel_test.py @@ -21,9 +21,9 @@ from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import from openpilot.tools.lateral_maneuvers import lateral_maneuversd as daemon -def plan(channel='c0', phase='maneuver', speed=SPEEDS[0], run_id=1, curvature=.01): +def plan(channel='c0', phase='maneuver', speed=SPEEDS[0], run_id=1, curvature=.01, keyboard_request_id=0): msg = log.LateralManeuverPlan.new_message(desiredCurvature=curvature) - msg.fordChannelTest = {'runId': run_id, 'channel': channel, 'phase': phase, 'speed': speed} + msg.fordChannelTest = {'runId': run_id, 'channel': channel, 'phase': phase, 'speed': speed, 'keyboardRequestId': keyboard_request_id} with log.LateralManeuverPlan.from_bytes(msg.to_bytes()) as reader: return reader.as_builder() @@ -88,10 +88,12 @@ def test_mode_selection_and_transient_parameter(tmp_path): @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 +@pytest.mark.parametrize('keyboard', [False, True]) +def test_real_injection_and_wire_match_normal_controller_on_selected_channel(pipeline, channel, speed, limited, keyboard): # noqa: F811 call, publication = pipeline normal = startup() - isolated = startup(params=SimpleNamespace(get_bool=lambda key: key in ('FordModelActionController', 'FordChannelTestMode'))) + mode = 'FordChannelKeyboardMode' if keyboard else 'FordChannelTestMode' + isolated = startup(params=SimpleNamespace(get_bool=lambda key: key in ('FordModelActionController', mode))) controllers = (normal, isolated) cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint=normal.CP.carFingerprint) sender = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP()) @@ -103,13 +105,16 @@ def test_real_injection_and_wire_match_normal_controller_on_selected_channel(pip 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)) + gasPressed=keyboard, brakePressed=False, cruiseState=SimpleNamespace(enabled=not keyboard)) for i in range(251): now = 10.+i*.01 request = (.5 if i < 100 else -.5)/speed**2 + if keyboard: + request = (.5 if 50 <= i < 150 else 0.)/speed**2 for control in controllers: sm = control.sm - sm.messages['lateralManeuverPlan'] = plan(channel=channel if control is isolated else 'none', curvature=request, speed=speed) + sm.messages['lateralManeuverPlan'] = plan(channel=channel if control is isolated else 'none', curvature=request, speed=speed, + keyboard_request_id=1 if keyboard and control is isolated else 0) sm.logMonoTime = dict.fromkeys(sm.logMonoTime, round(now*1e9)) status = sm['carStateSP'].fordPscmStatus status.valid, status.canMonoTime, status.lateralState, status.limit = True, round(now*1e9), 2, 2 if limited else 0 @@ -137,7 +142,10 @@ def test_real_injection_and_wire_match_normal_controller_on_selected_channel(pip if i == 50: assert isolated.desired_curvature > 0. if i == 200: - assert isolated.desired_curvature < 0. + if keyboard: + assert isolated.desired_curvature == 0. + else: + assert isolated.desired_curvature < 0. def run_daemon(monkeypatch, interrupt=False): diff --git a/openpilot/selfdrive/ui/layouts/settings/developer.py b/openpilot/selfdrive/ui/layouts/settings/developer.py index ef9c7f9e03..72a373d2e6 100644 --- a/openpilot/selfdrive/ui/layouts/settings/developer.py +++ b/openpilot/selfdrive/ui/layouts/settings/developer.py @@ -167,6 +167,7 @@ class DeveloperLayout(Widget): self._params.put_bool("SshEnabled", state, block=True) def _on_joystick_debug_mode(self, state: bool): + self._params.put_bool('FordChannelKeyboardMode', False, block=True) self._params.put_bool("JoystickDebugMode", state, block=True) self._params.put_bool("LongitudinalManeuverMode", False, block=True) self._long_maneuver_toggle.action_item.set_state(False) @@ -174,6 +175,7 @@ class DeveloperLayout(Widget): self._lat_maneuver_toggle.action_item.set_state(False) def _on_long_maneuver_mode(self, state: bool): + self._params.put_bool('FordChannelKeyboardMode', False, block=True) self._params.put_bool("LongitudinalManeuverMode", state, block=True) self._params.put_bool("JoystickDebugMode", False, block=True) self._joystick_toggle.action_item.set_state(False) @@ -181,6 +183,7 @@ class DeveloperLayout(Widget): self._lat_maneuver_toggle.action_item.set_state(False) def _on_lat_maneuver_mode(self, state: bool): + self._params.put_bool('FordChannelKeyboardMode', False, block=True) self._params.put_bool("LateralManeuverMode", state, block=True) self._params.put_bool("ExperimentalMode", False, block=True) self._params.put_bool("JoystickDebugMode", False, block=True) diff --git a/openpilot/selfdrive/ui/mici/layouts/settings/developer.py b/openpilot/selfdrive/ui/mici/layouts/settings/developer.py index 504f625cc9..0e948dcbe5 100644 --- a/openpilot/selfdrive/ui/mici/layouts/settings/developer.py +++ b/openpilot/selfdrive/ui/mici/layouts/settings/developer.py @@ -203,9 +203,11 @@ class DeveloperLayoutMici(NavScroller): def _clear_ford_channel_test(self): ui_state.params.put_bool('FordChannelTestMode', False, block=True) + ui_state.params.put_bool('FordChannelKeyboardMode', False, block=True) self._ford_channel_test_toggle.set_checked(False) def _on_ford_channel_test(self, state: bool): + ui_state.params.put_bool('FordChannelKeyboardMode', False, block=True) ui_state.params.put_bool('FordChannelTestMode', state, block=True) for key, toggle in (('LateralManeuverMode', self._lat_maneuver_toggle), ('LongitudinalManeuverMode', self._long_maneuver_toggle), ('JoystickDebugMode', self._joystick_toggle)): diff --git a/openpilot/tools/joystick/README.md b/openpilot/tools/joystick/README.md index 3ce308927c..ec92d4a8f6 100644 --- a/openpilot/tools/joystick/README.md +++ b/openpilot/tools/joystick/README.md @@ -5,6 +5,9 @@ With joystick_control, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard. joystick_control uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks. +For the custom Ford C0/C1 controller, use the [Ford keyboard channel test](../lateral_maneuvers/FORD_CHANNEL_TEST.md#keyboard-triggered-steps-acc-or-mads). +It preserves the normal controller and ACC/MADS, unlike stock Joystick Debug Mode below. + ## Usage The car must be off, and openpilot must be offroad before starting `joystick_control`. diff --git a/openpilot/tools/joystick/ford_keyboard_control.py b/openpilot/tools/joystick/ford_keyboard_control.py new file mode 100644 index 0000000000..7253ba4e1d --- /dev/null +++ b/openpilot/tools/joystick/ford_keyboard_control.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 +"""Keyboard frontend for Ford isolated-channel tests. Keeps controlsd and ACC/MADS running.""" +import argparse +import math +import time + +from opendbc.car.ford.values import FordFlags +from opendbc.car.structs import car +from openpilot.cereal import log, messaging +from openpilot.common.constants import CV +from openpilot.common.params import Params +from openpilot.common.realtime import Ratekeeper +from openpilot.selfdrive.controls.lib.ford_channel_test import CONFLICTS, KEYBOARD_PARAM, PARAM, SPEEDS +from openpilot.tools.lateral_maneuvers.ford_keyboard import MIN_ACCEL, MAX_ACCEL, ACCEL_INCREMENT +from openpilot.tools.lib.kbhit import KBHit + +HELP = '1 C0 | 2 C1 | V 15/20 mph | +/- strength | A left step | D right step | R cancel | Q quit' + + +class KeyboardControl: + def __init__(self, speed=15, accel=.5): + if speed not in (15, 20) or not math.isfinite(accel) or not MIN_ACCEL <= accel <= MAX_ACCEL: + raise ValueError(f'Use 15/20 mph and {MIN_ACCEL}–{MAX_ACCEL} m/s²') + self.request = log.Joystick.FordKeyboard.new_message(channel='c0', speed=speed*CV.MPH_TO_MS, accel=accel, action='idle') + self.last_key, self.last_key_time = '', -math.inf + + def action(self, action, now): + self.request.requestId = max(self.request.requestId+1, int(now*1e9)) + self.request.action = action + + def key(self, key, now): + key = key.lower() + repeated = key == self.last_key and now-self.last_key_time < .5 + self.last_key, self.last_key_time = key, now + if key in ('a', 'd'): + if not repeated: + self.action('left' if key == 'a' else 'right', now) + elif key in ('r', 'q'): + self.action('cancel', now) + else: + self.request.action = 'idle' + if key in ('1', '2'): + self.request.channel = 'c0' if key == '1' else 'c1' + elif key == 'v': + self.request.speed = SPEEDS[1] if abs(self.request.speed-SPEEDS[0]) < 1e-5 else SPEEDS[0] + elif key in ('+', '=', '-', '_'): + increment = ACCEL_INCREMENT if key in ('+', '=') else -ACCEL_INCREMENT + self.request.accel = min(MAX_ACCEL, max(MIN_ACCEL, self.request.accel+increment)) + return key != 'q' + + def message(self): + msg = messaging.new_message('testJoystick') + msg.valid = True + # Normal joystick consumers receive no gas/brake or steering-axis request. + msg.testJoystick.axes = [0., 0.] + msg.testJoystick.fordKeyboard = self.request + return msg + + +def enable(params): + offroad = params.get_bool('IsOffroad') + if not offroad and (not params.get_bool(KEYBOARD_PARAM) or any(params.get_bool(k) for k in (*CONFLICTS, PARAM))): + raise ValueError('Start this tool while the car is off and openpilot is offroad.') + if not params.get_bool('FordModelActionController'): + raise ValueError('Enable the Ford model-action controller first.') + raw = params.get('CarParamsPersistent') + if not raw: + raise ValueError('Drive once to identify the car before enabling this test.') + cp = messaging.log_from_bytes(raw, car.CarParams) + if cp.brand != 'ford' or not cp.flags & FordFlags.CANFD: + raise ValueError('This test requires a CAN FD Ford.') + if not offroad: + return # Reconnect to an already-selected keyboard mode without changing any driving mode. + for key in (*CONFLICTS, PARAM): + params.put_bool(key, False, block=True) + params.put_bool(KEYBOARD_PARAM, True, block=True) + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--speed', type=int, choices=(15, 20), default=15, help='target mph (default: 15)') + parser.add_argument('--accel', type=float, default=.5, help=f'step strength in m/s², {MIN_ACCEL}–{MAX_ACCEL} (default: 0.5)') + args = parser.parse_args() + params = Params() + try: + control = KeyboardControl(args.speed, args.accel) + kb = KBHit() + enable(params) + except (ValueError, OSError) as exc: + parser.exit(1, f'{exc}\n') + pm = messaging.PubMaster(['testJoystick']) + sm = messaging.SubMaster(['alertDebug', 'carState', 'carStateSP', 'carControlSP']) + rk = Ratekeeper(20, print_delay_threshold=None) + print(f'\nFord keyboard test connected. Use ACC or MADS with manual throttle.\n{HELP}\n', flush=True) + print('Strength is a maneuver target, not a percentage of C0/C1. R cancels the test; normal lateral control remains active.', flush=True) + running = True + try: + while running: + # Bound input work so pasted text cannot stall the heartbeat/watchdog. + for _ in range(32): + if not kb.kbhit(): + break + key = kb.getch() + if not key: + running = False + break + running = control.key(key, time.monotonic()) + if not running: + break + if not params.get_bool(KEYBOARD_PARAM): + print('\nKeyboard test mode cleared; exiting.', flush=True) + break + pm.send('testJoystick', control.message()) + sm.update(0) + if rk.frame % 10 == 0: + req, cs = control.request, sm['carState'] + path, pscm = sm['carControlSP'].fordLateralPath, sm['carStateSP'].fordPscmStatus + telemetry = (f'C0 {path.pathOffset:+.2f}m C1 {path.pathAngle:+.4f}rad | wheel {cs.steeringAngleDeg:+.1f}° | limit {pscm.limit}' + if sm.all_alive() and sm.all_valid() else 'Waiting for live vehicle data') + print(f'{str(req.channel).upper()} | {req.speed*CV.MS_TO_MPH:.0f}mph | {req.accel:.2f}m/s² | ' + + f'{sm["alertDebug"].alertText1} | {telemetry}', flush=True) + rk.keep_time() + except KeyboardInterrupt: + pass + finally: + control.action('cancel', time.monotonic()) + pm.send('testJoystick', control.message()) + kb.set_normal_term() + # Keep the daemon alive onroad so it can explicitly publish an inactive plan; + # otherwise controlsd would only see the last, now-stale active plan. + if params.get_bool('IsOffroad'): + params.put_bool(KEYBOARD_PARAM, False, block=True) + print('\nKeyboard disconnected; no further steps will start.', flush=True) + + +if __name__ == '__main__': + main() diff --git a/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md b/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md index bd9fb13fac..f0cf2b7ad8 100644 --- a/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md +++ b/openpilot/tools/lateral_maneuvers/FORD_CHANNEL_TEST.md @@ -21,3 +21,34 @@ python openpilot/tools/lateral_maneuvers/generate_report.py DEVICE/ROUTE ``` Channel runs use the standard lateral maneuver report, grouped by maneuver, speed and channel. It shows desired versus measured lateral acceleration, actual wheel angle, speed/jerk/roll, and decoded C0/C1 CAN commands. The report checks that the unused field remained zero. Archived raw-pulse routes still use their original report. + +## Keyboard-triggered steps (ACC or MADS) + +A laptop keyboard over SSH is enough. This mode keeps normal `controlsd` running, including ACC/MADS and the tested Ford formulas. **Do not enable stock Joystick Debug Mode:** that replaces `controlsd` and bypasses this controller. The keyboard tool selects its own transient mode and disables the automatic maneuver modes. + +With the car off and openpilot offroad, SSH into the comma and run from `/data/openpilot`: + +```sh +python openpilot/tools/joystick/ford_keyboard_control.py --speed 15 +``` + +Leave that terminal connected, start the car, and engage lateral control. You may use **ACC**, or **MADS with manual accelerator input**. Hold the selected speed, 15 or 20 mph, within ±0.7 m/s (about ±1.6 mph). Braking, steering intervention, loss of lateral engagement, invalid data, or leaving the speed window aborts the current run. MADS is engaged normally; this tool does not enable it or override its engagement rules. Use a passenger to operate the keyboard while you drive in the test area. + +| Key | Action | +| --- | --- | +| `1` / `2` | Select C0-only / C1-only for the next step | +| `V` | Select 15 / 20 mph for the next step | +| `+` / `-` | Adjust the next target by 0.25 m/s²; default 0.5, range 0.25–3.0 | +| `A` / `D` | Trigger one left / right step when the screen says Ready | +| `R` | Cancel the test and return to normal lateral control | +| `Q` or Ctrl-C | Cancel and exit the keyboard tool | + +The strength is a **lateral-acceleration target**, not a percentage of the C0/C1 field range. The upper setting matches the existing normal lateral-acceleration limit; normal curvature/jerk limits and field bounds still apply. For a different starting strength, add `--accel 1.0` to the command. Settings changed during a run apply only to the next run. + +Each keypress starts one **0.5 s baseline → 1.0 s step → 1.5 s release** sequence. The reference is the captured starting curvature plus the requested acceleration divided by current speed squared. Release returns the **target** to that baseline; it does not forcibly zero C1's normal P/I correction. The unused channel and C2/C3 remain zero throughout the sequence. Afterward, normal model following resumes with both channels. The terminal displays computed C0/C1, measured wheel angle, and PSCM limit status; the route also records actual transmitted CAN commands. + +The tool requires two seconds of stable, straight driving before accepting a trigger. A keypress while unready or busy is discarded, not queued. **It never automatically starts another run or retries an interrupted one.** Losing the keyboard heartbeat for more than 0.2 s aborts the run; the normal controller also retains its independent stale-plan/run-duration checks. `R` and `Q` cancel only the test, not MADS or ACC. Disengage lateral control normally if you want it off. The keyboard mode clears when the car goes offroad or manager restarts. + +If SSH disconnects or you exit the keyboard program, reconnect and run the same command. Reconnecting onroad is accepted only when keyboard mode is already selected; it does not enable a new driving mode. An interrupted step stays aborted and needs a fresh trigger. + +Use the same report command above after uploading the route. Keyboard runs add a PSCM limit-status plot and markers for the baseline/step/release transitions. These test the real controller with one output selected; C1 feedback and the vehicle response are both part of the measurement. diff --git a/openpilot/tools/lateral_maneuvers/ford_keyboard.py b/openpilot/tools/lateral_maneuvers/ford_keyboard.py new file mode 100644 index 0000000000..f8b0ac0a68 --- /dev/null +++ b/openpilot/tools/lateral_maneuvers/ford_keyboard.py @@ -0,0 +1,133 @@ +"""Manually triggered curvature steps through the normal Ford channel test path.""" +import math +import time + +from openpilot.cereal import messaging +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL, Ratekeeper +from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.controls.lib.drive_helpers import MAX_LATERAL_ACCEL_NO_ROLL, MIN_SPEED +from openpilot.selfdrive.controls.lib.ford_channel_test import SPEEDS, MAX_SPEED_ERROR +from openpilot.tools.lateral_maneuvers.lateral_maneuversd import MAX_CURV, MAX_ROLL, TIMER + +INPUT_TIMEOUT = .2 +ACCEL_INCREMENT = .25 +MIN_ACCEL, MAX_ACCEL = ACCEL_INCREMENT, MAX_LATERAL_ACCEL_NO_ROLL +BASELINE_S, STEP_S, RELEASE_S = .5, 1., 1.5 + + +class KeyboardManeuver: + def __init__(self): + self.last_request_id = 0 + self.ready_since = self.start = self.last_update = None + self.request_id = self.run_id = 0 + self.channel, self.speed, self.accel, self.baseline = 'none', SPEEDS[0], 0., 0. + self.phase, self.status = 'waiting', 'Waiting for keyboard' + + @property + def active(self): + return self.start is not None + + def stop(self, status, phase='aborted'): + self.start = self.ready_since = None + self.phase, self.status = phase, status + + def update(self, now, request, *, input_time, input_valid, healthy, ready, speed, curvature): + # Consume every new request, including ones received while busy/unready/stale. + # A rejected keypress must never become a delayed automatic start. + new_request = request.requestId > self.last_request_id + self.last_request_id = max(request.requestId, self.last_request_id) + valid_input = (input_valid and math.isfinite(now) and math.isfinite(input_time) + and -.005 <= now-input_time <= INPUT_TIMEOUT + and request.channel in ('c0', 'c1') and request.action in ('idle', 'left', 'right', 'cancel') + and math.isfinite(request.speed) and any(abs(request.speed-v) < 1e-5 for v in SPEEDS) + and math.isfinite(request.accel) and MIN_ACCEL-1e-6 <= request.accel <= MAX_ACCEL+1e-6) + continuous = self.last_update is None or 0. < now-self.last_update <= .15 + self.last_update = now + if not valid_input or not healthy or not continuous or not all(math.isfinite(v) for v in (speed, curvature)): + self.stop('Aborted: input/data lost' if self.active else 'Waiting for keyboard and active lateral control') + return + if new_request and request.action == 'cancel': + self.stop('Aborted: keyboard cancel') + return + if self.active: + if abs(speed-self.speed) > MAX_SPEED_ERROR: + self.stop('Aborted: speed out of range') + else: + elapsed = now-self.start + if elapsed >= BASELINE_S+STEP_S+RELEASE_S: + self.stop('Complete', 'complete') + else: + self.phase = 'baseline' if elapsed < BASELINE_S else ('pulse' if elapsed < BASELINE_S+STEP_S else 'release') + self.status = f'Active {self.channel.upper()} {self.phase}' + return + if not ready or abs(speed-request.speed) > MAX_SPEED_ERROR: + self.ready_since = None + self.status = f'Set {request.speed*CV.MS_TO_MPH:.0f} mph; straight and steady' + else: + if self.ready_since is None: + self.ready_since = now + self.status = 'Ready: A left / D right' if now-self.ready_since >= TIMER else 'Waiting: steady for 2 seconds' + if new_request and request.action in ('left', 'right'): + if self.ready_since is None or now-self.ready_since < TIMER: + self.status = 'Not ready; press A/D again when ready' + return + self.start = now + self.run_id += 1 + self.request_id = request.requestId + self.channel, self.speed = str(request.channel), request.speed + # Ford's desired-curvature coordinates are right-positive, like normal lat man. + self.accel = request.accel * (-1 if request.action == 'left' else 1) + self.baseline = curvature + self.phase, self.status = 'baseline', f'Active {self.channel.upper()} baseline' + + def plan(self, speed): + msg = messaging.new_message('lateralManeuverPlan') + msg.valid = self.active + plan = msg.lateralManeuverPlan + if self.active: + plan.desiredCurvature = self.baseline + (self.accel if self.phase == 'pulse' else 0.) / max(speed, MIN_SPEED)**2 + plan.fordChannelTest = {'runId': self.run_id, 'channel': self.channel, 'speed': self.speed, + 'phase': 'maneuver' if self.active else self.phase, 'delta': 0., + 'keyboardRequestId': self.request_id, 'keyboardPhase': self.phase, 'targetAccel': self.accel} + return msg + + @property + def description(self): + direction = 'right' if self.accel > 0 else 'left' + return f'step {direction} {abs(self.accel):.2f}m/s² {self.speed*CV.MS_TO_MPH:.0f}mph {self.channel.upper()} keyboard' + + +def main(): + services = ['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2', 'carStateSP', 'vehicleParameters'] + sm = messaging.SubMaster(services+['testJoystick'], frequency=1./DT_MDL) + pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug']) + test = KeyboardManeuver() + rk = Ratekeeper(1./DT_MDL, print_delay_threshold=None) + previous = None + while True: + sm.update(0) + cs, cc, status = sm['carState'], sm['carControl'], sm['carStateSP'].fordPscmStatus + now = time.monotonic() + healthy = (sm.all_checks(services) and cs.canValid and cc.latActive + and not (cs.steerFaultTemporary or cs.steerFaultPermanent or cs.steeringPressed or cs.brakePressed) + and math.isfinite(cs.steeringTorque) and abs(cs.steeringTorque) <= 1.) + curvature, roll = sm['controlsState'].desiredCurvature, sm['vehicleParameters'].roll + ready = (abs(curvature) < MAX_CURV and abs(sm['controlsState'].curvature) < MAX_CURV and abs(roll) < MAX_ROLL + and status.valid and -.005 <= now-status.canMonoTime*1e-9 <= .15 and status.lateralState == 2 + and not status.denied and status.limit != 3) + test.update(now, sm['testJoystick'].fordKeyboard, input_time=sm.logMonoTime['testJoystick']*1e-9, + input_valid=sm.valid['testJoystick'], healthy=healthy, ready=ready, speed=cs.vEgo, curvature=curvature) + alert = messaging.new_message('alertDebug') + alert.valid = True + alert.alertDebug.alertText1 = test.status + alert.alertDebug.alertText2 = test.description if test.run_id else 'Ford keyboard C0 / C1 test' + pm.send('alertDebug', alert) + pm.send('lateralManeuverPlan', test.plan(cs.vEgo)) + identity = (test.run_id, test.phase, test.status) + if identity != previous: + cloudlog.event('Ford keyboard test', run_id=test.run_id, request_id=test.request_id, phase=test.phase, status=test.status, + channel=test.channel, target_accel=test.accel, speed=cs.vEgo, wheel_angle=cs.steeringAngleDeg, + pscm_limit=status.limit) + previous = identity + rk.keep_time() diff --git a/openpilot/tools/lateral_maneuvers/ford_maneuversd.py b/openpilot/tools/lateral_maneuvers/ford_maneuversd.py index 92f3bd889d..8d13a5b2ab 100644 --- a/openpilot/tools/lateral_maneuvers/ford_maneuversd.py +++ b/openpilot/tools/lateral_maneuvers/ford_maneuversd.py @@ -1,7 +1,17 @@ #!/usr/bin/env python3 """Run the existing lateral maneuver suite through one Ford output at a time.""" -from openpilot.tools.lateral_maneuvers.lateral_maneuversd import main +from openpilot.tools.lateral_maneuvers.lateral_maneuversd import main as suite_main +from openpilot.common.params import Params +from openpilot.selfdrive.controls.lib.ford_channel_test import KEYBOARD_PARAM +from openpilot.tools.lateral_maneuvers.ford_keyboard import main as keyboard_main + + +def main(): + if Params().get_bool(KEYBOARD_PARAM): + keyboard_main() + else: + suite_main(ford_channels=True) if __name__ == '__main__': - main(ford_channels=True) + main() diff --git a/openpilot/tools/lateral_maneuvers/generate_report.py b/openpilot/tools/lateral_maneuvers/generate_report.py index e6ba28afed..475ccffce6 100755 --- a/openpilot/tools/lateral_maneuvers/generate_report.py +++ b/openpilot/tools/lateral_maneuvers/generate_report.py @@ -61,6 +61,7 @@ def report(platform, route, _description, CP, ID, maneuvers): t_lateralPlan, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid], strict=True) t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True) channel = str(lateralPlan[0].fordChannelTest.channel) if is_channel_maneuver(lateralPlan[0]) else None + keyboard = bool(channel and lateralPlan[0].fordChannelTest.keyboardRequestId) origin = t_lateralPlan[0] commands, command_problems = channel_commands(msgs, CP, channel, origin) if channel else (None, set()) @@ -81,6 +82,10 @@ def report(platform, route, _description, CP, ID, maneuvers): builder.append(f"

{title}

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

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

') + if keyboard: + builder.append('

Keyboard-triggered step: baseline, one-second target, then return to the baseline target. ' + + 'C1 feedback can remain nonzero during release. Accelerator input is allowed with MADS. ' + + 'The PSCM limit plot records limitReached=2; this does not by itself abort the step.

') if command_problems: builder.append(f'

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

') @@ -141,8 +146,9 @@ def report(platform, route, _description, CP, ID, maneuvers): target_cross_times.setdefault(description, []) plt.rcParams['font.size'] = 40 - 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 [])}) + fig = plt.figure(figsize=(30, 55 if keyboard else (50 if channel else 40))) + ratios = [5, 5, 3, 3, 3] + ([3, 3] if channel else []) + ([2] if keyboard else []) + ax = fig.subplots(len(ratios), 1, sharex=True, gridspec_kw={'height_ratios': ratios}) ax[0].grid(linewidth=4) desired_label = 'lateralManeuverPlan.desiredCurvature * vEgo^2' @@ -182,6 +188,24 @@ def report(platform, route, _description, CP, ID, maneuvers): ax[4+idx].set_ylabel(label) ax[4+idx].grid(linewidth=4) ax[4+idx].legend(prop={'size': 30}) + if keyboard: + pscm = [(float((m.logMonoTime-origin)*1e-9), m.carStateSP.fordPscmStatus.limit) + for m in msgs if m.which() == 'carStateSP' and m.valid and m.carStateSP.fordPscmStatus.valid] + if pscm: + times, limits = zip(*pscm, strict=True) + ax[7].step(times, limits, where='post', linewidth=6, label='PSCM limit status') + else: + ax[7].text(.05, .5, 'No valid PSCM status logged', transform=ax[7].transAxes) + ax[7].set_yticks([0, 1, 2, 3]) + ax[7].set_ylabel('PSCM limit\n2 = reached') + ax[7].grid(linewidth=4) + previous_phase = None + for t, plan in zip(t_lateralPlan, lateralPlan, strict=True): + phase = str(plan.fordChannelTest.keyboardPhase) + if phase != previous_phase: + for axis in ax: + axis.axvline(t, color='#777777', linestyle='--', linewidth=2) + previous_phase = phase ax[2].grid(linewidth=4) ax[2].plot(t_carState, [v * CV.MS_TO_MPH for v in v_ego], label='carState.vEgo', linewidth=6) diff --git a/openpilot/tools/lateral_maneuvers/tests/test_ford_keyboard.py b/openpilot/tools/lateral_maneuvers/tests/test_ford_keyboard.py new file mode 100644 index 0000000000..7bef8d8f69 --- /dev/null +++ b/openpilot/tools/lateral_maneuvers/tests/test_ford_keyboard.py @@ -0,0 +1,237 @@ +"""Offline keyboard, maneuver, and real controller/CAN checks; no hardware actuation.""" +import math +from types import SimpleNamespace + +import pytest + +from opendbc.car.structs import car +from openpilot.cereal import log, messaging +from openpilot.common.params import Params, ParamKeyFlag +from openpilot.selfdrive.controls.lib.ford_channel_test import KEYBOARD_PARAM, PARAM, SPEEDS, FordChannelTest +from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup +from openpilot.tools.joystick.ford_keyboard_control import KeyboardControl, enable +from openpilot.tools.lateral_maneuvers import ford_keyboard as daemon +from openpilot.tools.lateral_maneuvers.ford_keyboard import KeyboardManeuver, BASELINE_S, STEP_S, RELEASE_S + + +def request(**kw): + return log.Joystick.FordKeyboard.new_message(**({'requestId': 0, 'channel': 'c0', 'speed': SPEEDS[0], + 'accel': .5, 'action': 'idle'} | kw)) + + +def step(test, t, req, **kw): + test.update(t, req, **({'input_time': t, 'input_valid': True, 'healthy': True, 'ready': True, + 'speed': req.speed, 'curvature': .001} | kw)) + return test.plan(kw.get('speed', req.speed)) + + +def ready(test, req, start=10.): + for i in range(42): + step(test, start+i*.05, req) + assert test.status == 'Ready: A left / D right' + return start+2.10 + + +@pytest.mark.parametrize('channel', ['c0', 'c1']) +@pytest.mark.parametrize('speed', SPEEDS) +@pytest.mark.parametrize('direction,sign', [('left', -1), ('right', 1)]) +@pytest.mark.parametrize('amplitude', [.25, .5, 3.]) +def test_timed_step_uses_normal_curvature_then_returns_to_baseline(channel, speed, direction, sign, amplitude): + test = KeyboardManeuver() + req = request(channel=channel, speed=speed, accel=amplitude) + start = ready(test, req) + req.requestId, req.action = 1, direction + plans = [step(test, start+i*.05, req) for i in range(61)] + assert plans[0].valid and not plans[-1].valid + assert plans[-1].lateralManeuverPlan.fordChannelTest.phase == 'complete' + phases = [str(p.lateralManeuverPlan.fordChannelTest.keyboardPhase) for p in plans[:-1]] + assert {'baseline', 'pulse', 'release'} == set(phases) + assert sum(x == 'pulse' for x in phases)*.05 == pytest.approx(STEP_S, abs=.05) + for p in plans[:-1]: + plan = p.lateralManeuverPlan + expected = .001+(sign*amplitude/speed**2 if plan.fordChannelTest.keyboardPhase == 'pulse' else 0.) + assert plan.desiredCurvature == pytest.approx(expected) + assert plan.fordChannelTest.phase == 'maneuver' and plan.fordChannelTest.delta == 0. + assert plan.fordChannelTest.keyboardRequestId == 1 + assert plan.fordChannelTest.targetAccel == pytest.approx(sign*amplitude) + for i in range(1, 81): + step(test, start+3.+i*.05, req) + assert not test.active # holding the last trigger never repeats a step + + +@pytest.mark.parametrize('fault', [ + {'input_valid': False}, {'input_time': 1.}, {'input_time': 100.}, {'healthy': False}, + {'speed': SPEEDS[0]+.71}, {'speed': math.nan}, {'curvature': math.nan}, +]) +def test_abort_never_queues_or_retries_old_request(fault): + test, req = KeyboardManeuver(), request() + start = ready(test, req) + req.requestId, req.action = 1, 'left' + assert step(test, start, req).valid + assert not step(test, start+.05, req, **fault).valid + for i in range(1, 61): + assert not step(test, start+.05+i*.05, req).valid + req.requestId = 2 + assert step(test, start+3.10, req).valid + + +def test_unready_and_busy_keypresses_are_consumed_not_queued(): + test, req = KeyboardManeuver(), request(requestId=1, action='right') + start = ready(test, req) + assert not test.active + req.requestId = 2 + assert step(test, start, req).valid + req.requestId, req.action, req.channel, req.accel = 3, 'left', 'c1', 3. + for i in range(1, 121): + p = step(test, start+i*.05, req) + if p.valid: + assert p.lateralManeuverPlan.fordChannelTest.channel == 'c0' + assert p.lateralManeuverPlan.fordChannelTest.targetAccel == .5 + assert test.run_id == 1 and not test.active + + +@pytest.mark.parametrize('changes', [{'channel': 'none'}, {'accel': math.nan}, {'accel': 3.25}, {'accel': 0.}, {'speed': 0.}]) +def test_bad_input_cannot_trigger(changes): + test, req = KeyboardManeuver(), request() + start = ready(test, req) + req.requestId, req.action = 1, 'right' + for k, v in changes.items(): + setattr(req, k, v) + assert not step(test, start, req).valid + + +def test_frozen_time_clock_gap_and_cancel_abort(): + for delta in [0., -.1, .151]: + test, req = KeyboardManeuver(), request() + start = ready(test, req) + req.requestId, req.action = 1, 'right' + step(test, start, req) + assert not step(test, start+delta, req).valid + test, req = KeyboardManeuver(), request() + start = ready(test, req) + req.requestId, req.action = 1, 'right' + step(test, start, req) + req.requestId, req.action = 2, 'cancel' + assert not step(test, start+.05, req).valid + assert test.phase == 'aborted' + + +def test_keyboard_mapping_heartbeat_repeat_suppression_and_cancel(): + control = KeyboardControl() + for key in ('2', '+', 'v'): + assert control.key(key, 10.) + control.key('a', 11.) + p = control.message().testJoystick + assert list(p.axes) == [0., 0.] + assert p.fordKeyboard.channel == 'c1' and p.fordKeyboard.action == 'left' + assert p.fordKeyboard.accel == .75 and p.fordKeyboard.speed == pytest.approx(SPEEDS[1]) + first = p.fordKeyboard.requestId + for i in range(1, 101): + control.key('a', 11.+i*.05) + assert control.message().testJoystick.fordKeyboard.requestId == first + control.key('a', 17.) + assert control.request.requestId > first + control.key('r', 18.) + assert control.request.action == 'cancel' + assert not control.key('q', 19.) + + +@pytest.mark.parametrize('kwargs', [{'speed': 10}, {'accel': math.nan}, {'accel': math.inf}, {'accel': 0.}, {'accel': 3.1}]) +def test_cli_rejects_invalid_settings(kwargs): + with pytest.raises(ValueError): + KeyboardControl(**kwargs) + + +def test_offroad_setup_and_mode_lifecycle_use_real_params(tmp_path): + params = Params(str(tmp_path)) + cp = car.CarParams.new_message(brand='ford', flags=1) + params.put('CarParamsPersistent', cp.to_bytes(), block=True) + params.put_bool('FordModelActionController', True, block=True) + with pytest.raises(ValueError, match='offroad'): + enable(params) + params.put_bool('IsOffroad', True, block=True) + params.put_bool('JoystickDebugMode', True, block=True) + params.put_bool(PARAM, True, block=True) + enable(params) + assert params.get_bool(KEYBOARD_PARAM) and not params.get_bool(PARAM) and not params.get_bool('JoystickDebugMode') + assert startup(params=params).ford_channel_test.keyboard + params.put_bool('IsOffroad', False, block=True) + enable(params) # reconnect to the selected mode, without an ignition cycle + params.put_bool(PARAM, True, block=True) + with pytest.raises(ValueError, match='offroad'): + enable(params) + assert params.get_bool(PARAM) # a refused onroad mode change has no side effects + params.put_bool(PARAM, False, block=True) + for flag in (ParamKeyFlag.CLEAR_ON_MANAGER_START, ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION): + params.put_bool(KEYBOARD_PARAM, True, block=True) + params.clear_all(flag) + assert not params.get_bool(KEYBOARD_PARAM) + for brand, flags in [('toyota', 1), ('ford', 0)]: + params.put_bool('IsOffroad', True, block=True) + params.put('CarParamsPersistent', car.CarParams.new_message(brand=brand, flags=flags).to_bytes(), block=True) + with pytest.raises(ValueError, match='CAN FD Ford'): + enable(params) + + +def test_keyboard_plan_requires_keyboard_mode(): + test, req = KeyboardManeuver(), request() + start = ready(test, req) + req.requestId, req.action = 1, 'right' + p = step(test, start, req).lateralManeuverPlan + from openpilot.selfdrive.controls.tests.test_ford_channel_test import update + assert not update(FordChannelTest(), msg=p).valid + assert update(FordChannelTest(keyboard=True), msg=p).valid + + +@pytest.mark.parametrize('keyboard', [False, True]) +def test_manager_imported_entrypoint_selects_the_correct_daemon(monkeypatch, keyboard): + # Manager imports the module and calls main(); it does not execute __main__. + from openpilot.tools.lateral_maneuvers import ford_maneuversd + calls = [] + monkeypatch.setattr(ford_maneuversd, 'Params', lambda: SimpleNamespace(get_bool=lambda _key: keyboard)) + monkeypatch.setattr(ford_maneuversd, 'keyboard_main', lambda: calls.append('keyboard')) + monkeypatch.setattr(ford_maneuversd, 'suite_main', lambda **kw: calls.append(kw)) + ford_maneuversd.main() + assert calls == (['keyboard'] if keyboard else [{'ford_channels': True}]) + + +def test_real_daemon_allows_mads_manual_throttle_and_publishes_complete_run(monkeypatch): + class Finished(Exception): + pass + clock, plans = [10.], [] + class SM(messaging.SubMaster): + def __init__(self, services, **kwargs): + super().__init__(services, **kwargs) + self.events = {s: messaging.new_message(s) for s in services} + + def update(self, _timeout): + clock[0] += .05 + assert clock[0] < 20 + cs = self.events['carState'].carState + cs.vEgo, cs.canValid, cs.gasPressed, cs.cruiseState.enabled = SPEEDS[0], True, True, False + self.events['carControl'].carControl.latActive = True # MADS, selfdriveState.enabled remains false + self.events['carStateSP'].carStateSP.fordPscmStatus = {'valid': True, 'canMonoTime': round(clock[0]*1e9), 'lateralState': 2} + self.events['testJoystick'].testJoystick.fordKeyboard = request(requestId=1 if clock[0] > 12.5 else 0, action='right') + for msg in self.events.values(): + msg.valid, msg.logMonoTime = True, round(clock[0]*1e9) + self.update_msgs(clock[0], [m.as_reader() for m in self.events.values()]) + + class PM: + def __init__(self, _services): + pass + + def send(self, service, msg): + if service == 'lateralManeuverPlan': + plans.append(msg) + if msg.lateralManeuverPlan.fordChannelTest.phase == 'complete': + raise Finished + + monkeypatch.setattr(daemon.messaging, 'SubMaster', SM) + monkeypatch.setattr(daemon.messaging, 'PubMaster', PM) + monkeypatch.setattr(daemon, 'time', SimpleNamespace(monotonic=lambda: clock[0])) + monkeypatch.setattr(daemon, 'Ratekeeper', lambda *_a, **_kw: SimpleNamespace(keep_time=lambda: None)) + with pytest.raises(Finished): + daemon.main() + active = [p for p in plans if p.valid] + assert len(active)*.05 == pytest.approx(BASELINE_S+STEP_S+RELEASE_S, abs=.05) + assert {str(p.lateralManeuverPlan.fordChannelTest.keyboardPhase) for p in active} == {'baseline', 'pulse', 'release'} diff --git a/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py b/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py index 3c9eaec073..b2f2d54322 100644 --- a/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py +++ b/openpilot/tools/lateral_maneuvers/tests/test_ford_report.py @@ -120,7 +120,8 @@ def test_single_sample_and_gap_cannot_count_as_onset(): @pytest.mark.parametrize('channel', ['c0', 'c1']) -def test_normal_channel_report_uses_real_targets_and_decoded_output(channel, tmp_path, monkeypatch): +@pytest.mark.parametrize('keyboard', [False, True]) +def test_normal_channel_report_uses_real_targets_and_decoded_output(channel, keyboard, tmp_path, monkeypatch): from pathlib import Path from openpilot.tools.lateral_maneuvers import generate_report as generator from openpilot.tools.lateral_maneuvers.ford_report import channel_commands @@ -136,7 +137,9 @@ def test_normal_channel_report_uses_real_targets_and_decoded_output(channel, tmp if i % 5 == 0: m = event('lateralManeuverPlan', t) m.lateralManeuverPlan.desiredCurvature = curvature - m.lateralManeuverPlan.fordChannelTest = {'runId': 1, 'channel': channel, 'phase': 'maneuver', 'speed': speed} + m.lateralManeuverPlan.fordChannelTest = {'runId': 1, 'channel': channel, 'phase': 'maneuver', 'speed': speed, + 'keyboardRequestId': 1 if keyboard else 0, + 'keyboardPhase': 'pulse' if i < 105 else 'release'} messages.append(m) m = event('carState', t) m.carState.vEgo, m.carState.steeringAngleDeg = speed, 16. if i < 110 else -16. @@ -150,6 +153,10 @@ def test_normal_channel_report_uses_real_targets_and_decoded_output(channel, tmp m.carControl.orientationNED = [0., 0., 0.] messages.append(m) messages.append(event('carOutput', t)) + if keyboard: + m = event('carStateSP', t) + m.carStateSP.fordPscmStatus = {'valid': True, 'canMonoTime': m.logMonoTime, 'lateralState': 2, 'limit': 2 if 70 < i < 90 else 0} + messages.append(m) c0, c1 = (24.5*curvature, 0.) if channel == 'c0' else (0., speed*curvature) address, data, src = create_lat_ctl2_msg(packer, bus, 2, -c0, -c1, 0., 0., i % 16) m = event('sendcan', t) @@ -185,7 +192,10 @@ def test_normal_channel_report_uses_real_targets_and_decoded_output(channel, tmp 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)']] + expected = ['Lateral Accel (m/s^2)', 'Wheel angle (deg)', 'Velocity (mph)', 'Jerk (m/s^3)', 'Roll (deg)', 'C0 sent (m)', 'C1 sent (rad)'] + assert captured == [expected + (['PSCM limit\n2 = reached'] if keyboard else [])] + if keyboard: + assert 'Keyboard-triggered step' in html and 'Accelerator input is allowed with MADS' in html assert 'data:image/webp;base64,' in html # A live nonzero unused field must invalidate the isolated-channel measurement. assert 'channel isolation failed' in channel_commands(messages, cp, 'c1' if channel == 'c0' else 'c0', 10_000_000_000)[1]