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