gps_pvt 0.2.1 → 0.2.3
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- checksums.yaml +4 -4
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +434 -434
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +277 -14
- data/ext/ninja-scan-light/tool/navigation/GPS.h +8 -43
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +61 -147
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +54 -0
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +2 -2
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +55 -78
- data/ext/ninja-scan-light/tool/param/bit_array.h +4 -3
- data/ext/ninja-scan-light/tool/swig/GPS.i +167 -61
- data/ext/ninja-scan-light/tool/swig/SylphideMath.i +53 -6
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +19 -5
- data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb +38 -4
- data/gps_pvt.gemspec +63 -0
- data/lib/gps_pvt/receiver.rb +39 -27
- data/lib/gps_pvt/version.rb +1 -1
- metadata +4 -3
@@ -526,6 +526,9 @@ class GPS_SpaceNode {
|
|
526
526
|
return res;
|
527
527
|
}
|
528
528
|
static const float_t gamma_L1_L2;
|
529
|
+
static const float_t gamma_per_L1(const float_t &freq){
|
530
|
+
return std::pow(L1_Frequency / freq, 2);
|
531
|
+
}
|
529
532
|
|
530
533
|
protected:
|
531
534
|
static float_t rad2sc(const float_t &rad) {return rad / M_PI;}
|
@@ -2151,37 +2154,17 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
2151
2154
|
usr.llh(),
|
2152
2155
|
t);
|
2153
2156
|
}
|
2154
|
-
|
2155
|
-
struct IonospericCorrection {
|
2156
|
-
const GPS_SpaceNode<float_t> &space_node;
|
2157
|
-
float_t operator()(
|
2158
|
-
const enu_t &relative_pos,
|
2159
|
-
const llh_t &usrllh,
|
2160
|
-
const gps_time_t &t) const {
|
2161
|
-
return space_node.iono_correction(relative_pos, usrllh, t);
|
2162
|
-
}
|
2163
|
-
float_t operator()(
|
2164
|
-
const xyz_t &sat,
|
2165
|
-
const xyz_t &usr,
|
2166
|
-
const gps_time_t &t) const {
|
2167
|
-
return space_node.iono_correction(sat, usr, t);
|
2168
|
-
}
|
2169
|
-
};
|
2170
|
-
IonospericCorrection iono_correction() const {
|
2171
|
-
IonospericCorrection res = {*this};
|
2172
|
-
return res;
|
2173
|
-
}
|
2174
2157
|
|
2175
2158
|
/**
|
2176
|
-
* Calculate correction value in accordance with tropospheric model
|
2159
|
+
* Calculate correction value in accordance with tropospheric Hopfield model
|
2177
2160
|
*
|
2178
2161
|
* @param relative_pos satellite position (relative position, NEU)
|
2179
2162
|
* @param usrllh user position (absolute position, LLH)
|
2180
2163
|
* @return correction in meters
|
2181
2164
|
*/
|
2182
|
-
float_t tropo_correction(
|
2165
|
+
static float_t tropo_correction(
|
2183
2166
|
const enu_t &relative_pos,
|
2184
|
-
const llh_t &usrllh)
|
2167
|
+
const llh_t &usrllh){
|
2185
2168
|
|
2186
2169
|
// Elevation (rad)
|
2187
2170
|
float_t el(relative_pos.elevation());
|
@@ -2291,31 +2274,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
2291
2274
|
* @param usr user position (absolute position, XYZ)
|
2292
2275
|
* @return correction in meters
|
2293
2276
|
*/
|
2294
|
-
float_t tropo_correction(
|
2277
|
+
static float_t tropo_correction(
|
2295
2278
|
const xyz_t &sat,
|
2296
|
-
const xyz_t &usr)
|
2279
|
+
const xyz_t &usr) {
|
2297
2280
|
return tropo_correction(
|
2298
2281
|
enu_t::relative(sat, usr),
|
2299
2282
|
usr.llh());
|
2300
2283
|
}
|
2301
|
-
|
2302
|
-
struct TropospericCorrection {
|
2303
|
-
const GPS_SpaceNode<float_t> &space_node;
|
2304
|
-
float_t operator()(
|
2305
|
-
const enu_t &relative_pos,
|
2306
|
-
const llh_t &usrllh) const {
|
2307
|
-
return space_node.tropo_correction(relative_pos, usrllh);
|
2308
|
-
}
|
2309
|
-
float_t operator()(
|
2310
|
-
const xyz_t &sat,
|
2311
|
-
const xyz_t &usr) const {
|
2312
|
-
return space_node.tropo_correction(sat, usr);
|
2313
|
-
}
|
2314
|
-
};
|
2315
|
-
TropospericCorrection tropo_correction() const {
|
2316
|
-
TropospericCorrection res = {*this};
|
2317
|
-
return res;
|
2318
|
-
}
|
2319
2284
|
};
|
2320
2285
|
|
2321
2286
|
template <class FloatT>
|
@@ -49,7 +49,6 @@
|
|
49
49
|
|
50
50
|
#include "GPS.h"
|
51
51
|
#include "GPS_Solver_Base.h"
|
52
|
-
#include "SBAS.h"
|
53
52
|
#include "NTCM.h"
|
54
53
|
|
55
54
|
template <class FloatT>
|
@@ -57,66 +56,10 @@ struct GPS_Solver_GeneralOptions {
|
|
57
56
|
FloatT elevation_mask;
|
58
57
|
FloatT residual_mask;
|
59
58
|
|
60
|
-
enum ionospheric_model_t {
|
61
|
-
IONOSPHERIC_KLOBUCHAR,
|
62
|
-
IONOSPHERIC_SBAS,
|
63
|
-
IONOSPHERIC_NTCM_GL,
|
64
|
-
IONOSPHERIC_NONE, // which allows no correction
|
65
|
-
IONOSPHERIC_MODELS,
|
66
|
-
IONOSPHERIC_SKIP = IONOSPHERIC_MODELS, // which means delegating the next slot
|
67
|
-
};
|
68
|
-
ionospheric_model_t ionospheric_models[IONOSPHERIC_MODELS]; // lower index means having higher priority
|
69
|
-
|
70
|
-
int count_ionospheric_models() const {
|
71
|
-
int res(0);
|
72
|
-
for(std::size_t i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
|
73
|
-
if(ionospheric_models[i] != IONOSPHERIC_SKIP){res++;}
|
74
|
-
}
|
75
|
-
return res;
|
76
|
-
}
|
77
|
-
|
78
|
-
FloatT f_10_7;
|
79
|
-
|
80
59
|
GPS_Solver_GeneralOptions()
|
81
60
|
: elevation_mask(0), // elevation mask default is 0 [deg]
|
82
|
-
residual_mask(30)
|
83
|
-
f_10_7(-1) {
|
84
|
-
for(std::size_t i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
|
85
|
-
ionospheric_models[i] = IONOSPHERIC_SKIP;
|
86
|
-
}
|
87
|
-
// default: broadcasted Klobuchar parameters are at least required for solution.
|
88
|
-
ionospheric_models[0] = IONOSPHERIC_KLOBUCHAR;
|
89
|
-
}
|
90
|
-
|
91
|
-
bool insert_ionospheric_model(const ionospheric_model_t &model, const int &index = 0) {
|
92
|
-
if((index < 0)
|
93
|
-
|| (index >= sizeof(ionospheric_models) / sizeof(ionospheric_models[0]))){
|
94
|
-
return false;
|
95
|
-
}
|
61
|
+
residual_mask(30) { // range residual mask is 30 [m]
|
96
62
|
|
97
|
-
// shift, then replace
|
98
|
-
for(int i(index), j(i + 1); j < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i, ++j){
|
99
|
-
ionospheric_models[j] = ionospheric_models[i];
|
100
|
-
}
|
101
|
-
ionospheric_models[index] = model;
|
102
|
-
|
103
|
-
return true;
|
104
|
-
}
|
105
|
-
bool remove_ionospheric_model(const ionospheric_model_t &model) {
|
106
|
-
// shift, then replace
|
107
|
-
int j(0);
|
108
|
-
for(int i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
|
109
|
-
if(ionospheric_models[i] == model){continue;}
|
110
|
-
ionospheric_models[j++] = ionospheric_models[i];
|
111
|
-
}
|
112
|
-
if(j == (sizeof(ionospheric_models) / sizeof(ionospheric_models[0]))){
|
113
|
-
return false;
|
114
|
-
}
|
115
|
-
do{
|
116
|
-
ionospheric_models[j++] = IONOSPHERIC_SKIP;
|
117
|
-
}while(j < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]));
|
118
|
-
|
119
|
-
return true;
|
120
63
|
}
|
121
64
|
};
|
122
65
|
|
@@ -158,24 +101,23 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
158
101
|
inheritate_type(llh_t);
|
159
102
|
inheritate_type(enu_t);
|
160
103
|
|
161
|
-
typedef SBAS_SpaceNode<float_t> sbas_space_node_t;
|
162
|
-
|
163
104
|
inheritate_type(pos_t);
|
164
105
|
|
165
106
|
inheritate_type(prn_obs_t);
|
166
107
|
typedef typename base_t::measurement_t measurement_t;
|
167
108
|
inheritate_type(measurement_items_t);
|
168
109
|
typedef typename base_t::range_error_t range_error_t;
|
110
|
+
typedef typename base_t::range_corrector_t range_corrector_t;
|
111
|
+
typedef typename base_t::range_correction_t range_correction_t;
|
169
112
|
|
170
113
|
inheritate_type(relative_property_t);
|
171
114
|
inheritate_type(geometric_matrices_t);
|
172
115
|
inheritate_type(user_pvt_t);
|
173
116
|
#undef inheritate_type
|
174
117
|
|
175
|
-
const sbas_space_node_t *space_node_sbas; ///< optional
|
176
|
-
|
177
118
|
typedef typename GPS_Solver_Base<float_t>::options_t::template merge_t<
|
178
119
|
GPS_SinglePositioning_Options<float_t>, base_t> options_t;
|
120
|
+
|
179
121
|
protected:
|
180
122
|
const space_node_t &_space_node;
|
181
123
|
GPS_SinglePositioning_Options<float_t> _options;
|
@@ -183,43 +125,50 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
183
125
|
public:
|
184
126
|
const space_node_t &space_node() const {return _space_node;}
|
185
127
|
|
186
|
-
|
187
|
-
|
188
|
-
|
189
|
-
|
190
|
-
|
191
|
-
|
192
|
-
|
193
|
-
|
194
|
-
|
195
|
-
|
196
|
-
|
197
|
-
for(std::size_t i(0);
|
198
|
-
i < sizeof(opt.ionospheric_models) / sizeof(opt.ionospheric_models[0]); ++i){
|
199
|
-
bool usable(false);
|
200
|
-
switch(opt.ionospheric_models[i]){
|
201
|
-
case options_t::IONOSPHERIC_KLOBUCHAR:
|
202
|
-
// check whether Klobuchar parameters alpha and beta have been already received
|
203
|
-
if(_space_node.is_valid_iono()){usable = true;}
|
204
|
-
break;
|
205
|
-
case options_t::IONOSPHERIC_SBAS:
|
206
|
-
if(space_node_sbas){usable = true;}
|
207
|
-
break;
|
208
|
-
case options_t::IONOSPHERIC_NTCM_GL:
|
209
|
-
if(opt.f_10_7 >= 0){usable = true;}
|
210
|
-
// check F10.7 has been already configured
|
211
|
-
break;
|
212
|
-
default: break;
|
213
|
-
}
|
214
|
-
if(usable){
|
215
|
-
available_models++;
|
216
|
-
}else{
|
217
|
-
opt.ionospheric_models[i] = options_t::IONOSPHERIC_SKIP;
|
218
|
-
}
|
128
|
+
struct klobuchar_t : public range_corrector_t {
|
129
|
+
const space_node_t &space_node;
|
130
|
+
klobuchar_t(const space_node_t &sn) : range_corrector_t(), space_node(sn) {}
|
131
|
+
bool is_available(const gps_time_t &t) const {
|
132
|
+
return space_node.is_valid_iono();
|
133
|
+
}
|
134
|
+
float_t *calculate(
|
135
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
136
|
+
float_t &buf) const {
|
137
|
+
return &(buf = space_node.iono_correction(sat_rel_pos, usr_pos.llh, t));
|
219
138
|
}
|
139
|
+
} ionospheric_klobuchar;
|
220
140
|
|
221
|
-
|
222
|
-
|
141
|
+
struct ntcm_gl_t : public range_corrector_t {
|
142
|
+
float_t f_10_7;
|
143
|
+
ntcm_gl_t() : range_corrector_t(), f_10_7(-1) {}
|
144
|
+
bool is_available(const gps_time_t &t) const {
|
145
|
+
return f_10_7 >= 0;
|
146
|
+
}
|
147
|
+
float_t *calculate(
|
148
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
149
|
+
float_t &buf) const {
|
150
|
+
if(!is_available(t)){return NULL;}
|
151
|
+
typename space_node_t::pierce_point_res_t pp(
|
152
|
+
space_node_t::pierce_point(sat_rel_pos, usr_pos.llh));
|
153
|
+
return &(buf = -space_node_t::tec2delay(space_node_t::slant_factor(sat_rel_pos)
|
154
|
+
* NTCM_GL_Generic<float_t>::tec_vert(
|
155
|
+
pp.latitude, pp.longitude, t.year(), f_10_7)));
|
156
|
+
}
|
157
|
+
} ionospheric_ntcm_gl;
|
158
|
+
|
159
|
+
struct tropospheric_simplified_t : public range_corrector_t {
|
160
|
+
tropospheric_simplified_t() : range_corrector_t() {}
|
161
|
+
bool is_available(const gps_time_t &t) const {
|
162
|
+
return true;
|
163
|
+
}
|
164
|
+
float_t *calculate(
|
165
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
166
|
+
float_t &buf) const {
|
167
|
+
return &(buf = space_node_t::tropo_correction(sat_rel_pos, usr_pos.llh));
|
168
|
+
}
|
169
|
+
} tropospheric_simplified;
|
170
|
+
|
171
|
+
range_correction_t ionospheric_correction, tropospheric_correction;
|
223
172
|
|
224
173
|
options_t available_options() const {
|
225
174
|
return options_t(base_t::available_options(), _options);
|
@@ -227,20 +176,26 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
227
176
|
|
228
177
|
options_t available_options(const options_t &opt_wish) const {
|
229
178
|
GPS_SinglePositioning_Options<float_t> opt(opt_wish);
|
230
|
-
filter_ionospheric_models(opt);
|
231
179
|
return options_t(base_t::available_options(opt_wish), opt);
|
232
180
|
}
|
233
181
|
|
234
182
|
options_t update_options(const options_t &opt_wish){
|
235
|
-
|
183
|
+
_options = opt_wish;
|
236
184
|
return options_t(base_t::update_options(opt_wish), _options);
|
237
185
|
}
|
238
186
|
|
239
187
|
GPS_SinglePositioning(const space_node_t &sn)
|
240
|
-
: base_t(),
|
241
|
-
|
242
|
-
|
243
|
-
|
188
|
+
: base_t(), _space_node(sn), _options(available_options(options_t())),
|
189
|
+
ionospheric_klobuchar(sn), ionospheric_ntcm_gl(),
|
190
|
+
tropospheric_simplified(),
|
191
|
+
ionospheric_correction(), tropospheric_correction() {
|
192
|
+
|
193
|
+
// default ionospheric correction:
|
194
|
+
// Broadcasted Klobuchar parameters are at least required for solution.
|
195
|
+
ionospheric_correction.push_front(&ionospheric_klobuchar);
|
196
|
+
|
197
|
+
// default troposheric correction: simplified
|
198
|
+
tropospheric_correction.push_front(&tropospheric_simplified);
|
244
199
|
}
|
245
200
|
|
246
201
|
~GPS_SinglePositioning(){}
|
@@ -294,55 +249,14 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
294
249
|
enu_t relative_pos(enu_t::relative(sat_pos, usr_pos.xyz));
|
295
250
|
|
296
251
|
if(error.unknown_flag & range_error_t::MASK_IONOSPHERIC){
|
297
|
-
|
298
|
-
bool iono_model_hit(false);
|
299
|
-
for(std::size_t i(0);
|
300
|
-
i < sizeof(_options.ionospheric_models) / sizeof(_options.ionospheric_models[0]);
|
301
|
-
++i){
|
302
|
-
switch(_options.ionospheric_models[i]){
|
303
|
-
case options_t::IONOSPHERIC_KLOBUCHAR:
|
304
|
-
residual.residual += _space_node.iono_correction(relative_pos, usr_pos.llh, time_arrival);
|
305
|
-
break;
|
306
|
-
case options_t::IONOSPHERIC_SBAS: {
|
307
|
-
// placeholder of checking availability and performing correction
|
308
|
-
typedef typename SBAS_SpaceNode<float_t>::available_satellites_t sats_t;
|
309
|
-
sats_t sats(space_node_sbas->available_satellites(usr_pos.llh.longitude()));
|
310
|
-
|
311
|
-
typename SBAS_SpaceNode<float_t>::IonosphericGridPoints::PointProperty prop;
|
312
|
-
for(typename sats_t::const_iterator it(sats.begin()); it != sats.end(); ++it){
|
313
|
-
prop = it->second
|
314
|
-
->ionospheric_grid_points().iono_correction(relative_pos, usr_pos.llh);
|
315
|
-
break; // TODO The nearest satellite is only checked
|
316
|
-
}
|
317
|
-
if(!prop.is_available()){continue;}
|
318
|
-
residual.residual += prop.delay;
|
319
|
-
break;
|
320
|
-
}
|
321
|
-
case options_t::IONOSPHERIC_NTCM_GL: {
|
322
|
-
// TODO f_10_7 setup, optimization (mag_model etc.)
|
323
|
-
typename space_node_t::pierce_point_res_t pp(_space_node.pierce_point(relative_pos, usr_pos.llh));
|
324
|
-
residual.residual -= space_node_t::tec2delay(
|
325
|
-
_space_node.slant_factor(relative_pos)
|
326
|
-
* NTCM_GL_Generic<float_t>::tec_vert(
|
327
|
-
pp.latitude, pp.longitude,
|
328
|
-
time_arrival.year(), _options.f_10_7));
|
329
|
-
break;
|
330
|
-
}
|
331
|
-
case options_t::IONOSPHERIC_NONE:
|
332
|
-
break;
|
333
|
-
default:
|
334
|
-
continue;
|
335
|
-
}
|
336
|
-
iono_model_hit = true;
|
337
|
-
break;
|
338
|
-
}
|
252
|
+
residual.residual += ionospheric_correction(time_arrival, usr_pos, relative_pos);
|
339
253
|
}else{
|
340
254
|
residual.residual += error.value[range_error_t::IONOSPHERIC];
|
341
255
|
}
|
342
256
|
|
343
257
|
// Tropospheric
|
344
258
|
residual.residual += (error.unknown_flag & range_error_t::MASK_TROPOSPHERIC)
|
345
|
-
?
|
259
|
+
? tropospheric_correction(time_arrival, usr_pos, relative_pos)
|
346
260
|
: error.value[range_error_t::TROPOSPHERIC];
|
347
261
|
|
348
262
|
// Setup weight
|
@@ -488,7 +402,7 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
488
402
|
|
489
403
|
res.receiver_time = receiver_time;
|
490
404
|
|
491
|
-
if(
|
405
|
+
if(!ionospheric_correction.select(receiver_time)){
|
492
406
|
res.error_code = user_pvt_t::ERROR_INVALID_IONO_MODEL;
|
493
407
|
return;
|
494
408
|
}
|
@@ -38,6 +38,8 @@
|
|
38
38
|
#include <vector>
|
39
39
|
#include <map>
|
40
40
|
#include <utility>
|
41
|
+
#include <deque>
|
42
|
+
#include <algorithm>
|
41
43
|
|
42
44
|
#include <cmath>
|
43
45
|
#include <cstring>
|
@@ -101,6 +103,7 @@ struct GPS_Solver_Base {
|
|
101
103
|
L1_RANGE_RATE_SIGMA,
|
102
104
|
L1_SIGNAL_STRENGTH_dBHz,
|
103
105
|
L1_LOCK_SEC,
|
106
|
+
L1_FREQUENCY,
|
104
107
|
MEASUREMENT_ITEMS_PREDEFINED,
|
105
108
|
};
|
106
109
|
};
|
@@ -223,6 +226,53 @@ struct GPS_Solver_Base {
|
|
223
226
|
return res;
|
224
227
|
}
|
225
228
|
|
229
|
+
struct range_corrector_t {
|
230
|
+
virtual ~range_corrector_t() {}
|
231
|
+
virtual bool is_available(const gps_time_t &t) const {
|
232
|
+
return false;
|
233
|
+
}
|
234
|
+
virtual float_t *calculate(
|
235
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
236
|
+
float_t &buf) const = 0;
|
237
|
+
};
|
238
|
+
static const struct no_correction_t : public range_corrector_t {
|
239
|
+
bool is_available(const gps_time_t &t) const {
|
240
|
+
return true;
|
241
|
+
}
|
242
|
+
float_t *calculate(
|
243
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
244
|
+
float_t &buf) const {
|
245
|
+
return &(buf = 0);
|
246
|
+
}
|
247
|
+
} no_correction;
|
248
|
+
struct range_correction_t : public std::deque<const range_corrector_t *> {
|
249
|
+
typedef std::deque<const range_corrector_t *> super_t;
|
250
|
+
range_correction_t() : super_t() {}
|
251
|
+
const range_corrector_t *select(const gps_time_t &t) const {
|
252
|
+
for(typename super_t::const_iterator it(super_t::begin()), it_end(super_t::end());
|
253
|
+
it != it_end; ++it){
|
254
|
+
if((*it)->is_available(t)){return *it;}
|
255
|
+
}
|
256
|
+
return NULL;
|
257
|
+
}
|
258
|
+
float_t operator()(
|
259
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos) const {
|
260
|
+
float_t res;
|
261
|
+
for(typename super_t::const_iterator it(super_t::begin()), it_end(super_t::end());
|
262
|
+
it != it_end; ++it){
|
263
|
+
if((*it)->calculate(t, usr_pos, sat_rel_pos, res)){return res;}
|
264
|
+
}
|
265
|
+
return 0;
|
266
|
+
}
|
267
|
+
void remove(const typename super_t::value_type &v){
|
268
|
+
std::remove(super_t::begin(), super_t::end(), v);
|
269
|
+
}
|
270
|
+
void add(const typename super_t::value_type &v){
|
271
|
+
remove(v);
|
272
|
+
super_t::push_front(v);
|
273
|
+
}
|
274
|
+
};
|
275
|
+
|
226
276
|
/**
|
227
277
|
* Select appropriate solver, this is provision for GNSS extension
|
228
278
|
* @param prn satellite number
|
@@ -1043,6 +1093,10 @@ const typename GPS_Solver_Base<FloatT>::range_error_t
|
|
1043
1093
|
{0},
|
1044
1094
|
};
|
1045
1095
|
|
1096
|
+
template <class FloatT>
|
1097
|
+
const typename GPS_Solver_Base<FloatT>::no_correction_t
|
1098
|
+
GPS_Solver_Base<FloatT>::no_correction;
|
1099
|
+
|
1046
1100
|
template <class FloatT, class PVT_BaseT = typename GPS_Solver_Base<FloatT>::user_pvt_t>
|
1047
1101
|
struct GPS_PVT_Debug : public PVT_BaseT {
|
1048
1102
|
typename GPS_Solver_Base<FloatT>::matrix_t G, W, delta_r;
|
@@ -1536,10 +1536,10 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1536
1536
|
* @see A.4.2.4
|
1537
1537
|
* @return correction in meters
|
1538
1538
|
*/
|
1539
|
-
float_t tropo_correction(
|
1539
|
+
static float_t tropo_correction(
|
1540
1540
|
const float_t &year_utc,
|
1541
1541
|
const enu_t &relative_pos,
|
1542
|
-
const llh_t &usrllh)
|
1542
|
+
const llh_t &usrllh){
|
1543
1543
|
|
1544
1544
|
if(usrllh.height() > 10E3){ // same as RTKlib; troposphere ranges from 0 to approximately 11km
|
1545
1545
|
return 0;
|
@@ -49,11 +49,7 @@ struct SBAS_SinglePositioning_Options : public GPS_Solver_GeneralOptions<FloatT>
|
|
49
49
|
|
50
50
|
typename GPS_Solver_Base<FloatT>::options_t::template exclude_prn_t<120, 158> exclude_prn; // SBAS PRN ranges from 120 to 158
|
51
51
|
SBAS_SinglePositioning_Options()
|
52
|
-
: super_t() {
|
53
|
-
// default: SBAS IGP, then broadcasted Klobuchar.
|
54
|
-
super_t::ionospheric_models[0] = super_t::IONOSPHERIC_SBAS;
|
55
|
-
super_t::ionospheric_models[1] = super_t::IONOSPHERIC_KLOBUCHAR;
|
56
|
-
|
52
|
+
: super_t(), exclude_prn() {
|
57
53
|
exclude_prn.set(true); // SBAS ranging is default off.
|
58
54
|
}
|
59
55
|
};
|
@@ -86,6 +82,8 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
86
82
|
|
87
83
|
typedef typename base_t::measurement_t measurement_t;
|
88
84
|
typedef typename base_t::range_error_t range_error_t;
|
85
|
+
typedef typename base_t::range_corrector_t range_corrector_t;
|
86
|
+
typedef typename base_t::range_correction_t range_correction_t;
|
89
87
|
|
90
88
|
inheritate_type(relative_property_t);
|
91
89
|
#undef inheritate_type
|
@@ -93,8 +91,6 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
93
91
|
typedef typename GPS_Solver_Base<float_t>::options_t::template merge_t<
|
94
92
|
SBAS_SinglePositioning_Options<float_t>, base_t> options_t;
|
95
93
|
|
96
|
-
typedef GPS_SpaceNode<float_t> gps_space_node_t;
|
97
|
-
const gps_space_node_t *space_node_gps; ///< optional
|
98
94
|
protected:
|
99
95
|
const space_node_t &_space_node;
|
100
96
|
SBAS_SinglePositioning_Options<float_t> _options;
|
@@ -102,40 +98,47 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
102
98
|
public:
|
103
99
|
const space_node_t &space_node() const {return _space_node;}
|
104
100
|
|
105
|
-
|
106
|
-
|
107
|
-
|
108
|
-
|
109
|
-
|
110
|
-
|
111
|
-
|
112
|
-
|
113
|
-
|
114
|
-
|
115
|
-
|
116
|
-
|
117
|
-
|
118
|
-
|
119
|
-
|
120
|
-
|
121
|
-
|
122
|
-
|
123
|
-
|
124
|
-
|
125
|
-
|
126
|
-
|
127
|
-
break;
|
128
|
-
default:
|
129
|
-
break;
|
130
|
-
}
|
131
|
-
if(usable){
|
132
|
-
available_models++;
|
133
|
-
}else{
|
134
|
-
opt.ionospheric_models[i] = options_t::IONOSPHERIC_SKIP;
|
101
|
+
struct ionospheric_sbas_t : public range_corrector_t {
|
102
|
+
const space_node_t &space_node;
|
103
|
+
ionospheric_sbas_t(const space_node_t &sn) : range_corrector_t(), space_node(sn) {}
|
104
|
+
bool is_available(const gps_time_t &t) const {
|
105
|
+
return true;
|
106
|
+
}
|
107
|
+
float_t *calculate(
|
108
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
109
|
+
float_t &buf) const {
|
110
|
+
|
111
|
+
// placeholder of checking availability and performing correction
|
112
|
+
typedef typename space_node_t::available_satellites_t sats_t;
|
113
|
+
sats_t sats(space_node.available_satellites(usr_pos.llh.longitude()));
|
114
|
+
|
115
|
+
typename space_node_t::IonosphericGridPoints::PointProperty prop;
|
116
|
+
for(typename sats_t::const_iterator it(sats.begin()); it != sats.end(); ++it){
|
117
|
+
prop = it->second
|
118
|
+
->ionospheric_grid_points().iono_correction(sat_rel_pos, usr_pos.llh);
|
119
|
+
if(prop.is_available()){
|
120
|
+
return &(buf = prop.delay);
|
121
|
+
}
|
122
|
+
break; // TODO The nearest satellite is only checked
|
135
123
|
}
|
124
|
+
return NULL;
|
136
125
|
}
|
137
|
-
|
138
|
-
|
126
|
+
} ionospheric_sbas;
|
127
|
+
|
128
|
+
struct tropospheric_sbas_t : public range_corrector_t {
|
129
|
+
tropospheric_sbas_t() : range_corrector_t() {}
|
130
|
+
bool is_available(const gps_time_t &t) const {
|
131
|
+
return true;
|
132
|
+
}
|
133
|
+
float_t *calculate(
|
134
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
135
|
+
float_t &buf) const {
|
136
|
+
return &(buf = space_node_t::tropo_correction(
|
137
|
+
t.year(), sat_rel_pos, usr_pos.llh));
|
138
|
+
}
|
139
|
+
} tropospheric_sbas;
|
140
|
+
|
141
|
+
range_correction_t ionospheric_correction, tropospheric_correction;
|
139
142
|
|
140
143
|
options_t available_options() const {
|
141
144
|
return options_t(base_t::available_options(), _options);
|
@@ -143,19 +146,24 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
143
146
|
|
144
147
|
options_t available_options(const options_t &opt_wish) const {
|
145
148
|
SBAS_SinglePositioning_Options<float_t> opt(opt_wish);
|
146
|
-
filter_ionospheric_models(opt);
|
147
149
|
return options_t(base_t::available_options(opt_wish), opt);
|
148
150
|
}
|
149
151
|
|
150
152
|
options_t update_options(const options_t &opt_wish){
|
151
|
-
|
153
|
+
_options = opt_wish;
|
152
154
|
return options_t(base_t::update_options(opt_wish), _options);
|
153
155
|
}
|
154
156
|
|
155
|
-
SBAS_SinglePositioning(const space_node_t &sn
|
156
|
-
: base_t(),
|
157
|
-
|
158
|
-
|
157
|
+
SBAS_SinglePositioning(const space_node_t &sn)
|
158
|
+
: base_t(), _space_node(sn), _options(available_options(options_t())),
|
159
|
+
ionospheric_sbas(sn), tropospheric_sbas() {
|
160
|
+
|
161
|
+
// default ionospheric correction: Broadcasted IGP.
|
162
|
+
ionospheric_correction.push_front(&ionospheric_sbas);
|
163
|
+
|
164
|
+
// default troposheric correction: SBAS
|
165
|
+
tropospheric_correction.push_front(&tropospheric_sbas);
|
166
|
+
}
|
159
167
|
|
160
168
|
~SBAS_SinglePositioning(){}
|
161
169
|
|
@@ -245,43 +253,12 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
245
253
|
|
246
254
|
// Tropospheric (2.1.4.10.3, A.4.2.4)
|
247
255
|
res.range_residual += (range_error.unknown_flag & range_error_t::MASK_TROPOSPHERIC)
|
248
|
-
?
|
256
|
+
? tropospheric_correction(time_arrival, usr_pos, relative_pos)
|
249
257
|
: range_error.value[range_error_t::TROPOSPHERIC];
|
250
258
|
|
251
259
|
// Ionospheric (2.1.4.10.2, A.4.4.10.4)
|
252
260
|
if(range_error.unknown_flag & range_error_t::MASK_IONOSPHERIC){
|
253
|
-
|
254
|
-
bool iono_model_hit(false);
|
255
|
-
for(std::size_t i(0); i < sizeof(_options.ionospheric_models) / sizeof(_options.ionospheric_models[0]); ++i){
|
256
|
-
switch(_options.ionospheric_models[i]){
|
257
|
-
case options_t::IONOSPHERIC_KLOBUCHAR: // 20.3.3.5.2.5
|
258
|
-
res.range_residual += space_node_gps->iono_correction(relative_pos, usr_pos.llh, time_arrival);
|
259
|
-
break;
|
260
|
-
case options_t::IONOSPHERIC_SBAS: { // 2.1.4.10.2, A.4.4.10.4
|
261
|
-
typename space_node_t::IonosphericGridPoints::PointProperty prop(
|
262
|
-
sat->ionospheric_grid_points().iono_correction(relative_pos, usr_pos.llh));
|
263
|
-
if(!prop.is_available()){continue;}
|
264
|
-
res.range_residual += prop.delay;
|
265
|
-
break;
|
266
|
-
}
|
267
|
-
case options_t::IONOSPHERIC_NTCM_GL: {
|
268
|
-
// TODO f_10_7 setup, optimization (mag_model etc.)
|
269
|
-
typename gps_space_node_t::pierce_point_res_t pp(gps_space_node_t::pierce_point(relative_pos, usr_pos.llh));
|
270
|
-
res.range_residual -= gps_space_node_t::tec2delay(
|
271
|
-
gps_space_node_t::slant_factor(relative_pos)
|
272
|
-
* NTCM_GL_Generic<float_t>::tec_vert(
|
273
|
-
pp.latitude, pp.longitude,
|
274
|
-
time_arrival.year(), _options.f_10_7));
|
275
|
-
break;
|
276
|
-
}
|
277
|
-
case options_t::IONOSPHERIC_NONE:
|
278
|
-
break;
|
279
|
-
default:
|
280
|
-
continue;
|
281
|
-
}
|
282
|
-
iono_model_hit = true;
|
283
|
-
break;
|
284
|
-
}
|
261
|
+
res.range_residual += ionospheric_correction(time_arrival, usr_pos, relative_pos);
|
285
262
|
}else{
|
286
263
|
res.range_residual += range_error.value[range_error_t::IONOSPHERIC];
|
287
264
|
}
|
@@ -60,6 +60,7 @@ struct const_div_t<denom, denom, log2, 0> {
|
|
60
60
|
template <int MAX_SIZE, class ContainerT = unsigned char>
|
61
61
|
struct BitArray {
|
62
62
|
static const int bits_per_addr = (int)sizeof(ContainerT) * CHAR_BIT;
|
63
|
+
static const int max_size = MAX_SIZE;
|
63
64
|
ContainerT buf[(MAX_SIZE + bits_per_addr - 1) / bits_per_addr];
|
64
65
|
void set(const bool &new_bit = false) {
|
65
66
|
std::memset(buf, (new_bit ? (~0) : 0), sizeof(buf));
|
@@ -128,14 +129,14 @@ struct BitArray {
|
|
128
129
|
std::vector<int> res;
|
129
130
|
int idx(0);
|
130
131
|
static const const_div_t<bits_per_addr> qr(MAX_SIZE);
|
131
|
-
int rem(qr.rem);
|
132
|
-
for(
|
132
|
+
int rem(qr.rem), i(0);
|
133
|
+
for(; i < qr.quot; ++i, idx += bits_per_addr){
|
133
134
|
int idx2(idx);
|
134
135
|
for(ContainerT temp(buf[i]); temp > 0; temp >>= 1, ++idx2){
|
135
136
|
if(temp & 0x1){res.push_back(idx2);}
|
136
137
|
}
|
137
138
|
}
|
138
|
-
for(ContainerT temp(buf[
|
139
|
+
for(ContainerT temp(buf[i]); (temp > 0) && (rem > 0); --rem, ++idx, temp >>= 1){
|
139
140
|
if(temp & 0x1){res.push_back(idx);}
|
140
141
|
}
|
141
142
|
return res;
|