gps_pvt 0.1.7 → 0.2.0

Sign up to get free protection for your applications and to get access to all the features.
@@ -0,0 +1,2330 @@
1
+ /**
2
+ * @file Satellite based augmentation system (SBAS)
3
+ * @see DO-229D
4
+ */
5
+
6
+ /*
7
+ * Copyright (c) 2018, M.Naruoka (fenrir)
8
+ * All rights reserved.
9
+ *
10
+ * Redistribution and use in source and binary forms, with or without modification,
11
+ * are permitted provided that the following conditions are met:
12
+ *
13
+ * - Redistributions of source code must retain the above copyright notice,
14
+ * this list of conditions and the following disclaimer.
15
+ * - Redistributions in binary form must reproduce the above copyright notice,
16
+ * this list of conditions and the following disclaimer in the documentation
17
+ * and/or other materials provided with the distribution.
18
+ * - Neither the name of the naruoka.org nor the names of its contributors
19
+ * may be used to endorse or promote products derived from this software
20
+ * without specific prior written permission.
21
+ *
22
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
23
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
24
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
25
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
26
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
27
+ * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
28
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
30
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
31
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
32
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
33
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34
+ *
35
+ */
36
+
37
+ #ifndef __SBAS_H__
38
+ #define __SBAS_H__
39
+
40
+ #include <iostream>
41
+ #include <cmath>
42
+ #include <cstring>
43
+ #include <cstddef>
44
+ #include <map>
45
+ #include <algorithm>
46
+
47
+ #include "GPS.h"
48
+
49
+ template <class FloatT = double>
50
+ class SBAS_Signal {
51
+ public:
52
+ typedef FloatT float_t;
53
+ class G2 : public GPS_Signal<float_t>::PRN {
54
+ public:
55
+ typedef typename GPS_Signal<float_t>::PRN super_t;
56
+ G2(const int &initial_g2) : super_t((unsigned long)initial_g2) {}
57
+ ~G2(){}
58
+ bool get() const {return super_t::_content[9];}
59
+ void next(){
60
+ bool tmp(super_t::_content[1]
61
+ ^ super_t::_content[2]
62
+ ^ super_t::_content[5]
63
+ ^ super_t::_content[7]
64
+ ^ super_t::_content[8]
65
+ ^ super_t::_content[9]);
66
+ super_t::_content <<= 1;
67
+ super_t::_content[0] = tmp;
68
+ }
69
+ };
70
+ };
71
+
72
+ template <class FloatT = double>
73
+ class SBAS_SpaceNode {
74
+ public:
75
+ typedef FloatT float_t;
76
+ typedef GPS_SpaceNode<float_t> gps_space_node_t;
77
+
78
+ #define type_copy(type) \
79
+ typedef typename gps_space_node_t::type type
80
+ type_copy(gps_time_t);
81
+ type_copy(xyz_t);
82
+ type_copy(enu_t);
83
+ type_copy(llh_t);
84
+
85
+ type_copy(u8_t);
86
+ type_copy(s8_t);
87
+ type_copy(u16_t);
88
+ type_copy(s16_t);
89
+ type_copy(u32_t);
90
+ type_copy(s32_t);
91
+
92
+ type_copy(int_t);
93
+ type_copy(uint_t);
94
+ #undef type_copy
95
+
96
+ struct RangingCode {
97
+ int prn;
98
+ int g2_delay_chips;
99
+ int initial_g2;
100
+ float_t lng_deg;
101
+ const char *name;
102
+ typename SBAS_Signal<float_t>::G2 get_G2() const {
103
+ return typename SBAS_Signal<float_t>::G2(initial_g2);
104
+ }
105
+ struct prn_sorter_t {
106
+ bool operator()(const RangingCode *left, const RangingCode *right) const {
107
+ return (left->prn < right->prn);
108
+ }
109
+ };
110
+ struct lng_sorter_t {
111
+ bool operator()(const RangingCode *left, const RangingCode *right) const {
112
+ return (left->lng_deg < right->lng_deg);
113
+ }
114
+ };
115
+ struct lng_sorter2_t {
116
+ float_t base_lng_deg;
117
+ lng_sorter2_t(const float_t &lng_deg) : base_lng_deg(lng_deg) {}
118
+ bool operator()(const RangingCode *left, const RangingCode *right) const {
119
+ float_t delta_l(left->lng_deg - base_lng_deg);
120
+ if(delta_l < 0){delta_l *= -1;}
121
+ if(delta_l >= 180){delta_l = -delta_l + 360;}
122
+ float_t delta_r(right->lng_deg - base_lng_deg);
123
+ if(delta_r < 0){delta_r *= -1;}
124
+ if(delta_r >= 180){delta_r = -delta_r + 360;}
125
+ return (delta_l < delta_r);
126
+ }
127
+ };
128
+ struct name_sorter_t {
129
+ bool operator()(const RangingCode *left, const RangingCode *right) const {
130
+ int cmp(std::strcmp(left->name, right->name));
131
+ return cmp != 0 ? (cmp < 0) : (left->prn < right->prn);
132
+ }
133
+ };
134
+ };
135
+
136
+ struct KnownSatellites {
137
+ typedef std::vector<const RangingCode *> list_t;
138
+ template <typename T>
139
+ static list_t sort(T sorter);
140
+ static const list_t prn_ordered;
141
+ static const list_t longitude_ordered;
142
+ static const list_t name_ordered;
143
+ static list_t nearest_ordered(const float_t &lng_deg){
144
+ return sort(typename RangingCode::lng_sorter2_t(lng_deg));
145
+ }
146
+ };
147
+
148
+ enum MessageType {
149
+ DONT_USE = 0,
150
+ PRN_MASK = 1,
151
+ FAST_CORRECTION_2 = 2,
152
+ FAST_CORRECTION_3 = 3,
153
+ FAST_CORRECTION_4 = 4,
154
+ FAST_CORRECTION_5 = 5,
155
+ INTEGRITY_INFORMATION = 6,
156
+ FAST_CORRECTION_DEGRADATION = 7,
157
+ GEO_NAVIGATION = 9,
158
+ DEGRADATION_PARAMS = 10,
159
+ SBAS_NETWORK_TIME_UTC_OFFSET_PARAMS = 12,
160
+ GEO_SAT_ALNAMACS = 17,
161
+ IONO_GRID_POINT_MASKS = 18,
162
+ MIXED_CORRECTION_FAST_AND_LONG_TERM = 24,
163
+ LONG_TERM_CORRECTION = 25,
164
+ IONO_DELAY_CORRECTION = 26,
165
+ SERVICE_MESSAGE = 27,
166
+ CLOCK_EPHEMERIS_COV_MAT = 28,
167
+ INTERNAL_TEST_MESSAGE = 62,
168
+ NULL_MESSAGES = 63,
169
+ UNSUPPORTED_MESSAGE = 64,
170
+ }; ///< @see Table A-3
171
+
172
+ struct Timing {
173
+ enum {
174
+ DONT_USE_FOR_SAFETY_APPLICATIONS,
175
+ PRN_MASK,
176
+ UDREI,
177
+ FAST_CORRECTIONS,
178
+ LONG_TERM_CORRECTIONS,
179
+ GEO_NAVIGATION_DATA,
180
+ FAST_CORRECTION_DEGRADATION,
181
+ DEGRADATION_PARAMETERS,
182
+ IONOSPHERIC_GRID_MASK,
183
+ IONOSPHERIC_CORRECTIONS,
184
+ UTC_TIMING_DATA,
185
+ ALNAMAC_DATA,
186
+ SERVICE_LEVEL,
187
+ CLOCK_EPHEMERIS_COVARIANCE_MATRIX,
188
+ NUM_OF_TIMING_ITEMS,
189
+ };
190
+ struct values_t {
191
+ int interval, timeout_EN_Route_Terminal_LNAV, timeout_LNAV_VNAV_LP_LPV_approach;
192
+ };
193
+ static const values_t values[NUM_OF_TIMING_ITEMS];
194
+ }; ///< @see Table A-25;
195
+
196
+ struct DataBlock : public gps_space_node_t::DataParser {
197
+ typedef typename gps_space_node_t::DataParser parser_t;
198
+
199
+ #define convert_u(bits, offset_bits, length, name) \
200
+ template <class InputT> \
201
+ static u ## bits ## _t name(const InputT *buf){ \
202
+ return parser_t::template bits2num<u ## bits ## _t, InputT>( \
203
+ buf, offset_bits, length); \
204
+ }
205
+ #define convert_s(bits, offset_bits, length, name) \
206
+ template <class InputT> \
207
+ static s ## bits ## _t name(const InputT *buf){ \
208
+ return (s ## bits ## _t)parser_t::template bits2num<u ## bits ## _t, InputT>( \
209
+ buf, offset_bits) \
210
+ >> (bits - length); \
211
+ }
212
+ #define convert_u_ch(bits, offset_bits, length, bits_per_ch, name) \
213
+ template <class InputT> \
214
+ static u ## bits ## _t name(const InputT *buf, const uint_t &ch){ \
215
+ return parser_t::template bits2num<u ## bits ## _t>( \
216
+ buf, offset_bits + (bits_per_ch * ch), length); \
217
+ }
218
+ #define convert_s_ch(bits, offset_bits, length, bits_per_ch, name) \
219
+ template <class InputT> \
220
+ static s ## bits ## _t name(const InputT *buf, const uint_t &ch){ \
221
+ return (s ## bits ## _t)parser_t::template bits2num<u ## bits ## _t>( \
222
+ buf, offset_bits + (bits_per_ch * ch)) \
223
+ >> (bits - length); \
224
+ }
225
+
226
+ /**
227
+ * Convert bit pattern to indices array having '1'
228
+ * Ex) 0b00100100 => [2, 5]
229
+ * @param buf bit pattern
230
+ * @param indices array to which results are stored
231
+ * @param length length of bits to be inspected
232
+ * @param offset starting offset of bits
233
+ * @return number of '1'
234
+ */
235
+ template <class InputT>
236
+ static u8_t bits2linear(
237
+ const InputT *buf, u8_t *indices,
238
+ const u8_t &length, const u8_t &offset = 0){
239
+ u8_t *hits(indices);
240
+ std::div_t aligned(std::div(offset, (int)sizeof(InputT) * 8));
241
+ buf += aligned.quot;
242
+ InputT compared((InputT)0x1 << (sizeof(InputT) * 8 - aligned.rem - 1));
243
+ // [mask7, mask6, .., mask0], [mask15, mask14, .., mask8], ...
244
+ for(u8_t i(0); i < length; ++i, compared >>= 1){
245
+ if(compared == 0){ // rotate
246
+ compared = ((InputT)0x1 << (sizeof(InputT) * 8 - 1));
247
+ buf++;
248
+ }
249
+ if(*buf & compared){
250
+ *(hits++) = i;
251
+ }
252
+ }
253
+ return hits - indices;
254
+ }
255
+
256
+ convert_u(8, 0, 8, preamble);
257
+ convert_u(8, 8, 6, message_type);
258
+
259
+ struct Type1 { ///< @see Fig. A-6 PRN_MASK
260
+ struct mask_t {
261
+ u8_t valid;
262
+ static const int each_block = 13;
263
+ union {
264
+ u8_t prn_minus_one[210];
265
+ u8_t block[4][each_block];
266
+ };
267
+ };
268
+ template <class InputT>
269
+ static mask_t mask(const InputT *buf, const u8_t &band){
270
+ mask_t res;
271
+ res.valid = bits2linear(buf, res.prn_minus_one, 210, 14); // 14 bit shift
272
+ if(res.valid > 51){res.valid = 0;} // invalid, because up to 51 masks
273
+ return res;
274
+ }
275
+ convert_u(8, 224, 2, iodp);
276
+ };
277
+
278
+ struct Type2_5 { ///< @see Fig. A-7 FAST_CORRECTION_2-5
279
+ convert_u( 8, 14, 2, iodf);
280
+ convert_u( 8, 16, 2, iodp);
281
+ convert_s_ch(16, 18, 12, 12, prc_f); // pseudo range correction (fast)
282
+ convert_u_ch( 8, 174, 4, 4, udrei);
283
+ };
284
+
285
+ struct Type6 { ///< @see Fig. A-8, Table A-5 INTEGRITY_INFORMATION
286
+ convert_u_ch( 8, 14, 2, 2, iodf); // iodf2-5
287
+ convert_u_ch( 8, 22, 4, 4, udrei);
288
+ };
289
+
290
+ struct Type7 { ///< @see Fig. A-9, Table A-7 FAST_CORRECTION_DEGRADATION
291
+ convert_u( 8, 14, 4, t_lat);
292
+ convert_u( 8, 18, 2, iodp);
293
+ convert_u_ch( 8, 22, 4, 4, ai_i);
294
+ };
295
+
296
+ struct Type25 { ///< @see Fig. A-10,11, Table A-10,11 LONG_TERM_CORRECTION
297
+ convert_u_ch( 8, 14, 1, 106, vel_code);
298
+ convert_u_ch( 8, 15, 6, 106, prn_mask);
299
+ convert_u_ch( 8, 21, 8, 106, iod);
300
+ struct V0 {
301
+ convert_s_ch(16, 29, 9, 106, dx);
302
+ convert_s_ch(16, 38, 9, 106, dy);
303
+ convert_s_ch(16, 47, 9, 106, dz);
304
+ convert_s_ch(16, 56, 10, 106, da_f0);
305
+ convert_u_ch( 8, 66, 6, 106, prn_mask_2);
306
+ convert_u_ch( 8, 72, 8, 106, iod_2);
307
+ convert_s_ch(16, 80, 9, 106, dx_2);
308
+ convert_s_ch(16, 89, 9, 106, dy_2);
309
+ convert_s_ch(16, 98, 9, 106, dz_2);
310
+ convert_s_ch(16, 107, 10, 106, da_f0_2);
311
+ convert_u_ch( 8, 117, 2, 106, iodp);
312
+ };
313
+ struct V1 {
314
+ convert_s_ch(16, 29, 11, 106, dx);
315
+ convert_s_ch(16, 40, 11, 106, dy);
316
+ convert_s_ch(16, 51, 11, 106, dz);
317
+ convert_s_ch(16, 62, 11, 106, da_f0);
318
+ convert_s_ch( 8, 73, 8, 106, dx_dot);
319
+ convert_s_ch( 8, 81, 8, 106, dy_dot);
320
+ convert_s_ch( 8, 89, 8, 106, dz_dot);
321
+ convert_s_ch( 8, 97, 8, 106, da_f1);
322
+ convert_u_ch(16, 105, 13, 106, t_0);
323
+ convert_u_ch( 8, 118, 2, 106, iodp);
324
+ };
325
+ };
326
+
327
+ struct Type24 : public Type25 { ///< @see Fig. A-12 MIXED_CORRECTION_FAST_AND_LONG_TERM
328
+ convert_s_ch(16, 14, 12, 12, prc_f); // pseudo range correction (fast)
329
+ convert_u_ch( 8, 86, 4, 4, udrei);
330
+ convert_u( 8, 110, 2, iodp);
331
+ convert_u( 8, 112, 2, block_id); // 1-6(Type2), 14-19(Type3), 27-32(Type4), 40-45(Type5)
332
+ convert_u( 8, 114, 2, iodf);
333
+ // Second half of 106 bits are same as Type25(ch1)
334
+ };
335
+
336
+ struct Type18 { ///< @see Table A-15 IONO_GRID_POINT_MASKS
337
+ convert_u(8, 14, 4, broadcasted_bands);
338
+ convert_u(8, 18, 4, band);
339
+ convert_u(8, 22, 2, iodi);
340
+ struct mask_t {
341
+ u8_t valid;
342
+ static const int each_block = 15;
343
+ union {
344
+ u8_t linear[201];
345
+ u8_t block[14][each_block];
346
+ };
347
+ };
348
+ /**
349
+ * @param band [0,10]
350
+ * @return Number of mask bits
351
+ */
352
+ static u8_t mask_bits(const u8_t &band){
353
+ switch(band){
354
+ case 8:
355
+ return 200;
356
+ case 9:
357
+ case 10:
358
+ return 192;
359
+ default:
360
+ return 201;
361
+ }
362
+ }
363
+ template <class InputT>
364
+ static mask_t mask(const InputT *buf, const u8_t &band){
365
+ mask_t res;
366
+ res.valid = bits2linear(buf, res.linear, mask_bits(band), 24); // 24 bit shift
367
+ return res;
368
+ }
369
+ template <class InputT>
370
+ static mask_t mask(const InputT *buf){
371
+ return mask(buf, band(buf));
372
+ }
373
+ };
374
+
375
+ struct Type26 { ///< @see Table A-16 IONO_DELAY_CORRECTION
376
+ convert_u(8, 14, 4, band);
377
+ convert_u(8, 18, 4, block_id);
378
+ convert_u_ch(16, 22, 9, 13, delay);
379
+ convert_u_ch( 8, 31, 4, 13, error_indicator);
380
+ convert_u(8, 22 + (13 * 15), 2, iodi); // 22 + (13 * 15) + 2 + 7(spare) + 24(parity) = 250
381
+ };
382
+
383
+ struct Type9 { ///< @see Table A-18 GEO_NAVIGATION
384
+ convert_u(16, 22, 13, t0);
385
+ convert_u( 8, 35, 4, ura);
386
+ convert_s(32, 39, 30, x);
387
+ convert_s(32, 69, 30, y);
388
+ convert_s(32, 99, 25, z);
389
+ convert_s(32, 124, 17, dx);
390
+ convert_s(32, 141, 17, dy);
391
+ convert_s(32, 158, 18, dz);
392
+ convert_s(16, 176, 10, ddx);
393
+ convert_s(16, 186, 10, ddy);
394
+ convert_s(16, 196, 10, ddz);
395
+ convert_s(16, 206, 12, a_Gf0);
396
+ convert_s( 8, 218, 8, a_Gf1);
397
+ };
398
+
399
+ struct Type10 { ///< @see Table A-9 Degradation factors
400
+ convert_u(16, 14, 10, B_rcc);
401
+ convert_u(16, 24, 10, C_ltc_lsb);
402
+ convert_u(16, 34, 10, C_ltc_v1);
403
+ convert_u(16, 44, 9, I_ltc_v1);
404
+ convert_u(16, 53, 10, C_ltc_v0);
405
+ convert_u(16, 63, 9, I_ltc_v0);
406
+ convert_u(16, 72, 10, C_geo_lsb);
407
+ convert_u(16, 82, 10, C_geo_v);
408
+ convert_u(16, 92, 9, I_geo);
409
+ convert_u( 8, 101, 6, C_er);
410
+ convert_u(16, 107, 10, C_iono_step);
411
+ convert_u(16, 117, 9, I_iono);
412
+ convert_u(16, 126, 10, C_iono_ramp);
413
+ convert_u( 8, 136, 1, RSS_UDRE);
414
+ convert_u( 8, 137, 1, RSS_iono);
415
+ convert_u( 8, 138, 7, C_covariance); // 138 + 7 + 81(spare) + 24(parity) = 250
416
+ };
417
+
418
+ struct Type17 { ///< @see Table A-17 GEO_SAT_ALNAMACS
419
+ convert_u_ch( 8, 14, 2, 67, id);
420
+ convert_u_ch( 8, 16, 8, 67, prn);
421
+ convert_u_ch( 8, 24, 8, 67, health_status);
422
+ convert_s_ch(16, 32, 15, 67, x);
423
+ convert_s_ch(16, 47, 15, 67, y);
424
+ convert_s_ch(16, 62, 9, 67, z);
425
+ convert_s_ch( 8, 71, 3, 67, x_dot);
426
+ convert_s_ch( 8, 74, 3, 67, y_dot);
427
+ convert_s_ch( 8, 77, 4, 67, z_dot);
428
+ convert_u(16, 215, 11, t0);
429
+ };
430
+
431
+ struct Type12 { // @see Table A-22 SBAS network Time/UTC parameters
432
+ convert_s(32, 14, 24, A1_SNT);
433
+ convert_s(32, 38, 32, A0_SNT);
434
+ convert_u( 8, 70, 8, t_ot);
435
+ convert_u( 8, 78, 8, WN_t);
436
+ convert_s( 8, 86, 8, delta_t_LS);
437
+ convert_u( 8, 94, 8, WN_LSF);
438
+ convert_u( 8, 102, 8, DN);
439
+ convert_s( 8, 110, 8, delta_t_LSF);
440
+ convert_u( 8, 118, 3, UTC_standard_identifier);
441
+ convert_u(32, 121, 20, TOW);
442
+ convert_u(16, 141, 10, WN);
443
+ };
444
+ #undef convert_s_ch
445
+ #undef convert_u_ch
446
+ #undef convert_s
447
+ #undef convert_u
448
+ };
449
+
450
+ struct DegradationFactors {
451
+ float_t B_rcc;
452
+ float_t C_ltc_lsb;
453
+ float_t C_ltc_v1;
454
+ int I_ltc_v1;
455
+ float_t C_ltc_v0;
456
+ int I_ltc_v0;
457
+ float_t C_geo_lsb;
458
+ float_t C_geo_v;
459
+ int I_geo;
460
+ float_t C_er;
461
+ float_t C_iono_step;
462
+ int I_iono;
463
+ float_t C_iono_ramp;
464
+ bool RSS_UDRE;
465
+ bool RSS_iono;
466
+ float_t C_covariance;
467
+
468
+ struct raw_t {
469
+
470
+ enum {
471
+ SF_B_rcc,
472
+ SF_C_ltc_lsb,
473
+ SF_C_ltc_v1,
474
+ SF_C_ltc_v0,
475
+ SF_C_geo_lsb,
476
+ SF_C_geo_v,
477
+ SF_C_er,
478
+ SF_C_iono_step,
479
+ SF_C_iono_ramp,
480
+ SF_C_covariance,
481
+
482
+ SF_NUM,
483
+ };
484
+ static const float_t sf[SF_NUM];
485
+
486
+ template <class InputT>
487
+ static DegradationFactors fetch(const InputT *buf){
488
+ typedef typename DataBlock::Type10 msg_t;
489
+ DegradationFactors res = {
490
+ #define CONVERT(TARGET) \
491
+ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
492
+ CONVERT(B_rcc),
493
+ CONVERT(C_ltc_lsb),
494
+ CONVERT(C_ltc_v1),
495
+ msg_t::I_ltc_v1(buf),
496
+ CONVERT(C_ltc_v0),
497
+ msg_t::I_ltc_v0(buf),
498
+ CONVERT(C_geo_lsb),
499
+ CONVERT(C_geo_v),
500
+ msg_t::I_geo(buf),
501
+ CONVERT(C_er),
502
+ CONVERT(C_iono_step),
503
+ msg_t::I_iono(buf),
504
+ CONVERT(C_iono_ramp),
505
+ (msg_t::RSS_UDRE(buf) == 1),
506
+ (msg_t::RSS_iono(buf) == 1),
507
+ CONVERT(C_covariance),
508
+ #undef CONVERT
509
+ };
510
+ return res;
511
+ }
512
+ };
513
+ };
514
+
515
+ class IonosphericGridPoints {
516
+
517
+ public:
518
+ struct PointProperty {
519
+ float_t delay; // [m]
520
+ float_t sigma2; // [m^2], negative value means "not monitored"
521
+
522
+ struct raw_t {
523
+ u16_t delay;
524
+ u8_t error_indicator;
525
+
526
+ template <class InputT>
527
+ static raw_t fetch(const InputT *buf, const uint_t &ch){
528
+ typedef typename DataBlock::Type26 msg_t;
529
+ raw_t res = {
530
+ msg_t::delay(buf, ch), // delay
531
+ msg_t::error_indicator(buf, ch), // error_indicator
532
+ };
533
+ return res;
534
+ }
535
+
536
+ enum {
537
+ DELAY_DONT_USE = 0x1FF,
538
+ ERROR_INDICATOR_NOT_MONITORED = 15,
539
+ };
540
+
541
+ static const raw_t unavailable;
542
+
543
+ static float_t raw2delay(const u16_t &v){
544
+ return 0.125 * v;
545
+ }
546
+
547
+ static float_t raw2sigma2(const u8_t &v){
548
+ switch(v){ ///< @see Table A-17
549
+ case 0: return 0.0084;
550
+ case 1: return 0.0333;
551
+ case 2: return 0.0749;
552
+ case 3: return 0.1331;
553
+ case 4: return 0.2079;
554
+ case 5: return 0.2994;
555
+ case 6: return 0.4075;
556
+ case 7: return 0.5322;
557
+ case 8: return 0.6735;
558
+ case 9: return 0.8315;
559
+ case 10: return 1.1974;
560
+ case 11: return 1.8709;
561
+ case 12: return 3.3260;
562
+ case 13: return 20.7870;
563
+ case 14: return 187.0826;
564
+ }
565
+ return -1;
566
+ }
567
+ operator PointProperty() const {
568
+ PointProperty res = {
569
+ raw2delay(delay),
570
+ raw2sigma2(error_indicator),
571
+ };
572
+ return res;
573
+ }
574
+ bool is_available() const {
575
+ return (delay < DELAY_DONT_USE)
576
+ && (error_indicator < ERROR_INDICATOR_NOT_MONITORED);
577
+ }
578
+ };
579
+
580
+ bool is_available() const {
581
+ return (sigma2 > 0);
582
+ }
583
+
584
+ static const PointProperty unavailable;
585
+ };
586
+
587
+ struct position_index_t;
588
+
589
+ struct position_t {
590
+ int_t latitude_deg; ///< latitude in degrees, north is positive. [-85, 85].
591
+ int_t longitude_deg; ///< longitude in degrees, east is positive. [-180, 175]
592
+ operator position_index_t() const;
593
+ bool is_predefined() const {
594
+
595
+ // range check
596
+ if((latitude_deg < -85) || (latitude_deg > 85)){return false;}
597
+ if((longitude_deg < -180) || (longitude_deg >= 180)){return false;}
598
+
599
+ // at least, on 5 deg grid
600
+ if((latitude_deg + 85) % 5 != 0){return false;}
601
+ int_t lng_reg(longitude_deg + 180);
602
+ if(lng_reg % 5 != 0){return false;}
603
+
604
+ switch(latitude_deg){
605
+ case 80:
606
+ case -80:
607
+ return false;
608
+ case 85:
609
+ return (lng_reg % 30 == 0); // W180(=0), W150(=30), ...
610
+ case -85:
611
+ return (lng_reg % 30 == 10); // W170(=10), W140(=40), ...
612
+ }
613
+
614
+ if((latitude_deg >= 65) || (latitude_deg <= -65)){
615
+ return (lng_reg % 10 == 0);
616
+ }
617
+
618
+ return true;
619
+ }
620
+ /**
621
+ * Compute longitude difference from another IGP.
622
+ * @param from
623
+ * @return difference (always positive) [0, 360)
624
+ */
625
+ int_t delta_lng(const position_t &from) const {
626
+ int_t res(longitude_deg - from.longitude_deg);
627
+ return (res < 0) ? (res + 360) : res;
628
+ }
629
+ };
630
+ /**
631
+ * Resolve ionospheric grid point position
632
+ * @param band valid range is [0, 10]
633
+ * @param mask_pos valid range is [0, 200 (or 199, 191)] (be careful, not [1, 201])
634
+ * @return (pos_t) grid point position
635
+ * @see Table A-14
636
+ */
637
+ static position_t position(const u8_t &band, const u8_t &mask_pos){
638
+ position_t res;
639
+ if(band <= 8){
640
+ u8_t row_index_28(band & (~(u8_t)0x01)); // where 28 grid points on the same longitude are appeared
641
+ int row_index(0), col_index((int)mask_pos);
642
+ do{
643
+ int points(row_index_28 == row_index ? 28 : 27);
644
+ if(col_index < points){
645
+ col_index -= 2; // col_index => [-2, 24 (or 25)]
646
+ if((points > 27) && (band % 2 == 1)){ // col_index => [-3, 24]
647
+ col_index--;
648
+ }
649
+ break;
650
+ }
651
+ col_index -= points;
652
+ row_index++;
653
+
654
+ points = 23;
655
+ if(col_index < points){break;}
656
+ col_index -= points;
657
+ row_index++;
658
+ }while(row_index < 8);
659
+
660
+ if(row_index < 8){
661
+ res.longitude_deg = -180 + band * 40 + row_index * 5;
662
+ switch(col_index){
663
+ case -3: res.latitude_deg = -85; break;
664
+ case -2: res.latitude_deg = -75; break;
665
+ case -1: res.latitude_deg = -65; break;
666
+ case 23: res.latitude_deg = 65; break;
667
+ case 24: res.latitude_deg = 75; break;
668
+ case 25: res.latitude_deg = 85; break;
669
+ default:
670
+ res.latitude_deg = -55 + col_index * 5;
671
+ break;
672
+ }
673
+ }
674
+ }else if(band <= 10){
675
+ if(mask_pos < 72){
676
+ res.latitude_deg = 60 * ((band == 10) ? -1 : 1);
677
+ res.longitude_deg = mask_pos * 5 - 180;
678
+ }else if(mask_pos < 180){
679
+ std::div_t a(std::div(mask_pos - 72, 36));
680
+ res.latitude_deg = (65 + a.quot * 5) * ((band == 10) ? -1 : 1);
681
+ res.longitude_deg = a.rem * 10 - 180;
682
+ }else if(mask_pos < 192){
683
+ res.latitude_deg = 85;
684
+ res.longitude_deg = (mask_pos - 180) * 30 - 180;
685
+ if(band == 10){
686
+ res.latitude_deg *= -1;
687
+ res.longitude_deg += 10;
688
+ if(res.longitude_deg > 180){
689
+ res.longitude_deg -= 360;
690
+ }
691
+ }
692
+ }
693
+ }
694
+ return res;
695
+ }
696
+
697
+ struct position_index_t {
698
+ int_t lat_index; ///< Latitude index; N85(0), N75(1), N70(2), N65(3), N60(4), ..., 0(16) ..., S60(28), S65(29), S70(30), S75(31), S85(32)
699
+ int_t lng_index; ///< Longitude index; W180(0), W175(1), W170(2), ..., 0(36), ..., E170(70), E175(71)
700
+ enum {
701
+ LAT_INDEX_N85 = 0, // 30 deg grid
702
+ LAT_INDEX_N75 = 1, // 10 deg grid
703
+ LAT_INDEX_N65 = 3, // 10 deg grid
704
+ LAT_INDEX_S65 = 29, // 10 deg grid
705
+ LAT_INDEX_S75 = 31, // 10 deg grid
706
+ LAT_INDEX_S85 = 32, // 30 deg grid
707
+ LAT_INDEX_MAX = LAT_INDEX_S85,
708
+ };
709
+ enum {
710
+ LNG_INDEX_MAX = 71,
711
+ };
712
+ ///< Convert latitude in degrees to index
713
+ static int_t lat2idx(const int_t &lat_deg){
714
+ return (lat_deg == 85)
715
+ ? LAT_INDEX_N85
716
+ : ((lat_deg == -85)
717
+ ? LAT_INDEX_S85
718
+ : (80 - lat_deg) / 5);
719
+ }
720
+ ///< Convert latitude index to degrees
721
+ static int_t idx2lat(const int_t &lat_idx){
722
+ return (lat_idx % 32 == 0)
723
+ ? ((lat_idx == 0) ? 85 : -85)
724
+ : ((16 - lat_idx) * 5);
725
+ }
726
+ ///< Convert longitude in degrees to index
727
+ static int_t lng2idx(const int_t &lng_deg){
728
+ return (lng_deg + 180) / 5;
729
+ }
730
+ ///< Convert longitude index to degrees
731
+ static int_t idx2lng(const int_t &lng_idx){
732
+ return (lng_idx - 36) * 5;
733
+ }
734
+ operator position_t() const {
735
+ position_t res = {idx2lat(lat_index), idx2lng(lng_index)};
736
+ return res;
737
+ }
738
+ };
739
+
740
+ struct pivot_t {
741
+ position_t igp;
742
+ struct {
743
+ float_t latitude_deg, longitude_deg;
744
+ } delta;
745
+ };
746
+ /**
747
+ * Get a pivot IGP, and compute distance (delta) from the IGP.
748
+ * The "pivot" one means "nearest west, and north if in south hemisphere,
749
+ * south if otherwise (in north hemisphere, or on equator), one".
750
+ * If the specified input latitude is not identical to zero, and exactly same as a latitude where IGPs exist,
751
+ * then the return IGP is shifted to the nearest west (north hemisphere) or north (east hemisphere) point.
752
+ * For example:
753
+ * 1) (lat, lng) = (10, 0) => igp = {5, 0}, delta = {5, 0}
754
+ * 2) (lat, lng) = (85, 15) => igp = {75, 10}, delta = {10, 5}
755
+ * @param latitude_deg latitude in degrees; [-90, 90]
756
+ * @param longitude_deg longitude in degrees
757
+ * @return pivot IGP position and delta
758
+ */
759
+ template <class T>
760
+ static pivot_t get_pivot(const T &latitude_deg, const T &longitude_deg){
761
+
762
+ T lng(longitude_deg); // => [-180, 180)
763
+ int_t lng_reg; // => [0, 360), mapping W180(=> 0), ... E180(=> 360)
764
+ {
765
+ if(longitude_deg < -180){
766
+ lng += (((int_t)(-longitude_deg + 180) / 360) * 360); // => [-*, -180) => (-180, 180]
767
+ if(lng == 180){
768
+ lng -= 360; // 180 => -180
769
+ }
770
+ }else{
771
+ lng -= (((int_t)(longitude_deg + 180) / 360) * 360); // => [-180, +*] => [-180, 180)
772
+ }
773
+ lng_reg = 180 + lng; // => [0, 360)
774
+ }
775
+
776
+ pivot_t res;
777
+ if(latitude_deg > 85){
778
+ res.igp.latitude_deg = 85;
779
+ lng_reg = (lng_reg / 90) * 90; // W180, W90, ... @see A 4.4.10.2 d), and P-2
780
+ /*if(latitude_deg == 85){
781
+ // intentionally commented out; when exactly same as lat == 85,
782
+ // a lat-10 deg grid will be used, because of P-2.
783
+ lng_reg = (lng_reg / 30) * 30; // W180, W150, ...
784
+ }*/
785
+ }else if(latitude_deg < -85){
786
+ res.igp.latitude_deg = -85;
787
+ lng_reg = (lng_reg < 40) ? (130 + 180) : (((lng_reg - 40) / 90) * 90 + 40); // W140, W50, ... @see A 4.4.10.2 e), and P-2
788
+ /*if(latitude_deg == -85){
789
+ // intentionally commented out
790
+ lng_reg = (lng_reg < 10) ? (160 + 180) : (((lng_reg - 10) / 30) * 30 + 10); // W170, W140, ...
791
+ }*/
792
+ }else{
793
+ if(latitude_deg > 75){
794
+ res.igp.latitude_deg = 75;
795
+ }else if(latitude_deg < -75){
796
+ res.igp.latitude_deg = -75;
797
+ }else{
798
+ if(latitude_deg >= 0){
799
+ res.igp.latitude_deg = ((int_t)latitude_deg / 5) * 5;
800
+ }else{
801
+ res.igp.latitude_deg = -((int_t)(-latitude_deg) / 5) * 5;
802
+ }
803
+ if((res.igp.latitude_deg == latitude_deg) && (res.igp.latitude_deg != 0)){ // on grid point latitude, see P-2
804
+ res.igp.latitude_deg += (res.igp.latitude_deg > 0 ? -5 : 5);
805
+ }
806
+ }
807
+
808
+ if((res.igp.latitude_deg >= 60) || (res.igp.latitude_deg <= -60)){
809
+ lng_reg = (lng_reg / 10) * 10; // W180, W170, ...
810
+ }else{
811
+ lng_reg = (lng_reg / 5) * 5; // W180, W175, ...
812
+ }
813
+ }
814
+ res.igp.longitude_deg = lng_reg - 180; // [0, 360) => [-180, 180)
815
+ res.delta.latitude_deg = latitude_deg - res.igp.latitude_deg;
816
+ res.delta.longitude_deg = lng - res.igp.longitude_deg;
817
+ if(res.delta.longitude_deg < 0){res.delta.longitude_deg += 360;} // always east (delta.lng >= 0)
818
+ return res;
819
+ }
820
+
821
+ struct trapezoid_t {
822
+ /**
823
+ * igp(2)=[1] -- igp(1)=[0] igp(3)=[2] -- igp(4)=[3]
824
+ * | | in north, | | in south
825
+ * igp(3)=[2] -- igp(4)=[3] igp(2)=[1] -- igp(1)=[0]
826
+ * This is based on Fig.A-19.
827
+ * result of get_pivot() will be assigned to igp(3)=[2].
828
+ *
829
+ * assumption
830
+ * igp[0].lat = igp[1].lat, igp[2].lat = igp[3].lat
831
+ * igp[1].lng < igp[0].lng, igp[2].lng < igp[3].lng, (igp[1].lng is not necessarily indentical to igp[2].lng)
832
+ */
833
+ position_t igp[4];
834
+ bool checked[4]; ///< If igp[i] has been check to be available, true; otherwise, false.
835
+ float_t weight[4];
836
+
837
+ /**
838
+ * @param delta_phi
839
+ * @param delta_lambda
840
+ * @see Fig.A-19
841
+ */
842
+ void compute_weight(const float_t &delta_phi, const float_t &delta_lambda){
843
+ // (A-25)-(A-32)
844
+ float_t
845
+ w_lat10(delta_phi / (igp[1].latitude_deg - igp[2].latitude_deg)), // a.k.a. y_pp
846
+ w_lat23(1. - w_lat10),
847
+ w_lng0((delta_lambda + igp[2].delta_lng(igp[1])) / igp[0].delta_lng(igp[1])),
848
+ w_lng3(delta_lambda / igp[3].delta_lng(igp[2])); // a.k.a. x_pp
849
+ // res = (w_lng0 * [0] + (1. - w_lng0) * [1]) * w_lat10 + ((1. - w_lng3) * [2] + w_lng3 * [3]) * w_lat23;
850
+ weight[0] = w_lng0 * w_lat10;
851
+ weight[1] = (1. - w_lng0) * w_lat10;
852
+ weight[2] = (1. - w_lng3) * w_lat23;
853
+ weight[3] = w_lng3 * w_lat23;
854
+ }
855
+ void compute_weight_pole(const float_t &delta_phi, const float_t &delta_lambda){
856
+ float_t
857
+ y_pp(delta_phi * ((delta_phi < 0) ? -1 : 1) / 10), // (A-33)
858
+ x_pp((1. - y_pp * 2) * (delta_lambda / 90) + y_pp), // (A-34)
859
+ x_pp_inv(1. - x_pp),
860
+ y_pp_inv(1. - y_pp);
861
+ weight[0] = x_pp * y_pp;
862
+ weight[1] = x_pp_inv * y_pp;
863
+ weight[2] = x_pp_inv * y_pp_inv;
864
+ weight[3] = x_pp * y_pp_inv;
865
+ }
866
+ /**
867
+ * @return If extrapolation occurs, false is returned; otherwise true.
868
+ */
869
+ bool compute_weight_three(const float_t &delta_phi, const float_t &delta_lambda,
870
+ const int_t &skip){
871
+ float_t
872
+ y_pp(delta_phi / (igp[1].latitude_deg - igp[2].latitude_deg)),
873
+ x_pp(delta_lambda / igp[3].delta_lng(igp[2]));
874
+ switch(skip){ // assignment rule: sum is 1, weight of non-diagonal point is remain?
875
+ case 0:
876
+ weight[0] = 0;
877
+ weight[1] = y_pp;
878
+ weight[2] = 1. - x_pp - y_pp;
879
+ weight[3] = x_pp;
880
+ if(weight[2] < 0){return false;}
881
+ break;
882
+ case 1:
883
+ weight[0] = y_pp;
884
+ weight[1] = 0;
885
+ weight[2] = 1. - x_pp;
886
+ weight[3] = x_pp - y_pp;
887
+ if(weight[3] < 0){return false;}
888
+ break;
889
+ case 2:
890
+ weight[0] = x_pp + y_pp - 1;
891
+ weight[1] = 1. - x_pp;
892
+ weight[2] = 0;
893
+ weight[3] = 1. - y_pp;
894
+ if(weight[0] < 0){return false;}
895
+ break;
896
+ case 3:
897
+ default:
898
+ weight[0] = x_pp;
899
+ weight[1] = y_pp - x_pp;
900
+ weight[2] = 1. - y_pp;
901
+ weight[3] = 0;
902
+ if(weight[1] < 0){return false;}
903
+ break;
904
+ }
905
+ return true;
906
+ }
907
+ /**
908
+ * @return If three point interpolation is successfully prepared, true; otherwise false.
909
+ */
910
+ bool compute_weight_three(const float_t &delta_phi, const float_t &delta_lambda){
911
+ for(int i(0); i <= 3; i++){
912
+ if(!checked[i]){ // automatically find unavailable point
913
+ return compute_weight_three(delta_phi, delta_lambda, i);
914
+ }
915
+ }
916
+ return false;
917
+ }
918
+
919
+ PointProperty compute_property(
920
+ #if defined(SWIG) // work around for SWIG parser error
921
+ const typename PointProperty::raw_t *selected
922
+ #else
923
+ const typename PointProperty::raw_t *(&selected)[4]
924
+ #endif
925
+ ){
926
+ float_t delay_raw(0), sigma2(0);
927
+ bool use_sigma(true);
928
+ for(int i(0); i <= 3; i++){
929
+ if(!checked[i]){continue;}
930
+ delay_raw += weight[i] * selected[i]->delay;
931
+ if(selected[i]->error_indicator == PointProperty::raw_t::ERROR_INDICATOR_NOT_MONITORED){
932
+ use_sigma = false;
933
+ }else{
934
+ sigma2 += weight[i] * PointProperty::raw_t::raw2sigma2(selected[i]->error_indicator);
935
+ }
936
+ }
937
+ PointProperty res = {
938
+ PointProperty::raw_t::raw2delay(delay_raw),
939
+ use_sigma
940
+ ? sigma2
941
+ : PointProperty::raw_t::raw2sigma2(PointProperty::raw_t::ERROR_INDICATOR_NOT_MONITORED),
942
+ };
943
+ return res;
944
+ }
945
+
946
+ static trapezoid_t generate_rectangle(const position_t &pivot,
947
+ const int_t &delta_lat = 5, const int_t &delta_lng = 5){
948
+ int_t lng(pivot.longitude_deg + delta_lng);
949
+ if(lng >= 180){lng -= 360;}
950
+ trapezoid_t res = {{
951
+ {pivot.latitude_deg + delta_lat, lng},
952
+ {pivot.latitude_deg + delta_lat, pivot.longitude_deg},
953
+ pivot,
954
+ {pivot.latitude_deg, lng},
955
+ }, {
956
+ false, false, false, false,
957
+ }};
958
+ return res;
959
+ }
960
+ static trapezoid_t generate_rectangle_pole(const position_t &pivot){
961
+ int_t
962
+ lng0(pivot.longitude_deg - 180),
963
+ lng1(pivot.longitude_deg - 90),
964
+ lng3(pivot.longitude_deg + 90);
965
+ trapezoid_t res = {{
966
+ {pivot.latitude_deg, (lng0 < -180) ? (lng0 + 360) : lng0},
967
+ {pivot.latitude_deg, (lng1 < -180) ? (lng1 + 360) : lng1},
968
+ pivot,
969
+ {pivot.latitude_deg, (lng3 >= 180) ? (lng3 - 360) : lng3},
970
+ }, {
971
+ false, false, false, false,
972
+ }};
973
+ return res;
974
+ }
975
+ /**
976
+ * Get expanded rectangle
977
+ *
978
+ * @param delta_lat When positive, move a parallel between igp[1] and igp[0] toward the nearer pole;
979
+ * when negative, move a parallel between igp[2] and igp[3] toward the other pole.
980
+ * Be careful, when negative, the difference of IPP from pivot, i.e. igp[2], should be recalculated.
981
+ *
982
+ * @param delta_lng When positive, move a meridian between igp[0] and igp[3] east;
983
+ * when negative, move a meridian between igp[1] and igp[2] west.
984
+ * Be careful, when negative, the difference of IPP from pivot, i.e. igp[2], should be recalculated.
985
+ *
986
+ * @return Expanded rectangle
987
+ */
988
+ trapezoid_t expand_rectangle(const int_t &delta_lat, const int_t &delta_lng) const {
989
+ trapezoid_t res(*this);
990
+ if(delta_lat != 0){
991
+ int_t delta_lat2(delta_lat * ((res.igp[1].latitude_deg >= 0) ? 1 : -1)); // check hemisphere
992
+ if(delta_lat > 0){
993
+ res.igp[1].latitude_deg = (res.igp[0].latitude_deg += delta_lat2);
994
+ res.checked[1] = res.checked[0] = false;
995
+ }else{
996
+ res.igp[2].latitude_deg = (res.igp[3].latitude_deg += delta_lat2);
997
+ res.checked[2] = res.checked[3] = false;
998
+ }
999
+ }
1000
+ if(delta_lng > 0){
1001
+ res.igp[0].longitude_deg += delta_lng;
1002
+ if(res.igp[0].longitude_deg >= 180){res.igp[0].longitude_deg -= 360;}
1003
+ res.igp[3].longitude_deg = res.igp[0].longitude_deg;
1004
+ res.checked[3] = res.checked[0] = false;
1005
+ }else if(delta_lng < 0){
1006
+ res.igp[1].longitude_deg += delta_lng;
1007
+ if(res.igp[1].longitude_deg < -180){res.igp[1].longitude_deg += 360;}
1008
+ res.igp[2].longitude_deg = res.igp[1].longitude_deg;
1009
+ res.checked[2] = res.checked[1] = false;
1010
+ }
1011
+ return res;
1012
+ }
1013
+ };
1014
+
1015
+ protected:
1016
+ typename PointProperty::raw_t properties[position_index_t::LAT_INDEX_MAX + 1][position_index_t::LNG_INDEX_MAX + 1];
1017
+
1018
+ public:
1019
+ template <class T>
1020
+ T check_availability_hook(trapezoid_t &in, const T &out) const {
1021
+ return out; // for debug
1022
+ }
1023
+ /**
1024
+ * @return available IGP(s)
1025
+ */
1026
+ int_t check_availability(trapezoid_t &target,
1027
+ #if defined(SWIG) // work around for SWIG parser error
1028
+ const typename PointProperty::raw_t *cache
1029
+ #else
1030
+ const typename PointProperty::raw_t *(&cache)[4]
1031
+ #endif
1032
+ ) const {
1033
+ int_t res(0);
1034
+ for(int i(0); i < 4; ++i){
1035
+ if(target.checked[i]){res++; continue;}
1036
+ position_index_t index(target.igp[i]);
1037
+ if((cache[i] = &properties[index.lat_index][index.lng_index])->is_available()){
1038
+ target.checked[i] = true;
1039
+ res++;
1040
+ }
1041
+ }
1042
+ return check_availability_hook(target, res);
1043
+ }
1044
+
1045
+ /**
1046
+ * Select appropriate grids and perform interpolation with the selected grids
1047
+ *
1048
+ * @param latitude_deg Latitude in degrees
1049
+ * @param longitude_deg Longitude in degrees
1050
+ * @return Interpolated result; if interpolation fails, PointProperty::unavailable will be returned.
1051
+ * @see A.4.4.10.2, A.4.4.10.3
1052
+ */
1053
+ PointProperty interpolate(const float_t &latitude_deg, const float_t &longitude_deg) const {
1054
+ pivot_t pivot(get_pivot(latitude_deg, longitude_deg));
1055
+ const typename PointProperty::raw_t *selected[4];
1056
+
1057
+ bool north_hemisphere(latitude_deg >= 0);
1058
+ int_t lat_deg_abs(pivot.igp.latitude_deg * (north_hemisphere ? 1 : -1));
1059
+
1060
+ if(lat_deg_abs <= 55){
1061
+ trapezoid_t rect_5_5(trapezoid_t::generate_rectangle(pivot.igp, north_hemisphere ? 5 : -5, 5)); // A4.4.10.2 a-1)
1062
+ switch(check_availability(rect_5_5, selected)){
1063
+ case 4: // a-1)
1064
+ rect_5_5.compute_weight(pivot.delta.latitude_deg, pivot.delta.longitude_deg);
1065
+ return rect_5_5.compute_property(selected);
1066
+ case 3: // a-2)
1067
+ if(rect_5_5.compute_weight_three(pivot.delta.latitude_deg, pivot.delta.longitude_deg)){
1068
+ return rect_5_5.compute_property(selected);
1069
+ }
1070
+ }
1071
+
1072
+ struct {
1073
+ trapezoid_t rect;
1074
+ float_t delta_lat, delta_lng;
1075
+ int_t availability;
1076
+ } rect_10_10[] = { // A4.4.10.2 a-3), 5x5 => 10x10
1077
+ {rect_5_5.expand_rectangle( 5, 5), pivot.delta.latitude_deg, pivot.delta.longitude_deg},
1078
+ {rect_5_5.expand_rectangle( 5, -5), pivot.delta.latitude_deg, pivot.delta.longitude_deg + 5},
1079
+ {rect_5_5.expand_rectangle(-5, 5),
1080
+ pivot.delta.latitude_deg + (north_hemisphere ? 5 : -5), pivot.delta.longitude_deg},
1081
+ {rect_5_5.expand_rectangle(-5, -5),
1082
+ pivot.delta.latitude_deg + (north_hemisphere ? 5 : -5), pivot.delta.longitude_deg + 5},
1083
+ };
1084
+ for(int i(0); i < 4; ++i){ // a-3) four point interpolation
1085
+
1086
+ if((lat_deg_abs == 55)
1087
+ && (rect_10_10[i].rect.igp[1].latitude_deg * (north_hemisphere ? 1 : -1) == 65)
1088
+ && ((rect_10_10[i].rect.igp[1].longitude_deg + 180) % 10 != 0)){
1089
+ // When pivot lat = 55, -55, one 10x10 trapezoids are unable to be formed,
1090
+ // because of lack of grid points at lat = 65, -65.
1091
+ rect_10_10[i].availability = 0;
1092
+ continue;
1093
+ }
1094
+
1095
+ if((rect_10_10[i].availability = check_availability(rect_10_10[i].rect, selected)) == 4){
1096
+ rect_10_10[i].rect.compute_weight(rect_10_10[i].delta_lat, rect_10_10[i].delta_lng);
1097
+ return rect_10_10[i].rect.compute_property(selected);
1098
+ }
1099
+ }
1100
+ for(int i(0); i < 4; ++i){ // a-4) three point interpolation
1101
+ if((rect_10_10[i].availability == 3)
1102
+ && rect_10_10[i].rect.compute_weight_three(rect_10_10[i].delta_lat, rect_10_10[i].delta_lng)){
1103
+ return rect_10_10[i].rect.compute_property(selected);
1104
+ }
1105
+ }
1106
+ }else if(lat_deg_abs <= 70){
1107
+ trapezoid_t rect_5_10(trapezoid_t::generate_rectangle(pivot.igp, north_hemisphere ? 5 : -5, 10)); // A4.4.10.2 b-1)
1108
+ switch(check_availability(rect_5_10, selected)){
1109
+ case 4: // b-1)
1110
+ rect_5_10.compute_weight(pivot.delta.latitude_deg, pivot.delta.longitude_deg);
1111
+ return rect_5_10.compute_property(selected);
1112
+ case 3: // b-2)
1113
+ if(rect_5_10.compute_weight_three(pivot.delta.latitude_deg, pivot.delta.longitude_deg)){
1114
+ return rect_5_10.compute_property(selected);
1115
+ }
1116
+ }
1117
+
1118
+ struct {
1119
+ trapezoid_t rect;
1120
+ float_t delta_lat, delta_lng;
1121
+ int_t availability;
1122
+ } rect_10_10[] = { // A4.4.10.2 b-3) , 5x10 => 10x10
1123
+ {rect_5_10.expand_rectangle( 5, 0),
1124
+ pivot.delta.latitude_deg, pivot.delta.longitude_deg},
1125
+ {rect_5_10.expand_rectangle(-5, 0),
1126
+ pivot.delta.latitude_deg + (north_hemisphere ? 5 : -5), pivot.delta.longitude_deg},
1127
+ };
1128
+ for(int i(0); i < 2; ++i){ // b-3) four point interpolation
1129
+
1130
+ if((lat_deg_abs == 70)
1131
+ && (rect_10_10[i].rect.igp[1].latitude_deg * (north_hemisphere ? 1 : -1) == 80)){
1132
+ // When pivot lat = 70, -70, one 10x10 trapezoids are unable to be formed,
1133
+ // because of no grid point at lat = 80, -80.
1134
+ rect_10_10[i].availability = 0;
1135
+ continue;
1136
+ }
1137
+
1138
+ if((rect_10_10[i].availability = check_availability(rect_10_10[i].rect, selected)) == 4){
1139
+ rect_10_10[i].rect.compute_weight(rect_10_10[i].delta_lat, rect_10_10[i].delta_lng);
1140
+ return rect_10_10[i].rect.compute_property(selected);
1141
+ }
1142
+ }
1143
+ for(int i(0); i < 2; ++i){ // b-4) three point interpolation
1144
+ if((rect_10_10[i].availability == 3)
1145
+ && rect_10_10[i].rect.compute_weight_three(rect_10_10[i].delta_lat, rect_10_10[i].delta_lng)){
1146
+ return rect_10_10[i].rect.compute_property(selected);
1147
+ }
1148
+ }
1149
+ }else if(lat_deg_abs <= 75){
1150
+ trapezoid_t rect_10_10(trapezoid_t::generate_rectangle(pivot.igp, north_hemisphere ? 10 : -10, 10));
1151
+
1152
+ // maximum 4 kinds of trial
1153
+ // 1) 10x30, both 85 points are band 9-10 (30 deg separation)
1154
+ // 2,3) 10X30, one 85 point is in band 0-8, the other is in band 9-10
1155
+ // 4) 10x90, both 85 points are band 0-8 (90 deg separation)
1156
+
1157
+ int_t lng_85_west_low_band, lng_85_west_high_band;
1158
+ int_t lng_85_east_low_band, lng_85_east_high_band;
1159
+ {
1160
+ int_t lng_reg(pivot.igp.longitude_deg + 180); // [-180, 180) => [0, 360)
1161
+ if(north_hemisphere){
1162
+ lng_85_west_low_band = (lng_reg / 30 * 30) - 180; // W180, W150, ...
1163
+ lng_85_west_high_band = (lng_reg / 90 * 90) - 180; // W180, W90, ...
1164
+ }else{
1165
+ lng_85_west_low_band = (lng_reg < 10) ? 160 : (((lng_reg - 10) / 30) * 30 - 170); // W170, W140, ..., E160
1166
+ lng_85_west_high_band = (lng_reg < 40) ? 130 : (((lng_reg - 40) / 90) * 90 - 140); // W140, W50, ..., E130
1167
+ }
1168
+ lng_85_east_low_band = lng_85_west_low_band + 30;
1169
+ lng_85_east_high_band = lng_85_west_high_band + 90;
1170
+ if(lng_85_east_low_band >= 180){lng_85_east_low_band -= 360;}
1171
+ if(lng_85_east_high_band >= 180){lng_85_east_high_band -= 360;}
1172
+ }
1173
+
1174
+ { // check 1)
1175
+ rect_10_10.igp[1].longitude_deg = lng_85_west_low_band;
1176
+ rect_10_10.igp[0].longitude_deg = lng_85_east_low_band;
1177
+ if(check_availability(rect_10_10, selected) == 4){
1178
+ rect_10_10.compute_weight(pivot.delta.latitude_deg, pivot.delta.longitude_deg);
1179
+ return rect_10_10.compute_property(selected);
1180
+ }
1181
+ }
1182
+
1183
+ if(rect_10_10.checked[2] && rect_10_10.checked[3]){
1184
+ // Requirement: lower latitude point information is broadcasted.
1185
+
1186
+ bool check_again(true);
1187
+
1188
+ do{
1189
+ if(lng_85_west_low_band == lng_85_west_high_band){
1190
+ // |[1]<--(30)-->[0]|----(90)---->|
1191
+ if(rect_10_10.checked[1]){
1192
+ // prepare for the last trial
1193
+ rect_10_10.igp[0].longitude_deg = lng_85_east_high_band;
1194
+ //rect_10_10.checked[0] = false; // already set
1195
+ }else{
1196
+ check_again = false;
1197
+ }
1198
+ break;
1199
+ }
1200
+ if(lng_85_east_low_band == lng_85_east_high_band){
1201
+ // |<----(90)----|[1]<--(30)-->[0]|
1202
+ if(rect_10_10.checked[0]){
1203
+ // prepare for the last trial
1204
+ rect_10_10.igp[1].longitude_deg = lng_85_west_high_band;
1205
+ //rect_10_10.checked[1] = false; // already set
1206
+ }else{
1207
+ check_again = false;
1208
+ }
1209
+ break;
1210
+ }
1211
+
1212
+ // just middle case: |<--(90)--|[1]<--(30)-->[0]|---->|
1213
+
1214
+ if(!rect_10_10.checked[0]){ // check 2) |<--(90)--|[1]<--(30)-->[0]|---->[0]'|
1215
+ rect_10_10.igp[0].longitude_deg = lng_85_east_high_band;
1216
+ //rect_10_10.checked[0] = false; // already set, 1st trial
1217
+ }
1218
+
1219
+ if(!rect_10_10.checked[1]){ // check 3) |[1]'<--(90)--|[1]<--(30)-->[0]|---->|
1220
+ rect_10_10.igp[1].longitude_deg = lng_85_west_high_band;
1221
+ //rect_10_10.checked[1] = false; // already set, 1st trial
1222
+ }
1223
+ }while(false);
1224
+
1225
+ // check 2-4)
1226
+ if(check_again && (check_availability(rect_10_10, selected) == 4)){
1227
+ rect_10_10.compute_weight(pivot.delta.latitude_deg, pivot.delta.longitude_deg);
1228
+ return rect_10_10.compute_property(selected);
1229
+ }
1230
+ }
1231
+ }else{ // pole
1232
+ trapezoid_t rect(trapezoid_t::generate_rectangle_pole(pivot.igp));
1233
+ if(check_availability(rect, selected) == 4){
1234
+ rect.compute_weight_pole(pivot.delta.latitude_deg, pivot.delta.longitude_deg);
1235
+ return rect.compute_property(selected);
1236
+ }
1237
+ }
1238
+
1239
+ // Correction unavailable
1240
+ return PointProperty::unavailable;
1241
+ }
1242
+
1243
+ protected:
1244
+ static const u8_t IODI_INVALID = 4; // valid value ranges from 0 to 3.
1245
+ struct {
1246
+ u8_t iodi;
1247
+ typename DataBlock::Type18::mask_t mask;
1248
+ void clear() {
1249
+ iodi = IODI_INVALID;
1250
+ mask.valid = 0;
1251
+ }
1252
+ } masks[11], masks_new[11]; // "masks" for normal use, "masks_new" for temporal use for iodi transition
1253
+
1254
+ /**
1255
+ * Update mask and IGPs based on new mask having new IODI
1256
+ * @param band band number
1257
+ * @see 2.1.4.9.3 "The equipment shall be able to store and use two IGP masks ..."
1258
+ */
1259
+ void transit_to_new_mask(const u8_t &band) {
1260
+ // remove points which is not activated in the new mask
1261
+ typename DataBlock::Type18::mask_t
1262
+ &mask_old(masks[band].mask), &mask_new(masks_new[band].mask);
1263
+ for(u8_t i(0), j(0); i < mask_old.valid; ++i){
1264
+ bool still_use(false);
1265
+ for(; j < mask_new.valid; ++j){
1266
+ if(mask_old.linear[i] < mask_new.linear[j]){
1267
+ break;
1268
+ }
1269
+ if(mask_old.linear[i] == mask_new.linear[j]){
1270
+ still_use = true;
1271
+ ++j;
1272
+ break;
1273
+ }
1274
+ }
1275
+ if(still_use){continue;}
1276
+ position_index_t index(position(band, mask_old.linear[i]));
1277
+ properties[index.lat_index][index.lng_index] = PointProperty::raw_t::unavailable;
1278
+ }
1279
+ masks[band] = masks_new[band];
1280
+ masks_new[band].clear();
1281
+ }
1282
+
1283
+ void clear_igp() {
1284
+ for(unsigned int i(0); i < sizeof(properties) / sizeof(properties[0]); ++i){
1285
+ for(unsigned int j(0); j < sizeof(properties[0]) / sizeof(properties[0][0]); ++j){
1286
+ properties[i][j] = PointProperty::raw_t::unavailable;
1287
+ }
1288
+ }
1289
+ }
1290
+
1291
+ void clear_mask(){
1292
+ for(unsigned int i(0); i < sizeof(masks) / sizeof(masks[0]); ++i){
1293
+ masks[i].clear();
1294
+ }
1295
+ for(unsigned int i(0); i < sizeof(masks_new) / sizeof(masks_new[0]); ++i){
1296
+ masks_new[i].clear();
1297
+ }
1298
+ }
1299
+ public:
1300
+ /**
1301
+ * Update mask
1302
+ * @param band IGP band
1303
+ * @param iodi_new Issue of data, ionospheric
1304
+ * @param mask_new New mask
1305
+ * @return True if mask is up to date, otherwise false.
1306
+ */
1307
+ bool update_mask(const u8_t &band,
1308
+ const u8_t &iodi_new,
1309
+ const typename DataBlock::Type18::mask_t &mask_new){
1310
+ if((masks[band].iodi == iodi_new) || (masks_new[band].iodi == iodi_new)){
1311
+ // lazy decline, because the same iodi means up to date
1312
+ return true;
1313
+ }
1314
+ if(masks[band].iodi == IODI_INVALID){
1315
+ masks[band].iodi = iodi_new;
1316
+ masks[band].mask = mask_new;
1317
+ return true;
1318
+ }
1319
+ if(masks_new[band].iodi == IODI_INVALID){
1320
+ masks_new[band].iodi = iodi_new;
1321
+ masks_new[band].mask = mask_new;
1322
+ return true;
1323
+ }
1324
+ return false;
1325
+ }
1326
+ /**
1327
+ * Update mask with broadcasted data
1328
+ * @param type18 Type 18 message
1329
+ * @return True if mask is updated and changed, otherwise false.
1330
+ */
1331
+ template <class Input>
1332
+ bool update_mask(const Input *type18){
1333
+ typedef typename DataBlock::Type18 msg_t;
1334
+ u8_t band(msg_t::band(type18));
1335
+ return update_mask(band, msg_t::iodi(type18), msg_t::mask(type18, band));
1336
+ }
1337
+
1338
+ /**
1339
+ * Register new ionospheric grind point property
1340
+ * @param IGP position
1341
+ * @param prop New property
1342
+ * @return Always true
1343
+ */
1344
+ bool register_igp(const position_t &pos, const typename PointProperty::raw_t &prop){
1345
+ position_index_t index(pos);
1346
+ properties[index.lat_index][index.lng_index] = prop;
1347
+ return true;
1348
+ }
1349
+ /**
1350
+ * Register new properties of ionospheric grid points (IGPs) with broadcasted data
1351
+ * @param type26 Type 26 message
1352
+ * @return True if IGPs are registered, otherwise false
1353
+ */
1354
+ template <class InputT>
1355
+ bool register_igp(const InputT *type26){
1356
+ typedef typename DataBlock::Type26 msg_t;
1357
+ u8_t band(msg_t::band(type26)), iodi(msg_t::iodi(type26));
1358
+ if(masks[band].iodi != iodi){
1359
+ if(masks_new[band].iodi != iodi){
1360
+ return false;
1361
+ }
1362
+ // Change mask due to new arrival of IODI
1363
+ transit_to_new_mask(band);
1364
+ }
1365
+ u8_t *mask_pos(masks[band].mask.block[msg_t::block_id(type26)]);
1366
+ int i_max(masks[band].mask.valid - (mask_pos - masks[band].mask.linear));
1367
+ if(i_max > DataBlock::Type18::mask_t::each_block){
1368
+ i_max = DataBlock::Type18::mask_t::each_block;
1369
+ }
1370
+ for(int i(0); i < i_max; ++i, ++mask_pos){
1371
+ register_igp(position(band, *mask_pos), PointProperty::raw_t::fetch(type26, i));
1372
+ }
1373
+ return true;
1374
+ }
1375
+
1376
+ IonosphericGridPoints(){
1377
+ clear_igp();
1378
+ clear_mask();
1379
+ }
1380
+ virtual ~IonosphericGridPoints(){}
1381
+
1382
+ /**
1383
+ * Print delay map with ASCII characters
1384
+ */
1385
+ friend std::ostream &operator<<(std::ostream &out, const IonosphericGridPoints &igp){
1386
+ static const char base32_chr[] = "0123456789ABCdEFGHiJKLMNoPQRSTUV"; // base 32
1387
+
1388
+ // the first line
1389
+ out << " ";
1390
+ for(int j(0); j < sizeof(igp.properties[0]) / sizeof(igp.properties[0][0]); ++j){
1391
+ int lng(position_index_t::idx2lng(j));
1392
+ out << (lng == 0 ? '0' : (lng % 90 == 0 ? '|' : (lng % 30 == 0 ? '+' : ' ')));
1393
+ }
1394
+ out << std::endl;
1395
+
1396
+ for(int i(0); i < sizeof(igp.properties) / sizeof(igp.properties[0]); ++i){
1397
+ out.width(4);
1398
+ out << position_index_t::idx2lat(i) << ' ';
1399
+ for(int j(0); j < sizeof(igp.properties[0]) / sizeof(igp.properties[0][0]); ++j){
1400
+ if(!igp.properties[i][j].is_available()){
1401
+ out << ' ';
1402
+ }else{
1403
+ out << base32_chr[igp.properties[i][j].delay >> 4]; // 0-510 => 0-31
1404
+ }
1405
+ }
1406
+ out << std::endl;
1407
+ }
1408
+ return out;
1409
+ }
1410
+
1411
+ /**
1412
+ * Calculate correction value in accordance with ionospheric model
1413
+ *
1414
+ * @param relative_pos satellite position (relative position, NEU)
1415
+ * @param usrllh user position (absolute position, LLH)
1416
+ * @return correction in meters
1417
+ * @see A.4.4.10
1418
+ */
1419
+ PointProperty iono_correction(
1420
+ const enu_t &relative_pos,
1421
+ const llh_t &usrllh) const {
1422
+
1423
+ // A.4.4.10.1 Pierce point calculation
1424
+ typename gps_space_node_t::pierce_point_res_t pp(gps_space_node_t::pierce_point(relative_pos, usrllh));
1425
+
1426
+ // A.4.4.10.2 IGP selection, and A.4.4.10.3 Interpolation
1427
+ PointProperty prop(interpolate(pp.latitude / M_PI * 180, pp.longitude / M_PI * 180));
1428
+ if(prop.is_available()){
1429
+
1430
+ // A.4.4.10.4 Compute slant delay
1431
+ float_t fpp(gps_space_node_t::slant_factor(relative_pos));
1432
+ prop.delay *= fpp; // (A-41)
1433
+ prop.sigma2 *= (fpp * fpp); // (A-43)
1434
+ }
1435
+
1436
+ return prop;
1437
+ }
1438
+ };
1439
+
1440
+ class IonosphericGridPoints_with_Timeout : public IonosphericGridPoints {
1441
+ protected:
1442
+ gps_time_t t_last_mask;
1443
+ gps_time_t t_last_correction;
1444
+
1445
+ bool is_mask_timeout(const gps_time_t &t_reception, const bool &LNAV_VNAV_LP_LPV_approach){
1446
+ float_t delta_t(t_last_mask.interval(t_reception));
1447
+ return LNAV_VNAV_LP_LPV_approach
1448
+ ? (delta_t >= Timing::values[Timing::IONOSPHERIC_GRID_MASK].timeout_LNAV_VNAV_LP_LPV_approach)
1449
+ : (delta_t >= Timing::values[Timing::IONOSPHERIC_GRID_MASK].timeout_EN_Route_Terminal_LNAV);
1450
+ }
1451
+ bool is_correction_timeout(const gps_time_t &t_reception, const bool &LNAV_VNAV_LP_LPV_approach){
1452
+ float_t delta_t(t_last_correction.interval(t_reception));
1453
+ return LNAV_VNAV_LP_LPV_approach
1454
+ ? (delta_t >= Timing::values[Timing::IONOSPHERIC_CORRECTIONS].timeout_LNAV_VNAV_LP_LPV_approach)
1455
+ : (delta_t >= Timing::values[Timing::IONOSPHERIC_CORRECTIONS].timeout_EN_Route_Terminal_LNAV);
1456
+ }
1457
+
1458
+ public:
1459
+ IonosphericGridPoints_with_Timeout()
1460
+ : IonosphericGridPoints(),
1461
+ t_last_mask(0, 0), t_last_correction(0, 0) {}
1462
+
1463
+ using IonosphericGridPoints::update_mask;
1464
+ using IonosphericGridPoints::register_igp;
1465
+ using IonosphericGridPoints::iono_correction;
1466
+
1467
+ template <class Input>
1468
+ bool update_mask(
1469
+ const Input *type18,
1470
+ const gps_time_t &t_reception, const bool &LNAV_VNAV_LP_LPV_approach = false){
1471
+
1472
+ if(is_mask_timeout(t_reception, LNAV_VNAV_LP_LPV_approach)){
1473
+ IonosphericGridPoints::clear_mask();
1474
+ }
1475
+ bool res(update_mask(type18));
1476
+ if(res){
1477
+ t_last_mask = t_reception;
1478
+ }
1479
+ return res;
1480
+ }
1481
+
1482
+ template <class InputT>
1483
+ bool register_igp(
1484
+ const InputT *type26,
1485
+ const gps_time_t &t_reception, const bool &LNAV_VNAV_LP_LPV_approach = false){
1486
+
1487
+ if(is_mask_timeout(t_reception, LNAV_VNAV_LP_LPV_approach)){
1488
+ IonosphericGridPoints::clear_mask();
1489
+ return false;
1490
+ }
1491
+ if(is_correction_timeout(t_reception, LNAV_VNAV_LP_LPV_approach)){
1492
+ IonosphericGridPoints::clear_igp();
1493
+ }
1494
+ bool res(register_igp(type26));
1495
+ if(res){
1496
+ t_last_correction = t_reception;
1497
+ }
1498
+ return res;
1499
+ }
1500
+
1501
+ typename IonosphericGridPoints::PointProperty iono_correction(
1502
+ const enu_t &relative_pos, const llh_t &usrllh,
1503
+ const gps_time_t &t, const bool &LNAV_VNAV_LP_LPV_approach = false) const {
1504
+
1505
+ if(is_correction_timeout(t, LNAV_VNAV_LP_LPV_approach)){
1506
+ return IonosphericGridPoints::PointProperty::unavailable;
1507
+ }
1508
+ return iono_correction(relative_pos, usrllh);
1509
+ }
1510
+ };
1511
+
1512
+ /**
1513
+ * Calculate Sagnac correction range in meter, which must be accounted
1514
+ * for residual calculation of pseudo range.
1515
+ *
1516
+ * @param sat_pos Satellite position
1517
+ * @param usr_pos User position
1518
+ * @return correction range
1519
+ * @see A.4.4.11
1520
+ * @see SBAS_SpaceNode::Satellite::Ephemeris::constellation
1521
+ */
1522
+ static float_t sagnac_correction(
1523
+ const xyz_t sat_pos,
1524
+ const xyz_t usr_pos) {
1525
+ return WGS84::Omega_Earth_IAU
1526
+ * (sat_pos.x() * usr_pos.y() - sat_pos.y() * usr_pos.x())
1527
+ / gps_space_node_t::light_speed;
1528
+ }
1529
+
1530
+ /**
1531
+ * Calculate correction value in accordance with tropospheric model
1532
+ *
1533
+ * @param year_utc UTC floating-point year
1534
+ * @param relative_pos satellite position (relative position, NEU)
1535
+ * @param usrllh user position (absolute position, LLH)
1536
+ * @see A.4.2.4
1537
+ * @return correction in meters
1538
+ */
1539
+ float_t tropo_correction(
1540
+ const float_t &year_utc,
1541
+ const enu_t &relative_pos,
1542
+ const llh_t &usrllh) const {
1543
+
1544
+ if(usrllh.height() > 10E3){ // same as RTKlib; troposphere ranges from 0 to approximately 11km
1545
+ return 0;
1546
+ }
1547
+
1548
+ union MeteologicalParameter {
1549
+ struct {
1550
+ float_t p, T, e, beta, lambda; // mbar, K, mbar, K/m, [dimless]
1551
+ };
1552
+ float_t v[5];
1553
+ };
1554
+
1555
+ static const struct {
1556
+ MeteologicalParameter average, seasonal_variation;
1557
+ } preset[] = {
1558
+ {{1013.25, 299.65, 26.31, 6.30E-3, 2.77}, { 0.00, 0.00, 0.00, 0.00E-3, 0.00}}, // 15
1559
+ {{1017.25, 294.15, 21.79, 6.05E-3, 3.15}, {-3.75, 7.00, 8.85, 0.25E-3, 0.33}}, // 30
1560
+ {{1015.75, 283.15, 11.66, 5.58E-3, 2.57}, {-2.25, 11.00, 7.24, 0.32E-3, 0.46}}, // 45
1561
+ {{1011.75, 272.15, 6.78, 5.39E-3, 1.81}, {-1.75, 15.00, 5.36, 0.81E-3, 0.74}}, // 60
1562
+ {{1013.00, 263.65, 4.11, 4.53E-3, 1.55}, {-0.50, 14.50, 3.39, 0.62E-3, 0.30}}, // 75
1563
+ }; // Table A-2
1564
+ static const int preset_langth(sizeof(preset) / sizeof(preset[0]));
1565
+
1566
+ float_t preset_idx_f(std::abs(usrllh.latitude()) / (M_PI / 180 * 15));
1567
+ int preset_idx(preset_idx_f);
1568
+
1569
+ MeteologicalParameter average, seasonal_variation;
1570
+ if(preset_idx == 0){
1571
+ average = preset[preset_idx].average;
1572
+ seasonal_variation = preset[preset_idx].seasonal_variation;
1573
+ }else if(preset_idx >= (preset_langth - 1)){
1574
+ average = preset[preset_langth - 1].average;
1575
+ seasonal_variation = preset[preset_langth - 1].seasonal_variation;
1576
+ }else{
1577
+ // linear interpolation
1578
+ float_t
1579
+ weight_b(preset_idx_f - preset_idx),
1580
+ weight_a(1. - weight_b);
1581
+ for(std::size_t j(0); j < sizeof(average.v) / sizeof(average.v[0]); ++j){
1582
+ average.v[j]
1583
+ = preset[preset_idx].average.v[j] * weight_a
1584
+ + preset[preset_idx + 1].average.v[j] * weight_b; // (A-4)
1585
+ seasonal_variation.v[j]
1586
+ = preset[preset_idx].seasonal_variation.v[j] * weight_a
1587
+ + preset[preset_idx + 1].seasonal_variation.v[j] * weight_b; // (A-5)
1588
+ }
1589
+ }
1590
+
1591
+ float_t d_hyd, d_wet;
1592
+ {
1593
+ // (A-3)
1594
+ MeteologicalParameter param;
1595
+ {
1596
+ float_t Dmin_year(((usrllh.latitude() < 0) ? 211 : 28) / 365.25);
1597
+ float_t year_int;
1598
+ float_t k(std::cos(M_PI * 2 * (std::modf(year_utc, &year_int) - Dmin_year)));
1599
+ for(std::size_t j(0); j < sizeof(param.v) / sizeof(param.v[0]); ++j){
1600
+ param.v[j] = average.v[j] - seasonal_variation.v[j] * k;
1601
+ }
1602
+ }
1603
+
1604
+ static const float_t
1605
+ k1(77.604), k2(382000), Rd(287.054), gm(9.784); // K/mbar, K^2/mbar, J/(kg*K), m/s^2
1606
+ // Zero-altitude zenith delay term (z_hyd, z_wet)
1607
+ float_t
1608
+ z_hyd(1E-6 * k1 * Rd * param.p / gm), // (A-6)
1609
+ z_wet(1E-6 * k2 * Rd / (gm * (param.lambda + 1) - param.beta * Rd) * param.e / param.T); // (A-7)
1610
+
1611
+ {
1612
+ const float_t &h(usrllh.height()); // Altitude (m)
1613
+ static const float_t g(9.80665); // m/s^2
1614
+ if(h > 0){
1615
+ float_t x(1. - (param.beta * h / param.T)), y(g / Rd / param.beta);
1616
+ d_hyd = std::pow(x, y) * z_hyd; // (A-8)
1617
+ d_wet = std::pow(x, y * (param.lambda + 1) - 1) * z_wet; // (A-9)
1618
+ }else{
1619
+ d_hyd = z_hyd;
1620
+ d_wet = z_wet;
1621
+ }
1622
+ }
1623
+ }
1624
+
1625
+ float_t m_el;
1626
+ {
1627
+ // Elevation (rad)
1628
+ float_t el(relative_pos.elevation());
1629
+ m_el = 1.001 / std::sqrt(0.002001 + std::pow(std::sin(el), 2)); // (A-10a)
1630
+ }
1631
+
1632
+ return -(d_hyd + d_wet) * m_el;
1633
+ }
1634
+
1635
+ /**
1636
+ * UTC parameters
1637
+ *
1638
+ */
1639
+ struct UTC_Parameters {
1640
+ float_t A1; ///< UTC parameter for SBAS network time (s/s)
1641
+ float_t A0; ///< UTC parameter for SBAS network time (s)
1642
+ uint_t t_ot; ///< Epoch time (UTC) (s)
1643
+ uint_t WN_t; ///< Epoch time (UTC) (weeks)
1644
+ int_t delta_t_LS; ///< Current leap seconds (s)
1645
+ uint_t WN_LSF; ///< Last leap second update week (weeks)
1646
+ uint_t DN; ///< Last leap second update day (days)
1647
+ int_t delta_t_LSF; ///< Updated leap seconds (s)
1648
+
1649
+ struct raw_t {
1650
+ s32_t A1; ///< UTC parameter (-50, s/s)
1651
+ s32_t A0; ///< UTC parameter (-30, s)
1652
+ u8_t t_ot; ///< Epoch time (UTC) (12, s)
1653
+ u8_t WN_t; ///< Epoch time (UTC) (weeks, truncated)
1654
+ s8_t delta_t_LS; ///< Current leap seconds (s)
1655
+ u8_t WN_LSF; ///< Last leap second update week (weeks, truncated)
1656
+ u8_t DN; ///< Last leap second update day (days)
1657
+ s8_t delta_t_LSF; ///< Updated leap seconds (s)
1658
+
1659
+ template <class InputT>
1660
+ static raw_t fetch(const InputT *buf){
1661
+ typedef typename DataBlock::Type12 msg_t;
1662
+ raw_t res = {
1663
+ msg_t::A1_SNT(buf),
1664
+ msg_t::A0_SNT(buf),
1665
+ msg_t::t_ot(buf),
1666
+ msg_t::WN_t(buf),
1667
+ msg_t::delta_t_LS(buf),
1668
+ msg_t::WN_LSF(buf),
1669
+ msg_t::DN(buf),
1670
+ msg_t::delta_t_LSF(buf),
1671
+ };
1672
+ return res;
1673
+ }
1674
+
1675
+ enum {
1676
+ SF_A1,
1677
+ SF_A0,
1678
+
1679
+ SF_NUM,
1680
+ };
1681
+ static const float_t sf[SF_NUM];
1682
+
1683
+ operator UTC_Parameters() const {
1684
+ UTC_Parameters converted;
1685
+ #define CONVERT(TARGET) \
1686
+ {converted.TARGET = sf[SF_ ## TARGET] * TARGET;}
1687
+ CONVERT(A1);
1688
+ CONVERT(A0);
1689
+ converted.t_ot = ((uint_t)t_ot) << 12;
1690
+ converted.WN_t = WN_t;
1691
+ converted.delta_t_LS = delta_t_LS;
1692
+ converted.WN_LSF = WN_LSF;
1693
+ converted.DN = DN;
1694
+ converted.delta_t_LSF = delta_t_LSF;
1695
+ #undef CONVERT
1696
+ return converted;
1697
+ };
1698
+ };
1699
+ };
1700
+
1701
+ class SatelliteProperties {
1702
+ public:
1703
+ typedef typename gps_space_node_t::SatelliteProperties::constellation_t constellation_t;
1704
+
1705
+ /**
1706
+ * SBAS Ephemeris
1707
+ * @see Table A-18
1708
+ */
1709
+ struct Ephemeris {
1710
+ uint_t svid; ///< Satellite number
1711
+ uint_t WN; ///< Week number
1712
+
1713
+ float_t t_0; ///< Time of applicability (s) <= time of a week
1714
+ int_t URA; ///< User range accuracy (index)
1715
+ float_t x, y, z; ///< ECEF position (m)
1716
+ float_t dx, dy, dz; ///< ECEF velocity (m/s)
1717
+ float_t ddx, ddy, ddz; ///< ECEF acceleration (m/s^2)
1718
+ float_t a_Gf0; ///< Clock correction parameter (s)
1719
+ float_t a_Gf1; ///< Clock correction parameter (s/s)
1720
+
1721
+ /**
1722
+ * Adjust time of ephemeris with time of current
1723
+ */
1724
+ void adjust_time(const gps_time_t &t_current){
1725
+ WN = t_current.week;
1726
+ float_t sec_of_a_day(std::fmod(t_current.seconds, gps_time_t::seconds_day)), t_0_orig(t_0);
1727
+ t_0 += (t_current.seconds - sec_of_a_day);
1728
+
1729
+ // roll over check
1730
+ float_t delta(sec_of_a_day - t_0_orig);
1731
+ if(delta > (gps_time_t::seconds_day / 4 * 3)){
1732
+ // 0 --> t_0 ---------(3/4)---------> current --> day
1733
+ t_0 += gps_time_t::seconds_day; // probably, current --> t_0
1734
+ if(t_0 >= gps_time_t::seconds_week){
1735
+ WN++;
1736
+ t_0 -= gps_time_t::seconds_week;
1737
+ }
1738
+ }else if(-delta > (gps_time_t::seconds_day / 4 * 3)){
1739
+ // 0 --> current ---------(3/4)---------> t_0 --> day
1740
+ t_0 -= gps_time_t::seconds_day; // probably, t_0 --> current
1741
+ if(t_0 < 0){
1742
+ WN--;
1743
+ t_0 += gps_time_t::seconds_week;
1744
+ }
1745
+ }
1746
+ }
1747
+
1748
+ /**
1749
+ * Check availability
1750
+ * @param t target time
1751
+ * @return always true, because timeout, which is based on the reception time,
1752
+ * is the only way to make this ephemeris unavailable.
1753
+ * This class does not take the reception time into account.
1754
+ * @see 2.1.1.4.9, A.4.5.1.3.3
1755
+ */
1756
+ bool is_valid(const gps_time_t &t) const {
1757
+ return ((WN > 0) || (t_0 >= 0));
1758
+ }
1759
+
1760
+ /**
1761
+ * Check fitness based on time of applicability (t_0) of ephemeris
1762
+ * @param t target time
1763
+ * @return true when best fit, otherwise, false
1764
+ * @see A.4.5.1.3.3 (A-56)
1765
+ */
1766
+ bool maybe_better_one_avilable(const gps_time_t &t) const {
1767
+ float_t delta_t(-t.interval(WN, t_0));
1768
+ return (delta_t < 0) || (delta_t > Timing::values[Timing::GEO_NAVIGATION_DATA].interval);
1769
+ }
1770
+
1771
+ constellation_t constellation(
1772
+ const gps_time_t &t_rx, const float_t &pseudo_range = 0,
1773
+ const bool &with_velocity = true) const {
1774
+
1775
+ float_t t_G(-t_rx.interval(WN, 0) - (pseudo_range / gps_space_node_t::light_speed));
1776
+
1777
+ float_t t(t_G - (a_Gf0 + a_Gf1 * (t_G - t_0))); // Eq.(A-45)
1778
+ float_t t_dot(1.0 - a_Gf1);
1779
+
1780
+ float_t delta_t(t - t_0), delta_t2(delta_t * delta_t / 2);
1781
+
1782
+ constellation_t res = {
1783
+ xyz_t(
1784
+ x + dx * delta_t + ddx * delta_t2,
1785
+ y + dy * delta_t + ddy * delta_t2,
1786
+ z + dz * delta_t + ddz * delta_t2), // Eq. (A-44)
1787
+ xyz_t(
1788
+ (dx + ddx * delta_t) * t_dot,
1789
+ (dy + ddy * delta_t) * t_dot,
1790
+ (dz + ddz * delta_t) * t_dot),
1791
+ };
1792
+
1793
+ // Be careful, Sagnac correction must be performed before geometric distance calculation
1794
+ // @see SBAS_SpaceNode::sagnac_correction
1795
+
1796
+ return res;
1797
+ }
1798
+
1799
+ struct raw_t {
1800
+ u8_t svid; ///< Satellite number
1801
+
1802
+ u16_t t_0; ///< Time of applicability (16, s) [0,86384] <= time of a day
1803
+ u8_t URA; ///< User range accuracy
1804
+ s32_t x, y, z; ///< ECEF position (0.08(xy), 0.4(z) m)
1805
+ s32_t dx, dy, dz; ///< ECEF velocity (0.000625(xy), 0.004(z) m)
1806
+ s16_t ddx, ddy, ddz; ///< ECEF acceleration (0.0000125(xy), 0.0000625(z) m/s^2)
1807
+ s16_t a_Gf0; ///< Clock correction parameter (2^-31, s)
1808
+ s8_t a_Gf1; ///< Clock correction parameter (2^-40, s/s)
1809
+
1810
+ template <class InputT>
1811
+ static raw_t fetch(const InputT *buf, const u8_t &sv_number = 0){
1812
+ typedef typename DataBlock::Type9 msg_t;
1813
+ raw_t res = {
1814
+ sv_number, // svid
1815
+ msg_t::t0(buf), // t_0
1816
+ msg_t::ura(buf), // URA
1817
+ msg_t::x(buf), msg_t::y(buf), msg_t::z(buf), // x, y, z
1818
+ msg_t::dx(buf), msg_t::dy(buf), msg_t::dz(buf), // dx, dy, dz
1819
+ msg_t::ddx(buf), msg_t::ddy(buf), msg_t::ddz(buf), // ddx, ddy, ddz
1820
+ msg_t::a_Gf0(buf), msg_t::a_Gf1(buf), // a_Gf0, a_Gf1
1821
+ };
1822
+ return res;
1823
+ }
1824
+
1825
+ enum {
1826
+ SF_t_0,
1827
+ SF_xy, SF_z,
1828
+ SF_dxy, SF_dz,
1829
+ SF_ddxy, SF_ddz,
1830
+ SF_a_Gf0,
1831
+ SF_a_Gf1,
1832
+
1833
+ SF_NUM,
1834
+ };
1835
+ static const float_t sf[SF_NUM];
1836
+
1837
+ operator Ephemeris() const {
1838
+ Ephemeris converted;
1839
+ #define CONVERT2(TARGET, TARGET_SF) \
1840
+ {converted.TARGET = sf[SF_ ## TARGET_SF] * TARGET;}
1841
+ #define CONVERT(TARGET) CONVERT2(TARGET, TARGET)
1842
+ converted.svid = svid;
1843
+ converted.WN = 0; // Week number (must be configured later) @see adjust_time
1844
+
1845
+ converted.URA = URA;
1846
+ CONVERT(t_0); // Time of a day => time of a week (must be configured later) @see adjust_time
1847
+ CONVERT2(x, xy); CONVERT2(y, xy); CONVERT(z);
1848
+ CONVERT2(dx, dxy); CONVERT2(dy, dxy); CONVERT(dz);
1849
+ CONVERT2(ddx, ddxy); CONVERT2(ddy, ddxy); CONVERT(ddz);
1850
+ CONVERT(a_Gf0);
1851
+ CONVERT(a_Gf1);
1852
+ #undef CONVERT
1853
+ #undef CONVERT2
1854
+ return converted;
1855
+ }
1856
+
1857
+ raw_t &operator=(const Ephemeris &eph){
1858
+ #define CONVERT3(TARGET_DST, TARGET_SRC, TARGET_SF) \
1859
+ {TARGET_DST = (s32_t)((TARGET_SRC + 0.5 * sf[SF_ ## TARGET_SF]) / sf[SF_ ## TARGET_SF]);}
1860
+ #define CONVERT2(TARGET, TARGET_SF) CONVERT3(eph.TARGET, eph.TARGET, TARGET_SF)
1861
+ #define CONVERT(TARGET) CONVERT2(TARGET, TARGET)
1862
+ svid = eph.svid;
1863
+
1864
+ URA = eph.URA;
1865
+ CONVERT3(t_0, std::fmod(t_0, gps_time_t::seconds_day), t_0);
1866
+ CONVERT2(x, xy); CONVERT2(y, xy); CONVERT(z);
1867
+ CONVERT2(dx, dxy); CONVERT2(dy, dxy); CONVERT(dz);
1868
+ CONVERT2(ddx, ddxy); CONVERT2(ddy, ddxy); CONVERT(ddz);
1869
+ CONVERT(a_Gf0);
1870
+ CONVERT(a_Gf1);
1871
+ #undef CONVERT
1872
+ #undef CONVERT2
1873
+ #undef CONVERT3
1874
+ return *this;
1875
+ }
1876
+ };
1877
+
1878
+ bool is_equivalent(const Ephemeris &eph) const {
1879
+ do{
1880
+ if(WN != eph.WN){break;}
1881
+ if(URA != eph.URA){break;}
1882
+
1883
+ #define CHECK2(TARGET, TARGET_SF) \
1884
+ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET_SF]){break;}
1885
+ #define CHECK(TARGET) CHECK2(TARGET, TARGET)
1886
+ CHECK(t_0);
1887
+ CHECK2(x, xy); CHECK2(y, xy); CHECK(z);
1888
+ CHECK2(dx, dxy); CHECK2(dy, dxy); CHECK(dz);
1889
+ CHECK2(ddx, ddxy); CHECK2(ddy, ddxy); CHECK(ddz);
1890
+ CHECK(a_Gf0);
1891
+ CHECK(a_Gf1);
1892
+ #undef CHECK
1893
+ #undef CHECK2
1894
+ return true;
1895
+ }while(false);
1896
+ return false;
1897
+ }
1898
+
1899
+ gps_time_t base_time() const {
1900
+ return gps_time_t(WN, t_0);
1901
+ }
1902
+ };
1903
+
1904
+ struct Ephemeris_with_Timeout : public Ephemeris {
1905
+ gps_time_t t_reception;
1906
+
1907
+ Ephemeris_with_Timeout() : Ephemeris(), t_reception(0, 0) {}
1908
+
1909
+ /**
1910
+ * Constructor to convert Ephemeris
1911
+ * reception time is interpreted to be equivalent to time of applicability (t_0) of eph,
1912
+ * therefore, eph is assumed that its t_0, i.e., week number and time of week,
1913
+ * has already been adjusted.
1914
+ * @param eph Epehemris
1915
+ */
1916
+ Ephemeris_with_Timeout(const Ephemeris &eph)
1917
+ : Ephemeris(eph), t_reception(eph.WN, eph.t_0) {}
1918
+
1919
+ /**
1920
+ * Constructor to convert Ephemeris with reception time
1921
+ * reception time will be adjusted with time of reception.
1922
+ * @param eph Ephemeris
1923
+ * @param t_rx Reception time
1924
+ */
1925
+ Ephemeris_with_Timeout(const Ephemeris &eph, const gps_time_t &t_rx)
1926
+ : Ephemeris(eph), t_reception(t_rx) {
1927
+ Ephemeris::adjust_time(t_rx);
1928
+ }
1929
+
1930
+ /**
1931
+ * Check availability on EN_Route, Terminal, or LNAV
1932
+ * @param t target time
1933
+ * @return true when interval between message reception time
1934
+ * and current time is within timeout threshold, otherwise false.
1935
+ * @see 2.1.1.4.9, A.4.5.1.3.3
1936
+ */
1937
+ bool is_valid_EN_Route_Terminal_LNAV(const gps_time_t &t) const {
1938
+ float_t delta_t(t_reception.interval(t));
1939
+ return delta_t < Timing::values[
1940
+ Timing::GEO_NAVIGATION_DATA].timeout_EN_Route_Terminal_LNAV;
1941
+ }
1942
+
1943
+ /**
1944
+ * Check availability on LNAV, VNAV, LP, or LPV approach
1945
+ * @param t target time
1946
+ * @return true when interval between message reception time
1947
+ * and current time is within timeout threshold, otherwise false.
1948
+ * @see 2.1.1.4.9, A.4.5.1.3.3
1949
+ */
1950
+ bool is_valid_LNAV_VNAV_LP_LPV_approach(const gps_time_t &t) const {
1951
+ float_t delta_t(t_reception.interval(t));
1952
+ return delta_t < Timing::values[
1953
+ Timing::GEO_NAVIGATION_DATA].timeout_LNAV_VNAV_LP_LPV_approach;
1954
+ }
1955
+
1956
+ /**
1957
+ * Return reception time as base time, which is different from non-timeout version for time of applicability.
1958
+ * This function will be used to sort multiple ephemeris in order of reception time.
1959
+ * @see Ephemeris::base_time()
1960
+ */
1961
+ gps_time_t base_time() const {
1962
+ return t_reception;
1963
+ }
1964
+ };
1965
+
1966
+ /**
1967
+ * SBAS almanac
1968
+ * @see Table A-19
1969
+ */
1970
+ struct Almanac {
1971
+ uint_t data_id;
1972
+ uint_t prn; ///< PRN number
1973
+ uint_t SV_health; ///< Health status
1974
+ float_t x, y, z; ///< ECEF position (m)
1975
+ float_t dx, dy, dz; ///< ECEF velocity (m/s)
1976
+ float_t t_0; ///< Time of applicability (s)
1977
+
1978
+ ///< Up-cast to ephemeris
1979
+ operator Ephemeris() const {
1980
+ Ephemeris converted = {
1981
+ prn, // Satellite number
1982
+ t_0, // Time of applicability (s)
1983
+ -1, // User range accuracy (index)
1984
+ x, y, z, // ECEF position (m)
1985
+ dx, dy, dz, // ECEF velocity (m/s)
1986
+ 0, 0, 0, // ECEF acceleration (m/s^2)
1987
+ 0, // Clock correction parameter (s)
1988
+ 0, // Clock correction parameter (s/s)
1989
+ };
1990
+ return converted;
1991
+ }
1992
+
1993
+ struct raw_t {
1994
+ u8_t data_id;
1995
+ u8_t prn; ///< PRN number
1996
+ u8_t SV_health; ///< Health status
1997
+ s16_t x, y, z; ///< ECEF position (2600(xy), 26000(z), m)
1998
+ s8_t dx, dy, dz; ///< ECEF velocity (10(xy), 60(z), m/s)
1999
+ u16_t t_0; ///< Time of applicability (64, s)
2000
+
2001
+ template <class InputT>
2002
+ static raw_t fetch(const InputT *buf, const uint_t &ch){
2003
+ typedef typename DataBlock::Type17 msg_t;
2004
+ raw_t res = {
2005
+ msg_t::id(buf, ch), // data_id
2006
+ msg_t::prn(buf, ch), // prn
2007
+ msg_t::health_status(buf, ch), // SV_health
2008
+ msg_t::x(buf, ch), msg_t::y(buf, ch), msg_t::z(buf, ch), // x, y, z
2009
+ msg_t::dx(buf, ch), msg_t::dy(buf, ch), msg_t::dz(buf, ch), // dx, dy, dz
2010
+ msg_t::t0(buf), // t_0
2011
+ };
2012
+ return res;
2013
+ }
2014
+
2015
+ enum {
2016
+ SF_xy, SF_z,
2017
+ SF_dxy, SF_dz,
2018
+ SF_t_0,
2019
+
2020
+ SF_NUM,
2021
+ };
2022
+ static const float_t sf[SF_NUM];
2023
+
2024
+ operator Almanac() const {
2025
+ Almanac converted;
2026
+ #define CONVERT2(TARGET, TARGET_SF) \
2027
+ {converted.TARGET = sf[SF_ ## TARGET_SF] * TARGET;}
2028
+ #define CONVERT(TARGET) CONVERT2(TARGET, TARGET)
2029
+ converted.data_id = data_id;
2030
+ converted.prn = prn;
2031
+ converted.SV_health = SV_health;
2032
+ CONVERT2(x, xy); CONVERT2(y, xy); CONVERT(z);
2033
+ CONVERT2(dx, dxy); CONVERT2(dy, dxy); CONVERT(dz);
2034
+ CONVERT(t_0);
2035
+ #undef CONVERT
2036
+ #undef CONVERT2
2037
+ return converted;
2038
+ }
2039
+ };
2040
+ };
2041
+ };
2042
+
2043
+ class Satellite : public SatelliteProperties {
2044
+ friend class SBAS_SpaceNode;
2045
+ public:
2046
+ typedef typename SatelliteProperties::Ephemeris_with_Timeout eph_t;
2047
+ typedef typename gps_space_node_t::template PropertyHistory<eph_t> eph_list_t;
2048
+ typedef IonosphericGridPoints_with_Timeout igp_t;
2049
+ protected:
2050
+ eph_list_t eph_history;
2051
+ igp_t igp;
2052
+ public:
2053
+ Satellite() : eph_history(), igp() {
2054
+ // setup first ephemeris as invalid one
2055
+ eph_t &eph_current(const_cast<eph_t &>(eph_history.current()));
2056
+ eph_current.WN = 0;
2057
+ eph_current.t_0 = -1;
2058
+ }
2059
+
2060
+ template <class Functor>
2061
+ void each_ephemeris(
2062
+ Functor &functor,
2063
+ const typename eph_list_t::each_mode_t &mode = eph_list_t::EACH_ALL) const {
2064
+ eph_history.each(functor, mode);
2065
+ }
2066
+
2067
+ void register_ephemeris(const eph_t &eph, const int &priority_delta = 1){
2068
+ eph_history.add(eph, priority_delta);
2069
+ }
2070
+
2071
+ const eph_t &ephemeris() const {
2072
+ return eph_history.current();
2073
+ }
2074
+
2075
+ /**
2076
+ * Select appropriate ephemeris within registered ones.
2077
+ *
2078
+ * @param target_time time at measurement
2079
+ * @param LNAV_VNAV_LP_LPV_approach
2080
+ * @return if true, appropriate ephemeris is selected, otherwise, not selected.
2081
+ */
2082
+ bool select_ephemeris(
2083
+ const gps_time_t &target_time,
2084
+ const bool &LNAV_VNAV_LP_LPV_approach = false){
2085
+ bool (eph_t::*is_valid_func)(const gps_time_t &) const (
2086
+ LNAV_VNAV_LP_LPV_approach
2087
+ ? &eph_t::is_valid_LNAV_VNAV_LP_LPV_approach
2088
+ : &eph_t::is_valid_EN_Route_Terminal_LNAV);
2089
+ return (ephemeris().*is_valid_func)(target_time) // conservative
2090
+ || eph_history.select(target_time, is_valid_func);
2091
+ }
2092
+
2093
+ const igp_t &ionospheric_grid_points() const {
2094
+ return igp;
2095
+ }
2096
+ };
2097
+
2098
+ public:
2099
+ typedef std::map<int, Satellite> satellites_t;
2100
+ protected:
2101
+ satellites_t _satellites;
2102
+ public:
2103
+ SBAS_SpaceNode() : _satellites() {
2104
+
2105
+ }
2106
+ ~SBAS_SpaceNode(){
2107
+ _satellites.clear();
2108
+ }
2109
+ const satellites_t &satellites() const {
2110
+ return _satellites;
2111
+ }
2112
+ Satellite &satellite(const int &prn) {
2113
+ return _satellites[prn];
2114
+ }
2115
+ bool has_satellite(const int &prn) const {
2116
+ return _satellites.find(prn) != _satellites.end();
2117
+ }
2118
+ void update_all_ephemeris(const gps_time_t &target_time) {
2119
+ for(typename satellites_t::iterator it(_satellites.begin());
2120
+ it != _satellites.end(); ++it){
2121
+ it->second.select_ephemeris(target_time);
2122
+ }
2123
+ }
2124
+
2125
+ typedef std::vector<std::pair<int, const Satellite *> > available_satellites_t;
2126
+ /**
2127
+ * Return available satellites
2128
+ * @param lng_deg longitude of user position
2129
+ * @return (available_satellites_t) available satellites, nearer is faster
2130
+ */
2131
+ available_satellites_t available_satellites(const float_t &lng_deg) const {
2132
+ available_satellites_t res;
2133
+ typename KnownSatellites::list_t nearest(KnownSatellites::nearest_ordered(lng_deg));
2134
+ for(typename KnownSatellites::list_t::const_iterator it(nearest.begin());
2135
+ it != nearest.end();
2136
+ ++it){
2137
+ int prn((*it)->prn);
2138
+ if(!has_satellite(prn)){continue;}
2139
+ res.push_back(std::make_pair(prn, &(const_cast<SBAS_SpaceNode *>(this)->_satellites[prn])));
2140
+ }
2141
+ return res;
2142
+ }
2143
+
2144
+ template <class InputT>
2145
+ MessageType decode_message(
2146
+ const InputT *buf, const int &prn, const gps_time_t &t_reception,
2147
+ const bool &LNAV_VNAV_LP_LPV_approach = false){
2148
+
2149
+ MessageType message_type((MessageType)DataBlock::message_type(buf));
2150
+ Satellite &sat(_satellites[prn]);
2151
+
2152
+ switch(message_type){
2153
+ case GEO_NAVIGATION: { // 9
2154
+ typename Satellite::eph_t eph(
2155
+ Satellite::eph_t::raw_t::fetch(buf, prn), t_reception);
2156
+ sat.eph_history.add(eph);
2157
+ break;
2158
+ }
2159
+ case DEGRADATION_PARAMS: { // 10
2160
+ DegradationFactors dfactor(DegradationFactors::raw_t::fetch(buf));
2161
+ break;
2162
+ }
2163
+ case IONO_GRID_POINT_MASKS: // 18
2164
+ sat.igp.update_mask(buf, t_reception, LNAV_VNAV_LP_LPV_approach);
2165
+ break;
2166
+ case IONO_DELAY_CORRECTION: // 26
2167
+ if(!sat.igp.register_igp(buf, t_reception, LNAV_VNAV_LP_LPV_approach)){
2168
+ message_type = UNSUPPORTED_MESSAGE;
2169
+ }
2170
+ break;
2171
+ case NULL_MESSAGES: // 63
2172
+ break;
2173
+ default:
2174
+ message_type = UNSUPPORTED_MESSAGE;
2175
+ }
2176
+
2177
+ return message_type;
2178
+ }
2179
+ };
2180
+
2181
+ template <class FloatT>
2182
+ template <typename T>
2183
+ typename SBAS_SpaceNode<FloatT>::KnownSatellites::list_t
2184
+ SBAS_SpaceNode<FloatT>::KnownSatellites::sort(T sorter){
2185
+ static const typename SBAS_SpaceNode<FloatT>::RangingCode codes[] = {
2186
+ {121, 175, 01241, 5, "EGNOS (Eutelsat 5WB)"},
2187
+ {122, 52, 00267, 143.5, "SPAN (INMARSAT 4F1)"},
2188
+ {123, 21, 00232, 31.5, "EGNOS (ASTRA 5B)"},
2189
+ {124, 237, 01617, 0, "EGNOS (Reserved)"},
2190
+ {125, 235, 01076, -16, "SDCM (Luch-5A)"},
2191
+ {126, 886, 01764, 63.9, "EGNOS (INMARSAT 4F2)"},
2192
+ {127, 657, 00717, 55, "GAGAN (GSAT-8)"},
2193
+ {128, 634, 01532, 83, "GAGAN (GSAT-10)"},
2194
+ {129, 762, 01250, 127, "MSAS (QZS-3)"},
2195
+ {130, 355, 00341, 140, "BDSBAS (G6)"},
2196
+ {131, 1012, 00551, -117, "WAAS (Eutelsat 117West B)"},
2197
+ {132, 176, 00520, 93.5, "GAGAN (GSAT-15)"},
2198
+ {133, 603, 01731, -129, "WAAS (SES-15)"},
2199
+ {134, 130, 00706, 91.5, "KASS (MEASAT-3D)"},
2200
+ {135, 359, 01216, -125, "WAAS (Intelsat Galaxy 30)"},
2201
+ {136, 595, 00740, 5, "EGNOS (SES-5)"},
2202
+ {137, 68, 01007, 127, "MSAS (QZS-3)"},
2203
+ {138, 386, 00450, 107.3, "WAAS (ANIK F1R)"},
2204
+ {140, 456, 01653, 95, "SDCM (Luch-5V)"},
2205
+ {141, 499, 01411, 167, "SDCM (Luch-5A)"},
2206
+ {143, 307, 01312, 110.5, "BDSBAS (G3)"},
2207
+ {144, 127, 01060, 80, "BDSBAS (G2)"},
2208
+ {147, 118, 00355, 42.5, "NSAS (NIGCOMSAT-1R)"},
2209
+ {148, 163, 00335, -24.8, "ASAL (ALCOMSAT-1)"},
2210
+ }; ///< @see https://www.gps.gov/technical/prn-codes/L1-CA-PRN-code-assignments-2020-Apr.pdf
2211
+ list_t res;
2212
+ res.reserve(sizeof(codes) / sizeof(codes[0]));
2213
+ for(unsigned int i(0); i < sizeof(codes) / sizeof(codes[0]); ++i){
2214
+ res.push_back(&codes[i]);
2215
+ }
2216
+ std::sort(res.begin(), res.end(), sorter);
2217
+ return res;
2218
+ }
2219
+
2220
+ template <class FloatT>
2221
+ const typename SBAS_SpaceNode<FloatT>::KnownSatellites::list_t
2222
+ SBAS_SpaceNode<FloatT>::KnownSatellites::prn_ordered
2223
+ = SBAS_SpaceNode<FloatT>::KnownSatellites::sort(
2224
+ typename SBAS_SpaceNode<FloatT>::RangingCode::prn_sorter_t());
2225
+
2226
+ template <class FloatT>
2227
+ const typename SBAS_SpaceNode<FloatT>::KnownSatellites::list_t
2228
+ SBAS_SpaceNode<FloatT>::KnownSatellites::longitude_ordered
2229
+ = SBAS_SpaceNode<FloatT>::KnownSatellites::sort(
2230
+ typename SBAS_SpaceNode<FloatT>::RangingCode::lng_sorter_t());
2231
+
2232
+ template <class FloatT>
2233
+ const typename SBAS_SpaceNode<FloatT>::KnownSatellites::list_t
2234
+ SBAS_SpaceNode<FloatT>::KnownSatellites::name_ordered
2235
+ = SBAS_SpaceNode<FloatT>::KnownSatellites::sort(
2236
+ typename SBAS_SpaceNode<FloatT>::RangingCode::name_sorter_t());
2237
+
2238
+
2239
+ template <class FloatT>
2240
+ const typename SBAS_SpaceNode<FloatT>::Timing::values_t
2241
+ SBAS_SpaceNode<FloatT>::Timing::values[SBAS_SpaceNode<FloatT>::Timing::NUM_OF_TIMING_ITEMS] = {
2242
+ { 6}, // DONT_USE_FOR_SAFETY_APPLICATIONS (0)
2243
+ {120, 600, 600}, // PRN_MASK (1)
2244
+ { 6, 18, 12}, // UDREI (2-6, 24)
2245
+ {}, // FAST_CORRECTIONS (2-5, 24)
2246
+ {120, 360, 240}, // LONG_TERM_CORRECTIONS (24, 25)
2247
+ {120, 360, 240}, // GEO_NAVIGATION_DATA (9)
2248
+ {120, 360, 240}, // FAST_CORRECTION_DEGRADATION (7)
2249
+ {120, 360, 240}, // DEGRADATION_PARAMETERS (10)
2250
+ {300, 1200, 1200}, // IONOSPHERIC_GRID_MASK (18)
2251
+ {300, 600, 600}, // IONOSPHERIC_CORRECTIONS (26)
2252
+ {300, 86400, 86400}, // UTC_TIMING_DATA (12)
2253
+ {300}, // ALNAMAC_DATA (17)
2254
+ {300, 86400, 86400}, // SERVICE_LEVEL (27)
2255
+ {120, 360, 240}, // CLOCK_EPHEMERIS_COVARIANCE_MATRIX (28)
2256
+ }; ///< @see Table A-25
2257
+
2258
+ template <class FloatT>
2259
+ const typename SBAS_SpaceNode<FloatT>::IonosphericGridPoints::PointProperty::raw_t
2260
+ SBAS_SpaceNode<FloatT>::IonosphericGridPoints::PointProperty::raw_t::unavailable = {
2261
+ DELAY_DONT_USE,
2262
+ ERROR_INDICATOR_NOT_MONITORED,
2263
+ };
2264
+
2265
+ template <class FloatT>
2266
+ const typename SBAS_SpaceNode<FloatT>::IonosphericGridPoints::PointProperty
2267
+ SBAS_SpaceNode<FloatT>::IonosphericGridPoints::PointProperty::unavailable = {
2268
+ 0,
2269
+ raw_t::raw2sigma2(raw_t::ERROR_INDICATOR_NOT_MONITORED),
2270
+ };
2271
+
2272
+ template <class FloatT>
2273
+ SBAS_SpaceNode<FloatT>::IonosphericGridPoints::position_t::operator typename SBAS_SpaceNode<FloatT>::IonosphericGridPoints::position_index_t() const {
2274
+ SBAS_SpaceNode<FloatT>::IonosphericGridPoints::position_index_t res = {
2275
+ position_index_t::lat2idx(latitude_deg),
2276
+ position_index_t::lng2idx(longitude_deg)};
2277
+ return res;
2278
+ }
2279
+
2280
+ #define POWER_2(n) \
2281
+ (((n) >= 0) \
2282
+ ? (float_t)(1 << (n >= 0 ? n : 0)) \
2283
+ : (((float_t)1) / (1 << (-(n) >= 30 ? 30 : -(n > 0 ? 0 : n))) \
2284
+ / (1 << (-(n) >= 30 ? (-(n) - 30) : 0))))
2285
+
2286
+ template <class FloatT>
2287
+ const typename SBAS_SpaceNode<FloatT>::float_t SBAS_SpaceNode<FloatT>::DegradationFactors::raw_t::sf[] = {
2288
+ 0.002, // SF_B_rcc
2289
+ 0.002, // SF_C_ltc_lsb
2290
+ 0.00005, // SF_C_ltc_v1
2291
+ 0.002, // SF_C_ltc_v0
2292
+ 0.0005, // SF_C_geo_lsb
2293
+ 0.00005, // SF_C_geo_v
2294
+ 0.5, // SF_C_er
2295
+ 0.001, // SF_C_iono_step
2296
+ 0.000005, // SF_C_iono_ramp
2297
+ 0.1, // SF_C_covariance
2298
+ };
2299
+
2300
+ template <class FloatT>
2301
+ const typename SBAS_SpaceNode<FloatT>::float_t SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t::sf[] = {
2302
+ 16, // SF_t_0
2303
+ 0.08, // SF_xy
2304
+ 0.4, // SF_z
2305
+ 0.000625, // SF_dxy
2306
+ 0.004, // SF_dz
2307
+ 0.0000125, // SF_ddxy
2308
+ 0.0000625, // SF_ddz
2309
+ POWER_2(-31), // SF_a_Gf0
2310
+ POWER_2(-40), // SF_a_Gf1
2311
+ };
2312
+
2313
+ template <class FloatT>
2314
+ const typename SBAS_SpaceNode<FloatT>::float_t SBAS_SpaceNode<FloatT>::SatelliteProperties::Almanac::raw_t::sf[] = {
2315
+ 2600, // SF_xy
2316
+ 26000, // SF_z
2317
+ 10, // SF_dxy
2318
+ 60, // SF_dz
2319
+ 64, // SF_t_0
2320
+ };
2321
+
2322
+ template <class FloatT>
2323
+ const typename SBAS_SpaceNode<FloatT>::float_t SBAS_SpaceNode<FloatT>::UTC_Parameters::raw_t::sf[] = {
2324
+ POWER_2(-50), // SF_A1
2325
+ POWER_2(-30), // SF_A0
2326
+ };
2327
+
2328
+ #undef POWER_2
2329
+
2330
+ #endif /* __SBAS_H__ */