Initial commit

.gitignore

@@ -0,0 +1,64 @@
+# ---> C
+# Object files
+*.o
+*.ko
+*.obj
+*.elf
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+
+# ---> C++
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+

iic_6dof_imu/README.md

@@ -0,0 +1,9 @@
+#### 下载代码
+```
+git clone -b linux-dev https://code.corvin.cn:3000/corvin_zhang/rasp_imu_hat_6dof.git
+```
+
+#### 运行
+```
+python iic_6dof_imu.py
+```

iic_6dof_imu/iic_6dof_imu.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
+# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数，
+#    然后组装成ROS中的IMU消息格式，发布到/imu_data话题中，这样有需要的
+#    节点可以直接订阅该话题即可获取到imu当前的数据.
+# Author: corvin
+# History:
+#    20211122:Initial this file.
+import math
+import time
+
+from iic_6dof_imu_data import MyIMU
+
+
+degrees2rad = math.pi/180.0
+yaw = 0.0
+
+seq = 0
+accel_factor = 9.806  #sensor accel g convert to m/s^2.
+
+imu_iic_bus = 1
+imu_iic_addr = 0x50
+myIMU = MyIMU(imu_iic_bus, imu_iic_addr)
+
+print("Now 6DOF IMU Module is working ...")
+while True:
+    myIMU.get_YPRAG()
+
+    #rospy.loginfo("yaw:%f pitch:%f roll:%f", myIMU.raw_yaw,
+    #              myIMU.raw_pitch, myIMU.raw_roll)
+    yaw_deg = float(myIMU.raw_yaw)
+    if yaw_deg >= 180.0:
+        yaw_deg -= 360.0
+
+    #rospy.loginfo("yaw_deg: %f", yaw_deg)
+    yaw   = yaw_deg*degrees2rad
+    pitch = float(myIMU.raw_pitch)*degrees2rad
+    roll  = float(myIMU.raw_roll)*degrees2rad
+
+    print("roll: ", yaw, "pitch: ", pitch, "yaw: ", yaw)
+    myIMU.get_quatern()
+    print("q_x: ", myIMU.raw_q1, "q_y: ", myIMU.raw_q2, "q_z: ", myIMU.raw_q3, "q_w: ", myIMU.raw_q0)
+    print("a_v_x: ", float(myIMU.raw_gx)*degrees2rad, "a_v_y: ", float(myIMU.raw_gy)*degrees2rad, "a_v_z: ", float(myIMU.raw_gz)*degrees2rad)
+    print("l_acc_x: ", -float(myIMU.raw_ax)*accel_factor ,"l_acc_y: ", -float(myIMU.raw_ay)*accel_factor ,"l_acc_z: ", -float(myIMU.raw_az)*accel_factor)
+    time.sleep(0.1)

iic_6dof_imu/iic_6dof_imu_data.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
+# Author: corvin
+# Description: 从IIC接口中读取IMU模块的三轴加速度、三轴角速度、四元数。
+# History:
+#    20211122: Initial this file.
+import smbus
+import numpy as np
+
+class MyIMU(object):
+    def __init__(self, iic_bus, iic_addr):
+        self.i2c  = smbus.SMBus(iic_bus)
+        self.addr = iic_addr
+
+    def get_YPRAG(self):
+        try:
+            rpy_data  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
+            axyz_data = self.i2c.read_i2c_block_data(self.addr, 0x34, 6)
+            gxyz_data = self.i2c.read_i2c_block_data(self.addr, 0x37, 6)
+        except IOError:
+            print("Read IMU RPYAG date error !")
+        else:
+            self.raw_roll  = float(np.short(np.short(rpy_data[1]<<8)|rpy_data[0])/32768.0*180.0)
+            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])/32768.0*180.0)
+            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])/32768.0*180.0)
+
+            self.raw_ax = float(np.short(np.short(axyz_data[1]<<8)|axyz_data[0])/32768.0*16.0)
+            self.raw_ay = float(np.short(np.short(axyz_data[3]<<8)|axyz_data[2])/32768.0*16.0)
+            self.raw_az = float(np.short(np.short(axyz_data[5]<<8)|axyz_data[4])/32768.0*16.0)
+
+            self.raw_gx = float(np.short(np.short(gxyz_data[1]<<8)|gxyz_data[0])/32768.0*2000.0)
+            self.raw_gy = float(np.short(np.short(gxyz_data[3]<<8)|gxyz_data[2])/32768.0*2000.0)
+            self.raw_gz = float(np.short(np.short(gxyz_data[5]<<8)|gxyz_data[4])/32768.0*2000.0)
+
+    def get_quatern(self):
+        try:
+            quat_data = self.i2c.read_i2c_block_data(self.addr, 0x51, 8)
+        except IOError:
+            print("Read IMU quaternion date error !")
+        else:
+            self.raw_q0 = float((np.short(np.short(quat_data[1]<<8)|quat_data[0]))/32768.0)
+            self.raw_q1 = float((np.short(np.short(quat_data[3]<<8)|quat_data[2]))/32768.0)
+            self.raw_q2 = float((np.short(np.short(quat_data[5]<<8)|quat_data[4]))/32768.0)
+            self.raw_q3 = float((np.short(np.short(quat_data[7]<<8)|quat_data[6]))/32768.0)

serial_6dof_imu/CMakeLists.txt

@@ -0,0 +1,11 @@
+# CMakeList.txt: mini6DOFIMU 的 CMake 项目，在此处包括源代码并定义
+# 项目特定的逻辑。
+#
+cmake_minimum_required (VERSION 3.8)
+
+project ("mini6DOFIMU")
+
+# 将源代码添加到此项目的可执行文件。
+add_executable (mini6DOFIMU "src/serial_imu.cpp" "src/proc_serial_data.cpp")
+
+# TODO: 如有需要，请添加测试并安装目标。

serial_6dof_imu/README.md

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

serial_6dof_imu/include/imu_data.h

@@ -0,0 +1,46 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-11-26 10:13:03
+ * @Description: file content
+ * @History: 20210429:-adam_zhuo
+ */
+/*******************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口操作读取imu模块的代码头文件.
+ * Author: corvin
+ * History:
+ *   20211122:init this file.
+ *******************************************************/
+#ifndef _IMU_DATA_H_
+#define _IMU_DATA_H_
+
+#include<stdio.h>
+#include<stdlib.h>
+#include<fcntl.h>
+#include<unistd.h>
+#include<termios.h>
+#include<string.h>
+#include<sys/time.h>
+#include<sys/types.h>
+#include<iostream>
+#include<sstream>
+#include <iomanip>
+
+void init_keyboard(void);
+void close_keyboard(void);
+int kbhit(void);
+int initSerialPort(const char* path);
+
+int getImuData(void);
+int closeSerialPort(void);
+
+float getAcc(int flag);
+float getAngular(int flag);
+float getAngle(int flag);
+float getQuat(int flag);
+
+int makeYawZero(void);
+int updateIICAddr(std::string input);
+void show_imu_data(void);
+#endif

serial_6dof_imu/src/proc_serial_data.cpp

@@ -0,0 +1,422 @@
+/*******************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口方式读取和控制IMU模块信息.
+ * Author: corvin
+ * History:
+ *   20211122:init this file.
+******************************************************************/
+#include"../include/imu_data.h"
+
+using namespace std;
+
+#define  BYTE_CNT      55  //每次从串口中读取的字节数
+#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 int port_fd = -1;  //串口打开时的文件描述符
+static float acce[3],angular[3],angle[3],quater[4];
+
+static struct termios initial_settings, new_settings;
+
+void init_keyboard(void)
+{
+    tcgetattr(0,&initial_settings);
+    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;
+    new_settings.c_cc[VTIME] = 0;
+    tcsetattr(0, TCSANOW, &new_settings);
+}
+void close_keyboard(void)
+{
+    tcsetattr(0, TCSANOW, &initial_settings);
+}
+
+/**************************************
+ * Description:linux下kbhit的实现.
+ *************************************/
+int kbhit(void)
+{
+    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) 
+    {
+      return 1;
+    }
+    return 0;
+}
+
+/**************************************
+ * Description:关闭串口文件描述符.
+ *************************************/
+int closeSerialPort(void)
+{
+    int ret = close(port_fd);
+    return ret;
+}
+
+/*****************************************************************
+ * Description:向IMU模块发送解锁命令,发送完命令需要延迟10ms.
+ ****************************************************************/
+static int send_unlockCmd(int fd)
+{
+    int ret = 0;
+    unsigned char unLockCmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
+    ret = write(fd, unLockCmd, sizeof(unLockCmd));
+    if(ret != sizeof(unLockCmd))
+    { 
+        cout << "发送IMU模块解锁命令失败！！！" << endl;
+        return -1;
+    }
+    usleep(10 * 1000);
+    return 0;
+}
+
+/*****************************************************************
+ * Description:向IMU模块发送保存命令,发送完命令需要延迟100ms.
+ *****************************************************************/
+static int send_saveCmd(int fd)
+{
+    int ret = 0;
+    unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
+    ret = write(fd, saveCmd, sizeof(saveCmd));
+    if(ret != sizeof(saveCmd))
+    {
+        cout << "发送IMU模块保存命令失败！！！" << endl;
+        return -1;
+    }
+    usleep(100 * 1000);
+    return 0;
+}
+
+/*****************************************************************
+ * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
+ *   发送控制命令,最后就是需要发送两遍解锁命令、保存命令的操作.
+ *****************************************************************/
+static int send_data(int fd, unsigned char *send_buffer, int length)
+{
+    int ret = 0;
+
+    #if 0 //打印发送给IMU模块的数据,用于调试
+    printf("Send %d byte to IMU:", length);
+    for(int i=0; i<length; i++)
+    {
+        printf("0x%02x ", send_buffer[i]);
+    }
+    printf("\n");
+    #endif
+    send_unlockCmd(fd); //发送解锁命令
+
+    //发送完控制命令,需要延时100ms
+    ret = write(fd, send_buffer, length*sizeof(unsigned char));
+    if(ret != length)
+    {
+        cout << "发送IMU模块控制命令失败！！！" << endl;
+        return -2;
+    }
+    usleep(100 * 1000);
+    send_saveCmd(fd);  //发送保存命令
+    return 0;
+}
+
+/**************************************************************
+ * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
+ *   解析读取其中的44个字节,正好是4帧数据,每一帧数据都是11个字节.
+ *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ *************************************************************/
+static void parse_serialData(unsigned char chr)
+{
+    static unsigned char chrBuf[BYTE_CNT];
+    static unsigned char chrCnt = 0;  //记录读取的第几个字节
+
+    signed short sData[4]; //save 8 Byte有效信息
+    unsigned char i = 0;   //用于for循环
+    unsigned char frameSum = 0;  //存储数据帧的校验和
+
+    chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
+
+    //判断是否读取满一个完整数据帧11个字节,若没有则返回不进行解析
+    if(chrCnt < 11)
+        return;
+
+    //读取满完整一帧数据,计算数据帧的前十个字节的校验和,累加即可得到
+    for(i=0; i<10; i++)
+    {
+        frameSum += chrBuf[i];
+    }
+
+    //找到数据帧第一个字节是0x55,同时判断校验和是否正确,若两者有一个不正确,
+    //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
+    if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
+    {
+        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
+        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
+        return;
+    }
+
+    #if 0 //打印出完整的带帧头尾的数据帧
+    for(i=0; i<11; i++)
+      printf("0x%02x ",chrBuf[i]);
+    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;
+            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;
+            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;
+            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]);
+            break;
+        default:
+            cout << "转换IMU数据帧错误！！！" << endl;
+            break;
+    }
+    chrCnt = 0;
+}
+
+/********************************************************************
+ * Description:将yaw角度归零,这里需要通过串口发送的命令如下所示,
+ *  发送z轴角度归零的固定命令:0xFF 0xAA 0x01 0x04 0x00;
+ *******************************************************************/
+int makeYawZero(void)
+{
+    int ret = 0;
+    unsigned char yawZeroCmd[5] = {0xFF, 0xAA, 0x01, 0x04, 0x00};
+
+    ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
+    if(ret != 0) //通过串口发送命令失败
+    {
+        cout << "发送串口归零命令失败！！！" << endl;
+        return -1;
+    }
+    cout << "发送串口归零命令成功！" << endl;
+
+    return 0;
+}
+
+/******************************************************************
+ * Description:更新IMU模块的IIC地址,这里通过发送串口命令方式来更新IIC地址,
+ *   完整的控制命令为:0xFF 0xAA 0x1A 0x** 0x00,更新的IIC地址为第4个字节.
+ *****************************************************************/
+int updateIICAddr(std::string input)
+{
+    int ret = 0;
+    std::stringstream num;
+    int addr = 0;
+    const char *iicAddr = NULL;
+    unsigned char updateIICAddrCmd[5] = {0xFF, 0xAA, 0x1A, 0x00, 0x00};
+    
+    iicAddr = input.c_str();
+    num<<std::hex<<iicAddr+2<<std::endl;
+    num>>addr;
+    updateIICAddrCmd[3] = addr;
+
+    #if 0
+    for (int i=0; i<5; i++)
+    {
+        printf("0x%02x ", updateIICAddrCmd[i]);
+    }
+    printf("\n\n");
+    #endif
+ 
+    ret = send_data(port_fd, updateIICAddrCmd, sizeof(updateIICAddrCmd));
+    if(ret != 0)
+    {
+        cout << "更新IMU模块IIC地址失败！！！" << endl;
+        return -1;
+    }
+    cout << "更新IMU模块IIC地址成功！" << endl;
+
+    return 0;
+}
+
+/********************************************************************
+ * Description:显示IMU模块获取的数据。
+ *******************************************************************/
+void show_imu_data(void)
+{
+    float yaw, pitch, roll;
+    float degree2Rad = 3.1415926 / 180.0;
+    float acc_factor = 9.806;
+    init_keyboard();
+    while (true)
+    {
+        if (getImuData() < 0)
+        {
+            break;
+        }    
+        if (kbhit())
+        {
+            close_keyboard();
+            break;
+        }  
+        roll = getAngle(0) * degree2Rad;
+        pitch = getAngle(1) * degree2Rad;
+        yaw = getAngle(2) * degree2Rad;
+        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);
+    }
+}
+
+/*****************************************************************
+ * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
+ *   两个输入参数的意义如下:
+ *   const char* path:IMU设备的挂载地址;
+ ****************************************************************/
+int initSerialPort(const char* path)
+{
+    struct termios terminfo;
+    bzero(&terminfo, sizeof(terminfo));
+
+    port_fd = open(path, O_RDWR|O_NOCTTY);
+    if (-1 == port_fd)
+    {
+        return -1;
+    }
+
+    //判断当前连接的设备是否为终端设备
+    if (isatty(STDIN_FILENO) == 0)
+    {
+        cout << "IMU dev isatty error !" << endl;
+        return -2;
+    }
+
+    /*设置串口通信波特率-57600bps*/
+    cfsetispeed(&terminfo, B57600);
+    cfsetospeed(&terminfo, B57600);
+
+    //设置数据位 - 8 bit
+    terminfo.c_cflag |= CLOCAL|CREAD;
+    terminfo.c_cflag &= ~CSIZE;
+    terminfo.c_cflag |= CS8;
+
+    //不设置奇偶校验位 - N
+    terminfo.c_cflag &= ~PARENB;
+
+    //设置停止位 - 1
+    terminfo.c_cflag &= ~CSTOPB;
+
+    //设置等待时间和最小接收字符
+    terminfo.c_cc[VTIME] = 0;
+    terminfo.c_cc[VMIN]  = 1;
+
+    //清除串口缓存的数据
+    tcflush(port_fd, TCIFLUSH);
+
+    if((tcsetattr(port_fd, TCSANOW, &terminfo)) != 0)
+    {
+        cout << "set imu serial port attr error !" << endl;
+        return -3;
+    }
+
+    return 1;
+}
+
+/*****************************************
+ * Description:得到三轴加速度信息,输入
+ * 　参数可以为0,1,2分别代表x,y,z轴的
+ * 　加速度信息.
+ *****************************************/
+float getAcc(int flag)
+{
+    return acce[flag];
+}
+
+/******************************************
+ * Description:得到角速度信息,输入参数可
+ * 　以为0,1,2,分别代表x,y,z三轴的角速度
+ *   信息.
+ *****************************************/
+float getAngular(int flag)
+{
+    return angular[flag];
+}
+
+/********************************************
+ * Description:获取yaw,pitch,roll,输入参数0,
+ * 1,2可以分别获取到roll,pitch,yaw的数据.
+ *******************************************/
+float getAngle(int flag)
+{
+    return angle[flag];
+}
+
+/********************************************
+ * Description:输入参数0,1,2,3可以分别获取到
+ * 　四元素的q0,q1,q2,q3.但是这里对应的ros中
+ * 　的imu_msg.orientation.w,x,y,z的顺序,
+ * 　这里的q0是对应w参数,1,2,3对应x,y,z.
+ ********************************************/
+float getQuat(int flag)
+{
+    return quater[flag];
+}
+
+/*************************************************************
+ * Description:从串口读取数据,然后解析出各有效数据段,这里
+ *  一次性从串口中读取88个字节,然后需要从这些字节中进行
+ *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ *************************************************************/
+int getImuData(void)
+{
+    int data_len = 0;
+    bzero(r_buf, sizeof(r_buf)); //存储新数据前,将缓冲区清零
+
+    //开始从串口中读取数据,并将其保存到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++) //将读取到的数据进行解析
+    {
+        //printf("0x%02x ", r_buf[i]);
+        parse_serialData(r_buf[i]);
+    }
+    //printf("\n\n");
+
+    return 1;
+}

serial_6dof_imu/src/serial_imu.cpp

@@ -0,0 +1,85 @@
+/*****************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口来读取IMU的数据.
+ * Author: adam_zhuo
+ * History:
+ *   20211124: init this file.
+*****************************************************************/
+#include "../include/imu_data.h"
+
+using namespace std;
+
+void show_direction() {
+    cout << "----------------------------------------" << endl;
+    cout << "----------------------------------------" << endl;
+    cout << "                                        " << endl;
+    cout << "                                        " << endl;
+    cout << "------------  0命令, 退出  -------------" << endl;
+    cout << "------  1命令，IMU模块yaw角归0  --------" << endl;
+    cout << "------  2命令，修改I2C地址      --------" << endl;
+    cout << "------  3命令，显示IMU模块数据  --------" << endl;
+    cout << "                                        " << endl;
+    cout << "                                        " << endl;
+    cout << "----------------------------------------" << endl;
+    cout << "----------------------------------------" << endl;
+}
+
+int main(int argc, char** argv)
+{
+    
+    string imu_dev = "com1";
+    int order = 0;
+    string input;
+
+    while (true)
+    {
+        cout << "输入串口号（例如：/dev/ttyUSB0）：" << endl;
+        cin >> imu_dev;
+        
+        int ret = initSerialPort(imu_dev.c_str());
+        if (ret != 1) //通过串口连接IMU模块失败
+        {
+            cout << endl << "打开6自由度IMU模块串口失败 ！！！" << endl << endl;
+            closeSerialPort();
+            continue;
+        }
+        cout << endl << "打开6自由度IMU模块串口成功 !" << endl << endl;
+        break;
+    }
+
+    while (true)
+    {
+        show_direction();
+        cout << "输入指令：" << endl;
+        cin >> order;
+        if (order < 0 || order > 3)
+        {
+            cout << endl <<"输入正确的指令！！！" << endl << endl;
+            continue;
+        }
+        switch (order)
+        {
+        case 0:
+            closeSerialPort(); //关闭与IMU模块的串口连接
+            return 0;
+            break;
+        case 1:
+            makeYawZero();  //yaw角归0
+            break;
+        case 2:
+            cout << "输入I2C地址（0x00-0x7F）：" << endl;
+            cin >> input;
+            updateIICAddr(input);
+            break;
+        case 3:
+            show_imu_data();  //显示imu数据
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    closeSerialPort(); //关闭与IMU模块的串口连接
+    return 0;
+}