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
#include "kfapp.h"
#include "sinsapi.h"
//CKFApp kf;
#define rawdata919 1
//CFileRdWt dir("C:\\F\\SINS-GNSS\\Yan_PSINS_Program\\PSINS-CPP-VC60（shilei）\\PSINS-CPP\\Data\\");
MPU_Data_value mpu_Data_value;
gga_t gga;
vtg_t vtg;
dhv_t dhv;
double gga_delay = 0.0;
int gps_ok_flag = 0;
bool gpsUpdate = true;
int main(void)
{
CFileRdWt fin("rawdataDemo9.1906.txt", 27);// , fins("ins.bin", 0), fkf("kf.bin", 0), frk("rk.bin", 0);// realtime 不需要
//CFileRdWt fin("rawdata1.txt", 29), fins("ins.bin", 0), fkf("kf.bin", 0), frk("rk.bin", 0);// 非PSINS
CVect3 wm, vm, eb0, gpsvn, gpspos;
double timu, timu0;
for (int i = 1; i < 20000 * 50; i++)
{
fin.load(); if (fin.IsEOF()) break;
// 为启迪坐标系错误调整
if (i == 1) timu0 = fin.buff[0];
mpu_Data_value.time = fin.buff[0] - timu0;
#if rawdata919
mpu_Data_value.Gyro[0] = fin.buff[2];// dps // 启迪项目保存的rawdata 坐标系统一在ENU 单位也已标准化
mpu_Data_value.Gyro[1] = fin.buff[1];
mpu_Data_value.Gyro[2] = -fin.buff[3];
mpu_Data_value.Accel[0] = fin.buff[5];// m/s2
mpu_Data_value.Accel[1] = fin.buff[4];
mpu_Data_value.Accel[2] = -fin.buff[6];
#else
mpu_Data_value.Gyro[0] = -fin.buff[1];// dps // 启迪项目保存的rawdata 坐标系统一在ENU 单位也已标准化
mpu_Data_value.Gyro[1] = fin.buff[2];//QIDI 923.01 数据
mpu_Data_value.Gyro[2] = -fin.buff[3];
mpu_Data_value.Accel[0] = -fin.buff[4];// m/s2
mpu_Data_value.Accel[1] = fin.buff[5];
mpu_Data_value.Accel[2] = -fin.buff[6];
#endif
mpu_Data_value.Temp = fin.buff[26];//imu temprature
timu = mpu_Data_value.time;
//19开始是定位纬度整数
if (fin.buff[20] == 0&& fin.buff[19] == 0)
{ gpsUpdate = false;
}
else
{ gpsUpdate = true;
}
if (gpsUpdate)//纬度整数位,以及小数位比较
{
//vtg.cogt = atan2(fin.buff[16], fin.buff[17])/glv.deg;//deg 注意PSINS里边的航向是北偏西为正+-pi
//(vtg.cogt > 0.0)? vtg.cogt:(vtg.cogt + 360.0);
//vtg.kph = norm(*(CVect3*)&fin.buff[16])*3.6;
//vtg.cogm = fin.buff[16];//venu
//vtg.knots = fin.buff[17];
//vtg.kph = fin.buff[18];
dhv.vel_e = fin.buff[16];//venu
dhv.vel_n = fin.buff[17];
dhv.vel_u = fin.buff[18];
gga.lat_d = int(fin.buff[19]);
gga.lat_m = (fin.buff[19] - gga.lat_d)*60;
gga.lon_d =int( fin.buff[20]);//开发板数据保存成了2位
gga.lon_m = (fin.buff[20] - gga.lon_d)*60;
gga.altitude = fin.buff[21];
#if rawdata916
gga.h_DOP = 1.0;
#else
gga.h_DOP = fin.buff[22];
#endif
//gga_delay= fin.buff[23];
gga.num_satellite = fin.buff[24];
gga.fix = fin.buff[25];
//温度= fin.buff[26];
gps_ok_flag = 1;
}
else
{
gps_ok_flag = 0;
}
Out_Data *sins_out;//对外输出使用
sins_out = SINS_API(&mpu_Data_value, &gga, &vtg, &dhv, gga_delay, gps_ok_flag);
if (i % 1000 == 0) printf("kf time %.1f\n", timu);
}
return 0;
}