Merge pull request #29 from TapWithUs/28-native-raw-sensor-data-scaling

optional scaling for raw data

Author: ilouzl

History.md

@@ -7,13 +7,9 @@ ______________________
 * Mac and Linux backends unified to posix backend.
 
 ### Known Issues
-* Windows backend -  
-    * Raw sensor data rate might be lower than expected.
-    * Sometimes a Tap strap wouldn't be detected upon connection. In this case try restarting your Tap and/or the Python application. In worst case scenario re-pair your Tap. 
-    * Spatial features are still not available for Windows backend.
-* MacOS & Linux backends - 
-    * Doesn't support multiple Tap strap connections.
-    * Raw sensor data is given unscaled (i.e. unitless), thereforein order to scale to physical units need to multiply by the relevant scale factor
+* Doesn't support multiple Tap strap connections.
+* Windows backend can only connect with a Tap that is not connected to the PC.
+
 
 ## 0.5.1 (2024-01-01)
 ______________________

Readme.md

@@ -85,9 +85,9 @@ For example
     from tapsdk import TapInputMode
     tap_device.set_input_mode(TapInputMode("controller"))
     ```  
-    Also, when instantiating a ```TapInputMode``` for raw sensors mode, additional argument ```sensitivity``` (list with 3 integers) is optional for example
+    Also, when instantiating a ```TapInputMode``` for raw sensors mode, additional arguments ```sensitivity``` (list with 3 integers) and ```scaled``` are optional. When ```scaled``` is set to ```True```, the IMU values will be converted to dps and g according to the given sensitivity.
     ```python
-    tap_device.set_input_mode(TapInputMode("raw", sensitivity=[2,1,4]))
+    tap_device.set_input_mode(TapInputMode("raw", sensitivity=[2,1,4], scaled=True))
 
 2. ```set_input_type(self, input_type:InputType, identifier):```   
     > **Only for TapXR and with Spatial Control experimental firmware**

examples/basic.py

@@ -77,7 +77,7 @@ async def run():
 
     logger.info("Set Raw Mode for 5 seconds")
     await asyncio.sleep(2)
-    await client.set_input_mode(TapInputMode("raw", sensitivity=[0, 0, 0]))
+    await client.set_input_mode(TapInputMode("raw", sensitivity=[0, 0, 0], scaled=True))
     await asyncio.sleep(5)
 
 

tapsdk/inputmodes.py

@@ -5,18 +5,19 @@
 
 
 class TapInputMode:
-    def __init__(self, mode, sensitivity=[0, 0, 0]):
+    def __init__(self, mode, sensitivity=None, scaled=False):
         self._modes = {
                 "text": {"name": "Text Mode", "code": bytearray([0x3, 0xc, 0x0, 0x0])},
                 "controller": {"name": "Controller Mode", "code": bytearray([0x3, 0xc, 0x0, 0x1])},
                 "controller_text": {"name": "Controller and Text Mode", "code": bytearray([0x3, 0xc, 0x0, 0x3])},
                 "raw": {"name": "Raw sensors Mode", "code": bytearray([0x3, 0xc, 0x0, 0xa])}
                 }
-        self.sensitivity = sensitivity
+        self.sensitivity = sensitivity or [0, 0, 0]
+        self.scaled = scaled
         if mode in self._modes.keys():
             self.mode = mode
             if mode == "raw":
-                self._register_sensitivity(sensitivity)
+                self._register_sensitivity(self.sensitivity)
         else:
             logger.warning("Invalid mode \"%s\". Set to \"text\"", mode)
             self.mode = "text"

tapsdk/parsers.py

@@ -17,9 +17,10 @@ def tap_data_msg(data: bytearray):
     return [data[0]]
 
 
-def raw_data_msg(data: bytearray):
+def raw_data_msg(data: bytearray, scaled: bool = False, sensitivity=None):
     '''
-    raw data is packed into messages with the following structure:
+    Parses raw data messages into structured data with optional scaling.
+    Raw data is packed into messages with the following structure:
          [msg_type (1 bit)][timestamp (31 bit)][payload (12 - 30 bytes)]
              * msg type     - '0' for imu message
                         - '1' for accelerometers message
@@ -34,6 +35,13 @@ def raw_data_msg(data: bytearray):
                                         ...]
 
     '''
+    if sensitivity is None:
+        sensitivity = [0, 0, 0]
+
+    finger_acc_scales = [31.25, 3.91, 7.81, 15.62, 31.25]
+    gyro_scales = [17.5, 4.375, 8.75, 17.5, 35, 70]
+    imu_acc_scales = [0.122, 0.061, 0.122, 0.244, 0.488]
+
     L = len(data)
     ptr = 0
     messages = []
@@ -56,8 +64,19 @@ def raw_data_msg(data: bytearray):
         # parse payload
         payload = []
         for i in range(num_of_samples):
-            payload.append(int.from_bytes(data[ptr:ptr+2], "little", signed=True))
+            val = int.from_bytes(data[ptr:ptr+2], "little", signed=True)
             ptr += 2
+            payload.append(val)
+
+        if scaled:
+            if msg == "imu":
+                g_scale = gyro_scales[sensitivity[1]] / 1000.0
+                xl_scale = imu_acc_scales[sensitivity[2]] / 1000.0
+                payload = [payload[j] * g_scale if j < 3 else payload[j] * xl_scale
+                           for j in range(num_of_samples)]
+            else:  # accl message
+                acc_scale = finger_acc_scales[sensitivity[0]] / 1000.0
+                payload = [v * acc_scale for v in payload]
 
         messages.append({"type": msg, "ts": ts, "payload": payload})
     return messages

tapsdk/tap.py

@@ -178,7 +178,12 @@ def on_tapped(self, identifier, data):
 
     def on_raw_data(self, identifier, data):
         if self.raw_data_event_cb:
-            args = parsers.raw_data_msg(data)
+            scale = False
+            sensitivity = [0, 0, 0]
+            if isinstance(self.input_mode, TapInputMode) and self.input_mode.mode == "raw":
+                scale = getattr(self.input_mode, "scaled", scale)
+                sensitivity = getattr(self.input_mode, "sensitivity", sensitivity)
+            args = parsers.raw_data_msg(data, scaled=scale, sensitivity=sensitivity)
             self.raw_data_event_cb(identifier, args)
 
     def on_air_gesture(self, identifier, data):

tests/test_inputmodes.py

@@ -13,5 +13,11 @@ def test_input_mode_raw_with_sensitivity():
     assert mode.get_command() == bytearray([0x3, 0xc, 0x0, 0xa, 1, 2, 3])
 
 
+def test_input_mode_raw_scaled():
+    mode = TapInputMode("raw", scaled=True)
+    assert mode.scaled is True
+    assert mode.get_command() == bytearray([0x3, 0xc, 0x0, 0xa, 0, 0, 0])
+
+
 def test_input_type_command():
     assert input_type_command(InputType.MOUSE) == bytearray([0x3, 0xd, 0x0, InputType.MOUSE.value])

tests/test_parsers.py

@@ -53,3 +53,24 @@ def test_raw_data_msg():
         {'type': 'imu', 'ts': 123, 'payload': imu_samples},
         {'type': 'accl', 'ts': 456, 'payload': accl_samples}
     ]
+
+
+def test_raw_data_msg_scaled():
+    ts = 123
+    imu_ts = ts
+    imu_bytes = imu_ts.to_bytes(4, 'little', signed=False)
+    imu_samples = [100, -100, 200, -200, 300, -300]
+    payload = b''
+    for v in imu_samples:
+        payload += v.to_bytes(2, 'little', signed=True)
+    packet = bytearray(imu_bytes + payload)
+    result = parsers.raw_data_msg(packet, scaled=True, sensitivity=[0, 0, 0])
+    g_scale = 17.5 / 1000
+    a_scale = 0.122 / 1000
+    expected = [imu_samples[i] * g_scale if i < 3 else imu_samples[i] * a_scale
+                for i in range(6)]
+    assert result == [{
+        'type': 'imu',
+        'ts': 123,
+        'payload': expected
+    }]