# coding:UTF-8 import time import bleak import asyncio import logging # 设备实例 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): self.logger = logging.getLogger("device.imu.witmotion") self.logger.info("初始化IMU设备模型") # 设备名称（自定义） 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 = {} self._battery_ts = 0.0 @staticmethod def _battery_percent_from_reg(reg_value: int) -> int: try: v = int(reg_value) except Exception: return 0 if v > 396: return 100 if v >= 393: return 90 if v >= 387: return 75 if v >= 382: return 60 if v >= 379: return 50 if v >= 377: return 40 if v >= 373: return 30 if v >= 370: return 20 if v >= 368: return 15 if v >= 350: return 10 if v >= 340: return 5 return 0 # 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): 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" target_characteristic_uuid_read = "0000ffe4-0000-1000-8000-00805f9a34fb" target_characteristic_uuid_write = "0000ffe9-0000-1000-8000-00805f9a34fb" notify_characteristic = None self.logger.info("正在匹配服务...") await asyncio.sleep(0.3) services = [] for i in range(10): tmp_services = None get_services = getattr(client, 'get_services', None) if callable(get_services): try: tmp_services = await get_services() except Exception: tmp_services = None if not tmp_services: backend = getattr(client, "_backend", None) backend_get_services = getattr(backend, "get_services", None) if callable(backend_get_services): try: tmp_services = await backend_get_services() except Exception: tmp_services = None if not tmp_services: tmp_services = getattr(client, 'services', None) try: services = list(tmp_services) if tmp_services else [] except Exception: services = [] for service in services: try: svc_uuid = str(getattr(service, "uuid", "") or "").lower() except Exception: svc_uuid = "" if svc_uuid == str(target_service_uuid).lower(): self.logger.info(f"匹配到服务: {service}") self.logger.info("正在匹配特征...") chars = [] try: chars = list(getattr(service, "characteristics", None) or []) except Exception: chars = [] for characteristic in chars: try: chr_uuid = str(getattr(characteristic, "uuid", "") or "").lower() except Exception: chr_uuid = "" if chr_uuid == str(target_characteristic_uuid_read).lower(): notify_characteristic = characteristic if chr_uuid == str(target_characteristic_uuid_write).lower(): self.writer_characteristic = characteristic if notify_characteristic: break if notify_characteristic: break await asyncio.sleep(0.2 + i * 0.1) if 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: while self.isOpen: try: if not bool(getattr(client, "is_connected", False)): self.isOpen = False break except Exception: self.isOpen = False break await asyncio.sleep(1) except asyncio.CancelledError: pass finally: # 在退出时停止通知 Stop notification on exit try: await client.stop_notify(notify_characteristic.uuid) except Exception: pass else: try: svc_list = [] for s in services: try: svc_list.append(str(getattr(s, "uuid", "") or "")) except Exception: pass self.logger.warning(f"未找到匹配的服务或特征，可用服务: {svc_list}") except Exception: self.logger.warning("未找到匹配的服务或特征") raise RuntimeError("未找到匹配的服务或特征") # 关闭设备 close Device def closeDevice(self): self.isOpen = False self.logger.info("设备已关闭") # 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) == 20: self.processData(self.TempBytes) self.TempBytes.clear() # 数据解析 data analysis def processData(self, Bytes): if Bytes[1] == 0x61: 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("AngX", round(AngX, 3)) self.set("AngY", round(AngY, 3)) self.set("AngZ", round(AngZ, 3)) self.callback_method(self) return if Bytes[1] == 0x71: start_reg = int(Bytes[2]) regs = [] for i in range(8): base = 4 + i * 2 if base + 1 >= len(Bytes): break regs.append(int(Bytes[base]) | (int(Bytes[base + 1]) << 8)) for idx, val in enumerate(regs): self.set(f"Reg_{start_reg + idx:02X}", val) if regs and start_reg == 0x64: raw = int(regs[0]) self.set("BatteryRaw", raw) self.set("BatteryVoltage", round(raw / 100.0, 2)) self.set("BatteryPercent", int(self._battery_percent_from_reg(raw))) self._battery_ts = time.time() self.set("BatteryTS", self._battery_ts) self.callback_method(self) return # 获得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: self.logger.warning(f"发送数据失败: {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)) async def readBattery(self, timeout: float = 2.0): if not bool(self.isOpen): return None client = getattr(self, "client", None) if client is None or not bool(getattr(client, "is_connected", False)): return None prev_ts = float(self.deviceData.get("BatteryTS", 0.0) or 0.0) await self.readReg(0x64) deadline = time.time() + max(0.1, float(timeout)) while time.time() < deadline: cur_ts = float(self.deviceData.get("BatteryTS", 0.0) or 0.0) if cur_ts > prev_ts: return { "raw": self.deviceData.get("BatteryRaw"), "voltage": self.deviceData.get("BatteryVoltage"), "percent": self.deviceData.get("BatteryPercent"), } await asyncio.sleep(0.05) return None # 写入寄存器 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)