|
@@ -4,6 +4,10 @@
|
|
|
# Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
|
|
|
# Description: 从串口中读取6DOF IMU模块的三轴加速度、角度、四元数,
|
|
|
# 然后将其打印输出.这里需要注意IMU模块使用的坐标系为ENU东北天坐标系.
|
|
|
+# imu模块的串口波特率是固定的57600bps,对于树莓派的串口这里使用的是ttyAMA0,
|
|
|
+# 就是排针处的硬件串口,数据通信更为稳定.要想使用该串口需要交换树莓派默认
|
|
|
+# 的ttyS0和ttyAMA0,使ttyAMA0为主串口,这里需要先在rasp-config中关闭串口
|
|
|
+# 登录功能,启用串口才可以.
|
|
|
# Author: corvin
|
|
|
# History:
|
|
|
# 20220525:Initial this file.
|
|
@@ -11,48 +15,51 @@
|
|
|
import serial
|
|
|
|
|
|
|
|
|
-ACCData=[0.0]*8
|
|
|
-GYROData=[0.0]*8
|
|
|
-AngleData=[0.0]*8
|
|
|
-FrameState = 0 #通过0x后面的值判断属于哪一种情况
|
|
|
-Bytenum = 0 #读取到这一段的第几位
|
|
|
-CheckSum = 0 #求和校验位
|
|
|
+ACCData = [0.0]*8
|
|
|
+GYROData = [0.0]*8
|
|
|
+AngleData = [0.0]*8
|
|
|
|
|
|
-a = [0.0]*3
|
|
|
-w = [0.0]*3
|
|
|
-Angle = [0.0]*3
|
|
|
+FrameState = 0 #保存当前数据帧类型
|
|
|
+Bytenum = 0 #保存读取到数据帧的第几个字节
|
|
|
+CheckSum = 0 #数据帧求和校验位
|
|
|
+
|
|
|
+acc_data = [0.0]*3
|
|
|
+angular_data = [0.0]*3
|
|
|
+rpy_data = [0.0]*3
|
|
|
|
|
|
# 使用树莓派串口与IMU板进行通信
|
|
|
serial_port="/dev/ttyAMA0"
|
|
|
|
|
|
|
|
|
-def DueData(inputdata): #对数据进行划分,各自读到对应的数组里
|
|
|
- global FrameState #在局部修改全局变量,要进行global的定义
|
|
|
+#对数据进行划分,各自读到对应的数组里
|
|
|
+def DueData(inputdata):
|
|
|
+ #在局部修改全局变量,要进行global的定义
|
|
|
+ global FrameState
|
|
|
global Bytenum
|
|
|
global CheckSum
|
|
|
- global a
|
|
|
- global w
|
|
|
- global Angle
|
|
|
+ global acc_data
|
|
|
+ global angular_data
|
|
|
+ global rpy_data
|
|
|
|
|
|
for data in inputdata: #在输入的数据进行遍历
|
|
|
- if FrameState==0: #当未确定状态的时候,进入以下判断
|
|
|
- if data==0x55 and Bytenum==0: #0x55位于第一位时候,开始读取数据,增大bytenum
|
|
|
- CheckSum=data
|
|
|
- Bytenum=1
|
|
|
+ if FrameState == 0: #未读取到数据帧头
|
|
|
+ if data == 0x55 and Bytenum == 0: #读取到数据帧头,增大bytenum
|
|
|
+ CheckSum = data
|
|
|
+ Bytenum = 1
|
|
|
continue
|
|
|
- elif data==0x51 and Bytenum==1: #在byte不为0 且 识别到 0x51 的时候,改变frame
|
|
|
- CheckSum+=data
|
|
|
- FrameState=1
|
|
|
- Bytenum=2
|
|
|
- elif data==0x52 and Bytenum==1: #同理
|
|
|
- CheckSum+=data
|
|
|
- FrameState=2
|
|
|
- Bytenum=2
|
|
|
+ elif data == 0x51 and Bytenum == 1: #在byte不为0 且识别到 0x51的时候,改变frame
|
|
|
+ CheckSum += data
|
|
|
+ FrameState = 1
|
|
|
+ Bytenum = 2
|
|
|
+ elif data == 0x52 and Bytenum == 1:
|
|
|
+ CheckSum += data
|
|
|
+ FrameState = 2
|
|
|
+ Bytenum = 2
|
|
|
elif data==0x53 and Bytenum==1:
|
|
|
CheckSum+=data
|
|
|
FrameState=3
|
|
|
Bytenum=2
|
|
|
- elif FrameState==1: # acc #已确定数据代表加速度
|
|
|
+ elif FrameState == 1: # acc #已确定数据代表加速度
|
|
|
if Bytenum<10: # 读取8个数据
|
|
|
ACCData[Bytenum-2]=data # 从0开始
|
|
|
CheckSum+=data
|
|
@@ -61,9 +68,9 @@ def DueData(inputdata): #对数据进行划分,各自读到对应的数组里
|
|
|
if data == (CheckSum&0xff): #假如校验位正确
|
|
|
a = get_acc(ACCData)
|
|
|
print("Acc:", a)
|
|
|
- CheckSum=0 #各数据归零,进行新的循环判断
|
|
|
- Bytenum=0
|
|
|
- FrameState=0
|
|
|
+ CheckSum = 0 #各数据归零,进行新的循环判断
|
|
|
+ Bytenum = 0
|
|
|
+ FrameState = 0
|
|
|
elif FrameState==2: # gyro
|
|
|
if Bytenum<10:
|
|
|
GYROData[Bytenum-2]=data
|
|
@@ -166,9 +173,11 @@ def get_angle(datahex):
|
|
|
|
|
|
|
|
|
if __name__=='__main__':
|
|
|
- ser = serial.Serial(serial_port, 57600, timeout=0.5)
|
|
|
- print(ser.is_open)
|
|
|
+ # 开启树莓派串口开始获取数据,这里波特率是固定的57600bps
|
|
|
+ mySerial = serial.Serial(serial_port, 57600, timeout=0.5)
|
|
|
+ print("Now IMU module is working...")
|
|
|
|
|
|
- while(1):
|
|
|
- datahex = ser.read(33)
|
|
|
+ while True:
|
|
|
+ # 开始从串口读取44字节数据,共计4帧有效数据进行处理
|
|
|
+ datahex = mySerial.read(44)
|
|
|
DueData(datahex)
|