WIP: rumble - feedback please

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')
 

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)

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