diff --git a/tools/joystick/joystick_control.py b/tools/joystick/joystick_control.py index 29a32cc84..d6904e8b5 100755 --- a/tools/joystick/joystick_control.py +++ b/tools/joystick/joystick_control.py @@ -1,18 +1,31 @@ #!/usr/bin/env python3 import os +import time import argparse import threading import numpy as np +import inputs from inputs import UnpluggedError, get_gamepad from cereal import messaging from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper +from openpilot.common.swaglog import cloudlog from openpilot.system.hardware import HARDWARE from openpilot.tools.lib.kbhit import KBHit EXPO = 0.4 +# Per-controller config keyed on a substring of the `inputs` gamepad name. +# accel = right stick vertical (up = gas). Axis codes and raw ranges (signed vs +# unsigned) differ per pad — use tools/joystick/joystick_probe.py to add one. +CONTROLLER_PROFILES = { + 'Stadia': {'name': 'Stadia', 'steer': 'ABS_X', 'accel': 'ABS_RZ', 'lo': 0., 'hi': 255.}, + 'X-Box': {'name': 'Xbox', 'steer': 'ABS_X', 'accel': 'ABS_RY', 'lo': -32768., 'hi': 32767.}, + 'DualSense': {'name': 'DualSense', 'steer': 'ABS_X', 'accel': 'ABS_RY', 'lo': 0., 'hi': 255.}, +} +DEFAULT_PROFILE = 'X-Box' + class Keyboard: def __init__(self): @@ -42,35 +55,55 @@ class Keyboard: class Joystick: def __init__(self): - # This class supports a PlayStation 5 DualSense controller on the comma 3X - # TODO: find a way to get this from API or detect gamepad/PC, perhaps "inputs" doesn't support it - self.cancel_button = 'BTN_NORTH' # BTN_NORTH=X/triangle - if HARDWARE.get_device_type() == 'pc': - accel_axis = 'ABS_Z' - steer_axis = 'ABS_RX' - # TODO: once the longcontrol API is finalized, we can replace this with outputting gas/brake and steering - self.flip_map = {'ABS_RZ': accel_axis} - else: - accel_axis = 'ABS_RX' - steer_axis = 'ABS_X' - self.flip_map = {'ABS_RY': accel_axis} + self.cancel_button = 'BTN_NORTH' + self.is_pc = HARDWARE.get_device_type() == 'pc' + self._last_scan = 0. + self._load_profile() - self.min_axis_value = {accel_axis: 0., steer_axis: 0.} - self.max_axis_value = {accel_axis: 255., steer_axis: 255.} + def _load_profile(self): + if self.is_pc: + # DualSense over a laptop for development + accel_axis, steer_axis = 'ABS_Z', 'ABS_RX' + self.flip_map = {'ABS_RZ': accel_axis} + raw_min, raw_max, self.deadzone = 0., 255., 0.03 + name, prof_name = 'pc', 'pc' + else: + name = inputs.devices.gamepads[0].name if inputs.devices.gamepads else '' + prof = next((p for key, p in CONTROLLER_PROFILES.items() if key in name), CONTROLLER_PROFILES[DEFAULT_PROFILE]) + accel_axis, steer_axis = prof['accel'], prof['steer'] + self.flip_map = {} + raw_min, raw_max, self.deadzone = prof['lo'], prof['hi'], 0.10 + prof_name = prof['name'] + + cloudlog.info(f"joystick_control: gamepad='{name}' using profile '{prof_name}'") + self.min_axis_value = {accel_axis: raw_min, steer_axis: raw_min} + self.max_axis_value = {accel_axis: raw_max, steer_axis: raw_max} self.axes_values = {accel_axis: 0., steer_axis: 0.} self.axes_order = [accel_axis, steer_axis] self.cancel = False + def _rescan(self): + # `inputs` enumerates /dev/input once at import, so a pad that wasn't ready at boot (or was + # hot-swapped) never gets read. Re-scan so it's picked up without a restart. Throttled to 1s. + now = time.monotonic() + if now - self._last_scan < 1.0: + return + self._last_scan = now + inputs.devices = inputs.DeviceManager() + if not self.is_pc and inputs.devices.gamepads: + self._load_profile() + def update(self): try: joystick_event = get_gamepad()[0] except (OSError, UnpluggedError): self.axes_values = dict.fromkeys(self.axes_values, 0.) + self._rescan() + time.sleep(0.1) # no controller; avoid busy-spin return False event = (joystick_event.code, joystick_event.state) - # flip left trigger to negative accel if event[0] in self.flip_map: event = (self.flip_map[event[0]], -event[1]) @@ -80,11 +113,8 @@ class Joystick: elif event[1] == 0: # state 0 is falling edge self.cancel = False elif event[0] in self.axes_values: - self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]]) - self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]]) - norm = -float(np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])) - norm = norm if abs(norm) > 0.03 else 0. # center can be noisy, deadzone of 3% + norm = norm if abs(norm) > self.deadzone else 0. # center can be noisy self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm # less action near center for fine control else: return False @@ -100,9 +130,11 @@ def send_thread(joystick): if rk.frame % 20 == 0: print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items())) + # _rescan() may swap the axis map from another thread + values, order = joystick.axes_values, joystick.axes_order joystick_msg = messaging.new_message('testJoystick') joystick_msg.valid = True - joystick_msg.testJoystick.axes = [joystick.axes_values[ax] for ax in joystick.axes_order] + joystick_msg.testJoystick.axes = [values.get(ax, 0.) for ax in order] pm.send('testJoystick', joystick_msg) diff --git a/tools/joystick/joystick_probe.py b/tools/joystick/joystick_probe.py new file mode 100644 index 000000000..c8994d4af --- /dev/null +++ b/tools/joystick/joystick_probe.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +""" +joystick_probe.py — identify a gamepad's axis codes and value ranges so you can +add it to joystick mode. + +WHY THIS EXISTS + Joystick mode (tools/joystick/joystick_control.py) maps a physical controller's + sticks to steering + gas/brake. Every controller model reports its sticks under + different evdev axis codes AND different raw value ranges, so joystick_control.py + keeps a CONTROLLER_PROFILES table. This probe is how you discover the two facts + you need to add a new row to that table: + 1. which axis CODE the left-stick-horizontal and right-stick-vertical emit + 2. the raw value RANGE those axes span (and whether it is signed or unsigned) + +HOW TO RUN (on the comma device, offroad, controller plugged in) + cd /data/openpilot && python tools/joystick/joystick_probe.py + (If it says "Permission denied", run it through python as above rather than + executing the file directly — it just isn't marked executable.) + + Then move ONE stick/axis at a time, fully in both directions, and watch the + output. Each line shows: the axis CODE, its current value (state), and the + min/max range it has spanned so far. + +WHAT TO RECORD (joystick mode uses exactly two axes) + * STEER = left stick, horizontal -> push left and right, note the CODE + (usually ABS_X) and its range_so_far (e.g. -32768..32767 or 0..255) + * ACCEL = right stick, VERTICAL -> push up and down, note the CODE + (Xbox: ABS_RY, Stadia: ABS_RZ, DualSense: ABS_RY) and its range + Also note the controller NAME printed under "Detected gamepads:" — a unique + substring of it is the table key used to auto-detect this pad. + +INTERPRETING THE RANGE + * If the values swing symmetrically around 0 (e.g. -32768..32767) the pad is + SIGNED 16-bit, centered at 0. -> lo=-32768, hi=32767 + * If the values swing around ~128 within 0..255 the pad is UNSIGNED 8-bit, + centered at 128. -> lo=0, hi=255 + * The rest/center value doesn't matter for the table — only the endpoints do. + joystick_control.py maps lo -> -1 and hi -> +1 via np.interp, so center lands + near 0 automatically as long as lo/hi are the true extremes. + +ADDING YOUR CONTROLLER (edit tools/joystick/joystick_control.py) + Add a row to CONTROLLER_PROFILES keyed on a substring of the controller name: + + 'MySubstr': {'name': 'MyPad', 'steer': '', 'accel': '', + 'lo': ., 'hi': .}, + + e.g. for the Xbox One pad ("Microsoft X-Box One pad"): + 'X-Box': {'name': 'Xbox', 'steer': 'ABS_X', 'accel': 'ABS_RY', + 'lo': -32768., 'hi': 32767.}, + + Notes: + * accel is the right stick's VERTICAL axis; up = gas, down = brake. The leading + minus sign in joystick_control.py's normalization makes "up = positive accel" + for BOTH signed and unsigned ranges, so you do NOT need a flip/remap. + * Keys are matched by substring against the reported name, so 'X-Box' matches + "Microsoft X-Box One pad (Firmware 2015)". Pick a substring unique to your pad. + * On-device controllers get a 0.10 deadzone (absorbs stick drift). If your pad + creeps at rest, that's the knob to raise. + +VERIFYING (after editing and restarting the joystick process) + With joystick mode on, the on-screen Gas/Steer numbers should move. Those come + from carControl.actuators, not raw axes, so they only move when the engagement + gate passes (steer needs lateral active/AOL; gas needs full openpilot long). + To confirm raw input independently, watch the testJoystick message directly. + +TROUBLESHOOTING + * "Detected gamepads: " -> the `inputs` library can't see the pad. It's a + driver/enumeration problem, not a mapping one; no table edit will help until + the pad is detected. Try re-plugging or a different USB port/cable. + * All values stay at 0 while a stick moves -> the code column will still change; + if it doesn't, that physical axis isn't emitting events under any code. +""" +import sys + +try: + from inputs import get_gamepad, devices, UnpluggedError +except Exception as e: + print(f"inputs import failed: {e}") + sys.exit(1) + +print("Detected gamepads:") +if not devices.gamepads: + print(" -> 'inputs' does not see your controller (driver/enumeration issue)") +else: + for g in devices.gamepads: + print(f" {g}") + +print("\nMove ONE stick at a time, fully both ways. Note the CODE + range for:") +print(" STEER = left stick horizontal | ACCEL = right stick vertical") +print("Ctrl-C to stop.\n") +seen = {} +while True: + try: + for ev in get_gamepad(): + if ev.ev_type in ("Absolute", "Key"): + lo, hi = seen.get(ev.code, (ev.state, ev.state)) + seen[ev.code] = (min(lo, ev.state), max(hi, ev.state)) + rng = seen[ev.code] + print(f"type={ev.ev_type:9s} code={ev.code:12s} state={ev.state:<8} range_so_far={rng}") + except (OSError, UnpluggedError): + print("get_gamepad() raised UnpluggedError -> controller not readable") + break + except KeyboardInterrupt: + break diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index 789dad562..3bd28b3bd 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -19,7 +19,7 @@ def joystickd_thread(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) VM = VehicleModel(CP) - sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL) + sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'starpilotCarState', 'testJoystick'], frequency=1. / DT_CTRL) pm = messaging.PubMaster(['carControl', 'controlsState']) rk = Ratekeeper(100, print_delay_threshold=None) @@ -30,7 +30,9 @@ def joystickd_thread(): cc_msg.valid = True CC = cc_msg.carControl CC.enabled = sm['selfdriveState'].enabled - CC.latActive = sm['selfdriveState'].active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent + # steer in full engagement or AOL; gas/brake only in full engagement (AOL uses pedals) + lateral_allowed = (CC.enabled and sm['selfdriveState'].active) or sm['starpilotCarState'].alwaysOnLateralEnabled + CC.latActive = lateral_allowed and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in sm['onroadEvents']) and CP.openpilotLongitudinalControl CC.cruiseControl.cancel = sm['carState'].cruiseState.enabled and (not CC.enabled or not CP.pcmCruise) CC.hudControl.leadDistanceBars = 2