初步调试好串口处理数据

README.md

@@ -1,9 +1,9 @@
 #### 下载代码
 ```
-git clone -b linux-python3-iic-devel https://code.corvin.cn:3000/corvin_zhang/rasp_imu_hat_6dof.git
+git clone -b linux-python3-serial-devel https://code.corvin.cn:3000/corvin_zhang/rasp_imu_hat_6dof.git
 ```
 
 #### 运行
 ```
-python3 imu_6dof_iic.py
+python3 imu_6dof_serial.py
 ```

imu_6dof_iic.py

@@ -1,51 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: UTF-8 -*-
-
-# Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
-# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数,
-#   然后将其打印输出.这里需要注意IMU模块使用的坐标系为ENU东北天坐标系.
-# Author: corvin
-# History:
-#    20220523:Initial this file.
-
-import math,time
-from imu_data_proc import MyIMUClass
-
-
-# 默认IMU模块的iic地址为0x50
-imu_iic_addr = 0x50
-
-# 将度数转换为弧度
-deg2rad = math.pi/180.0
-acc_factor = 9.806  #sensor acc g convert to m/s^2.
-
-
-if __name__=='__main__':
-    myIMU = MyIMUClass(imu_iic_addr)
-    print("Now 6DOF IMU Module is working ...\n")
-
-    while True:
-        myIMU.get_YPRAG()
-
-        yaw_deg = myIMU.raw_yaw
-        if yaw_deg >= 180.0:
-            yaw_deg -= 360.0
-
-        yaw_data   = yaw_deg*deg2rad
-        pitch_data = myIMU.raw_pitch*deg2rad
-        roll_data  = myIMU.raw_roll*deg2rad
-        print("roll:",roll_data, " pitch:",pitch_data, " yaw:",yaw_data)
-
-        # 读取四元数,将其打印输出
-        myIMU.get_quatern()
-        print("quat_x:",myIMU.raw_q1, " quat_y:",myIMU.raw_q2, " quat_z:",myIMU.raw_q3, " quat_w:",myIMU.raw_q0)
-        
-        # 打印输出三轴角速度,弧度值
-        print("angular_x:",myIMU.raw_gx*deg2rad, " angular_y:",myIMU.raw_gy*deg2rad, " angular_z:",myIMU.raw_gz*deg2rad)
-
-        # 打印输出三轴线加速度
-        print("line_acc_x:",myIMU.raw_ax*acc_factor, " line_acc_y:",myIMU.raw_ay*acc_factor, " line_acc_z:",myIMU.raw_az*acc_factor)
-
-        print("------------------------------------------------------------------------------------\n")
-        time.sleep(0.1)
-

imu_6dof_serial.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+# -*- coding: UTF-8 -*-
+
+# Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
+# Description: 从串口中读取6DOF IMU模块的三轴加速度、角度、四元数,
+#   然后将其打印输出.这里需要注意IMU模块使用的坐标系为ENU东北天坐标系.
+# Author: corvin
+# History:
+#    20220525:Initial this file.
+
+import serial 
+
+
+ACCData=[0.0]*8
+GYROData=[0.0]*8
+AngleData=[0.0]*8
+FrameState = 0     #通过0x后面的值判断属于哪一种情况
+Bytenum = 0        #读取到这一段的第几位
+CheckSum = 0       #求和校验位
+
+a = [0.0]*3
+w = [0.0]*3
+Angle = [0.0]*3
+
+# 使用树莓派串口与IMU板进行通信
+serial_port="/dev/ttyAMA0"
+
+
+def DueData(inputdata):   #对数据进行划分,各自读到对应的数组里
+    global  FrameState    #在局部修改全局变量，要进行global的定义
+    global  Bytenum
+    global  CheckSum
+    global  a
+    global  w
+    global  Angle
+
+    for data in inputdata:  #在输入的数据进行遍历
+        if FrameState==0:   #当未确定状态的时候，进入以下判断
+            if data==0x55 and Bytenum==0: #0x55位于第一位时候，开始读取数据，增大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==0x53 and Bytenum==1:
+                CheckSum+=data
+                FrameState=3
+                Bytenum=2
+        elif FrameState==1: # acc    #已确定数据代表加速度
+            if Bytenum<10:            # 读取8个数据
+                ACCData[Bytenum-2]=data # 从0开始
+                CheckSum+=data
+                Bytenum+=1
+            else:
+                if data == (CheckSum&0xff):  #假如校验位正确
+                    a = get_acc(ACCData)
+                    print("Acc:", a)
+                CheckSum=0                  #各数据归零，进行新的循环判断
+                Bytenum=0
+                FrameState=0
+        elif FrameState==2: # gyro
+            if Bytenum<10:
+                GYROData[Bytenum-2]=data
+                CheckSum+=data
+                Bytenum+=1
+            else:
+                if data == (CheckSum&0xff):
+                    w = get_gyro(GYROData)
+                    print("Angular:", w)
+                CheckSum=0
+                Bytenum=0
+                FrameState=0
+        elif FrameState==3: # angle
+            if Bytenum<10:
+                AngleData[Bytenum-2]=data
+                CheckSum+=data
+                Bytenum+=1
+            else:
+                if data == (CheckSum&0xff):
+                    Angle = get_angle(AngleData)
+                    print("Angle(deg):", Angle)
+                CheckSum=0
+                Bytenum=0
+                FrameState=0
+
+
+def get_acc(datahex):
+    axl = datahex[0]
+    axh = datahex[1]
+    ayl = datahex[2]
+    ayh = datahex[3]
+    azl = datahex[4]
+    azh = datahex[5]
+    
+    k_acc = 16.0
+    acc_x = (axh << 8 | axl) / 32768.0 * k_acc
+    acc_y = (ayh << 8 | ayl) / 32768.0 * k_acc
+    acc_z = (azh << 8 | azl) / 32768.0 * k_acc
+
+    if acc_x >= k_acc:
+        acc_x -= 2 * k_acc
+
+    if acc_y >= k_acc:
+        acc_y -= 2 * k_acc
+
+    if acc_z >= k_acc:
+        acc_z-= 2 * k_acc
+
+    return acc_x,acc_y,acc_z
+
+
+def get_gyro(datahex):
+    wxl = datahex[0]
+    wxh = datahex[1]
+    wyl = datahex[2]
+    wyh = datahex[3]
+    wzl = datahex[4]
+    wzh = datahex[5]
+
+    k_gyro = 2000.0
+    gyro_x = (wxh << 8 | wxl) / 32768.0 * k_gyro
+    gyro_y = (wyh << 8 | wyl) / 32768.0 * k_gyro
+    gyro_z = (wzh << 8 | wzl) / 32768.0 * k_gyro
+
+    if gyro_x >= k_gyro:
+        gyro_x -= 2 * k_gyro
+
+    if gyro_y >= k_gyro:
+        gyro_y -= 2 * k_gyro
+
+    if gyro_z >=k_gyro:
+        gyro_z-= 2 * k_gyro
+
+    return gyro_x,gyro_y,gyro_z
+
+
+def get_angle(datahex):
+    rxl = datahex[0]
+    rxh = datahex[1]
+    ryl = datahex[2]
+    ryh = datahex[3]
+    rzl = datahex[4]
+    rzh = datahex[5]
+
+    k_angle = 180.0
+    angle_x = (rxh << 8 | rxl) / 32768.0 * k_angle
+    angle_y = (ryh << 8 | ryl) / 32768.0 * k_angle
+    angle_z = (rzh << 8 | rzl) / 32768.0 * k_angle
+
+    if angle_x >= k_angle:
+        angle_x -= 2 * k_angle
+
+    if angle_y >= k_angle:
+        angle_y -= 2 * k_angle
+
+    if angle_z >=k_angle:
+        angle_z-= 2 * k_angle
+
+    return angle_x,angle_y,angle_z
+
+
+if __name__=='__main__':
+    ser = serial.Serial(serial_port, 57600, timeout=0.5)  
+    print(ser.is_open)
+
+    while(1):
+        datahex = ser.read(33)
+        DueData(datahex)

imu_data_proc.py

@@ -1,61 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: UTF-8 -*-
-
-# Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
-# Description: 使用python3从IIC接口中读取IMU模块的三轴线加速度,三轴角速度,四元数原始数据。
-# Author: corvin
-# History:
-#    20220523: Initial this file.
-
-import smbus
-import numpy as np
-
-
-class MyIMUClass(object):
-    def __init__(self, dev_iic_addr):
-        self.i2c  = smbus.SMBus(1)
-        self.addr = dev_iic_addr
-
-        # 三个数据常量,依次分别代表180.0/32768, 16.0/32768.0, 2000.0/32768.0
-        self.RPY_CONST = 0.005493164
-        self.ACC_CONST = 0.000488281
-        self.ANG_CONST = 0.061035156
-
-
-    # 读取roll,pitch,yaw数据,三轴角速度,三轴线加速度数据
-    def get_YPRAG(self):
-        try:
-            # 读取三轴角度,共计6个字节
-            rpy_data = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
-            # 读取三轴加速度,共计6个字节
-            acc_data = self.i2c.read_i2c_block_data(self.addr, 0x34, 6)
-            # 读取三轴角速度,共计6个字节
-            ang_data = self.i2c.read_i2c_block_data(self.addr, 0x37, 6)
-        except IOError:
-            print("Read IMU RPYAG date error !")
-        else:
-            self.raw_roll  = float(np.short(np.short(rpy_data[1]<<8)|rpy_data[0])*self.RPY_CONST)
-            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])*self.RPY_CONST)
-            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])*self.RPY_CONST)
-
-            self.raw_ax = float(np.short(np.short(acc_data[1]<<8)|acc_data[0])*self.ACC_CONST)
-            self.raw_ay = float(np.short(np.short(acc_data[3]<<8)|acc_data[2])*self.ACC_CONST)
-            self.raw_az = float(np.short(np.short(acc_data[5]<<8)|acc_data[4])*self.ACC_CONST)
-
-            self.raw_gx = float(np.short(np.short(ang_data[1]<<8)|ang_data[0])*self.ANG_CONST)
-            self.raw_gy = float(np.short(np.short(ang_data[3]<<8)|ang_data[2])*self.ANG_CONST)
-            self.raw_gz = float(np.short(np.short(ang_data[5]<<8)|ang_data[4])*self.ANG_CONST)
-
-
-    # 读取四元数,从0x51寄存器连续读取8个字节数据
-    def get_quatern(self):
-        try:
-            quat_data = self.i2c.read_i2c_block_data(self.addr, 0x51, 8)
-        except IOError:
-            print("Read IMU quaternion date error !")
-        else:
-            self.raw_q0 = float((np.short(np.short(quat_data[1]<<8)|quat_data[0]))/32768.0)
-            self.raw_q1 = float((np.short(np.short(quat_data[3]<<8)|quat_data[2]))/32768.0)
-            self.raw_q2 = float((np.short(np.short(quat_data[5]<<8)|quat_data[4]))/32768.0)
-            self.raw_q3 = float((np.short(np.short(quat_data[7]<<8)|quat_data[6]))/32768.0)
-