优化代码,提高运行速度

imu_6dof_iic.py

@@ -12,12 +12,12 @@ import math,time
 from imu_data_proc import MyIMUClass
 
 
-# 默认IIC模块的iic地址为0x50
+# 默认IMU模块的iic地址为0x50
 imu_iic_addr = 0x50
 
 # 将度数转换为弧度
 deg2rad = math.pi/180.0
-accel_factor = 9.806  #sensor accel g convert to m/s^2.
+acc_factor = 9.806  #sensor acc g convert to m/s^2.
 
 
 if __name__=='__main__':
@@ -27,24 +27,24 @@ if __name__=='__main__':
     while True:
         myIMU.get_YPRAG()
 
-        yaw_deg = float(myIMU.raw_yaw)
+        yaw_deg = 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_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("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)
+        print("angular_x:",myIMU.raw_gx*deg2rad, " angular_y:",myIMU.raw_gy*deg2rad, " angular_z:",myIMU.raw_gz*deg2rad)
 
         # 打印输出三轴线加速度
-        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)
+        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_data_proc.py

@@ -16,27 +16,35 @@ class MyIMUClass(object):
         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:
-            rpy_data  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
-            axyz_data = self.i2c.read_i2c_block_data(self.addr, 0x34, 6)
-            gxyz_data = self.i2c.read_i2c_block_data(self.addr, 0x37, 6)
+            # 读取三轴角度,共计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])/32768.0*180.0)
-            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])/32768.0*180.0)
-            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])/32768.0*180.0)
+            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(axyz_data[1]<<8)|axyz_data[0])/32768.0*16.0)
-            self.raw_ay = float(np.short(np.short(axyz_data[3]<<8)|axyz_data[2])/32768.0*16.0)
-            self.raw_az = float(np.short(np.short(axyz_data[5]<<8)|axyz_data[4])/32768.0*16.0)
+            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(gxyz_data[1]<<8)|gxyz_data[0])/32768.0*2000.0)
-            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)
+            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个字节数据