gps_pvt 0.1.5 → 0.2.1
Sign up to get free protection for your applications and to get access to all the features.
- 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__ */
|