Ubuntu 获取 imu 数据

main.h
#include <pthread.h>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <iomanip>

#include <iostream>

using namespace cv;
using namespace std;

struct STime
{
    unsigned char ucYear;
    unsigned char ucMonth;
    unsigned char ucDay;
    unsigned char ucHour;
    unsigned char ucMinute;
    unsigned char ucSecond;
    unsigned short usMiliSecond;
};
struct SAcc
{
    short a[3];
    short T;
};
struct SGyro
{
    short w[3];
    short T;
};
struct SAngle
{
    short Angle[3];
    short T;
};
struct SMag
{
    short h[3];
    short T;
};

struct SDStatus
{
    short sDStatus[4];
};

struct SPress
{
    long lPressure;
    long lAltitude;
};

struct SLonLat
{
    long lLon;
    long lLat;
};

struct SGPSV
{
    short sGPSHeight;
    short sGPSYaw;
    long lGPsveLocity;
};
struct FOUR
{
    short four[4] ;
    
};

struct STime stcTime;
struct SAcc stcAcc;
struct SGyro stcGyro;
struct SAngle stcAngle;
struct SMag stcMag;
struct SDStatus stcDStatus;
struct SPress stcPress;
struct SLonLat stcLonLat;
struct SGPSV stcGPSV;
struct FOUR  stcFour;
Mat capFrame;
#include <stdio.h> /*标准输入输出定义*/#include <stdlib.h> /*标准函数库定义*/#include <unistd.h> /*Unix 标准函数定义*/#include <sys/types.h> #include <sys/stat.h> #include <fcntl.h> /*文件控制定义*/#include <termios.h> /*PPSIX 终端控制定义*/#include <errno.h> /*错误号定义*/#include <string.h>#include
 <math.h>#include "main.h"#define SERIAL_PORT "/dev/ttyUSB0" //串口地址#define G 9.79361 #define PI 3.1415926#define alpha 1 float length_x=0;float length_y=0;float length=0;int main(){ int fd;struct termios opt;fd = open(SERIAL_PORT,O_RDWR | O_NOCTTY | O_NDELAY);
 //默认为阻塞读方式if (fd == -1){perror("open serial 0\n");return 0;}tcgetattr(fd,&opt);cfsetispeed(&opt,B115200);if (tcsetattr(fd,TCSANow,&opt) != 0){perror("tcsetattr error");return 0;}opt.c_cflag &= ~CSIZE;opt.c_cflag |= CS8;opt.c_cflag &= ~CSTOPB;opt.c_cflag
 &= ~PARENB;opt.c_cflag &= ~INPCK;opt.c_cflag |= (CLOCAL | CREAD);opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);opt.c_oflag &= ~OPOST;opt.c_oflag &= ~(ONLCR | OCRNL);opt.c_iflag &= ~(ICRNL | INLCR);opt.c_iflag &= ~(IXON | IXOFF | IXANY);opt.c_cc[VTIME] = 0;opt.c_cc[VMIN]
 = 0;tcflush(fd,TCIOFLUSH);printf("configure complete\n");if (tcsetattr(fd,&opt) != 0){perror("serial error");return 0;}printf("start send and receive data\n"); /*ret >0,表示写入了多少字节 =0，没有写入字节 =-1，出错了 ret = write(fd,br_str,sizeof(br_str)); if (ret
 != sizeof(br_str)) { ERR_MSG("Write %s Failed!\n",led->brightness); close(fd); return -1; } *//*usRxLength=write(fd,&turn_,1);while(1){ char turn_; turn_=0x11;usRxLength=write(fd,1);}*/ float div;int i=0;int p=0; int len; int n; char read_buf[200];
 float ax=0;float ay=0;float az=0;float vx=0;float vy=0;float vz=0;float roll;float pitch;float yaw;FILE *fpWrite=fopen("data.txt","w"); if(fpWrite==NULL) { return 0; } while(1) { int usRxLength = read(fd,read_buf,200); printf("usRxLength=%d\r\n",usRxLength);
 if (usRxLength < 11) continue; usleep(14000); static unsigned char chrTemp[200]; memcpy(chrTemp,usRxLength);while (usRxLength >= 11){if (chrTemp[0] != 0x55){usRxLength--;memcpy(&chrTemp[0],&chrTemp[1],usRxLength);continue;}switch (chrTemp[1]){case
 0x50: printf("stcTime\r\n");memcpy(&stcTime,&chrTemp[2],8);break;case 0x51:memcpy(&stcAcc,8);break;case 0x52:memcpy(&stcGyro,8);break;case 0x53:memcpy(&stcAngle,8);break;case 0x54:memcpy(&stcMag,8);break;case
 0x55:memcpy(&stcDStatus,8);break;case 0x56:memcpy(&stcPress,8);break;case 0x57:memcpy(&stcLonLat,8);break;case 0x58:memcpy(&stcGPSV,8);break;case 0x59:memcpy(&stcFour,8);break;}usRxLength
 -= 11;memcpy(&chrTemp[0],&chrTemp[11],usRxLength);}printf("Time:20%d-%d-%d %d:%d:%.3f\r\n",(short)stcTime.ucYear,(short)stcTime.ucMonth,(short)stcTime.ucDay,(short)stcTime.ucHour,(short)stcTime.ucMinute,(float)stcTime.ucSecond + (float)stcTime.usMiliSecond
 / 1000);printf("stcFour:%.4f %.4f %.4f %.4f\r\n",(float)stcFour.four[0] / 32768,(float)stcFour.four[1] / 32768,(float)stcFour.four[2] / 32768,(float)stcFour.four[3] / 32768);printf("Acc:%.4f %.4f %.4f\r\n",(float)stcAcc.a[0] / 32768 * 16,(float)stcAcc.a[1]
 / 32768 * 16,(float)stcAcc.a[2] / 32768 * 16); fprintf(fpWrite,"Time:20%d-%d-%d %d:%d:%.3f ",(float)stcTime.ucSecond + (float)stcTime.usMiliSecond
 / 1000); fprintf(fpWrite,"Angle:%.3f %.3f %.3f",(float)stcAngle.Angle[0] / 32768 * 180,(float)stcAngle.Angle[1] / 32768 * 180,(float)stcAngle.Angle[2] / 32768 * 180); fprintf(fpWrite," Acc_offset: %.4f %.4f %.4f\r\n",ax,ay,(float) stcAcc.a[2] / 32768
 * 16*G); }fclose(fpWrite);return 0;}// g++ -o read read.cpp `pkg-config opencv --cflags --libs`