增加imu设备相关代码
This commit is contained in:
parent
c1cb2a0970
commit
6ec453a63d
@ -39,7 +39,7 @@ fourcc = MJPG
|
||||
backend = directshow
|
||||
|
||||
[FEMTOBOLT]
|
||||
enable = True
|
||||
enable = False
|
||||
algorithm_type = plt
|
||||
color_resolution = 1080P
|
||||
depth_mode = NFOV_2X2BINNED
|
||||
@ -50,19 +50,16 @@ fps = 15
|
||||
synchronized_images_only = False
|
||||
|
||||
[DEVICES]
|
||||
imu_enable = False
|
||||
imu_device_type = ble
|
||||
imu_port = COM9
|
||||
imu_mac_address = ef:3c:1a:0a:fe:02
|
||||
imu_baudrate = 9600
|
||||
imu_enable = True
|
||||
imu_use_mock = False
|
||||
imu_mac_address = FA:E8:88:06:FE:F3
|
||||
pressure_enable = False
|
||||
pressure_device_type = real
|
||||
pressure_use_mock = False
|
||||
pressure_port = COM5
|
||||
pressure_baudrate = 115200
|
||||
|
||||
[REMOTE]
|
||||
enable = True
|
||||
enable = False
|
||||
port = COM6
|
||||
baudrate = 115200
|
||||
timeout = 0.1
|
||||
|
||||
@ -246,6 +246,7 @@ class DatabaseManager:
|
||||
id TEXT PRIMARY KEY, -- 记录唯一标识(YYYYMMDDHHMMSS)年月日时分秒
|
||||
session_id TEXT NOT NULL, -- 检测会话ID（外键）
|
||||
head_pose TEXT , -- 头部姿态数据（JSON格式）
|
||||
head_data_image TEXT , -- 头部姿态截图存储路径
|
||||
body_pose TEXT , -- 身体姿态数据（JSON格式）
|
||||
foot_data TEXT , -- 足部姿态数据（JSON格式）
|
||||
body_image TEXT, -- 身体视频截图存储路径
|
||||
@ -490,7 +491,7 @@ class DatabaseManager:
|
||||
UPDATE patients SET
|
||||
name = ?, gender = ?, birth_date = ?, nationality = ?,
|
||||
residence = ?, height = ?, weight = ?, shoe_size = ?, phone = ?, email = ?,
|
||||
occupation = ?, workplace = ?, medical_history = ?, idcode = ?, notes = ?, updated_at = ?
|
||||
occupation = ?, workplace = ?, idcode = ?, notes = ?, updated_at = ?
|
||||
WHERE id = ?
|
||||
''', (
|
||||
patient_data.get('name'),
|
||||
@ -505,7 +506,6 @@ class DatabaseManager:
|
||||
patient_data.get('email'),
|
||||
patient_data.get('occupation'),
|
||||
patient_data.get('workplace'),
|
||||
patient_data.get('medical_history'),
|
||||
patient_data.get('idcode'),
|
||||
patient_data.get('notes'),
|
||||
china_time,
|
||||
@ -955,13 +955,14 @@ class DatabaseManager:
|
||||
# 根据表结构保存数据
|
||||
cursor.execute('''
|
||||
INSERT INTO detection_data (
|
||||
id, session_id, head_pose, body_pose, foot_data,
|
||||
id, session_id, head_pose, head_data_image, body_pose, foot_data,
|
||||
body_image, screen_image, foot_data_image, foot1_image, foot2_image, timestamp
|
||||
) VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)
|
||||
) VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)
|
||||
''', (
|
||||
data_id,
|
||||
session_id,
|
||||
json.dumps(data.get('head_pose')) if data.get('head_pose') else None,
|
||||
data.get('head_data_image'),
|
||||
json.dumps(data.get('body_pose')) if data.get('body_pose') else None,
|
||||
json.dumps(data.get('foot_data')) if data.get('foot_data') else None,
|
||||
data.get('body_image'),
|
||||
|
||||
246
backend/devices/device_model.py
Normal file
246
backend/devices/device_model.py
Normal file
@ -0,0 +1,246 @@
|
||||
# coding:UTF-8
|
||||
import threading
|
||||
import time
|
||||
import struct
|
||||
import bleak
|
||||
import asyncio
|
||||
|
||||
|
||||
# 设备实例 Device instance
|
||||
class DeviceModel:
|
||||
# region 属性 attribute
|
||||
# 设备名称 deviceName
|
||||
deviceName = "我的设备"
|
||||
|
||||
# 设备数据字典 Device Data Dictionary
|
||||
deviceData = {}
|
||||
|
||||
# 设备是否开启
|
||||
isOpen = False
|
||||
|
||||
# 临时数组 Temporary array
|
||||
TempBytes = []
|
||||
|
||||
# endregion
|
||||
|
||||
def __init__(self, deviceName, BLEDevice, callback_method):
|
||||
print("Initialize device model")
|
||||
# 设备名称（自定义） Device Name
|
||||
self.deviceName = deviceName
|
||||
self.BLEDevice = BLEDevice
|
||||
self.client = None
|
||||
self.writer_characteristic = None
|
||||
self.isOpen = False
|
||||
self.callback_method = callback_method
|
||||
self.deviceData = {}
|
||||
|
||||
# region 获取设备数据 Obtain device data
|
||||
# 设置设备数据 Set device data
|
||||
def set(self, key, value):
|
||||
# 将设备数据存到键值 Saving device data to key values
|
||||
self.deviceData[key] = value
|
||||
|
||||
# 获得设备数据 Obtain device data
|
||||
def get(self, key):
|
||||
# 从键值中获取数据，没有则返回None Obtaining data from key values
|
||||
if key in self.deviceData:
|
||||
return self.deviceData[key]
|
||||
else:
|
||||
return None
|
||||
|
||||
# 删除设备数据 Delete device data
|
||||
def remove(self, key):
|
||||
# 删除设备键值
|
||||
del self.deviceData[key]
|
||||
|
||||
# endregion
|
||||
|
||||
# 打开设备 open Device
|
||||
async def openDevice(self):
|
||||
print("Opening device......")
|
||||
# 获取设备的服务和特征 Obtain the services and characteristic of the device
|
||||
async with bleak.BleakClient(self.BLEDevice, timeout=15) as client:
|
||||
self.client = client
|
||||
self.isOpen = True
|
||||
# 设备UUID常量 Device UUID constant
|
||||
target_service_uuid = "0000ffe5-0000-1000-8000-00805f9a34fb"
|
||||
target_characteristic_uuid_read = "0000ffe4-0000-1000-8000-00805f9a34fb"
|
||||
target_characteristic_uuid_write = "0000ffe9-0000-1000-8000-00805f9a34fb"
|
||||
notify_characteristic = None
|
||||
|
||||
print("Matching services......")
|
||||
for service in client.services:
|
||||
if service.uuid == target_service_uuid:
|
||||
print(f"Service: {service}")
|
||||
print("Matching characteristic......")
|
||||
for characteristic in service.characteristics:
|
||||
if characteristic.uuid == target_characteristic_uuid_read:
|
||||
notify_characteristic = characteristic
|
||||
if characteristic.uuid == target_characteristic_uuid_write:
|
||||
self.writer_characteristic = characteristic
|
||||
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}")
|
||||
# 设置通知以接收数据 Set up notifications to receive data
|
||||
await client.start_notify(notify_characteristic.uuid, self.onDataReceived)
|
||||
|
||||
# 保持连接打开 Keep connected and open
|
||||
try:
|
||||
while self.isOpen:
|
||||
await asyncio.sleep(1)
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
finally:
|
||||
# 在退出时停止通知 Stop notification on exit
|
||||
await client.stop_notify(notify_characteristic.uuid)
|
||||
else:
|
||||
print("No matching services or characteristic found")
|
||||
|
||||
# 关闭设备 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)
|
||||
|
||||
# region 数据解析 data analysis
|
||||
# 串口数据处理 Serial port data processing
|
||||
def onDataReceived(self, sender, data):
|
||||
tempdata = bytes.fromhex(data.hex())
|
||||
for var in tempdata:
|
||||
self.TempBytes.append(var)
|
||||
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):
|
||||
del self.TempBytes[0]
|
||||
continue
|
||||
if len(self.TempBytes) == 20:
|
||||
self.processData(self.TempBytes)
|
||||
self.TempBytes.clear()
|
||||
|
||||
# 数据解析 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
|
||||
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
|
||||
def getSignInt16(num):
|
||||
if num >= pow(2, 15):
|
||||
num -= pow(2, 16)
|
||||
return num
|
||||
|
||||
# endregion
|
||||
|
||||
# 发送串口数据 Sending serial port data
|
||||
async def sendData(self, data):
|
||||
try:
|
||||
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)
|
||||
|
||||
# 读取寄存器 read register
|
||||
async def readReg(self, regAddr):
|
||||
# 封装读取指令并向串口发送数据 Encapsulate read instructions and send data to the serial port
|
||||
await self.sendData(self.get_readBytes(regAddr))
|
||||
|
||||
# 写入寄存器 Write Register
|
||||
async def writeReg(self, regAddr, sValue):
|
||||
# 解锁 unlock
|
||||
self.unlock()
|
||||
# 延迟100ms Delay 100ms
|
||||
time.sleep(0.1)
|
||||
# 封装写入指令并向串口发送数据
|
||||
await self.sendData(self.get_writeBytes(regAddr, sValue))
|
||||
# 延迟100ms Delay 100ms
|
||||
time.sleep(0.1)
|
||||
# 保存 save
|
||||
self.save()
|
||||
|
||||
# 读取指令封装 Read instruction encapsulation
|
||||
@staticmethod
|
||||
def get_readBytes(regAddr):
|
||||
# 初始化
|
||||
tempBytes = [None] * 5
|
||||
tempBytes[0] = 0xff
|
||||
tempBytes[1] = 0xaa
|
||||
tempBytes[2] = 0x27
|
||||
tempBytes[3] = regAddr
|
||||
tempBytes[4] = 0
|
||||
return tempBytes
|
||||
|
||||
# 写入指令封装 Write instruction encapsulation
|
||||
@staticmethod
|
||||
def get_writeBytes(regAddr, rValue):
|
||||
# 初始化
|
||||
tempBytes = [None] * 5
|
||||
tempBytes[0] = 0xff
|
||||
tempBytes[1] = 0xaa
|
||||
tempBytes[2] = regAddr
|
||||
tempBytes[3] = rValue & 0xff
|
||||
tempBytes[4] = rValue >> 8
|
||||
return tempBytes
|
||||
|
||||
# 解锁 unlock
|
||||
def unlock(self):
|
||||
cmd = self.get_writeBytes(0x69, 0xb588)
|
||||
self.sendData(cmd)
|
||||
|
||||
# 保存 save
|
||||
def save(self):
|
||||
cmd = self.get_writeBytes(0x00, 0x0000)
|
||||
self.sendData(cmd)
|
||||
@ -4,10 +4,8 @@
|
||||
IMU传感器管理器
|
||||
负责IMU传感器的连接、校准和头部姿态数据采集
|
||||
"""
|
||||
import serial
|
||||
import threading
|
||||
import time
|
||||
import numpy as np
|
||||
from typing import Optional, Dict, Any
|
||||
import logging
|
||||
from collections import deque
|
||||
@ -24,27 +22,29 @@ except ImportError:
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
class BleIMUDevice:
|
||||
"""蓝牙IMU设备，基于bleak实现，解析逻辑参考tests/testblueimu.py"""
|
||||
"""蓝牙IMU设备（WitMotion WT9011DCL-BT50），基于 device_model.py 官方接口"""
|
||||
def __init__(self, mac_address: str):
|
||||
self.mac_address = mac_address
|
||||
self.loop = None
|
||||
self.loop_thread = None
|
||||
self.client = None
|
||||
self.running = False
|
||||
self._lock = threading.Lock()
|
||||
self.disconnected_event = None
|
||||
self.calibration_data = None
|
||||
self.head_pose_offset = {'rotation': 0, 'tilt': 0, 'pitch': 0}
|
||||
self.last_data = {
|
||||
'roll': 0.0,
|
||||
'pitch': 0.0,
|
||||
'yaw': 0.0,
|
||||
'temperature': 25.0
|
||||
'temperature': 25.0,
|
||||
}
|
||||
self._connected = False
|
||||
# GATT特征（参考测试脚本中的handle/short uuid）
|
||||
self._notify_char = 0x0007
|
||||
self._write_char = 0x0005
|
||||
self._device_model = None
|
||||
self._open_task = 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
|
||||
|
||||
def set_calibration(self, calibration: Dict[str, Any]):
|
||||
self.calibration_data = calibration
|
||||
@ -97,6 +97,8 @@ class BleIMUDevice:
|
||||
asyncio.set_event_loop(self.loop)
|
||||
try:
|
||||
self.loop.run_until_complete(self._connect_and_listen())
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
except Exception as e:
|
||||
logger.error(f'BLE IMU事件循环异常: {e}', exc_info=True)
|
||||
finally:
|
||||
@ -107,21 +109,42 @@ class BleIMUDevice:
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
async def _disconnect(self):
|
||||
def _on_device_update(self, dm):
|
||||
try:
|
||||
if self.client and self.client.is_connected:
|
||||
try:
|
||||
await self.client.stop_notify(self._notify_char)
|
||||
except Exception:
|
||||
pass
|
||||
await self.client.disconnect()
|
||||
roll = dm.get("AngX")
|
||||
pitch = dm.get("AngY")
|
||||
yaw = dm.get("AngZ")
|
||||
if roll is None or pitch is None or yaw is None:
|
||||
return
|
||||
with self._lock:
|
||||
self.last_data['roll'] = float(roll)
|
||||
self.last_data['pitch'] = float(pitch)
|
||||
self.last_data['yaw'] = float(yaw)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
async def _disconnect(self):
|
||||
try:
|
||||
if self._device_model is not None:
|
||||
try:
|
||||
self._device_model.closeDevice()
|
||||
except Exception:
|
||||
pass
|
||||
if self._open_task is not None and not self._open_task.done():
|
||||
self._open_task.cancel()
|
||||
try:
|
||||
await self._open_task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
except Exception:
|
||||
pass
|
||||
finally:
|
||||
self._open_task = None
|
||||
self._device_model = None
|
||||
|
||||
async def _connect_and_listen(self):
|
||||
try:
|
||||
from bleak import BleakClient, BleakScanner
|
||||
from bleak.backends.characteristic import BleakGATTCharacteristic
|
||||
from bleak import BleakScanner
|
||||
except Exception as e:
|
||||
logger.error(f"未安装bleak或导入失败: {e}")
|
||||
self.running = False
|
||||
@ -129,142 +152,50 @@ class BleIMUDevice:
|
||||
|
||||
while self.running:
|
||||
try:
|
||||
# logger.info(f"扫描并连接蓝牙IMU: {self.mac_address} ...")
|
||||
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))
|
||||
|
||||
if device is None:
|
||||
# logger.warning(f"未找到设备: {self.mac_address}")
|
||||
await asyncio.sleep(2.0)
|
||||
continue
|
||||
|
||||
self.disconnected_event = asyncio.Event()
|
||||
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())
|
||||
|
||||
def _on_disconnected(_client):
|
||||
logger.info("BLE IMU已断开连接")
|
||||
connected = False
|
||||
for _ in range(50):
|
||||
if not self.running:
|
||||
break
|
||||
if self._open_task.done():
|
||||
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)
|
||||
|
||||
self._connected = connected
|
||||
if not connected:
|
||||
await self._disconnect()
|
||||
self._connected = False
|
||||
try:
|
||||
self.disconnected_event.set()
|
||||
except Exception:
|
||||
pass
|
||||
await asyncio.sleep(2.0)
|
||||
continue
|
||||
|
||||
self.client = BleakClient(device, disconnected_callback=_on_disconnected)
|
||||
logger.info("正在连接BLE IMU...")
|
||||
await self.client.connect()
|
||||
self._connected = True
|
||||
logger.info("BLE IMU连接成功")
|
||||
|
||||
# 订阅通知
|
||||
await self.client.start_notify(self._notify_char, self._notification_handler)
|
||||
|
||||
# 发送初始化指令（参考测试脚本）
|
||||
try:
|
||||
await self.client.write_gatt_char(self._write_char, bytes([0x29])) # 保持连接
|
||||
await asyncio.sleep(0.2)
|
||||
await self.client.write_gatt_char(self._write_char, bytes([0x46])) # 高速模式
|
||||
await asyncio.sleep(0.2)
|
||||
|
||||
isCompassOn = 0
|
||||
barometerFilter = 2
|
||||
Cmd_ReportTag = 0x0FFF
|
||||
params = bytearray([0x00 for _ in range(11)])
|
||||
params[0] = 0x12
|
||||
params[1] = 5
|
||||
params[2] = 255
|
||||
params[3] = 0
|
||||
params[4] = ((barometerFilter & 3) << 1) | (isCompassOn & 1)
|
||||
params[5] = 60 # 发送帧率
|
||||
params[6] = 1
|
||||
params[7] = 3
|
||||
params[8] = 5
|
||||
params[9] = Cmd_ReportTag & 0xff
|
||||
params[10] = (Cmd_ReportTag >> 8) & 0xff
|
||||
await self.client.write_gatt_char(self._write_char, params)
|
||||
await asyncio.sleep(0.2)
|
||||
|
||||
await self.client.write_gatt_char(self._write_char, bytes([0x19])) # 开始上报
|
||||
except Exception as e:
|
||||
logger.warning(f"BLE IMU写入初始化指令失败: {e}")
|
||||
|
||||
# 保持连接直到停止或断开
|
||||
while self.running and self.client and self.client.is_connected:
|
||||
while self.running and self._open_task is not None and not self._open_task.done():
|
||||
await asyncio.sleep(1.0)
|
||||
|
||||
# 退出前尝试停止通知
|
||||
try:
|
||||
await self.client.stop_notify(self._notify_char)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
await self.client.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
await self._disconnect()
|
||||
self._connected = False
|
||||
|
||||
except asyncio.CancelledError:
|
||||
break
|
||||
except Exception as e:
|
||||
logger.error(f"BLE IMU连接/监听失败: {e}", exc_info=True)
|
||||
self._connected = False
|
||||
await asyncio.sleep(2.0)
|
||||
|
||||
def _notification_handler(self, characteristic, data: bytearray):
|
||||
"""通知回调：解析IMU数据，更新欧拉角"""
|
||||
try:
|
||||
buf = data
|
||||
if len(buf) < 3:
|
||||
return
|
||||
if buf[0] != 0x11:
|
||||
return
|
||||
|
||||
# 比例系数
|
||||
scaleAngle = 0.0054931640625 # 180/32768
|
||||
scaleTemperature = 0.01
|
||||
|
||||
ctl = (buf[2] << 8) | buf[1]
|
||||
L = 7 # 数据偏移起点
|
||||
tmp_temperature = None
|
||||
|
||||
# 跳过前面不关心的标志位，关注角度与温度
|
||||
if (ctl & 0x0001) != 0:
|
||||
L += 6 # aX aY aZ
|
||||
if (ctl & 0x0002) != 0:
|
||||
L += 6 # AX AY AZ
|
||||
if (ctl & 0x0004) != 0:
|
||||
L += 6 # GX GY GZ
|
||||
if (ctl & 0x0008) != 0:
|
||||
L += 6 # CX CY CZ
|
||||
if (ctl & 0x0010) != 0:
|
||||
# 温度
|
||||
tmpX = np.short((np.short(buf[L+1]) << 8) | buf[L]) * scaleTemperature
|
||||
L += 2
|
||||
# 气压与高度各3字节
|
||||
# 气压
|
||||
tmpU32 = np.uint32(((np.uint32(buf[L+2]) << 16) | (np.uint32(buf[L+1]) << 8) | np.uint32(buf[L])))
|
||||
if (tmpU32 & 0x800000) == 0x800000:
|
||||
tmpU32 = (tmpU32 | 0xff000000)
|
||||
tmpY = np.int32(tmpU32) * 0.0002384185791
|
||||
L += 3
|
||||
# 高度
|
||||
tmpU32 = np.uint32(((np.uint32(buf[L+2]) << 16) | (np.uint32(buf[L+1]) << 8) | np.uint32(buf[L])))
|
||||
if (tmpU32 & 0x800000) == 0x800000:
|
||||
tmpU32 = (tmpU32 | 0xff000000)
|
||||
tmpZ = np.int32(tmpU32) * 0.0010728836
|
||||
L += 3
|
||||
tmp_temperature = float(tmpX)
|
||||
if (ctl & 0x0020) != 0:
|
||||
L += 8 # 四元数 wxyz
|
||||
if (ctl & 0x0040) != 0:
|
||||
angleX = float(np.short((np.short(buf[L+1]) << 8) | buf[L]) * scaleAngle); L += 2
|
||||
angleY = float(np.short((np.short(buf[L+1]) << 8) | buf[L]) * scaleAngle); L += 2
|
||||
angleZ = float(np.short((np.short(buf[L+1]) << 8) | buf[L]) * scaleAngle); L += 2
|
||||
with self._lock:
|
||||
# 映射：roll=X, pitch=Y, yaw=Z
|
||||
self.last_data['roll'] = round(angleX*-1, 1)
|
||||
self.last_data['pitch'] = round(angleY*-1, 1)
|
||||
self.last_data['yaw'] = round(angleZ*-1, 1)
|
||||
if tmp_temperature is not None:
|
||||
self.last_data['temperature'] = tmp_temperature
|
||||
except Exception:
|
||||
# 解析失败忽略
|
||||
pass
|
||||
|
||||
@property
|
||||
def connected(self) -> bool:
|
||||
return self._connected
|
||||
@ -386,9 +317,7 @@ class IMUManager(BaseDevice):
|
||||
self._socketio = socketio
|
||||
|
||||
# 设备配置
|
||||
self.port = config.get('port', 'COM7')
|
||||
self.baudrate = config.get('baudrate', 9600)
|
||||
self.device_type = config.get('device_type', 'ble') # 'real' | 'mock' | 'ble'
|
||||
self.use_mock = bool(config.get('use_mock', False))
|
||||
self.mac_address = config.get('mac_address', '')
|
||||
# IMU设备实例
|
||||
self.imu_device = None
|
||||
@ -409,7 +338,7 @@ class IMUManager(BaseDevice):
|
||||
self.data_buffer = deque(maxlen=100)
|
||||
self.last_valid_data = None
|
||||
|
||||
self.logger.info(f"IMU管理器初始化完成 - 端口: {self.port}, 设备类型: {self.device_type}, MAC: {self.mac_address}")
|
||||
self.logger.info(f"IMU管理器初始化完成 - use_mock: {self.use_mock}, MAC: {self.mac_address}")
|
||||
|
||||
def initialize(self) -> bool:
|
||||
"""
|
||||
@ -422,10 +351,10 @@ class IMUManager(BaseDevice):
|
||||
self.logger.info(f"正在初始化IMU设备...")
|
||||
|
||||
# 使用构造函数中已加载的配置，避免并发读取配置文件
|
||||
self.logger.info(f"使用已加载配置: port={self.port}, baudrate={self.baudrate}, device_type={self.device_type}, mac={self.mac_address}")
|
||||
self.logger.info(f"使用已加载配置: use_mock={self.use_mock}, mac={self.mac_address}")
|
||||
|
||||
# 根据配置选择设备类型
|
||||
if self.device_type == 'ble':
|
||||
if not self.use_mock:
|
||||
if not self.mac_address:
|
||||
self.logger.error("IMU BLE设备未配置MAC地址")
|
||||
self.is_connected = False
|
||||
@ -443,8 +372,6 @@ class IMUManager(BaseDevice):
|
||||
self.set_connected(True)
|
||||
|
||||
self._device_info.update({
|
||||
'port': self.port,
|
||||
'baudrate': self.baudrate,
|
||||
'mac_address': self.mac_address,
|
||||
})
|
||||
|
||||
@ -631,8 +558,6 @@ class IMUManager(BaseDevice):
|
||||
"""
|
||||
status = super().get_status()
|
||||
status.update({
|
||||
'port': self.port,
|
||||
'baudrate': self.baudrate,
|
||||
'is_streaming': self.imu_streaming,
|
||||
'is_calibrated': self.is_calibrated,
|
||||
'data_count': self.data_count,
|
||||
@ -640,7 +565,7 @@ class IMUManager(BaseDevice):
|
||||
'buffer_size': len(self.data_buffer),
|
||||
'has_data': self.last_valid_data is not None,
|
||||
'head_pose_offset': self.head_pose_offset,
|
||||
'device_type': self.device_type,
|
||||
'device_type': 'mock' if self.use_mock else 'ble',
|
||||
'mac_address': self.mac_address
|
||||
})
|
||||
return status
|
||||
@ -688,9 +613,7 @@ class IMUManager(BaseDevice):
|
||||
config = self.config_manager.get_device_config('imu')
|
||||
|
||||
# 更新配置属性
|
||||
self.port = config.get('port', 'COM7')
|
||||
self.baudrate = config.get('baudrate', 9600)
|
||||
self.device_type = config.get('device_type', 'mock')
|
||||
self.use_mock = bool(config.get('use_mock', False))
|
||||
self.mac_address = config.get('mac_address', '')
|
||||
|
||||
# 更新数据缓存队列大小
|
||||
@ -704,7 +627,7 @@ class IMUManager(BaseDevice):
|
||||
for data in current_data[-buffer_size:]:
|
||||
self.data_buffer.append(data)
|
||||
|
||||
self.logger.info(f"IMU配置重新加载成功 - 端口: {self.port}, 波特率: {self.baudrate}, 设备类型: {self.device_type}, MAC: {self.mac_address}")
|
||||
self.logger.info(f"IMU配置重新加载成功 - use_mock: {self.use_mock}, MAC: {self.mac_address}")
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
@ -719,37 +642,21 @@ class IMUManager(BaseDevice):
|
||||
if not self.imu_device:
|
||||
return False
|
||||
|
||||
# 检查设备类型并分别处理
|
||||
if isinstance(self.imu_device, RealIMUDevice):
|
||||
# 对于真实串口设备，检查串口连接状态
|
||||
if hasattr(self.imu_device, 'ser') and self.imu_device.ser:
|
||||
# 检查串口是否仍然打开
|
||||
if hasattr(self.imu_device, 'connected'):
|
||||
return bool(getattr(self.imu_device, 'connected'))
|
||||
|
||||
if hasattr(self.imu_device, 'ser') and getattr(self.imu_device, 'ser', None):
|
||||
if not self.imu_device.ser.is_open:
|
||||
return False
|
||||
|
||||
# 尝试读取数据来验证连接
|
||||
try:
|
||||
# 保存当前超时设置
|
||||
original_timeout = self.imu_device.ser.timeout
|
||||
self.imu_device.ser.timeout = 0.1 # 设置短超时
|
||||
|
||||
# 尝试读取少量数据
|
||||
test_data = self.imu_device.ser.read(1)
|
||||
|
||||
# 恢复原始超时设置
|
||||
self.imu_device.ser.timeout = 0.1
|
||||
self.imu_device.ser.read(1)
|
||||
self.imu_device.ser.timeout = original_timeout
|
||||
|
||||
return True # 如果没有异常，认为连接正常
|
||||
return True
|
||||
except Exception:
|
||||
return False
|
||||
else:
|
||||
return False
|
||||
|
||||
elif isinstance(self.imu_device, BleIMUDevice):
|
||||
# 对于蓝牙设备，检查连接状态
|
||||
return self.imu_device.connected
|
||||
|
||||
# 对于模拟设备或其他类型，总是返回True
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
|
||||
69
backend/devices/imu_test.py
Normal file
69
backend/devices/imu_test.py
Normal file
@ -0,0 +1,69 @@
|
||||
import asyncio
|
||||
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......")
|
||||
try:
|
||||
devices = await bleak.BleakScanner.discover(timeout=20.0)
|
||||
print("Search ended")
|
||||
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!")
|
||||
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)
|
||||
|
||||
|
||||
# 指定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())
|
||||
else:
|
||||
print("This BLEDevice was not found!!")
|
||||
@ -673,7 +673,7 @@ class PressureManager(BaseDevice):
|
||||
|
||||
# 设备实例
|
||||
self.device = None
|
||||
self.device_type = self.config.get('device_type', 'mock') # 'real' 或 'mock'
|
||||
self.use_mock = bool(self.config.get('use_mock', False))
|
||||
|
||||
# 数据流相关
|
||||
self.streaming_thread = None
|
||||
@ -695,7 +695,7 @@ class PressureManager(BaseDevice):
|
||||
self.read_fail_threshold = int(self.config.get('read_fail_threshold', 30))
|
||||
self._last_connected_state = None # 去抖动状态广播
|
||||
|
||||
self.logger.info(f"压力板管理器初始化完成 - 设备类型: {self.device_type}")
|
||||
self.logger.info(f"压力板管理器初始化完成 - use_mock: {self.use_mock}")
|
||||
|
||||
def initialize(self) -> bool:
|
||||
"""
|
||||
@ -708,10 +708,10 @@ class PressureManager(BaseDevice):
|
||||
self.logger.info(f"正在初始化压力板设备...")
|
||||
|
||||
# 使用构造函数中已加载的配置，避免并发读取配置文件
|
||||
self.logger.info(f"使用已加载配置: device_type={self.device_type}, stream_interval={self.stream_interval}")
|
||||
self.logger.info(f"使用已加载配置: use_mock={self.use_mock}, stream_interval={self.stream_interval}")
|
||||
|
||||
# 根据设备类型创建设备实例
|
||||
if self.device_type == 'real':
|
||||
if not self.use_mock:
|
||||
self.device = RealPressureDevice()
|
||||
else:
|
||||
self.device = MockPressureDevice()
|
||||
@ -719,11 +719,11 @@ class PressureManager(BaseDevice):
|
||||
# 使用set_connected方法启动连接监控线程
|
||||
self.set_connected(True)
|
||||
self._device_info.update({
|
||||
'device_type': self.device_type,
|
||||
'device_type': 'mock' if self.use_mock else 'real',
|
||||
'matrix_size': '4x4' if hasattr(self.device, 'rows') else 'unknown'
|
||||
})
|
||||
|
||||
self.logger.info(f"压力板初始化成功 - 设备类型: {self.device_type}")
|
||||
self.logger.info(f"压力板初始化成功 - use_mock: {self.use_mock}")
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
@ -890,7 +890,7 @@ class PressureManager(BaseDevice):
|
||||
Dict[str, Any]: 设备状态信息
|
||||
"""
|
||||
return {
|
||||
'device_type': self.device_type,
|
||||
'device_type': 'mock' if self.use_mock else 'real',
|
||||
'is_connected': self.is_connected,
|
||||
'is_streaming': self.is_streaming,
|
||||
'is_calibrated': self.is_calibrated,
|
||||
@ -966,14 +966,14 @@ class PressureManager(BaseDevice):
|
||||
|
||||
# 更新配置属性
|
||||
self.config = new_config
|
||||
self.device_type = new_config.get('device_type', 'mock')
|
||||
self.use_mock = bool(new_config.get('use_mock', False))
|
||||
self.stream_interval = new_config.get('stream_interval', 0.1)
|
||||
# 动态更新重连参数
|
||||
self.max_reconnect_attempts = int(new_config.get('max_reconnect_attempts', self.max_reconnect_attempts))
|
||||
self.reconnect_delay = float(new_config.get('reconnect_delay', self.reconnect_delay))
|
||||
self.read_fail_threshold = int(new_config.get('read_fail_threshold', self.read_fail_threshold))
|
||||
|
||||
self.logger.info(f"压力板配置重新加载成功 - 设备类型: {self.device_type}, 流间隔: {self.stream_interval}")
|
||||
self.logger.info(f"压力板配置重新加载成功 - use_mock: {self.use_mock}, 流间隔: {self.stream_interval}")
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
|
||||
@ -223,6 +223,7 @@ class RecordingManager:
|
||||
data = {
|
||||
'session_id': session_id,
|
||||
'head_pose': detection_data.get('head_pose'),
|
||||
'head_data_image': detection_data.get('head_data_image'),
|
||||
'screen_location': detection_data.get('screen_location'),
|
||||
'body_pose': None,
|
||||
'body_image': None,
|
||||
@ -268,10 +269,15 @@ class RecordingManager:
|
||||
|
||||
except Exception as e:
|
||||
self.logger.error(f'保存{field}失败: {e}')
|
||||
|
||||
# 完整屏幕截图--根据screen_location 进行截图
|
||||
screen_image = self._capture_screen_image(data_dir, data.get('screen_location'),'screen', timestamp=timestamp)
|
||||
if screen_image:
|
||||
data['screen_image'] = str(os.path.join( patient_id, session_id, f"image_{timestamp}", screen_image))
|
||||
# 头部数据屏幕截图——根据head_data_image 进行截图
|
||||
head_data_image = self._capture_screen_image(data_dir, data.get('head_data_image'),'head_data', timestamp=timestamp)
|
||||
if head_data_image:
|
||||
data['head_data_image'] = str(os.path.join( patient_id, session_id, f"image_{timestamp}", head_data_image))
|
||||
# 足部压力屏幕截图——根据foot_data_image 进行截图
|
||||
foot_data_image = self._capture_screen_image(data_dir, data.get('foot_data_image'),'foot_data', timestamp=timestamp)
|
||||
if foot_data_image:
|
||||
|
||||
@ -96,8 +96,9 @@ class ConfigManager:
|
||||
|
||||
# 默认设备配置
|
||||
self.config['DEVICES'] = {
|
||||
'imu_port': 'COM7',
|
||||
'imu_baudrate': '9600',
|
||||
'imu_enable': 'False',
|
||||
'imu_use_mock': 'False',
|
||||
'imu_mac_address': '',
|
||||
'pressure_port': 'COM8',
|
||||
'pressure_baudrate': '115200'
|
||||
}
|
||||
@ -233,12 +234,8 @@ class ConfigManager:
|
||||
"""
|
||||
return {
|
||||
'enable': self.config.getboolean('DEVICES', 'imu_enable', fallback=False),
|
||||
'device_type': self.config.get('DEVICES', 'imu_device_type', fallback='mock'),
|
||||
'port': self.config.get('DEVICES', 'imu_port', fallback='COM7'),
|
||||
'baudrate': self.config.getint('DEVICES', 'imu_baudrate', fallback=9600),
|
||||
'timeout': self.config.getfloat('DEVICES', 'imu_timeout', fallback=1.0),
|
||||
'calibration_samples': self.config.getint('DEVICES', 'imu_calibration_samples', fallback=100),
|
||||
'mac_address': self.config.get('DEVICES', 'imu_mac_address', fallback='ef:3c:1a:0a:fe:02'),
|
||||
'use_mock': self.config.getboolean('DEVICES', 'imu_use_mock', fallback=False),
|
||||
'mac_address': self.config.get('DEVICES', 'imu_mac_address', fallback='FA:E8:88:06:FE:F3'),
|
||||
}
|
||||
|
||||
def _get_pressure_config(self) -> Dict[str, Any]:
|
||||
@ -250,11 +247,9 @@ class ConfigManager:
|
||||
"""
|
||||
return {
|
||||
'enable': self.config.getboolean('DEVICES', 'pressure_enable', fallback=False),
|
||||
'device_type': self.config.get('DEVICES', 'pressure_device_type', fallback='mock'),
|
||||
'use_mock': self.config.getboolean('DEVICES', 'pressure_use_mock', fallback=False),
|
||||
'port': self.config.get('DEVICES', 'pressure_port', fallback='COM8'),
|
||||
'baudrate': self.config.getint('DEVICES', 'pressure_baudrate', fallback=115200),
|
||||
'timeout': self.config.getfloat('DEVICES', 'pressure_timeout', fallback=1.0),
|
||||
'calibration_samples': self.config.getint('DEVICES', 'pressure_calibration_samples', fallback=50)
|
||||
'baudrate': self.config.getint('DEVICES', 'pressure_baudrate', fallback=115200)
|
||||
}
|
||||
|
||||
def _get_remote_config(self) -> Dict[str, Any]:
|
||||
@ -384,8 +379,8 @@ class ConfigManager:
|
||||
# 验证设备配置
|
||||
try:
|
||||
imu_config = self.get_device_config('imu')
|
||||
if not imu_config.get('port'):
|
||||
warnings.append("IMU串口未配置")
|
||||
if imu_config.get('enable') and not imu_config.get('use_mock') and not imu_config.get('mac_address'):
|
||||
warnings.append("IMU未配置MAC地址")
|
||||
except Exception as e:
|
||||
errors.append(f"IMU配置验证失败: {e}")
|
||||
|
||||
@ -433,14 +428,10 @@ class ConfigManager:
|
||||
config_data = configs['imu']
|
||||
if 'enable' in config_data:
|
||||
self.set_config_value('DEVICES', 'imu_enable', str(config_data['enable']))
|
||||
if 'device_type' in config_data:
|
||||
self.set_config_value('DEVICES', 'imu_device_type', config_data['device_type'])
|
||||
if 'use_mock' in config_data:
|
||||
self.set_config_value('DEVICES', 'imu_use_mock', str(config_data['use_mock']))
|
||||
if 'port' in config_data:
|
||||
self.set_config_value('DEVICES', 'imu_port', config_data['port'])
|
||||
if 'baudrate' in config_data:
|
||||
self.set_config_value('DEVICES', 'imu_baudrate', str(config_data['baudrate']))
|
||||
if 'mac_address' in config_data:
|
||||
self.set_config_value('DEVICES', 'imu_mac_address', config_data['mac_address'])
|
||||
|
||||
results['imu'] = {
|
||||
'success': True,
|
||||
@ -460,8 +451,6 @@ class ConfigManager:
|
||||
config_data = configs['pressure']
|
||||
if 'enable' in config_data:
|
||||
self.set_config_value('DEVICES', 'pressure_enable', str(config_data['enable']))
|
||||
if 'device_type' in config_data:
|
||||
self.set_config_value('DEVICES', 'pressure_device_type', config_data['device_type'])
|
||||
if 'use_mock' in config_data:
|
||||
self.set_config_value('DEVICES', 'pressure_use_mock', str(config_data['use_mock']))
|
||||
if 'port' in config_data:
|
||||
@ -638,11 +627,12 @@ class ConfigManager:
|
||||
Args:
|
||||
configs: 所有设备配置数据
|
||||
{
|
||||
'imu': {'device_type': 'real', 'port': 'COM7', 'baudrate': 9600},
|
||||
'pressure': {'device_type': 'real', 'port': 'COM8', 'baudrate': 115200},
|
||||
'imu': {'enable': True, 'use_mock': False, 'mac_address': 'FA:E8:88:06:FE:F3'},
|
||||
'pressure': {'enable': True, 'device_type': 'real', 'use_mock': False, 'port': 'COM8', 'baudrate': 115200},
|
||||
'camera1': {'device_index': 0, 'width': 1280, 'height': 720, 'fps': 30},
|
||||
'camera2': {'device_index': 1, 'width': 1280, 'height': 720, 'fps': 30},
|
||||
'femtobolt': {'color_resolution': '1080P', 'depth_mode': 'NFOV_UNBINNED', 'fps': 15}
|
||||
'femtobolt': {'color_resolution': '1080P', 'depth_mode': 'NFOV_UNBINNED', 'fps': 15},
|
||||
'remote': {'enable': True, 'port': 'COM6', 'baudrate': 115200, 'timeout': 0.1, 'strict_crc': False}
|
||||
}
|
||||
|
||||
Returns:
|
||||
|
||||
@ -1125,7 +1125,6 @@ class AppServer:
|
||||
'occupation': data.get('occupation'),
|
||||
'workplace': data.get('workplace'),
|
||||
'idcode': data.get('idcode'),
|
||||
'medical_history': data.get('medical_history'),
|
||||
'notes': data.get('notes')
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user