This commit is contained in:
limengnan 2026-01-14 11:19:27 +08:00
commit 6ff9e0c8ca
14 changed files with 477 additions and 229 deletions

View File

@ -52,6 +52,7 @@ synchronized_images_only = False
[DEVICES]
imu_enable = True
imu_use_mock = False
imu_ble_name = WT901BLE67
imu_mac_address = FA:E8:88:06:FE:F3
pressure_enable = False
pressure_use_mock = False

View File

@ -33,6 +33,7 @@ class BaseDevice(ABC):
self.config = config
self.is_connected = False
self.is_streaming = False
self._initializing = False
self.socket_namespace = f"/{device_name}"
self.logger = logging.getLogger(f"device.{device_name}")
self._lock = threading.RLock() # 可重入锁

View File

@ -812,17 +812,19 @@ class CameraManager(BaseDevice):
Returns:
Dict[str, Any]: 设备状态信息
"""
status = super().get_status()
status.update({
return {
'device_type': 'camera',
'is_connected': self.is_connected,
'is_streaming': self.is_streaming,
'device_index': self.device_index,
'resolution': f"{self.width}x{self.height}",
'target_fps': self.fps,
'actual_fps': self.actual_fps,
'frame_count': self.frame_count,
'dropped_frames': self.dropped_frames,
'has_frame': self.last_frame is not None
})
return status
'has_frame': self.last_frame is not None,
'device_info': self.get_device_info()
}
def capture_image(self, save_path: Optional[str] = None) -> Optional[np.ndarray]:
"""

View File

@ -75,6 +75,8 @@ class DeviceCoordinator:
'device_errors': defaultdict(int),
'reconnect_attempts': defaultdict(int)
}
self._last_restart_ts = defaultdict(float)
self._restart_in_progress = defaultdict(bool)
# 线程池
self.executor = ThreadPoolExecutor(max_workers=8, thread_name_prefix="DeviceCoord")
@ -117,12 +119,12 @@ class DeviceCoordinator:
if not self._initialize_devices():
self.logger.warning("设备初始化失败,将以降级模式继续运行")
# 启动监控线程
self._start_monitor()
self.is_initialized = True
self.stats['start_time'] = time.time()
# 启动监控线程
self._start_monitor()
self.logger.info("设备协调器初始化成功")
self._emit_event('coordinator_initialized', {'devices': list(self.devices.keys())})
@ -191,7 +193,8 @@ class DeviceCoordinator:
success_count = 0
for device_name, future in futures:
try:
result = future.result(timeout=30) # 30秒超时
timeout_s = 45 if device_name == 'imu' else 30
result = future.result(timeout=timeout_s)
if result:
success_count += 1
self.logger.info(f"{device_name}设备初始化成功")
@ -587,12 +590,22 @@ class DeviceCoordinator:
was_streaming = False
try:
if self._restart_in_progress[device_name]:
self.logger.warning(f"{device_name} 设备正在重启中,跳过重复重启请求")
return False
self._restart_in_progress[device_name] = True
self._last_restart_ts[device_name] = time.time()
self.logger.info(f"开始彻底重启设备: {device_name}")
# 第一步:检查并停止数据流
stop_start = time.time()
if hasattr(device, 'is_streaming'):
was_streaming = device.is_streaming
try:
if hasattr(device, 'get_status'):
was_streaming = bool((device.get_status() or {}).get('is_streaming', False))
elif hasattr(device, 'is_streaming'):
was_streaming = bool(device.is_streaming)
except Exception:
was_streaming = False
if hasattr(device, 'stop_streaming') and was_streaming:
self.logger.info(f"正在停止 {device_name} 设备推流...")
@ -609,6 +622,11 @@ class DeviceCoordinator:
# 第二步:断开连接并彻底清理资源
cleanup_start = time.time()
self.logger.info(f"正在彻底清理 {device_name} 设备...")
try:
if hasattr(device, '_init_abort'):
device._init_abort.set()
except Exception:
pass
# 断开连接但暂时不广播状态变化,避免重启过程中的状态冲突
if hasattr(device, 'disconnect'):
@ -646,7 +664,7 @@ class DeviceCoordinator:
self.logger.info(f"{device_name} 设备实例已销毁")
# 短暂等待,确保资源完全释放
time.sleep(0.2)
time.sleep(1.5 if device_name == 'imu' else 0.2)
destroy_time = (time.time() - destroy_start) * 1000
# 第四步:重新创建设备实例
@ -754,8 +772,6 @@ class DeviceCoordinator:
if not new_device.initialize():
init_time = (time.time() - init_start) * 1000
self.logger.error(f"{device_name} 设备初始化失败 (耗时: {init_time:.1f}ms)")
# 初始化失败,从设备字典中移除
self.devices.pop(device_name, None)
return False
init_time = (time.time() - init_start) * 1000
@ -797,6 +813,8 @@ class DeviceCoordinator:
error_msg = f"彻底重启设备 {device_name} 异常: {e} (耗时: {total_time:.1f}ms)"
self.logger.error(error_msg)
return False
finally:
self._restart_in_progress[device_name] = False
def _start_monitor(self):
"""
@ -822,15 +840,18 @@ class DeviceCoordinator:
while self.is_initialized:
try:
# 检查设备健康状态
for device_name, device in self.devices.items():
for device_name, device in list(self.devices.items()):
try:
if self._restart_in_progress.get(device_name, False) or getattr(device, '_initializing', False):
continue
status = device.get_status()
if not status.get('is_connected', False):
self.logger.warning(f"设备 {device_name} 连接丢失")
self.stats['device_errors'][device_name] += 1
# 尝试重连
if self.stats['device_errors'][device_name] <= 3:
now = time.time()
if now - self._last_restart_ts[device_name] >= 15.0:
self._last_restart_ts[device_name] = now
self.logger.info(f"尝试重连设备: {device_name}")
if self.restart_device(device_name):
self.stats['device_errors'][device_name] = 0
@ -1098,7 +1119,7 @@ if __name__ == "__main__":
# 执行测试
# 可选值: 'camera1', 'camera2', 'imu', 'pressure', 'femtobolt'
success = test_restart_device('pressure')
success = test_restart_device('imu')
if success:
print("\n🎉 所有测试通过!")

View File

@ -1,9 +1,8 @@
# coding:UTF-8
import threading
import time
import struct
import bleak
import asyncio
import logging
# 设备实例 Device instance
@ -24,7 +23,8 @@ class DeviceModel:
# endregion
def __init__(self, deviceName, BLEDevice, callback_method):
print("Initialize device model")
self.logger = logging.getLogger("device.imu.witmotion")
self.logger.info("初始化IMU设备模型")
# 设备名称(自定义) Device Name
self.deviceName = deviceName
self.BLEDevice = BLEDevice
@ -57,10 +57,12 @@ class DeviceModel:
# 打开设备 open Device
async def openDevice(self):
print("Opening device......")
# 获取设备的服务和特征 Obtain the services and characteristic of the device
start_ts = time.perf_counter()
self.logger.info("正在打开蓝牙IMU设备...")
connect_start = time.perf_counter()
async with bleak.BleakClient(self.BLEDevice, timeout=15) as client:
self.client = client
self.logger.info(f"蓝牙连接建立完成(耗时: {(time.perf_counter() - connect_start)*1000:.1f}ms")
self.isOpen = True
# 设备UUID常量 Device UUID constant
target_service_uuid = "0000ffe5-0000-1000-8000-00805f9a34fb"
@ -68,11 +70,28 @@ class DeviceModel:
target_characteristic_uuid_write = "0000ffe9-0000-1000-8000-00805f9a34fb"
notify_characteristic = None
print("Matching services......")
for service in client.services:
self.logger.info("正在匹配服务...")
services = None
for _ in range(3):
get_services = getattr(client, 'get_services', None)
if callable(get_services):
services = await get_services()
else:
backend = getattr(client, "_backend", None)
backend_get_services = getattr(backend, "get_services", None)
if callable(backend_get_services):
services = await backend_get_services()
else:
services = getattr(client, 'services', None)
if services:
break
await asyncio.sleep(0.2)
if not services:
services = []
for service in services:
if service.uuid == target_service_uuid:
print(f"Service: {service}")
print("Matching characteristic......")
self.logger.info(f"匹配到服务: {service}")
self.logger.info("正在匹配特征...")
for characteristic in service.characteristics:
if characteristic.uuid == target_characteristic_uuid_read:
notify_characteristic = characteristic
@ -81,16 +100,12 @@ class DeviceModel:
if notify_characteristic:
break
if self.writer_characteristic:
# 读取磁场四元数 Reading magnetic field quaternions
print("Reading magnetic field quaternions")
time.sleep(3)
asyncio.create_task(self.sendDataTh())
if notify_characteristic:
print(f"Characteristic: {notify_characteristic}")
self.logger.info(f"匹配到特征: {notify_characteristic}")
# 设置通知以接收数据 Set up notifications to receive data
await client.start_notify(notify_characteristic.uuid, self.onDataReceived)
self.logger.info("开始接收姿态数据XYZ欧拉角")
self.logger.info(f"设备打开完成(耗时: {(time.perf_counter() - start_ts)*1000:.1f}ms")
# 保持连接打开 Keep connected and open
try:
@ -102,19 +117,13 @@ class DeviceModel:
# 在退出时停止通知 Stop notification on exit
await client.stop_notify(notify_characteristic.uuid)
else:
print("No matching services or characteristic found")
self.logger.warning("未找到匹配的服务或特征")
raise RuntimeError("未找到匹配的服务或特征")
# 关闭设备 close Device
def closeDevice(self):
self.isOpen = False
print("The device is turned off")
async def sendDataTh(self):
while self.isOpen:
await self.readReg(0x3A)
time.sleep(0.1)
await self.readReg(0x51)
time.sleep(0.1)
self.logger.info("设备已关闭")
# region 数据解析 data analysis
# 串口数据处理 Serial port data processing
@ -125,7 +134,7 @@ class DeviceModel:
if len(self.TempBytes) == 1 and self.TempBytes[0] != 0x55:
del self.TempBytes[0]
continue
if len(self.TempBytes) == 2 and (self.TempBytes[1] != 0x61 and self.TempBytes[1] != 0x71):
if len(self.TempBytes) == 2 and self.TempBytes[1] != 0x61:
del self.TempBytes[0]
continue
if len(self.TempBytes) == 20:
@ -134,47 +143,15 @@ class DeviceModel:
# 数据解析 data analysis
def processData(self, Bytes):
if Bytes[1] == 0x61:
Ax = self.getSignInt16(Bytes[3] << 8 | Bytes[2]) / 32768 * 16
Ay = self.getSignInt16(Bytes[5] << 8 | Bytes[4]) / 32768 * 16
Az = self.getSignInt16(Bytes[7] << 8 | Bytes[6]) / 32768 * 16
Gx = self.getSignInt16(Bytes[9] << 8 | Bytes[8]) / 32768 * 2000
Gy = self.getSignInt16(Bytes[11] << 8 | Bytes[10]) / 32768 * 2000
Gz = self.getSignInt16(Bytes[13] << 8 | Bytes[12]) / 32768 * 2000
if Bytes[1] != 0x61:
return
AngX = self.getSignInt16(Bytes[15] << 8 | Bytes[14]) / 32768 * 180
AngY = self.getSignInt16(Bytes[17] << 8 | Bytes[16]) / 32768 * 180
AngZ = self.getSignInt16(Bytes[19] << 8 | Bytes[18]) / 32768 * 180
self.set("AccX", round(Ax, 3))
self.set("AccY", round(Ay, 3))
self.set("AccZ", round(Az, 3))
self.set("AsX", round(Gx, 3))
self.set("AsY", round(Gy, 3))
self.set("AsZ", round(Gz, 3))
self.set("AngX", round(AngX, 3))
self.set("AngY", round(AngY, 3))
self.set("AngZ", round(AngZ, 3))
self.callback_method(self)
else:
# 磁场 magnetic field
if Bytes[2] == 0x3A:
Hx = self.getSignInt16(Bytes[5] << 8 | Bytes[4]) / 120
Hy = self.getSignInt16(Bytes[7] << 8 | Bytes[6]) / 120
Hz = self.getSignInt16(Bytes[9] << 8 | Bytes[8]) / 120
self.set("HX", round(Hx, 3))
self.set("HY", round(Hy, 3))
self.set("HZ", round(Hz, 3))
# 四元数 Quaternion
elif Bytes[2] == 0x51:
Q0 = self.getSignInt16(Bytes[5] << 8 | Bytes[4]) / 32768
Q1 = self.getSignInt16(Bytes[7] << 8 | Bytes[6]) / 32768
Q2 = self.getSignInt16(Bytes[9] << 8 | Bytes[8]) / 32768
Q3 = self.getSignInt16(Bytes[11] << 8 | Bytes[10]) / 32768
self.set("Q0", round(Q0, 5))
self.set("Q1", round(Q1, 5))
self.set("Q2", round(Q2, 5))
self.set("Q3", round(Q3, 5))
else:
pass
# 获得int16有符号数 Obtain int16 signed number
@staticmethod
@ -191,7 +168,7 @@ class DeviceModel:
if self.client.is_connected and self.writer_characteristic is not None:
await self.client.write_gatt_char(self.writer_characteristic.uuid, bytes(data))
except Exception as ex:
print(ex)
self.logger.warning(f"发送数据失败: {ex}")
# 读取寄存器 read register
async def readReg(self, regAddr):

View File

@ -939,8 +939,10 @@ class FemtoBoltManager(BaseDevice):
Returns:
Dict[str, Any]: 设备状态信息
"""
status = super().get_status()
status.update({
return {
'device_type': 'femtobolt',
'is_connected': self.is_connected,
'is_streaming': self.is_streaming,
'color_resolution': self.color_resolution,
'depth_mode': self.depth_mode,
'target_fps': self.fps,
@ -949,9 +951,9 @@ class FemtoBoltManager(BaseDevice):
'dropped_frames': self.dropped_frames,
'depth_range': f"{self.depth_range_min}-{self.depth_range_max}mm",
'has_depth_frame': self.last_depth_frame is not None,
'has_color_frame': self.last_color_frame is not None
})
return status
'has_color_frame': self.last_color_frame is not None,
'device_info': self.get_device_info()
}

View File

@ -23,8 +23,9 @@ logger = logging.getLogger(__name__)
class BleIMUDevice:
"""蓝牙IMU设备WitMotion WT9011DCL-BT50基于 device_model.py 官方接口"""
def __init__(self, mac_address: str):
def __init__(self, mac_address: str, ble_name: str = ""):
self.mac_address = mac_address
self.ble_name = ble_name
self.loop = None
self.loop_thread = None
self.running = False
@ -40,11 +41,14 @@ class BleIMUDevice:
self._connected = False
self._device_model = None
self._open_task = None
self._main_task = None
self._last_update_ts = None
try:
from . import device_model as wit_device_model
except Exception:
import device_model as wit_device_model
self._wit_device_model = wit_device_model
logger.info(f"BLE IMU实例创建: mac={self.mac_address}")
def set_calibration(self, calibration: Dict[str, Any]):
self.calibration_data = calibration
@ -75,7 +79,21 @@ class BleIMUDevice:
self.running = False
try:
if self.loop:
asyncio.run_coroutine_threadsafe(self._disconnect(), self.loop)
try:
if self._main_task is not None and not self._main_task.done():
self.loop.call_soon_threadsafe(self._main_task.cancel)
except Exception:
pass
try:
fut = asyncio.run_coroutine_threadsafe(self._disconnect(), self.loop)
fut.result(timeout=5.0)
except Exception:
pass
except Exception:
pass
try:
if self.loop_thread and self.loop_thread.is_alive():
self.loop_thread.join(timeout=6.0)
except Exception:
pass
@ -96,18 +114,28 @@ class BleIMUDevice:
self.loop = asyncio.new_event_loop()
asyncio.set_event_loop(self.loop)
try:
self.loop.run_until_complete(self._connect_and_listen())
self._main_task = self.loop.create_task(self._connect_and_listen())
self.loop.run_until_complete(self._main_task)
except asyncio.CancelledError:
pass
except Exception as e:
logger.error(f'BLE IMU事件循环异常: {e}', exc_info=True)
finally:
try:
if not self.loop.is_closed():
self.loop.stop()
self.loop.close()
try:
pending = asyncio.all_tasks(self.loop)
for t in pending:
t.cancel()
if pending:
self.loop.run_until_complete(asyncio.gather(*pending, return_exceptions=True))
except Exception:
pass
if not self.loop.is_closed():
self.loop.stop()
except Exception:
pass
self._main_task = None
self.loop = None
def _on_device_update(self, dm):
try:
@ -120,6 +148,8 @@ class BleIMUDevice:
self.last_data['roll'] = float(roll)
self.last_data['pitch'] = float(pitch)
self.last_data['yaw'] = float(yaw)
self._last_update_ts = time.time()
self._connected = True
except Exception:
pass
@ -130,6 +160,12 @@ class BleIMUDevice:
self._device_model.closeDevice()
except Exception:
pass
try:
client = getattr(self._device_model, "client", None)
if client is not None and getattr(client, "is_connected", False):
await client.disconnect()
except Exception:
pass
if self._open_task is not None and not self._open_task.done():
self._open_task.cancel()
try:
@ -141,6 +177,7 @@ class BleIMUDevice:
finally:
self._open_task = None
self._device_model = None
self._last_update_ts = None
async def _connect_and_listen(self):
try:
@ -150,39 +187,86 @@ class BleIMUDevice:
self.running = False
return
async def find_device() -> Optional[Any]:
scan_timeout_s = 30.0
if self.ble_name:
find_by_name = getattr(BleakScanner, "find_device_by_name", None)
if callable(find_by_name):
try:
device = await find_by_name(self.ble_name, timeout=scan_timeout_s)
if device is not None:
if self.mac_address and (getattr(device, "address", "") or "").lower() != self.mac_address.lower():
return None
return device
except Exception:
pass
try:
found = await BleakScanner.discover(timeout=scan_timeout_s)
except Exception:
found = []
if self.ble_name:
for d in found:
if (getattr(d, "name", None) or "") != self.ble_name:
continue
if self.mac_address and (getattr(d, "address", "") or "").lower() != self.mac_address.lower():
continue
return d
if self.mac_address:
target = self.mac_address.lower()
for d in found:
addr = getattr(d, "address", "") or ""
if addr.lower() == target:
return d
candidates = [d for d in found if (getattr(d, "name", "") or "").startswith("WT")]
if len(candidates) == 1:
return candidates[0]
return None
while self.running:
try:
try:
device = await BleakScanner.find_device_by_address(self.mac_address, timeout=20.0)
except TypeError:
device = await BleakScanner.find_device_by_address(self.mac_address, cb=dict(use_bdaddr=False))
attempt_ts = time.perf_counter()
logger.info(f"BLE IMU开始扫描并连接: name={self.ble_name}, mac={self.mac_address}")
device = await find_device()
if device is None:
logger.info(f"BLE IMU扫描未发现设备 (耗时: {(time.perf_counter()-attempt_ts)*1000:.1f}ms)")
await asyncio.sleep(2.0)
continue
device_addr = getattr(device, "address", None)
device_name = getattr(device, "name", None)
logger.info(f"BLE IMU发现设备 (耗时: {(time.perf_counter()-attempt_ts)*1000:.1f}ms, address={device_addr}, name={device_name})")
self._connected = False
self._last_update_ts = None
self._device_model = self._wit_device_model.DeviceModel("WitMotionBle5.0", device, self._on_device_update)
self._open_task = asyncio.create_task(self._device_model.openDevice())
connected = False
for _ in range(50):
if not self.running:
ready = False
ready_timeout_s = 20.0
deadline = time.time() + ready_timeout_s
while self.running and time.time() < deadline:
if self._open_task is not None and self._open_task.done():
try:
exc = self._open_task.exception()
except Exception:
exc = None
if exc:
logger.warning(f"BLE IMU打开失败: {type(exc).__name__}: {repr(exc)}")
break
if self._open_task.done():
if self.has_received_data:
ready = True
break
client = getattr(self._device_model, "client", None)
if client is not None and getattr(client, "is_connected", False):
connected = True
break
await asyncio.sleep(0.2)
await asyncio.sleep(0.1)
self._connected = connected
if not connected:
if not ready:
logger.warning(f"BLE IMU未获取到姿态数据 (耗时: {(time.perf_counter()-attempt_ts)*1000:.1f}ms)")
await self._disconnect()
self._connected = False
await asyncio.sleep(2.0)
continue
logger.info(f"BLE IMU连接并开始产出数据 (耗时: {(time.perf_counter()-attempt_ts)*1000:.1f}ms)")
while self.running and self._open_task is not None and not self._open_task.done():
await asyncio.sleep(1.0)
@ -200,6 +284,14 @@ class BleIMUDevice:
def connected(self) -> bool:
return self._connected
@property
def has_received_data(self) -> bool:
return self._last_update_ts is not None
@property
def last_update_time(self):
return self._last_update_ts
class MockIMUDevice:
def __init__(self):
self.running = False
@ -215,6 +307,7 @@ class MockIMUDevice:
'temperature': 25.0
}
self._phase = 0.0
self._last_update_ts = None
def set_calibration(self, calibration: Dict[str, Any]):
self.calibration_data = calibration
@ -286,6 +379,7 @@ class MockIMUDevice:
self.last_data['roll'] = round(roll, 1)
# 温度模拟以25℃为基准叠加±0.5℃的轻微波动
self.last_data['temperature'] = round(25.0 + math.sin(self._phase * 0.2) * 0.5, 2)
self._last_update_ts = time.time()
# 控制输出频率为约60Hz
time.sleep(1.0 / 30.0)
except Exception:
@ -296,6 +390,14 @@ class MockIMUDevice:
def connected(self) -> bool:
return self._connected
@property
def has_received_data(self) -> bool:
return self._last_update_ts is not None
@property
def last_update_time(self):
return self._last_update_ts
class IMUManager(BaseDevice):
"""IMU传感器管理器"""
@ -319,11 +421,12 @@ class IMUManager(BaseDevice):
# 设备配置
self.use_mock = bool(config.get('use_mock', False))
self.mac_address = config.get('mac_address', '')
self.ble_name = config.get('ble_name', '')
# IMU设备实例
self.imu_device = None
self._init_abort = threading.Event()
# 推流相关
self.imu_streaming = False
self.imu_thread = None
# 统计信息
@ -338,7 +441,7 @@ class IMUManager(BaseDevice):
self.data_buffer = deque(maxlen=100)
self.last_valid_data = None
self.logger.info(f"IMU管理器初始化完成 - use_mock: {self.use_mock}, MAC: {self.mac_address}")
self.logger.info(f"IMU管理器初始化完成 - use_mock: {self.use_mock}, BLE_NAME: {self.ble_name}, MAC: {self.mac_address}")
def initialize(self) -> bool:
"""
@ -347,35 +450,62 @@ class IMUManager(BaseDevice):
Returns:
bool: 初始化是否成功
"""
self._initializing = True
self._init_abort.clear()
try:
start_ts = time.perf_counter()
self.logger.info(f"正在初始化IMU设备...")
# 使用构造函数中已加载的配置,避免并发读取配置文件
self.logger.info(f"使用已加载配置: use_mock={self.use_mock}, mac={self.mac_address}")
self.logger.info(f"使用已加载配置: use_mock={self.use_mock}, name={self.ble_name}, mac={self.mac_address}")
# 根据配置选择设备类型
if not self.use_mock:
if not self.mac_address:
self.logger.error("IMU BLE设备未配置MAC地址")
if not self.mac_address and not self.ble_name:
self.logger.error("IMU BLE设备未配置蓝牙名称或MAC地址")
self.is_connected = False
return False
self.logger.info(f"使用蓝牙IMU设备 - MAC: {self.mac_address}")
self.imu_device = BleIMUDevice(self.mac_address)
self.logger.info(f"使用蓝牙IMU设备 - NAME: {self.ble_name}, MAC: {self.mac_address}")
try:
if self.imu_device and hasattr(self.imu_device, 'stop'):
self.imu_device.stop()
except Exception:
pass
self.imu_device = BleIMUDevice(self.mac_address, self.ble_name)
self.imu_device.start()
# 使用set_connected方法来正确启动连接监控线程
self.set_connected(True)
connect_timeout_s = float(self.config.get('connect_timeout', 40.0))
deadline = time.time() + max(0.1, connect_timeout_s)
while time.time() < deadline:
if self._init_abort.is_set():
self.logger.warning("IMU初始化被中断")
return False
if bool(getattr(self.imu_device, 'connected', False)):
break
time.sleep(0.1)
connected = bool(getattr(self.imu_device, 'connected', False))
self._start_connection_monitor()
self.set_connected(connected)
if not connected:
self.logger.error(f"IMU蓝牙连接超时等待 {connect_timeout_s:.1f}s")
try:
self.imu_device.stop()
except Exception:
pass
self.imu_device = None
self.set_connected(False)
return False
else:
self.logger.info("使用模拟IMU设备")
self.imu_device = MockIMUDevice()
self.imu_device.start()
# 使用set_connected方法来正确启动连接监控线程
self._start_connection_monitor()
self.set_connected(True)
self._device_info.update({
'mac_address': self.mac_address,
})
self.logger.info("IMU初始化成功")
self.logger.info(f"IMU初始化完成耗时: {(time.perf_counter() - start_ts)*1000:.1f}ms当前连接状态: {self.is_connected}")
return True
except Exception as e:
@ -383,6 +513,8 @@ class IMUManager(BaseDevice):
self.is_connected = False
self.imu_device = None
return False
finally:
self._initializing = False
def _quick_calibrate_imu(self) -> Dict[str, Any]:
"""
@ -464,20 +596,39 @@ class IMUManager(BaseDevice):
bool: 启动是否成功
"""
try:
if self.is_streaming:
self.logger.warning("IMU数据流已在运行")
return True
if not self.is_connected or not self.imu_device:
if not self.initialize():
return False
if self.imu_streaming:
self.logger.warning("IMU数据流已在运行")
return True
if not self.is_connected:
self.logger.error("IMU设备未连接")
return False
first_data_timeout_s = float(self.config.get('first_data_timeout', 30.0))
deadline = time.time() + max(0.1, first_data_timeout_s)
while time.time() < deadline:
if not self.imu_device:
break
if hasattr(self.imu_device, 'connected') and not bool(getattr(self.imu_device, 'connected')):
break
if bool(getattr(self.imu_device, 'has_received_data', False)):
break
time.sleep(0.05)
if not self.imu_device or not bool(getattr(self.imu_device, 'has_received_data', False)):
self.logger.error(f"IMU未获取到有效数据等待 {first_data_timeout_s:.1f}s")
return False
# 启动前进行快速校准
if not self.is_calibrated:
self.logger.info("启动前进行快速零点校准...")
self._quick_calibrate_imu()
self.imu_streaming = True
self.is_streaming = True
self.update_heartbeat()
self.imu_thread = threading.Thread(target=self._imu_streaming_thread, daemon=True)
self.imu_thread.start()
@ -486,7 +637,7 @@ class IMUManager(BaseDevice):
except Exception as e:
self.logger.error(f"IMU数据流启动失败: {e}")
self.imu_streaming = False
self.is_streaming = False
return False
def stop_streaming(self) -> bool:
@ -497,7 +648,7 @@ class IMUManager(BaseDevice):
bool: 停止是否成功
"""
try:
self.imu_streaming = False
self.is_streaming = False
if self.imu_thread and self.imu_thread.is_alive():
self.imu_thread.join(timeout=3.0)
@ -522,19 +673,25 @@ class IMUManager(BaseDevice):
"""
self.logger.info("IMU数据流工作线程启动")
while self.imu_streaming:
while self.is_streaming:
try:
if self.imu_device:
# 读取IMU数据
data = self.imu_device.read_data(apply_calibration=True)
if data:
if data and isinstance(data, dict) and data.get('head_pose') is not None:
# 发送数据到前端
if self._socketio:
self._socketio.emit('imu_data', data, namespace='/devices')
# 更新统计
self.data_count += 1
self.update_heartbeat()
self.last_valid_data = data
try:
self.data_buffer.append(data)
except Exception:
pass
else:
self.error_count += 1
@ -556,19 +713,19 @@ class IMUManager(BaseDevice):
Returns:
Dict[str, Any]: 设备状态信息
"""
status = super().get_status()
status.update({
'is_streaming': self.imu_streaming,
return {
'device_type': 'mock' if self.use_mock else 'ble',
'mac_address': self.mac_address,
'is_connected': self.is_connected,
'is_streaming': self.is_streaming,
'is_calibrated': self.is_calibrated,
'data_count': self.data_count,
'error_count': self.error_count,
'buffer_size': len(self.data_buffer),
'has_data': self.last_valid_data is not None,
'head_pose_offset': self.head_pose_offset,
'device_type': 'mock' if self.use_mock else 'ble',
'mac_address': self.mac_address
})
return status
'device_info': self.get_device_info()
}
def get_latest_data(self) -> Optional[Dict[str, float]]:
"""
@ -586,9 +743,15 @@ class IMUManager(BaseDevice):
断开IMU设备连接
"""
try:
self._init_abort.set()
self.stop_streaming()
if self.imu_device:
try:
if hasattr(self.imu_device, 'stop'):
self.imu_device.stop()
except Exception:
pass
self.imu_device = None
self.is_connected = False
@ -643,7 +806,18 @@ class IMUManager(BaseDevice):
return False
if hasattr(self.imu_device, 'connected'):
return bool(getattr(self.imu_device, 'connected'))
connected = bool(getattr(self.imu_device, 'connected'))
if connected:
last_ts = getattr(self.imu_device, 'last_update_time', None)
if last_ts:
try:
if time.time() - float(last_ts) <= float(self._connection_timeout):
self.update_heartbeat()
except Exception:
self.update_heartbeat()
else:
self.update_heartbeat()
return connected
if hasattr(self.imu_device, 'ser') and getattr(self.imu_device, 'ser', None):
if not self.imu_device.ser.is_open:
@ -653,6 +827,7 @@ class IMUManager(BaseDevice):
self.imu_device.ser.timeout = 0.1
self.imu_device.ser.read(1)
self.imu_device.ser.timeout = original_timeout
self.update_heartbeat()
return True
except Exception:
return False
@ -668,6 +843,7 @@ class IMUManager(BaseDevice):
清理资源
"""
try:
self._init_abort.set()
# 停止连接监控
self._cleanup_monitoring()

View File

@ -1,69 +1,84 @@
import argparse
import asyncio
import time
from statistics import mean
import bleak
import device_model
# 扫描到的设备 Scanned devices
devices = []
# 蓝牙设备 BLEDevice
BLEDevice = None
# 扫描蓝牙设备并过滤名称
# Scan Bluetooth devices and filter names
async def scan():
global devices
global BLEDevice
find = []
print("Searching for Bluetooth devices......")
async def find_device_by_address(address: str, timeout_s: float):
try:
devices = await bleak.BleakScanner.discover(timeout=20.0)
print("Search ended")
return await bleak.BleakScanner.find_device_by_address(address, timeout=timeout_s)
except TypeError:
return await bleak.BleakScanner.find_device_by_address(address, cb=dict(use_bdaddr=False))
async def find_device_by_name(name: str, timeout_s: float):
scanner_fn = getattr(bleak.BleakScanner, "find_device_by_name", None)
if callable(scanner_fn):
return await scanner_fn(name, timeout=timeout_s)
devices = await bleak.BleakScanner.discover(timeout=timeout_s)
for d in devices:
if d.name is not None and "WT" in d.name:
find.append(d)
print(d)
if len(find) == 0:
print("No devices found in this search!")
if (getattr(d, "name", None) or "") == name:
return d
return None
async def run_trials(label: str, finder, runs: int, cooldown_s: float):
ok_times = []
fail = 0
for i in range(1, runs + 1):
start = time.perf_counter()
device = await finder()
ms = (time.perf_counter() - start) * 1000
if device is None:
fail += 1
print(f"[{label}] [{i:03d}] FAIL {ms:.1f}ms")
else:
user_input = input("Please enter the Mac address you want to connect to (e.g. DF:E9:1F:2C:BD:59)")
for d in devices:
if d.address == user_input:
BLEDevice = d
break
except Exception as ex:
print("Bluetooth search failed to start")
print(ex)
addr = getattr(device, "address", None)
name = getattr(device, "name", None)
ok_times.append(ms)
print(f"[{label}] [{i:03d}] OK {ms:.1f}ms address={addr} name={name}")
if cooldown_s > 0:
await asyncio.sleep(cooldown_s)
# 指定MAC地址搜索并连接设备
# Specify MAC address to search and connect devices
async def scanByMac(device_mac):
global BLEDevice
print("Searching for Bluetooth devices......")
BLEDevice = await bleak.BleakScanner.find_device_by_address(device_mac, timeout=20)
# 数据更新时会调用此方法 This method will be called when data is updated
def updateData(DeviceModel):
# 直接打印出设备数据字典 Directly print out the device data dictionary
print(DeviceModel.deviceData)
# 获得X轴加速度 Obtain X-axis acceleration
# print(DeviceModel.get("AccX"))
if __name__ == '__main__':
# 方式一:广播搜索和连接蓝牙设备
# # Method 1:Broadcast search and connect Bluetooth devices
# asyncio.run(scan())
# # 方式二指定MAC地址搜索并连接设备
# # Method 2: Specify MAC address to search and connect devices
asyncio.run(scanByMac("FA:E8:88:06:FE:F3"))
if BLEDevice is not None:
# 创建设备 Create device
device = device_model.DeviceModel("MyBle5.0", BLEDevice, updateData)
# 开始连接设备 Start connecting devices
asyncio.run(device.openDevice())
if ok_times:
print(f"[{label}] runs={runs} success={len(ok_times)} fail={fail} avg={mean(ok_times):.1f}ms min={min(ok_times):.1f}ms max={max(ok_times):.1f}ms")
else:
print("This BLEDevice was not found!!")
print(f"[{label}] runs={runs} success=0 fail={fail}")
def parse_args():
parser = argparse.ArgumentParser()
parser.add_argument("--address", default="FA:E8:88:06:FE:F3")
parser.add_argument("--name", default="WT901BLE67")
parser.add_argument("--runs", type=int, default=10)
parser.add_argument("--timeout", type=float, default=30.0)
parser.add_argument("--cooldown", type=float, default=0.3)
parser.add_argument("--mode", choices=["mac", "name", "both"], default="both")
return parser.parse_args()
async def main():
args = parse_args()
if args.mode in ("mac", "both"):
await run_trials(
"mac",
lambda: find_device_by_address(args.address, args.timeout),
args.runs,
args.cooldown,
)
if args.mode in ("name", "both"):
await run_trials(
"name",
lambda: find_device_by_name(args.name, args.timeout),
args.runs,
args.cooldown,
)
if __name__ == "__main__":
asyncio.run(main())

View File

@ -98,6 +98,7 @@ class ConfigManager:
self.config['DEVICES'] = {
'imu_enable': 'False',
'imu_use_mock': 'False',
'imu_ble_name': '',
'imu_mac_address': '',
'pressure_port': 'COM8',
'pressure_baudrate': '115200'
@ -235,6 +236,7 @@ class ConfigManager:
return {
'enable': self.config.getboolean('DEVICES', 'imu_enable', fallback=False),
'use_mock': self.config.getboolean('DEVICES', 'imu_use_mock', fallback=False),
'ble_name': self.config.get('DEVICES', 'imu_ble_name', fallback=''),
'mac_address': self.config.get('DEVICES', 'imu_mac_address', fallback='FA:E8:88:06:FE:F3'),
}

View File

@ -18,6 +18,7 @@ import logging
from flask_socketio import SocketIO, emit
import configparser
import argparse
from collections import defaultdict
# 添加当前目录到路径
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
@ -81,6 +82,7 @@ class AppServer:
# 数据推送状态
self.is_pushing_data = False
self._device_op_locks = defaultdict(threading.RLock)
# 设备管理器
self.config_manager = None
@ -98,6 +100,9 @@ class AppServer:
self._register_routes()
self._register_socketio_events()
def _get_device_lock(self, device_name: str):
return self._device_op_locks[device_name]
def _init_logging(self):
"""初始化日志配置"""
# 日志目录
@ -228,6 +233,10 @@ class AppServer:
self.logger.info('正在初始化设备协调器...')
self.device_coordinator = DeviceCoordinator(self.socketio)
# Flask应用启动后异步初始化设备如果尚未初始化
if self.device_coordinator and not self.device_coordinator.is_initialized:
threading.Thread(target=self._initialize_devices, daemon=True).start()
# 初始化录制管理器
self.logger.info('正在初始化录制管理器...')
@ -755,9 +764,7 @@ class AppServer:
self.logger.info(f'用户 {username} 登录成功')
# 登录成功后,异步初始化设备(如果尚未初始化)
if self.device_coordinator and not self.device_coordinator.is_initialized:
threading.Thread(target=self._initialize_devices, daemon=True).start()
return jsonify({
'success': True,
@ -1964,6 +1971,7 @@ class AppServer:
self.logger.info(f'开始重启 {device_type} 设备...')
# 调用设备协调器的重启方法
with self._get_device_lock(device_type):
success = self.device_coordinator.restart_device(device_type)
if success:
@ -2023,6 +2031,25 @@ class AppServer:
def initialize_device(device_name, manager):
"""设备初始化工作函数"""
try:
with self._get_device_lock(device_name):
restarting = bool(getattr(self.device_coordinator, '_restart_in_progress', {}).get(device_name, False))
if restarting:
device_results[device_name] = False
self.logger.warning(f'{device_name}设备正在重启,跳过启动')
return
initializing = bool(getattr(manager, '_initializing', False))
if initializing:
wait_deadline = time.time() + (60.0 if device_name == 'imu' else 30.0)
while time.time() < wait_deadline and bool(getattr(manager, '_initializing', False)):
time.sleep(0.2)
if hasattr(manager, 'is_connected') and manager.is_connected:
if hasattr(manager, 'is_streaming') and not manager.is_streaming:
manager.start_streaming()
device_results[device_name] = True
self.logger.info(f'{device_name}设备已连接,启动成功')
return
# 检查设备是否已连接,避免重复初始化
if hasattr(manager, 'is_connected') and manager.is_connected:
print(f"[DEBUG] {device_name} 已连接,跳过初始化")

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 7.1 KiB

View File

@ -794,9 +794,12 @@
<div class="pop-up-camera-title">IMU设备</div>
</div>
<div class="pop-up-camera-display" style="padding-top: 10px;">
<div class="pop-up-camera-name">蓝牙名称</div>
<el-input v-model="cameraForm.imu.ble_name" placeholder="请输入"
style="width: 180px;" />
<div class="pop-up-camera-name">Mac地址</div>
<el-input v-model="cameraForm.imu.mac_address" placeholder="请输入"
style="width: 434px;" />
style="width: 180px;" />
<el-checkbox v-model="cameraForm.imu.enable" label="有效" size="large" style="width: 60px;margin-left:10px ;" />
</div>
@ -1774,47 +1777,50 @@ function handleIMUData(data) {
function updateHeadPoseMaxValues(headPose) {
try {
//
if (headPose.rotation < 0) {
if (headPose.rotation > 0) {
//
headPoseMaxValues.value.rotationLeftMax = Math.max(
headPoseMaxValues.value.rotationLeftMax,
Math.abs(headPose.rotation)
headPose.rotation
)
} else if (headPose.rotation > 0) {
} else if (headPose.rotation < 0) {
//
headPoseMaxValues.value.rotationRightMax = Math.max(
headPoseMaxValues.value.rotationRightMax,
headPose.rotation
Math.abs(headPose.rotation)
)
}
//
if (headPose.tilt < 0) {
if (headPose.tilt > 0) {
//
headPoseMaxValues.value.tiltLeftMax = Math.max(
headPoseMaxValues.value.tiltLeftMax,
Math.abs(headPose.tilt)
headPose.tilt
)
} else if (headPose.tilt > 0) {
} else if (headPose.tilt < 0) {
//
headPoseMaxValues.value.tiltRightMax = Math.max(
headPoseMaxValues.value.tiltRightMax,
headPose.tilt
Math.abs(headPose.tilt)
)
}
//
if (headPose.pitch < 0) {
if (headPose.pitch >0) {
//
headPoseMaxValues.value.pitchDownMax = Math.max(
headPoseMaxValues.value.pitchDownMax,
Math.abs(headPose.pitch)
headPose.pitch
)
} else if (headPose.pitch > 0) {
} else if (headPose.pitch < 0) {
//
headPoseMaxValues.value.pitchUpMax = Math.max(
headPoseMaxValues.value.pitchUpMax,
headPose.pitch
Math.abs(headPose.pitch)
)
}
@ -3360,7 +3366,7 @@ function viewClick(e){
font-style: normal;
font-size: 14px;
color: rgba(255,255,255,0.6);
width: 60px;
width: 70px;
text-align: right;
margin-right: 20px;
}

View File

@ -1,13 +1,16 @@
<template>
<div id="containermodel"></div>
<div id="containermodel">
<img class="axes-overlay" :src="axesBgUrl" alt="" />
</div>
</template>
<script setup>
import { onMounted, onUnmounted, watch } from 'vue'
import * as THREE from 'three';
import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js';
import maleUrl from '@/assets/glb/male.glb?url'
import femaleUrl from '@/assets/glb/female.glb?url'
import modelUrl from '@/assets/glb/head.glb?url'
import axesBgUrl from '@/assets/glb/xyz-axes.png?url'
const props = defineProps({
rotation: { type: [Number, String], default: 0 },
@ -25,7 +28,7 @@ let onResizeHandler = null;
//
let lastRenderTime = 0;
const TARGET_FPS = 10; // 25
const TARGET_FPS = 20; // 25
const FRAME_INTERVAL = 1000 / TARGET_FPS;
onMounted(() => {
@ -121,7 +124,7 @@ function setupLights() {
function loadModel() {
const loader = new GLTFLoader();
const url = (props.gender === '女') ? femaleUrl : maleUrl;
const url = modelUrl;
loader.load(url,
(gltf) => {
@ -132,11 +135,11 @@ function loadModel() {
const box = new THREE.Box3().setFromObject(model);
const size = box.getSize(new THREE.Vector3());
const maxDim = Math.max(size.x, size.y, size.z);
const scale = 5 / maxDim;
const scale = 3.8 / maxDim;
model.scale.set(scale, scale, scale);
const center = box.getCenter(new THREE.Vector3());
model.position.set(-center.x * scale, -center.y * scale + 0.2, -center.z * scale);
model.position.set(-center.x * scale, -center.y * scale-0.2, -center.z * scale);
model.rotation.set(0, 0, 0);
model.traverse((child) => {
@ -270,4 +273,19 @@ watch(
position: relative;
overflow: hidden; /* 防止 canvas 溢出 */
}
.axes-overlay {
position: absolute;
inset: 0;
width: 100%;
height: 100%;
object-fit: contain;
z-index: 2;
pointer-events: none;
}
:global(#containermodel canvas) {
position: relative;
z-index: 1;
}
</style>