使用Python对IMU数据读取
时间：2022-08-26 02:00:00
使用Python对IMU数据读取
- IMU的连接方式
-
- 读取传感器数据
- 2字节16位补码的坑
- 代码
-
- 模块讲解
- 完整代码
最近在做设备时需要用电脑对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): # 将字符串转换为字符组格式，恢复每行 asd = [] i = 0 TLong = len(OData) while OData[i 1] is not None: asd.append(OData[i:i + 2])
i = i + 2
if i >= TLong:
break
return asd
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):
# 将字符串转换为字符组格式，并还原每一行
asd = []
i = 0
TLong = len(OData)
while OData[i + 1] is not None:
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):
# 检索角度数据并反馈数据流
for i in range(len(OData) - 12):
if OData[i:i + 3] == ["55", "55", "01"] and i + 11 <= len(OData):
Slop = OData[i + 4:i + 10]
for m in range(len(Slop)):
Slop[m] = byte16ToInt(0x00 + 16 * dic[Slop[m][0]] + dic[Slop[m][1]])
getSloop(setSloop(Slop))
OData[:i] = []
def setSloop(OData):
# 角度算法，数组有三个模块，分别为x,y,z方向的角度
return [round((byte16ToInt(OData[1]) << 8 | byte16ToInt(OData[0])) / 32768 * 180, 3),
round((byte16ToInt(OData[3]) << 8 | byte16ToInt(OData[2])) / 32768 * 180, 3),
round((byte16ToInt(OData[5]) << 8 | byte16ToInt(OData[4])) / 32768 * 180, 3)]
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("com3", 115200)
print("获取句柄成功，进入循环：")
count = 0
data = []
while True:
Fdata = str(binascii.b2a_hex(ser.readline()))
Fdata = Fdata[2:-1]
Fdata = setChars(Fdata)
data.extend(Fdata)
searchFF1(data)
# print(data)
count = count + 1
if count == 100:
break
