增加iic模式下读取imu模块数据调试代码

rasp_imu_hat_6dof/cfg/param.yaml

@@ -1,8 +1,13 @@
-##################################################
-# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+###################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
 # Description:使用IIC总线来读取IMU模块的数据,并
 #   通过话题将imu数据发送出来.这里是可以配置的
-#   一些参数.
+#   一些参数.各参数意义如下:
+#   pub_data_topic:发布imu数据的话题名.
+#   pub_temp_topic:发布当前imu模块温度的话题名.
+#   yaw_topic:发布当前yaw偏航角的话题,单位弧度.
+#   link_name:imu模块的urdf中link名字.
+#   pub_hz:上述各话题发布的频率.
 # Author: corvin
 # History:
 #   20200406:init this file.

rasp_imu_hat_6dof/launch/imu_data_pub.launch

@@ -1,9 +1,9 @@
 <!---
-  Copyright:2016-2019 https://www.corvin.cn ROS小课堂
+  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
+  Description:该launch启动文件是为了启动扩展板,使其进入正常工作状态.启动后就会在
+  /imu_data话题上发布imu传感器信息.需要该信息的节点,只需订阅该话题即可.同时我们还可
+  以使用dynamic_reconfigure来动态实时的修正z轴的角度.
   Author: corvin
-  Description:该launch启动文件是为了启动扩展板，使其进入正常工作状态。启动后，就会在
-    /imu话题上发布imu传感器信息。需要该信息的节点，订阅该话题即可。同时，我们还可以使
-    用dynamic_reconfigure来动态的修正z轴的角度。
   History:
     20191031:Initial this launch file.
 -->

rasp_imu_hat_6dof/launch/imu_rviz_display.launch

@@ -1,13 +1,13 @@
 <!---
-  Copyright:2016-2019 https://www.corvin.cn ROS小课堂
+  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
+  Description:该launch启动文件是为了在启动扩展板,使其进入正常工作状态后,
+    可以在rviz中可视化查看imu扩展板当前的姿态信息.
   Author: corvin
-  Description:该launch启动文件是为了在启动扩展板，使其进入正常工作状态后。
-    可以在rviz中可视化imu扩展板当前的姿态信息。
   History:
     20191031:Initial this launch file.
 -->
 <launch>
-  <node pkg="rviz" type="rviz" name="imu_rviz_node" 
+  <node pkg="rviz" type="rviz" name="imu_rviz_node"
         args="-d $(find rasp_imu_hat_6dof)/cfg/imu_display.rviz">
   </node>
 </launch>

rasp_imu_hat_6dof/nodes/imu_data.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 # -*- coding: UTF-8 -*-
 
-# Copyright: 2016-2019 https://www.corvin.cn ROS小课堂
+# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
 # Author: corvin
 # Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
 #    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
@@ -21,9 +21,15 @@ 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)
@@ -38,9 +44,12 @@ class MyIMU(object):
         except IOError:
             rospy.logerr("Read IMU YPRAG date error !")
         else:
-            self.raw_roll  = float(((roll_tmp[1]<<8) |roll_tmp[0])/32768.0*180.0)
-            self.raw_pitch = float(((pitch_tmp[1]<<8)|pitch_tmp[0])/32768.0*180.0)
-            self.raw_yaw  = (yaw_tmp[1] << 8 | yaw_tmp[0])/32768.0*180
+            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_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)

rasp_imu_hat_6dof/nodes/imu_node.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 # -*- coding: UTF-8 -*-
 
-# Copyright: 2016-2020 https://www.corvin.cn ROS小课堂
+# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
 # Author: corvin
 # Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
 #    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
@@ -13,7 +13,8 @@
 #    20191209:新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
 #    20200406:增加可以直接发布yaw数据的话题.
 #    20200410:增加通过service方式发布yaw数据,方便其他节点调用.
-#    20200703:修改三轴线性加速度的方向,沿着各轴正方向数据为正,否则加速度就为负值,单位m/s2.
+#    20200703:修改三轴线性加速度的方向,沿着各轴正方向数据为正,
+#      否则加速度就为负值,单位m/s2.
 import rospy
 import math
 
@@ -47,8 +48,8 @@ rospy.init_node("imu_node")
 data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
 temp_topic_name = rospy.get_param('~pub_temp_topic', 'imu_temp')
 link_name  = rospy.get_param('~link_name', 'base_imu_link')
-pub_hz = rospy.get_param('~pub_hz', '20')
 yaw_topic_name = rospy.get_param('~yaw_topic', 'yaw_topic')
+pub_hz = rospy.get_param('~pub_hz', '20')
 
 data_pub = rospy.Publisher(data_topic_name, Imu, queue_size=1)
 temp_pub = rospy.Publisher(temp_topic_name, Float32, queue_size=1)
@@ -91,7 +92,7 @@ rospy.loginfo("IMU Module is working ...")
 while not rospy.is_shutdown():
     myIMU.get_YPRAG()
 
-    #rospy.loginfo("yaw:%f pitch:%f  roll:%f", myIMU.raw_yaw,
+    #rospy.loginfo("yaw:%f pitch:%f roll:%f", myIMU.raw_yaw,
     #              myIMU.raw_pitch, myIMU.raw_roll)
     yaw_deg = float(myIMU.raw_yaw)
     yaw_deg = yaw_deg + imu_yaw_calibration