增加可以修改串口通信波特率的服务,注意修改波特率后需要使用新波特率连接IMU

README.md

@@ -2,12 +2,21 @@
 
 为树莓派IMU扩展板创建的代码仓库，直接将该软件包源码放在ROS工作区src目录下即可。然后使用catkin_make就可以进行编译了。
 
+----
 ## 使用串口发布IMU数据
+当编译完成后可以使用如下命令启动发布imu数据:
 `roslaunch serial_imu_hat_6dof serial_imu_hat.launch`
+* 发布imu数据的话题名: /imu_data
+* 发布imu模块温度的话题名: /imu_temp
+* 发布yaw角度的话题名: /yaw_data
+* 可将yaw角度归零的话题名: /yaw_zero_topic
+* 可将yaw角度归零的服务名: /yaw_zero_service
 
+----
 ## 使用IIC发布IMU数据
 `roslaunch rasp_imu_hat_6dof imu_data_pub.launch`
 
+----
 ## 在Rviz中显示查看IMU效果
 `roslaunch rasp_imu_hat_6dof imu_rviz_display.launch`
 

serial_imu_hat_6dof/CMakeLists.txt

@@ -53,6 +53,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_service_files(
   FILES
   setYawZero.srv
+  setBaudRate.srv
 )
 
 ## Generate actions in the 'action' folder

serial_imu_hat_6dof/cfg/param.yaml

@@ -1,38 +1,40 @@
-########################################################
+##########################################################
 # Copyright: 2016-2021 www.corvin.cn ROS小课堂
-# Description:使用串口进行通信时的配置信息.可以配置
-# 的各参数如下:
-#   imu_dev:指示串口设备挂在点,如果使用usb转串口
-#     模块,都是写ttyUSB0,1,2等.如果使用树莓派的
+# Description:使用串口与IMU模块进行通信时的配置信息.
+# 可以配置的各参数如下:
+#  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
+#     模块进行通信,都是写ttyUSB0,1,2等.如果使用树莓派的
 #     GPIO排针处串口,则使用ttyAMA0.需要注意是这个ttyAMA0
 #     默认是给树莓派蓝牙使用的,所以要想使用该硬件串口,
 #     就需要在raspi-config中关闭串口登录功能,打开硬件
 #     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
-#   baud_rate:串口通信波特率,旧模块的波特率为9600,
+#  baud_rate:串口通信波特率,旧模块的波特率为9600,
 #     新版本的波特率为57600,如果串口没数据可以修改.
-#   pub_data_topic:发布imu数据的ROS话题名.
-#   pub_temp_topic:发布imu模块温度的ROS话题名.
-#   link_name:imu模块的link名.
-#   pub_hz:话题发布的数据频率,默认设置为20Hz.
-#   yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
-#   yaw_pub_topic:将yaw信息通过该话题发送出来.
+#  pub_data_topic:发布imu数据的ROS话题名.
+#  pub_temp_topic:发布imu模块温度的ROS话题名.
+#  link_name:imu模块的link名.
+#  pub_hz:话题发布的数据频率,默认设置为20Hz.
+#  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
+#  yaw_pub_topic:将yaw信息通过该话题发送出来.
 # Author: corvin
 # History:
 #   20200402:init this file.
 #   20200406:增加直接发布yaw数据的ROS话题.
 #   20210317:去掉数据位,奇偶校验,停止位参数,这几个
 #     参数模块已经固定死了,无需修改.
-#   20200318:增加yaw角度归零的话题配置参数yaw_zero_topic.
+#   20210318:增加yaw角度归零的话题配置参数yaw_zero_topic.
 #     增加使用服务方式将yaw归零的名称yaw_zero_service.
-#########################################################
-imu_dev: /dev/ttyAMA0
+#   20210319:增加修改串口波特率的配置参数baud_update_srv.
+##########################################################
+imu_dev: /dev/ttyUSB0
 baud_rate: 57600
+pub_hz: 20
 
+link_name: base_imu_link
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
 yaw_pub_topic: yaw_data
 yaw_zero_topic: yaw_zero_topic
 yaw_zero_service: yaw_zero_service
-link_name: base_imu_link
-pub_hz: 20
+baud_update_srv: baud_update_service
 

serial_imu_hat_6dof/include/imu_data.h

@@ -4,6 +4,7 @@
  * Author: corvin
  * History:
  *   20210318:init this file.
+ *   20210319:增加修改串口通信波特率的函数.
  *******************************************************/
 #ifndef _IMU_DATA_H_
 #define _IMU_DATA_H_
@@ -25,5 +26,6 @@ float getAngleX();
 float getAngleY();
 float getAngleZ();
 
-int makeYawZero();
+int makeYawZero(void);
+int setIMUBaudRate(const int flag);
 #endif

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -6,7 +6,8 @@
  *   20200401:init this file.
  *   20200702:将读取到的各数据的高低自己合成后要转换为short类型,
  *     这样才能进行/32768.0*16.0运算.
- *  20210318:增加可以将yaw角度归零的函数yawZeroCmd().
+ *   20210318:增加可以将yaw角度归零的函数yawZeroCmd().
+ *   20210319:增加可以修改串口通信波特率的函数setIMUBaudRate().
 ******************************************************************/
 #include<ros/ros.h>
 #include<stdio.h>
@@ -19,7 +20,7 @@
 #include<sys/types.h>
 
 
-unsigned char r_buf[88];  //一次从串口中读取的数据存储缓冲区
+static unsigned char r_buf[88];  //一次从串口中读取的数据存储缓冲区
 static int port_fd = -1; //串口打开时的文件描述符
 float acce[3],angular[3],angle[3],quater[4];
 
@@ -39,7 +40,7 @@ int openSerialPort(const char *path, const int baud)
     fd = open(path, O_RDWR|O_NOCTTY);
     if (-1 == fd)
     {
-        ROS_ERROR("Open imu dev error:%s, baud:%d", path, baud);
+        ROS_ERROR("Open IMU dev error:%s, baud:%d", path, baud);
         return -1;
     }
 
@@ -68,7 +69,7 @@ int openSerialPort(const char *path, const int baud)
            cfsetospeed(&terminfo, B115200);
            break;
 
-       default://设置默认波特率9600
+       default: //设置默认波特率9600
            cfsetispeed(&terminfo, B9600);
            cfsetospeed(&terminfo, B9600);
            break;
@@ -110,12 +111,25 @@ int closeSerialPort()
     return ret;
 }
 
-/*****************************************************
- * Description:向IMU模块的串口中发送数据.
- ****************************************************/
+/*****************************************************************
+ * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
+ *   发送控制命令,最后就是需要发送命令保存刚才的操作.解锁命令是固
+ *   定的0xFF 0xAA 0x69 0x88 0xB5,保存命令也是固定的0xFF 0xAA 0x00
+ *   0x00 0x00.
+ *****************************************************************/
 int send_data(int fd, unsigned char *send_buffer, int length)
 {
-    int len = -1;
+    int ret = -1;
+    unsigned char unLockCmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
+    unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
+
+    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才能发送其他配置命令
 
     #if 0 //打印发送给IMU模块的数据,用于调试
     printf("Send %d byte to IMU:", length);
@@ -126,8 +140,24 @@ int send_data(int fd, unsigned char *send_buffer, int length)
     printf("\n");
     #endif
 
-    len = write(fd, send_buffer, length*sizeof(unsigned char));
-    return len;
+    //发送控制命令
+    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;
 }
 
 /*******************************************************
@@ -213,42 +243,57 @@ void parse_serialData(unsigned char chr)
 }
 
 /********************************************************************
- * Description:将yaw角度归零,这里需要通过串口发送的命令分为3个步骤,
- *  首先需要进行解锁操作,就是固定的命令:0xFF,0xAA,0x69,0x88,0xB5;
- *  其次就是发送z轴角度归零的固定命令:0xFF 0xAA 0x01 0x04 0x00;
- *  最后就是保存操作,也是固定命令:0xFF 0xAA 0x00 0x00 0x00;
+ * Description:将yaw角度归零,这里需要通过串口发送的命令如下所示,
+ *  发送z轴角度归零的固定命令:0xFF 0xAA 0x01 0x04 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};
+    unsigned char yawZeroCmd[5] = {0xFF, 0xAA, 0x01, 0x04, 0x00};
 
-    ret = send_data(port_fd, unLockCmd, sizeof(unLockCmd));
-    if(ret != sizeof(unLockCmd))
+    ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
+    if(ret != 0)
     {
-        ROS_ERROR("Send yaw zero unlock cmd error !");
+        ROS_ERROR("Send yaw zero control cmd error !");
         return -1;
     }
-    ros::Duration(0.01).sleep(); //delay 10ms才能发送其他配置命令
+    ROS_INFO("set yaw to zero radian successfully !");
+    return 0;
+}
 
-    ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
-    if(ret != sizeof(yawZeroCmd))
+/******************************************************************
+ * Description:更新串口通信波特率,根据输入参数的不同,修改为不同的
+ *   波特率,1修改为9600,2修改为57600,3修改为115200.修改波特率就是
+ *   发送3条指令,第一条命令是解除锁定,然后是发送串口波特率更新命令,
+ *   最后是保存操作的指令.
+ ******************************************************************/
+int setIMUBaudRate(const int flag)
+{
+    int ret = 0;
+    unsigned char baudRate[5] = {0xFF, 0xAA, 0x04, 0x00, 0x00};
+
+    switch(flag)
     {
-        ROS_ERROR("Send yaw zero control cmd error !");
-        return -2;
+        case 1: //set baud 9600 bps
+            baudRate[3] = 0x02;
+            break;
+        case 2: //set baud 57600 bps
+            baudRate[3] = 0x05;
+            break;
+        case 3: //set baud 115200 bps
+            baudRate[3] = 0x06;
+            break;
+        default: //set baud 57600 bps
+            baudRate[3] = 0x05;
+            break;
     }
-
-    ret = send_data(port_fd, saveCmd, sizeof(saveCmd));
-    if(ret != sizeof(saveCmd))
+    ret = send_data(port_fd, baudRate, sizeof(baudRate));
+    if(ret != 0)
     {
-        ROS_ERROR("Send yaw zero save cmd error !");
-        return -3;
+        ROS_ERROR("Send baud rate update cmd error !");
+        return -1;
     }
-    ros::Duration(0.1).sleep(); //delay 100ms才能进行其他操作
-    ROS_INFO("set yaw to zero radian successfully !");
-
+    ROS_INFO("set new baud rate successfully !");
     return 0;
 }
 

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,4 +1,4 @@
-/************************************************************
+/****************************************************************
  * Copyright:2016-2021 www.corvin.cn ROS小课堂
  * Description:使用串口来读取IMU模块的数据,并通过ROS话题
  *   topic将数据发布出来.芯片中默认读取出来的加速度数据
@@ -12,7 +12,10 @@
  *   20210317:去掉从配置文件中读取的停止位,数据位这些无法
  *     修改的配置参数,因为都是固定的,无需自己修改配置.
  *   20210318:增加订阅话题,话题消息格式Empty,可以将yaw角度归零.
-**************************************************************/
+ *     增加使用service方式可将yaw角度归零.
+ *   20210319:增加使用service方式可以修改imu模块串口通信波特率.
+ *     注意修改了波特率后,需要将IMU模块短开使用新波特率连接.
+****************************************************************/
 #include <ros/ros.h>
 #include <tf/tf.h>
 #include "imu_data.h"
@@ -20,9 +23,10 @@
 #include <std_msgs/Float32.h>
 #include <std_msgs/Empty.h>
 #include "serial_imu_hat_6dof/setYawZero.h"
+#include "serial_imu_hat_6dof/setBaudRate.h"
 
 
-/*********************************************************
+/**********************************************************
  * Description:将yaw角度归零的话题回调函数,只要往话题中
  *   发布一条Empty消息,即可将yaw角度归零.
  *********************************************************/
@@ -31,7 +35,7 @@ void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
     makeYawZero();
 }
 
-/*****************************************************************
+/******************************************************************
  * Description:使用service方式来进行yaw角度归零,这里是服务的回调
  *   函数,当有客户端发送yaw归零的服务时,自动调用该函数,如果正确
  *   执行了yaw归零,则response反馈为0,如果为其他负数则表明yaw归零
@@ -44,6 +48,22 @@ bool yawZeroService(serial_imu_hat_6dof::setYawZero::Request &req,
     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;
+}
+
 int main(int argc, char **argv)
 {
     float yaw, pitch, roll;
@@ -56,6 +76,7 @@ int main(int argc, char **argv)
     std::string yaw_pub_topic;
     std::string yaw_zero_topic;
     std::string yaw_zero_service;
+    std::string baud_update_service;
 
     int pub_hz = 0;
     float degree2Rad = 3.1415926/180.0;
@@ -67,13 +88,14 @@ int main(int argc, char **argv)
     //从yaml配置文件中读取配置参数
     ros::param::get("~imu_dev",   imu_dev);
     ros::param::get("~baud_rate", baud);
+    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_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("~pub_hz",    pub_hz);
+    ros::param::get("~baud_update_srv", baud_update_service);
 
     //初始化imu模块串口,根据设备号和波特率进行连接
     int ret = initSerialPort(imu_dev.c_str(), baud);
@@ -85,7 +107,10 @@ int main(int argc, char **argv)
     }
     ROS_INFO("IMU module is working... ");
 
-    ros::ServiceServer service = handle.advertiseService(yaw_zero_service, yawZeroService);
+    //定义两个服务,分别是更新波特率和yaw角度归零
+    ros::ServiceServer baudSrv = handle.advertiseService(baud_update_service, setBaudService);
+    ros::ServiceServer yawSrv  = handle.advertiseService(yaw_zero_service, yawZeroService);
+
     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_pub_topic, 1);
@@ -106,7 +131,7 @@ int main(int argc, char **argv)
         if(yaw >= 3.1415926)
             yaw -= 6.2831852;
 
-        //publish yaw data
+        //publish yaw data to topic
         yaw_msg.data = yaw;
         yaw_pub.publish(yaw_msg);
 

serial_imu_hat_6dof/srv/setBaudRate.srv

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