sensor-sdk 0.0.65__tar.gz → 0.0.67__tar.gz

{sensor_sdk-0.0.65/sensor_sdk.egg-info → sensor_sdk-0.0.67}/PKG-INFO

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

{sensor_sdk-0.0.65 → sensor_sdk-0.0.67}/sensor/sensor_data.py

@@ -30,7 +30,6 @@ class Sample:
         self.isLost = False
         self.timeStampInMs = 0
         self.channelIndex = 0
-        self.sampleIndex = 0
 
 
 
@@ -94,6 +93,7 @@ class SensorData:
         self.lastPackageIndex = 0
         self.lostPackageCount = 0
         self.resolutionBits = 0
+        self.resolutionSigned = 0
         self.channelMask = 0
         self.minPackageSampleCount = 0
         self.K = 0

{sensor_sdk-0.0.65 → sensor_sdk-0.0.67}/sensor/sensor_data_context.py

@@ -16,6 +16,7 @@ from enum import Enum, IntEnum
 
 from sensor.sensor_device import DeviceInfo
 
+QUAT_SCALE = 1 / 1073741824.0  # 2^30
 
 class SensorDataType(IntEnum):
     DATA_TYPE_EEG = 0
@@ -76,12 +77,8 @@ 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
+        self.ppgModel = PPGDataMode.PPG_AND_SPO2
 
         self.sensorDatas: List[SensorData] = list()
         for idx in range(0, SensorDataType.DATA_TYPE_COUNT):
@@ -177,6 +174,7 @@ class SensorProfileDataCtx:
         data.dataType = DataType.NTF_EMG
         data.sampleRate = 500
         data.resolutionBits = 0
+        data.resolutionSigned = 0
         data.channelCount = 8
         data.channelMask = config.channel_mask
         data.minPackageSampleCount = packageCount
@@ -198,6 +196,7 @@ class SensorProfileDataCtx:
             # new emg
             data.packageIndexLength = 2
             data.resolutionBits = 0
+            data.resolutionSigned = 1
             gain = 6
             data.K = 4000000.0 / 8388607.0 / gain
             config.resolution = 8
@@ -205,6 +204,7 @@ class SensorProfileDataCtx:
             # old emg
             data.packageIndexLength = 1
             data.resolutionBits = 7
+            data.resolutionSigned = 1
             gain = 1200
             min_voltage = -1.25 * 1000000
             max_voltage = 1.25 * 100000
@@ -242,6 +242,7 @@ class SensorProfileDataCtx:
         data.dataType = DataType.NTF_EEG
         data.sampleRate = config.fs
         data.resolutionBits = config.resolution
+        data.resolutionSigned = 1
         data.channelCount = cap.channel_count
         data.channelMask = config.channel_mask
         data.minPackageSampleCount = packageCount
@@ -259,6 +260,7 @@ class SensorProfileDataCtx:
         data.dataType = DataType.NTF_ECG
         data.sampleRate = config.fs
         data.resolutionBits = config.resolution
+        data.resolutionSigned = 1   
         data.channelCount = 1
         data.channelMask = config.channel_mask
         data.minPackageSampleCount = packageCount
@@ -270,63 +272,54 @@ class SensorProfileDataCtx:
         return data.channelCount
     async def initPPG(self, packageCount: int) -> int:
 
-
         config = await self.gForce.get_ppg_raw_data_config()
-        
-
-        config.fs = self.sampleRate
-        config.mode = self.model
-        
+        config.mode = self.ppgModel
+        config.period = 1
+        config.fs = 50
+        await self.gForce.set_ppg_raw_data_config(config)
 
         data = SensorData()
+        data.dataType = DataType.NTF_PPG
         data.deviceMac = self.deviceMac
         data.sampleRate = config.fs
         data.channelMask = 255
         data.minPackageSampleCount = packageCount
         data.packageSampleCount = config.batch_len
         data.K = 1.0
-        if self.model == PPGDataMode.PPG_RAW:
-
-            data.dataType = DataType.NTF_PPG
-            data.resolutionBits = 24
-            data.channelCount = 2
-            self.sensorDatas[SensorDataType.DATA_TYPE_PPG] = data
-        elif self.model == PPGDataMode.SPO2_AND_HR:
-            data.dataType = DataType.NTF_SPO2
-            config.period = self.period
-            config.fs = SamplingRate.HZ_100
-            data.resolutionBits = 16
-            data.channelCount = 2
-            self.sensorDatas[SensorDataType.DATA_TYPE_SPO2] = data
-        else:
-
-            data.dataType = DataType.NTF_PPG
-            data.resolutionBits = 24
-            data.channelCount = 2
-            self.sensorDatas[SensorDataType.DATA_TYPE_PPG] = data
-            data.dataType = DataType.NTF_SPO2
-            config.period = self.period
-            config.fs = self.sampleRate
-            data.resolutionBits = 16
-            data.channelCount = 1
-            self.sensorDatas[SensorDataType.DATA_TYPE_SPO2] = data
+        data.resolutionBits = 24
+        data.resolutionSigned = 0
+        data.channelCount = 2
+        self.sensorDatas[SensorDataType.DATA_TYPE_PPG] = data
+        data.clear()
 
-        await self.gForce.set_ppg_raw_data_config(config)
-        self.notifyDataFlag=DataSubscription.DNF_PPG;
+        data = SensorData()
+        data.dataType = DataType.NTF_SPO2
+        data.deviceMac = self.deviceMac
+        data.sampleRate = config.period
+        data.channelMask = 255
+        data.minPackageSampleCount = 1
+        data.packageSampleCount = config.batch_len
+        data.K = 1.0
+        data.resolutionBits = 16
+        data.resolutionSigned = 1
+        data.channelCount = 2
+        self.sensorDatas[SensorDataType.DATA_TYPE_SPO2] = data
         data.clear()
+
+        self.notifyDataFlag |= DataSubscription.DNF_PPG;
         
         return data.channelCount
 
-    def setPPGMode(self, mode: int, sampleRate: int = None, period: int = None):
+    # def setPPGMode(self, mode: int, sampleRate: int = None, period: int = None):
 
-        if mode not in [PPGDataMode.SPO2_AND_HR, PPGDataMode.PPG_RAW]:
-            raise ValueError(f"Invalid PPG mode: {mode}. Must be 0 (SPO2AndHR) or 1 (PPGRaw)")
+    #     if mode not in [PPGDataMode.SPO2_AND_HR, PPGDataMode.PPG_RAW]:
+    #         raise ValueError(f"Invalid PPG mode: {mode}. Must be 0 (SPO2AndHR) or 1 (PPGRaw)")
         
-        self.model = mode
-        self.period = period
-        self.sampleRate = sampleRate
+    #     self.ppgModel = mode
+    #     self.period = period
+    #     self.sampleRate = sampleRate
         
-        print(f"PPG mode set to: {'SPO2AndHR' if mode == 0 else 'PPGRaw'}, Sample rate: {self.sampleRate}Hz")
+    #     print(f"PPG mode set to: {'SPO2AndHR' if mode == 0 else 'PPGRaw'}, Sample rate: {self.sampleRate}Hz")
 
 
     async def initIMU(self, packageCount: int) -> int:
@@ -346,6 +339,7 @@ class SensorProfileDataCtx:
         data.dataType = DataType.NTF_ACC
         data.sampleRate = config.fs
         data.resolutionBits = 16
+        data.resolutionSigned = 1
         data.channelCount = 3
         data.channelMask = 255
         data.minPackageSampleCount = min_package_sample_count
@@ -359,6 +353,7 @@ class SensorProfileDataCtx:
         data.dataType = DataType.NTF_GYRO
         data.sampleRate = config.fs
         data.resolutionBits = 16
+        data.resolutionSigned = 1
         data.channelCount = 3
         data.channelMask = 255
         data.minPackageSampleCount = min_package_sample_count
@@ -373,9 +368,10 @@ class SensorProfileDataCtx:
             data.deviceMac = self.deviceMac
             data.dataType = DataType.NTF_QUATERNION
             data.sampleRate = config.fs
-            data.resolutionBits = 32
+            data.resolutionBits = 31 # 32-bit signed integer
+            data.resolutionSigned = 1
             data.channelCount = 4  # w, x, y, z
-            data.channelMask = 15  # 0b1111
+            data.channelMask = 0b1110  # we don't read the first channel for quaternion data
             data.minPackageSampleCount = min_package_sample_count
             data.packageSampleCount = config.batch_len
             data.K = 1.0 / 1073741824.0  # 1 / 2^30
@@ -386,7 +382,8 @@ class SensorProfileDataCtx:
             data.deviceMac = self.deviceMac
             data.dataType = DataType.NTF_EULER_DATA
             data.sampleRate = config.fs
-            data.resolutionBits = 32        # float32
+            data.resolutionBits = 0
+            data.resolutionSigned = 1
             data.channelCount = 3           # pitch, roll, yaw
             data.channelMask = 0b0111
             data.packageIndexLength = 0
@@ -411,8 +408,8 @@ class SensorProfileDataCtx:
         data.resolutionBits = 32        # float32
         data.channelCount = 3           # roll, pitch, yaw
         data.channelMask = 0b0111
-        data.packageIndexLength = 0
-        data.minPackageSampleCount = 2
+        data.packageIndexLength = 1
+        data.minPackageSampleCount = 1
         data.packageSampleCount = 1
         data.K = 1.0
         data.clear()
@@ -431,8 +428,8 @@ class SensorProfileDataCtx:
         data.resolutionBits = 32        # float32
         data.channelCount = 4           # w, x, y, z
         data.channelMask = 0b1111
-        data.packageIndexLength = 0
-        data.minPackageSampleCount = 2
+        data.packageIndexLength = 1
+        data.minPackageSampleCount = 1
         data.packageSampleCount = 1
         data.K = 1.0
         data.clear()
@@ -446,13 +443,14 @@ class SensorProfileDataCtx:
         data.deviceMac = self.deviceMac
         data.dataType = DataType.NTF_ACC
         data.sampleRate = 40
-        data.resolutionBits = 32        # int32
+        data.resolutionBits = 31        # int32
+        data.resolutionSigned = 1
         data.channelCount = 3           # x, y, z
         data.channelMask = 0b0111
         data.packageIndexLength = 1
-        data.minPackageSampleCount = 2
+        data.minPackageSampleCount = 1
         data.packageSampleCount = 1
-        data.K = 1.0
+        data.K = 1.0 / 65536.0
         data.clear()
         self.sensorDatas[SensorDataType.DATA_TYPE_ACC] = data
         self.notifyDataFlag |= DataSubscription.ACCELERATE
@@ -464,13 +462,14 @@ class SensorProfileDataCtx:
         data.deviceMac = self.deviceMac
         data.dataType = DataType.NTF_GYRO
         data.sampleRate = 40
-        data.resolutionBits = 32        # int32
+        data.resolutionBits = 31        # int32
+        data.resolutionSigned = 1
         data.channelCount = 3           # x, y, z
         data.channelMask = 0b0111
         data.packageIndexLength = 1
-        data.minPackageSampleCount = 2
+        data.minPackageSampleCount = 1
         data.packageSampleCount = 1
-        data.K = 1.0
+        data.K = 1.0 / 65536.0
         data.clear()
         self.sensorDatas[SensorDataType.DATA_TYPE_GYRO] = data
         self.notifyDataFlag |= DataSubscription.GYROSCOPE
@@ -484,9 +483,10 @@ class SensorProfileDataCtx:
         data.dataType = DataType.NTF_BRTH
         data.sampleRate = config.fs
         data.resolutionBits = config.resolution
+        data.resolutionSigned = 1
         data.channelCount = 1
         data.channelMask = config.channel_mask
-        data.minPackageSampleCount = packageCount
+        data.minPackageSampleCount = 1
         data.packageSampleCount = config.batch_len
         data.K = config.K
         data.clear()
@@ -500,9 +500,10 @@ class SensorProfileDataCtx:
         data.dataType = DataType.NTF_MAG_ANGLE_DATA
         data.sampleRate = 40
         data.resolutionBits = 8
+        data.resolutionSigned = 0
         data.channelCount = 1
         data.channelMask = 1
-        data.minPackageSampleCount = 2
+        data.minPackageSampleCount = 1
         data.packageSampleCount = 1
         data.K = 1
         data.packageIndexLength = 2
@@ -599,15 +600,16 @@ class SensorProfileDataCtx:
             if self.hasPPG() and (self.init_map["NTF_PPG_RAW"] == "ON"):
                 info.PpgChannelCount = await self.initPPG(packageCount)
 
-                if self.model == PPGDataMode.PPG_RAW:
+                if self.ppgModel == PPGDataMode.PPG_RAW:
 
                     info.Spo2SampleRate = self.sensorDatas[SensorDataType.DATA_TYPE_SPO2].sampleRate
-                if self.model == PPGDataMode.SPO2_AND_HR:
+                if self.ppgModel == PPGDataMode.SPO2_AND_HR:
 
                     info.PpgSampleRate = self.sensorDatas[SensorDataType.DATA_TYPE_SPO2].sampleRate
                 else:
                     info.PpgSampleRate = self.sensorDatas[SensorDataType.DATA_TYPE_PPG].sampleRate
                     info.Spo2SampleRate = self.sensorDatas[SensorDataType.DATA_TYPE_SPO2].sampleRate
+                    
             if self.hasMagAngle() and (self.init_map["NTF_MAG_ANGLE"] == "ON"):
                 magAngleChannelCount = await self.initMagAngle(packageCount)
                 info.MagAngleChannelCount = magAngleChannelCount
@@ -848,277 +850,22 @@ class SensorProfileDataCtx:
         elif v == DataType.NTF_SPO2 and self.hasPPG():
             self._process_spo2_data(data, buf)
         elif v == DataType.NTF_EULER_DATA and self.hasEuler():
-            self._process_gforce_float_data(data, buf, SensorDataType.DATA_TYPE_EULER, 3)
+            sensor_data = self.sensorDatas[SensorDataType.DATA_TYPE_EULER]
+            if self.checkReadSamples(data, sensor_data, sensor_data.packageIndexLength + 1, 0, on_error_callback):
+                self.sendSensorData(sensor_data, buf)
         elif v == DataType.NTF_QUATERNION and self.hasQuat():
-            self._process_gforce_float_data(data, buf, SensorDataType.DATA_TYPE_GFORCE_QUAT, 4)
+            sensor_data = self.sensorDatas[SensorDataType.DATA_TYPE_GFORCE_QUAT]
+            if self.checkReadSamples(data, sensor_data, sensor_data.packageIndexLength + 1, 0, on_error_callback):
+                self.sendSensorData(sensor_data, buf)
         elif v == DataType.NTF_ACC and self.hasAcc():
-            self._process_gforce_int_data(data, buf, SensorDataType.DATA_TYPE_ACC, 3)
+            sensor_data = self.sensorDatas[SensorDataType.DATA_TYPE_ACC]
+            if self.checkReadSamples(data, sensor_data, sensor_data.packageIndexLength + 1, 0, on_error_callback):
+                self.sendSensorData(sensor_data, buf)
         elif v == DataType.NTF_GYRO and self.hasGyro():
-            self._process_gforce_int_data(data, buf, SensorDataType.DATA_TYPE_GYRO, 3)
-
-    def _process_gforce_float_data(self, data: bytes, buf: Queue[SensorData],
-                                    data_type_index: int, channel_count: int):
-
-        HEADER_SIZE = 2
-        FLOAT_SIZE = 4
-
-        EXPECTED_SIZE = HEADER_SIZE + channel_count * FLOAT_SIZE
-
-        
-        if len(data) != EXPECTED_SIZE:
-            print(f"[DataTask] 异常: len(data){len(data)} ")
-            return
-
-        sensor_data = self.sensorDatas[data_type_index]
-
-
-        if not sensor_data.channelSamples:
-            sensor_data.channelSamples = [[] for _ in range(channel_count)]
-
-        if sensor_data.lastPackageCounter < 0:
-            sensor_data.lastPackageCounter = 0
-
-        offset = HEADER_SIZE
-        sample_idx = sensor_data.lastPackageCounter
-        interval_ms = (1000 // sensor_data.sampleRate) if sensor_data.sampleRate > 0 else 0
-
-        for ch in range(channel_count):
-            raw_float = struct.unpack_from("<f", data, offset)[0]
-            offset += FLOAT_SIZE
-            val = raw_float if math.isfinite(raw_float) else 0.0
-
-            sample = Sample()
-            sample.channelIndex = ch
-            sample.sampleIndex = sample_idx
-            sample.timeStampInMs = sample_idx * interval_ms
-            sample.rawData = raw_float
-            sample.data = val
-            sample.impedance = 0.0
-            sample.saturation = 0.0
-            sample.isLost = False
-            sensor_data.channelSamples[ch].append(sample)
-
-        sensor_data.lastPackageCounter += 1
-        self.sendSensorData(sensor_data, buf)
-
-    def _process_gforce_int_data(self, data: bytes, buf: Queue[SensorData],
-                                  data_type_index: int, channel_count: int):
-
-        HEADER_SIZE  = 1
-        PKG_IDX_SIZE = 1
-        INT_SIZE     = 4
-        EXPECTED_SIZE = HEADER_SIZE + PKG_IDX_SIZE + channel_count * INT_SIZE
-
-        if len(data) != EXPECTED_SIZE:
-            print(f"[INT_DATA] type=0x{data[0]:02X} 包长不匹配 expected={EXPECTED_SIZE} actual={len(data)}")
-            return
-
-        sensor_data = self.sensorDatas[data_type_index]
-
-
-        pkg_index = data[HEADER_SIZE] & 0xFF
-
-
-        if not sensor_data.channelSamples:
-            sensor_data.channelSamples = [[] for _ in range(channel_count)]
-
-
-        if sensor_data.lastPackageCounter < 0:
-            sensor_data.lastPackageCounter = 0
-            sensor_data.lastPackageIndex = pkg_index
-        else:
-            if pkg_index == sensor_data.lastPackageIndex:
-                return
-        sensor_data.lastPackageIndex = pkg_index
-
-
-        offset = HEADER_SIZE + PKG_IDX_SIZE
-        sample_idx = sensor_data.lastPackageCounter
-        interval_ms = (1000 // sensor_data.sampleRate) if sensor_data.sampleRate > 0 else 0
-
-        channel_values = []
-        for ch in range(channel_count):
-            raw_int = struct.unpack_from("<i", data, offset)[0]
-            offset += INT_SIZE
-            channel_values.append(raw_int)
-
-            sample = Sample()
-            sample.channelIndex  = ch
-            sample.sampleIndex   = sample_idx
-            sample.timeStampInMs = sample_idx * interval_ms
-            sample.rawData       = raw_int
-            sample.data          = float(raw_int)
-            sample.impedance     = 0.0
-            sample.saturation    = 0.0
-            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]}")
-
-        sensor_data.lastPackageCounter += 1
-        self.sendSensorData(sensor_data, buf)
-
-    def _process_spo2_data(self, data: bytes, buf: Queue[SensorData]):
-
-
-        DATA_HEADER_SIZE = 3
-        SPO2_CHANNEL_INDEX = 0
-        HEART_RATE_CHANNEL_INDEX = 1
-        REQUIRED_DATA_SIZE = 4
-
-        offset = DATA_HEADER_SIZE
-
-        if len(data) < offset + REQUIRED_DATA_SIZE:
-            return
-
-
-        spo2_value = struct.unpack_from(">H", data, offset)[0]
-        offset += 2
-        heart_rate = struct.unpack_from(">H", data, offset)[0]
-
-
-        sensor_data = self.sensorDatas[SensorDataType.DATA_TYPE_SPO2]
-        if not sensor_data.channelSamples:
-            sensor_data.channelSamples = [[], []]
-
-
-        self._append_spo2_sample(sensor_data, SPO2_CHANNEL_INDEX, spo2_value)
-        self._append_spo2_sample(sensor_data, HEART_RATE_CHANNEL_INDEX, heart_rate)
-
-
-        sensor_data.lastPackageCounter += 1
-        self.sendSensorData(sensor_data, buf)
-
-    def _append_spo2_sample(self, sensor_data: SensorData, channel_index: int, raw_value: int):
-
-        sample = Sample()
-        sample.channelIndex = channel_index
-        sample.sampleIndex = sensor_data.lastPackageCounter
-        sample.timeStampInMs = self._calculate_timestamp(sensor_data)
-        sample.rawData = raw_value
-        sample.data = float(raw_value)
-        sample.impedance = 0.0
-        sample.saturation = 0.0
-        sample.isLost = False
-
-        sensor_data.channelSamples[channel_index].append(sample)
-
-    def _calculate_timestamp(self, sensor_data: SensorData) -> int:
-
-        if sensor_data.sampleRate > 0:
-            return sensor_data.lastPackageCounter * (1000 // sensor_data.sampleRate)
-        return 0
-
-    def _process_imu_samples(self, data: bytes, buf: Queue[SensorData], on_error_callback=None):
-        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, sensor_data_euler, on_error_callback):
-            for sensor_data in sensor_datas:
+            sensor_data = self.sensorDatas[SensorDataType.DATA_TYPE_GYRO]
+            if self.checkReadSamples(data, sensor_data, sensor_data.packageIndexLength + 1, 0, on_error_callback):
                 self.sendSensorData(sensor_data, buf)
-
-    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
-
-        try:
-            sensor_data_ref = sensor_datas[0]
-            offset = 1
-            packageIndex = 0
-            maxPackageIndex = 0
-
-            if len(data) < offset + sensor_data_ref.packageIndexLength:
-                return False
-
-            if sensor_data_ref.packageIndexLength == 2:
-                packageIndex = ((data[offset + 1] & 0xFF) << 8) | (data[offset] & 0xFF)
-                maxPackageIndex = 65535
-            elif sensor_data_ref.packageIndexLength == 1:
-                packageIndex = data[offset] & 0xFF
-                maxPackageIndex = 255
-
-            offset += sensor_data_ref.packageIndexLength
-            if not self._has_complete_imu_packet(data, sensor_data_ref, sensor_data_quat, offset):
-                return False
-
-            if sensor_data_ref.packageIndexLength <= 0:
-                for sensor_data in sensor_datas:
-                    if sensor_data.lastPackageCounter < 0:
-                        sensor_data.lastPackageIndex = 0
-                        sensor_data.lastPackageCounter = 0
-            else:
-                newPackageIndex = packageIndex
-                lastPackageIndex = sensor_data_ref.lastPackageIndex
-
-                if sensor_data_ref.lastPackageCounter < 0:
-                    if newPackageIndex == 0:
-                        lastPackageIndex = maxPackageIndex
-                    else:
-                        lastPackageIndex = newPackageIndex - 1
-
-                    for sensor_data in sensor_datas:
-                        sensor_data.lastPackageIndex = lastPackageIndex
-                        sensor_data.lastPackageCounter = 0
-
-                if packageIndex < lastPackageIndex:
-                    packageIndex += maxPackageIndex + 1
-                elif packageIndex == lastPackageIndex:
-                    return False
-
-                deltaPackageIndex = packageIndex - lastPackageIndex
-                if deltaPackageIndex > 1:
-                    lostSampleCount = sensor_data_ref.packageSampleCount * (deltaPackageIndex - 1)
-
-                    if lostSampleCount < 100:
-                        self._read_imu_frame_samples(data, sensor_datas, sensor_data_quat, 0, lostSampleCount)
-
-                    if newPackageIndex == 0:
-                        lastPackageIndex = maxPackageIndex
-                    else:
-                        lastPackageIndex = newPackageIndex - 1
-
-                    for sensor_data in sensor_datas:
-                        sensor_data.lastPackageIndex = lastPackageIndex
-                        sensor_data.lastPackageCounter += deltaPackageIndex - 1
-
-                    lostLog = (
-                            "MSG|LOST SAMPLE|MAC|"
-                            + str(sensor_data_ref.deviceMac)
-                            + "|TYPE|"
-                            + str(DataType.NTF_IMU)
-                            + "|COUNT|"
-                            + str(lostSampleCount)
-                    )
-                    if on_error_callback is not None:
-                        try:
-                            asyncio.get_event_loop().run_in_executor(None, on_error_callback, lostLog)
-                        except Exception as e:
-                            import traceback
-                            traceback.print_exc()
-
-                for sensor_data in sensor_datas:
-                    sensor_data.lastPackageIndex = newPackageIndex
-
-            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:
-                sensor_data.lastPackageCounter += 1
-
-            return True
-        except Exception as e:
-            return False
-
+    
     def _has_complete_imu_packet(
             self,
             data: bytes,
@@ -1131,158 +878,93 @@ class SensorProfileDataCtx:
             frameSize += 12
 
         return len(data) >= dataOffset + sensor_data_ref.packageSampleCount * frameSize
+    
+    def _process_imu_samples(self, data: bytes, buf: Queue[SensorData], on_error_callback=None):
+        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
 
-    def _read_imu_frame_samples(
-            self,
-            data: bytes,
-            sensor_datas: List[SensorData],
-            sensor_data_quat: SensorData,
-            sensor_data_euler: SensorData,
-            dataOffset: int,
-            lostSampleCount: int,
-    ) -> bool:
-        sensor_data_acc = sensor_datas[0]
-        sensor_data_gyro = sensor_datas[1]
-        sampleCount = sensor_data_acc.packageSampleCount
-        if lostSampleCount > 0:
-            sampleCount = lostSampleCount
-
-        frameSize = 12
-        if sensor_data_quat is not None:
-            frameSize += 12
-
-        if lostSampleCount <= 0 and len(data) < dataOffset + sampleCount * frameSize:
-            return False
-
-        for sensor_data in sensor_datas:
-            if not sensor_data.channelSamples:
-                sensor_data.channelSamples = [[] for _ in range(sensor_data.channelCount)]
-
-        lastSampleIndex = sensor_data_acc.lastPackageCounter * sensor_data_acc.packageSampleCount
-        offset = dataOffset
-
-        for sampleOffset in range(sampleCount):
-            sampleIndex = lastSampleIndex + sampleOffset
-
-            if lostSampleCount > 0:
-                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, sensor_data_euler, sampleIndex, self.lastQuatData, True)
-
-                continue
-
-            accRaw = [
-                struct.unpack_from("<h", data, offset)[0],
-                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, 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
-                quatRaw = [
-                    struct.unpack_from("<i", data, quatOffset)[0],
-                    struct.unpack_from("<i", data, quatOffset + 4)[0],
-                    struct.unpack_from("<i", data, quatOffset + 8)[0],
-                ]
-                self._append_imu_quaternion_sample(sensor_data_quat, sensor_data_euler, sampleIndex, quatRaw, False)
-
-            offset += frameSize
-
-        return True
-
-    def _append_imu_vector_sample(self, sensor_data: SensorData, sampleIndex: int, rawValues: List[int], isLost: bool):
-        sampleInterval = 1000 // sensor_data.sampleRate if sensor_data.sampleRate > 0 else 0
+        if self.isContainQAT6:
+            sensor_data_quat = self.sensorDatas[SensorDataType.DATA_TYPE_QUATERNION]
+            sensor_data_euler = self.sensorDatas[SensorDataType.DATA_TYPE_EULER]
 
-        for channelIndex in range(sensor_data.channelCount):
-            if (sensor_data.channelMask & (1 << channelIndex)) == 0:
-                continue
+        if not self._has_complete_imu_packet(data, sensor_data_acc, sensor_data_quat, 3):
+            if on_error_callback:
+                on_error_callback("Incomplete IMU packet received")
+            return
 
-            rawData = 0 if isLost else rawValues[channelIndex]
+        if not self.isContainQAT6:
+            if self.checkReadSamples(data, sensor_data_acc, 3, 6, on_error_callback):
+                self.sendSensorData(sensor_data_acc, buf)
 
-            sample = Sample()
-            sample.channelIndex = channelIndex
-            sample.sampleIndex = sampleIndex
-            sample.timeStampInMs = sampleIndex * sampleInterval
-            sample.rawData = rawData
-            sample.data = 0.0 if isLost else rawData * sensor_data.K
-            sample.impedance = 0.0
-            sample.saturation = 0.0
-            sample.isLost = isLost
-            sensor_data.channelSamples[channelIndex].append(sample)
+            if self.checkReadSamples(data, sensor_data_gyro, 9, 6, on_error_callback):
+                self.sendSensorData(sensor_data_gyro, buf)
+        else:
+            if self.checkReadSamples(data, sensor_data_acc, 3, 18, on_error_callback):
+                self.sendSensorData(sensor_data_acc, buf)
 
-    def _append_imu_quaternion_sample(
-            self,
-            sensor_data_quat: SensorData,
-            sensor_data_euler: SensorData,
-            sampleIndex: int,
-            rawValues: List[int],
-            isLost: bool,
-    ):
-        scale = 1073741824.0  # 2^30
-        sampleInterval = 1000 // sensor_data_quat.sampleRate if sensor_data_quat.sampleRate > 0 else 0
-
-        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
+            if self.checkReadSamples(data, sensor_data_gyro, 9, 18, on_error_callback):
+                self.sendSensorData(sensor_data_gyro, buf)
 
-            sample = Sample()
-            sample.channelIndex = channelIndex
-            sample.sampleIndex = sampleIndex
-            sample.timeStampInMs = sampleIndex * sampleInterval
-            sample.rawData = raw[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
+            if sensor_data_euler.channelSamples is None or len(sensor_data_euler.channelSamples) == 0:
+                sensor_data_euler.channelSamples = [[], [], []]
+        
+            if self.checkReadSamples(data, sensor_data_quat, 15, 12, on_error_callback):
+                #add w
+                sampleCount = sensor_data_quat.packageSampleCount
+                for sampleIndex in range(sampleCount):
+                    try:
+                        x_sample = sensor_data_quat.channelSamples[1][sampleIndex]
+                        y_sample = sensor_data_quat.channelSamples[2][sampleIndex]
+                        z_sample = sensor_data_quat.channelSamples[3][sampleIndex]
+                        x = x_sample.data
+                        y = y_sample.data
+                        z = z_sample.data
+                        w = math.sqrt(max(0.0, 1.0 - x ** 2 - y ** 2 - z ** 2))
+                        dataItem = Sample()
+                        dataItem.channelIndex = 0
+                        dataItem.sampleIndex = x_sample.sampleIndex
+                        dataItem.timeStampInMs = x_sample.timeStampInMs
+                        dataItem.rawData = 0
+                        dataItem.data = w
+                        dataItem.isLost = x_sample.isLost
+                        sensor_data_quat.channelSamples[0].append(dataItem)
+
+                        #add euler
+                        R = math.atan2(2 * (w * x + y * z), 1 - 2 * (x ** 2 + y ** 2)) * 180 / math.pi
+                        R_sample = Sample()
+                        R_sample.channelIndex = 0
+                        R_sample.sampleIndex = x_sample.sampleIndex
+                        R_sample.timeStampInMs = x_sample.timeStampInMs
+                        R_sample.rawData = 0
+                        R_sample.data = R
+                        R_sample.isLost = x_sample.isLost
+                        sensor_data_euler.channelSamples[0].append(R_sample)
+                        P = math.asin(max(-1.0, min(1.0, 2 * (w * y - z * x)))) * 180 / math.pi
+                        P_sample = Sample()
+                        P_sample.channelIndex = 1
+                        P_sample.sampleIndex = x_sample.sampleIndex
+                        P_sample.timeStampInMs = x_sample.timeStampInMs
+                        P_sample.rawData = 0
+                        P_sample.data = P
+                        P_sample.isLost = x_sample.isLost
+                        sensor_data_euler.channelSamples[1].append(P_sample)
+                        Y = math.atan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2)) * 180 / math.pi
+                        Y_sample = Sample()
+                        Y_sample.channelIndex = 2
+                        Y_sample.sampleIndex = x_sample.sampleIndex
+                        Y_sample.timeStampInMs = x_sample.timeStampInMs
+                        Y_sample.rawData = 0
+                        Y_sample.data = Y
+                        Y_sample.isLost = x_sample.isLost
+                        sensor_data_euler.channelSamples[2].append(Y_sample)
+                    except Exception as e:
+                        import traceback
+                        traceback.print_exc()
 
-            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)
+            self.sendSensorData(sensor_data_quat, buf)
+            self.sendSensorData(sensor_data_euler, buf)
 
     def checkReadSamples(self, data: bytes, sensorData: SensorData, dataOffset: int, dataGap: int, on_error_callback=None):
         offset = 1
@@ -1361,7 +1043,8 @@ class SensorProfileDataCtx:
 
             sensorData.lastPackageCounter += 1
         except Exception as e:
-            # print(e)
+            import traceback
+            traceback.print_exc()
             return False
         return True
 
@@ -1379,11 +1062,33 @@ class SensorProfileDataCtx:
             dataGap: int,
             lostSampleCount: int,
     ):
-        # 基本越界保护：offset 必须在 data 范围内
-        if data is None or offset < 0 or offset > len(data):
-            return
-
         sampleCount = sensorData.packageSampleCount
+        if lostSampleCount <= 0:
+            if data is None or offset < 0 or offset > len(data):
+                raise ValueError("Invalid data or offset")
+
+            if sensorData.resolutionBits in (7, 8):
+                bytesPerChannel = 1
+            elif sensorData.resolutionBits in (12, 16, 0):
+                bytesPerChannel = 2
+            elif sensorData.resolutionBits == 24:
+                bytesPerChannel = 3
+            elif sensorData.resolutionBits in (31, 32):
+                bytesPerChannel = 4
+            else:
+                bytesPerChannel = 2
+
+            dataLength = len(data)
+
+            realChannelCount = 0
+            for channelIndex, impedanceChannelIndex in enumerate(range(sensorData.channelCount)):
+                if (sensorData.channelMask & (1 << channelIndex)) != 0:
+                    ++realChannelCount
+
+            if offset + ((bytesPerChannel + dataGap) * realChannelCount * sampleCount) > dataLength:
+                raise ValueError(f"Invalid dataLength:{dataLength}")
+        
+
         sampleInterval = (
             1000 // sensorData.sampleRate if sensorData.sampleRate > 0 else 0
         )
@@ -1401,18 +1106,7 @@ class SensorProfileDataCtx:
             for channelIndex in range(sensorData.channelCount):
                 channelSamples.append([])
 
-        # 根据分辨率计算每个通道占用的字节数，用于后续越界检查
-        if sensorData.resolutionBits in (7, 8):
-            bytesPerChannel = 1
-        elif sensorData.resolutionBits in (12, 16, 0):
-            bytesPerChannel = 2
-        elif sensorData.resolutionBits == 24:
-            bytesPerChannel = 3
-        else:
-            bytesPerChannel = 2
-
-        dataLength = len(data)
-
+        
         for sampleIndex in range(sampleCount):
             for channelIndex, impedanceChannelIndex in enumerate(range(sensorData.channelCount)):
                 if (sensorData.channelMask & (1 << channelIndex)) != 0:
@@ -1440,10 +1134,6 @@ class SensorProfileDataCtx:
                         dataItem.saturation = saturation
                         dataItem.isLost = True
                     else:
-                        # 在读取任何数据前先检查剩余字节是否足够
-                        if offset + bytesPerChannel > dataLength:
-                            return
-
                         rawData = 0
                         if sensorData.resolutionBits == 7:
                             rawData = data[offset]
@@ -1453,24 +1143,29 @@ class SensorProfileDataCtx:
                             rawData = data[offset] & 0xFF
                             offset += 1
                         elif sensorData.resolutionBits == 12:
-                            rawData = int.from_bytes(
-                                data[offset: offset + 2],
-                                byteorder="little",
-                                signed=True,
-                            )
+                            rawData = struct.unpack_from("<h", data, offset)[0]
+                            rawData -= 2000
                             offset += 2
                         elif sensorData.resolutionBits == 16:
-                            rawData = int.from_bytes(
-                                data[offset: offset + 2],
-                                byteorder="little",
-                                signed=True,
-                            )
+                            if (sensorData.resolutionSigned):
+                                rawData = struct.unpack_from("<h", data, offset)[0]
+                            else:
+                                rawData = struct.unpack_from("<H", data, offset)[0]
                             offset += 2
                         elif sensorData.resolutionBits == 24:
                             rawData = (data[offset] << 16) | (data[offset + 1] << 8) | data[offset + 2]
-                            if sensorData.dataType != DataType.NTF_PPG:
+                            if (sensorData.resolutionSigned):
                                 rawData -= 8388608
                             offset += 3
+                        elif sensorData.resolutionBits == 31:
+                            if (sensorData.resolutionSigned):
+                                rawData = struct.unpack_from("<i", data, offset)[0]
+                            else:
+                                rawData = struct.unpack_from("<I", data, offset)[0]
+                            offset += 4
+                        elif sensorData.resolutionBits == 32:
+                            rawData = struct.unpack_from("f", data, offset)[0]
+                            offset += 4
                         elif sensorData.resolutionBits == 0:
                             rawData = struct.unpack_from("<h", data, offset)[0]
                             offset += 2
@@ -1513,7 +1208,10 @@ class SensorProfileDataCtx:
                 oldSamples = oldChannelSamples[channelIndex]
                 newSamples = []
                 for sampleIndex in range(sensorData.minPackageSampleCount):
-                    newSamples.append(oldSamples[startIndex + sampleIndex])
+                    try:
+                        newSamples.append(oldSamples[startIndex + sampleIndex])
+                    except IndexError:
+                        pass
                 resultChannelSamples.append(newSamples)
 
             sensorDataResult = SensorData()

{sensor_sdk-0.0.65 → sensor_sdk-0.0.67/sensor_sdk.egg-info}/PKG-INFO

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

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