123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190 |
- #!/usr/bin/env python3
- # set up wheel
- import array
- import os
- import struct
- from fcntl import ioctl
- from typing import NoReturn
- from openpilot.tools.sim.bridge.common import control_cmd_gen
- # Iterate over the joystick devices.
- print('Available devices:')
- for fn in os.listdir('/dev/input'):
- if fn.startswith('js'):
- print(f' /dev/input/{fn}')
- # We'll store the states here.
- axis_states: dict[str, float] = {}
- button_states: dict[str, float] = {}
- # These constants were borrowed from linux/input.h
- axis_names = {
- 0x00 : 'x',
- 0x01 : 'y',
- 0x02 : 'z',
- 0x03 : 'rx',
- 0x04 : 'ry',
- 0x05 : 'rz',
- 0x06 : 'trottle',
- 0x07 : 'rudder',
- 0x08 : 'wheel',
- 0x09 : 'gas',
- 0x0a : 'brake',
- 0x10 : 'hat0x',
- 0x11 : 'hat0y',
- 0x12 : 'hat1x',
- 0x13 : 'hat1y',
- 0x14 : 'hat2x',
- 0x15 : 'hat2y',
- 0x16 : 'hat3x',
- 0x17 : 'hat3y',
- 0x18 : 'pressure',
- 0x19 : 'distance',
- 0x1a : 'tilt_x',
- 0x1b : 'tilt_y',
- 0x1c : 'tool_width',
- 0x20 : 'volume',
- 0x28 : 'misc',
- }
- button_names = {
- 0x120 : 'trigger',
- 0x121 : 'thumb',
- 0x122 : 'thumb2',
- 0x123 : 'top',
- 0x124 : 'top2',
- 0x125 : 'pinkie',
- 0x126 : 'base',
- 0x127 : 'base2',
- 0x128 : 'base3',
- 0x129 : 'base4',
- 0x12a : 'base5',
- 0x12b : 'base6',
- 0x12f : 'dead',
- 0x130 : 'a',
- 0x131 : 'b',
- 0x132 : 'c',
- 0x133 : 'x',
- 0x134 : 'y',
- 0x135 : 'z',
- 0x136 : 'tl',
- 0x137 : 'tr',
- 0x138 : 'tl2',
- 0x139 : 'tr2',
- 0x13a : 'select',
- 0x13b : 'start',
- 0x13c : 'mode',
- 0x13d : 'thumbl',
- 0x13e : 'thumbr',
- 0x220 : 'dpad_up',
- 0x221 : 'dpad_down',
- 0x222 : 'dpad_left',
- 0x223 : 'dpad_right',
- # XBox 360 controller uses these codes.
- 0x2c0 : 'dpad_left',
- 0x2c1 : 'dpad_right',
- 0x2c2 : 'dpad_up',
- 0x2c3 : 'dpad_down',
- }
- axis_name_list: list[str] = []
- button_name_list: list[str] = []
- def wheel_poll_thread(q: 'Queue[str]') -> NoReturn:
- # Open the joystick device.
- fn = '/dev/input/js0'
- print(f'Opening {fn}...')
- jsdev = open(fn, 'rb')
- # Get the device name.
- #buf = bytearray(63)
- buf = array.array('B', [0] * 64)
- ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
- js_name = buf.tobytes().rstrip(b'\x00').decode('utf-8')
- print(f'Device name: {js_name}')
- # Get number of axes and buttons.
- buf = array.array('B', [0])
- ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES
- num_axes = buf[0]
- buf = array.array('B', [0])
- ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
- num_buttons = buf[0]
- # Get the axis map.
- buf = array.array('B', [0] * 0x40)
- ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
- for _axis in buf[:num_axes]:
- axis_name = axis_names.get(_axis, f'unknown(0x{_axis:02x})')
- axis_name_list.append(axis_name)
- axis_states[axis_name] = 0.0
- # Get the button map.
- buf = array.array('H', [0] * 200)
- ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP
- for btn in buf[:num_buttons]:
- btn_name = button_names.get(btn, f'unknown(0x{btn:03x})')
- button_name_list.append(btn_name)
- button_states[btn_name] = 0
- print('%d axes found: %s' % (num_axes, ', '.join(axis_name_list)))
- print('%d buttons found: %s' % (num_buttons, ', '.join(button_name_list)))
- # Enable FF
- import evdev
- from evdev import ecodes, InputDevice
- device = evdev.list_devices()[0]
- evtdev = InputDevice(device)
- val = 24000
- evtdev.write(ecodes.EV_FF, ecodes.FF_AUTOCENTER, val)
- while True:
- evbuf = jsdev.read(8)
- value, mtype, number = struct.unpack('4xhBB', evbuf)
- # print(mtype, number, value)
- if mtype & 0x02: # wheel & paddles
- axis = axis_name_list[number]
- if axis == "z": # gas
- fvalue = value / 32767.0
- axis_states[axis] = fvalue
- normalized = (1 - fvalue) * 50
- q.put(control_cmd_gen(f"throttle_{normalized:f}"))
- elif axis == "rz": # brake
- fvalue = value / 32767.0
- axis_states[axis] = fvalue
- normalized = (1 - fvalue) * 50
- q.put(control_cmd_gen(f"brake_{normalized:f}"))
- elif axis == "x": # steer angle
- fvalue = value / 32767.0
- axis_states[axis] = fvalue
- normalized = fvalue
- q.put(control_cmd_gen(f"steer_{normalized:f}"))
- elif mtype & 0x01: # buttons
- if value == 1: # press down
- if number in [0, 19]: # X
- q.put(control_cmd_gen("cruise_down"))
- elif number in [3, 18]: # triangle
- q.put(control_cmd_gen("cruise_up"))
- elif number in [1, 6]: # square
- q.put(control_cmd_gen("cruise_cancel"))
- elif number in [10, 21]: # R3
- q.put(control_cmd_gen("reverse_switch"))
- if __name__ == '__main__':
- from multiprocessing import Process, Queue
- q: 'Queue[str]' = Queue()
- p = Process(target=wheel_poll_thread, args=(q,))
- p.start()
|