iic读取模块寄存器中数据时,改为一次性读取多个字节,不是读取多个寄存器每次读２字节,发布的四元素数据直接从imu中读取不是使用rpy自己计算的

rasp_imu_hat_6dof/nodes/imu_data.py

@@ -10,6 +10,8 @@
 #    20191031: Initial this file.
 #    20191209：Add get imu chip temperature function-get_temp().
 #    20200702: 读取各数据后需要使用np.short转换才能进行后续数据处理.
+#    20210319:从寄存器地址读取数据时,直接一次性读取多个字节,不是依次
+#      读取多个寄存器地址,每个地址读取２字节.
 import smbus
 import rospy
 import numpy as np
@@ -21,60 +23,34 @@ class MyIMU(object):
 
     def get_YPRAG(self):
         try:
-            #roll_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
-            #pitch_tmp = self.i2c.read_i2c_block_data(self.addr, 0x3e, 2)
-            #yaw_tmp   = self.i2c.read_i2c_block_data(self.addr, 0x3f, 2)
-            roll_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 2)
-            pitch_tmp = self.i2c.read_i2c_block_data(self.addr, 0x3e, 2)
-            yaw_tmp   = self.i2c.read_i2c_block_data(self.addr, 0x3f, 2)
-            #rospy.loginfo("Read roll_tmp:%#x %#x",  roll_tmp[0], roll_tmp[1])
-            #rospy.loginfo("Read pitch_tmp:%#x %#x", roll_tmp[2], roll_tmp[3])
-            #rospy.loginfo("Read yaw_tmp:%#x %#x",   roll_tmp[4], roll_tmp[5])
-
-            ax_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x34, 2)
-            ay_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x35, 2)
-            az_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x36, 2)
-            #rospy.loginfo("Read i2c accX:" + str(ax_tmp))
-            #rospy.loginfo("Read i2c accY:" + str(ay_tmp))
-            #rospy.loginfo("Read i2c accZ:" + str(az_tmp))
-
-            gx_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x37, 2)
-            gy_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x38, 2)
-            gz_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x39, 2)
+            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)
         except IOError:
-            rospy.logerr("Read IMU YPRAG date error !")
+            rospy.logerr("Read IMU RPYAG date error !")
         else:
-            self.raw_roll  = float(np.short(np.short(roll_tmp[1]<<8)|roll_tmp[0])/32768.0*180.0)
-            self.raw_pitch = float(np.short(np.short(pitch_tmp[1]<<8)|pitch_tmp[0])/32768.0*180.0)
-            self.raw_yaw   = float(np.short(np.short(yaw_tmp[1]<<8)|yaw_tmp[0])/32768.0*180.0)
-            #self.raw_roll  = float(np.short(np.short(roll_tmp[1]<<8)|roll_tmp[0])/32768.0*180.0)
-            #self.raw_pitch = float(np.short(np.short(roll_tmp[3]<<8)|roll_tmp[2])/32768.0*180.0)
-            #self.raw_yaw   = float(np.short(np.short(roll_tmp[5]<<8)|roll_tmp[4])/32768.0*180.0)
+            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_ax = float(np.short(np.short(ax_tmp[1]<<8)|ax_tmp[0])/32768.0*16.0)
-            self.raw_ay = float(np.short(np.short(ay_tmp[1]<<8)|ay_tmp[0])/32768.0*16.0)
-            self.raw_az = float(np.short(np.short(az_tmp[1]<<8)|az_tmp[0])/32768.0*16.0)
-            #rospy.loginfo("Read raw accX:" + str(self.raw_ax))
-            #rospy.loginfo("Read raw accY:" + str(self.raw_ay))
-            #rospy.loginfo("Read raw accZ:" + str(self.raw_az))
+            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_gx = float(np.short(np.short(gx_tmp[1]<<8)|gx_tmp[0])/32768.0*2000.0)
-            self.raw_gy = float(np.short(np.short(gy_tmp[1]<<8)|gy_tmp[0])/32768.0*2000.0)
-            self.raw_gz = float(np.short(np.short(gz_tmp[1]<<8)|gz_tmp[0])/32768.0*2000.0)
+            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)
 
     def get_quatern(self):
         try:
-            q0 = self.i2c.read_i2c_block_data(self.addr, 0x51, 2)
-            q1 = self.i2c.read_i2c_block_data(self.addr, 0x52, 2)
-            q2 = self.i2c.read_i2c_block_data(self.addr, 0x53, 2)
-            q3 = self.i2c.read_i2c_block_data(self.addr, 0x54, 2)
+            quat_data = self.i2c.read_i2c_block_data(self.addr, 0x51, 8)
         except IOError:
             rospy.logerr("Read IMU quaternion date error !")
         else:
-            self.raw_q0 = float((np.short((q0[1]<<8)|q0[0]))/32768.0)
-            self.raw_q1 = float((np.short((q1[1]<<8)|q1[0]))/32768.0)
-            self.raw_q2 = float((np.short((q2[1]<<8)|q2[0]))/32768.0)
-            self.raw_q3 = float((np.short((q3[1]<<8)|q3[0]))/32768.0)
+            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)
 
     def get_two_float(self, data, n):
         data = str(data)

rasp_imu_hat_6dof/nodes/imu_node.py

@@ -15,11 +15,11 @@
 #    20200410:增加通过service方式发布yaw数据,方便其他节点调用.
 #    20200703:修改三轴线性加速度的方向,沿着各轴正方向数据为正,
 #      否则加速度就为负值,单位m/s2.
+#    20210319:计算四元素时,不用自己使用rpy数据来计算,直接使用imu模块提供的.
 import rospy
 import math
 
 from rasp_imu_hat_6dof.srv import GetYawData,GetYawDataResponse
-from tf.transformations import quaternion_from_euler
 from dynamic_reconfigure.server import Server
 from rasp_imu_hat_6dof.cfg import imuConfig
 from std_msgs.msg import Float32
@@ -119,18 +119,11 @@ while not rospy.is_shutdown():
     imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad
 
     # From IMU module get quatern param
-    #myIMU.get_quatern()
-    #imuMsg.orientation.x = myIMU.raw_q1
-    #imuMsg.orientation.y = myIMU.raw_q2
-    #imuMsg.orientation.z = myIMU.raw_q3
-    #imuMsg.orientation.w = myIMU.raw_q0
-
-    # Calculate quatern param from roll,pitch,yaw by ourselves
-    q = quaternion_from_euler(roll, pitch, yaw)
-    imuMsg.orientation.x = q[0]
-    imuMsg.orientation.y = q[1]
-    imuMsg.orientation.z = q[2]
-    imuMsg.orientation.w = q[3]
+    myIMU.get_quatern()
+    imuMsg.orientation.x = myIMU.raw_q1
+    imuMsg.orientation.y = myIMU.raw_q2
+    imuMsg.orientation.z = myIMU.raw_q3
+    imuMsg.orientation.w = myIMU.raw_q0
 
     imuMsg.header.stamp= rospy.Time.now()
     imuMsg.header.frame_id = link_name