更新imu代码,保持与单独的imu板代码仓库同步更新

src/rasp_imu_hat_6dof/.gitignore

@@ -0,0 +1,60 @@
+# ---> Python
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+env/
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*,cover
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+

src/rasp_imu_hat_6dof/README.md

@@ -0,0 +1,51 @@
+# rasp_imu_hat_6dof
+
+为树莓派IMU扩展板创建的ROS代码仓库，直接将该软件包源码放在ROS工作区src目录下即可。然后使用catkin_make就可以进行编译了。完整的下载命令如下:  
+`git clone https://code.corvin.cn:3000/corvin_zhang/rasp_imu_hat_6dof.git`  
+
+----
+## 使用串口发布IMU数据
+当编译完成后可以使用如下命令启动发布imu数据:  
+`roslaunch serial_imu_hat_6dof serial_imu_hat.launch`  
+
+当启动上述命令后,可以在ROS中查看到如下话题和服务可以调用:  
+* 发布imu数据的话题名: /imu_data
+* 发布imu模块温度的话题名: /imu_temp
+* 发布yaw角度的话题名: /yaw_data
+* 可将yaw角度归零的话题名: /yaw_zero_topic
+* 可将yaw角度归零的服务名: /yaw_zero_service
+* 可修改串口波特率的服务: /baud_update_service
+* 控制D0,D1,D2,D3输出数字高低电平的服务: /pin_outHL_service  
+* 可得到当前yaw角度的服务: /imu_node/GetYawData  
+对于以上这些话题名和服务名,都可以在serial_imu_hat_6dof/cfg/param.yaml配置文件中进行修改.  
+
+1. 如果想使用话题方式,将yaw角度归零,那就可以执行如下命令:  
+`rostopic pub -1 /yaw_zero_topic std_msgs/Empty "{}"`  
+
+2. 如果想使用服务调用方式,将yaw角度归零,那执行如下命令:  
+`rosservice call /yaw_zero_service "{}"`  
+
+3. 如果想修改串口通信的波特率,那就可以执行如下命令:  
+`rosservice call /baud_update_service "index: 1"`  
+这里需要注意index后面的数字,１表示更新波特率为9600,2表示为57600,3表示115200,
+当更新完波特率后,需要修改本地配置文件中串口波特率为新的,否则会无法正常获取imu数据.  
+
+4. 如果需要控制D0引脚输出高电平,那就可以使用如下命令:  
+rosservice call /pin_outHL_service "D0: 1  
+D1: 0  
+D2: 0  
+D3: 0"  
+这里可以看到D0后面的数字为1,那就表明D0引脚输出高电平.若想修改其他引脚的高低电平状态,那就修改D0,D1,D2,D3后面的对应数字即可.  
+
+5. 如果想通过服务调用方式来获取到当前的yaw角度信息,可在终端中执行如下命令:  
+`rosservice call /imu_node/GetYawData`  
+当然这些不仅可以在终端中执行命令获取到yaw数据,还可以在代码中编写服务的客户端程序,请求字段为空,就可以在response字段中获取到当前yaw角度.  
+
+----
+## 使用IIC发布IMU数据
+`roslaunch rasp_imu_hat_6dof imu_data_pub.launch`  
+
+----
+## 在Rviz中显示查看IMU效果
+`roslaunch rasp_imu_hat_6dof imu_rviz_display.launch`  
+

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/CMakeLists.txt

@@ -10,7 +10,6 @@ find_package(catkin REQUIRED COMPONENTS
 add_service_files(
     FILES
     GetYawData.srv
-    GetYpositionData.srv
 )
 
 generate_messages(

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/cfg/imu_display.rviz

@@ -5,10 +5,9 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /Status1
         - /Grid1/Offset1
       Splitter Ratio: 0.6222222447395325
-    Tree Height: 774
+    Tree Height: 344
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -28,6 +27,8 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -52,11 +53,11 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Acceleration properties:
-        Acc. vector alpha: 0.6000000238418579
+        Acc. vector alpha: 0.800000011920929
         Acc. vector color: 179; 200; 255
-        Acc. vector scale: 0.029999999329447746
+        Acc. vector scale: 0.05000000074505806
         Derotate acceleration: false
-        Enable acceleration: true
+        Enable acceleration: false
       Axes properties:
         Axes scale: 1
         Enable axes: true
@@ -89,7 +90,10 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -99,7 +103,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.5020098686218262
+      Distance: 0.9545544385910034
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -114,18 +118,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.22039949893951416
+      Pitch: 0.4303995668888092
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.515486478805542
+      Yaw: 1.730486273765564
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: true
-  Height: 1144
+  Height: 714
   Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003c9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000044000003c9000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064000000044fc0100000002fb0000000800540069006d00650100000000000006400000028000fffffffb0000000800540069006d0065010000000000000450000000000000000000000640000003c900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000021bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000440000021b000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000040000000044fc0100000002fb0000000800540069006d00650100000000000004000000028000fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000021b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -134,6 +138,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1600
+  Width: 1024
   X: 0
-  Y: 28
+  Y: 96

src/rasp_imu_hat_6dof/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.
@@ -11,6 +16,5 @@
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
 yaw_topic: yaw_data
-pub_yposition_topic: yposition_data
 link_name: base_imu_link
-pub_hz: 120
+pub_hz: 20

src/rasp_imu_hat_6dof/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.
 -->

src/rasp_imu_hat_6dof/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>

src/rasp_imu_hat_6dof/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接口
@@ -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,51 +23,34 @@ class MyIMU(object):
 
     def get_YPRAG(self):
         try:
-            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)
-
-            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(((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(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)

src/rasp_imu_hat_6dof/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,12 +13,13 @@
 #    20191209:新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
 #    20200406:增加可以直接发布yaw数据的话题.
 #    20200410:增加通过service方式发布yaw数据,方便其他节点调用.
-#    20200703:修改三轴线性加速度的方向,沿着各轴正方向数据为正,否则加速度就为负值,单位m/s2.
+#    20200703:修改三轴线性加速度的方向,沿着各轴正方向数据为正,
+#      否则加速度就为负值,单位m/s2.
+#    20210319:计算四元素时,不用自己使用rpy数据来计算,直接使用imu模块提供的.
 import rospy
 import math
 
-from rasp_imu_hat_6dof.srv import GetYawData,GetYawDataResponse,GetYpositionData,GetYpositionDataResponse
-from tf.transformations import quaternion_from_euler
+from rasp_imu_hat_6dof.srv import GetYawData,GetYawDataResponse
 from dynamic_reconfigure.server import Server
 from rasp_imu_hat_6dof.cfg import imuConfig
 from std_msgs.msg import Float32
@@ -30,21 +31,10 @@ degrees2rad = math.pi/180.0
 imu_yaw_calibration = 0.0
 yaw = 0.0
 
-y_position = 0
-y1_velocity = 0
-y2_velocity = 0
-y_acceleration = 0
-y_const_acceleration = 0
-y1_update_flag = 0
-
 # ros server return Yaw data
 def return_yaw_data(req):
     return GetYawDataResponse(yaw)
 
-# Ros server return Yposition data
-def return_yposition_data(req):
-    return GetYpositionDataResponse(y_position)
-
 # Callback for dynamic reconfigure requests
 def reconfig_callback(config, level):
     global imu_yaw_calibration
@@ -57,20 +47,15 @@ rospy.init_node("imu_node")
 # Get DIY config param
 data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
 temp_topic_name = rospy.get_param('~pub_temp_topic', 'imu_temp')
-yposition_topic_name = rospy.get_param("~pub_yposition_topic","yposition_data")
 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')
-
-delta_time = 0.05
+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)
 yaw_pub = rospy.Publisher(yaw_topic_name, Float32, queue_size=1)
-yposition_pub = rospy.Publisher(yposition_topic_name, Float32, queue_size=1)
 srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
 yaw_srv = rospy.Service('imu_node/GetYawData', GetYawData, return_yaw_data)
-yposition_srv = rospy.Service('imu_node/GetYpositionData', GetYpositionData, return_yposition_data)
 
 imuMsg = Imu()
 yawMsg = Float32()
@@ -105,14 +90,9 @@ rate = rospy.Rate(pub_hz)
 
 rospy.loginfo("IMU Module is working ...")
 while not rospy.is_shutdown():
-
-    #Calculate y_position and Publish
-    y_position = y_position + y1_velocity * delta_time + 0.5 * y_acceleration * delta_time * delta_time
-    yposition_pub.publish(y_position)
-
     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
@@ -139,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
@@ -162,15 +135,5 @@ while not rospy.is_shutdown():
     myIMU.get_temp()
     temp_pub.publish(myIMU.temp)
 
-    # Update y_velocity and y_accelerate
-    if y1_update_flag != 0:
-        y1_velocity = y2_velocity
-    else:
-        y_const_acceleration = imuMsg.linear_acceleration.y
-        y1_update_flag += 1
-    y_acceleration = imuMsg.linear_acceleration.y - y_const_acceleration
-    y2_velocity = y2_velocity + y_acceleration * delta_time
-    
-
     rate.sleep()
 

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/srv/GetYpositionData.srv

@@ -1,2 +0,0 @@
----
-float32 yposition

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/CMakeLists.txt

@@ -18,12 +18,6 @@ find_package(catkin REQUIRED COMPONENTS
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
 ################################################
 ## Declare ROS messages, services and actions ##
 ################################################
@@ -43,8 +37,6 @@ find_package(catkin REQUIRED COMPONENTS
 ##     find_package(catkin REQUIRED COMPONENTS ...)
 ##   * add "message_runtime" and every package in MSG_DEP_SET to
 ##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
 ##   * uncomment the generate_messages entry below
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
@@ -52,22 +44,17 @@ find_package(catkin REQUIRED COMPONENTS
 # add_message_files(
 #   FILES
 #   Message1.msg
-#   Message2.msg
 # )
 
 ## Generate services in the 'srv' folder
 add_service_files(
   FILES
-  GetYawData.srv
+  setYawZero.srv
+  setBaudRate.srv
+  setPinOutHL.srv
+  getYawData.srv
 )
 
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
@@ -92,7 +79,6 @@ generate_messages(
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 # generate_dynamic_reconfigure_options(
 #   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
 # )
 
 ###################################
@@ -105,7 +91,7 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+  INCLUDE_DIRS include
 #  LIBRARIES serial_imu_hat_6dof
 #  CATKIN_DEPENDS dynamic_reconfigure roscpp sensor_msgs
 #  DEPENDS system_lib
@@ -159,13 +145,6 @@ target_link_libraries(serial_imu_node
 # all install targets should use catkin DESTINATION variables
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
 
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
 # install(TARGETS ${PROJECT_NAME}_node

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/cfg/param.yaml

@@ -1,34 +1,46 @@
-################################################
-# Copyright: 2016-2020 www.corvin.cn ROS小课堂
-# Description:使用串口进行通信时的配置信息.
-#   imu_dev:指示串口设备挂在点,如果使用usb转串口
-#     模块,都是写ttyUSB0,1,2等.如果使用树莓派的
-#     GPIO排针处串口,则使用ttyS0.
-#   baud_rate:串口通信波特率,旧模块的波特率为9600,
-#     新版本的波特率为57600,如果串口没数据可以修改.
-#   data_bits:数据位
-#   parity:数据校验位
-#   stop_bits:停止位
-#   pub_data_topic:发布imu数据的话题名.
-#   pub_temp_topic:发布imu模块温度的话题名.
-#   link_name: imu模块的link名.
-#   pub_hz:话题发布的数据频率.
+###########################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
+# Description:使用串口与IMU模块进行通信时的配置信息.
+# 可以配置的各参数如下,这些话题名或者服务名,发布频率这些都
+# 可以根据需要来修改:
+#  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
+#     模块进行通信,都是写ttyUSB0,1,2等.如果使用树莓派的
+#     GPIO排针处串口,则使用ttyAMA0.需要注意是这个ttyAMA0
+#     默认是给树莓派蓝牙使用的,所以要想使用该硬件串口,
+#     就需要在raspi-config中关闭串口登录功能,打开硬件
+#     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
+#  baud_rate:串口通信波特率,默认为57600,如果查看发布的
+#  　　话题中没有IMU数据可以尝试换波特率.
+#  pub_data_topic:发布imu数据的ROS话题名.
+#  pub_temp_topic:发布imu模块温度的ROS话题名.
+#  link_name:imu模块的link名.
+#  pub_hz:话题发布的数据频率,默认设置为30Hz.
+#  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
+#  yaw_pub_topic:将yaw信息通过该话题发送出来.
+#  pin_outHL_srv:控制IMU板上D0-D4的输出电平高低的服务.
+#  yaw_data_srv:可以获取到yaw当前角度的服务.
 # Author: corvin
 # History:
-#   20200402: init this file.
-#   20200406: 增加直接发布yaw数据的话题.
-################################################
-imu_dev: /dev/ttyS0
+#   20200402:init this file.
+#   20200406:增加直接发布yaw数据的ROS话题.
+#   20210317:去掉数据位,奇偶校验,停止位参数,这几个
+#     参数模块已经固定死了,无需修改.
+#   20210318:增加yaw角度归零的话题配置参数yaw_zero_topic.
+#     增加使用服务方式将yaw归零的名称yaw_zero_service.
+#   20210319:增加修改串口波特率的配置参数baud_update_srv.
+#   20210321:增加可以设置D0-D4共4个引脚输出数字高低电平的
+#     服务配置参数pin_outHL_srv.增加获取yaw数据的服务.
+###########################################################
+imu_dev: /dev/ttyAMA0
 baud_rate: 57600
-data_bits: 8
-parity: N
-stop_bits: 1
+pub_hz: 30
 
+link_name: base_imu_link
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
-yaw_topic: yaw_data
-link_name: base_imu_link
-pub_hz: 120
-
-yaw_service: /imu_node/GetYawData
-
+yaw_pub_topic: yaw_data
+yaw_zero_topic: yaw_zero_topic
+yaw_zero_service: yaw_zero_service
+baud_update_srv: baud_update_service
+pin_outHL_srv: pin_outHL_service
+yaw_data_srv: /imu_node/GetYawData

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/include/imu_data.h

@@ -1,22 +1,26 @@
+/*******************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:操作imu模块的代码头文件.
+ * Author: corvin
+ * History:
+ *   20210318:init this file.
+ *   20210319:增加修改串口通信波特率的函数.
+ *   20210321:增加控制IMU引脚数字高低电平的函数.
+ *******************************************************/
 #ifndef _IMU_DATA_H_
 #define _IMU_DATA_H_
 
-int initSerialPort(const char* path, const int baud, const int dataBits,
-        const char* parity, const int stopBit);
+int initSerialPort(const char* path, const int baud);
 
 int getImuData();
 int closeSerialPort();
 
-float getAccX();
-float getAccY();
-float getAccZ();
-
-float getAngularX();
-float getAngularY();
-float getAngularZ();
-
-float getAngleX();
-float getAngleY();
-float getAngleZ();
+float getAcc(int flag);
+float getAngular(int flag);
+float getAngle(int flag);
+float getQuat(int flag);
 
+int makeYawZero(void);
+int setIMUBaudRate(const int flag);
+int setIMUPinOutHL(const int d0,const int d1,const int d2,const int d3);
 #endif

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/launch/serial_imu_hat.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2020 https://www.corvin.cn ROS小课堂
+  Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
   Description:使用串口来读取imu模块的数据,并在ros中发布使用话题发布出来.
   Author: corvin
   History:

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/package.xml

@@ -1,13 +1,11 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>serial_imu_hat_6dof</name>
-  <version>0.0.0</version>
+  <version>0.0.1</version>
   <description>The serial_imu_hat_6dof package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
+  <maintainer email="corvin_zhang@corvin.cn">corvin_zhang</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
@@ -53,28 +51,19 @@
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
-  <build_depend>std_msgs</build_depend>
   <build_depend>message_generation</build_depend>
-  <build_depend>message_runtime</build_depend>
 
   <build_export_depend>dynamic_reconfigure</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
-  <build_export_depend>std_msgs</build_export_depend>
-  <build_export_depend>message_generation</build_export_depend>
-  <build_export_depend>message_runtime</build_export_depend>
 
   <exec_depend>dynamic_reconfigure</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>message_generation</exec_depend>
   <exec_depend>message_runtime</exec_depend>
 
-
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -1,11 +1,14 @@
-/*****************************************************************
- * Copyright:2016-2020 www.corvin.cn ROS小课堂
- * Description:使用串口方式读取IMU模块信息.
+/*******************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口方式读取和控制IMU模块信息.
  * Author: corvin
  * History:
  *   20200401:init this file.
  *   20200702:将读取到的各数据的高低自己合成后要转换为short类型,
  *     这样才能进行/32768.0*16.0运算.
+ *   20210318:增加可以将yaw角度归零的函数yawZeroCmd().
+ *   20210319:增加可以修改串口通信波特率的函数setIMUBaudRate().
+ *   20210321:增加控制引脚输出数字高低电平的函数setIMUPinOutHL().
 ******************************************************************/
 #include<ros/ros.h>
 #include<stdio.h>
@@ -18,15 +21,18 @@
 #include<sys/types.h>
 
 
-char r_buf[512];
-int port_fd = 0;
-float acce[3],angular[3],angle[3],quater[4];
+static unsigned char r_buf[88];  //一次从串口中读取的数据存储缓冲区
+static int port_fd = -1; //串口打开时的文件描述符
+static float acce[3],angular[3],angle[3],quater[4];
 
-/**********************************************
- * Description:打开串口,输入各参数即可.
- *********************************************/
-int openSerialPort(const char *path, int baud, int dataBits,
-                const char* parity, int stopBit)
+/******************************************************
+ * Description:打开IMU模块串口,输入两个参数即可连接.
+ *   open()打开失败时,返回-1,成功打开时返回文件描述符.
+ *   各参数意义如下:
+ *   const char *path:IMU设备的挂载地址;
+ *   const int baud:串口通讯的波特率;
+ *****************************************************/
+int openSerialPort(const char *path, const int baud)
 {
     int fd = 0;
     struct termios terminfo;
@@ -35,11 +41,12 @@ int openSerialPort(const char *path, int baud, int dataBits,
     fd = open(path, O_RDWR|O_NOCTTY);
     if (-1 == fd)
     {
-        ROS_ERROR("Open imu dev error:%s", path);
+        ROS_ERROR("Open IMU dev error:%s, baud:%d", path, baud);
         return -1;
     }
 
-    if(isatty(STDIN_FILENO) == 0)
+    //判断当前连接的设备是否为终端设备
+    if (isatty(STDIN_FILENO) == 0)
     {
         ROS_ERROR("IMU dev isatty error !");
         return -2;
@@ -53,16 +60,6 @@ int openSerialPort(const char *path, int baud, int dataBits,
            cfsetospeed(&terminfo, B9600); //设置输出速度
            break;
 
-        case 19200:
-           cfsetispeed(&terminfo, B19200);
-           cfsetospeed(&terminfo, B19200);
-           break;
-
-        case 38400:
-           cfsetispeed(&terminfo, B38400);
-           cfsetospeed(&terminfo, B38400);
-           break;
-
         case 57600:
            cfsetispeed(&terminfo, B57600);
            cfsetospeed(&terminfo, B57600);
@@ -73,81 +70,34 @@ int openSerialPort(const char *path, int baud, int dataBits,
            cfsetospeed(&terminfo, B115200);
            break;
 
-       default://设置默认波特率9600
+       default: //设置默认波特率9600
            cfsetispeed(&terminfo, B9600);
            cfsetospeed(&terminfo, B9600);
            break;
     }
 
-    //设置数据位
+    //设置数据位 - 8 bit
     terminfo.c_cflag |= CLOCAL|CREAD;
     terminfo.c_cflag &= ~CSIZE;
-    switch(dataBits)
-    {
-        case 7:
-           terminfo.c_cflag |= CS7;
-           break;
-
-        case 8:
-           terminfo.c_cflag |= CS8;
-           break;
-
-        default:
-           ROS_ERROR("set dataBit error !");
-           return -3;
-    }
-
-    //设置奇偶校验位
-    switch(*parity)
-    {
-        case 'o': //设置奇校验
-        case 'O':
-           terminfo.c_cflag |= PARENB;
-           terminfo.c_cflag |= PARODD;
-           terminfo.c_iflag |= (INPCK|ISTRIP);
-           break;
-
-        case 'e': //设置偶校验
-        case 'E':
-           terminfo.c_iflag |= (INPCK|ISTRIP);
-           terminfo.c_cflag |= PARENB;
-           terminfo.c_cflag &= ~PARODD;
-           break;
-
-        case 'n': //不设置奇偶校验位
-        case 'N':
-           terminfo.c_cflag &= ~PARENB;
-           break;
+    terminfo.c_cflag |= CS8;
 
-        default:
-           ROS_ERROR("set parity error !");
-          return -4;
-    }
-
-    //设置停止位
-    switch(stopBit)
-    {
-        case 1:
-            terminfo.c_cflag &= ~CSTOPB;
-            break;
+    //不设置奇偶校验位 - N
+    terminfo.c_cflag &= ~PARENB;
 
-        case 2:
-            terminfo.c_cflag |= CSTOPB;
-            break;
+    //设置停止位 - 1
+    terminfo.c_cflag &= ~CSTOPB;
 
-        default:
-            ROS_ERROR("set stopBit error !");
-            return -5;
-    }
+    //设置等待时间和最小接收字符
+    terminfo.c_cc[VTIME] = 0;
+    terminfo.c_cc[VMIN]  = 1;
 
-    terminfo.c_cc[VTIME] = 10;
-    terminfo.c_cc[VMIN]  = 0;
+    //清除串口缓存的数据
     tcflush(fd, TCIFLUSH);
 
     if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
     {
         ROS_ERROR("set imu serial port attr error !");
-        return -6;
+        return -3;
     }
 
     return fd;
@@ -162,37 +112,70 @@ int closeSerialPort()
     return ret;
 }
 
-/*****************************************************
- * Description:向串口中发送数据.
- ****************************************************/
-int send_data(int fd, char *send_buffer, int length)
+/*****************************************************************
+ * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
+ *   发送控制命令,最后就是需要发送命令保存刚才的操作.解锁命令是固
+ *   定的0xFF 0xAA 0x69 0x88 0xB5,保存命令也是固定的0xFF 0xAA 0x00
+ *   0x00 0x00.
+ *****************************************************************/
+int send_data(int fd, unsigned char *send_buffer, int length)
 {
-	length = write(fd, send_buffer, length*sizeof(unsigned char));
+    int ret = -1;
+    unsigned char unLockCmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
+    unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
 
-	return length;
-}
+    ret = write(fd, unLockCmd, sizeof(unLockCmd));
+    if(ret != sizeof(unLockCmd))
+    {
+        ROS_ERROR("Send IMU module unlock cmd error !");
+        return -1;
+    }
+    ros::Duration(0.01).sleep(); //delay 10ms才能发送其他配置命令
 
-/*******************************************************
- * Description:从串口中接收数据.
- *******************************************************/
-int recv_data(int fd, char* recv_buffer, int len)
-{
-    int length = read(fd, recv_buffer, len);
-    return length;
+    #if 0 //打印发送给IMU模块的数据,用于调试
+    printf("Send %d byte to IMU:", length);
+    for(int i=0; i<length; i++)
+    {
+        printf("0x%02x ", send_buffer[i]);
+    }
+    printf("\n");
+    #endif
+
+    //发送控制命令
+    ret = write(fd, send_buffer, length*sizeof(unsigned char));
+    if(ret != length)
+    {
+        ROS_ERROR("Send IMU module control cmd error !");
+        return -2;
+    }
+
+    //发送保存命令
+    ret = write(fd, saveCmd, sizeof(saveCmd));
+    if(ret != sizeof(saveCmd))
+    {
+        ROS_ERROR("Send IMU module save cmd error !");
+        return -3;
+    }
+    ros::Duration(0.1).sleep(); //delay 100ms才能进行其他操作
+
+    return 0;
 }
 
-/************************************************************
- * Description:根据串口数据协议来解析有效数据,
- ***********************************************************/
-void parse_serialData(char chr)
+/**************************************************************
+ * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
+ *   解析读取其中的44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ *************************************************************/
+void parse_serialData(unsigned char chr)
 {
     static unsigned char chrBuf[100];
     static unsigned char chrCnt = 0;
+
     signed short sData[4]; //save 8 Byte valid info
     unsigned char i = 0;
     unsigned char frameSum = 0;
 
-    //printf("%d \n",chrCnt);
     chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
 
     //判断是否满一个完整数据帧11个字节
@@ -205,12 +188,6 @@ void parse_serialData(char chr)
         frameSum += chrBuf[i];
     }
 
-    //for(i=0;i<11; i++)
-      //ROS_INFO("0x%02x ",chrBuf[i]);
-      //printf("0x%02x ",chrBuf[i]);
-    //printf("\n");
-    //printf("sum is 0x%02x\n",frameSum);
-
     //找到数据帧第一个字节是0x55,校验和是否正确,若两者有一个不正确,
     //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
     if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
@@ -220,9 +197,11 @@ void parse_serialData(char chr)
         return;
     }
 
-    //for(i=0;i<11; i++)
-    //  printf("0x%02x ",chrBuf[i]);
-    //printf("\n");
+    #if 0 //打印出完整的带帧头尾的数据帧
+    for(i=0;i<11; i++)
+      printf("0x%02x ",chrBuf[i]);
+    printf("\n");
+    #endif
 
     memcpy(&sData[0], &chrBuf[2], 8);
     switch(chrBuf[1])
@@ -250,85 +229,211 @@ void parse_serialData(char chr)
             quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
             quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
             quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
+            //printf("%f %f %f %f\n", quater[0], quater[1], quater[2], quater[3]);
             break;
     }
     chrCnt = 0;
 }
 
-/*****************************************************************
- * Description:根据串口配置信息来配置打开串口,准备进行数据通信
- ****************************************************************/
-int initSerialPort(const char* path, const int baud, const int dataBits,
-          const char* parity, const int stopBit)
+/********************************************************************
+ * Description:将yaw角度归零,这里需要通过串口发送的命令如下所示,
+ *  发送z轴角度归零的固定命令:0xFF 0xAA 0x01 0x04 0x00;
+ *******************************************************************/
+int makeYawZero(void)
 {
-    bzero(r_buf, 512);
-    port_fd = openSerialPort(path, baud, dataBits, parity, stopBit);
-    if(port_fd < 0)
+    int ret = 0;
+    unsigned char yawZeroCmd[5] = {0xFF, 0xAA, 0x01, 0x04, 0x00};
+
+    ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
+    if(ret != 0)
     {
-        ROS_ERROR("openSerialPort error !");
+        ROS_ERROR("Send yaw zero control cmd error !");
         return -1;
     }
-
+    ROS_INFO("set yaw to zero radian successfully !");
     return 0;
 }
 
-float getAccX()
-{
-    return acce[0];
-}
-float getAccY()
-{
-    return acce[1];
-}
-float getAccZ()
+/******************************************************************
+ * Description:更新串口通信波特率,根据输入参数的不同,修改为不同的
+ *   波特率,1修改为9600,2修改为57600,3修改为115200.修改波特率就是
+ *   发送3条指令,第一条命令是解除锁定,然后是发送串口波特率更新命令,
+ *   最后是保存操作的指令.
+ ******************************************************************/
+int setIMUBaudRate(const int flag)
 {
-    return acce[2];
+    int ret = 0;
+    int baud = 0;
+    unsigned char baudRate[5] = {0xFF, 0xAA, 0x04, 0x00, 0x00};
+
+    switch(flag)
+    {
+        case 1: //set baud 9600 bps
+            baudRate[3] = 0x02;
+            baud = 9600;
+            break;
+        case 2: //set baud 57600 bps
+            baudRate[3] = 0x05;
+            baud = 57600;
+            break;
+        case 3: //set baud 115200 bps
+            baudRate[3] = 0x06;
+            baud = 115200;
+            break;
+        default: //default set baud 57600 bps
+            baudRate[3] = 0x05;
+            baud = 57600;
+            break;
+    }
+    ret = send_data(port_fd, baudRate, sizeof(baudRate));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send baud rate update cmd error !");
+        return -1;
+    }
+    ROS_INFO("Set new baud rate:[ %d bps] successfully !", baud);
+    ROS_INFO("Please reconnect IMU module with new baud !");
+
+    return 0;
 }
 
-float getAngularX()
+/*********************************************************************
+ * Description:要想控制引脚输出数字高低电平,其实就是发送固定的控制命令,
+ *   这里的高低电平的控制字节是命令中的第4个字节,0x03表明是输出低电平,
+ *   要想输出高电平就是将该字节改为0x02即可.剩下就是依次发送这4个引脚
+ *   的控制命令就可以了.
+ ********************************************************************/
+int setIMUPinOutHL(const int d0,const int d1,const int d2,const int d3)
 {
-    return angular[0];
+    int ret = -1;
+    unsigned char pinD0CtlCmd[5] = {0xFF, 0xAA, 0x0E, 0x03, 0x00};
+    unsigned char pinD1CtlCmd[5] = {0xFF, 0xAA, 0x0F, 0x03, 0x00};
+    unsigned char pinD2CtlCmd[5] = {0xFF, 0xAA, 0x10, 0x03, 0x00};
+    unsigned char pinD3CtlCmd[5] = {0xFF, 0xAA, 0x11, 0x03, 0x00};
+
+    if(d0 == 1)
+        pinD0CtlCmd[3] = 0x02;
+    if(d1 == 1)
+        pinD1CtlCmd[3] = 0x02;
+    if(d2 == 1)
+        pinD2CtlCmd[3] = 0x02;
+    if(d3 == 1)
+        pinD3CtlCmd[3] = 0x02;
+
+    ret = send_data(port_fd, pinD0CtlCmd, sizeof(pinD0CtlCmd));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send pin D0 control cmd error !");
+        return -1;
+    }
+    ret = send_data(port_fd, pinD1CtlCmd, sizeof(pinD1CtlCmd));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send pin D1 control cmd error !");
+        return -2;
+    }
+    ret = send_data(port_fd, pinD2CtlCmd, sizeof(pinD2CtlCmd));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send pin D2 control cmd error !");
+        return -3;
+    }
+    ret = send_data(port_fd, pinD3CtlCmd, sizeof(pinD3CtlCmd));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send pin D3 control cmd error !");
+        return -4;
+    }
+
+    return 0;
 }
 
-float getAngularY()
+/*****************************************************************
+ * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
+ *   两个输入参数的意义如下:
+ *   const char* path:IMU设备的挂载地址;
+ *   const int baud:IMU模块的串口通信波特率;
+ ****************************************************************/
+int initSerialPort(const char* path, const int baud)
 {
-    return angular[1];
-}
+    port_fd = openSerialPort(path, baud);
+    if(port_fd < 0)
+    {
+        ROS_ERROR("openSerialPort error !");
+        return -1;
+    }
 
-float getAngularZ()
+    return 0;
+}
+/*****************************************
+ * Description:得到三轴加速度信息,输入
+ * 　参数可以为0,1,2分别代表x,y,z轴的
+ * 　加速度信息.
+ *****************************************/
+float getAcc(int flag)
 {
-    return angular[2];
+    return acce[flag];
 }
 
-float getAngleX()
+/******************************************
+ * Description:得到角速度信息,输入参数可
+ * 　以为0,1,2,分别代表x,y,z三轴的角速度
+ *   信息.
+ *****************************************/
+float getAngular(int flag)
 {
-    return angle[0];
+    return angular[flag];
 }
-float getAngleY()
+
+/*******************************************
+ * Description:获取yaw,pitch,roll,输入参数0,
+ * 1,2可以分别获取到roll,pitch,yaw的数据.
+ *******************************************/
+float getAngle(int flag)
 {
-    return angle[1];
+    return angle[flag];
 }
-float getAngleZ()
+
+/********************************************
+ * Description:输入参数0,1,2,3可以分别获取到
+ * 　四元素的q0,q1,q2,q3.但是这里对应的ros中
+ * 　的imu_msg.orientation.w,x,y,z的顺序,
+ * 　这里的q0是对应w参数,1,2,3对应x,y,z.
+ ********************************************/
+float getQuat(int flag)
 {
-    return angle[2];
+    return quater[flag];
 }
 
-int getImuData()
+/*************************************************************
+ * Description:从串口读取数据,然后解析出各有效数据段,这里
+ *  一次性从串口中读取88个字节,然后需要从这些字节中进行
+ *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ *************************************************************/
+int getImuData(void)
 {
-    int ret = 0;
-    ret = recv_data(port_fd, r_buf,  44);
-    if(ret == -1)
+    int data_len = 0;
+    bzero(r_buf, sizeof(r_buf)); //存储新数据前,将缓冲区清零
+
+    //开始从串口中读取数据,并将其保存到r_buf缓冲区中
+    data_len = read(port_fd, r_buf, sizeof(r_buf));
+    if(data_len <= 0)
     {
         ROS_ERROR("read serial port data failed !\n");
         closeSerialPort();
-
         return -1;
     }
 
-    for (int i=0; i<ret; i++)
+    //printf("recv data len:%d\n", data_len);
+    for (int i=0; i<data_len; i++)
     {
+        //printf("0x%02x ", r_buf[i]);
         parse_serialData(r_buf[i]);
     }
+    //printf("\n\n");
 
     return 0;
 }
+

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,76 +1,151 @@
-/****************************************************
- * Copyright:2016-2020 www.corvin.cn ROS小课堂
- * Description:使用串口来读取IMU模块的数据,并通过
- *   topic将数据发布出来.芯片中默认读取出来的加速度数据
- *   单位是g,需要将其转换为ROS中加速度规定的m/s2才能发布.
+/*****************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口来读取IMU模块的数据,并通过ROS话题topic将
+ *   数据发布出来.芯片中默认读取出来的加速度数据单位是g,需要将其
+ *   转换为ROS中加速度规定的m/s2才能发布.
  * Author: corvin
  * History:
  *   20200403: init this file.
  *   20200406:增加发布yaw数据的话题.
  *   20200703:修改三轴加速度数据正负,按照ROS规定沿着各轴
  *     正方向时数据为正,否则为负.单位m/s2。
-*****************************************************/
+ *   20210317:去掉从配置文件中读取的停止位,数据位这些无法
+ *     修改的配置参数,因为都是固定的,无需自己修改配置.
+ *   20210318:增加订阅话题,话题消息格式Empty,可以将yaw角度归零.
+ *     增加使用service方式可将yaw角度归零.
+ *   20210319:增加使用service方式可以修改imu模块串口通信波特率.
+ *     注意修改了波特率后,需要将IMU模块短开使用新波特率连接.
+ *   20210321:增加可以设置D0,D1,D2,D3共4个IO引脚输出数字高低电平
+ *     的服务,这里的高电平为3.3V.增加通过服务方式获取yaw数据.
+*****************************************************************/
 #include <ros/ros.h>
 #include <tf/tf.h>
-#include "imu_data.h"
+#include <imu_data.h>
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/Float32.h>
-#include "serial_imu_hat_6dof/GetYawData.h"
+#include <std_msgs/Empty.h>
+#include "serial_imu_hat_6dof/setYawZero.h"
+#include "serial_imu_hat_6dof/setBaudRate.h"
+#include "serial_imu_hat_6dof/setPinOutHL.h"
+#include "serial_imu_hat_6dof/getYawData.h"
 
-float response_yaw;
-bool return_yaw_data(serial_imu_hat_6dof::GetYawData::Request &req,serial_imu_hat_6dof::GetYawData::Response &res){
-    res.yaw = response_yaw;
+
+static float g_yawData; //全局变量,存储当前yaw值,可以通过服务来得到该值
+
+/**********************************************************
+ * Description:将yaw角度归零的话题回调函数,只要往话题中
+ *   发布一条Empty消息,即可将yaw角度归零.
+ *********************************************************/
+void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
+{
+    makeYawZero();
+}
+
+/******************************************************************
+ * Description:使用service方式来进行yaw角度归零,这里是服务的回调
+ *   函数,当有客户端发送yaw归零的服务时,自动调用该函数,如果正确
+ *   执行了yaw归零,则response反馈为0,如果为其他负数则表明yaw归零
+ *   命令执行失败.
+ *****************************************************************/
+bool yawZeroService(serial_imu_hat_6dof::setYawZero::Request &req,
+                    serial_imu_hat_6dof::setYawZero::Response &res)
+{
+    res.status = makeYawZero();
+    return true;
+}
+
+/********************************************************************
+ * Description:使用服务调用方式来修改与IMU模块通信的波特率,这里根据
+ *   客户端发送过来的request来决定修改的波特率,请求内容对应的波特率:
+ *   请求发送1,修改波特率为9600;
+ *   请求发送2,修改波特率为57600;
+ *   请求发送3,修改波特率为115200;
+ *   当修改IMU模块的波特率成功后函数会返回0,若返回其他负值,则表示
+ *   修改波特率失败,可以重新调用服务来修改波特率.
+ *******************************************************************/
+bool setBaudService(serial_imu_hat_6dof::setBaudRate::Request &req,
+                    serial_imu_hat_6dof::setBaudRate::Response &res)
+{
+    res.status = setIMUBaudRate(req.index);
+    return true;
+}
+
+/********************************************************************
+ * Description:控制IMU板的D0,D1,D2,D3共4个引脚的输出数字高低电平信号,
+ *   这里的服务request一次性控制4个引脚的状态,对应引脚0,则输出低电平,
+ *   请求为1,则输出高电平.根据反馈状态status,可以得知控制的结果,如果
+ *   反馈0,表明控制成功,负值的话控制失败.这里的高电平为3.3V.
+ ********************************************************************/
+bool pinOutHLService(serial_imu_hat_6dof::setPinOutHL::Request &req,
+                     serial_imu_hat_6dof::setPinOutHL::Response &res)
+{
+    res.status = setIMUPinOutHL(req.D0, req.D1, req.D2, req.D3);
     return true;
 }
+
+bool getYawDataService(serial_imu_hat_6dof::getYawData::Request &req,
+                       serial_imu_hat_6dof::getYawData::Response &res)
+{
+    res.yaw = g_yawData;
+    return true;
+}
+
 int main(int argc, char **argv)
 {
     float yaw, pitch, roll;
     std::string imu_dev;
     int baud = 0;
-    int dataBit = 0;
-    std::string parity;
-    int stopBit = 0;
 
     std::string link_name;
     std::string imu_topic;
     std::string temp_topic;
-    std::string yaw_topic;
+    std::string yaw_pub_topic;
+    std::string yaw_zero_topic;
+    std::string yaw_zero_service;
+    std::string baud_update_service;
+    std::string pin_outHL_service;
+    std::string yaw_data_service;
+
     int pub_hz = 0;
-    std::string yaw_service;
     float degree2Rad = 3.1415926/180.0;
     float acc_factor = 9.806;
 
     ros::init(argc, argv, "imu_data_pub_node");
     ros::NodeHandle handle;
 
-    ros::param::get("~imu_dev", imu_dev);
+    //从yaml配置文件中读取配置参数
+    ros::param::get("~imu_dev",   imu_dev);
     ros::param::get("~baud_rate", baud);
-    ros::param::get("~data_bits", dataBit);
-    ros::param::get("~parity", parity);
-    ros::param::get("~stop_bits", stopBit);
-
+    ros::param::get("~pub_hz",    pub_hz);
     ros::param::get("~link_name", link_name);
-    ros::param::get("~pub_data_topic", imu_topic);
-    ros::param::get("~pub_temp_topic", temp_topic);
-    ros::param::get("~yaw_topic", yaw_topic);
-    ros::param::get("~pub_hz", pub_hz);
-
-    ros::param::get("~yaw_service", yaw_service);
-
-
-
-    int ret = initSerialPort(imu_dev.c_str(), baud, dataBit, parity.c_str(), stopBit);
+    ros::param::get("~pub_data_topic",   imu_topic);
+    ros::param::get("~pub_temp_topic",   temp_topic);
+    ros::param::get("~yaw_zero_topic",   yaw_zero_topic);
+    ros::param::get("~yaw_zero_service", yaw_zero_service);
+    ros::param::get("~yaw_pub_topic",    yaw_pub_topic);
+    ros::param::get("~baud_update_srv",  baud_update_service);
+    ros::param::get("~pin_outHL_srv",    pin_outHL_service);
+    ros::param::get("~yaw_data_srv",     yaw_data_service);
+
+    //初始化imu模块串口,根据设备号和波特率进行连接
+    int ret = initSerialPort(imu_dev.c_str(), baud);
     if(ret < 0)
     {
         ROS_ERROR("init serial port error !");
         closeSerialPort();
         return -1;
     }
-    ROS_INFO("IMU module is working... ");
+    ROS_INFO("IMU module is working...");
+
+    //定义四个服务,分别是更新波特率,yaw角度归零,控制引脚输出高低电平和获取yaw当前角度
+    ros::ServiceServer setBaudSrv= handle.advertiseService(baud_update_service, setBaudService);
+    ros::ServiceServer setyawSrv = handle.advertiseService(yaw_zero_service,  yawZeroService);
+    ros::ServiceServer pinOutSrv = handle.advertiseService(pin_outHL_service, pinOutHLService);
+    ros::ServiceServer getYawSrv = handle.advertiseService(yaw_data_service,  getYawDataService);
 
+    ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
     ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 1);
-    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_topic, 1);
-    ros::ServiceServer yaw_srv = handle.advertiseService(yaw_service, return_yaw_data);
+    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 1);
     ros::Rate loop_rate(pub_hz);
 
     sensor_msgs::Imu imu_msg;
@@ -82,34 +157,37 @@ int main(int argc, char **argv)
             break;
 
         imu_msg.header.stamp = ros::Time::now();
-        roll  = getAngleX()*degree2Rad;
-        pitch = getAngleY()*degree2Rad;
-        yaw   = getAngleZ()*degree2Rad;
+        roll  = getAngle(0)*degree2Rad;
+        pitch = getAngle(1)*degree2Rad;
+        yaw   = getAngle(2)*degree2Rad;
         if(yaw >= 3.1415926)
             yaw -= 6.2831852;
 
-        response_yaw = yaw;
-        //publish yaw data
+        g_yawData = yaw; //获取yaw值,可以通过服务来得到该值
+        //publish yaw data to topic
         yaw_msg.data = yaw;
         yaw_pub.publish(yaw_msg);
 
         //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
-        imu_msg.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        imu_msg.orientation.x = getQuat(1);
+        imu_msg.orientation.y = getQuat(2);
+        imu_msg.orientation.z = getQuat(3);
+        imu_msg.orientation.w = getQuat(0);
         imu_msg.orientation_covariance = {0.0025, 0, 0,
                                           0, 0.0025, 0,
                                           0, 0, 0.0025};
 
-        imu_msg.angular_velocity.x = getAngularX()*degree2Rad;
-        imu_msg.angular_velocity.y = getAngularY()*degree2Rad;
-        imu_msg.angular_velocity.z = getAngularZ()*degree2Rad;
+        imu_msg.angular_velocity.x = getAngular(0)*degree2Rad;
+        imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
+        imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
         imu_msg.angular_velocity_covariance = {0.02, 0, 0,
                                                0, 0.02, 0,
                                                0, 0, 0.02};
 
-        //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAccX(), getAccY(), getAccZ());
-        imu_msg.linear_acceleration.x = -getAccX()*acc_factor;
-        imu_msg.linear_acceleration.y = -getAccY()*acc_factor;
-        imu_msg.linear_acceleration.z = -getAccZ()*acc_factor;
+        //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAcc(0), getAcc(1), getAcc(2));
+        imu_msg.linear_acceleration.x = -getAcc(0)*acc_factor;
+        imu_msg.linear_acceleration.y = -getAcc(1)*acc_factor;
+        imu_msg.linear_acceleration.z = -getAcc(2)*acc_factor;
         imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
                                                    0, 0.04, 0,
                                                    0, 0, 0.04};
@@ -119,7 +197,7 @@ int main(int argc, char **argv)
         loop_rate.sleep();
     }
 
-    closeSerialPort();
+    closeSerialPort(); //关闭与IMU模块的串口连接
     return 0;
 }
 

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/setBaudRate.srv

@@ -0,0 +1,3 @@
+int32 index
+---
+int32 status

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/setPinOutHL.srv

@@ -0,0 +1,6 @@
+int32 D0
+int32 D1
+int32 D2
+int32 D3
+---
+int32 status

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/setYawZero.srv

@@ -0,0 +1,2 @@
+---
+int32 status