优化串口读取IMU数据代码

README.md

@@ -15,8 +15,10 @@
 
 如果想使用话题方式,将yaw角度归零,那就可以执行如下命令:  
 `rostopic pub -1 /yaw_zero_topic std_msgs/Empty "{}"`  
+
 如果想使用服务调用方式,将yaw角度归零,那执行如下命令:  
 `rosservice call /yaw_zero_service "{}"`  
+
 如果想修改串口通信的波特率,那就可以执行如下命令:  
 `rosservice call /baud_update_service "index: 1"`  
 这里需要注意index后面的数字,１表示更新波特率为9600,2表示为57600,3表示115200,

serial_imu_hat_6dof/cfg/param.yaml

@@ -9,7 +9,7 @@
 #     就需要在raspi-config中关闭串口登录功能,打开硬件
 #     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
 #  baud_rate:串口通信波特率,默认为57600,如果查看发布的
-#  　　话题中没IMU数据可以尝试修改波特率.
+#  　　话题中没有IMU数据可以尝试换波特率.
 #  pub_data_topic:发布imu数据的ROS话题名.
 #  pub_temp_topic:发布imu模块温度的ROS话题名.
 #  link_name:imu模块的link名.

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -160,15 +160,6 @@ int send_data(int fd, unsigned char *send_buffer, int length)
     return 0;
 }
 
-/*******************************************************
- * Description:从串口中接收数据.
- *******************************************************/
-int recv_data(int fd, unsigned char* recv_buffer, int len)
-{
-    int length = read(fd, recv_buffer, len);
-    return length;
-}
-
 /**************************************************************
  * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
  *   解析读取其中的44个字节,正好是4帧数据,每一帧数据是11个字节.
@@ -271,21 +262,26 @@ int makeYawZero(void)
 int setIMUBaudRate(const int flag)
 {
     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: //set baud 57600 bps
+        default: //default set baud 57600 bps
             baudRate[3] = 0x05;
+            baud = 57600;
             break;
     }
     ret = send_data(port_fd, baudRate, sizeof(baudRate));
@@ -294,7 +290,9 @@ int setIMUBaudRate(const int flag)
         ROS_ERROR("Send baud rate update cmd error !");
         return -1;
     }
-    ROS_INFO("set new baud rate successfully !");
+    ROS_INFO("Set new baud rate:[ %d bps] successfully !", baud);
+    ROS_INFO("Please reconnect IMU module with new baud !");
+
     return 0;
 }
 
@@ -357,19 +355,19 @@ float getQuat(int flag)
 
 /*************************************************************
  * Description:从串口读取数据,然后解析出各有效数据段,这里
- *  一次性从串口中读取100个字节,然后需要从这些字节中进行
+ *  一次性从串口中读取88个字节,然后需要从这些字节中进行
  *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
  *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
  *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
- ***********************************************************/
-int getImuData()
+ *************************************************************/
+int getImuData(void)
 {
     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)
+    data_len = read(port_fd, r_buf, sizeof(r_buf));
+    if(data_len <= 0)
     {
         ROS_ERROR("read serial port data failed !\n");
         closeSerialPort();