sensor-sdk 0.0.49__tar.gz → 0.0.51__tar.gz

{sensor_sdk-0.0.49/sensor_sdk.egg-info → sensor_sdk-0.0.51}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: sensor-sdk
-Version: 0.0.49
+Version: 0.0.51
 Summary: Python sdk for Synchroni
 Home-page: https://github.com/oymotion/SynchroniSDKPython
 Author: Martin Ye

{sensor_sdk-0.0.49 → sensor_sdk-0.0.51}/sensor/bleak_process.py

@@ -148,7 +148,7 @@ class BleakProcess(multiprocessing.Process):
         if self._scanner is None:
             self._scanner = BleakScanner(
                 detection_callback=_match_device,
-                service_uuids=[SERVICE_GUID, RFSTAR_SERVICE_GUID],
+                service_uuids=[RFSTAR_SERVICE_GUID, SERVICE_GUID],
             )
 
     def _ensure_gforce_loop(self):

{sensor_sdk-0.0.49 → sensor_sdk-0.0.51}/sensor/gforce.py

@@ -502,34 +502,11 @@ class GForce:
             raise ConnectionError("Connect %s fail: %s" % (self._device.name , e))
 
     def _on_data_response(self, q: queue.Queue[bytes], bs: bytearray):
-        # bs = bytes(bs)
-
-        # full_packet = []
-
-        # is_partial_data = bs[0] == ResponseCode.PARTIAL_PACKET
-        # if is_partial_data:
-        #     packet_id = bs[1]
-        #     if self.packet_id != 0 and self.packet_id != packet_id + 1:
-        #         raise Exception(
-        #             "Unexpected packet id: expected {} got {}".format(
-        #                 self.packet_id + 1,
-        #                 packet_id,
-        #             )
-        #         )
-        #     elif self.packet_id == 0 or self.packet_id > packet_id:
-        #         self.packet_id = packet_id
-        #         self.data_packet += bs[2:]
-
-        #         if self.packet_id == 0:
-        #             full_packet = self.data_packet
-        #             self.data_packet = []
-        # else:
-        #     full_packet = bs
-
-        full_packet = bs
-        if len(full_packet) == 0:
-            return
-        q.put_nowait(bytes(full_packet))
+        try:
+            q.put_nowait(bytes(bs))
+        except queue.Full:
+            while not q.empty():
+                q.get_nowait()
 
     @staticmethod
     def _convert_acceleration_to_g(data: bytes) -> np.ndarray[np.float32]:
@@ -591,7 +568,12 @@ class GForce:
         return emg_gesture_data.reshape(-1, num_channels)
 
     def _on_universal_response(self, _: BleakGATTCharacteristic, bs: bytearray):
-        self._raw_data_buf.put_nowait(bytes(bs))
+        q = self._raw_data_buf
+        try:
+            q.put_nowait(bytes(bs))
+        except queue.Full:
+            while not q.empty():
+                q.get_nowait()
 
     def _on_cmd_response(self, _: BleakGATTCharacteristic, bs: bytearray):
         sensor_utils.async_exec(self.async_on_cmd_response(bs), self.event_loop)

{sensor_sdk-0.0.49 → sensor_sdk-0.0.51}/sensor/sensor_controller.py

@@ -137,6 +137,7 @@ class SensorController:
                 if mac is None:
                     continue
                 if deviceMap.get(mac) is not None:
+                    self._sensor_profiles[mac].BLEDevice.RSSI = serialized.get("rssi")
                     devices.append(self._sensor_profiles[mac].BLEDevice)
                 else:
                     newSensor = SensorProfile(serialized=serialized, bleak_host=self._bleak_host)

{sensor_sdk-0.0.49 → sensor_sdk-0.0.51}/sensor/sensor_data_context.py

@@ -76,7 +76,10 @@ class SensorProfileDataCtx:
 
         # Quaternion support
         self.isContainQAT6 = False
-
+        self.lastQuatData: List[float] = [0.0, 0.0, 0.0, 0.0]
+        self.lastEulerData: List[float] = [0.0, 0.0, 0.0]
+        self.lastAccData: List[float] = [0.0, 0.0, 0.0]
+        self.lastGyroData: List[float] = [0.0, 0.0, 0.0]
         # PPG configuration
         self.model = PPGDataMode.PPG_RAW
 
@@ -373,6 +376,20 @@ class SensorProfileDataCtx:
             data.clear()
             self.sensorDatas[SensorDataType.DATA_TYPE_QUATERNION] = data
 
+            data = SensorData()
+            data.deviceMac = self.deviceMac
+            data.dataType = DataType.NTF_EULER_DATA
+            data.sampleRate = config.fs
+            data.resolutionBits = 32        # float32
+            data.channelCount = 3           # pitch, roll, yaw
+            data.channelMask = 0b0111
+            data.packageIndexLength = 0
+            data.minPackageSampleCount = packageCount
+            data.packageSampleCount = 1
+            data.K = 1.0
+            data.clear()
+            self.sensorDatas[SensorDataType.DATA_TYPE_EULER] = data
+
         self.notifyDataFlag |= DataSubscription.DNF_IMU
 
         return config.channel_count
@@ -549,7 +566,11 @@ class SensorProfileDataCtx:
                 info.GyroChannelCount = 3
                 info.AccSampleRate = self.sensorDatas[SensorDataType.DATA_TYPE_ACC].sampleRate
                 info.GyroSampleRate = self.sensorDatas[SensorDataType.DATA_TYPE_GYRO].sampleRate
-
+                if self.isContainQAT6:
+                    info.QuatChannelCount = 4
+                    info.EulerChannelCount = 3
+                    info.QuatSampleRate = self.sensorDatas[SensorDataType.DATA_TYPE_QUATERNION].sampleRate
+                    info.EulerSampleRate = self.sensorDatas[SensorDataType.DATA_TYPE_EULER].sampleRate
 
             if self.hasEuler() and (self.init_map["NTF_GFORCE_EULER"] == "ON"):
                 info.EulerChannelCount = await self.initEuler(packageCount)
@@ -818,16 +839,12 @@ class SensorProfileDataCtx:
         elif v == DataType.NTF_SPO2:
             self._process_spo2_data(data, buf)
         elif v == DataType.NTF_EULER_DATA:
-
             self._process_gforce_float_data(data, buf, SensorDataType.DATA_TYPE_EULER, 3)
         elif v == DataType.NTF_QUATERNION:
-
             self._process_gforce_float_data(data, buf, SensorDataType.DATA_TYPE_GFORCE_QUAT, 4)
         elif v == DataType.NTF_ACC:
-
             self._process_gforce_int_data(data, buf, SensorDataType.DATA_TYPE_ACC, 3)
         elif v == DataType.NTF_GYRO:
-
             self._process_gforce_int_data(data, buf, SensorDataType.DATA_TYPE_GYRO, 3)
 
     def _process_gforce_float_data(self, data: bytes, buf: Queue[SensorData],
@@ -838,9 +855,9 @@ class SensorProfileDataCtx:
 
         EXPECTED_SIZE = HEADER_SIZE + channel_count * FLOAT_SIZE
 
-        print(f"[DataTask] 异常: len(data){len(data)} ")
-
+        
         if len(data) != EXPECTED_SIZE:
+            print(f"[DataTask] 异常: len(data){len(data)} ")
             return
 
         sensor_data = self.sensorDatas[data_type_index]
@@ -927,8 +944,8 @@ class SensorProfileDataCtx:
             sample.isLost        = False
             sensor_data.channelSamples[ch].append(sample)
 
-        type_name = "ACC" if data[0] & 0x7F == DataType.NTF_ACC else "GYRO"
-        print(f"[{type_name}] pkgIdx={pkg_index} x={channel_values[0]} y={channel_values[1]} z={channel_values[2]}")
+        # type_name = "ACC" if data[0] & 0x7F == DataType.NTF_ACC else "GYRO"
+        # print(f"[{type_name}] pkgIdx={pkg_index} x={channel_values[0]} y={channel_values[1]} z={channel_values[2]}")
 
         sensor_data.lastPackageCounter += 1
         self.sendSensorData(sensor_data, buf)
@@ -988,17 +1005,20 @@ class SensorProfileDataCtx:
         sensor_data_acc = self.sensorDatas[SensorDataType.DATA_TYPE_ACC]
         sensor_data_gyro = self.sensorDatas[SensorDataType.DATA_TYPE_GYRO]
         sensor_data_quat = None
+        sensor_data_euler = None
 
         sensor_datas = [sensor_data_acc, sensor_data_gyro]
         if self.isContainQAT6:
             sensor_data_quat = self.sensorDatas[SensorDataType.DATA_TYPE_QUATERNION]
+            sensor_data_euler = self.sensorDatas[SensorDataType.DATA_TYPE_EULER]
             sensor_datas.append(sensor_data_quat)
+            sensor_datas.append(sensor_data_euler)
 
-        if self._check_read_imu_samples(data, sensor_datas, sensor_data_quat, on_error_callback):
+        if self._check_read_imu_samples(data, sensor_datas, sensor_data_quat, sensor_data_euler, on_error_callback):
             for sensor_data in sensor_datas:
                 self.sendSensorData(sensor_data, buf)
 
-    def _check_read_imu_samples(self, data: bytes, sensor_datas: List[SensorData], sensor_data_quat, on_error_callback=None):
+    def _check_read_imu_samples(self, data: bytes, sensor_datas: List[SensorData], sensor_data_quat, sensor_data_euler,on_error_callback=None):
         if not self._is_data_transfering or not sensor_datas:
             return False
 
@@ -1079,7 +1099,7 @@ class SensorProfileDataCtx:
                 for sensor_data in sensor_datas:
                     sensor_data.lastPackageIndex = newPackageIndex
 
-            if not self._read_imu_frame_samples(data, sensor_datas, sensor_data_quat, offset, 0):
+            if not self._read_imu_frame_samples(data, sensor_datas, sensor_data_quat, sensor_data_euler, offset, 0):
                 return False
 
             for sensor_data in sensor_datas:
@@ -1107,6 +1127,7 @@ class SensorProfileDataCtx:
             data: bytes,
             sensor_datas: List[SensorData],
             sensor_data_quat: SensorData,
+            sensor_data_euler: SensorData,
             dataOffset: int,
             lostSampleCount: int,
     ) -> bool:
@@ -1134,10 +1155,11 @@ class SensorProfileDataCtx:
             sampleIndex = lastSampleIndex + sampleOffset
 
             if lostSampleCount > 0:
-                self._append_imu_vector_sample(sensor_data_acc, sampleIndex, [0, 0, 0], True)
-                self._append_imu_vector_sample(sensor_data_gyro, sampleIndex, [0, 0, 0], True)
+                self._append_imu_vector_sample(sensor_data_acc, sampleIndex, self.lastAccData, True)
+                self._append_imu_vector_sample(sensor_data_gyro, sampleIndex, self.lastGyroData, True)
                 if sensor_data_quat is not None:
-                    self._append_imu_quaternion_sample(sensor_data_quat, sampleIndex, [0, 0, 0], True)
+                    self._append_imu_quaternion_sample(sensor_data_quat, sensor_data_euler, sampleIndex, self.lastQuatData, True)
+
                 continue
 
             accRaw = [
@@ -1145,15 +1167,17 @@ class SensorProfileDataCtx:
                 struct.unpack_from("<h", data, offset + 2)[0],
                 struct.unpack_from("<h", data, offset + 4)[0],
             ]
+            self.lastAccData = accRaw.copy()
             gyroOffset = offset + 6
             gyroRaw = [
                 struct.unpack_from("<h", data, gyroOffset)[0],
                 struct.unpack_from("<h", data, gyroOffset + 2)[0],
                 struct.unpack_from("<h", data, gyroOffset + 4)[0],
             ]
+            self.lastGyroData = gyroRaw.copy()
 
-            self._append_imu_vector_sample(sensor_data_acc, sampleIndex, accRaw, False)
-            self._append_imu_vector_sample(sensor_data_gyro, sampleIndex, gyroRaw, False)
+            self._append_imu_vector_sample(sensor_data_acc, sampleIndex, self.lastAccData, False)
+            self._append_imu_vector_sample(sensor_data_gyro, sampleIndex, self.lastGyroData, False)
 
             if sensor_data_quat is not None:
                 quatOffset = offset + 12
@@ -1162,7 +1186,7 @@ class SensorProfileDataCtx:
                     struct.unpack_from("<i", data, quatOffset + 4)[0],
                     struct.unpack_from("<i", data, quatOffset + 8)[0],
                 ]
-                self._append_imu_quaternion_sample(sensor_data_quat, sampleIndex, quatRaw, False)
+                self._append_imu_quaternion_sample(sensor_data_quat, sensor_data_euler, sampleIndex, quatRaw, False)
 
             offset += frameSize
 
@@ -1191,6 +1215,7 @@ class SensorProfileDataCtx:
     def _append_imu_quaternion_sample(
             self,
             sensor_data_quat: SensorData,
+            sensor_data_euler: SensorData,
             sampleIndex: int,
             rawValues: List[int],
             isLost: bool,
@@ -1200,16 +1225,25 @@ class SensorProfileDataCtx:
 
         raw = [0, 0, 0, 0]
         value = [0.0, 0.0, 0.0, 0.0]
+        euler = [0.0, 0.0, 0.0]
 
         if not isLost:
             raw[1] = rawValues[0]
             raw[2] = rawValues[1]
             raw[3] = rawValues[2]
+            
             value[1] = raw[1] / scale
             value[2] = raw[2] / scale
             value[3] = raw[3] / scale
             value[0] = math.sqrt(max(0.0, 1.0 - value[1] ** 2 - value[2] ** 2 - value[3] ** 2))
 
+            euler[0] = math.atan2(2 * (value[0] * value[1] + value[2] * value[3]), 1 - 2 * (value[1] ** 2 + value[2] ** 2)) * 180 / math.pi
+            euler[1] = math.asin(max(-1.0, min(1.0, 2 * (value[0] * value[2] - value[3] * value[1])))) * 180 / math.pi
+            euler[2] = math.atan2(2 * (value[0] * value[3] + value[1] * value[2]), 1 - 2 * (value[2] ** 2 + value[3] ** 2)) * 180 / math.pi
+
+            self.lastQuatData = value.copy()
+            self.lastEulerData = euler.copy()
+
         for channelIndex in range(sensor_data_quat.channelCount):
             if (sensor_data_quat.channelMask & (1 << channelIndex)) == 0:
                 continue
@@ -1219,12 +1253,27 @@ class SensorProfileDataCtx:
             sample.sampleIndex = sampleIndex
             sample.timeStampInMs = sampleIndex * sampleInterval
             sample.rawData = raw[channelIndex]
-            sample.data = value[channelIndex]
+            sample.data = self.lastQuatData[channelIndex]
             sample.impedance = 0.0
             sample.saturation = 0.0
             sample.isLost = isLost
             sensor_data_quat.channelSamples[channelIndex].append(sample)
 
+        for channelIndex in range(sensor_data_euler.channelCount):
+            if (sensor_data_euler.channelMask & (1 << channelIndex)) == 0:
+                continue
+
+            sample = Sample()
+            sample.channelIndex = channelIndex
+            sample.sampleIndex = sampleIndex
+            sample.timeStampInMs = sampleIndex * sampleInterval
+            sample.rawData = raw[channelIndex]
+            sample.data = self.lastEulerData[channelIndex]
+            sample.impedance = 0.0
+            sample.saturation = 0.0
+            sample.isLost = isLost
+            sensor_data_euler.channelSamples[channelIndex].append(sample)
+
     def checkReadSamples(self, data: bytes, sensorData: SensorData, dataOffset: int, dataGap: int, on_error_callback=None):
         offset = 1
 

{sensor_sdk-0.0.49 → sensor_sdk-0.0.51}/sensor/sensor_profile.py

@@ -127,7 +127,9 @@ class SensorProfile:
             new_state = _STATE_NAME_MAP.get(state_name, self._device_state)
             self._set_device_state(new_state)
         elif msg_type == "power_changed":
-            self._power = msg.get("power", -1)
+            power = msg.get("power", -1)
+            if power < self._power or self._power == -1:
+                self._power = power
             if self._on_power_changed is not None:
                 try:
                     self._on_power_changed(self, self._power)

{sensor_sdk-0.0.49 → sensor_sdk-0.0.51/sensor_sdk.egg-info}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: sensor-sdk
-Version: 0.0.49
+Version: 0.0.51
 Summary: Python sdk for Synchroni
 Home-page: https://github.com/oymotion/SynchroniSDKPython
 Author: Martin Ye

{sensor_sdk-0.0.49 → sensor_sdk-0.0.51}/setup.py

@@ -8,7 +8,7 @@ with open(os.path.join(this_directory, "README.md"), "r", encoding="utf-8") as f
 
 setup(
     name="sensor-sdk",
-    version="0.0.49",
+    version="0.0.51",
     description="Python sdk for Synchroni",
     long_description=long_description,
     long_description_content_type="text/markdown",