增加串口命令可以将yaw角度归零的,通过向话题中发布数据即可

serial_imu_hat_6dof/cfg/param.yaml

@@ -1,4 +1,4 @@
-####################################################
+########################################################
 # Copyright: 2016-2021 www.corvin.cn ROS小课堂
 # Description:使用串口进行通信时的配置信息.可以配置
 # 的各参数如下:
@@ -10,20 +10,23 @@
 #   pub_data_topic:发布imu数据的ROS话题名.
 #   pub_temp_topic:发布imu模块温度的ROS话题名.
 #   link_name:imu模块的link名.
-#   pub_hz:话题发布的数据频率.
+#   pub_hz:话题发布的数据频率,默认设置为20Hz.
+#   yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
 # Author: corvin
 # History:
 #   20200402:init this file.
 #   20200406:增加直接发布yaw数据的ROS话题.
 #   20210317:去掉数据位,奇偶校验,停止位参数,这几个
 #     参数模块已经固定死了,无需修改.
-####################################################
+#   20200318:增加yaw角度归零的话题配置参数yaw_zero_topic.
+#########################################################
 imu_dev: /dev/ttyS0
 baud_rate: 57600
 
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
 yaw_topic: yaw_data
+yaw_zero_topic: yaw_zero
 link_name: base_imu_link
-pub_hz: 10
+pub_hz: 20
 

serial_imu_hat_6dof/include/imu_data.h

@@ -1,3 +1,10 @@
+/*******************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:操作imu模块的代码头文件.
+ * Author: corvin
+ * History:
+ *   20210318:init this file.
+ *******************************************************/
 #ifndef _IMU_DATA_H_
 #define _IMU_DATA_H_
 
@@ -18,4 +25,5 @@ float getAngleX();
 float getAngleY();
 float getAngleZ();
 
+int makeYawZero();
 #endif

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -6,6 +6,7 @@
  *   20200401:init this file.
  *   20200702:将读取到的各数据的高低自己合成后要转换为short类型,
  *     这样才能进行/32768.0*16.0运算.
+ *  20210318:增加可以将yaw角度归零的函数yawZeroCmd().
 ******************************************************************/
 #include<ros/ros.h>
 #include<stdio.h>
@@ -22,13 +23,13 @@ unsigned char r_buf[88];  //一次从串口中读取的数据存储缓冲区
 static int port_fd = -1; //串口打开时的文件描述符
 float acce[3],angular[3],angle[3],quater[4];
 
-/****************************************************
+/******************************************************
  * Description:打开IMU模块串口,输入两个参数即可连接.
  *   open()打开失败时,返回-1,成功打开时返回文件描述符.
  *   各参数意义如下:
  *   const char *path:IMU设备的挂载地址;
  *   const int baud:串口通讯的波特率;
- ***************************************************/
+ *****************************************************/
 int openSerialPort(const char *path, const int baud)
 {
     int fd = 0;
@@ -110,12 +111,23 @@ int closeSerialPort()
 }
 
 /*****************************************************
- * Description:向串口中发送数据.
+ * Description:向IMU模块的串口中发送数据.
  ****************************************************/
-int send_data(int fd, char *send_buffer, int length)
+int send_data(int fd, unsigned char *send_buffer, int length)
 {
-	length = write(fd, send_buffer, length*sizeof(unsigned char));
-	return length;
+    int len = -1;
+
+    #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
+
+    len = write(fd, send_buffer, length*sizeof(unsigned char));
+    return len;
 }
 
 /*******************************************************
@@ -127,12 +139,12 @@ int recv_data(int fd, unsigned char* recv_buffer, int len)
     return length;
 }
 
-/************************************************************
+/**************************************************************
  * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
  *   解析读取其中的44个字节,正好是4帧数据,每一帧数据是11个字节.
  *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
  *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
- ***********************************************************/
+ *************************************************************/
 void parse_serialData(unsigned char chr)
 {
     static unsigned char chrBuf[100];
@@ -163,9 +175,11 @@ void parse_serialData(unsigned char chr)
         return;
     }
 
-    //for(i=0;i<11; i++)
-    //  printf("0x%x ",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])
@@ -198,6 +212,46 @@ void parse_serialData(unsigned char chr)
     chrCnt = 0;
 }
 
+/********************************************************************
+ * Description:将yaw角度归零,这里需要通过串口发送的命令分为3个步骤,
+ *  首先需要进行解锁操作,就是固定的命令:0xFF,0xAA,0x69,0x88,0xB5;
+ *  其次就是发送z轴角度归零的固定命令:0xFF 0xAA 0x01 0x04 0x00;
+ *  最后就是保存操作,也是固定命令:0xFF 0xAA 0x00 0x00 0x00;
+ *******************************************************************/
+int makeYawZero(void)
+{
+    int ret = 0;
+    unsigned char unLockCmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
+    unsigned char yawZeroCmd[5]= {0xFF, 0xAA, 0x01, 0x04, 0x00};
+    unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
+
+    ret = send_data(port_fd, unLockCmd, sizeof(unLockCmd));
+    if(ret != sizeof(unLockCmd))
+    {
+        ROS_ERROR("Send yaw zero unlock cmd error !");
+        return -1;
+    }
+    ros::Duration(0.01).sleep(); //delay 10ms才能发送其他配置命令
+
+    ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
+    if(ret != sizeof(yawZeroCmd))
+    {
+        ROS_ERROR("Send yaw zero control cmd error !");
+        return -2;
+    }
+
+    ret = send_data(port_fd, saveCmd, sizeof(saveCmd));
+    if(ret != sizeof(saveCmd))
+    {
+        ROS_ERROR("Send yaw zero save cmd error !");
+        return -3;
+    }
+    ros::Duration(0.1).sleep(); //delay 100ms才能进行其他操作
+    ROS_INFO("set yaw to zero radian successfully !");
+
+    return 0;
+}
+
 /*****************************************************************
  * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
  *   两个输入参数的意义如下:

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,4 +1,4 @@
-/********************************************************
+/************************************************************
  * Copyright:2016-2021 www.corvin.cn ROS小课堂
  * Description:使用串口来读取IMU模块的数据,并通过ROS话题
  *   topic将数据发布出来.芯片中默认读取出来的加速度数据
@@ -11,14 +11,25 @@
  *     正方向时数据为正,否则为负.单位m/s2。
  *   20210317:去掉从配置文件中读取的停止位,数据位这些无法
  *     修改的配置参数,因为都是固定的,无需自己修改配置.
-**********************************************************/
+ *   20210318:增加订阅话题,话题消息格式Empty,可以将yaw角度归零.
+**************************************************************/
 #include <ros/ros.h>
 #include <tf/tf.h>
 #include "imu_data.h"
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/Float32.h>
+#include <std_msgs/Empty.h>
 
 
+/*********************************************************
+ * Description:将yaw角度归零的话题回调函数,只要往话题中
+ *   发布一条Empty消息,即可将yaw角度归零.
+ *********************************************************/
+void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
+{
+    makeYawZero();
+}
+
 int main(int argc, char **argv)
 {
     float yaw, pitch, roll;
@@ -29,6 +40,8 @@ int main(int argc, char **argv)
     std::string imu_topic;
     std::string temp_topic;
     std::string yaw_topic;
+    std::string yaw_zero_topic;
+
     int pub_hz = 0;
     float degree2Rad = 3.1415926/180.0;
     float acc_factor = 9.806;
@@ -36,12 +49,13 @@ int main(int argc, char **argv)
     ros::init(argc, argv, "imu_data_pub_node");
     ros::NodeHandle handle;
 
+    //从yaml配置文件中读取配置参数
     ros::param::get("~imu_dev",   imu_dev);
     ros::param::get("~baud_rate", baud);
-
     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_zero_topic", yaw_zero_topic);
     ros::param::get("~yaw_topic", yaw_topic);
     ros::param::get("~pub_hz",    pub_hz);
 
@@ -55,6 +69,7 @@ int main(int argc, char **argv)
     }
     ROS_INFO("IMU module is working... ");
 
+    ros::Subscriber yaw_zero_sub = 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::Rate loop_rate(pub_hz);
@@ -104,7 +119,7 @@ int main(int argc, char **argv)
         loop_rate.sleep();
     }
 
-    closeSerialPort(); //关闭串口连接
+    closeSerialPort(); //关闭与IMU模块的串口连接
     return 0;
 }