Joystick Mode Refresh

OP Long: Left stick horizontal steer
	            Right stick vertical gas/brake

AOL:  Left stick horizontal steer

Controller profiles added:
Google Stadia Controller added
Xbox Controller has its own profile
Dual Sense has its own profile

joystick_probe.py tool allows people to discover their own controllers.

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)
 

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': '<steer code>', 'accel': '<accel code>',
+                   'lo': <min>., 'hi': <max>.},
+
+  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: <none>"  -> 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("  <none>  -> '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

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