From f8de4e500c90ad92123fb3e94b6e05835aa533ca Mon Sep 17 00:00:00 2001 From: Patrick Moessler Date: Fri, 30 Oct 2020 02:00:14 +0100 Subject: [PATCH] initial --- .vscode/launch.json | 15 +++++ pyiforce.py | 156 ++++++++++++++++++++++++++++++++++++++++++++ pyvjoy_ffb.py | 15 +++++ test.py | 76 +++++++++++++++++++++ 4 files changed, 262 insertions(+) create mode 100644 .vscode/launch.json create mode 100644 pyiforce.py create mode 100644 pyvjoy_ffb.py create mode 100644 test.py diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..b3f92e6 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,15 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "Python: test.py", + "type": "python", + "request": "launch", + "program": "test.py", + "console": "integratedTerminal" + } + ] +} \ No newline at end of file diff --git a/pyiforce.py b/pyiforce.py new file mode 100644 index 0000000..abb7914 --- /dev/null +++ b/pyiforce.py @@ -0,0 +1,156 @@ +_rx_stream = None +_pkt_handlers = {} + + +class Packet: + SOP = 0x2B + MAX_LENGTH = 255 # TBD + + INPUT_JOYSTICK = 0x01 + INPUT_WHEEL = 0x03 + EFFECT_STATES = 0x02 + + SET_CONTROL = 0x40 + CONTROL = 0x41 + SET_EFFECT_STATE = 0x42 + SET_OVERALL = 0x43 + + QUERY = 0xFF + + Types = [ + INPUT_JOYSTICK, + INPUT_WHEEL, + EFFECT_STATES, + SET_CONTROL, + CONTROL, + SET_EFFECT_STATE, + SET_OVERALL, + QUERY + ] + + +class ControlType: + DEAD_ZONE = 0x00 + IGNORE_DEADMAN_SWITCH = 0x01 + ENABLE_COMM_WATCHDOG = 0x02 + SET_SPRING_STRENGTH = 0x03 + ENABLE_SPRING = 0x04 + SET_AXIS_SATURATION = 0x05 + + +class OverallControlType: + GAIN = 0x00 + + +class Query: + # Queries + BufferSize = 0x42 # ('B'uffer size) + Manufacturer = 0x4d # ('M'anufacturer) + Product = 0x50 # ('P'roduct) + Version = 0x56 # ('V'ersion) + NumberOfEffects = 0x4e # ('N'umber of effects) + Effect = 0x45 # ('E')ffect + + # Control + Open = 0x4f # ('O'pen) + Close = 0x43 # ('C')lose + + +def tx_packet(command, data): + cs = Packet.SOP + yield Packet.SOP + cs ^= command + yield command + cs ^= len(data) + yield len(data) + for d in data: + cs ^= d + yield d + yield cs + + +def rx_packet(instream): + while instream: + # sync start + c = 0 + cs = Packet.SOP + while c != Packet.SOP: + c = next(instream) + + # packet type + pkt_type = next(instream) + cs ^= pkt_type + if pkt_type not in Packet.Types: + continue + + # packet length + pkt_len = next(instream) + cs ^= pkt_len + assert pkt_len < Packet.MAX_LENGTH + + # data + data = [] + for _ in range(pkt_len): + d = next(instream) + cs ^= d + data.append(d) + + # checksum + cs_in = next(instream) + cs ^= cs_in + + yield (pkt_type, data, (cs == 0)) + + +def create_rx_stream(input_stream): + global _rx_stream + _rx_stream = rx_packet(input_stream) + + +def rx_pump(): + global _rx_stream + command, data, ok = next(_rx_stream) + if ok: + handle_pkt(command, data) + + +def set_handler(pkt_type, handler): + global _pkt_handlers + _pkt_handlers[pkt_type] = handler + + +def handle_pkt(command, data): + global _pkt_handlers + if command == Packet.INPUT_WHEEL: + output = { + 'axis': { + 'wheel': int.from_bytes(data[0:2], byteorder='little', signed=True) / 2048, + 'gas': 1 - data[2] / 255, + 'brake': 1 - data[3] / 255 + }, + 'buttons': { + 'B': bool(data[5] & 0x20), + 'A': bool(data[5] & 0x10), + + '4': bool(data[5] & 0x08), + '3': bool(data[5] & 0x04), + '2_PaddleDown': bool(data[5] & 0x02), + '1_PaddleUp_Start': bool(data[5] & 0x01) + }, + 'hat': { + 'center': (data[6] & 0xF0) == 0xF0, + 'up': (data[6] & 0xF0) == 0x00, + 'down': (data[6] & 0xF0) == 0x40, + 'left': (data[6] & 0xF0) == 0x60, + 'right': (data[6] & 0xF0) == 0x20 + } + } + elif command == Packet.QUERY: + output = { + 'query': data[0], + 'data': data[1:] + } + else: + output = data + if _pkt_handlers.get(command): + _pkt_handlers[command](output) diff --git a/pyvjoy_ffb.py b/pyvjoy_ffb.py new file mode 100644 index 0000000..4c01c33 --- /dev/null +++ b/pyvjoy_ffb.py @@ -0,0 +1,15 @@ +from ctypes import CFUNCTYPE, c_void_p + +# void CALLBACK FfbFunction1(PVOID FfbPacket, PVOID userdata) +PYVJOY_CB_FUNC = CFUNCTYPE(None, c_void_p, c_void_p) + + +def pyvjoy_ffb_default_cb(ffb_pkt, userdata): + print(' '.join('%02x' % x for x in ffb_pkt)) + + +# typedef struct _FFB_DATA { +# ULONG size; +# ULONG cmd; +# UCHAR *data; +# } FFB_DATA; diff --git a/test.py b/test.py new file mode 100644 index 0000000..4e1467b --- /dev/null +++ b/test.py @@ -0,0 +1,76 @@ +import pyiforce as ifr +import serial +import pyvjoy + +PORT = 'COM7' +BAUD = 38400 + + +def send_pkt(port, command, data): + pkt = bytes(ifr.tx_packet(command, data)) + port.write(pkt) + +def hex_print(d): + print(' '.join('%02x'%x for x in d)) + +def query_print(d): + print('QUERY[%02x]:%s'%(d['query'],' '.join('%02x'%x for x in d['data']))) + +def setup_wheel(port): + ifr.set_handler(ifr.Packet.QUERY, query_print) + send_pkt(port, ifr.Packet.QUERY, [ifr.Query.Manufacturer]) + ifr.rx_pump() + send_pkt(port, ifr.Packet.QUERY, [ifr.Query.Product]) + ifr.rx_pump() + send_pkt(port, ifr.Packet.QUERY, [ifr.Query.Version]) + ifr.rx_pump() + send_pkt(port, ifr.Packet.QUERY, [ifr.Query.NumberOfEffects]) + ifr.rx_pump() + send_pkt(port, ifr.Packet.QUERY, [ifr.Query.BufferSize]) + ifr.rx_pump() + + # send_pkt(port, ifr.Packet.SET_OVERALL, [ifr.OverallControlType.GAIN, 0x80]) + send_pkt(port, ifr.Packet.QUERY, [ifr.Query.Open]) + + +def serial_reader(sp): + while sp.is_open: + in_byte = sp.read(1)[0] + # print(f'rx:%02X'%in_byte) + yield in_byte + + +with serial.Serial(port=PORT, baudrate=BAUD) as port: + + port.rts = True + ifr.create_rx_stream(serial_reader(port)) + + setup_wheel(port) + + j = pyvjoy.VJoyDevice(1) + + + # def handle_wheel(w): + # j.data.wAxisX = int(0x4000 * (w['axis']['wheel'] + 1)) + # j.data.wAxisY = int(0x8000 * w['axis']['gas']) + # j.data.wAxisZ = int(0x8000 * w['axis']['brake']) + # j.data.lButtons = ( + # 0x0000000000000001 if w['buttons']['A'] else 0) + ( + # 0x0000000000000002 if w['buttons']['B'] else 0) + ( + # 0x0000000000000004 if w['buttons']['1_PaddleUp_Start'] else 0) + ( + # 0x0000000000000008 if w['buttons']['2_PaddleDown'] else 0) + ( + # 0x0000000000000010 if w['buttons']['3'] else 0) + ( + # 0x0000000000000020 if w['buttons']['4'] else 0) + # j.data.bHats = ( + # -1 if w['hat']['center'] else + # 0 if w['hat']['up'] else + # 1 if w['hat']['right'] else + # 2 if w['hat']['down'] else + # 3 if w['hat']['left'] else -1) + # j.update() + + # ifr.set_handler(ifr.Packet.INPUT_WHEEL, handle_wheel) + ifr.set_handler(ifr.Packet.INPUT_WHEEL, print) + + while True: + ifr.rx_pump()