281 lines
13 KiB
Python
281 lines
13 KiB
Python
import asyncio
|
||
from bleak import BleakClient, BleakScanner
|
||
from bleak.backends.characteristic import BleakGATTCharacteristic
|
||
from array import array
|
||
import numpy as np
|
||
|
||
#设备的Characteristic UUID
|
||
# par_notification_characteristic="0000ae02-0000-1000-8000-00805f9b34fb"
|
||
par_notification_characteristic=0x0007
|
||
#设备的Characteristic UUID（具备写属性Write）
|
||
# par_write_characteristic="0000ae01-0000-1000-8000-00805f9b34fb"
|
||
par_write_characteristic=0x0005
|
||
|
||
par_device_addr="ef:3c:1a:0a:fe:02" #设备的MAC地址 此处需要填入设备的mac地址
|
||
|
||
#准备发送的消息，为“hi world\n”的HEX形式（包括回车符0x0A 0x0D）
|
||
# send_str=bytearray([0x68,0x69,0x20,0x77,0x6F,0x72,0x6C,0x64,0x0A,0x0D])
|
||
|
||
#监听回调函数，此处为打印消息
|
||
def notification_handler(characteristic: BleakGATTCharacteristic, data: bytearray):
|
||
#print("rev data:",data)
|
||
parse_imu(data)
|
||
|
||
def parse_imu(buf):
|
||
scaleAccel = 0.00478515625 # 加速度 [-16g~+16g] 9.8*16/32768
|
||
scaleQuat = 0.000030517578125 # 四元数 [-1~+1] 1/32768
|
||
scaleAngle = 0.0054931640625 # 角度 [-180~+180] 180/32768
|
||
scaleAngleSpeed = 0.06103515625 # 角速度 [-2000~+2000] 2000/32768
|
||
scaleMag = 0.15106201171875 # 磁场 [-4950~+4950] 4950/32768
|
||
scaleTemperature = 0.01 # 温度
|
||
scaleAirPressure = 0.0002384185791 # 气压 [-2000~+2000] 2000/8388608
|
||
scaleHeight = 0.0010728836 # 高度 [-9000~+9000] 9000/8388608
|
||
|
||
imu_dat = array('f',[0.0 for i in range(0,34)])
|
||
|
||
if buf[0] == 0x11:
|
||
ctl = (buf[2] << 8) | buf[1]
|
||
# print("\n subscribe tag: 0x%04x"%ctl)
|
||
# print(" ms: ", ((buf[6]<<24) | (buf[5]<<16) | (buf[4]<<8) | (buf[3]<<0)))
|
||
|
||
L =7 # 从第7字节开始根据 订阅标识tag来解析剩下的数据
|
||
if ((ctl & 0x0001) != 0):
|
||
tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
|
||
# print("\taX: %.3f"%tmpX); # x加速度aX
|
||
tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
|
||
# print("\taY: %.3f"%tmpY); # y加速度aY
|
||
tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
|
||
# print("\taZ: %.3f"%tmpZ); # z加速度aZ
|
||
|
||
imu_dat[0] = float(tmpX)
|
||
imu_dat[1] = float(tmpY)
|
||
imu_dat[2] = float(tmpZ)
|
||
|
||
if ((ctl & 0x0002) != 0):
|
||
tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
|
||
# print("\tAX: %.3f"%tmpX) # x加速度AX
|
||
tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
|
||
# print("\tAY: %.3f"%tmpY) # y加速度AY
|
||
tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
|
||
# print("\tAZ: %.3f"%tmpZ) # z加速度AZ
|
||
|
||
imu_dat[3] = float(tmpX)
|
||
imu_dat[4] = float(tmpY)
|
||
imu_dat[5] = float(tmpZ)
|
||
|
||
if ((ctl & 0x0004) != 0):
|
||
tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAngleSpeed; L += 2
|
||
# print("\tGX: %.3f"%tmpX) # x角速度GX
|
||
tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAngleSpeed; L += 2
|
||
# print("\tGY: %.3f"%tmpY) # y角速度GY
|
||
tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAngleSpeed; L += 2
|
||
# print("\tGZ: %.3f"%tmpZ) # z角速度GZ
|
||
|
||
imu_dat[6] = float(tmpX)
|
||
imu_dat[7] = float(tmpY)
|
||
imu_dat[8] = float(tmpZ)
|
||
|
||
if ((ctl & 0x0008) != 0):
|
||
tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleMag; L += 2
|
||
# print("\tCX: %.3f"%tmpX); # x磁场CX
|
||
tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleMag; L += 2
|
||
# print("\tCY: %.3f"%tmpY); # y磁场CY
|
||
tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleMag; L += 2
|
||
# print("\tCZ: %.3f"%tmpZ); # z磁场CZ
|
||
|
||
imu_dat[9] = float(tmpX)
|
||
imu_dat[10] = float(tmpY)
|
||
imu_dat[11] = float(tmpZ)
|
||
|
||
if ((ctl & 0x0010) != 0):
|
||
tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleTemperature; L += 2
|
||
# print("\ttemperature: %.2f"%tmpX) # 温度
|
||
|
||
tmpU32 = np.uint32(((np.uint32(buf[L+2]) << 16) | (np.uint32(buf[L+1]) << 8) | np.uint32(buf[L])))
|
||
if ((tmpU32 & 0x800000) == 0x800000): # 若24位数的最高位为1则该数值为负数，需转为32位负数，直接补上ff即可
|
||
tmpU32 = (tmpU32 | 0xff000000)
|
||
tmpY = np.int32(tmpU32) * scaleAirPressure; L += 3
|
||
# print("\tairPressure: %.3f"%tmpY); # 气压
|
||
|
||
tmpU32 = np.uint32((np.uint32(buf[L+2]) << 16) | (np.uint32(buf[L+1]) << 8) | np.uint32(buf[L]))
|
||
if ((tmpU32 & 0x800000) == 0x800000): # 若24位数的最高位为1则该数值为负数，需转为32位负数，直接补上ff即可
|
||
tmpU32 = (tmpU32 | 0xff000000)
|
||
tmpZ = np.int32(tmpU32) * scaleHeight; L += 3
|
||
# print("\theight: %.3f"%tmpZ); # 高度
|
||
|
||
imu_dat[12] = float(tmpX)
|
||
imu_dat[13] = float(tmpY)
|
||
imu_dat[14] = float(tmpZ)
|
||
|
||
if ((ctl & 0x0020) != 0):
|
||
tmpAbs = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleQuat; L += 2
|
||
# print("\tw: %.3f"%tmpAbs); # w
|
||
tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleQuat; L += 2
|
||
# print("\tx: %.3f"%tmpX); # x
|
||
tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleQuat; L += 2
|
||
# print("\ty: %.3f"%tmpY); # y
|
||
tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleQuat; L += 2
|
||
# print("\tz: %.3f"%tmpZ); # z
|
||
|
||
imu_dat[15] = float(tmpAbs)
|
||
imu_dat[16] = float(tmpX)
|
||
imu_dat[17] = float(tmpY)
|
||
imu_dat[18] = float(tmpZ)
|
||
|
||
if ((ctl & 0x0040) != 0):
|
||
tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAngle; L += 2
|
||
# print("\tangleX: %.3f"%tmpX); # x角度
|
||
tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAngle; L += 2
|
||
# print("\tangleY: %.3f"%tmpY); # y角度
|
||
tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAngle; L += 2
|
||
# print("\tangleZ: %.3f"%tmpZ); # z角度
|
||
print(f"\tangleX: {tmpX:.3f}, angleY: {tmpY:.3f}, angleZ: {tmpZ:.3f}")
|
||
imu_dat[19] = float(tmpX)
|
||
imu_dat[20] = float(tmpY)
|
||
imu_dat[21] = float(tmpZ)
|
||
|
||
if ((ctl & 0x0080) != 0):
|
||
tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) / 1000.0; L += 2
|
||
# print("\toffsetX: %.3f"%tmpX); # x坐标
|
||
tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) / 1000.0; L += 2
|
||
# print("\toffsetY: %.3f"%tmpY); # y坐标
|
||
tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) / 1000.0; L += 2
|
||
# print("\toffsetZ: %.3f"%tmpZ); # z坐标
|
||
|
||
imu_dat[22] = float(tmpX)
|
||
imu_dat[23] = float(tmpY)
|
||
imu_dat[24] = float(tmpZ)
|
||
|
||
# if ((ctl & 0x0100) != 0):
|
||
# tmpU32 = ((buf[L+3]<<24) | (buf[L+2]<<16) | (buf[L+1]<<8) | (buf[L]<<0)); L += 4
|
||
# print("\tsteps: %u"%tmpU32); # 计步数
|
||
# tmpU8 = buf[L]; L += 1
|
||
# if (tmpU8 & 0x01):# 是否在走路
|
||
# print("\t walking yes")
|
||
# imu_dat[25] = 100
|
||
# else:
|
||
# print("\t walking no")
|
||
# imu_dat[25] = 0
|
||
# if (tmpU8 & 0x02):# 是否在跑步
|
||
# print("\t running yes")
|
||
# imu_dat[26] = 100
|
||
# else:
|
||
# print("\t running no")
|
||
# imu_dat[26] = 0
|
||
# if (tmpU8 & 0x04):# 是否在骑车
|
||
# print("\t biking yes")
|
||
# imu_dat[27] = 100
|
||
# else:
|
||
# print("\t biking no")
|
||
# imu_dat[27] = 0
|
||
# if (tmpU8 & 0x08):# 是否在开车
|
||
# print("\t driving yes")
|
||
# imu_dat[28] = 100
|
||
# else:
|
||
# print("\t driving no")
|
||
# imu_dat[28] = 0
|
||
|
||
if ((ctl & 0x0200) != 0):
|
||
tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
|
||
# print("\tasX: %.3f"%tmpX); # x加速度asX
|
||
tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
|
||
# print("\tasY: %.3f"%tmpY); # y加速度asY
|
||
tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
|
||
# print("\tasZ: %.3f"%tmpZ); # z加速度asZ
|
||
|
||
imu_dat[29] = float(tmpX)
|
||
imu_dat[30] = float(tmpY)
|
||
imu_dat[31] = float(tmpZ)
|
||
|
||
if ((ctl & 0x0400) != 0):
|
||
tmpU16 = ((buf[L+1]<<8) | (buf[L]<<0)); L += 2
|
||
# print("\tadc: %u"%tmpU16); # adc测量到的电压值，单位为mv
|
||
imu_dat[32] = float(tmpU16)
|
||
|
||
if ((ctl & 0x0800) != 0):
|
||
tmpU8 = buf[L]; L += 1
|
||
# print("\t GPIO1 M:%X, N:%X"%((tmpU8>>4)&0x0f, (tmpU8)&0x0f))
|
||
imu_dat[33] = float(tmpU8)
|
||
|
||
else:
|
||
print("[error] data head not define")
|
||
|
||
async def main():
|
||
print("starting scan...")
|
||
|
||
#基于MAC地址查找设备
|
||
device = await BleakScanner.find_device_by_address(
|
||
par_device_addr, cb=dict(use_bdaddr=False) #use_bdaddr判断是否是MOC系统
|
||
)
|
||
if device is None:
|
||
print("could not find device with address '%s'", par_device_addr)
|
||
return
|
||
|
||
#事件定义
|
||
disconnected_event = asyncio.Event()
|
||
|
||
#断开连接事件回调
|
||
def disconnected_callback(client):
|
||
print("Disconnected callback called!")
|
||
disconnected_event.set()
|
||
|
||
print("connecting to device...")
|
||
async with BleakClient(device,disconnected_callback=disconnected_callback) as client:
|
||
print("Connected")
|
||
await client.start_notify(par_notification_characteristic, notification_handler)
|
||
|
||
# 保持连接 0x29
|
||
wakestr=bytes([0x29])
|
||
await client.write_gatt_char(par_write_characteristic, wakestr)
|
||
await asyncio.sleep(0.2)
|
||
print("------------------------------------------------")
|
||
# 尝试采用蓝牙高速通信特性 0x46
|
||
fast=bytes([0x46])
|
||
await client.write_gatt_char(par_write_characteristic, fast)
|
||
await asyncio.sleep(0.2)
|
||
|
||
# GPIO 上拉
|
||
#upstr=bytes([0x27,0x10])
|
||
#await client.write_gatt_char(par_write_characteristic, upstr)
|
||
#await asyncio.sleep(0.2)
|
||
|
||
# 参数设置
|
||
isCompassOn = 0 #1=使用磁场融合姿态，0=不使用
|
||
barometerFilter = 2
|
||
Cmd_ReportTag = 0x0FFF # 功能订阅标识
|
||
params = bytearray([0x00 for i in range(0,11)])
|
||
params[0] = 0x12
|
||
params[1] = 5 #静止状态加速度阀值
|
||
params[2] = 255 #静止归零速度(单位cm/s) 0:不归零 255:立即归零
|
||
params[3] = 0 #动态归零速度(单位cm/s) 0:不归零
|
||
params[4] = ((barometerFilter&3)<<1) | (isCompassOn&1);
|
||
params[5] = 60 #数据主动上报的传输帧率[取值0-250HZ], 0表示0.5HZ
|
||
params[6] = 1 #陀螺仪滤波系数[取值0-2],数值越大越平稳但实时性越差
|
||
params[7] = 3 #加速计滤波系数[取值0-4],数值越大越平稳但实时性越差
|
||
params[8] = 5 #磁力计滤波系数[取值0-9],数值越大越平稳但实时性越差
|
||
params[9] = Cmd_ReportTag&0xff
|
||
params[10] = (Cmd_ReportTag>>8)&0xff
|
||
await client.write_gatt_char(par_write_characteristic, params)
|
||
await asyncio.sleep(0.2)
|
||
|
||
notes=bytes([0x19])
|
||
await client.write_gatt_char(par_write_characteristic, notes)
|
||
|
||
#await asyncio.sleep(2.0) #延迟一下等角度稳定后，再进行下一步的清零操作
|
||
#await client.write_gatt_char(par_write_characteristic, bytes([0x05])) # z轴角归零 0x05 有需要的用户可开启
|
||
#await asyncio.sleep(0.3)
|
||
#await client.write_gatt_char(par_write_characteristic, bytes([0x06])) # xyz坐标系清零 0x06 有需要的用户可开启
|
||
|
||
|
||
#await asyncio.sleep(0.2)
|
||
#await client.write_gatt_char(par_write_characteristic, bytes([0x51,0xAA,0xBB])) # 用总圈数代替欧拉角传输 并清零圈数 0x51
|
||
#await client.write_gatt_char(par_write_characteristic, bytes([0x51,0x00,0x00])) # 输出欧拉角 0x51
|
||
|
||
# 添加一个循环，使程序在接收数据时不会退出
|
||
while not disconnected_event.is_set():
|
||
await asyncio.sleep(1.0)
|
||
|
||
#await disconnected_event.wait() #休眠直到设备断开连接，有延迟。此处为监听设备直到断开为止
|
||
#await client.stop_notify(par_notification_characteristic)
|
||
|
||
asyncio.run(main()) |