diff --git a/joycontrol/protocol.py b/joycontrol/protocol.py index f83a9d9a..6f03081f 100644 --- a/joycontrol/protocol.py +++ b/joycontrol/protocol.py @@ -25,6 +25,51 @@ def create_controller_protocol(): return create_controller_protocol +class Signal: + + def __init__(self): + self.subscribers = set() + + def subscribe(self, subscriber): + self.subscribers.add(subscriber) + + def unsubscribe(self, subscriber): + self.subscribers.remove(subscriber) + + def dispatch(self, *args): + for subscriber in self.subscribers: + try: + result = subscriber(*args) + if asyncio.iscoroutine(result): + asyncio.ensure_future(result) + except: + logger.exception('Error executing subscriber %s', subscriber) + + +def freq(val): + return 2**(val / 32) * 10 + + +def amp(val): + # valid input range 0-200, but not exactly linear + return abs(val / 200) + + +def rumble(data): + if data == [0x00] * 4: + return None + + high_rumble_hf = data[0] + (data[1] << 8) + hf = (high_rumble_hf & 0x01ff) / 4 + 96 + ha = (data[1] & 0xfe) / 2 + + lf = (data[2] & 0x7f) + 64 + low_rumble_la = ((data[2] & 0x80) >> 7) + data[3] + la = (low_rumble_la - 64) * 2 + return freq(hf), amp(ha), freq(lf), amp(la) + + + class ControllerProtocol(BaseProtocol): def __init__(self, controller: Controller, spi_flash: FlashMemory = None): self.controller = controller @@ -46,6 +91,29 @@ def __init__(self, controller: Controller, spi_flash: FlashMemory = None): # This event gets triggered once the Switch assigns a player number to the controller and accepts user inputs self.sig_set_player_lights = asyncio.Event() + self.raw_rumble = Signal() + self.rumble = Signal() + self._rumble_data = None + self.raw_rumble.subscribe(self.summarize_rumble) + + + def summarize_rumble(self, left, right): + amp = () + if left: + amp += (left[1], left[3]) + if right: + amp += (right[1], right[3]) + if any(amp): + if not self._rumble_data: + self._rumble_data = [time.time(), max(*amp)] + else: + self._rumble_data[1] = max(self._rumble_data[1], *amp) + else: + if self._rumble_data: + self.rumble.dispatch(time.time() - self._rumble_data[0], self._rumble_data[1]) + self._rumble_data = None + + async def send_controller_state(self): """ Waits for the controller state to be send. @@ -157,13 +225,15 @@ async def input_report_mode_full(self): output_report_id = report.get_output_report_id() if output_report_id == OutputReportID.RUMBLE_ONLY: - # TODO - pass + data = report.get_rumble_data() + self.raw_rumble.dispatch(rumble(data[:4]), rumble(data[4:])) elif output_report_id == OutputReportID.SUB_COMMAND: + data = report.get_rumble_data() + self.raw_rumble.dispatch(rumble(data[:4]), rumble(data[4:])) reply_send = await self._reply_to_sub_command(report) elif output_report_id == OutputReportID.REQUEST_IR_NFC_MCU: # TODO NFC - raise NotImplementedError('NFC communictation is not implemented.') + raise NotImplementedError('NFC communictation is not implemented.') else: logger.warning(f'Report unknown output report "{output_report_id}" - IGNORE') except ValueError as v_err: @@ -194,7 +264,6 @@ async def input_report_mode_full(self): if sleep_time < 0: # logger.warning(f'Code is running {abs(sleep_time)} s too slow!') sleep_time = 0 - await asyncio.sleep(sleep_time) except NotConnectedError as err: @@ -225,8 +294,6 @@ async def report_received(self, data: Union[bytes, Text], addr: Tuple[str, int]) if output_report_id == OutputReportID.SUB_COMMAND: await self._reply_to_sub_command(report) - # elif output_report_id == OutputReportID.RUMBLE_ONLY: - # pass else: logger.warning(f'Output report {output_report_id} not implemented - ignoring') diff --git a/joycontrol/server.py b/joycontrol/server.py index 7d7a212b..24ce5b06 100644 --- a/joycontrol/server.py +++ b/joycontrol/server.py @@ -113,6 +113,8 @@ async def create_hid_server(protocol_factory, ctl_psm=17, itr_psm=19, device_id= client_ctl.setblocking(False) client_itr.setblocking(False) + print(client_itr.getsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF)) + client_itr.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 4096) # create transport for the established connection and activate the HID protocol transport = L2CAP_Transport(asyncio.get_event_loop(), protocol, client_itr, client_ctl, 50, capture_file=capture_file) protocol.connection_made(transport) diff --git a/run_controller_cli.py b/run_controller_cli.py index c84597b6..ce87deb7 100755 --- a/run_controller_cli.py +++ b/run_controller_cli.py @@ -14,6 +14,7 @@ from joycontrol.memory import FlashMemory from joycontrol.protocol import controller_protocol_factory from joycontrol.server import create_hid_server +from joycontrol.transport import NotConnectedError logger = logging.getLogger(__name__) @@ -265,6 +266,10 @@ async def nfc(*args): cli.add_command(nfc.__name__, nfc) +def print_rumble(duration, amp): + logger.info(f'Duration: {duration:.3f}s, Amplitude: {amp:.3f}') + + async def _main(args): # parse the spi flash if args.spi_flash: @@ -287,6 +292,7 @@ async def _main(args): device_id=args.device_id) controller_state = protocol.get_controller_state() + protocol.rumble.subscribe(print_rumble) # Create command line interface and add some extra commands cli = ControllerCLI(controller_state) @@ -325,6 +331,11 @@ async def _main(args): args = parser.parse_args() loop = asyncio.get_event_loop() - loop.run_until_complete( - _main(args) - ) + while True: + try: + loop.run_until_complete( + _main(args) + ) + except NotConnectedError: + continue + break \ No newline at end of file