初始化linux下python3使用iic读取imu模块的代码

README.md

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

iic_6dof_imu/README.md

@@ -1,9 +0,0 @@
-#### 下载代码
-```
-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 → imu_6dof_iic.py

@@ -1,47 +1,44 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: UTF-8 -*-
 
-# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
-# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数，
-#    然后组装成ROS中的IMU消息格式，发布到/imu_data话题中，这样有需要的
-#    节点可以直接订阅该话题即可获取到imu当前的数据.
+# Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
+# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数,
+#   然后将其打印输出.
 # Author: corvin
 # History:
-#    20211122:Initial this file.
+#    20220523:Initial this file.
+
 import math
 import time
 
-from iic_6dof_imu_data import MyIMU
+from imu_data_proc import MyIMUClass
 
+imu_iic_addr = 0x50
 
 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)
+myIMU = MyIMUClass(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: ", roll, "pitch: ", pitch, "yaw: ", yaw)
 
-    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)
+
+    time.sleep(0.1)
+

iic_6dof_imu/iic_6dof_imu_data.py → imu_data_proc.py

@@ -1,18 +1,20 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: UTF-8 -*-
 
-# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
+# Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
 # Author: corvin
-# Description: 从IIC接口中读取IMU模块的三轴加速度、三轴角速度、四元数。
+# Description: 使用python3从IIC接口中读取IMU模块的三轴加速度、三轴角速度、四元数。
 # History:
-#    20211122: Initial this file.
+#    20220523: 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
+
+class MyIMUClass(object):
+    def __init__(self, dev_iic_addr):
+        self.i2c  = smbus.SMBus(1)
+        self.addr = dev_iic_addr
 
     def get_YPRAG(self):
         try:
@@ -44,3 +46,4 @@ class MyIMU(object):
             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

@@ -1,11 +0,0 @@
-# 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

@@ -1,12 +0,0 @@
-#### 下载代码
-```
-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

@@ -1,46 +0,0 @@
-/*
- * @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

@@ -1,422 +0,0 @@
-/*******************************************************************
- * 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

@@ -1,85 +0,0 @@
-/*****************************************************************
- * 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;
-}