Przeglądaj źródła

删除代码中数据位停止位这些参数

corvin 4 lat temu
rodzic
commit
6668de44f7

+ 1 - 2
serial_imu_hat_6dof/include/imu_data.h

@@ -1,8 +1,7 @@
 #ifndef _IMU_DATA_H_
 #define _IMU_DATA_H_
 
-int initSerialPort(const char* path, const int baud, const int dataBits,
-        const char* parity, const int stopBit);
+int initSerialPort(const char* path, const int baud);
 
 int getImuData();
 int closeSerialPort();

+ 1 - 1
serial_imu_hat_6dof/launch/serial_imu_hat.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2020 https://www.corvin.cn ROS小课堂
+  Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
   Description:使用串口来读取imu模块的数据,并在ros中发布使用话题发布出来.
   Author: corvin
   History:

+ 14 - 63
serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -1,5 +1,5 @@
 /*****************************************************************
- * Copyright:2016-2020 www.corvin.cn ROS小课堂
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
  * Description:使用串口方式读取IMU模块信息.
  * Author: corvin
  * History:
@@ -23,10 +23,12 @@ int port_fd = 0;
 float acce[3],angular[3],angle[3],quater[4];
 
 /**********************************************
- * Description:打开串口,输入各参数即可.
+ * Description:打开串口,输入各参数即可.各参数
+ * 意义如下:
+ * const char *path:IMU设备的挂载地址;
+ * const int baud:串口通讯的波特率;
  *********************************************/
-int openSerialPort(const char *path, int baud, int dataBits,
-                const char* parity, int stopBit)
+int openSerialPort(const char *path, const int baud)
 {
     int fd = 0;
     struct termios terminfo;
@@ -82,63 +84,13 @@ int openSerialPort(const char *path, int baud, int dataBits,
     //设置数据位
     terminfo.c_cflag |= CLOCAL|CREAD;
     terminfo.c_cflag &= ~CSIZE;
-    switch(dataBits)
-    {
-        case 7:
-           terminfo.c_cflag |= CS7;
-           break;
-
-        case 8:
-           terminfo.c_cflag |= CS8;
-           break;
-
-        default:
-           ROS_ERROR("set dataBit error !");
-           return -3;
-    }
-
-    //设置奇偶校验位
-    switch(*parity)
-    {
-        case 'o': //设置奇校验
-        case 'O':
-           terminfo.c_cflag |= PARENB;
-           terminfo.c_cflag |= PARODD;
-           terminfo.c_iflag |= (INPCK|ISTRIP);
-           break;
-
-        case 'e': //设置偶校验
-        case 'E':
-           terminfo.c_iflag |= (INPCK|ISTRIP);
-           terminfo.c_cflag |= PARENB;
-           terminfo.c_cflag &= ~PARODD;
-           break;
-
-        case 'n': //不设置奇偶校验位
-        case 'N':
-           terminfo.c_cflag &= ~PARENB;
-           break;
-
-        default:
-           ROS_ERROR("set parity error !");
-          return -4;
-    }
+    terminfo.c_cflag |= CS8;
 
-    //设置停止位
-    switch(stopBit)
-    {
-        case 1:
-            terminfo.c_cflag &= ~CSTOPB;
-            break;
+    //不设置奇偶校验位 - N
+    terminfo.c_cflag &= ~PARENB;
 
-        case 2:
-            terminfo.c_cflag |= CSTOPB;
-            break;
-
-        default:
-            ROS_ERROR("set stopBit error !");
-            return -5;
-    }
+    //设置停止位 - 1
+    terminfo.c_cflag &= ~CSTOPB;
 
     terminfo.c_cc[VTIME] = 10;
     terminfo.c_cc[VMIN]  = 0;
@@ -147,7 +99,7 @@ int openSerialPort(const char *path, int baud, int dataBits,
     if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
     {
         ROS_ERROR("set imu serial port attr error !");
-        return -6;
+        return -3;
     }
 
     return fd;
@@ -251,11 +203,10 @@ void parse_serialData(char chr)
 /*****************************************************************
  * Description:根据串口配置信息来配置打开串口,准备进行数据通信
  ****************************************************************/
-int initSerialPort(const char* path, const int baud, const int dataBits,
-          const char* parity, const int stopBit)
+int initSerialPort(const char* path, const int baud)
 {
     bzero(r_buf, 512);
-    port_fd = openSerialPort(path, baud, dataBits, parity, stopBit);
+    port_fd = openSerialPort(path, baud);
     if(port_fd < 0)
     {
         ROS_ERROR("openSerialPort error !");

+ 1 - 7
serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -22,9 +22,6 @@ int main(int argc, char **argv)
     float yaw, pitch, roll;
     std::string imu_dev;
     int baud = 0;
-    int dataBit = 0;
-    std::string parity;
-    int stopBit = 0;
 
     std::string link_name;
     std::string imu_topic;
@@ -39,9 +36,6 @@ int main(int argc, char **argv)
 
     ros::param::get("~imu_dev", imu_dev);
     ros::param::get("~baud_rate", baud);
-    ros::param::get("~data_bits", dataBit);
-    ros::param::get("~parity", parity);
-    ros::param::get("~stop_bits", stopBit);
 
     ros::param::get("~link_name", link_name);
     ros::param::get("~pub_data_topic", imu_topic);
@@ -49,7 +43,7 @@ int main(int argc, char **argv)
     ros::param::get("~yaw_topic", yaw_topic);
     ros::param::get("~pub_hz", pub_hz);
 
-    int ret = initSerialPort(imu_dev.c_str(), baud, dataBit, parity.c_str(), stopBit);
+    int ret = initSerialPort(imu_dev.c_str(), baud);
     if(ret < 0)
     {
         ROS_ERROR("init serial port error !");