forked from berthubert/galmon
-
Notifications
You must be signed in to change notification settings - Fork 0
/
gps.hh
330 lines (275 loc) · 10.4 KB
/
gps.hh
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
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
#pragma once
#include <bitset>
#include <string>
#include <map>
#include "bits.hh"
#include <iostream>
#include <math.h>
std::basic_string<uint8_t> getCondensedGPSMessage(std::basic_string_view<uint8_t> payload);
struct GPSAlmanac
{
int dataid{-1};
int sv;
uint32_t t0a{0};
uint32_t e{0}, sqrtA{0};
int32_t M0, Omega0, deltai, omega, omegadot;
int health;
int af0, af1;
double getMu() const
{
return 3.986005 * pow(10.0, 14.0);
} // m^3/s^2
// same for galileo & gps
double getOmegaE() const { return 7.2921151467 * pow(10.0, -5.0);} // rad/s
double getE() const
{
return ldexp(e, -21);
}
double getT0e() const
{
return ldexp(t0a, 12);
}
double getI0() const
{
return M_PI*0.3 + ldexp(M_PI*deltai, -19);
}
double getOmegadot() const
{
return ldexp(M_PI * omegadot, -38);
}
double getSqrtA() const
{
return ldexp(sqrtA, -11);
}
double getOmega0() const
{
return ldexp(M_PI * Omega0, -23);
}
double getOmega() const
{
return ldexp(M_PI * omega, -23);
}
double getM0() const
{
return ldexp(M_PI * M0, -23);
}
double getIdot() const { return 0; } // radians/s
double getCic() const { return 0; } // radians
double getCis() const { return 0; } // radians
double getCuc() const { return 0; } // radians
double getCus() const { return 0; } // radians
double getCrc() const { return 0; } // meters
double getCrs() const { return 0; } // meters
double getDeltan()const { return 0; } //radians/s
};
struct GPSState
{
struct SVIOD
{
std::bitset<32> words;
int gnssid;
uint32_t t0e;
uint32_t e, sqrtA;
int32_t m0, omega0, i0, omega, idot, omegadot, deltan;
int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0};
// 16 seconds
uint16_t t0c;
// 2^-31 2^-43
int32_t af0 , af1;
// ???
int8_t af2;
uint32_t wn{0}, tow{0};
double getMu() const
{
return 3.986005 * pow(10.0, 14.0);
} // m^3/s^2
// same for galileo & gps
double getOmegaE() const { return 7.2921151467 * pow(10.0, -5.0);} // rad/s
uint32_t getT0e() const { return t0e; }
double getSqrtA() const { return ldexp(sqrtA, -19); }
double getE() const { return ldexp(e, -33); }
double getCuc() const { return ldexp(cuc, -29); } // radians
double getCus() const { return ldexp(cus, -29); } // radians
double getCrc() const { return ldexp(crc, -5); } // meters
double getCrs() const { return ldexp(crs, -5); } // meters
double getM0() const { return ldexp(m0 * M_PI, -31); } // radians
double getDeltan()const { return ldexp(deltan *M_PI, -43); } //radians/s
double getI0() const { return ldexp(i0 * M_PI, -31); } // radians
double getCic() const { return ldexp(cic, -29); } // radians
double getCis() const { return ldexp(cis, -29); } // radians
double getOmegadot() const { return ldexp(omegadot * M_PI, -43); } // radians/s
double getOmega0() const { return ldexp(omega0 * M_PI, -31); } // radians
double getIdot() const { return ldexp(idot * M_PI, -43); } // radians/s
double getOmega() const { return ldexp(omega * M_PI, -31); } // radians
};
GPSAlmanac gpsalma;
uint8_t gpshealth{0};
uint16_t ai0{0};
int16_t ai1{0}, ai2{0};
int BGDE1E5a{0}, BGDE1E5b{0};
bool disturb1{false}, disturb2{false}, disturb3{false}, disturb4{false}, disturb5{false};
uint16_t wn{0}; // we put the "unrolled" week number here!
uint32_t tow{0}; // "last seen"
//
// 2^-30 2^-50 1 8-bit week
int32_t a0{0}, a1{0}, t0t{0}, wn0t{0};
int32_t a0g{0}, a1g{0}, t0g{0}, wn0g{0};
int8_t dtLS{0}, dtLSF{0};
uint16_t wnLSF{0};
uint8_t dn; // leap second day number
// 1 2^-31 2^-43 2^-55 16 second
int ura, af0, af1, af2, t0c; // GPS parameters that should not be here XXX
int gpsiod{-1};
std::map<int, SVIOD> iods;
SVIOD& getEph(int i) { return iods[i]; } // XXXX gps adaptor
void checkCompleteAndClean(int iod)
{
if(iods[iod].words[2] && iods[iod].words[3]) {
if(iods.size() > 1) {
auto tmp = iods[iod];
iods.clear();
iods[iod] = tmp;
}
}
}
bool isComplete(int iod)
{
return iods[iod].words[2] && iods[iod].words[3];
}
};
template<typename T>
int getT0c(const T& eph)
{
return eph.t0c * 16;
}
template<typename T>
std::pair<double, double> getGPSAtomicOffset(int tow, const T& eph)
{
int delta = ephAge(tow, getT0c(eph));
double cur = eph.af0 + ldexp(delta*eph.af1, -12) + ldexp(delta*delta*eph.af2, -24);
double trend = ldexp(eph.af1, -12) + ldexp(2*delta*eph.af2, -24);
// now in units of 2^-31 seconds, which are 0.5 nanoseconds each
double factor = ldexp(1000000000, -31);
return {factor * cur, factor * trend};
}
template<typename T>
std::pair<double, double> getGPSUTCOffset(int tow, int wn, const T& eph)
{
// 2^-30 2^-50 3600
// a0 a1 t0t
int dw = (int)(uint8_t)wn - (int)(uint8_t) eph.wn0t;
int delta = dw*7*86400 + tow - eph.t0t; // this is pre-scaled for GPS..
double cur = eph.a0 + ldexp(1.0*delta*eph.a1, -20);
double trend = ldexp(eph.a1, -20);
// now in units of 2^-30 seconds, which are ~1.1 nanoseconds each
double factor = ldexp(1000000000, -30);
return {factor * cur, factor * trend};
}
// expects input as 24 bit read to to use messages, returns frame number
template<typename T>
int parseGPSMessage(std::basic_string_view<uint8_t> cond, T& out, uint8_t* pageptr=0)
{
using namespace std;
int frame = getbitu(&cond[0], 24+19, 3);
// 10 * 4 bytes in payload now
out.tow = 1.5*(getbitu(&cond[0], 24, 17)*4);
// cerr << "Preamble: "<<getbitu(&cond[0], 0, 8) <<", frame: "<< frame<<", truncated TOW: "<<out.tow<<endl;
if(frame == 1) {
// word 1, word 2 are TLM and HOW
// 2 bits of padding on each word
// word3:
// 1-10: WN
// 11-12: Which codes 00 = invalid, 01 = P-code on, 10 = C/A code on, 11 invalid
// 13-16: URA, 0-15 scale
// 17-22: 0 is ok
// 23-24: MSB of IODC
// word 8:
// 1-8: LSB of IODC
// 9-24:
out.wn = 2048 + getbitu(&cond[0], 2*24, 10);
out.ura = getbitu(&cond[0], 2*24+12, 4);
out.gpshealth = getbitu(&cond[0], 2*24+16, 6);
// cerr<<"GPS Week Number: "<< out.wn <<", URA: "<< (int)out.ura<<", health: "<<
// (int)out.gpshealth <<endl;
out.af2 = getbits(&cond[0], 8*24, 8); // * 2^-55
out.af1 = getbits(&cond[0], 8*24 + 8, 16); // * 2^-43
out.af0 = getbits(&cond[0], 9*24, 22); // * 2^-31
out.t0c = getbits(&cond[0], 7*24 + 8, 16); // * 16
// cerr<<"t0c*16: "<<out.t0c*16<<", af2: "<< (int)out.af2 <<", af1: "<< out.af1 <<", af0: "<<
// out.af0 <<endl;
}
else if(frame == 2) {
out.gpsiod = getbitu(&cond[0], 2*24, 8);
auto& eph = out.getEph(out.gpsiod);
eph.words[2]=1;
eph.t0e = getbitu(&cond[0], 9*24, 16) * 16.0; // WE SCALE THIS FOR THE USER!!
// cerr<<"IODe "<<(int)iod<<", t0e "<< eph.t0e << " = "<< 16* eph.t0e <<"s"<<endl;
eph.e= getbitu(&cond[0], 5*24+16, 32);
// cerr<<"e: "<<ldexp(eph.e, -33)<<", ";
// sqrt(A), 32 bits, 2^-19
eph.sqrtA= getbitu(&cond[0], 7*24+ 16, 32);
// double sqrtA=ldexp(eph.sqrtA, -19); // 2^-19
// cerr<<"Radius: "<<sqrtA*sqrtA<<endl;
eph.crs = getbits(&cond[0], 2*24 + 8, 16); // 2^-5 meters
eph.deltan = getbits(&cond[0], 3*24, 16); // 2^-43 semi-circles/s
eph.m0 = getbits(&cond[0], 3*24+16, 32); // 2^-31 semi-circles
eph.cuc = getbits(&cond[0], 5*24, 16); // 2^-29 RADIANS
eph.cus = getbits(&cond[0], 7*24, 16); // 2^-29 RADIANS
out.checkCompleteAndClean(out.gpsiod);
}
else if(frame == 3) {
out.gpsiod = getbitu(&cond[0], 9*24, 8);
auto& eph = out.getEph(out.gpsiod);
eph.words[3]=1;
eph.cic = getbits(&cond[0], 2*24, 16); // 2^-29 RADIANS
eph.omega0 = getbits(&cond[0], 2*24 + 16, 32); // 2^-31 semi-circles
eph.cis = getbits(&cond[0], 4*24, 16); // 2^-29 radians
eph.i0 = getbits(&cond[0], 4*24 + 16, 32); // 2^-31, semicircles
eph.crc = getbits(&cond[0], 6*24, 16); // 2^-5, meters
eph.omega = getbits(&cond[0], 6*24+16, 32); // 2^-31, semi-circles
eph.omegadot = getbits(&cond[0], 8*24, 24); // 2^-43, semi-circles/s
eph.idot = getbits(&cond[0], 9*24+8, 14); // 2^-43, semi-cirlces/s
out.checkCompleteAndClean(out.gpsiod);
}
else if(frame == 4) { // this is a carousel frame
int page = getbitu(&cond[0], 2*24 + 2, 6);
if(pageptr)
*pageptr=0;
// cerr<<"Frame 4, page "<<page;
if(page == 56) { // 56 is the new 18 somehow? See table 20-V of the ICD
if(pageptr)
*pageptr=18;
out.a0 = getbits(&cond[0], 6*24 , 32); // 2^-30
out.a1 = getbits(&cond[0], 5*24 , 24); // 2^-50
out.t0t = getbitu(&cond[0], 7*24 + 8, 8) * 4096; // WE SCALE THIS FOR THE USER!
out.wn0t = getbitu(&cond[0], 7*24 + 16, 8);
out.dtLS = getbits(&cond[0], 8*24, 8);
out.dtLSF = getbits(&cond[0], 9*24, 8);
// cerr<<": a0: "<<out.a0<<", a1: "<<out.a1<<", t0t: "<< out.t0t * (1<<12) <<", wn0t: "<< out.wn0t<<", rough offset: "<<ldexp(out.a0, -30)<<endl;
// cerr<<"deltaTLS: "<< (int)out.dtLS<<", post "<< (int)out.dtLSF<<endl;
}
//else cerr<<endl;
// page 18 contains UTC -> 56
// page 25 -> 63
// 2-10 -> 25 -> 32 ??
}
if(frame == 5 || frame==4) { // this is a caroussel frame
out.gpsalma.dataid = getbitu(&cond[0], 2*24, 2);
out.gpsalma.sv = getbitu(&cond[0], 2*24+2, 6);
if(pageptr)
*pageptr= out.gpsalma.sv;
out.gpsalma.e = getbitu(&cond[0], 2*24 + 8, 16);
out.gpsalma.t0a = getbitu(&cond[0], 3*24, 8);
out.gpsalma.deltai = getbits(&cond[0], 3*24 +8 , 16);
out.gpsalma.omegadot = getbits(&cond[0], 4*24, 16);
out.gpsalma.health = getbitu(&cond[0], 4*24 +16, 8);
out.gpsalma.sqrtA = getbitu(&cond[0], 5*24, 24);
out.gpsalma.Omega0 = getbits(&cond[0], 6*24, 24);
out.gpsalma.omega = getbits(&cond[0], 7*24, 24);
out.gpsalma.M0 = getbits(&cond[0], 8*24, 24);
out.gpsalma.af0 = (getbits(&cond[0], 9*24, 8) << 3) + getbits(&cond[0], 9*24 +19, 3);
out.gpsalma.af1 = getbits(&cond[0], 9*24 + 8, 11);
// cerr<<"Frame 5, SV: "<<getbitu(&cond[0], 2*32 + 2 +2, 6)<<endl;
}
return frame;
}