更新串口读取imu模块数据代码,使读取数据更稳定

serial_imu_hat_6dof/CMakeLists.txt

@@ -17,11 +17,6 @@ find_package(catkin REQUIRED COMPONENTS
 # 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 ##
 ################################################

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -18,16 +18,17 @@
 #include<sys/types.h>
 
 
-char r_buf[512];
-int port_fd = 0;
+unsigned char r_buf[88];  //一次从串口中读取的数据存储缓冲区
+static int port_fd = -1; //串口打开时的文件描述符
 float acce[3],angular[3],angle[3],quater[4];
 
-/**********************************************
- * Description:打开串口,输入各参数即可.各参数
- * 意义如下:
- * const char *path:IMU设备的挂载地址;
- * const int baud:串口通讯的波特率;
- *********************************************/
+/****************************************************
+ * Description:打开IMU模块串口,输入两个参数即可连接.
+ *   open()打开失败时,返回-1,成功打开时返回文件描述符.
+ *   各参数意义如下:
+ *   const char *path:IMU设备的挂载地址;
+ *   const int baud:串口通讯的波特率;
+ ***************************************************/
 int openSerialPort(const char *path, const int baud)
 {
     int fd = 0;
@@ -37,11 +38,12 @@ 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", 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;
@@ -55,16 +57,6 @@ int openSerialPort(const char *path, const int baud)
            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);
@@ -81,7 +73,7 @@ int openSerialPort(const char *path, const int baud)
            break;
     }
 
-    //设置数据位
+    //设置数据位 - 8 bit
     terminfo.c_cflag |= CLOCAL|CREAD;
     terminfo.c_cflag &= ~CSIZE;
     terminfo.c_cflag |= CS8;
@@ -92,8 +84,11 @@ int openSerialPort(const char *path, const int baud)
     //设置停止位 - 1
     terminfo.c_cflag &= ~CSTOPB;
 
-    terminfo.c_cc[VTIME] = 10;
-    terminfo.c_cc[VMIN]  = 0;
+    //设置等待时间和最小接收字符
+    terminfo.c_cc[VTIME] = 0;
+    terminfo.c_cc[VMIN]  = 1;
+
+    //清除串口缓存的数据
     tcflush(fd, TCIFLUSH);
 
     if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
@@ -120,26 +115,29 @@ int closeSerialPort()
 int send_data(int fd, char *send_buffer, int length)
 {
 	length = write(fd, send_buffer, length*sizeof(unsigned char));
-
 	return length;
 }
 
 /*******************************************************
  * Description:从串口中接收数据.
  *******************************************************/
-int recv_data(int fd, char* recv_buffer, int len)
+int recv_data(int fd, unsigned char* recv_buffer, int len)
 {
     int length = read(fd, recv_buffer, len);
     return length;
 }
 
 /************************************************************
- * Description:根据串口数据协议来解析有效数据,
+ * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
+ *   解析读取其中的44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
  ***********************************************************/
-void parse_serialData(char chr)
+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;
@@ -201,11 +199,13 @@ void parse_serialData(char chr)
 }
 
 /*****************************************************************
- * Description:根据串口配置信息来配置打开串口,准备进行数据通信
+ * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
+ *   两个输入参数的意义如下:
+ *   const char* path:IMU设备的挂载地址;
+ *   const int baud:IMU模块的串口通信波特率;
  ****************************************************************/
 int initSerialPort(const char* path, const int baud)
 {
-    bzero(r_buf, 512);
     port_fd = openSerialPort(path, baud);
     if(port_fd < 0)
     {
@@ -257,22 +257,36 @@ float getAngleZ()
     return angle[2];
 }
 
+/********************************************************
+ * Description:从串口读取数据,然后解析出各有效数据段,这里
+ *   一次性从串口中读取100个字节,然后需要从这些字节中进行
+ *   解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ ********************************************************/
 int getImuData()
 {
-    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 = recv_data(port_fd, r_buf, sizeof(r_buf));
+    if(data_len == -1)
     {
         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;
 }
+

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,6 +1,6 @@
-/****************************************************
- * Copyright:2016-2020 www.corvin.cn ROS小课堂
- * Description:使用串口来读取IMU模块的数据,并通过
+/********************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口来读取IMU模块的数据,并通过ROS话题
  *   topic将数据发布出来.芯片中默认读取出来的加速度数据
  *   单位是g,需要将其转换为ROS中加速度规定的m/s2才能发布.
  * Author: corvin
@@ -9,7 +9,9 @@
  *   20200406:增加发布yaw数据的话题.
  *   20200703:修改三轴加速度数据正负,按照ROS规定沿着各轴
  *     正方向时数据为正,否则为负.单位m/s2。
-*****************************************************/
+ *   20210317:去掉从配置文件中读取的停止位,数据位这些无法
+ *     修改的配置参数,因为都是固定的,无需自己修改配置.
+**********************************************************/
 #include <ros/ros.h>
 #include <tf/tf.h>
 #include "imu_data.h"
@@ -34,15 +36,16 @@ int main(int argc, char **argv)
     ros::init(argc, argv, "imu_data_pub_node");
     ros::NodeHandle handle;
 
-    ros::param::get("~imu_dev", imu_dev);
+    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_topic", yaw_topic);
-    ros::param::get("~pub_hz", pub_hz);
+    ros::param::get("~pub_hz",    pub_hz);
 
+    //初始化imu模块串口,根据设备号和波特率进行连接
     int ret = initSerialPort(imu_dev.c_str(), baud);
     if(ret < 0)
     {
@@ -101,7 +104,7 @@ int main(int argc, char **argv)
         loop_rate.sleep();
     }
 
-    closeSerialPort();
+    closeSerialPort(); //关闭串口连接
     return 0;
 }