BodyBalanceEvaluation/backend/devices/device_model.py

# 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)