gps_pvt 0.4.1 → 0.6.0
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/README.md +11 -6
- data/Rakefile +42 -23
- data/exe/gps_pvt +47 -8
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +44 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +9198 -7614
- data/ext/ninja-scan-light/tool/algorithm/interpolate.h +121 -0
- data/ext/ninja-scan-light/tool/navigation/ANTEX.h +747 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +63 -43
- data/ext/ninja-scan-light/tool/navigation/GPS.h +92 -4
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +58 -34
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +50 -17
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +19 -144
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +19 -4
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +63 -43
- data/ext/ninja-scan-light/tool/navigation/SP3.h +1178 -0
- data/ext/ninja-scan-light/tool/navigation/coordinate.h +81 -1
- data/ext/ninja-scan-light/tool/swig/GPS.i +131 -43
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +503 -16
- data/ext/ninja-scan-light/tool/util/text_helper.h +248 -0
- data/{sig/gps_pvt.rbs → gps_pvt.rbs} +0 -0
- data/lib/gps_pvt/receiver.rb +32 -5
- data/lib/gps_pvt/util.rb +50 -0
- data/lib/gps_pvt/version.rb +1 -1
- metadata +8 -3
@@ -75,7 +75,6 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
75
75
|
|
76
76
|
typedef GLONASS_SpaceNode<float_t> space_node_t;
|
77
77
|
inheritate_type(gps_time_t);
|
78
|
-
typedef typename space_node_t::Satellite satellite_t;
|
79
78
|
|
80
79
|
inheritate_type(xyz_t);
|
81
80
|
inheritate_type(enu_t);
|
@@ -83,9 +82,10 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
83
82
|
inheritate_type(pos_t);
|
84
83
|
|
85
84
|
typedef typename base_t::measurement_t measurement_t;
|
86
|
-
|
87
|
-
|
88
|
-
|
85
|
+
typedef typename base_t::satellite_t satellite_t;
|
86
|
+
typedef typename base_t::range_error_t range_error_t;
|
87
|
+
typedef typename base_t::range_corrector_t range_corrector_t;
|
88
|
+
typedef typename base_t::range_correction_t range_correction_t;
|
89
89
|
|
90
90
|
inheritate_type(relative_property_t);
|
91
91
|
typedef typename super_t::measurement_items_t measurement_items_t;
|
@@ -97,11 +97,51 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
97
97
|
GLONASS_SinglePositioning_Options<float_t>, base_t> options_t;
|
98
98
|
|
99
99
|
protected:
|
100
|
-
const space_node_t &_space_node;
|
101
100
|
GLONASS_SinglePositioning_Options<float_t> _options;
|
102
101
|
|
103
102
|
public:
|
104
|
-
|
103
|
+
struct satellites_t {
|
104
|
+
const void *impl;
|
105
|
+
satellite_t (*impl_select)(const void *, const prn_t &, const gps_time_t &);
|
106
|
+
inline satellite_t select(const prn_t &prn, const gps_time_t &receiver_time) const {
|
107
|
+
return impl_select(impl, prn, receiver_time);
|
108
|
+
}
|
109
|
+
static satellite_t select_broadcast(
|
110
|
+
const void *ptr, const prn_t &prn, const gps_time_t &receiver_time){
|
111
|
+
const typename space_node_t::satellites_t &sats(
|
112
|
+
reinterpret_cast<const space_node_t *>(ptr)->satellites());
|
113
|
+
const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn));
|
114
|
+
if((it_sat == sats.end()) // has ephemeris?
|
115
|
+
|| (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
|
116
|
+
return satellite_t::unavailable();
|
117
|
+
}
|
118
|
+
struct impl_t {
|
119
|
+
static inline const typename space_node_t::Satellite &sat(const void *ptr) {
|
120
|
+
return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
|
121
|
+
}
|
122
|
+
static xyz_t position(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
123
|
+
return sat(ptr).constellation(t, pseudo_range).position;
|
124
|
+
}
|
125
|
+
static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
126
|
+
return sat(ptr).constellation(t, pseudo_range).velocity;
|
127
|
+
}
|
128
|
+
static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
129
|
+
return sat(ptr).clock_error(t, pseudo_range);
|
130
|
+
}
|
131
|
+
static float_t clock_error_dot(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
132
|
+
// Clock rate error is already taken into account in constellation()
|
133
|
+
return 0;
|
134
|
+
}
|
135
|
+
};
|
136
|
+
satellite_t res = {
|
137
|
+
&(it_sat->second),
|
138
|
+
impl_t::position, impl_t::velocity,
|
139
|
+
impl_t::clock_error, impl_t::clock_error_dot};
|
140
|
+
return res;
|
141
|
+
}
|
142
|
+
satellites_t(const space_node_t &sn)
|
143
|
+
: impl(&sn), impl_select(select_broadcast) {}
|
144
|
+
} satellites;
|
105
145
|
|
106
146
|
range_correction_t ionospheric_correction, tropospheric_correction;
|
107
147
|
|
@@ -120,7 +160,8 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
120
160
|
}
|
121
161
|
|
122
162
|
GLONASS_SinglePositioning(const space_node_t &sn, const options_t &opt_wish = options_t())
|
123
|
-
: base_t(),
|
163
|
+
: base_t(), _options(available_options(opt_wish)),
|
164
|
+
satellites(sn),
|
124
165
|
ionospheric_correction(), tropospheric_correction() {
|
125
166
|
|
126
167
|
// default ionospheric correction: no correction
|
@@ -163,25 +204,18 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
163
204
|
}
|
164
205
|
|
165
206
|
/**
|
166
|
-
*
|
207
|
+
* Select satellite by using PRN and time
|
167
208
|
*
|
168
209
|
* @param prn satellite number
|
169
210
|
* @param receiver_time receiver time
|
170
|
-
* @return (
|
171
|
-
* otherwise NULL.
|
211
|
+
* @return (satellite_t) If available, satellite.is_available() returning true is returned.
|
172
212
|
*/
|
173
|
-
|
174
|
-
const
|
213
|
+
satellite_t select_satellite(
|
214
|
+
const prn_t &prn,
|
175
215
|
const gps_time_t &receiver_time) const {
|
176
|
-
|
177
|
-
|
178
|
-
|
179
|
-
if((it_sat == sats.end()) // has ephemeris?
|
180
|
-
|| (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
|
181
|
-
return NULL;
|
182
|
-
}
|
183
|
-
|
184
|
-
return &(it_sat->second);
|
216
|
+
prn_t prn_(prn & 0xFF);
|
217
|
+
if(_options.exclude_prn[prn_]){return satellite_t::unavailable();}
|
218
|
+
return satellites.select(prn_, receiver_time);
|
185
219
|
}
|
186
220
|
|
187
221
|
/**
|
@@ -205,32 +239,27 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
205
239
|
|
206
240
|
relative_property_t res = {0};
|
207
241
|
|
208
|
-
if(_options.exclude_prn[prn & 0xFF]){return res;}
|
209
|
-
|
210
242
|
float_t range;
|
211
243
|
range_error_t range_error;
|
212
244
|
if(!this->range(measurement, range, &range_error)){
|
213
245
|
return res; // If no range entry, return with weight = 0
|
214
246
|
}
|
215
247
|
|
216
|
-
|
217
|
-
if(!sat){return res;} // If satellite is unavailable, return with weight = 0
|
248
|
+
satellite_t sat(select_satellite(prn, time_arrival));
|
249
|
+
if(!sat.is_available()){return res;} // If satellite is unavailable, return with weight = 0
|
218
250
|
|
219
251
|
range -= receiver_error;
|
220
252
|
|
221
253
|
// Clock correction, which will be considered in the next constellation()
|
222
254
|
// as extra transmission time by using extra psuedo range.
|
223
255
|
if(range_error.unknown_flag & range_error_t::SATELLITE_CLOCK){
|
224
|
-
range += (sat
|
256
|
+
range += (sat.clock_error(time_arrival, range) * space_node_t::light_speed);
|
225
257
|
}else{
|
226
258
|
range += range_error.value[range_error_t::SATELLITE_CLOCK];
|
227
259
|
}
|
228
260
|
|
229
|
-
// Calculate satellite position
|
230
|
-
|
231
|
-
sat->constellation(time_arrival, range));
|
232
|
-
|
233
|
-
const xyz_t &sat_pos(sat_pos_vel.position);
|
261
|
+
// Calculate satellite position
|
262
|
+
xyz_t sat_pos(sat.position(time_arrival, range));
|
234
263
|
float_t geometric_range(usr_pos.xyz.dist(sat_pos));
|
235
264
|
|
236
265
|
// Calculate residual
|
@@ -278,24 +307,15 @@ class GLONASS_SinglePositioning : public SolverBaseT {
|
|
278
307
|
|
279
308
|
res.range_corrected = range;
|
280
309
|
|
281
|
-
xyz_t rel_vel(
|
310
|
+
xyz_t rel_vel(sat.velocity(time_arrival, range) - usr_vel); // Calculate velocity
|
282
311
|
|
283
|
-
// Note: clock rate error is already accounted for in constellation()
|
284
312
|
res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
|
285
313
|
+ res.los_neg[1] * rel_vel.y()
|
286
|
-
+ res.los_neg[2] * rel_vel.z()
|
314
|
+
+ res.los_neg[2] * rel_vel.z()
|
315
|
+
+ sat.clock_error_dot(time_arrival, range) * space_node_t::light_speed;
|
287
316
|
|
288
317
|
return res;
|
289
318
|
}
|
290
|
-
|
291
|
-
xyz_t *satellite_position(
|
292
|
-
const prn_t &prn,
|
293
|
-
const gps_time_t &time,
|
294
|
-
xyz_t &res) const {
|
295
|
-
|
296
|
-
const satellite_t *sat(is_available(prn, time));
|
297
|
-
return sat ? &(res = sat->position(time)) : NULL;
|
298
|
-
}
|
299
319
|
};
|
300
320
|
|
301
321
|
template <class FloatT, class SolverBaseT>
|
@@ -452,7 +452,7 @@ struct GPS_Time {
|
|
452
452
|
struct {
|
453
453
|
int week;
|
454
454
|
float_t seconds;
|
455
|
-
} uncorrected; // to work around of "incomplete type" error within g++
|
455
|
+
} uncorrected, corrected; // to work around of "incomplete type" error within g++
|
456
456
|
leap_second_event_t(
|
457
457
|
const int &year, const int &month, const int &day,
|
458
458
|
const int &leap)
|
@@ -462,9 +462,11 @@ struct GPS_Time {
|
|
462
462
|
t.tm_year = tm_year;
|
463
463
|
t.tm_mon = tm_mon;
|
464
464
|
t.tm_mday = tm_mday;
|
465
|
-
GPS_Time
|
466
|
-
uncorrected.week =
|
467
|
-
uncorrected.seconds =
|
465
|
+
GPS_Time t_gps_uc(t), t_gps(t_gps_uc + leap);
|
466
|
+
uncorrected.week = t_gps_uc.week;
|
467
|
+
uncorrected.seconds = t_gps_uc.seconds;
|
468
|
+
corrected.week = t_gps.week;
|
469
|
+
corrected.seconds = t_gps.seconds;
|
468
470
|
}
|
469
471
|
};
|
470
472
|
static const leap_second_event_t leap_second_events[];
|
@@ -484,6 +486,92 @@ struct GPS_Time {
|
|
484
486
|
}
|
485
487
|
return 0;
|
486
488
|
}
|
489
|
+
|
490
|
+
int leap_seconds() const {
|
491
|
+
// Treat *this as (normal) GPS time, to which leap seconds are added when it is converted from std::tm
|
492
|
+
for(const leap_second_event_t *i(&leap_second_events[0]); i->leap_seconds > 0; ++i){
|
493
|
+
if(*this >= GPS_Time(i->corrected.week, i->corrected.seconds)){return i->leap_seconds;}
|
494
|
+
}
|
495
|
+
return 0;
|
496
|
+
}
|
497
|
+
float_t julian_date() const {
|
498
|
+
struct conv_t {
|
499
|
+
static int ymd2jd(const int &year, const int &month, const int &day){
|
500
|
+
// @see https://en.wikipedia.org/wiki/Julian_day#Converting_Gregorian_calendar_date_to_Julian_Day_Number
|
501
|
+
return std::div(1461 * (year + 4800 + std::div(month - 14, 12).quot), 4).quot
|
502
|
+
+ std::div(367 * (month - 2 - 12 * std::div((month - 14), 12).quot), 12).quot
|
503
|
+
- std::div(3 * std::div((year + 4900 + std::div(month - 14, 12).quot), 100).quot, 4).quot
|
504
|
+
+ day - 32075;
|
505
|
+
}
|
506
|
+
};
|
507
|
+
// origin of Julian day is "noon (not midnight)" BC4713/1/1
|
508
|
+
static const float_t t0(conv_t::ymd2jd(1980, 1, 6) - 0.5);
|
509
|
+
// GPS Time is advanced by leap seconds compared with UTC.
|
510
|
+
// The following calculation is incorrect for a day in which a leap second is inserted or truncated.
|
511
|
+
// @see https://en.wikipedia.org/wiki/Julian_day#Julian_date_calculation
|
512
|
+
return t0 + week * 7 + (seconds - leap_seconds()) / seconds_day;
|
513
|
+
}
|
514
|
+
float_t julian_date_2000() const {
|
515
|
+
static const std::tm tm_2000 = {0, 0, 12, 1, 0, 2000 - 1900};
|
516
|
+
static const float_t jd2000(GPS_Time(tm_2000, 13).julian_date());
|
517
|
+
return julian_date() - jd2000;
|
518
|
+
}
|
519
|
+
std::tm utc() const {return c_tm(leap_seconds());}
|
520
|
+
|
521
|
+
float_t greenwich_mean_sidereal_time_sec_ires1996(const float_t &delta_ut1 = float_t(0)) const {
|
522
|
+
float_t jd2000(julian_date_2000() + delta_ut1 / seconds_day);
|
523
|
+
float_t jd2000_day(float_t(0.5) + std::floor(jd2000 - 0.5)); // +/-0.5, +/-1.5, ...
|
524
|
+
|
525
|
+
// @see Chapter 2 of Orbits(978-3540785217) by Xu Guochang
|
526
|
+
// @see Chapter 5 of IERS Conventions (1996) https://www.iers.org/IERS/EN/Publications/TechnicalNotes/tn21.html
|
527
|
+
float_t jc(jd2000_day / 36525), // Julian century
|
528
|
+
jc2(std::pow(jc, 2)), jc3(std::pow(jc, 3));
|
529
|
+
float_t gmst0(24110.54841 // = 6h 41m 50.54841s
|
530
|
+
+ jc * 8640184.812866
|
531
|
+
+ jc2 * 0.093104
|
532
|
+
- jc3 * 6.2E-6);
|
533
|
+
// ratio of universal to sidereal time as given by Aoki et al. (1982)
|
534
|
+
float_t r(1.002737909350795 // 7.2921158553E-5 = 1.002737909350795 / 86400 * 2pi
|
535
|
+
+ jc * 5.9006E-11 // 4.3E-15 = 5.9E-11 / 86400 * 2pi
|
536
|
+
- jc2 * 5.9E-15); // in seconds
|
537
|
+
return gmst0 + r * (jd2000 - jd2000_day) * seconds_day;
|
538
|
+
}
|
539
|
+
|
540
|
+
/**
|
541
|
+
* ERA (Earth rotation rate) defined in Sec. 5.5.3 in IERS 2010(Technical Note No.36)
|
542
|
+
*/
|
543
|
+
float_t earth_rotation_angle(const float_t &delta_ut1 = float_t(0),
|
544
|
+
const float_t &scale_factor = float_t(M_PI * 2)) const {
|
545
|
+
float_t jd2000(julian_date_2000() + delta_ut1 / seconds_day);
|
546
|
+
return (jd2000 * 1.00273781191135448 + 0.7790572732640) * scale_factor; // Eq.(5.14)
|
547
|
+
}
|
548
|
+
|
549
|
+
float_t greenwich_mean_sidereal_time_sec_ires2010(const float_t &delta_ut1 = float_t(0)) const {
|
550
|
+
float_t era(earth_rotation_angle(delta_ut1, seconds_day));
|
551
|
+
|
552
|
+
float_t t(julian_date_2000() / 36525);
|
553
|
+
// @see Eq.(5.32) Chapter 5 of IERS Conventions (2010)
|
554
|
+
#define AS2SEC(as) (((float_t)seconds_day / (360 * 3600)) * (as))
|
555
|
+
return era
|
556
|
+
+ AS2SEC(0.014506)
|
557
|
+
+ t * AS2SEC(4612.156534)
|
558
|
+
+ std::pow(t, 2) * AS2SEC(1.3915817)
|
559
|
+
- std::pow(t, 3) * AS2SEC(0.00000044)
|
560
|
+
- std::pow(t, 4) * AS2SEC(0.000029956)
|
561
|
+
- std::pow(t, 5) * AS2SEC(0.0000000368);
|
562
|
+
#undef AS2SEC
|
563
|
+
}
|
564
|
+
|
565
|
+
/**
|
566
|
+
* Calculate Greenwich mean sidereal time (GMST) in seconds
|
567
|
+
* Internally, greenwich_mean_sidereal_time_sec_ired2010() is called.
|
568
|
+
* @param delta_ut1 time difference of UTC and UT1; UT1 = UTC + delta_UT1,
|
569
|
+
* @return GMST in seconds. 86400 seconds correspond to one rotation
|
570
|
+
* @see greenwich_mean_sidereal_time_sec_ires2010()
|
571
|
+
*/
|
572
|
+
float_t greenwich_mean_sidereal_time_sec(const float_t &delta_ut1 = float_t(0)) const {
|
573
|
+
return greenwich_mean_sidereal_time_sec_ires2010(delta_ut1);
|
574
|
+
}
|
487
575
|
};
|
488
576
|
|
489
577
|
template <class FloatT>
|
@@ -95,7 +95,6 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
95
95
|
|
96
96
|
typedef typename base_t::space_node_t space_node_t;
|
97
97
|
inheritate_type(gps_time_t);
|
98
|
-
typedef typename space_node_t::Satellite satellite_t;
|
99
98
|
|
100
99
|
inheritate_type(xyz_t);
|
101
100
|
inheritate_type(llh_t);
|
@@ -106,6 +105,7 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
106
105
|
inheritate_type(prn_obs_t);
|
107
106
|
typedef typename base_t::measurement_t measurement_t;
|
108
107
|
inheritate_type(measurement_items_t);
|
108
|
+
typedef typename base_t::satellite_t satellite_t;
|
109
109
|
typedef typename base_t::range_error_t range_error_t;
|
110
110
|
typedef typename base_t::range_corrector_t range_corrector_t;
|
111
111
|
typedef typename base_t::range_correction_t range_correction_t;
|
@@ -119,11 +119,52 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
119
119
|
GPS_SinglePositioning_Options<float_t>, base_t> options_t;
|
120
120
|
|
121
121
|
protected:
|
122
|
-
const space_node_t &_space_node;
|
123
122
|
GPS_SinglePositioning_Options<float_t> _options;
|
124
123
|
|
125
124
|
public:
|
126
|
-
|
125
|
+
struct satellites_t {
|
126
|
+
const void *impl;
|
127
|
+
satellite_t (*impl_select)(const void *, const prn_t &, const gps_time_t &);
|
128
|
+
inline satellite_t select(const prn_t &prn, const gps_time_t &receiver_time) const {
|
129
|
+
return impl_select(impl, prn, receiver_time);
|
130
|
+
}
|
131
|
+
static satellite_t select_broadcast(
|
132
|
+
const void *ptr, const prn_t &prn, const gps_time_t &receiver_time){
|
133
|
+
// If this static function is defined in inner struct,
|
134
|
+
// C2440 error raises with VC2010
|
135
|
+
const typename space_node_t::satellites_t &sats(
|
136
|
+
reinterpret_cast<const space_node_t *>(ptr)->satellites());
|
137
|
+
const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn));
|
138
|
+
if((it_sat == sats.end()) // has ephemeris?
|
139
|
+
|| (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
|
140
|
+
return satellite_t::unavailable();
|
141
|
+
}
|
142
|
+
struct impl_t {
|
143
|
+
static inline const typename space_node_t::Satellite &sat(const void *ptr) {
|
144
|
+
return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
|
145
|
+
}
|
146
|
+
static xyz_t position(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
147
|
+
return sat(ptr).position(t, pseudo_range);
|
148
|
+
}
|
149
|
+
static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
150
|
+
return sat(ptr).velocity(t, pseudo_range);
|
151
|
+
}
|
152
|
+
static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
153
|
+
return sat(ptr).clock_error(t, pseudo_range);
|
154
|
+
}
|
155
|
+
static float_t clock_error_dot(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
156
|
+
return sat(ptr).clock_error_dot(t, pseudo_range);
|
157
|
+
}
|
158
|
+
};
|
159
|
+
satellite_t res = {
|
160
|
+
&(it_sat->second),
|
161
|
+
impl_t::position, impl_t::velocity,
|
162
|
+
impl_t::clock_error, impl_t::clock_error_dot};
|
163
|
+
return res;
|
164
|
+
}
|
165
|
+
satellites_t(const space_node_t &sn)
|
166
|
+
: impl(&sn), impl_select(select_broadcast) {}
|
167
|
+
} satellites;
|
127
168
|
|
128
169
|
struct klobuchar_t : public range_corrector_t {
|
129
170
|
const space_node_t &space_node;
|
@@ -134,6 +175,7 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
134
175
|
float_t *calculate(
|
135
176
|
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
136
177
|
float_t &buf) const {
|
178
|
+
if(!is_available(t)){return NULL;}
|
137
179
|
return &(buf = space_node.iono_correction(sat_rel_pos, usr_pos.llh, t));
|
138
180
|
}
|
139
181
|
} ionospheric_klobuchar;
|
@@ -185,7 +227,8 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
185
227
|
}
|
186
228
|
|
187
229
|
GPS_SinglePositioning(const space_node_t &sn)
|
188
|
-
: base_t(),
|
230
|
+
: base_t(), _options(available_options(options_t())),
|
231
|
+
satellites(sn),
|
189
232
|
ionospheric_klobuchar(sn), ionospheric_ntcm_gl(),
|
190
233
|
tropospheric_simplified(),
|
191
234
|
ionospheric_correction(), tropospheric_correction() {
|
@@ -309,27 +352,17 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
309
352
|
}
|
310
353
|
|
311
354
|
/**
|
312
|
-
*
|
355
|
+
* Select satellite by using PRN and time
|
313
356
|
*
|
314
357
|
* @param prn satellite number
|
315
358
|
* @param receiver_time receiver time
|
316
|
-
* @return (
|
317
|
-
* otherwise NULL.
|
359
|
+
* @return (satellite_t) If available, satellite.is_available() returning true is returned.
|
318
360
|
*/
|
319
|
-
|
320
|
-
const
|
361
|
+
satellite_t select_satellite(
|
362
|
+
const prn_t &prn,
|
321
363
|
const gps_time_t &receiver_time) const {
|
322
|
-
|
323
|
-
|
324
|
-
|
325
|
-
const typename space_node_t::satellites_t &sats(_space_node.satellites());
|
326
|
-
const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn));
|
327
|
-
if((it_sat == sats.end()) // has ephemeris?
|
328
|
-
|| (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
|
329
|
-
return NULL;
|
330
|
-
}
|
331
|
-
|
332
|
-
return &(it_sat->second);
|
364
|
+
if(_options.exclude_prn[prn]){return satellite_t::unavailable();}
|
365
|
+
return satellites.select(prn, receiver_time);
|
333
366
|
}
|
334
367
|
|
335
368
|
/**
|
@@ -359,8 +392,8 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
359
392
|
return res; // If no range entry, return with weight = 0
|
360
393
|
}
|
361
394
|
|
362
|
-
|
363
|
-
if(!sat){return res;} // If satellite is unavailable, return with weight = 0
|
395
|
+
satellite_t sat(select_satellite(prn, time_arrival));
|
396
|
+
if(!sat.is_available()){return res;} // If satellite is unavailable, return with weight = 0
|
364
397
|
|
365
398
|
residual_t residual = {
|
366
399
|
res.range_residual,
|
@@ -369,9 +402,9 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
369
402
|
};
|
370
403
|
|
371
404
|
res.range_corrected = range_corrected(
|
372
|
-
|
405
|
+
sat, range - receiver_error, time_arrival,
|
373
406
|
usr_pos, residual, range_error);
|
374
|
-
res.rate_relative_neg = rate_relative_neg(
|
407
|
+
res.rate_relative_neg = rate_relative_neg(sat, res.range_corrected, time_arrival, usr_vel,
|
375
408
|
res.los_neg[0], res.los_neg[1], res.los_neg[2]);
|
376
409
|
|
377
410
|
return res;
|
@@ -415,7 +448,7 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
415
448
|
float_t range;
|
416
449
|
if(!this->range(it->second, range)){continue;} // No range entry
|
417
450
|
|
418
|
-
if(!
|
451
|
+
if(!(select_satellite(it->first, receiver_time).is_available())){continue;} // No satellite
|
419
452
|
|
420
453
|
typename base_t::measurement2_t::value_type v = {
|
421
454
|
it->first, &(it->second), this}; // prn, measurement, solver
|
@@ -426,15 +459,6 @@ class GPS_SinglePositioning : public SolverBaseT {
|
|
426
459
|
measurement2, receiver_time, user_position_init, receiver_error_init,
|
427
460
|
typename base_t::user_pvt_opt_t(good_init, with_velocity));
|
428
461
|
}
|
429
|
-
|
430
|
-
xyz_t *satellite_position(
|
431
|
-
const prn_t &prn,
|
432
|
-
const gps_time_t &time,
|
433
|
-
xyz_t &res) const {
|
434
|
-
|
435
|
-
const satellite_t *sat(is_available(prn, time));
|
436
|
-
return sat ? &(res = sat->position(time)) : NULL;
|
437
|
-
}
|
438
462
|
};
|
439
463
|
|
440
464
|
#endif /* __GPS_SOLVER_H__ */
|
@@ -226,6 +226,55 @@ struct GPS_Solver_Base {
|
|
226
226
|
return res;
|
227
227
|
}
|
228
228
|
|
229
|
+
struct satellite_t {
|
230
|
+
const void *impl;
|
231
|
+
xyz_t (*impl_position)(const void *, const gps_time_t &, const float_t &);
|
232
|
+
xyz_t (*impl_velocity)(const void *, const gps_time_t &, const float_t &);
|
233
|
+
float_t (*impl_clock_error)(const void *, const gps_time_t &, const float_t &);
|
234
|
+
float_t (*impl_clock_error_dot)(const void *, const gps_time_t &, const float_t &);
|
235
|
+
inline bool is_available() const {
|
236
|
+
return impl != NULL;
|
237
|
+
}
|
238
|
+
inline xyz_t position(const gps_time_t &t, const float_t &pseudo_range = 0) const {
|
239
|
+
return impl_position(impl, t, pseudo_range);
|
240
|
+
}
|
241
|
+
inline xyz_t velocity(const gps_time_t &t, const float_t &pseudo_range = 0) const {
|
242
|
+
return impl_velocity(impl, t, pseudo_range);
|
243
|
+
}
|
244
|
+
inline float_t clock_error(const gps_time_t &t, const float_t &pseudo_range = 0) const {
|
245
|
+
return impl_clock_error(impl, t, pseudo_range);
|
246
|
+
}
|
247
|
+
inline float_t clock_error_dot(const gps_time_t &t, const float_t &pseudo_range = 0) const {
|
248
|
+
return impl_clock_error_dot(impl, t, pseudo_range);
|
249
|
+
}
|
250
|
+
static const satellite_t &unavailable() {
|
251
|
+
struct impl_t {
|
252
|
+
static xyz_t v3(const void *, const gps_time_t &, const float_t &){
|
253
|
+
return xyz_t(0, 0, 0);
|
254
|
+
}
|
255
|
+
static float_t v(const void *, const gps_time_t &, const float_t &){
|
256
|
+
return float_t(0);
|
257
|
+
}
|
258
|
+
};
|
259
|
+
static const satellite_t res = {NULL, impl_t::v3, impl_t::v3, impl_t::v, impl_t::v};
|
260
|
+
return res;
|
261
|
+
}
|
262
|
+
};
|
263
|
+
|
264
|
+
/**
|
265
|
+
* Select satellite by using PRN and time
|
266
|
+
* This function should be overridden.
|
267
|
+
*
|
268
|
+
* @param prn satellite number
|
269
|
+
* @param receiver_time receiver time
|
270
|
+
* @return (satellite_t) If available, satellite.is_available() returning true is returned.
|
271
|
+
*/
|
272
|
+
virtual satellite_t select_satellite(
|
273
|
+
const prn_t &prn,
|
274
|
+
const gps_time_t &receiver_time) const {
|
275
|
+
return satellite_t::unavailable();
|
276
|
+
}
|
277
|
+
|
229
278
|
struct range_corrector_t {
|
230
279
|
virtual ~range_corrector_t() {}
|
231
280
|
virtual bool is_available(const gps_time_t &t) const {
|
@@ -838,21 +887,6 @@ protected:
|
|
838
887
|
}
|
839
888
|
|
840
889
|
public:
|
841
|
-
/**
|
842
|
-
* Calculate satellite position
|
843
|
-
*
|
844
|
-
* @param prn satellite number
|
845
|
-
* @param time GPS time
|
846
|
-
* @param res object to which results are stored
|
847
|
-
* @return If position is available, &res will be returned, otherwise NULL.
|
848
|
-
*/
|
849
|
-
virtual xyz_t *satellite_position(
|
850
|
-
const prn_t &prn,
|
851
|
-
const gps_time_t &time,
|
852
|
-
xyz_t &res) const {
|
853
|
-
return NULL;
|
854
|
-
}
|
855
|
-
|
856
890
|
/**
|
857
891
|
* Calculate User position/velocity with hint
|
858
892
|
* This is multi-constellation version,
|
@@ -882,8 +916,7 @@ public:
|
|
882
916
|
it != it_end; ++it){
|
883
917
|
typename measurement2_t::value_type v = {
|
884
918
|
it->first, &(it->second), &select(it->first)}; // prn, measurement, solver
|
885
|
-
|
886
|
-
if(!v.solver->satellite_position(v.prn, receiver_time, sat)){
|
919
|
+
if(!v.solver->select_satellite(v.prn, receiver_time).is_available()){
|
887
920
|
// pre-check satellite availability; remove it when its position is unknown
|
888
921
|
continue;
|
889
922
|
}
|