gps_pvt 0.4.1 → 0.6.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- checksums.yaml +4 -4
- data/README.md +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
|
}
|