diff --git a/tools/joystick/joystick_control.py b/tools/joystick/joystick_control.py index 11d17e587e8451..a86a8d321d91a4 100755 --- a/tools/joystick/joystick_control.py +++ b/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: diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index 789dad562318f2..0c3d07f799f11e 100755 --- a/tools/joystick/joystickd.py +++ b/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)