|
最近在做设备时需要用电脑对IMU的数据进行提取和分析，但发现目前该模块主要都是基于嵌入式给出的库函数，或者基于Linux系统的程序，作为初入大二的小白，发现目前在该方面的基于Windows系统和Python的解决方案几乎为空白，于是尝试弥补。
同时该.py程序移植至树莓派，测试良好
IMU的连接方式
我这里使用的是正点原子的ATK-IMU901型号，使用USB转串口进行数据传输。其中VCC接5V供电，GND接GND，RX接TX，TX接RX，接入电脑，可以使用串口调试助手进行端口号检测和IMU设置，具体的通信协议在用户手册里均有，大家可以去官网自行下载用户手册，且大部分IMU的通信协议是相同的，所以我觉得一通百通。
一路走过的坎坷
在这里，我就稍微对我目前发现的坑进行说明：
传感器数据的读取
传感器是以字节流的形式进行数据的传输，通常数据是以16进制2字节的数字进行传输，但通过Python中serial的数据读取，还要再经过binascii进行转换才能得到我们想要的数据。
注：我使用的该型号IMU的波特率为115200，不同传感器的波特率不同，请注意。
ser = serial.Serial("com5", 115200)
while True:
recv_data = ser.readline()
data = str(binascii.b2a_hex(ser.readline()))
2字节16位补码的坑
传感器回传的数据为2字节补码形式，注意，是补码！！
而我查遍了百度发现Python的解码形式都是默认为原码进行解码
例如：
int(“number”,16)
如果number为"0x56",那么万事大吉，如果第一位的数字大于7，该函数返回的仍然为正数，但以补码格式读取该数字为负数，而非正数
于是我自己写了一个补码转原码的函数才得以解决
def byte16ToInt(byte16):
# 补码转换为原码
if (byte16 & 0x80) == 0:
r = byte16
else:
byte16 = byte16 ^ 0xff
byte16 = byte16 + 1
r = -byte16
return r
代码
模块讲解
1.分割模块
由于通过函数*binascii.b2a_hex（）*得到的数据为16位无限长，所以通过该函数将数据以两字节长度进行分割：
def setCharS(OData, Ndata):
# 通过指针进行数据将字符串转换为字符组格式，并还原每一行
TLong = len(OData)
i = 0
while TRUE:
Ndata.append(OData[i:i + 2])
i = i + 2
if i >= TLong:
break
2.转码模块
将反码转换为原码，具体原因前文有提：
def byte16ToInt(byte16):
# 补码转换为原码
if (byte16 & 0x80) == 0:
r = byte16
else:
byte16 = byte16 ^ 0xff
byte16 = byte16 + 1
r = -byte16
return r
3.转位函数
IMU反馈的函数为16进制数字，需要转为10进制数据进行计算
def NumChange2(OData):
TLong = len(OData)
i = 0
asd = []
while TRUE:
a = 0x00 + 16 * dic[OData[i][0]] + dic[OData[i][1]]
b = byte16ToInt(a)
asd.append(b)
i = i + 1
if i == TLong:
break
return asd
完整代码
后续代码注释均有注解，就不一一讲解了。
该代码能成功读取IMU的x，y，z轴的角度、加速度、角加速度以及四元数，但系统还能返回磁力计和气压计数据，由于不需要，故没有添加相关的算法模块。
以下是程序的完整代码：
import serial
from tkinter import *
import binascii
import sys
dic = {
'0': 0, '1': 1, '2': 2,
'3': 3, '4': 4, '5': 5,
'6': 6, '7': 7, '8': 8,
'9': 9, 'a': 10, 'b': 11,
'c': 12, 'd': 13, 'e': 14,
'f': 15,
}
def setCharS(OData, Ndata):
# 通过指针进行数据将字符串转换为字符组格式，并还原每一行
TLong = len(OData)
i = 0
while TRUE:
Ndata.append(OData[i:i + 2])
i = i + 2
if i >= TLong:
break
def setChars(OData):
# 将字符串转换为字符组格式，并还原每一行
asd = []
i = 0
TLong = len(OData)
while TRUE:
asd.append(OData[i:i + 2])
i = i + 2
if i >= TLong:
break
return asd
def byte16ToInt(byte16):
# 补码转换为原码
if (byte16 & 0x80) == 0:
r = byte16
else:
byte16 = byte16 ^ 0xff
byte16 = byte16 + 1
r = -byte16
return r
def NumChange2(OData):
TLong = len(OData)
i = 0
asd = []
while TRUE:
a = 0x00 + 16 * dic[OData[i][0]] + dic[OData[i][1]]
b = byte16ToInt(a)
asd.append(b)
i = i + 1
if i == TLong:
break
return asd
def searchFF1(OData):
# 检索角度数据并反馈数据流
TLong = len(OData)
i = 0
asd = []
while TRUE:
if OData[i:i + 3] == ["55", "55", "01"] and i + 10 <= TLong:
asd = NumChange2(OData[i + 4:i + 10])
asd = setSloop(asd)
i = i + 1
if i >= TLong - 11:
break
return asd
def searchFF2(OData):
# 检测四元数
TLong = len(OData)
i = 0
asd = []
while TRUE:
bin(OData[i])
if OData[i:i + 3] == ["55", "55", "02"] and i + 13 <= TLong:
asd = NumChange2(OData[i + 4:i + 12])
asd = setFourNum(asd)
i = i + 1
if i >= TLong - 11:
break
return asd
def searchFF3(OData):
# 得到加速度
TLong = len(OData)
i = 0
asd = []
while TRUE:
if OData[i:i + 3] == ["55", "55", "03"] and i + 16 <= TLong:
asd = NumChange2(OData[i + 4:i + 16])
asd = setAccSpeed(asd)
i = i + 1
if i >= TLong - 17:
break
return asd
def setAccSpeed(OData):
# x，y，z方向的加速度和角加速度
return [round((OData[1] << 8 | OData[0]) / 32768 * 4, 3), round((OData[3] << 8 | OData[2]) / 32768 * 4, 3),
round((OData[5] << 8 | OData[4]) / 32768 * 4, 3), round((OData[7] << 8 | OData[6]) / 32768 * 2000, 3),
round((OData[9] << 8 | OData[8]) / 32768 * 2000, 3),
round((OData[11] << 8 | OData[10]) / 32768 * 2000, 3)]
def setFourNum(OData):
# 四元数算法
return [round((OData[1] << 8 | OData[0]) / 32768, 3), round((OData[3] << 8 | OData[2]) / 32768, 3),
round((OData[5] << 8 | OData[4]) / 32768, 3), round((OData[7] << 8 | OData[6]) / 32768, 3)]
def setSloop(OData):
# 角度算法，数组有三个模块，分别为x,y,z方向的角度
return [round((OData[1] << 8 | OData[0]) / 32768 * 180, 3), round((OData[3] << 8 | OData[2]) / 32768 * 180, 3),
round((OData[5] << 8 | OData[4]) / 32768 * 180, 3)]
def getAccSpeed(OData):
# 得到加速度
if OData:
sys.stdout.write(
"X轴加速度为：" + str(OData[0]) + "\t" + "Y轴加速度为：" + str(OData[1]) + "\t" + "Z轴加速度为：" + str(OData[2]) + "\n")
def getAccSloop(OData):
if OData:
sys.stdout.write(
"X轴角加速度为：" + str(OData[3]) + " " + "y轴角加速度为：" + str(OData[4]) + " " + "z轴角加速度为：" + str(OData[5]) + "\n")
def getSloop(OData):
# 得到角度度数
if OData:
sys.stdout.write(
"X轴的角度为：" + str(OData[0]) + "\t" + "Y轴的角度为：" + str(OData[1]) + "\t" + "Z轴的角度为：" + str(OData[2]) + "\n")
if __name__ == '__main__':
print("开始测试：")
ser = serial.Serial("com5", 115200)
print("获取句柄成功，进入循环：")
count = 0
Ndata = []
while True:
recv_data = ser.readline()
data = str(binascii.b2a_hex(ser.readline())) # 该代码耗时长，尽可能考虑优化
data = data[2:-1]
# print(data)
data = setChars(data)
SpeedQwe = searchFF1(data)
AccSloopQwe = searchFF3(data)
getAccSloop(AccSloopQwe)
count = count + 1
if count == 100:
break
|