gps_pvt 0.1.5 → 0.2.1

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -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__ */