删除可以更新波特率的服务,同时优化代码结构

.gitignore

@@ -4,6 +4,8 @@ __pycache__/
 *.py[cod]
 *$py.class
 
+.vscode/
+
 # C extensions
 *.so
 

serial_6dof_imu/CMakeLists.txt

@@ -52,8 +52,8 @@ find_package(catkin REQUIRED COMPONENTS
 add_service_files(
   FILES
   getYawData.srv
-  setBaudRate.srv
   setYawZero.srv
+  setIICAddr.srv
 )
 
 ## Generate added messages and services with any dependencies listed here

serial_6dof_imu/cfg/param.yaml

@@ -4,8 +4,6 @@
 # 可以配置的各参数如下(这里需要注意参数名称和参数之间要有空格隔开):
 #  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
 #     模块进行通信,都是写ttyUSB0,1,2.
-#  baud_rate:串口通信波特率,默认为115200,如果查看发布的
-#  　　话题中没有IMU数据可以尝试换波特率为9600或57600或115200.
 #  pub_topic_hz:话题发布的数据频率,默认设置为100Hz.
 #  imu_link_name:imu模块的link名,该名称一般在urdf模型中定义.
 #  pub_data_topic:发布imu数据的ROS话题名.
@@ -15,15 +13,12 @@
 #  roll_pub_topic:将roll数据通过该话题发布出来.
 #  yaw_zero_topic:通过向该话题发布空消息即可将yaw角度归零.
 #  yaw_zero_service:通过向该服务发送空请求即可将yaw归零.
-#  baud_update_srv:通过向该服务发送1,2,3可以将波特率更新为
-#    9600bps,57600bps,115200bps,三种通信波特率.
-#  yaw_data_srv:可以获取到yaw当前角度的服务.
+#  get_yaw_data_srv:可以获取到yaw当前角度的服务.
 # Author: corvin
 # History:
 #   20211122:init this file.
 ###########################################################
 imu_dev: /dev/ttyUSB0
-baud_rate: 115200
 pub_topic_hz: 100
 
 imu_link_name: base_imu_link
@@ -34,5 +29,5 @@ roll_pub_topic: roll_data
 yaw_zero_topic: yaw_zero_topic
 
 yaw_zero_service: /serial_imu_service/setYawZero_service
-baud_update_srv: /serial_imu_service/updateBaud_service
-yaw_data_srv: /serial_imu_service/getYawData_service
+get_yaw_data_srv: /serial_imu_service/getYawData_service
+set_iic_addr_srv: /serial_imu_service/setIICAddr_service

serial_6dof_imu/include/imu_data.h

@@ -3,14 +3,12 @@
  * Description:使用串口操作读取imu模块的代码头文件.
  * Author: corvin
  * History:
- *   20210318:init this file.
- *   20210319:增加修改串口通信波特率的函数.
- *   20210321:增加控制IMU引脚数字高低电平的函数.
+ *   20211122:init this file.
  *******************************************************/
 #ifndef _IMU_DATA_H_
 #define _IMU_DATA_H_
 
-int initSerialPort(const char* path, const int baud);
+int initSerialPort(const char* path);
 
 int getImuData(void);
 int closeSerialPort(void);
@@ -21,6 +19,5 @@ 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);
+int updateIICAddr(std::string input);
 #endif

serial_6dof_imu/src/proc_serial_data.cpp

@@ -15,93 +15,15 @@
 #include<sys/time.h>
 #include<sys/types.h>
 
-#define  BYTE_CNT      88  //每次从串口中读取的字节数
+#define  BYTE_CNT      55  //每次从串口中读取的字节数
 #define  ACCE_CONST    0.000488281   //用于计算加速度的常量16.0/32768.0
 #define  ANGULAR_CONST 0.061035156   //用于计算角速度的常量2000.0/32768.0
 #define  ANGLE_CONST   0.005493164   //用于计算欧拉角的常量180.0/32768.0
 
 static unsigned char r_buf[BYTE_CNT];  //一次从串口中读取的数据存储缓冲区
-static int port_fd = -1; //串口打开时的文件描述符
+static int port_fd = -1;  //串口打开时的文件描述符
 static 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;
-    struct termios terminfo;
-    bzero(&terminfo, sizeof(terminfo));
-
-    fd = open(path, O_RDWR|O_NOCTTY);
-    if (-1 == fd)
-    {
-        ROS_ERROR("Open 6DOF IMU dev error:%s, baud:%d", path, baud);
-        return -1;
-    }
-
-    //判断当前连接的设备是否为终端设备
-    if (isatty(STDIN_FILENO) == 0)
-    {
-        ROS_ERROR("IMU dev isatty error !");
-        return -2;
-    }
-
-    /*设置串口通信波特率-bps*/
-    switch(baud)
-    {
-        case 9600:
-           cfsetispeed(&terminfo, B9600); //设置输入速度
-           cfsetospeed(&terminfo, B9600); //设置输出速度
-           break;
-
-        case 57600:
-           cfsetispeed(&terminfo, B57600);
-           cfsetospeed(&terminfo, B57600);
-           break;
-
-       case 115200:
-           cfsetispeed(&terminfo, B115200);
-           cfsetospeed(&terminfo, B115200);
-           break;
-
-       default: //设置默认波特率9600
-           cfsetispeed(&terminfo, B9600);
-           cfsetospeed(&terminfo, B9600);
-           break;
-    }
-
-    //设置数据位 - 8 bit
-    terminfo.c_cflag |= CLOCAL|CREAD;
-    terminfo.c_cflag &= ~CSIZE;
-    terminfo.c_cflag |= CS8;
-
-    //不设置奇偶校验位 - N
-    terminfo.c_cflag &= ~PARENB;
-
-    //设置停止位 - 1
-    terminfo.c_cflag &= ~CSTOPB;
-
-    //设置等待时间和最小接收字符
-    terminfo.c_cc[VTIME] = 0;
-    terminfo.c_cc[VMIN]  = 1;
-
-    //清除串口缓存的数据
-    tcflush(fd, TCIFLUSH);
-
-    if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
-    {
-        ROS_ERROR("set imu serial port attr error !");
-        return -3;
-    }
-
-    return fd;
-}
-
 /**************************************
  * Description:关闭串口文件描述符.
  *************************************/
@@ -112,24 +34,48 @@ int closeSerialPort(void)
 }
 
 /*****************************************************************
- * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
- *   发送控制命令,最后就是需要发送命令保存刚才的操作.解锁命令是固
- *   定的0xFF 0xAA 0x69 0x88 0xB5,保存命令也是固定的0xFF 0xAA 0x00
- *   0x00 0x00.
- *****************************************************************/
-int send_data(int fd, unsigned char *send_buffer, int length)
+ * Description:向IMU模块发送解锁命令,发送完命令需要延迟250ms.
+ ****************************************************************/
+static int send_unlockCmd(int fd)
 {
     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才能发送其他配置命令
+    ros::Duration(0.25).sleep(); //delay 250ms才能发送其他配置命令
+
+    return 0;
+}
+
+/*****************************************************************
+ * Description:向IMU模块发送保存命令,发送完命令需要延迟250ms.
+ *****************************************************************/
+static int send_saveCmd(int fd)
+{
+    int ret = -1;
+    unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
+    ret = write(fd, saveCmd, sizeof(saveCmd));
+    if(ret != sizeof(saveCmd))
+    {
+        ROS_ERROR("Send IMU module save cmd error !");
+        return -3;
+    }
+    ros::Duration(0.25).sleep(); //delay 250ms才能发送其他配置命令
+
+    return 0;
+}
+
+/*****************************************************************
+ * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
+ *   发送控制命令,最后就是需要发送两遍解锁命令、保存命令的操作.
+ *****************************************************************/
+static int send_data(int fd, unsigned char *send_buffer, int length)
+{
+    int ret = -1;
 
     #if 0 //打印发送给IMU模块的数据,用于调试
     printf("Send %d byte to IMU:", length);
@@ -139,34 +85,33 @@ int send_data(int fd, unsigned char *send_buffer, int length)
     }
     printf("\n");
     #endif
+    send_unlockCmd(fd);//发送解锁命令
 
-    //发送控制命令
+    //发送控制命令,需要延时250ms
     ret = write(fd, send_buffer, length*sizeof(unsigned char));
     if(ret != length)
     {
         ROS_ERROR("Send IMU module control cmd error !");
         return -2;
     }
+    ros::Duration(0.25).sleep(); //delay 250ms才能发送其他配置命令
 
-    //发送保存命令
-    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才能进行其他操作
+    //发送两遍解锁保存命令
+    send_unlockCmd(fd);  //发送解锁命令
+    send_saveCmd(fd);    //发送保存命令
+    send_unlockCmd(fd);  //发送解锁命令
+    send_saveCmd(fd);    //发送保存命令
 
     return 0;
 }
 
 /**************************************************************
  * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
- *   解析读取其中的44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *   解析读取其中的44个字节,正好是4帧数据,每一帧数据都是11个字节.
  *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
  *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
  *************************************************************/
-void parse_serialData(unsigned char chr)
+static void parse_serialData(unsigned char chr)
 {
     static unsigned char chrBuf[BYTE_CNT];
     static unsigned char chrCnt = 0;  //记录读取的第几个字节
@@ -249,50 +194,43 @@ int makeYawZero(void)
         ROS_ERROR("Send yaw zero control cmd error !");
         return -1;
     }
-    ROS_INFO("set yaw to zero radian successfully !");
+    ROS_WARN("Set yaw to zero radian successfully !");
 
     return 0;
 }
 
 /******************************************************************
- * Description:更新串口通信波特率,根据输入参数的不同,修改为不同的
- *   波特率,1修改为9600,2修改为57600,3修改为115200.修改波特率就是
- *   发送3条指令,第一条命令是解除锁定,然后是发送串口波特率更新命令,
- *   最后是保存操作的指令.
- ******************************************************************/
-int setIMUBaudRate(const int flag)
+ * Description:更新IMU模块的IIC地址,这里通过发送串口命令方式来更新IIC地址,
+ *   完整的控制命令为:0xFF 0xAA 0x1A 0x** 0x00,更新的IIC地址为第4个字节.
+ *****************************************************************/
+int updateIICAddr(std::string input)
 {
     int ret = 0;
-    int baud = 0;
-    unsigned char baudRate[5] = {0xFF, 0xAA, 0x04, 0x00, 0x00};
-
-    switch(flag)
+    std::stringstream num;
+    int addr = 0;
+    const char *iicAddr = NULL;
+    unsigned char updateIICAddrCmd[5] = {0xFF, 0xAA, 0x1A, 0x00, 0x00};
+    
+    iicAddr = input.c_str();
+    num<<std::hex<<iicAddr+2<<std::endl;
+    num>>addr;
+    updateIICAddrCmd[3] = addr;
+
+    #if 0
+    for (int i=0; i<5; i++)
     {
-        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;
+        printf("0x%02x ", updateIICAddrCmd[i]);
     }
-    ret = send_data(port_fd, baudRate, sizeof(baudRate));
+    printf("\n\n");
+    #endif
+ 
+    ret = send_data(port_fd, updateIICAddrCmd, sizeof(updateIICAddrCmd));
     if(ret != 0)
     {
-        ROS_ERROR("Send baud rate update cmd error !");
+        ROS_ERROR("Update IMU Module IIC Address Error !");
         return -1;
     }
-    ROS_INFO("Set imu new baud rate:[ %d bps] successfully !", baud);
-    ROS_INFO("Please reconnect IMU module with new baud !");
+    ROS_WARN("Update IMU Module IIC Address [%s] successfully !", input.c_str());
 
     return 0;
 }
@@ -301,17 +239,54 @@ int setIMUBaudRate(const int flag)
  * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
  *   两个输入参数的意义如下:
  *   const char* path:IMU设备的挂载地址;
- *   const int baud:IMU模块的串口通信波特率;
  ****************************************************************/
-int initSerialPort(const char* path, const int baud)
+int initSerialPort(const char* path)
 {
-    port_fd = openSerialPort(path, baud);
-    if(port_fd < 0) //打开IMU模块串口失败
+    struct termios terminfo;
+    bzero(&terminfo, sizeof(terminfo));
+
+    port_fd = open(path, O_RDWR|O_NOCTTY);
+    if (-1 == port_fd)
     {
-        ROS_ERROR("Open 6DOF IMU Module Serial Port error !");
+        ROS_ERROR("Open 6DOF IMU dev error:%s", path);
         return -1;
     }
 
+    //判断当前连接的设备是否为终端设备
+    if (isatty(STDIN_FILENO) == 0)
+    {
+        ROS_ERROR("IMU dev isatty error !");
+        return -2;
+    }
+
+    /*设置串口通信波特率-115200bps*/
+    cfsetispeed(&terminfo, B115200);
+    cfsetospeed(&terminfo, B115200);
+
+    //设置数据位 - 8 bit
+    terminfo.c_cflag |= CLOCAL|CREAD;
+    terminfo.c_cflag &= ~CSIZE;
+    terminfo.c_cflag |= CS8;
+
+    //不设置奇偶校验位 - N
+    terminfo.c_cflag &= ~PARENB;
+
+    //设置停止位 - 1
+    terminfo.c_cflag &= ~CSTOPB;
+
+    //设置等待时间和最小接收字符
+    terminfo.c_cc[VTIME] = 0;
+    terminfo.c_cc[VMIN]  = 1;
+
+    //清除串口缓存的数据
+    tcflush(port_fd, TCIFLUSH);
+
+    if((tcsetattr(port_fd, TCSANOW, &terminfo)) != 0)
+    {
+        ROS_ERROR("set imu serial port attr error !");
+        return -3;
+    }
+
     return 0;
 }
 
@@ -376,7 +351,7 @@ int getImuData(void)
         return -1;
     }
 
-    //printf("recv data len:%d\n", data_len); //一次性从串口中读取的总数据字节数
+    //printf("recv data:%d byte\n", data_len); //一次性从串口中读取的总数据字节数
     for (int i=0; i<data_len; i++) //将读取到的数据进行解析
     {
         //printf("0x%02x ", r_buf[i]);

serial_6dof_imu/src/serial_imu_node.cpp

@@ -9,14 +9,13 @@
 *****************************************************************/
 #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>
+#include <imu_data.h>
 #include "serial_6dof_imu/setYawZero.h"
-#include "serial_6dof_imu/setBaudRate.h"
 #include "serial_6dof_imu/getYawData.h"
-
+#include "serial_6dof_imu/setIICAddr.h"
 
 static float g_yawData;  //全局变量,存储当前yaw值,可以通过服务来得到该值
 
@@ -42,22 +41,6 @@ bool yawZeroService(serial_6dof_imu::setYawZero::Request &req,
     return true;
 }
 
-/********************************************************************
- * Description:使用服务调用方式来修改与IMU模块通信的波特率,这里根据
- *   客户端发送过来的request来决定修改的波特率,请求内容对应的波特率:
- *   请求发送1,修改波特率为9600;
- *   请求发送2,修改波特率为57600;
- *   请求发送3,修改波特率为115200;
- *   当修改IMU模块的波特率成功后函数会返回0,若返回其他负值,则表示
- *   修改波特率失败,可以重新调用服务来修改波特率.
- *******************************************************************/
-bool setBaudService(serial_6dof_imu::setBaudRate::Request &req,
-                    serial_6dof_imu::setBaudRate::Response &res)
-{
-    res.status = setIMUBaudRate(req.index);
-    return true;
-}
-
 /**********************************************************************
  * Description:通过service服务调用方式来获取到yaw角度信息.
  **********************************************************************/
@@ -68,12 +51,25 @@ bool getYawDataService(serial_6dof_imu::getYawData::Request &req,
     return true;
 }
 
+/*******************************************************************
+ * Description:通过service方式来更新IIC地址,默认地址为0x50,可以更新的地址
+ *   范围为0x00 - 0x7F,注意更新的地址不能与IIC总线上其他设备地址冲突,这样才
+ *   能在IIC总线上读取到所有地址上IIC设备的数据.
+ ******************************************************************/
+bool setIICAddrService(serial_6dof_imu::setIICAddr::Request &req,
+                       serial_6dof_imu::setIICAddr::Response &res)
+{
+    res.status = updateIICAddr(req.address);
+    return true;
+}
+
+/********************************************************************
+ * Description:入口主函数,负责通过topic和service来发布处理数据.
+ *******************************************************************/
 int main(int argc, char **argv)
 {
     float yaw, pitch, roll;
     std::string imu_dev;
-    int baud = 0;
-
     std::string imu_link_name;
     std::string imu_topic_name;
     std::string yaw_pub_topic;
@@ -81,8 +77,8 @@ int main(int argc, char **argv)
     std::string roll_pub_topic;
     std::string yaw_zero_topic;
     std::string yaw_zero_service;
-    std::string baud_update_service;
     std::string yaw_data_service;
+    std::string set_iic_addr_service;
 
     int pub_topic_hz = 0;  //话题发布imu数据的频率
     float degree2Rad = 3.1415926/180.0;
@@ -91,9 +87,8 @@ int main(int argc, char **argv)
     ros::init(argc, argv, "imu_data_pub_node");
     ros::NodeHandle handle;
 
-    //launch文件中加载yaml配置文件，然后从配置文件中读取参数
+    //launch文件中加载yaml配置文件，然后从yaml配置文件中读取参数
     ros::param::get("~imu_dev",          imu_dev);
-    ros::param::get("~baud_rate",        baud);
     ros::param::get("~imu_link_name",    imu_link_name);
     ros::param::get("~pub_topic_hz",     pub_topic_hz);
     ros::param::get("~pub_data_topic",   imu_topic_name);
@@ -102,11 +97,11 @@ int main(int argc, char **argv)
     ros::param::get("~yaw_pub_topic",    yaw_pub_topic);
     ros::param::get("~pitch_pub_topic",  pitch_pub_topic);
     ros::param::get("~roll_pub_topic",   roll_pub_topic);
-    ros::param::get("~baud_update_srv",  baud_update_service);
-    ros::param::get("~yaw_data_srv",     yaw_data_service);
+    ros::param::get("~get_yaw_data_srv", yaw_data_service);
+    ros::param::get("~set_iic_addr_srv", set_iic_addr_service);
 
-    //初始化imu模块串口,根据设备号和波特率建立连接
-    int ret = initSerialPort(imu_dev.c_str(), baud);
+    //初始化imu模块串口,根据设备号与IMU建立连接
+    int ret = initSerialPort(imu_dev.c_str());
     if(ret < 0) //通过串口连接IMU模块失败
     {
         ROS_ERROR("init 6DOF IMU module serial port error !");
@@ -115,10 +110,10 @@ int main(int argc, char **argv)
     }
     ROS_INFO("Now 6DOF IMU module is working...");
 
-    //定义四个服务,分别是更新波特率,yaw角度归零和获取yaw当前角度
-    ros::ServiceServer setBaudSrv = handle.advertiseService(baud_update_service, setBaudService);
+    //定义服务,分别是yaw角度归零和获取yaw当前角度,更新IIC地址
     ros::ServiceServer setyawSrv  = handle.advertiseService(yaw_zero_service,  yawZeroService);
     ros::ServiceServer getYawSrv  = handle.advertiseService(yaw_data_service,  getYawDataService);
+    ros::ServiceServer setIICSrv  = handle.advertiseService(set_iic_addr_service, setIICAddrService);
 
     ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
     ros::Publisher imu_pub   = handle.advertise<sensor_msgs::Imu>(imu_topic_name, 2);

serial_6dof_imu/srv/setBaudRate.srv

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

serial_6dof_imu/srv/setIICAddr.srv

@@ -0,0 +1,3 @@
+string address
+---
+int32 status