GNSS_INS_LooseCouple/Decode_IMR.cpp at main · iceberg1369/GNSS_INS_LooseCouple · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
#include"Decode_IMR.h"

using namespace std;

/*******************************************************
* 解码IMR文件中的文件头
* @paras[in]   IMR文件路径
* @paras[out]  none
* @return -1:解码失败 0:解码成功
*******************************************************/
int IMRDATA::decodeHeader(FILE* imr_fp)
{
	char buff[IMR_HEADER];
	if (fread(buff, IMR_HEADER, 1, imr_fp) < 1) {
		cout << "read imr header in file error! " << endl;
		header.imr_header_status = false;
		return -1;
	}
	memcpy(&header.szHeader, buff, 8);
	memcpy(&header.bIsIntelOrMotorola, buff + 8, 1);
	memcpy(&header.dVersionNumber, buff + 9, 8);
	memcpy(&header.bDeltaTheta, buff + 17, 4);
	memcpy(&header.bDeltaVelocity, buff + 21, 4);
	memcpy(&header.dDataRateHz, buff + 25, 8);
	memcpy(&header.dGyroScaleFactor, buff + 33, 8);
	memcpy(&header.dAccelScaleFactor, buff + 41, 8);
	memcpy(&header.iUtcOrGpsTime, buff + 49, 4);
	memcpy(&header.iRcvTimeOrCorrTime, buff + 53, 4);
	memcpy(&header.dTimeTagBias, buff + 57, 8);
	memcpy(&header.szImuName, buff + 65, 32);
	memcpy(&header.blank, buff + 97, 4);
	memcpy(&header.szProgramName, buff + 101, 32);
	memcpy(&header.tCreate, buff + 133, 12);
	memcpy(&header.bLeverArmValid, buff + 145, 1);
	memcpy(&header.lXoffset, buff + 146, 4);
	memcpy(&header.lYoffset, buff + 150, 4);
	memcpy(&header.lZoffset, buff + 154, 4);
	memcpy(&header.reserved, buff + 158, 354);
	header.imr_header_status = true;
	return 0;
}

/*******************************************************
* 解码IMR文件中直接的数据
* @paras[in]   IMR文件路径
* @paras[out]  none
* @return -1:解码失败 0:解码成功
*******************************************************/
int IMRDATA::decodedirectdata(FILE* imr_fp)
{
	if (!header.imr_header_status) {
		cout << "imr header has not been read! " << endl;
		return -1;//imr header has not been read; exit if error
	}
	char buff[IMR_RAW_DATA];
	if (fread(buff, IMR_RAW_DATA, 1, imr_fp) < 1) {
		cout << "read imr data in file error! " << endl;
		return -1;
	}
	memcpy(&rawdata.time, buff, 8);
	memcpy(&rawdata.gx, buff + 8, 4);
	memcpy(&rawdata.gy, buff + 12, 4);
	memcpy(&rawdata.gz, buff + 16, 4);
	memcpy(&rawdata.ax, buff + 20, 4);
	memcpy(&rawdata.ay, buff + 24, 4);
	memcpy(&rawdata.az, buff + 28, 4);
	return 0;
}

/*******************************************************
* 将IMR文件中直接的数据转换为IMU数据
* @paras[in]   none
* @paras[out]  none
* @return      none
*******************************************************/
void IMRDATA::Processdirectdata()
{
	if (header.iUtcOrGpsTime == 1)
	{
		imudata.time = rawdata.time + 18;//GPST-UTC=18s
	}
	else
	{
		imudata.time = rawdata.time;
	}
	/*-- imr record in deg/s and m/s2 --*/
	if (header.bDeltaTheta == 0) {
	// 把IMR中的数据乘上尺度因子
		imudata.gx = rawdata.gx * header.dGyroScaleFactor;
		imudata.gy = rawdata.gy * header.dGyroScaleFactor;
		imudata.gz = rawdata.gz * header.dGyroScaleFactor;
		imudata.ax = rawdata.ax * header.dAccelScaleFactor;
		imudata.ay = rawdata.ay * header.dAccelScaleFactor;
		imudata.az = rawdata.az * header.dAccelScaleFactor;
	}
	/*-- imr record in deg and m/s --*/
	else {
		imudata.gx = rawdata.gx * header.dGyroScaleFactor * header.dDataRateHz;
		imudata.gy = rawdata.gy * header.dGyroScaleFactor * header.dDataRateHz;
		imudata.gz = rawdata.gz * header.dGyroScaleFactor * header.dDataRateHz;
		imudata.ax = rawdata.ax * header.dAccelScaleFactor * header.dDataRateHz;
		imudata.ay = rawdata.ay * header.dAccelScaleFactor * header.dDataRateHz;
		imudata.az = rawdata.az * header.dAccelScaleFactor * header.dDataRateHz;
	}
}