sensor-sdk 0.1.5__tar.gz → 0.2.0__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {sensor_sdk-0.1.5/sensor_sdk.egg-info → sensor_sdk-0.2.0}/PKG-INFO +1 -1
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/__init__.py +6 -0
- sensor_sdk-0.2.0/sensor/bleak_no_ack_patch.py +75 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/bleak_process.py +65 -18
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/sensor_data_context.py +184 -123
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0/sensor_sdk.egg-info}/PKG-INFO +1 -1
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor_sdk.egg-info/SOURCES.txt +1 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/setup.py +1 -1
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/LICENSE.txt +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/README.md +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/bleak_host.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/fb/DataType.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/fb/Sample.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/fb/SensorData.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/fb/__init__.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/gforce.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/sdk_log.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/sensor_controller.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/sensor_data.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/sensor_data_pool.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/sensor_device.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/sensor_profile.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/sensor_utils.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor/winrt_high_throughput.py +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor_sdk.egg-info/dependency_links.txt +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor_sdk.egg-info/requires.txt +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor_sdk.egg-info/top_level.txt +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/sensor_sdk.egg-info/zip-safe +0 -0
- {sensor_sdk-0.1.5 → sensor_sdk-0.2.0}/setup.cfg +0 -0
|
@@ -3,10 +3,16 @@ from sensor.sensor_profile import SensorProfile
|
|
|
3
3
|
from sensor.sensor_device import BLEDevice, DeviceInfo, DeviceStateEx
|
|
4
4
|
from sensor.sensor_data import DataType, Sample, SensorData
|
|
5
5
|
from sensor.winrt_high_throughput import apply as _apply_winrt_high_throughput_patch
|
|
6
|
+
from sensor.bleak_no_ack_patch import apply as _apply_bleak_no_ack_patch
|
|
7
|
+
import os
|
|
6
8
|
|
|
7
9
|
# Windows 下尝试给 bleak WinRT backend 打高吞吐率连接参数补丁
|
|
8
10
|
_apply_winrt_high_throughput_patch()
|
|
9
11
|
|
|
12
|
+
# 如果设置了环境变量 SENSOR_SDK_FORCE_NO_ACK=1,则对命令特征强制 write-without-response
|
|
13
|
+
if os.environ.get("SENSOR_SDK_FORCE_NO_ACK", "0") == "1":
|
|
14
|
+
_apply_bleak_no_ack_patch()
|
|
15
|
+
|
|
10
16
|
__all__ = [
|
|
11
17
|
"SensorController",
|
|
12
18
|
"SensorControllerInstance",
|
|
@@ -0,0 +1,75 @@
|
|
|
1
|
+
"""强制 bleak 对指定特征使用 write-without-response(no ACK)的补丁。
|
|
2
|
+
|
|
3
|
+
某些 Windows 主机上 write-with-response 会因为系统 ACK 等待导致命令超时,
|
|
4
|
+
把命令特征强制改成无响应写入可以显著提升命令通道的实时性。
|
|
5
|
+
|
|
6
|
+
默认只作用于 SDK 已知的两个 CMD 特征 UUID;传入空列表或 `None` 则全局生效。
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
import logging
|
|
10
|
+
from functools import wraps
|
|
11
|
+
|
|
12
|
+
logger = logging.getLogger(__name__)
|
|
13
|
+
|
|
14
|
+
# SDK 默认使用的两条命令特征
|
|
15
|
+
DEFAULT_CMD_CHAR_UUIDS = {
|
|
16
|
+
"f000ffe1-0451-4000-b000-000000000000", # OYM CMD
|
|
17
|
+
"00000002-0000-1000-8000-00805f9b34fb", # RFSTAR CMD
|
|
18
|
+
}
|
|
19
|
+
|
|
20
|
+
|
|
21
|
+
def _char_uuid(char_specifier) -> str:
|
|
22
|
+
"""尽量把 char_specifier 转成小写 UUID 字符串。"""
|
|
23
|
+
if isinstance(char_specifier, str):
|
|
24
|
+
return char_specifier.lower()
|
|
25
|
+
try:
|
|
26
|
+
# bleak 的 BleakGATTCharacteristic 对象有 uuid 属性
|
|
27
|
+
return str(char_specifier.uuid).lower()
|
|
28
|
+
except Exception:
|
|
29
|
+
return ""
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
def apply(cmd_char_uuids=DEFAULT_CMD_CHAR_UUIDS):
|
|
33
|
+
"""给 BleakClient.write_gatt_char 打补丁,强制指定特征使用 response=False。
|
|
34
|
+
|
|
35
|
+
Args:
|
|
36
|
+
cmd_char_uuids: 需要强制 no-ack 的特征 UUID 集合。传 None 表示所有写入都强制 no-ack。
|
|
37
|
+
"""
|
|
38
|
+
try:
|
|
39
|
+
from bleak import BleakClient
|
|
40
|
+
except Exception as e:
|
|
41
|
+
logger.debug("bleak not available, skip no-ack patch: %s", e)
|
|
42
|
+
return
|
|
43
|
+
|
|
44
|
+
if getattr(BleakClient, "_no_ack_patched", False):
|
|
45
|
+
return
|
|
46
|
+
|
|
47
|
+
orig_write = BleakClient.write_gatt_char
|
|
48
|
+
|
|
49
|
+
@wraps(orig_write)
|
|
50
|
+
async def patched_write(self, char_specifier, data, response=None):
|
|
51
|
+
if cmd_char_uuids is None or _char_uuid(char_specifier) in cmd_char_uuids:
|
|
52
|
+
response = False
|
|
53
|
+
return await orig_write(self, char_specifier, data, response=response)
|
|
54
|
+
|
|
55
|
+
BleakClient.write_gatt_char = patched_write
|
|
56
|
+
BleakClient._no_ack_patched = True # type: ignore[attr-defined]
|
|
57
|
+
logger.debug(
|
|
58
|
+
"Applied bleak write-without-response patch (targets: %s)",
|
|
59
|
+
"all chars" if cmd_char_uuids is None else cmd_char_uuids,
|
|
60
|
+
)
|
|
61
|
+
|
|
62
|
+
|
|
63
|
+
def reset():
|
|
64
|
+
"""移除补丁(主要用于测试)。"""
|
|
65
|
+
try:
|
|
66
|
+
from bleak import BleakClient
|
|
67
|
+
except Exception:
|
|
68
|
+
return
|
|
69
|
+
|
|
70
|
+
if not getattr(BleakClient, "_no_ack_patched", False):
|
|
71
|
+
return
|
|
72
|
+
|
|
73
|
+
# 找到原始的未绑定方法比较麻烦,这里简单把标记清掉,
|
|
74
|
+
# 实际撤销需要保存原始方法;apply 没有保存,故 reset 仅作占位。
|
|
75
|
+
BleakClient._no_ack_patched = False # type: ignore[attr-defined]
|
|
@@ -839,14 +839,17 @@ class BleakProcess(multiprocessing.Process):
|
|
|
839
839
|
|
|
840
840
|
async def _battery_loop(self, device_mac: str):
|
|
841
841
|
|
|
842
|
-
|
|
843
|
-
|
|
842
|
+
while True:
|
|
843
|
+
interval = self._power_intervals.get(device_mac, 0)
|
|
844
|
+
if interval <= 0 or device_mac not in self._gforces:
|
|
845
|
+
break
|
|
844
846
|
await asyncio.sleep(interval / 1000)
|
|
845
847
|
try:
|
|
846
848
|
power = await self._gforces[device_mac].get_battery_level()
|
|
847
849
|
self._publish("power_changed", device_mac=device_mac, power=power)
|
|
848
850
|
except Exception:
|
|
849
|
-
|
|
851
|
+
# 单次刷新失败不中断循环,否则一次超时/丢包后就再也无法刷新电量
|
|
852
|
+
SdkLog.exception(_TAG, f"Battery refresh failed: {device_mac}")
|
|
850
853
|
|
|
851
854
|
async def _device_data_loop(self, device_mac: str):
|
|
852
855
|
|
|
@@ -864,15 +867,24 @@ class BleakProcess(multiprocessing.Process):
|
|
|
864
867
|
def on_error(message: str):
|
|
865
868
|
self._publish("error", device_mac=device_mac, message=message)
|
|
866
869
|
|
|
867
|
-
|
|
868
|
-
|
|
869
|
-
|
|
870
|
-
|
|
871
|
-
|
|
872
|
-
|
|
873
|
-
|
|
874
|
-
|
|
875
|
-
|
|
870
|
+
# 持续运行,异常后自动重启,避免单包错误导致整个解析线程退出
|
|
871
|
+
while not self._should_exit and device_mac in self._data_ctxs:
|
|
872
|
+
try:
|
|
873
|
+
if ctx.isUniversalStream:
|
|
874
|
+
await ctx._processUniversalData(local_buf, on_data, on_error)
|
|
875
|
+
else:
|
|
876
|
+
await ctx._process_data(local_buf, on_data, on_error)
|
|
877
|
+
# 正常返回说明 ctx 已关闭或停止
|
|
878
|
+
break
|
|
879
|
+
except asyncio.CancelledError:
|
|
880
|
+
SdkLog.i(_TAG, f"Data loop cancelled: {device_mac}")
|
|
881
|
+
break
|
|
882
|
+
except Exception as e:
|
|
883
|
+
SdkLog.exception(_TAG, f"Error in data loop, restarting: {device_mac}")
|
|
884
|
+
try:
|
|
885
|
+
await asyncio.sleep(0.5)
|
|
886
|
+
except asyncio.CancelledError:
|
|
887
|
+
break
|
|
876
888
|
|
|
877
889
|
async def _do_start_notification(self, cmd: dict):
|
|
878
890
|
|
|
@@ -1099,6 +1111,12 @@ class BleakProcess(multiprocessing.Process):
|
|
|
1099
1111
|
sorted_keys = sorted(ctx.notify_map.keys())
|
|
1100
1112
|
result = "|".join(f"{k}|{ctx.notify_map[k]}" for k in sorted_keys)
|
|
1101
1113
|
|
|
1114
|
+
if key == "NTF_IMU":
|
|
1115
|
+
imu_sub_keys = ["NTF_GFORCE_ACC", "NTF_GFORCE_GYRO", "NTF_GFORCE_QUAT", "NTF_GFORCE_EULER"]
|
|
1116
|
+
result = "ON" if all(ctx.notify_map.get(k) == "ON" for k in imu_sub_keys) else "OFF"
|
|
1117
|
+
elif key in ctx.notify_map:
|
|
1118
|
+
result = ctx.notify_map[key]
|
|
1119
|
+
|
|
1102
1120
|
if key == "DEBUG_LOG_PATH":
|
|
1103
1121
|
result = SdkLog.get_log_path() or ""
|
|
1104
1122
|
|
|
@@ -1156,6 +1174,12 @@ class BleakProcess(multiprocessing.Process):
|
|
|
1156
1174
|
if key == "NTF_PPG_RAW":
|
|
1157
1175
|
map_key = "NTF_PPG"
|
|
1158
1176
|
|
|
1177
|
+
# IMU 总开关同时控制 ACC/GYRO/QUAT/EULER
|
|
1178
|
+
imu_sub_keys = ["NTF_GFORCE_ACC", "NTF_GFORCE_GYRO", "NTF_GFORCE_QUAT", "NTF_GFORCE_EULER"]
|
|
1179
|
+
if map_key == "NTF_IMU":
|
|
1180
|
+
for sub in imu_sub_keys:
|
|
1181
|
+
ctx.notify_map[sub] = value
|
|
1182
|
+
|
|
1159
1183
|
# 老版本 EMG 设备上 Gesture 与 EMG 互斥,自动切换
|
|
1160
1184
|
if not ctx.isNewEMG:
|
|
1161
1185
|
if map_key == "NTF_GEST" and value == "ON" and ctx.notify_map.get("NTF_EMG") == "ON":
|
|
@@ -1170,17 +1194,40 @@ class BleakProcess(multiprocessing.Process):
|
|
|
1170
1194
|
ctx.notify_map[map_key] = value
|
|
1171
1195
|
result = "OK"
|
|
1172
1196
|
else:
|
|
1197
|
+
# 新 EMG:Gesture 依赖 EMG 数据,关闭 EMG 时同步关闭 Gesture;打开 Gesture 时自动打开 EMG
|
|
1198
|
+
if map_key == "NTF_EMG" and value == "OFF":
|
|
1199
|
+
ctx.notify_map["NTF_GEST"] = "OFF"
|
|
1200
|
+
elif map_key == "NTF_GEST" and value == "ON" and ctx.notify_map.get("NTF_EMG") != "ON":
|
|
1201
|
+
ctx.notify_map["NTF_EMG"] = "ON"
|
|
1173
1202
|
ctx.notify_map[map_key] = value
|
|
1174
1203
|
result = "OK"
|
|
1175
|
-
|
|
1204
|
+
|
|
1205
|
+
# 单个 IMU 子开关变化时,同步更新 NTF_IMU 总开关的聚合状态
|
|
1206
|
+
if map_key in imu_sub_keys:
|
|
1207
|
+
ctx.notify_map["NTF_IMU"] = "ON" if all(ctx.notify_map.get(k) == "ON" for k in imu_sub_keys) else "OFF"
|
|
1208
|
+
|
|
1209
|
+
if result == "OK" and ctx.hasInit():
|
|
1176
1210
|
ctx._buildNotifyDataFlag()
|
|
1177
|
-
|
|
1178
|
-
|
|
1211
|
+
|
|
1212
|
+
# 新 EMG 设备通过 function switch 控制 EMG/Gesture 输出,bit0=gesture, bit1=emg
|
|
1213
|
+
if ctx.isNewEMG and map_key in ("NTF_EMG", "NTF_GEST"):
|
|
1214
|
+
emg_bit = 1 if ctx.notify_map.get("NTF_EMG") == "ON" else 0
|
|
1215
|
+
gest_bit = 1 if (emg_bit and ctx.notify_map.get("NTF_GEST") == "ON") else 0
|
|
1216
|
+
func_switch = (emg_bit << 1) | gest_bit
|
|
1179
1217
|
try:
|
|
1180
|
-
await ctx.gForce.
|
|
1218
|
+
await ctx.gForce.set_function_switch(func_switch)
|
|
1219
|
+
await asyncio.sleep(0.5)
|
|
1181
1220
|
except Exception as e:
|
|
1182
|
-
SdkLog.exception(_TAG, f"_do_set_param
|
|
1183
|
-
result = "ERROR:
|
|
1221
|
+
SdkLog.exception(_TAG, f"_do_set_param set_function_switch failed: {device_mac}")
|
|
1222
|
+
result = "ERROR: set_function_switch fail: " + str(e)
|
|
1223
|
+
elif ctx.isDataTransfering:
|
|
1224
|
+
needs_restart = True
|
|
1225
|
+
if ctx.getChipType() == BLEChipType.OYM:
|
|
1226
|
+
try:
|
|
1227
|
+
await ctx.gForce.set_subscription(ctx.notifyDataFlag)
|
|
1228
|
+
except Exception as e:
|
|
1229
|
+
SdkLog.exception(_TAG, f"_do_set_param set_subscription failed: {device_mac}")
|
|
1230
|
+
result = "ERROR: set_subscription fail: " + str(e)
|
|
1184
1231
|
|
|
1185
1232
|
if key in ["FILTER_50HZ", "FILTER_60HZ", "FILTER_HPF", "FILTER_LPF"]:
|
|
1186
1233
|
if value in ["ON", "OFF"]:
|
|
@@ -390,8 +390,12 @@ class SensorProfileDataCtx:
|
|
|
390
390
|
config.batch_len = 128
|
|
391
391
|
|
|
392
392
|
if isNewEMG:
|
|
393
|
-
|
|
394
|
-
|
|
393
|
+
# 新版 EMG:bit0=gesture,bit1=emg;Gesture 依赖 EMG,EMG 关闭时 Gesture 同步关闭
|
|
394
|
+
emg_bit = 1 if self.notify_map.get("NTF_EMG") == "ON" else 0
|
|
395
|
+
gest_bit = 1 if (emg_bit and self.notify_map.get("NTF_GEST") == "ON") else 0
|
|
396
|
+
await self.gForce.set_function_switch((emg_bit << 1) | gest_bit)
|
|
397
|
+
await asyncio.sleep(0.5)
|
|
398
|
+
|
|
395
399
|
await self.gForce.set_emg_raw_data_config(config)
|
|
396
400
|
await self.gForce.set_package_id(True)
|
|
397
401
|
|
|
@@ -544,7 +548,7 @@ class SensorProfileDataCtx:
|
|
|
544
548
|
async def initIMU(self, packageCount: int) -> int:
|
|
545
549
|
SdkLog.d(_TAG, "initIMU(...)")
|
|
546
550
|
IMU_TYPE_QAT6 = 0x0004
|
|
547
|
-
min_package_sample_count = 2
|
|
551
|
+
min_package_sample_count = 2 if self._chip_type == BLEChipType.OYM else 1
|
|
548
552
|
self.isContainQAT6 = False
|
|
549
553
|
|
|
550
554
|
if not self.hasIMU():
|
|
@@ -1099,39 +1103,44 @@ class SensorProfileDataCtx:
|
|
|
1099
1103
|
SdkLog.exception(_TAG, "Unexpected error")
|
|
1100
1104
|
|
|
1101
1105
|
if self.notifyDataFlag & DataSubscription.DNF_CONCAT_BLE != 0:
|
|
1102
|
-
|
|
1103
|
-
|
|
1104
|
-
|
|
1106
|
+
try:
|
|
1107
|
+
index = 0
|
|
1108
|
+
last_cut = -1
|
|
1109
|
+
data_size = len(self._concatDataBuffer)
|
|
1105
1110
|
|
|
1106
|
-
|
|
1107
|
-
|
|
1108
|
-
|
|
1111
|
+
while self._is_running:
|
|
1112
|
+
if index >= data_size:
|
|
1113
|
+
break
|
|
1109
1114
|
|
|
1110
|
-
|
|
1111
|
-
|
|
1112
|
-
|
|
1113
|
-
|
|
1114
|
-
|
|
1115
|
-
|
|
1115
|
+
if self._concatDataBuffer[index] == 0x55:
|
|
1116
|
+
if (index + 1) >= data_size:
|
|
1117
|
+
index = data_size
|
|
1118
|
+
continue
|
|
1119
|
+
n = self._concatDataBuffer[index + 1]
|
|
1120
|
+
if n < 2 or (index + 1 + n + 1) >= data_size:
|
|
1121
|
+
index += 1
|
|
1122
|
+
continue
|
|
1123
|
+
crc8 = (self._concatDataBuffer[index + 1 + n + 1])
|
|
1124
|
+
calc_crc = sensor_utils.calc_crc8(self._concatDataBuffer[index + 2: index + 2 + n])
|
|
1125
|
+
if crc8 != calc_crc:
|
|
1126
|
+
index += 1
|
|
1127
|
+
continue
|
|
1128
|
+
if self._is_data_transfering:
|
|
1129
|
+
data_package = bytes(self._concatDataBuffer[index + 2: index + 2 + n])
|
|
1130
|
+
if self._processDataPackage(data_package, buf, on_error_callback):
|
|
1131
|
+
last_cut = index = index + 2 + n
|
|
1116
1132
|
index += 1
|
|
1117
|
-
|
|
1118
|
-
crc8 = (self._concatDataBuffer[index + 1 + n + 1])
|
|
1119
|
-
calc_crc = sensor_utils.calc_crc8(self._concatDataBuffer[index + 2: index + 2 + n])
|
|
1120
|
-
if crc8 != calc_crc:
|
|
1133
|
+
else:
|
|
1121
1134
|
index += 1
|
|
1122
|
-
continue
|
|
1123
|
-
if self._is_data_transfering:
|
|
1124
|
-
data_package = bytes(self._concatDataBuffer[index + 2: index + 2 + n])
|
|
1125
|
-
if self._processDataPackage(data_package, buf, on_error_callback):
|
|
1126
|
-
last_cut = index = index + 2 + n
|
|
1127
|
-
index += 1
|
|
1128
|
-
else:
|
|
1129
|
-
index += 1
|
|
1130
1135
|
|
|
1131
|
-
|
|
1132
|
-
|
|
1133
|
-
|
|
1134
|
-
|
|
1136
|
+
if last_cut > 0:
|
|
1137
|
+
self._concatDataBuffer = self._concatDataBuffer[last_cut + 1:]
|
|
1138
|
+
last_cut = -1
|
|
1139
|
+
index = 0
|
|
1140
|
+
|
|
1141
|
+
self._last_progress_time = time.time()
|
|
1142
|
+
except Exception as e:
|
|
1143
|
+
SdkLog.exception(_TAG, "Unexpected error in concat data processing")
|
|
1135
1144
|
|
|
1136
1145
|
def _processDataPackage(self, data: bytes, buf: Queue[bytes], on_error_callback=None) -> bool:
|
|
1137
1146
|
if not data:
|
|
@@ -1240,7 +1249,11 @@ class SensorProfileDataCtx:
|
|
|
1240
1249
|
if on_error_callback:
|
|
1241
1250
|
on_error_callback("Incomplete Impedance packet received")
|
|
1242
1251
|
return False
|
|
1243
|
-
|
|
1252
|
+
|
|
1253
|
+
sensor_data = self.sensorDatas[SensorDataType.DATA_TYPE_IMPEDANCE]
|
|
1254
|
+
self.checkReadSamples(data, sensor_data, 0, -1, on_error_callback)
|
|
1255
|
+
sampleInterval = 1000.0 / sensor_data.sampleRate if sensor_data.sampleRate > 0 else 0
|
|
1256
|
+
|
|
1244
1257
|
for index in range(channelCount):
|
|
1245
1258
|
impedance = struct.unpack_from("<f", data, offset)[0]
|
|
1246
1259
|
offset += 4
|
|
@@ -1257,20 +1270,24 @@ class SensorProfileDataCtx:
|
|
|
1257
1270
|
|
|
1258
1271
|
self.impedanceData = impedanceData
|
|
1259
1272
|
self.saturationData = saturationData
|
|
1260
|
-
|
|
1261
|
-
sensor_data = self.sensorDatas[SensorDataType.DATA_TYPE_IMPEDANCE]
|
|
1262
|
-
self.checkReadSamples(data, sensor_data, 0, -1)
|
|
1263
|
-
sampleInterval = 1000.0 / sensor_data.sampleRate if sensor_data.sampleRate > 0 else 0
|
|
1264
1273
|
lastSampleIndex = sensor_data.lastPackageCounter * sensor_data.packageSampleCount
|
|
1265
1274
|
|
|
1266
1275
|
sensor_data.channelSamples = []
|
|
1267
1276
|
for index in range(channelCount):
|
|
1268
1277
|
samples = []
|
|
1269
1278
|
sample = self._object_pool.acquire_sample()
|
|
1270
|
-
|
|
1271
|
-
|
|
1272
|
-
|
|
1273
|
-
|
|
1279
|
+
|
|
1280
|
+
impedanceValue = impedanceData[index]
|
|
1281
|
+
saturationValue = saturationData[index]
|
|
1282
|
+
if not math.isfinite(impedanceValue):
|
|
1283
|
+
impedanceValue = 0.0
|
|
1284
|
+
if not math.isfinite(saturationValue):
|
|
1285
|
+
saturationValue = 0.0
|
|
1286
|
+
|
|
1287
|
+
sample.rawData = int(saturationValue)
|
|
1288
|
+
sample.data = impedanceValue
|
|
1289
|
+
sample.impedance = impedanceValue
|
|
1290
|
+
sample.saturation = saturationValue
|
|
1274
1291
|
sample.sampleIndex = lastSampleIndex
|
|
1275
1292
|
sample.timeStampInMs = int(lastSampleIndex * sampleInterval)
|
|
1276
1293
|
sample.channelIndex = index
|
|
@@ -1291,7 +1308,8 @@ class SensorProfileDataCtx:
|
|
|
1291
1308
|
if sensor_data_quat is not None:
|
|
1292
1309
|
frameSize += 12
|
|
1293
1310
|
|
|
1294
|
-
|
|
1311
|
+
expected = dataOffset + sensor_data_ref.packageSampleCount * frameSize
|
|
1312
|
+
return len(data) == expected
|
|
1295
1313
|
|
|
1296
1314
|
def _process_imu_samples(self, data: bytes, buf: Queue[bytes], on_error_callback=None) -> bool:
|
|
1297
1315
|
sensor_data_acc = self.sensorDatas[SensorDataType.DATA_TYPE_ACC]
|
|
@@ -1398,6 +1416,47 @@ class SensorProfileDataCtx:
|
|
|
1398
1416
|
return ReadSamplesResult.Error
|
|
1399
1417
|
if sensorData is None or sensorData.packageSampleCount <= 0 or sensorData.channelCount <= 0 or sensorData.minPackageSampleCount <= 0 or sensorData.K <= 0:
|
|
1400
1418
|
return ReadSamplesResult.Error
|
|
1419
|
+
|
|
1420
|
+
def _type_name():
|
|
1421
|
+
try:
|
|
1422
|
+
return DataType(sensorData.dataType).name
|
|
1423
|
+
except Exception:
|
|
1424
|
+
return str(sensorData.dataType)
|
|
1425
|
+
|
|
1426
|
+
# 长度预检:在解析包序号/样本前就发现数据长度不足,并报告数据类型
|
|
1427
|
+
if dataGap >= 0 and sensorData.packageSampleCount > 0:
|
|
1428
|
+
if sensorData.resolutionBits in (7, 8):
|
|
1429
|
+
bytesPerChannel = 1
|
|
1430
|
+
elif sensorData.resolutionBits in (12, 16, 17, 0):
|
|
1431
|
+
bytesPerChannel = 2
|
|
1432
|
+
elif sensorData.resolutionBits == 24:
|
|
1433
|
+
bytesPerChannel = 3
|
|
1434
|
+
elif sensorData.resolutionBits in (31, 32, 33):
|
|
1435
|
+
bytesPerChannel = 4
|
|
1436
|
+
else:
|
|
1437
|
+
bytesPerChannel = 2
|
|
1438
|
+
|
|
1439
|
+
realChannelCount = 0
|
|
1440
|
+
for i in range(sensorData.channelCount):
|
|
1441
|
+
if (sensorData.channelMask & (1 << i)) != 0:
|
|
1442
|
+
realChannelCount += 1
|
|
1443
|
+
|
|
1444
|
+
expected = (
|
|
1445
|
+
dataOffset
|
|
1446
|
+
+ bytesPerChannel * realChannelCount * sensorData.packageSampleCount
|
|
1447
|
+
+ dataGap * (sensorData.packageSampleCount - 1)
|
|
1448
|
+
)
|
|
1449
|
+
if dataGap == 0:
|
|
1450
|
+
# 单一流数据包:要求长度完全一致
|
|
1451
|
+
if expected != len(data):
|
|
1452
|
+
SdkLog.i(_TAG, f"Invalid dataLength:{len(data)} (expected {expected}) for data type {_type_name()}")
|
|
1453
|
+
return ReadSamplesResult.Error
|
|
1454
|
+
else:
|
|
1455
|
+
# IMU 复合包:允许包含多个子流,只检查长度不足
|
|
1456
|
+
if expected > len(data):
|
|
1457
|
+
SdkLog.i(_TAG, f"Invalid dataLength:{len(data)} (expected at least {expected}) for data type {_type_name()}")
|
|
1458
|
+
return ReadSamplesResult.Error
|
|
1459
|
+
|
|
1401
1460
|
try:
|
|
1402
1461
|
packageIndex = 0
|
|
1403
1462
|
maxPackageIndex = 0
|
|
@@ -1416,39 +1475,58 @@ class SensorProfileDataCtx:
|
|
|
1416
1475
|
offset += sensorData.packageIndexLength
|
|
1417
1476
|
newPackageIndex = packageIndex
|
|
1418
1477
|
lastPackageIndex = sensorData.lastPackageIndex
|
|
1419
|
-
|
|
1420
|
-
|
|
1478
|
+
|
|
1479
|
+
# 首包:把上一包序号设为合法的前一个值,便于后续统一判断
|
|
1480
|
+
if sensorData.lastPackageCounter < 0:
|
|
1421
1481
|
sensorData.lastPackageCounter = 0
|
|
1482
|
+
if newPackageIndex > 0:
|
|
1483
|
+
lastPackageIndex = newPackageIndex - 1
|
|
1484
|
+
else:
|
|
1485
|
+
lastPackageIndex = maxPackageIndex
|
|
1486
|
+
sensorData.lastPackageIndex = lastPackageIndex
|
|
1422
1487
|
|
|
1423
|
-
|
|
1424
|
-
|
|
1425
|
-
|
|
1488
|
+
# 合法翻卷区间:last 在 [max-2, max] 且 new 在 [0, 2]
|
|
1489
|
+
if newPackageIndex <= 2 and lastPackageIndex >= maxPackageIndex - 2:
|
|
1490
|
+
packageIndex = maxPackageIndex + 1 + newPackageIndex
|
|
1491
|
+
elif newPackageIndex == lastPackageIndex:
|
|
1426
1492
|
return ReadSamplesResult.Repeated
|
|
1493
|
+
elif newPackageIndex < lastPackageIndex:
|
|
1494
|
+
SdkLog.i(_TAG, (
|
|
1495
|
+
"Illegal package index backward|MAC|" + str(sensorData.deviceMac)
|
|
1496
|
+
+ "|TYPE|" + str(sensorData.dataType)
|
|
1497
|
+
+ "|LAST_IDX|" + str(lastPackageIndex)
|
|
1498
|
+
+ "|CURR_IDX|" + str(newPackageIndex)
|
|
1499
|
+
+ "|DELTA|" + str(lastPackageIndex - newPackageIndex)
|
|
1500
|
+
))
|
|
1501
|
+
return ReadSamplesResult.Error
|
|
1427
1502
|
|
|
1428
1503
|
deltaPackageIndex = packageIndex - lastPackageIndex
|
|
1504
|
+
if deltaPackageIndex > 20:
|
|
1505
|
+
SdkLog.i(_TAG, (
|
|
1506
|
+
"Illegal package index jump|MAC|" + str(sensorData.deviceMac)
|
|
1507
|
+
+ "|TYPE|" + str(sensorData.dataType)
|
|
1508
|
+
+ "|LAST_IDX|" + str(lastPackageIndex)
|
|
1509
|
+
+ "|CURR_IDX|" + str(newPackageIndex)
|
|
1510
|
+
+ "|DELTA|" + str(deltaPackageIndex)
|
|
1511
|
+
))
|
|
1512
|
+
return ReadSamplesResult.Error
|
|
1513
|
+
|
|
1429
1514
|
lostPackageCounter = deltaPackageIndex - 1
|
|
1430
|
-
if lostPackageCounter > 65534:
|
|
1431
|
-
lostPackageCounter = 1
|
|
1432
|
-
elif lostPackageCounter > 50:
|
|
1433
|
-
lostPackageCounter = 50
|
|
1434
1515
|
sensorData.lostPackageCount = sensorData.lostPackageCount + lostPackageCounter
|
|
1435
1516
|
|
|
1436
1517
|
if deltaPackageIndex > 1:
|
|
1437
|
-
lostSampleCount = sensorData.packageSampleCount *
|
|
1518
|
+
lostSampleCount = sensorData.packageSampleCount * lostPackageCounter
|
|
1438
1519
|
SdkLog.i(_TAG, (
|
|
1439
1520
|
"MSG|LOST SAMPLE|MAC|" + str(sensorData.deviceMac)
|
|
1440
1521
|
+ "|TYPE|" + str(sensorData.dataType)
|
|
1441
1522
|
+ "|COUNT|" + str(lostSampleCount)
|
|
1442
1523
|
))
|
|
1443
1524
|
|
|
1444
|
-
if
|
|
1525
|
+
if lostPackageCounter < 20:
|
|
1445
1526
|
self.readSamples(data, sensorData, 0, dataGap, lostSampleCount)
|
|
1446
1527
|
|
|
1447
|
-
|
|
1448
|
-
|
|
1449
|
-
else:
|
|
1450
|
-
sensorData.lastPackageIndex = newPackageIndex - 1
|
|
1451
|
-
sensorData.lastPackageCounter += deltaPackageIndex - 1
|
|
1528
|
+
sensorData.lastPackageIndex = newPackageIndex - 1
|
|
1529
|
+
sensorData.lastPackageCounter += lostPackageCounter
|
|
1452
1530
|
|
|
1453
1531
|
sensorData.lastPackageIndex = newPackageIndex
|
|
1454
1532
|
|
|
@@ -1486,28 +1564,6 @@ class SensorProfileDataCtx:
|
|
|
1486
1564
|
if data is None or offset < 0 or offset > len(data):
|
|
1487
1565
|
raise ValueError(f"Invalid data or offset for data type {_type_name()}")
|
|
1488
1566
|
|
|
1489
|
-
if sensorData.resolutionBits in (7, 8):
|
|
1490
|
-
bytesPerChannel = 1
|
|
1491
|
-
elif sensorData.resolutionBits in (12, 16, 17, 0):
|
|
1492
|
-
bytesPerChannel = 2
|
|
1493
|
-
elif sensorData.resolutionBits == 24:
|
|
1494
|
-
bytesPerChannel = 3
|
|
1495
|
-
elif sensorData.resolutionBits in (31, 32, 33):
|
|
1496
|
-
bytesPerChannel = 4
|
|
1497
|
-
else:
|
|
1498
|
-
bytesPerChannel = 2
|
|
1499
|
-
|
|
1500
|
-
dataLength = len(data)
|
|
1501
|
-
|
|
1502
|
-
realChannelCount = 0
|
|
1503
|
-
for channelIndex, impedanceChannelIndex in enumerate(range(sensorData.channelCount)):
|
|
1504
|
-
if (sensorData.channelMask & (1 << channelIndex)) != 0:
|
|
1505
|
-
realChannelCount += 1
|
|
1506
|
-
|
|
1507
|
-
if offset + ((bytesPerChannel * realChannelCount * sampleCount) + (dataGap * (sampleCount - 1))) > dataLength:
|
|
1508
|
-
raise ValueError(f"Invalid dataLength:{dataLength} for data type {_type_name()}")
|
|
1509
|
-
|
|
1510
|
-
|
|
1511
1567
|
sampleInterval = (
|
|
1512
1568
|
int(1000.0 / sensorData.sampleRate) if sensorData.sampleRate > 0 else 0
|
|
1513
1569
|
)
|
|
@@ -1748,55 +1804,60 @@ class SensorProfileDataCtx:
|
|
|
1748
1804
|
except Exception as e:
|
|
1749
1805
|
SdkLog.exception(_TAG, "Error reading raw data buffer")
|
|
1750
1806
|
|
|
1751
|
-
|
|
1752
|
-
|
|
1753
|
-
|
|
1807
|
+
try:
|
|
1808
|
+
index = 0
|
|
1809
|
+
last_cut = -1
|
|
1810
|
+
data_size = len(self._concatDataBuffer)
|
|
1754
1811
|
|
|
1755
|
-
|
|
1756
|
-
|
|
1757
|
-
|
|
1812
|
+
while self._is_running:
|
|
1813
|
+
if index >= data_size:
|
|
1814
|
+
break
|
|
1758
1815
|
|
|
1759
|
-
|
|
1760
|
-
|
|
1761
|
-
|
|
1762
|
-
|
|
1763
|
-
|
|
1764
|
-
|
|
1765
|
-
|
|
1766
|
-
|
|
1767
|
-
|
|
1768
|
-
|
|
1769
|
-
|
|
1816
|
+
if self._concatDataBuffer[index] == 0x55:
|
|
1817
|
+
if (index + 1) >= data_size:
|
|
1818
|
+
index = data_size
|
|
1819
|
+
continue
|
|
1820
|
+
n = self._concatDataBuffer[index + 1]
|
|
1821
|
+
if n < 2 or (index + 1 + n + 2) >= data_size:
|
|
1822
|
+
index += 1
|
|
1823
|
+
continue
|
|
1824
|
+
crc16 = (self._concatDataBuffer[index + 1 + n + 2] << 8) | self._concatDataBuffer[index + 1 + n + 1]
|
|
1825
|
+
calc_crc = sensor_utils.crc16_cal(self._concatDataBuffer[index + 2: index + 2 + n], n)
|
|
1826
|
+
if crc16 != calc_crc:
|
|
1827
|
+
index += 1
|
|
1828
|
+
continue
|
|
1829
|
+
if self._is_data_transfering:
|
|
1830
|
+
data_package = bytes(self._concatDataBuffer[index + 2: index + 2 + n])
|
|
1831
|
+
if self._processDataPackage(data_package, buf, on_error_callback):
|
|
1832
|
+
last_cut = index = index + 2 + n + 1
|
|
1770
1833
|
index += 1
|
|
1771
|
-
|
|
1772
|
-
|
|
1834
|
+
elif self._concatDataBuffer[index] == 0xAA:
|
|
1835
|
+
if (index + 1) >= data_size:
|
|
1836
|
+
index = data_size
|
|
1837
|
+
continue
|
|
1838
|
+
n = self._concatDataBuffer[index + 1]
|
|
1839
|
+
if n < 2 or (index + 1 + n + 2) >= data_size:
|
|
1840
|
+
index += 1
|
|
1841
|
+
continue
|
|
1842
|
+
crc16 = (self._concatDataBuffer[index + 1 + n + 2] << 8) | self._concatDataBuffer[index + 1 + n + 1]
|
|
1843
|
+
calc_crc = sensor_utils.crc16_cal(self._concatDataBuffer[index + 2: index + 2 + n], n)
|
|
1844
|
+
if crc16 != calc_crc:
|
|
1845
|
+
index += 1
|
|
1846
|
+
continue
|
|
1773
1847
|
data_package = bytes(self._concatDataBuffer[index + 2: index + 2 + n])
|
|
1774
|
-
|
|
1775
|
-
|
|
1776
|
-
|
|
1777
|
-
|
|
1778
|
-
if (index + 1) >= data_size:
|
|
1779
|
-
index = data_size
|
|
1780
|
-
continue
|
|
1781
|
-
n = self._concatDataBuffer[index + 1]
|
|
1782
|
-
if n < 2 or (index + 1 + n + 2) >= data_size:
|
|
1848
|
+
|
|
1849
|
+
if not sensor_utils._terminated:
|
|
1850
|
+
await self.gForce.async_on_cmd_response(data_package)
|
|
1851
|
+
last_cut = index = index + 2 + n + 1
|
|
1783
1852
|
index += 1
|
|
1784
|
-
|
|
1785
|
-
crc16 = (self._concatDataBuffer[index + 1 + n + 2] << 8) | self._concatDataBuffer[index + 1 + n + 1]
|
|
1786
|
-
calc_crc = sensor_utils.crc16_cal(self._concatDataBuffer[index + 2: index + 2 + n], n)
|
|
1787
|
-
if crc16 != calc_crc:
|
|
1853
|
+
else:
|
|
1788
1854
|
index += 1
|
|
1789
|
-
continue
|
|
1790
|
-
data_package = bytes(self._concatDataBuffer[index + 2: index + 2 + n])
|
|
1791
1855
|
|
|
1792
|
-
|
|
1793
|
-
|
|
1794
|
-
last_cut =
|
|
1795
|
-
index
|
|
1796
|
-
else:
|
|
1797
|
-
index += 1
|
|
1856
|
+
if last_cut > 0:
|
|
1857
|
+
self._concatDataBuffer = self._concatDataBuffer[last_cut + 1:]
|
|
1858
|
+
last_cut = -1
|
|
1859
|
+
index = 0
|
|
1798
1860
|
|
|
1799
|
-
|
|
1800
|
-
|
|
1801
|
-
|
|
1802
|
-
index = 0
|
|
1861
|
+
self._last_progress_time = time.time()
|
|
1862
|
+
except Exception as e:
|
|
1863
|
+
SdkLog.exception(_TAG, "Unexpected error in universal concat data processing")
|
|
@@ -8,7 +8,7 @@ with open(os.path.join(this_directory, "README.md"), "r", encoding="utf-8") as f
|
|
|
8
8
|
|
|
9
9
|
setup(
|
|
10
10
|
name="sensor-sdk",
|
|
11
|
-
version="0.
|
|
11
|
+
version="0.2.0",
|
|
12
12
|
description="Python sdk for Synchroni",
|
|
13
13
|
long_description=long_description,
|
|
14
14
|
long_description_content_type="text/markdown",
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|