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.
- checksums.yaml +4 -4
- data/README.md +16 -4
- data/Rakefile +9 -4
- data/exe/gps_pvt +32 -23
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +5 -3
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +4302 -529
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +7 -5
- data/ext/ninja-scan-light/tool/navigation/GPS.h +5 -4
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +62 -15
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +33 -25
- data/ext/ninja-scan-light/tool/navigation/QZSS.h +62 -0
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +583 -115
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +2330 -0
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +329 -0
- data/ext/ninja-scan-light/tool/swig/GPS.i +284 -99
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +18 -1
- data/lib/gps_pvt/receiver.rb +323 -141
- data/lib/gps_pvt/version.rb +1 -1
- metadata +9 -7
- data/gps_pvt.gemspec +0 -57
@@ -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__ */
|