更新代码结构

imu_6dof_serial.py

@@ -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)