Joystick fixes + added Stadia controller

tools/joystick/joystick_control.py

@@ -3,7 +3,7 @@
 import argparse
 import threading
 import numpy as np
-from inputs import UnpluggedError, get_gamepad
+from inputs import UnpluggedError, get_gamepad, devices
 
 from cereal import messaging
 from openpilot.common.params import Params
@@ -42,21 +42,33 @@ def update(self):
 
 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'
+
+    gamepad_name = ''
+    if devices.gamepads:
+      gp0 = devices.gamepads[0]
+      gamepad_name = getattr(gp0, 'name', '') or str(gp0)
+      print(f"Detected: {gamepad_name}\n")
+
+    if 'Stadia' in gamepad_name:
+      print("Google Stadia axes mapping")
+      steer_axis = 'ABS_X'
+      accel_axis = 'ABS_GAS'
+      self.flip_map = {'ABS_BRAKE': accel_axis}
+    elif HARDWARE.get_device_type() == 'pc':
+      print("PlayStation 5 DualSense (PC) axes mapping")
+      accel_axis = 'ABS_RZ'
       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}
+      self.flip_map = {'ABS_Z': accel_axis}
     else:
-      accel_axis = 'ABS_RX'
+      print("PlayStation 5 DualSense axes mapping")
+      accel_axis = 'ABS_RY'
       steer_axis = 'ABS_Z'
-      self.flip_map = {'ABS_RY': accel_axis}
+      self.flip_map = {'ABS_RX': accel_axis}
 
-    self.min_axis_value = {accel_axis: 0., steer_axis: 0.}
-    self.max_axis_value = {accel_axis: 255., steer_axis: 255.}
+    self.min_axis_value = {accel_axis: -255 if accel_axis in self.flip_map.values() else 0,
+                           steer_axis: 0}
+    self.max_axis_value = {accel_axis: 255, steer_axis: 255}
     self.axes_values = {accel_axis: 0., steer_axis: 0.}
     self.axes_order = [accel_axis, steer_axis]
     self.cancel = False
@@ -70,7 +82,7 @@ def update(self):
 
     event = (joystick_event.code, joystick_event.state)
 
-    # flip left trigger to negative accel
+    # flip left trigger (brake) to negative accel
     if event[0] in self.flip_map:
       event = (self.flip_map[event[0]], -event[1])
 
@@ -83,7 +95,7 @@ def update(self):
       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 = 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%
       self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm  # less action near center for fine control
     else:

tools/joystick/joystickd.py

@@ -54,7 +54,7 @@ def joystickd_thread():
       max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5)
       max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['liveParameters'].roll))
 
-      actuators.torque = float(np.clip(joystick_axes[1], -1, 1))
+      actuators.torque = -float(np.clip(joystick_axes[1], -1, 1))
       actuators.steeringAngleDeg, actuators.curvature = actuators.torque * max_angle, actuators.torque * -max_curvature
 
     pm.send('carControl', cc_msg)