IMU数据解析

c文件

/*****************************************************************************
* filename:mian.c
* function:  原始数据解码为十进制
* version :1.0
* date    ：
* designer：JHLR
* note    ：
*****************************************************************************/
#include "IMU2BCD.h"
#define U1(p) (*((unsigned char*)(p)))
#define I1(p) (*((signed char*)(p)))
static unsigned short U2(unsigned char* p) { unsigned short u; memcpy(&u, p, 2); return u; }
static unsigned int U4(unsigned char* p) 
{
	unsigned int u; memcpy(&u, p, 4); return u;
}
static int I4(unsigned char* p) { int u; memcpy(&u, p, 4); return u; }
static float R4(unsigned char* p) { float u; memcpy(&u, p, 4); return u; }
static double R8(unsigned char* p) { double u; memcpy(&u, p, 8); return u; }

static void str_init(strfile* str)
{
	str->accel[0] = str->accel[1] = str->accel[2] = 0;
	str->gyro[0] = str->gyro[1] = str->gyro[2] = 0;
	str->week = 0;
	str->sec = 0;
	str->nbyte = 0;
	str->imustat = -1;
}
static int sync_rawimu(unsigned char* buff, unsigned char* data)
{
	buff[0] = buff[1]; buff[1] = buff[2]; buff[2] = data;
	return buff[0] == RAWIMUSYNC1 && buff[1] == RAWIMUSYNC2 && buff[2] == RAWIMUSYNC3;
}
static unsigned int imu_32CRC(const unsigned char* buff, int len)
{
	unsigned int crc = 0;
	int i, j;

	for (i = 0; i < len; i++)
	{
		crc ^= buff[i];
		for (j = 0; j < 8; j++)
		{
			if (crc & 1)crc = (crc >> 1) ^ POLYCRC32; else crc >>= 1;
		}
	}
	return crc;
}
static int imu2bcd(strfile* str,FILE *fp)
{
	int i, data;
	int datalen;
	int week;
	double sec;
	unsigned char* p = str->buff + 12;
	if (str->nbyte == 0)
	{
		for (i =0;; i++)
		{
			if ((data = fgetc(fp)) == EOF) return -2;//返回-2 到达文件末尾
			if (sync_rawimu(str->buff, (unsigned char)data))break;
			if (i > 4096) return 0;
		}
	}
	if (fread(str->buff + 3, 9, 1, fp) < 1) return -2;
	str->nbyte = 12;
	str->week = U2(str->buff + 6);
	str->sec = U4(str->buff + 8) * 0.001;//周内秒化成毫秒
	datalen = U1(str->buff + 3) + RAWHEADLEN;

	fread(str->buff + 12, datalen - 8, 1, fp);
	week = U4(p);
	sec = R8(p + 4);
	加速度计输出
	//str->accel[0] =( I4(p+ 24 )* ACCMSCALEFACTOR)/0.0305175628/4.0;

	//str->accel[1] =( I4(p+20)* ACCMSCALEFACTOR)/ 0.0305175628/4.0;
	//
	//str->accel[2] = (I4(p+16) * ACCMSCALEFACTOR)/ 0.0305175628/4.0;
	陀螺仪的输出
	//str->gyro[0] = (I4(p+36) * GYROSCALEFACTOR)/0.0539293486/3.0;
	//str->gyro[1] = (I4(p+32) * GYROSCALEFACTOR)/ 0.0539293486 / 3.0;
	//str->gyro[2] = (I4(p+28) * GYROSCALEFACTOR)/ 0.0539293486 / 3.0;

	
	str->accel[0] = I4(p + 24)*HG_ACCL_FACTOR;
	str->accel[1] = I4(p + 20)*HG_ACCL_FACTOR;
	str->accel[2] = I4(p + 16)*HG_ACCL_FACTOR;
	//陀螺仪的输出
	str->gyro[0] = I4(p + 36)*HG_GYRO_FACTOR;
	str->gyro[1] = I4(p + 32)*HG_GYRO_FACTOR;
	str->gyro[2] = I4(p + 28)*HG_GYRO_FACTOR;

	//CRC校验
	if (imu_32CRC(str->buff, datalen) != U4(str->buff + datalen)) {
		printf("jiaoyancuowu!");
		return 0;
	}
	str->nbyte = 0;
	return 1;
}
static void outdata(strfile* str, FILE* fp)
{
	fprintf(fp, "%15d%15.5lf%15.10lf%15.10lf%15.10lf%15.10lf%15.10lf%15.10lf\n",str->week,str->sec, str->gyro[0], str->gyro[1], str->gyro[2], str->accel[0], str->accel[1], str->accel[2]);
}

static int openfile(char *infile, char* outfile, int n)
{
	strfile* str;
	FILE* infp, * outfp,*out_imu_ie=NULL;
	char outfile_ie[128];
	outfile_ie[0] = 'I'; outfile_ie[1] = 'E'; 
	if (!(str = (strfile*)calloc(sizeof(strfile), 1))) return NULL;// 分配空间
	str_init(str);
	if (!(infp = fopen(infile,"rb")))
	{
		printf("infile open error!");
		return 0;
	}
	if (!(outfp = fopen(outfile,"w")))
	{
		printf("outfile open error!");
		return 0;
	}
	/*if (!(out_imu_ie=fopen(outfile_ie,'w')))
	{
		printf("outfile_open_error!");
		return 0;
	}*/
	while (!feof(infp))
	{
		//文件解码
		if(!imu2bcd(str,infp))continue;
		//解码后的文件输出
		outdata(str, outfp);
		
		
	}
	free(str); fclose(infp); fclose(outfp); 
	return 1;
}
// main
int main(int argc,char **argv)
{
	int i, j,n=0;
	int a = 3;
	char *infile=argv[1],*outfile="",*p;
	char *tempfile=NULL;
	/*for (i = 1,n=0; i < argc; i++)
	{
		infile[n++] = argv[i];
	}*/
	if (!*outfile)
	{
		if (*infile)
		{
			tempfile = (char*)malloc(128);
			strcpy(tempfile, infile);
			outfile = tempfile;
			if (p = strrchr(outfile, '.'))
				strcpy(p,".data");	
		}
		else printf("not find infile!!!\n");
	}
	if (!openfile(infile, outfile, n))
	{
		printf("data convert success\n");
	}
	//else printf("data convert fall");
	free(tempfile);
	return 1;
}

h文件