receive joycon imu script

2021-03-07 14:27:57 +01:00
parent 887d7c8687
commit 1511174bcc
2 changed files with 79 additions and 1 deletions
@@ -373,7 +373,7 @@ class ControllerProtocol(BaseProtocol):
        input_report.set_misc()

        input_report.set_ack(0x80)
-        input_report.reply_to_subcommand_id(0x03)
+        input_report.reply_to_subcommand_id(SubCommand.SET_INPUT_REPORT_MODE)

        await self.write(input_report)

@@ -0,0 +1,78 @@
+import asyncio
+import logging
+
+import numpy as np
+
+from joycontrol import logging_default as log
+from joycontrol.hid import get_blt_hid_device, AsyncHID
+from joycontrol.report import OutputReport, OutputReportID, SubCommand, InputReport
+
+logger = logging.getLogger(__name__)
+
+
+async def _main():
+    logger.info('Waiting for HID devices... Please connect one JoyCon (left OR right), '
+                'or a Pro Controller over Bluetooth. '
+                'Note: The bluez "input" plugin needs to be enabled (default)')
+
+    controller = await get_blt_hid_device()
+    logger.info(f'Found controller "{controller}".')
+
+    timer = 0
+
+    with AsyncHID(path=controller['path'], loop=loop) as hid_controller:
+        # enable imu
+        output_report = OutputReport()
+        output_report.set_output_report_id(OutputReportID.SUB_COMMAND)
+        output_report.set_sub_command(SubCommand.ENABLE_6AXIS_SENSOR)
+        output_report.set_sub_command_data([0x01])
+        output_report.set_timer(timer)
+        timer += 1
+
+        await hid_controller.write(bytes(output_report)[1:])
+
+        # wait for ack
+        while True:
+            data = await hid_controller.read(50)
+            report = InputReport([0xA1] + list(data))
+            if report.get_input_report_id() == 0x21 and report.get_ack() == 0x80:
+                logger.info(f'Ack received {report.get_reply_to_subcommand_id()}')
+                break
+
+        # switch to 0x30 input report mode
+        output_report = OutputReport()
+        output_report.set_output_report_id(OutputReportID.SUB_COMMAND)
+        output_report.set_sub_command(SubCommand.SET_INPUT_REPORT_MODE)
+        output_report.set_sub_command_data([0x30])
+        output_report.set_timer(timer)
+        timer += 1
+
+        await hid_controller.write(bytes(output_report)[1:])
+
+        # wait for ack
+        while True:
+            data = await hid_controller.read(50)
+            report = InputReport([0xA1] + list(data))
+            if report.get_input_report_id() == 0x21 and report.get_ack() == 0x80:
+                logger.info(f'Ack received {report.get_reply_to_subcommand_id()}')
+                break
+
+        try:
+            while True:
+                data = await hid_controller.read(50)
+                report = InputReport([0xA1] + list(data))
+                acc, gyro = report.get_imu_data()
+                print(np.array(acc))
+                # print(report.data)
+
+        except KeyboardInterrupt:
+            pass
+
+if __name__ == '__main__':
+    # setup logging
+    log.configure()
+
+    loop = asyncio.get_event_loop()
+    loop.run_until_complete(
+        _main()
+    )