优化代码，增加注释信息，提高数据输出频率

serial_6dof_imu/README.md

@@ -1,6 +1,6 @@
 #### 下载代码
 ```
-git clone -b linux-dev https://code.corvin.cn:3000/corvin_zhang/mini_6dof_imu.git
+git clone -b linux-dev https://code.xturtle.cn/corvin_zhang/mini_6dof_imu.git
 ```
 
 #### 编译

serial_6dof_imu/src/proc_serial_data.cpp

@@ -4,36 +4,75 @@
  * Author: corvin
  * History:
  *   20211122:init this file.
+ *   20260429:优化代码结构,增加注释说明，提供数据输出频率;
 ******************************************************************/
 #include"../include/imu_data.h"
+#include <errno.h>
+#include <sys/select.h>
 
 using namespace std;
 
-#define  BYTE_CNT      55  //每次从串口中读取的字节数
+#define  READ_BUF_SIZE 256 //每次从串口批量读取的缓冲区大小,足够缓存多组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];  //一次从串口中读取的数据存储缓冲区
+#define  FRAME_HEADER  0x55
+#define  FRAME_ACCE    0x51
+#define  FRAME_GYRO    0x52
+#define  FRAME_ANGLE   0x53
+#define  FRAME_QUAT    0x59
+
+#define  FRAME_MASK_ACCE  0x01
+#define  FRAME_MASK_GYRO  0x02
+#define  FRAME_MASK_ANGLE 0x04
+#define  FRAME_MASK_QUAT  0x08
+#define  FRAME_MASK_ALL   (FRAME_MASK_ACCE | FRAME_MASK_GYRO | FRAME_MASK_ANGLE | FRAME_MASK_QUAT)
+
+static unsigned char r_buf[READ_BUF_SIZE];  //一次从串口中读取的数据存储缓冲区
 static int port_fd = -1;  //串口打开时的文件描述符
 static float acce[3],angular[3],angle[3],quater[4],temp;
 
 static struct termios initial_settings, new_settings;
+static int initial_stdin_flags = -1;
+static bool keyboard_inited = false;
 
 void init_keyboard(void)
 {
-    tcgetattr(0,&initial_settings);
+    if (tcgetattr(STDIN_FILENO, &initial_settings) != 0)
+    {
+        return;
+    }
+
     new_settings = initial_settings;
     new_settings.c_lflag &= ~ICANON;
     new_settings.c_lflag &= ~ECHO;
     new_settings.c_lflag &= ~ISIG;
-    new_settings.c_cc[VMIN] = 1;
+    //键盘读设置成立即返回,这样kbhit()不需要每次循环反复修改termios.
+    new_settings.c_cc[VMIN] = 0;
     new_settings.c_cc[VTIME] = 0;
-    tcsetattr(0, TCSANOW, &new_settings);
+    tcsetattr(STDIN_FILENO, TCSANOW, &new_settings);
+
+    initial_stdin_flags = fcntl(STDIN_FILENO, F_GETFL, 0);
+    if (initial_stdin_flags != -1)
+    {
+        fcntl(STDIN_FILENO, F_SETFL, initial_stdin_flags | O_NONBLOCK);
+    }
+    keyboard_inited = true;
 }
 void close_keyboard(void)
 {
-    tcsetattr(0, TCSANOW, &initial_settings);
+    if (keyboard_inited)
+    {
+        tcsetattr(STDIN_FILENO, TCSANOW, &initial_settings);
+        keyboard_inited = false;
+    }
+    if (initial_stdin_flags != -1)
+    {
+        fcntl(STDIN_FILENO, F_SETFL, initial_stdin_flags);
+        initial_stdin_flags = -1;
+    }
 }
 
 /**************************************
@@ -41,16 +80,22 @@ void close_keyboard(void)
  *************************************/
 int kbhit(void)
 {
+    fd_set readfds;
+    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(&readfds);
+    FD_SET(STDIN_FILENO, &readfds);
+    timeout.tv_sec = 0;
+    timeout.tv_usec = 0;
+
+    //select只检查键盘是否有输入,避免read阻塞影响100Hz数据输出.
+    if (select(STDIN_FILENO + 1, &readfds, NULL, NULL, &timeout) > 0)
     {
-      return 1;
+        if (read(STDIN_FILENO, &ch, 1) == 1)
+        {
+            return 1;
+        }
     }
     return 0;
 }
@@ -60,7 +105,12 @@ int kbhit(void)
  *************************************/
 int closeSerialPort(void)
 {
+    if (port_fd < 0)
+    {
+        return 0;
+    }
     int ret = close(port_fd);
+    port_fd = -1;
     return ret;
 }
 
@@ -74,7 +124,7 @@ static int send_unlockCmd(int fd)
     ret = write(fd, unLockCmd, sizeof(unLockCmd));
     if(ret != sizeof(unLockCmd))
     { 
-        cout << "发送IMU模块解锁命令失败！！！" << endl;
+        cout << "发送IMU模块解锁命令失败!!!" << endl;
         return -1;
     }
     usleep(10 * 1000);
@@ -91,7 +141,7 @@ static int send_saveCmd(int fd)
     ret = write(fd, saveCmd, sizeof(saveCmd));
     if(ret != sizeof(saveCmd))
     {
-        cout << "发送IMU模块保存命令失败！！！" << endl;
+        cout << "发送IMU模块保存命令失败!!!" << endl;
         return -1;
     }
     usleep(100 * 1000);
@@ -120,7 +170,7 @@ static int send_data(int fd, unsigned char *send_buffer, int length)
     ret = write(fd, send_buffer, length*sizeof(unsigned char));
     if(ret != length)
     {
-        cout << "发送IMU模块控制命令失败！！！" << endl;
+        cout << "发送IMU模块控制命令失败!!!" << endl;
         return -2;
     }
     usleep(100 * 1000);
@@ -134,34 +184,63 @@ 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 short bytes_to_short(unsigned char low, unsigned char high)
+{
+    return (short)(((unsigned short)high << 8) | low);
+}
+
+static bool parse_serialData(unsigned char chr)
 {
-    static unsigned char chrBuf[BYTE_CNT];
+    static unsigned char chrBuf[FRAME_LEN];
     static unsigned char chrCnt = 0;  //记录读取的第几个字节
+    static unsigned char frameMask = 0; //记录当前一组IMU数据中已经解析到哪些有效帧
 
-    signed short sData[4]; //save 8 Byte有效信息
     unsigned char i = 0;   //用于for循环
     unsigned char frameSum = 0;  //存储数据帧的校验和
 
+    //未同步到帧头时直接丢弃杂散字节,减少无效memcpy开销.
+    if ((chrCnt == 0) && (chr != FRAME_HEADER))
+    {
+        return false;
+    }
+
     chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
 
     //判断是否读取满一个完整数据帧11个字节,若没有则返回不进行解析
-    if(chrCnt < 11)
-        return;
+    if(chrCnt < FRAME_LEN)
+        return false;
 
     //读取满完整一帧数据,计算数据帧的前十个字节的校验和,累加即可得到
-    for(i=0; i<10; i++)
+    for(i=0; i<FRAME_LEN - 1; i++)
     {
         frameSum += chrBuf[i];
     }
 
     //找到数据帧第一个字节是0x55,同时判断校验和是否正确,若两者有一个不正确,
     //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
-    if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
+    if ((chrBuf[0] != FRAME_HEADER)||(frameSum != chrBuf[FRAME_LEN - 1]))
     {
-        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
-        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
-        return;
+        int nextHeader = -1;
+        for (i = 1; i < FRAME_LEN; i++)
+        {
+            if (chrBuf[i] == FRAME_HEADER)
+            {
+                nextHeader = i;
+                break;
+            }
+        }
+
+        //如果错误帧中包含新的帧头,保留这段数据继续同步;否则从头重新找帧头.
+        if (nextHeader > 0)
+        {
+            chrCnt = FRAME_LEN - nextHeader;
+            memmove(&chrBuf[0], &chrBuf[nextHeader], chrCnt);
+        }
+        else
+        {
+            chrCnt = 0;
+        }
+        return false;
     }
 
     #if 0 //打印出完整的带帧头尾的数据帧
@@ -170,37 +249,49 @@ 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;
+        case FRAME_ACCE: //x,y,z轴 加速度输出
+            acce[0] = bytes_to_short(chrBuf[2], chrBuf[3])*ACCE_CONST;
+            acce[1] = bytes_to_short(chrBuf[4], chrBuf[5])*ACCE_CONST;
+            acce[2] = bytes_to_short(chrBuf[6], chrBuf[7])*ACCE_CONST;
+            frameMask |= FRAME_MASK_ACCE;
             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;
-            temp       = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/100.0;
+        case FRAME_GYRO: //角速度输出
+            angular[0] = bytes_to_short(chrBuf[2], chrBuf[3])*ANGULAR_CONST;
+            angular[1] = bytes_to_short(chrBuf[4], chrBuf[5])*ANGULAR_CONST;
+            angular[2] = bytes_to_short(chrBuf[6], chrBuf[7])*ANGULAR_CONST;
+            temp       = bytes_to_short(chrBuf[8], chrBuf[9])/100.0;
+            frameMask |= FRAME_MASK_GYRO;
             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;
+        case FRAME_ANGLE: //欧拉角度输出, roll, pitch, yaw
+            angle[0] = bytes_to_short(chrBuf[2], chrBuf[3])*ANGLE_CONST;
+            angle[1] = bytes_to_short(chrBuf[4], chrBuf[5])*ANGLE_CONST;
+            angle[2] = bytes_to_short(chrBuf[6], chrBuf[7])*ANGLE_CONST;
+            frameMask |= FRAME_MASK_ANGLE;
             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;
+        case FRAME_QUAT: //四元素输出
+            quater[0] = bytes_to_short(chrBuf[2], chrBuf[3])/32768.0;
+            quater[1] = bytes_to_short(chrBuf[4], chrBuf[5])/32768.0;
+            quater[2] = bytes_to_short(chrBuf[6], chrBuf[7])/32768.0;
+            quater[3] = bytes_to_short(chrBuf[8], chrBuf[9])/32768.0;
+            frameMask |= FRAME_MASK_QUAT;
             //printf("%f %f %f %f\n", quater[0], quater[1], quater[2], quater[3]);
             break;
         default:
-            cout << "转换IMU数据帧错误！！！" << endl;
+            //有些模块会额外输出磁力计、端口状态等帧。它们校验正确但当前程序不用,
+            //这里直接忽略,避免每秒大量打印错误信息拖慢读取处理速度.
             break;
     }
     chrCnt = 0;
+
+    //加速度、角速度、欧拉角、四元数都更新后,才认为完成了一组可输出的IMU数据.
+    if ((frameMask & FRAME_MASK_ALL) == FRAME_MASK_ALL)
+    {
+        frameMask = 0;
+        return true;
+    }
+    return false;
 }
 
 /********************************************************************
@@ -215,7 +306,7 @@ int makeYawZero(void)
     ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
     if(ret != 0) //通过串口发送命令失败
     {
-        cout << "发送串口归零命令失败！！！" << endl;
+        cout << "发送串口归零命令失败!!!" << endl;
         return -1;
     }
     cout << "发送串口归零命令成功！" << endl;
@@ -267,7 +358,11 @@ void show_imu_data(void)
     float yaw, pitch, roll, temp;
     float degree2Rad = 3.1415926 / 180.0;
     float acc_factor = 9.806;
+    unsigned long seq = 0;
+    char out_buf[512];
+
     init_keyboard();
+    cout << "开始显示IMU数据, 按任意键返回菜单..." << endl;
     while (true)
     {
         if (getImuData() < 0)
@@ -276,7 +371,6 @@ void show_imu_data(void)
         }    
         if (kbhit())
         {
-            close_keyboard();
             break;
         }  
         roll  = getAngle(0) * degree2Rad;
@@ -286,15 +380,33 @@ 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;
-        cout << "temp:    " << setw(12) << temp << endl;
-        //usleep(10 * 1000);
+        ++seq;
+
+        //每组数据只格式化成一行,并用write一次性输出,避免多次std::endl强制flush拖慢100Hz输出.
+        int out_len = snprintf(out_buf, sizeof(out_buf),
+            "seq:%lu roll:%.6f pitch:%.6f yaw:%.6f "
+            "q_x:%.6f q_y:%.6f q_z:%.6f q_w:%.6f "
+            "a_v_x:%.6f a_v_y:%.6f a_v_z:%.6f "
+            "l_acc_x:%.6f l_acc_y:%.6f l_acc_z:%.6f temp:%.2f\n",
+            seq, 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,
+            temp);
+        if (out_len > 0)
+        {
+            if (out_len > (int)sizeof(out_buf))
+            {
+                out_len = sizeof(out_buf) - 1;
+            }
+            write(STDOUT_FILENO, out_buf, out_len);
+        }
     }
+    close_keyboard();
 }
 
 /*****************************************************************
@@ -314,12 +426,23 @@ 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);
+
     /*设置串口通信波特率-115200bps*/
     cfsetispeed(&terminfo, B115200);
     cfsetospeed(&terminfo, B115200);
@@ -335,6 +458,12 @@ int initSerialPort(const char* path)
     //设置停止位 - 1
     terminfo.c_cflag &= ~CSTOPB;
 
+    //关闭软件/硬件流控,避免控制字符或握手状态影响持续数据流.
+    terminfo.c_iflag &= ~(IXON | IXOFF | IXANY);
+#ifdef CRTSCTS
+    terminfo.c_cflag &= ~CRTSCTS;
+#endif
+
     //设置等待时间和最小接收字符
     terminfo.c_cc[VTIME] = 0;
     terminfo.c_cc[VMIN]  = 1;
@@ -400,33 +529,48 @@ float getTemp(void)
 }
 
 /*************************************************************
- * Description:从串口读取数据,然后解析出各有效数据段,这里
- *  一次性从串口中读取88个字节,然后需要从这些字节中进行
- *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
- *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
- *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ * Description:从串口批量读取数据并持续解析。read()一次可能读到
+ *  半组、一组或多组IMU数据,因此这里会保留未消费完的缓存。
+ *  只有解析到加速度(0x55 0x51)、角速度(0x55 0x52)、
+ *  角度(0x55 0x53)、四元数(0x55 0x59)各一帧后才返回,
+ *  这样上层每输出一行就对应一组完整IMU数据。
  *************************************************************/
 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) //从串口中读取数据失败
-    {
-        cout << "读串口数据失败\n" << endl;
-        closeSerialPort();
-        return -1;
-    }
-
-    //printf("recv data:%d byte\n", data_len); //一次性从串口中读取的总数据字节数
-    for (int i=0; i<data_len; i++) //将读取到的数据进行解析
+    while (true)
     {
-        //printf("0x%02x ", r_buf[i]);
-        parse_serialData(r_buf[i]);
+        //优先消费上一次read后尚未解析完的数据,避免同一批数据中后续样本被丢掉.
+        while (buf_pos < buf_len)
+        {
+            if (parse_serialData(r_buf[buf_pos++]))
+            {
+                return 1;
+            }
+        }
+
+        buf_pos = 0;
+        buf_len = 0;
+
+        //开始从串口中批量读取数据,解析到一组完整IMU数据后立即返回给上层输出.
+        data_len = read(port_fd, r_buf, sizeof(r_buf));
+        if(data_len < 0)
+        {
+            if (errno == EINTR)
+            {
+                continue;
+            }
+            cout << "读串口数据失败\n" << endl;
+            closeSerialPort();
+            return -1;
+        }
+        if(data_len == 0)
+        {
+            continue;
+        }
+        buf_len = data_len;
     }
-    //printf("\n\n");
-
-    return 1;
-}
+}