轮趣N100 IMU(数据封装与解包)
有个IMU来着一直也没有使用，今天看见文档了，写个小程序试试看：
就是这样的
不过资料我在官网也找不到，只能在淘宝和店家要了一份。
这个是Arduino的接线
STM32+CAN通讯
输出的结果
现在市面的几种封装
系统原理框图
N100的参数
引脚分布
串口，SPI，CAN三种通讯方式
其实原理图看不看都没有什么用
原理图我放在GT上面了
这里就先分析一个简单的Arduino的实现：
一开始定义好我们需要的宏变量
两个数据包
IMU的包
AHRS的数据包
主要的逻辑很简单，就是读取和打印
这个不用解释什么
因为接收的数据，多种多样的，就需要分类的接收数据，串口一次只能发送一个八位的数据，要把发送的数据存到一个80的数组里面，根据指令和长度判断数据的类型，在多次确认数据有效的情况下，把数据分类放在不同的数组里面，IMU的是64，ARHS是56.
现在收到的数据是16进制的数据，一字节是8bit，需要将4个字节组合在一起，就是合并在一起（HEX），在转换成IEEE754里面的浮点数：
这样的
float HEX_to_Float(uint8_t data1, uint8_t data2, uint8_t data3, uint8_t data4)
{
// 数据由高位到地位排序 ：data1 data2 data3 data4
// 其中接收数据时，高位存放在后面，低位存放在前面
short sign; // 符号计算
unsigned long temp; // 32位16进制数
uint16_t zhishu; // 存放指数
float weishu; // 存放尾数
float ref; // 存放计算结果
uint16_t H_16, L_16; // 存放高16位、低16位
H_16 = data1 << 8 | data2; // 数据第一次融合
L_16 = data3 << 8 | data4;
// 将融合成16位的数据组合成32位数据
temp = (unsigned long)H_16 << 16 | (unsigned long)L_16;
// 浮点数转化开始
// 先确定符号
sign = (temp & 0x80000000) ? -1 : 1; // 最高位是1就是负数，0就是正数
// 计算指数
// 127是偏移量，使得指数有正负（指数的范围是 -127 ~ +128 ）
zhishu = ((temp >> 23) & 0xff) - 127;
// 获取尾数部分 将（temp & 0x7fffff）获得的值将小数点左移23位 (除以2的23次方)
weishu = 1 + ((float)(temp & 0x7fffff) / 0x800000);
// 最终的公式
ref = sign * weishu * pow(2, zhishu);
return ref;
}
这是选择头和尾部
把通过校验的数据打包的放在数组里面，使用的是memcpy函数
AHRS也是一样的
接下来就是数据的解包：
现在是从数组里面解数据，写的比较烂了哦
在解包之前还要确实要再校验一下数据的正确性
数据帧的打包格式
指令的类别
剩下的打包
我们这里看一下IMU的打包格式
把数据写里面
一期一会，记得把标志位抹掉
直接打印
如果写成STM32的也是可以的
再写一个Python解包的：
把要在解包协议的一些标志位写好
具体的这个我就不说了，不是那么重要的东西，反正就是让出口打开就好
开始读取里面的文件并且转换为16进制的数字
又是漫长校验，反正就是为了正确的解包
害，Python就是香
现成的解包函数
打印出来就好了：
import argparse
import serial # 导入模块
import serial.tools.list_ports
import threading
import struct
from serial import EIGHTBITS, PARITY_NONE, STOPBITS_ONE
# 宏定义参数
PI = 3.1415926
FRAME_HEAD = str('fc')
FRAME_END = str('fd')
TYPE_IMU = str('40')
TYPE_AHRS = str('41')
TYPE_INSGPS = str('42')
TYPE_GEODETIC_POS = str('5c')
TYPE_GROUND = str('f0')
IMU_LEN = str('38') # //56
AHRS_LEN = str('30') # //48
INSGPS_LEN = str('48') # //80
GEODETIC_POS_LEN = str('20') # //32
PI = 3.141592653589793
DEG_TO_RAD = 0.017453292519943295
# 获取命令行输入参数
def parse_opt(known=False):
parser = argparse.ArgumentParser()
# parser.add_argument('--debugs', type=bool, default=False, help='if debug info output in terminal ')
parser.add_argument('--port', type=str, default='com1',
help='the models serial port receive data')
parser.add_argument('--bps', type=int, default=921600,
help='the models baud rate set on wincc')
parser.add_argument('--timeout', type=int, default=20,
help='set the serial port timeout')
# parser.add_argument('--device_type', type=int, default=0, help='0: origin_data, 1: for single imu or ucar in ROS')
receive_params = parser.parse_known_args(
)[0] if known else parser.parse_args()
return receive_params
# 接收数据线程
def receive_data():
open_port()
# 尝试打开串口
try:
serial_ = serial.Serial(port=opt.port,
baudrate=opt.bps,
bytesize=EIGHTBITS,
parity=PARITY_NONE,
stopbits=STOPBITS_ONE,
timeout=opt.timeout)
print("baud rates = " + str(serial_.baudrate))
except:
print("error: unable to open port .")
exit(1)
# 循环读取数据
while serial_.isOpen() and tr.is_alive():
check_head = serial_.read().hex()
# 校验帧头
if check_head != FRAME_HEAD:
continue
head_type = serial_.read().hex()
# 校验数据类型
if (head_type != TYPE_IMU and
head_type != TYPE_AHRS and
head_type != TYPE_INSGPS and
head_type != TYPE_GEODETIC_POS and
head_type != 0x50 and head_type != TYPE_GROUND
):
continue
check_len = serial_.read().hex()
# 校验数据类型的长度
if head_type == TYPE_IMU and check_len != IMU_LEN:
continue
elif head_type == TYPE_AHRS and check_len != AHRS_LEN:
continue
elif head_type == TYPE_INSGPS and check_len != INSGPS_LEN:
continue
elif head_type == TYPE_GEODETIC_POS and check_len != GEODETIC_POS_LEN:
continue
elif head_type == TYPE_GROUND or head_type == 0x50:
continue
check_sn = serial_.read().hex()
head_crc8 = serial_.read().hex()
crc16_H_s = serial_.read().hex()
crc16_L_s = serial_.read().hex()
# 读取并解析IMU数据
if head_type == TYPE_IMU:
data_s = serial_.read(int(IMU_LEN, 16))
print("Gyroscope_X(rad/s): " +
str(struct.unpack('f', data_s[0:4])[0]))
print("Gyroscope_Y(rad/s) : " +
str(struct.unpack('f', data_s[4:8])[0]))
print("Gyroscope_Z(rad/s) : " +
str(struct.unpack('f', data_s[8:12])[0]))
print("Accelerometer_X(m/s^2) : " +
str(struct.unpack('f', data_s[12:16])[0]))
print("Accelerometer_Y(m/s^2) : " +
str(struct.unpack('f', data_s[16:20])[0]))
print("Accelerometer_Z(m/s^2) : " +
str(struct.unpack('f', data_s[20:24])[0]))
# 读取并解析AHRS数据
elif head_type == TYPE_AHRS:
data_s = serial_.read(int(AHRS_LEN, 16))
print("RollSpeed(rad/s): " +
str(struct.unpack('f', data_s[0:4])[0]))
print("PitchSpeed(rad/s) : " +
str(struct.unpack('f', data_s[4:8])[0]))
print("HeadingSpeed(rad) : " +
str(struct.unpack('f', data_s[8:12])[0]))
print("Roll(rad) : " + str(struct.unpack('f', data_s[12:16])[0]))
print("Pitch(rad) : " + str(struct.unpack('f', data_s[16:20])[0]))
print("Heading(rad) : " +
str(struct.unpack('f', data_s[20:24])[0]))
print("Q1 : " + str(struct.unpack('f', data_s[24:28])[0]))
print("Q2 : " + str(struct.unpack('f', data_s[28:32])[0]))
print("Q3 : " + str(struct.unpack('f', data_s[32:36])[0]))
print("Q4 : " + str(struct.unpack('f', data_s[36:40])[0]))
# print("Timestamp(us) : " + str(struct.unpack('ii', data_s[40:48])[0]))
# 读取并解析INSGPS数据
elif head_type == TYPE_INSGPS:
data_s = serial_.read(int(INSGPS_LEN, 16))
print("BodyVelocity_X:(m/s)" +
str(struct.unpack('f', data_s[0:4])[0]))
print("BodyVelocity_Y:(m/s)" +
str(struct.unpack('f', data_s[4:8])[0]))
print("BodyVelocity_Z:(m/s)" +
str(struct.unpack('f', data_s[8:12])[0]))
print("BodyAcceleration_X:(m/s^2)" +
str(struct.unpack('f', data_s[12:16])[0]))
print("BodyAcceleration_Y:(m/s^2)" +
str(struct.unpack('f', data_s[16:20])[0]))
print("BodyAcceleration_Z:(m/s^2)" +
str(struct.unpack('f', data_s[20:24])[0]))
print("Location_North:(m)" +
str(struct.unpack('f', data_s[24:28])[0]))
print("Location_East:(m)" +
str(struct.unpack('f', data_s[28:32])[0]))
print("Location_Down:(m)" +
str(struct.unpack('f', data_s[32:36])[0]))
print("Velocity_North:(m)" +
str(struct.unpack('f', data_s[36:40])[0]))
print("Velocity_East:(m/s)" +
str(struct.unpack('f', data_s[40:44])[0]))
print("Velocity_Down:(m/s)" +
str(struct.unpack('f', data_s[44:48])[0]))
elif head_type == TYPE_GEODETIC_POS:
data_s = serial_.read(int(GEODETIC_POS_LEN, 16))
print(" Latitude:(rad)" + str(struct.unpack('d', data_s[0:8])[0]))
print("Longitude:(rad)" + str(struct.unpack('d', data_s[8:16])[0]))
print("Height:(m)" + str(struct.unpack('d', data_s[16:24])[0]))
# 寻找输入的port串口
def find_serial():
port_list = list(serial.tools.list_ports.comports())
for port in port_list:
if port.name.lower() == opt.port.lower():
return True
return False
def open_port():
if find_serial():
print("find this port : " + opt.port)
else:
print("error: unable to find this port : " + opt.port)
exit(1)
if __name__ == "__main__":
opt = parse_opt()
tr = threading.Thread(target=receive_data)
tr.start()
1.安装依赖
pip install serial
pip install pyserial2.使用
python .\IMUtest.py -h 查看运行所需参数
optional arguments:
-h, --help show this help message and exit
--port PORT the models serial port receive data //IMU模块串口号 默认 com1
--bps BPS the models baud rate set on wincc //串口波特率 默认921600
--timeout TIMEOUT set the serial port timeout // 串口时延3.在命令行输入
python .\IMUtest.py --port=惯导串口 --bps=惯导设置波特率来运行程序
4. 常见错误：
error: unable to find this port : com4 没有找到设备端口
error: unable to open port . 找到端口，但是不能打开
感谢CY同学提供的瓦力一个~
还有来自TI家的毫米波雷达我也摸到了
http://bbs.wheeltec.net/forum.php?mod=viewthread&tid=3561&extra=page%3D2本文参与 腾讯云自媒体分享计划，分享自微信公众号。
原始发表：2022-10-27，如有侵权请联系 cloudcommunity@tencent.com 删除
评论
登录后参与评论
推荐阅读