优化代码增加输出imu数据的频率

serial_6dof_imu/CMakeLists.txt

@@ -3,9 +3,11 @@
 #
 cmake_minimum_required (VERSION 3.8)
 
-project ("mini6DOFIMU")
-
-# 将源代码添加到此项目的可执行文件。
-add_executable (mini6DOFIMU "src/serial_imu.cpp" "src/proc_serial_data.cpp")
-
-# TODO: 如有需要，请添加测试并安装目标。
+project ("mini6DOFIMU")
+
+# 将源代码添加到此项目的可执行文件。
+add_executable (mini6DOFIMU "src/serial_imu.cpp" "src/proc_serial_data.cpp")
+target_include_directories(mini6DOFIMU PRIVATE include)
+target_compile_features(mini6DOFIMU PRIVATE cxx_std_11)
+
+# TODO: 如有需要，请添加测试并安装目标。

serial_6dof_imu/README.md

@@ -1,10 +1,11 @@
 #### 下载代码
 ```
-git clone -b linux-dev https://code.corvin.cn:3000/corvin_zhang/rasp_imu_hat_6dof.git
+git clone -b linux-dev https://code.xturtle.cn/corvin_zhang/rasp_imu_hat_6dof.git
 ```
 
-#### 编译
+#### 编译serial_6dof_imu代码
 ```
+cd serial_6dof_imu
 mkdir build
 cd build
 cmkae ..

serial_6dof_imu/include/imu_data.h

@@ -23,6 +23,7 @@
 #include<string.h>
 #include<sys/time.h>
 #include<sys/types.h>
+#include<sys/select.h>
 #include<iostream>
 #include<sstream>
 #include <iomanip>
@@ -43,4 +44,4 @@ float getQuat(int flag);
 int makeYawZero(void);
 int updateIICAddr(std::string input);
 void show_imu_data(void);
-#endif
+#endif

serial_6dof_imu/src/proc_serial_data.cpp

@@ -7,17 +7,30 @@
 ******************************************************************/
 #include"../include/imu_data.h"
 
+#include <cerrno>
+#include <chrono>
+#include <cstdint>
+
 using namespace std;
 
-#define  BYTE_CNT      55  //每次从串口中读取的字节数
+#define  READ_BUF_SIZE 256  //每次read最多读取的字节数,允许一次缓存多组IMU数据
+#define  FRAME_LEN     11   //IMU协议中每个数据帧固定为11字节
 #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 unsigned char r_buf[READ_BUF_SIZE];  //串口读取缓存,未解析完的数据会保留到下次继续使用
 static int port_fd = -1;  //串口打开时的文件描述符
 static float acce[3],angular[3],angle[3],quater[4];
 
+// 4种关键帧都解析到后,才认为凑齐一组完整IMU数据并允许上层输出一次。
+static const unsigned int ACC_FRAME_MASK   = 1 << 0;
+static const unsigned int GYRO_FRAME_MASK  = 1 << 1;
+static const unsigned int ANGLE_FRAME_MASK = 1 << 2;
+static const unsigned int QUAT_FRAME_MASK  = 1 << 3;
+static const unsigned int FULL_FRAME_MASK  = ACC_FRAME_MASK | GYRO_FRAME_MASK |
+                                            ANGLE_FRAME_MASK | QUAT_FRAME_MASK;
+
 static struct termios initial_settings, new_settings;
 
 void init_keyboard(void)
@@ -27,7 +40,8 @@ void init_keyboard(void)
     new_settings.c_lflag &= ~ICANON;
     new_settings.c_lflag &= ~ECHO;
     new_settings.c_lflag &= ~ISIG;
-    new_settings.c_cc[VMIN] = 1;
+    // stdin设置为非阻塞查询模式,避免每次循环都tcsetattr影响100Hz输出。
+    new_settings.c_cc[VMIN] = 0;
     new_settings.c_cc[VTIME] = 0;
     tcsetattr(0, TCSANOW, &new_settings);
 }
@@ -41,16 +55,21 @@ void close_keyboard(void)
  *************************************/
 int kbhit(void)
 {
+    fd_set read_fds;
+    struct timeval timeout;
     char ch;
-    int nread;
-    new_settings.c_cc[VMIN]=0;
-    tcsetattr(0, TCSANOW, &new_settings);
-    nread = read(0,&ch,1);
-    new_settings.c_cc[VMIN]=1;
-    tcsetattr(0, TCSANOW, &new_settings);
-    if(nread == 1) 
+
+    FD_ZERO(&read_fds);
+    FD_SET(STDIN_FILENO, &read_fds);
+    timeout.tv_sec = 0;
+    timeout.tv_usec = 0;
+
+    // select只检查键盘是否有输入,不会阻塞IMU数据读取循环。
+    if(select(STDIN_FILENO + 1, &read_fds, NULL, NULL, &timeout) > 0)
     {
-      return 1;
+        // 读掉一个按键,否则下一轮仍会检测到同一个输入。
+        read(STDIN_FILENO, &ch, 1);
+        return 1;
     }
     return 0;
 }
@@ -60,7 +79,11 @@ int kbhit(void)
  *************************************/
 int closeSerialPort(void)
 {
+    if(port_fd < 0)
+        return 0;
+
     int ret = close(port_fd);
+    port_fd = -1;
     return ret;
 }
 
@@ -134,34 +157,52 @@ static int send_data(int fd, unsigned char *send_buffer, int length)
  *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
  *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
  *************************************************************/
-static void parse_serialData(unsigned char chr)
+static int16_t to_int16(unsigned char low_byte, unsigned char high_byte)
 {
-    static unsigned char chrBuf[BYTE_CNT];
-    static unsigned char chrCnt = 0;  //记录读取的第几个字节
+    uint16_t value = (static_cast<uint16_t>(high_byte) << 8) | low_byte;
+    return static_cast<int16_t>(value);
+}
 
-    signed short sData[4]; //save 8 Byte有效信息
-    unsigned char i = 0;   //用于for循环
-    unsigned char frameSum = 0;  //存储数据帧的校验和
+static bool parse_serialData(unsigned char chr)
+{
+    static unsigned char chrBuf[FRAME_LEN];
+    static unsigned char chrCnt = 0;       //记录当前帧已经收到的字节数
+    static unsigned int parsedMask = 0;    //记录当前一组IMU数据已经收到哪些帧
 
-    chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
+    // 帧头必须是0x55。若当前还没有进入帧,直接丢弃杂字节,可以更快重新同步。
+    if(chrCnt == 0 && chr != 0x55)
+        return false;
 
-    //判断是否读取满一个完整数据帧11个字节,若没有则返回不进行解析
-    if(chrCnt < 11)
-        return;
+    chrBuf[chrCnt++] = chr;
+    if(chrCnt < FRAME_LEN)
+        return false;
 
-    //读取满完整一帧数据,计算数据帧的前十个字节的校验和,累加即可得到
-    for(i=0; i<10; i++)
-    {
+    unsigned char frameSum = 0;
+    for(unsigned char i=0; i<FRAME_LEN-1; i++)
         frameSum += chrBuf[i];
-    }
 
-    //找到数据帧第一个字节是0x55,同时判断校验和是否正确,若两者有一个不正确,
-    //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
-    if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
+    if ((chrBuf[0] != 0x55) || (frameSum != chrBuf[10]))
     {
-        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
-        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
-        return;
+        // 校验失败时,在当前11字节中继续查找下一个0x55,减少丢帧后的同步时间。
+        int nextHead = -1;
+        for(int i=1; i<FRAME_LEN; i++)
+        {
+            if(chrBuf[i] == 0x55)
+            {
+                nextHead = i;
+                break;
+            }
+        }
+        if(nextHead >= 0)
+        {
+            chrCnt = FRAME_LEN - nextHead;
+            memmove(chrBuf, chrBuf + nextHead, chrCnt);
+        }
+        else
+        {
+            chrCnt = 0;
+        }
+        return false;
     }
 
     #if 0 //打印出完整的带帧头尾的数据帧
@@ -170,36 +211,46 @@ static void parse_serialData(unsigned char chr)
     printf("\n");
     #endif
 
-    memcpy(&sData[0], &chrBuf[2], 8);
     switch(chrBuf[1])  //根据数据帧不同类型来进行解析数据
     {
         case 0x51: //x,y,z轴 加速度输出
-            acce[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ACCE_CONST;
-            acce[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ACCE_CONST;
-            acce[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ACCE_CONST;
+            acce[0] = to_int16(chrBuf[2], chrBuf[3]) * ACCE_CONST;
+            acce[1] = to_int16(chrBuf[4], chrBuf[5]) * ACCE_CONST;
+            acce[2] = to_int16(chrBuf[6], chrBuf[7]) * ACCE_CONST;
+            parsedMask |= ACC_FRAME_MASK;
             break;
         case 0x52: //角速度输出
-            angular[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGULAR_CONST;
-            angular[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGULAR_CONST;
-            angular[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGULAR_CONST;
+            angular[0] = to_int16(chrBuf[2], chrBuf[3]) * ANGULAR_CONST;
+            angular[1] = to_int16(chrBuf[4], chrBuf[5]) * ANGULAR_CONST;
+            angular[2] = to_int16(chrBuf[6], chrBuf[7]) * ANGULAR_CONST;
+            parsedMask |= GYRO_FRAME_MASK;
             break;
         case 0x53: //欧拉角度输出, roll, pitch, yaw
-            angle[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGLE_CONST;
-            angle[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGLE_CONST;
-            angle[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGLE_CONST;
+            angle[0] = to_int16(chrBuf[2], chrBuf[3]) * ANGLE_CONST;
+            angle[1] = to_int16(chrBuf[4], chrBuf[5]) * ANGLE_CONST;
+            angle[2] = to_int16(chrBuf[6], chrBuf[7]) * ANGLE_CONST;
+            parsedMask |= ANGLE_FRAME_MASK;
             break;
         case 0x59: //四元素输出
-            quater[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0;
-            quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
-            quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
-            quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
-            //printf("%f %f %f %f\n", quater[0], quater[1], quater[2], quater[3]);
+            quater[0] = to_int16(chrBuf[2], chrBuf[3]) / 32768.0;
+            quater[1] = to_int16(chrBuf[4], chrBuf[5]) / 32768.0;
+            quater[2] = to_int16(chrBuf[6], chrBuf[7]) / 32768.0;
+            quater[3] = to_int16(chrBuf[8], chrBuf[9]) / 32768.0;
+            parsedMask |= QUAT_FRAME_MASK;
             break;
         default:
-            cout << "转换IMU数据帧错误！！！" << endl;
+            // 模块可能开启了其他输出帧,未知帧直接忽略,不影响4类核心IMU数据。
             break;
     }
     chrCnt = 0;
+
+    if((parsedMask & FULL_FRAME_MASK) == FULL_FRAME_MASK)
+    {
+        parsedMask = 0;
+        return true;
+    }
+
+    return false;
 }
 
 /********************************************************************
@@ -266,11 +317,19 @@ void show_imu_data(void)
     float yaw, pitch, roll;
     float degree2Rad = 3.1415926 / 180.0;
     float acc_factor = 9.806;
+    unsigned long long frameCount = 0;
+    unsigned long long lastFrameCount = 0;
+    double currentHz = 0.0;
+    auto lastHzTime = std::chrono::steady_clock::now();
+
     init_keyboard();
+    // 清掉进入显示模式前残留的回车键,避免刚进入就被kbhit当成退出命令。
+    while(kbhit()) {}
     while (true)
     {
         if (getImuData() < 0)
         {
+            close_keyboard();
             break;
         }    
         if (kbhit())
@@ -284,14 +343,30 @@ void show_imu_data(void)
         if (yaw >= 3.1415926)
             yaw -= 6.2831852;
 
-        cout << endl;
-        cout.setf(std::ios::right);
-        cout << "roll:    " << setw(12) << roll << "  pitch:   " << setw(12) << pitch << "  yaw:     " << setw(12) << yaw << endl;
-        cout << "q_x:     " << setw(12) << getQuat(1) << "  q_y:     " << setw(12) << getQuat(2) << "  q_z:     " << setw(12) << getQuat(3) << "  q_w: " << setw(12) << getQuat(0) << endl;
-        cout << "a_v_x:   " << setw(12) << getAngular(0) * degree2Rad << "  a_v_y:   " << setw(12) << getAngular(1) * degree2Rad << "  a_v_z:   " << setw(12) << getAngular(2) * degree2Rad << endl;
-        cout << "l_acc_x: " << setw(12) << -getAcc(0) * acc_factor << "  l_acc_y: " << setw(12) << -getAcc(1) * acc_factor << "  l_acc_z: " << setw(12) << -getAcc(2) * acc_factor << endl;
-
-        //usleep(10 * 1000);
+        frameCount++;
+        auto now = std::chrono::steady_clock::now();
+        double seconds = std::chrono::duration<double>(now - lastHzTime).count();
+        if(seconds >= 1.0)
+        {
+            currentHz = (frameCount - lastFrameCount) / seconds;
+            lastFrameCount = frameCount;
+            lastHzTime = now;
+        }
+
+        // 终端打印本身很慢,原来每帧打印4行并使用endl刷新会明显拖慢循环。
+        // 这里改为每组IMU数据只输出1行,并使用printf普通换行,更适合100Hz连续输出。
+        printf("seq:%06llu hz:%6.1f roll:% .6f pitch:% .6f yaw:% .6f "
+               "qx:% .6f qy:% .6f qz:% .6f qw:% .6f "
+               "avx:% .6f avy:% .6f avz:% .6f "
+               "lax:% .6f lay:% .6f laz:% .6f\n",
+               frameCount, currentHz, roll, pitch, yaw,
+               getQuat(1), getQuat(2), getQuat(3), getQuat(0),
+               getAngular(0) * degree2Rad,
+               getAngular(1) * degree2Rad,
+               getAngular(2) * degree2Rad,
+               -getAcc(0) * acc_factor,
+               -getAcc(1) * acc_factor,
+               -getAcc(2) * acc_factor);
     }
 }
 
@@ -303,7 +378,6 @@ void show_imu_data(void)
 int initSerialPort(const char* path)
 {
     struct termios terminfo;
-    bzero(&terminfo, sizeof(terminfo));
 
     port_fd = open(path, O_RDWR|O_NOCTTY);
     if (-1 == port_fd)
@@ -312,12 +386,27 @@ int initSerialPort(const char* path)
     }
 
     //判断当前连接的设备是否为终端设备
-    if (isatty(STDIN_FILENO) == 0)
+    if (isatty(port_fd) == 0)
     {
         cout << "IMU dev isatty error !" << endl;
+        closeSerialPort();
         return -2;
     }
 
+    if(tcgetattr(port_fd, &terminfo) != 0)
+    {
+        cout << "get imu serial port attr error !" << endl;
+        closeSerialPort();
+        return -3;
+    }
+
+    // 使用raw模式关闭行处理、回显、软件流控等功能,串口字节才能原样进入解析器。
+    cfmakeraw(&terminfo);
+    terminfo.c_iflag &= ~(IXON | IXOFF | IXANY);
+#ifdef CRTSCTS
+    terminfo.c_cflag &= ~CRTSCTS;
+#endif
+
     /*设置串口通信波特率-57600bps*/
     cfsetispeed(&terminfo, B57600);
     cfsetospeed(&terminfo, B57600);
@@ -334,6 +423,7 @@ int initSerialPort(const char* path)
     terminfo.c_cflag &= ~CSTOPB;
 
     //设置等待时间和最小接收字符
+    // VMIN=1表示只要收到至少1字节read就返回,上层解析器会自己拼完整帧。
     terminfo.c_cc[VTIME] = 0;
     terminfo.c_cc[VMIN]  = 1;
 
@@ -343,6 +433,7 @@ int initSerialPort(const char* path)
     if((tcsetattr(port_fd, TCSANOW, &terminfo)) != 0)
     {
         cout << "set imu serial port attr error !" << endl;
+        closeSerialPort();
         return -3;
     }
 
@@ -390,33 +481,47 @@ float getQuat(int flag)
 }
 
 /*************************************************************
- * Description:从串口读取数据,然后解析出各有效数据段,这里
- *  一次性从串口中读取88个字节,然后需要从这些字节中进行
- *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
- *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
- *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ * Description:从串口持续读取数据并解析有效数据段。read可能
+ *  一次返回半帧、一帧或多组IMU数据,因此这里用静态缓存保存
+ *  未处理完的字节,每解析到一组完整IMU数据才返回给显示层。
+ *  一组有效数据包括加速度(0x55 0x51)、角速度(0x55 0x52)、
+ *  角度(0x55 0x53)、四元数(0x55 0x59)四类帧。
  *************************************************************/
 int getImuData(void)
 {
-    int data_len = 0;
-    bzero(r_buf, sizeof(r_buf)); //存储新数据前,将缓冲区清零
+    static int buf_pos = 0;
+    static int buf_len = 0;
 
-    //开始从串口中读取数据,并将其保存到r_buf缓冲区中
-    data_len = read(port_fd, r_buf, sizeof(r_buf));
-    if(data_len <= 0) //从串口中读取数据失败
+    while(true)
     {
-        cout << "读串口数据失败\n" << endl;
-        closeSerialPort();
-        return -1;
-    }
-
-    //printf("recv data:%d byte\n", data_len); //一次性从串口中读取的总数据字节数
-    for (int i=0; i<data_len; i++) //将读取到的数据进行解析
-    {
-        //printf("0x%02x ", r_buf[i]);
-        parse_serialData(r_buf[i]);
+        if(buf_pos >= buf_len)
+        {
+            // 读取一批串口字节。若驱动一次返回多组数据,不会丢弃,而是缓存后逐组输出。
+            ssize_t data_len = read(port_fd, r_buf, sizeof(r_buf));
+            if(data_len < 0)
+            {
+                if(errno == EINTR)
+                    continue;
+
+                cout << "读串口数据失败: " << strerror(errno) << endl;
+                closeSerialPort();
+                return -1;
+            }
+            if(data_len == 0)
+                continue;
+
+            buf_pos = 0;
+            buf_len = static_cast<int>(data_len);
+        }
+
+        while(buf_pos < buf_len)
+        {
+            if(parse_serialData(r_buf[buf_pos++]))
+            {
+                // 已解析到完整一组IMU数据,立即返回给显示层输出一次。
+                // 缓冲区里剩余字节保留下次继续解析,避免一次read吞掉多组数据。
+                return 1;
+            }
+        }
     }
-    //printf("\n\n");
-
-    return 1;
 }