sensor-sdk 0.0.50__tar.gz → 0.0.52__tar.gz

{sensor_sdk-0.0.50/sensor_sdk.egg-info → sensor_sdk-0.0.52}/PKG-INFO

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

{sensor_sdk-0.0.50 → sensor_sdk-0.0.52}/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.50 → sensor_sdk-0.0.52}/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
 
@@ -323,6 +326,7 @@ class SensorProfileDataCtx:
 
     async def initIMU(self, packageCount: int) -> int:
         IMU_TYPE_QAT6 = 0x0004
+        min_package_sample_count = 2
         self.isContainQAT6 = False
         config = await self.gForce.get_imu_raw_data_config()
         imu_cap = await self.gForce.get_imu_cap_data_config()
@@ -339,7 +343,7 @@ class SensorProfileDataCtx:
         data.resolutionBits = 16
         data.channelCount = 3
         data.channelMask = 255
-        data.minPackageSampleCount = 1
+        data.minPackageSampleCount = min_package_sample_count
         data.packageSampleCount = config.batch_len
         data.K = config.accK
         data.clear()
@@ -352,7 +356,7 @@ class SensorProfileDataCtx:
         data.resolutionBits = 16
         data.channelCount = 3
         data.channelMask = 255
-        data.minPackageSampleCount = 1
+        data.minPackageSampleCount = min_package_sample_count
         data.packageSampleCount = config.batch_len
         data.K = config.gyroK
         data.clear()
@@ -367,12 +371,26 @@ class SensorProfileDataCtx:
             data.resolutionBits = 32
             data.channelCount = 4  # w, x, y, z
             data.channelMask = 15  # 0b1111
-            data.minPackageSampleCount = 1
+            data.minPackageSampleCount = min_package_sample_count
             data.packageSampleCount = config.batch_len
             data.K = 1.0 / 1073741824.0  # 1 / 2^30
             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 = min_package_sample_count
+            data.packageSampleCount = config.batch_len
+            data.K = 1.0
+            data.clear()
+            self.sensorDatas[SensorDataType.DATA_TYPE_EULER] = data
+
         self.notifyDataFlag |= DataSubscription.DNF_IMU
 
         return config.channel_count
@@ -389,7 +407,7 @@ class SensorProfileDataCtx:
         data.channelCount = 3           # roll, pitch, yaw
         data.channelMask = 0b0111
         data.packageIndexLength = 0
-        data.minPackageSampleCount = packageCount
+        data.minPackageSampleCount = 2
         data.packageSampleCount = 1
         data.K = 1.0
         data.clear()
@@ -409,7 +427,7 @@ class SensorProfileDataCtx:
         data.channelCount = 4           # w, x, y, z
         data.channelMask = 0b1111
         data.packageIndexLength = 0
-        data.minPackageSampleCount = packageCount
+        data.minPackageSampleCount = 2
         data.packageSampleCount = 1
         data.K = 1.0
         data.clear()
@@ -427,7 +445,7 @@ class SensorProfileDataCtx:
         data.channelCount = 3           # x, y, z
         data.channelMask = 0b0111
         data.packageIndexLength = 1
-        data.minPackageSampleCount = packageCount
+        data.minPackageSampleCount = 2
         data.packageSampleCount = 1
         data.K = 1.0
         data.clear()
@@ -445,7 +463,7 @@ class SensorProfileDataCtx:
         data.channelCount = 3           # x, y, z
         data.channelMask = 0b0111
         data.packageIndexLength = 1
-        data.minPackageSampleCount = packageCount
+        data.minPackageSampleCount = 2
         data.packageSampleCount = 1
         data.K = 1.0
         data.clear()
@@ -479,7 +497,7 @@ class SensorProfileDataCtx:
         data.resolutionBits = 8
         data.channelCount = 1
         data.channelMask = 1
-        data.minPackageSampleCount = 1
+        data.minPackageSampleCount = 2
         data.packageSampleCount = 1
         data.K = 1
         data.packageIndexLength = 2
@@ -549,7 +567,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)
@@ -686,7 +708,8 @@ class SensorProfileDataCtx:
                         break
                     if not sensor_utils._terminated and sensorData != None and on_data_callback != None:
                         try:
-                            asyncio.get_event_loop().run_in_executor(self.dataPool, on_data_callback, sensorData)
+                            on_data_callback(sensorData)
+                            # asyncio.get_event_loop().run_in_executor(self.dataPool, on_data_callback, sensorData)
                         except Exception as e:
                             pass
 
@@ -818,16 +841,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 +857,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 +946,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 +1007,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 +1101,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 +1129,7 @@ class SensorProfileDataCtx:
             data: bytes,
             sensor_datas: List[SensorData],
             sensor_data_quat: SensorData,
+            sensor_data_euler: SensorData,
             dataOffset: int,
             lostSampleCount: int,
     ) -> bool:
@@ -1134,10 +1157,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 +1169,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 +1188,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 +1217,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 +1227,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 +1255,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
 
@@ -1489,7 +1540,8 @@ class SensorProfileDataCtx:
                         break
                     if not sensor_utils._terminated and sensorData != None and on_data_callback != None:
                         try:
-                            asyncio.get_event_loop().run_in_executor(self.dataPool, on_data_callback, sensorData)
+                            on_data_callback(sensorData)
+                            # asyncio.get_event_loop().run_in_executor(self.dataPool, on_data_callback, sensorData)
                         except Exception as e:
                             raise RuntimeError("Data callback %s fail: %s" % (self._device_info.DeviceName, e))
 

{sensor_sdk-0.0.50 → sensor_sdk-0.0.52}/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.50 → sensor_sdk-0.0.52/sensor_sdk.egg-info}/PKG-INFO

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

{sensor_sdk-0.0.50 → sensor_sdk-0.0.52}/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.50",
+    version="0.0.52",
     description="Python sdk for Synchroni",
     long_description=long_description,
     long_description_content_type="text/markdown",