247 lines
9.0 KiB
Python
247 lines
9.0 KiB
Python
|
# 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)
|