优化代码

imu_6dof_iic.py

@@ -3,42 +3,49 @@
 
 # Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
 # Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数,
-#   然后将其打印输出.
+#   然后将其打印输出.这里需要注意IMU模块使用的坐标系为ENU东北天坐标系.
 # Author: corvin
 # History:
 #    20220523:Initial this file.
 
-import math
-import time
-
+import math,time
 from imu_data_proc import MyIMUClass
 
-imu_iic_addr = 0x50
 
-degrees2rad = math.pi/180.0
-yaw = 0.0
+# 默认IIC模块的iic地址为0x50
+imu_iic_addr = 0x50
 
+# 将度数转换为弧度
+deg2rad = math.pi/180.0
 accel_factor = 9.806  #sensor accel g convert to m/s^2.
 
-myIMU = MyIMUClass(imu_iic_addr)
 
-print("Now 6DOF IMU Module is working ...")
-while True:
-    myIMU.get_YPRAG()
+if __name__=='__main__':
+    myIMU = MyIMUClass(imu_iic_addr)
+    print("Now 6DOF IMU Module is working ...\n")
+
+    while True:
+        myIMU.get_YPRAG()
+
+        yaw_deg = float(myIMU.raw_yaw)
+        if yaw_deg >= 180.0:
+            yaw_deg -= 360.0
 
-    yaw_deg = float(myIMU.raw_yaw)
-    if yaw_deg >= 180.0:
-        yaw_deg -= 360.0
+        yaw   = yaw_deg*deg2rad
+        pitch = float(myIMU.raw_pitch)*deg2rad
+        roll  = float(myIMU.raw_roll)*deg2rad
+        print("roll: ", roll, "pitch: ", pitch, "yaw: ", yaw)
 
-    yaw   = yaw_deg*degrees2rad
-    pitch = float(myIMU.raw_pitch)*degrees2rad
-    roll  = float(myIMU.raw_roll)*degrees2rad
-    print("roll: ", roll, "pitch: ", pitch, "yaw: ", yaw)
+        # 读取四元数,将其打印输出
+        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: ",float(myIMU.raw_gx)*deg2rad, "angular_y: ",float(myIMU.raw_gy)*deg2rad, "angular_z: ",float(myIMU.raw_gz)*deg2rad)
 
-    myIMU.get_quatern()
-    print("q_x: ", myIMU.raw_q1, "q_y: ", myIMU.raw_q2, "q_z: ", myIMU.raw_q3, "q_w: ", myIMU.raw_q0)
-    print("a_v_x: ", float(myIMU.raw_gx)*degrees2rad, "a_v_y: ", float(myIMU.raw_gy)*degrees2rad, "a_v_z: ", float(myIMU.raw_gz)*degrees2rad)
-    print("l_acc_x: ", -float(myIMU.raw_ax)*accel_factor ,"l_acc_y: ", -float(myIMU.raw_ay)*accel_factor ,"l_acc_z: ", -float(myIMU.raw_az)*accel_factor)
+        # 打印输出三轴线加速度
+        print("line_acc_x: ",float(myIMU.raw_ax)*accel_factor ,"line_acc_y: ",float(myIMU.raw_ay)*accel_factor ,"line_acc_z: ",float(myIMU.raw_az)*accel_factor)
 
-    time.sleep(0.1)
+        print("------------------------------------------------------------------------------------\n")
+        time.sleep(0.1)
 

imu_data_proc.py

@@ -2,8 +2,8 @@
 # -*- coding: UTF-8 -*-
 
 # Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
+# Description: 使用python3从IIC接口中读取IMU模块的三轴线加速度,三轴角速度,四元数原始数据。
 # Author: corvin
-# Description: 使用python3从IIC接口中读取IMU模块的三轴加速度、三轴角速度、四元数。
 # History:
 #    20220523: Initial this file.
 
@@ -16,6 +16,8 @@ class MyIMUClass(object):
         self.i2c  = smbus.SMBus(1)
         self.addr = dev_iic_addr
 
+
+    # 读取roll,pitch,yaw数据,三轴角速度,三轴线加速度数据
     def get_YPRAG(self):
         try:
             rpy_data  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
@@ -36,6 +38,8 @@ class MyIMUClass(object):
             self.raw_gy = float(np.short(np.short(gxyz_data[3]<<8)|gxyz_data[2])/32768.0*2000.0)
             self.raw_gz = float(np.short(np.short(gxyz_data[5]<<8)|gxyz_data[4])/32768.0*2000.0)
 
+
+    # 读取四元数,从0x51寄存器连续读取8个字节数据
     def get_quatern(self):
         try:
             quat_data = self.i2c.read_i2c_block_data(self.addr, 0x51, 8)