gps_pvt 0.8.0 → 0.8.2
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/Rakefile +1 -0
- data/exe/gps_pvt +1 -17
- data/exe/to_ubx +162 -32
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +1677 -399
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +137 -9
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +5 -3
- data/ext/ninja-scan-light/tool/navigation/GPS.h +301 -50
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +5 -3
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_MultiFrequency.h +1 -0
- data/ext/ninja-scan-light/tool/navigation/QZSS.h +48 -15
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +106 -4
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +5 -3
- data/ext/ninja-scan-light/tool/param/bit_array.h +53 -16
- data/ext/ninja-scan-light/tool/swig/GPS.i +265 -25
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +64 -0
- data/ext/ninja-scan-light/tool/util/bit_counter.h +84 -0
- data/lib/gps_pvt/receiver/extension.rb +51 -0
- data/lib/gps_pvt/receiver.rb +49 -32
- data/lib/gps_pvt/version.rb +1 -1
- metadata +4 -2
@@ -29,6 +29,7 @@
|
|
29
29
|
#include "navigation/GPS_Solver_Base.h"
|
30
30
|
#include "navigation/GPS_Solver.h"
|
31
31
|
#include "navigation/GPS_Solver_RAIM.h"
|
32
|
+
#include "navigation/GPS_Solver_MultiFrequency.h"
|
32
33
|
#include "navigation/SBAS_Solver.h"
|
33
34
|
#include "navigation/GLONASS_Solver.h"
|
34
35
|
|
@@ -273,6 +274,25 @@ struct GPS_Ionospheric_UTC_Parameters : public GPS_SpaceNode<FloatT>::Ionospheri
|
|
273
274
|
(typename GPS_SpaceNode<FloatT>::Ionospheric_UTC_Parameters &)res = raw;
|
274
275
|
return res;
|
275
276
|
}
|
277
|
+
%typemap(in,numinputs=0) unsigned int buf_brdc[ANY] (unsigned int temp[$1_dim0] = {0}) "$1 = temp;"
|
278
|
+
%typemap(argout) unsigned int buf_brdc[ANY] {
|
279
|
+
for(int i(0); i < $1_dim0; ++i){
|
280
|
+
%append_output(SWIG_From(unsigned int)(($1)[i]));
|
281
|
+
}
|
282
|
+
}
|
283
|
+
/**
|
284
|
+
* Return broadcasted raw data related to ionospheric and UTC parameters.
|
285
|
+
* @param buf_brdc pointer to store raw data of subframe 4 page 18.
|
286
|
+
* Each 30bit length word (MSB 2 bits are padding) is stored in each successive address of the pointer.
|
287
|
+
* @param t GPS time at broadcasting
|
288
|
+
*/
|
289
|
+
void dump(unsigned int buf_brdc[10], const GPS_Time<FloatT> &t){
|
290
|
+
typedef typename GPS_SpaceNode<FloatT>
|
291
|
+
::BroadcastedMessage<unsigned int, 30> dump_t;
|
292
|
+
dump_t::how_set(buf_brdc, t);
|
293
|
+
GPS_SpaceNode<FloatT>::Ionospheric_UTC_Parameters::raw_t raw;
|
294
|
+
(raw = *self).dump<2, 0>(buf_brdc);
|
295
|
+
}
|
276
296
|
}
|
277
297
|
|
278
298
|
%inline %{
|
@@ -287,6 +307,11 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
287
307
|
|| (this->iode != this->iode_subframe3)
|
288
308
|
|| ((this->iodc & 0xFF) != this->iode));
|
289
309
|
}
|
310
|
+
bool is_valid(const GPS_Time<FloatT> &t) const {
|
311
|
+
return is_consistent() && GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::is_valid(t);
|
312
|
+
}
|
313
|
+
GPS_Time<FloatT> t_clock() const {return GPS_Time<FloatT>(this->WN, this->t_oc);}
|
314
|
+
GPS_Time<FloatT> t_ephemeris() const {return GPS_Time<FloatT>(this->WN, this->t_oe);}
|
290
315
|
GPS_Ephemeris() : GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris() {
|
291
316
|
invalidate();
|
292
317
|
}
|
@@ -303,7 +328,7 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
303
328
|
MAKE_ACCESSOR(svid, unsigned int);
|
304
329
|
|
305
330
|
MAKE_ACCESSOR(WN, unsigned int);
|
306
|
-
MAKE_ACCESSOR(URA,
|
331
|
+
MAKE_ACCESSOR(URA, FloatT);
|
307
332
|
MAKE_ACCESSOR(SV_health, unsigned int);
|
308
333
|
MAKE_ACCESSOR(iodc, int);
|
309
334
|
MAKE_ACCESSOR(t_GD, FloatT);
|
@@ -384,6 +409,83 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
384
409
|
break;
|
385
410
|
}
|
386
411
|
}
|
412
|
+
%apply unsigned int buf_brdc[ANY] {
|
413
|
+
unsigned int buf_sf1[10], unsigned int buf_sf2[10], unsigned int buf_sf3[10]};
|
414
|
+
/**
|
415
|
+
* Return broadcasted raw data of ephemeris data.
|
416
|
+
* @param buf_sf1 pointer to store raw data of subframe 1.
|
417
|
+
* Each 30bit length word (MSB 2 bits are padding) is stored in each successive address of the pointer.
|
418
|
+
* @param buf_sf2 pointer to store raw data of subframe 2. Its structue is same as sf1.
|
419
|
+
* @param buf_sf3 pointer to store raw data of subframe 3. Its structue is same as sf1.
|
420
|
+
* @param t GPS time at broadcasting
|
421
|
+
*/
|
422
|
+
void dump(
|
423
|
+
unsigned int buf_sf1[10], unsigned int buf_sf2[10], unsigned int buf_sf3[10],
|
424
|
+
const GPS_Time<FloatT> &t){
|
425
|
+
typedef typename GPS_SpaceNode<FloatT>
|
426
|
+
::BroadcastedMessage<unsigned int, 30> dump_t;
|
427
|
+
GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t raw;
|
428
|
+
raw = *self;
|
429
|
+
unsigned int *buf[10] = {buf_sf1, buf_sf2, buf_sf3};
|
430
|
+
for(int i(0); i < 3; ++i){
|
431
|
+
dump_t::how_set(buf[i], t);
|
432
|
+
raw.dump<2, 0>(buf[i], i + 1);
|
433
|
+
}
|
434
|
+
}
|
435
|
+
int parse_almanac(const unsigned int buf[10]){
|
436
|
+
typedef GPS_SpaceNode<FloatT>::BroadcastedMessage<unsigned int, 30> parse_t;
|
437
|
+
switch(parse_t::subframe_id(buf)){
|
438
|
+
case 4:
|
439
|
+
case 5:
|
440
|
+
break;
|
441
|
+
default: return -1;
|
442
|
+
}
|
443
|
+
typedef GPS_SpaceNode<FloatT>::SatelliteProperties::Almanac almanac_t;
|
444
|
+
almanac_t::raw_t raw;
|
445
|
+
switch(parse_t::data_id(buf)){
|
446
|
+
case 1: // GPS
|
447
|
+
raw.update<2, 0>(buf);
|
448
|
+
if((raw.svid < 1) || (raw.svid > 32)){return -1;}
|
449
|
+
break;
|
450
|
+
case 3: // QZSS
|
451
|
+
reinterpret_cast<QZSS_SpaceNode<FloatT>::SatelliteProperties::Almanac::raw_t &>(raw)
|
452
|
+
.update<2, 0>(buf);
|
453
|
+
if((raw.svid < 193) || (raw.svid > 202)){return -1;}
|
454
|
+
break;
|
455
|
+
default:
|
456
|
+
return -1;
|
457
|
+
}
|
458
|
+
almanac_t almanac;
|
459
|
+
*self = (GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris)(almanac = raw);
|
460
|
+
return self->svid;
|
461
|
+
}
|
462
|
+
/**
|
463
|
+
* Return broadcasted raw data of almanac data.
|
464
|
+
* @param buf_brdc pointer to store raw data of subframe 4 or 5.
|
465
|
+
* Each 30bit (MSB 2 bits are padding) length word is stored in each successive address of the pointer.
|
466
|
+
* @param t GPS time at broadcasting
|
467
|
+
* @param qzss_subframe if 4 or 5, this ephemeris is treated as QZSS, otherwise GPS (default).
|
468
|
+
*/
|
469
|
+
void dump_almanac(
|
470
|
+
unsigned int buf_brdc[10], const GPS_Time<FloatT> &t,
|
471
|
+
const unsigned int &qzss_subframe = 0){
|
472
|
+
typedef typename GPS_SpaceNode<FloatT>
|
473
|
+
::BroadcastedMessage<unsigned int, 30> dump_t;
|
474
|
+
dump_t::how_set(buf_brdc, t);
|
475
|
+
typedef GPS_SpaceNode<FloatT>::SatelliteProperties::Almanac almanac_t;
|
476
|
+
almanac_t almanac;
|
477
|
+
almanac_t::raw_t raw;
|
478
|
+
raw = (almanac = *self);
|
479
|
+
switch(qzss_subframe){
|
480
|
+
case 4:
|
481
|
+
case 5:
|
482
|
+
reinterpret_cast<QZSS_SpaceNode<FloatT>::SatelliteProperties::Almanac::raw_t &>(raw)
|
483
|
+
.dump<2, 0>(buf_brdc, qzss_subframe);
|
484
|
+
break;
|
485
|
+
default:
|
486
|
+
raw.dump<2, 0>(buf_brdc);
|
487
|
+
}
|
488
|
+
}
|
387
489
|
%typemap(out) constellation_res_t {
|
388
490
|
%append_output(SWIG_NewPointerObj((new System_XYZ<FloatT, WGS84>($1.position)),
|
389
491
|
$descriptor(System_XYZ<FloatT, WGS84> *), SWIG_POINTER_OWN));
|
@@ -402,6 +504,7 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
402
504
|
}
|
403
505
|
#if defined(SWIGRUBY)
|
404
506
|
%rename("consistent?") is_consistent;
|
507
|
+
%rename("valid?") is_valid;
|
405
508
|
#endif
|
406
509
|
}
|
407
510
|
|
@@ -444,6 +547,10 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
|
|
444
547
|
SBAS_Ephemeris() : SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris() {}
|
445
548
|
SBAS_Ephemeris(const typename SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris &eph)
|
446
549
|
: SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris(eph) {}
|
550
|
+
bool is_valid(const GPS_Time<FloatT> &t) const {
|
551
|
+
return SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::is_valid(t);
|
552
|
+
}
|
553
|
+
GPS_Time<FloatT> t_applicable() const {return GPS_Time<FloatT>(this->WN, this->t_0);}
|
447
554
|
};
|
448
555
|
%}
|
449
556
|
%extend SBAS_Ephemeris {
|
@@ -457,6 +564,18 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
|
|
457
564
|
MAKE_ACCESSOR(ddx, FloatT); MAKE_ACCESSOR(ddy, FloatT); MAKE_ACCESSOR(ddz, FloatT);
|
458
565
|
MAKE_ACCESSOR(a_Gf0, FloatT);
|
459
566
|
MAKE_ACCESSOR(a_Gf1, FloatT);
|
567
|
+
/**
|
568
|
+
* Return broadcasted raw data of SBAS ephemeris
|
569
|
+
* @param buf_brdc pointer to store raw data of Type 9 message.
|
570
|
+
* 250 of 256 bits are effective, LSB 6 bits of last word are padding.
|
571
|
+
* @param preamble_idx index of preamble, 0(0x53), 1(0x9A), or 2(0xC6)
|
572
|
+
*/
|
573
|
+
void dump(unsigned int buf_brdc[8], const unsigned int &preamble_idx = 0){
|
574
|
+
SBAS_SpaceNode<FloatT>::DataBlock::preamble_set2(buf_brdc, preamble_idx);
|
575
|
+
SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t raw;
|
576
|
+
(raw = *self).dump(buf_brdc);
|
577
|
+
SBAS_SpaceNode<FloatT>::DataBlock::parity_set(buf_brdc);
|
578
|
+
}
|
460
579
|
typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
|
461
580
|
const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0,
|
462
581
|
const bool &with_velocity = true) const {
|
@@ -466,6 +585,9 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
|
|
466
585
|
pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot(t_tx)};
|
467
586
|
return res;
|
468
587
|
}
|
588
|
+
#if defined(SWIGRUBY)
|
589
|
+
%rename("valid?") is_valid;
|
590
|
+
#endif
|
469
591
|
}
|
470
592
|
|
471
593
|
%extend SBAS_SpaceNode {
|
@@ -519,6 +641,9 @@ struct GLONASS_Ephemeris
|
|
519
641
|
bool is_consistent() const {
|
520
642
|
return has_string == 0x1F;
|
521
643
|
}
|
644
|
+
bool is_valid(const GPS_Time<FloatT> &t) const {
|
645
|
+
return is_consistent() && eph_t::is_valid(t);
|
646
|
+
}
|
522
647
|
GLONASS_Ephemeris() : eph_t() {
|
523
648
|
invalidate();
|
524
649
|
}
|
@@ -607,6 +732,31 @@ struct GLONASS_Ephemeris
|
|
607
732
|
self->has_string = has_string;
|
608
733
|
return updated;
|
609
734
|
}
|
735
|
+
%apply unsigned int buf_brdc[ANY] {
|
736
|
+
unsigned int buf_str1[3], unsigned int buf_str2[3], unsigned int buf_str3[3],
|
737
|
+
unsigned int buf_str4[3], unsigned int buf_str5[3]};
|
738
|
+
/**
|
739
|
+
* Return broadcasted raw data of GLONASS ephemeris data.
|
740
|
+
* @param buf_str1 pointer to store raw data of string 1.
|
741
|
+
* 85bit length data (LSB 11 bits are padding) is stored in successive address of the pointer.
|
742
|
+
* @param buf_str2 pointer to store raw data of string 2. Its structue is same as str1.
|
743
|
+
* @param buf_str3 pointer to store raw data of string 3. Its structue is same as str1.
|
744
|
+
* @param buf_str4 pointer to store raw data of string 4. Its structue is same as str1.
|
745
|
+
* @param buf_str5 pointer to store raw data of string 5. Its structue is same as str1.
|
746
|
+
* @param t GPS time at broadcasting
|
747
|
+
*/
|
748
|
+
void dump(
|
749
|
+
unsigned int buf_str1[3], unsigned int buf_str2[3], unsigned int buf_str3[3],
|
750
|
+
unsigned int buf_str4[3], unsigned int buf_str5[3],
|
751
|
+
const GPS_Time<FloatT> &t){
|
752
|
+
typename GLONASS_Ephemeris<FloatT>::eph_t::raw_t raw;
|
753
|
+
raw = *self;
|
754
|
+
unsigned int *buf[4] = {buf_str1, buf_str2, buf_str3, buf_str4};
|
755
|
+
for(int i(0); i < 4; ++i){
|
756
|
+
raw.GLONASS_Ephemeris<FloatT>::Ephemeris::raw_t::dump<0, 0>(buf[i], i + 1);
|
757
|
+
}
|
758
|
+
raw.GLONASS_Ephemeris<FloatT>::TimeProperties::raw_t::dump<0, 0>(buf_str5);
|
759
|
+
}
|
610
760
|
typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
|
611
761
|
const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0) const {
|
612
762
|
typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t pv(
|
@@ -617,6 +767,7 @@ struct GLONASS_Ephemeris
|
|
617
767
|
}
|
618
768
|
#if defined(SWIGRUBY)
|
619
769
|
%rename("consistent?") is_consistent;
|
770
|
+
%rename("valid?") is_valid;
|
620
771
|
%rename("in_range?") is_in_range;
|
621
772
|
#endif
|
622
773
|
bool is_in_range(const GPS_Time<FloatT> &t) const {
|
@@ -969,6 +1120,16 @@ struct GPS_Measurement {
|
|
969
1120
|
L1_SIGNAL_STRENGTH_dBHz,
|
970
1121
|
L1_LOCK_SEC,
|
971
1122
|
L1_FREQUENCY,
|
1123
|
+
#define make_entry(key) L2CM_ ## key, L2CL_ ## key
|
1124
|
+
#define make_entry2(key) make_entry(key), make_entry(key ## _SIGMA)
|
1125
|
+
make_entry2(PSEUDORANGE),
|
1126
|
+
make_entry2(CARRIER_PHASE),
|
1127
|
+
make_entry2(DOPPLER),
|
1128
|
+
make_entry2(RANGE_RATE),
|
1129
|
+
make_entry(SIGNAL_STRENGTH_dBHz),
|
1130
|
+
make_entry(LOCK_SEC),
|
1131
|
+
#undef make_entry2
|
1132
|
+
#undef make_entry
|
972
1133
|
ITEMS_PREDEFINED,
|
973
1134
|
};
|
974
1135
|
void add(const int &prn, const int &key, const FloatT &value){
|
@@ -992,7 +1153,7 @@ const type &get_ ## name () const {
|
|
992
1153
|
MAKE_ACCESSOR2(residual_mask, FloatT);
|
993
1154
|
#undef MAKE_ACCESSOR2
|
994
1155
|
MAKE_VECTOR2ARRAY(int);
|
995
|
-
%ignore
|
1156
|
+
%ignore cast_general;
|
996
1157
|
}
|
997
1158
|
%inline %{
|
998
1159
|
template <class FloatT>
|
@@ -1006,17 +1167,23 @@ struct GPS_SolverOptions_Common {
|
|
1006
1167
|
%extend GPS_SolverOptions {
|
1007
1168
|
%ignore base_t;
|
1008
1169
|
%ignore cast_general;
|
1170
|
+
#ifdef SWIGRUBY
|
1171
|
+
%rename("exclude_L2C?") get_exclude_L2C;
|
1172
|
+
%rename("exclude_L2C=") set_exclude_L2C;
|
1173
|
+
#endif
|
1009
1174
|
MAKE_VECTOR2ARRAY(int);
|
1010
1175
|
}
|
1011
1176
|
%inline %{
|
1012
1177
|
template <class FloatT>
|
1013
1178
|
struct GPS_SolverOptions
|
1014
|
-
: public GPS_SinglePositioning<FloatT>::options_t,
|
1179
|
+
: public GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >::options_t,
|
1015
1180
|
GPS_SolverOptions_Common<FloatT> {
|
1016
|
-
typedef typename GPS_SinglePositioning<FloatT>::options_t base_t;
|
1181
|
+
typedef typename GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >::options_t base_t;
|
1017
1182
|
void exclude(const int &prn){base_t::exclude_prn.set(prn);}
|
1018
1183
|
void include(const int &prn){base_t::exclude_prn.reset(prn);}
|
1019
1184
|
std::vector<int> excluded() const {return base_t::exclude_prn.excluded();}
|
1185
|
+
bool get_exclude_L2C() {return base_t::exclude_L2C;}
|
1186
|
+
bool set_exclude_L2C(const bool &b) {return base_t::exclude_L2C = b;}
|
1020
1187
|
GPS_Solver_GeneralOptions<FloatT> *cast_general(){return this;}
|
1021
1188
|
const GPS_Solver_GeneralOptions<FloatT> *cast_general() const {return this;}
|
1022
1189
|
};
|
@@ -1081,6 +1248,44 @@ struct GPS_RangeCorrector
|
|
1081
1248
|
};
|
1082
1249
|
}
|
1083
1250
|
|
1251
|
+
%{
|
1252
|
+
template <class BaseT, class HookT>
|
1253
|
+
struct HookableSolver : public BaseT {
|
1254
|
+
typedef BaseT base_t;
|
1255
|
+
HookT *hook;
|
1256
|
+
template <class ArgT>
|
1257
|
+
HookableSolver(const ArgT &);
|
1258
|
+
virtual typename base_t::relative_property_t relative_property(
|
1259
|
+
const typename base_t::prn_t &prn,
|
1260
|
+
const typename base_t::measurement_t::mapped_type &measurement,
|
1261
|
+
const typename base_t::float_t &receiver_error,
|
1262
|
+
const typename base_t::gps_time_t &time_arrival,
|
1263
|
+
const typename base_t::pos_t &usr_pos,
|
1264
|
+
const typename base_t::xyz_t &usr_vel) const {
|
1265
|
+
typename base_t::relative_property_t res(
|
1266
|
+
base_t::relative_property(
|
1267
|
+
prn, measurement, receiver_error, time_arrival,
|
1268
|
+
usr_pos, usr_vel));
|
1269
|
+
if(hook){
|
1270
|
+
res = hook->relative_property(
|
1271
|
+
prn, measurement, receiver_error, time_arrival,
|
1272
|
+
usr_pos, usr_vel,
|
1273
|
+
res);
|
1274
|
+
}
|
1275
|
+
return res;
|
1276
|
+
}
|
1277
|
+
virtual typename base_t::satellite_t select_satellite(
|
1278
|
+
const typename base_t::prn_t &prn,
|
1279
|
+
const typename base_t::gps_time_t &time) const {
|
1280
|
+
typename base_t::satellite_t res(base_t::select_satellite(prn, time));
|
1281
|
+
if(hook){
|
1282
|
+
res = hook->select_satellite(prn, time, res);
|
1283
|
+
}
|
1284
|
+
return res;
|
1285
|
+
}
|
1286
|
+
};
|
1287
|
+
%}
|
1288
|
+
|
1084
1289
|
%extend GPS_Solver {
|
1085
1290
|
%ignore super_t;
|
1086
1291
|
%ignore base_t;
|
@@ -1090,7 +1295,7 @@ struct GPS_RangeCorrector
|
|
1090
1295
|
%ignore sbas;
|
1091
1296
|
%ignore glonass_t;
|
1092
1297
|
%ignore glonass;
|
1093
|
-
%ignore
|
1298
|
+
%ignore select;
|
1094
1299
|
%ignore relative_property;
|
1095
1300
|
%ignore select_satellite;
|
1096
1301
|
%ignore update_position_solution;
|
@@ -1102,6 +1307,20 @@ struct GPS_RangeCorrector
|
|
1102
1307
|
fragment=SWIG_From_frag(int),
|
1103
1308
|
fragment=SWIG_Traits_frag(FloatT),
|
1104
1309
|
fragment=SWIG_Traits_frag(GPS_Measurement<FloatT>)){
|
1310
|
+
template <> template <>
|
1311
|
+
HookableSolver<
|
1312
|
+
GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >,
|
1313
|
+
GPS_Solver<FloatT> >
|
1314
|
+
::HookableSolver<GPS_SpaceNode<FloatT> >(const GPS_SpaceNode<FloatT> &sn)
|
1315
|
+
: GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >(sn), hook(NULL) {}
|
1316
|
+
template <> template <>
|
1317
|
+
HookableSolver<SBAS_SinglePositioning<FloatT>, GPS_Solver<FloatT> >
|
1318
|
+
::HookableSolver<SBAS_SpaceNode<FloatT> >(const SBAS_SpaceNode<FloatT> &sn)
|
1319
|
+
: SBAS_SinglePositioning<FloatT>(sn), hook(NULL) {}
|
1320
|
+
template <> template <>
|
1321
|
+
HookableSolver<GLONASS_SinglePositioning<FloatT>, GPS_Solver<FloatT> >
|
1322
|
+
::HookableSolver<GLONASS_SpaceNode<FloatT> >(const GLONASS_SpaceNode<FloatT> &sn)
|
1323
|
+
: GLONASS_SinglePositioning<FloatT>(sn), hook(NULL) {}
|
1105
1324
|
template <>
|
1106
1325
|
GPS_Solver<FloatT>::base_t::relative_property_t
|
1107
1326
|
GPS_Solver<FloatT>::relative_property(
|
@@ -1110,14 +1329,12 @@ struct GPS_RangeCorrector
|
|
1110
1329
|
const GPS_Solver<FloatT>::base_t::float_t &receiver_error,
|
1111
1330
|
const GPS_Solver<FloatT>::base_t::gps_time_t &time_arrival,
|
1112
1331
|
const GPS_Solver<FloatT>::base_t::pos_t &usr_pos,
|
1113
|
-
const GPS_Solver<FloatT>::base_t::xyz_t &usr_vel
|
1332
|
+
const GPS_Solver<FloatT>::base_t::xyz_t &usr_vel,
|
1333
|
+
const GPS_Solver<FloatT>::base_t::relative_property_t &res_orig) const {
|
1114
1334
|
union {
|
1115
1335
|
base_t::relative_property_t prop;
|
1116
1336
|
FloatT values[7];
|
1117
|
-
} res = {
|
1118
|
-
select_solver(prn).relative_property(
|
1119
|
-
prn, measurement, receiver_error, time_arrival,
|
1120
|
-
usr_pos, usr_vel)};
|
1337
|
+
} res = {res_orig};
|
1121
1338
|
#ifdef SWIGRUBY
|
1122
1339
|
do{
|
1123
1340
|
static const VALUE key(ID2SYM(rb_intern("relative_property")));
|
@@ -1197,9 +1414,9 @@ struct GPS_RangeCorrector
|
|
1197
1414
|
template <>
|
1198
1415
|
GPS_Solver<FloatT>::base_t::satellite_t GPS_Solver<FloatT>::select_satellite(
|
1199
1416
|
const GPS_Solver<FloatT>::base_t::prn_t &prn,
|
1200
|
-
const GPS_Solver<FloatT>::base_t::gps_time_t &time
|
1201
|
-
|
1202
|
-
|
1417
|
+
const GPS_Solver<FloatT>::base_t::gps_time_t &time,
|
1418
|
+
const GPS_Solver<FloatT>::base_t::satellite_t &res_orig) const {
|
1419
|
+
GPS_Solver<FloatT>::base_t::satellite_t res(res_orig);
|
1203
1420
|
#ifdef SWIGRUBY
|
1204
1421
|
if(!res.is_available()){
|
1205
1422
|
static const VALUE key(ID2SYM(rb_intern("relative_property")));
|
@@ -1396,7 +1613,9 @@ struct GPS_Solver
|
|
1396
1613
|
struct gps_t {
|
1397
1614
|
GPS_SpaceNode<FloatT> space_node;
|
1398
1615
|
GPS_SolverOptions<FloatT> options;
|
1399
|
-
|
1616
|
+
HookableSolver<
|
1617
|
+
GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >,
|
1618
|
+
GPS_Solver<FloatT> > solver;
|
1400
1619
|
struct ephemeris_proxy_t {
|
1401
1620
|
struct item_t {
|
1402
1621
|
const void *impl;
|
@@ -1418,18 +1637,20 @@ struct GPS_Solver
|
|
1418
1637
|
solver.satellites.impl_select = forward;
|
1419
1638
|
}
|
1420
1639
|
} ephemeris_proxy;
|
1421
|
-
gps_t() : space_node(), options(), solver(space_node), ephemeris_proxy(solver) {
|
1640
|
+
gps_t() : space_node(), options(), solver(space_node), ephemeris_proxy(solver) {
|
1641
|
+
options.exclude_L2C = true;
|
1642
|
+
}
|
1422
1643
|
} gps;
|
1423
1644
|
struct sbas_t {
|
1424
1645
|
SBAS_SpaceNode<FloatT> space_node;
|
1425
1646
|
SBAS_SolverOptions<FloatT> options;
|
1426
|
-
SBAS_SinglePositioning<FloatT> solver;
|
1647
|
+
HookableSolver<SBAS_SinglePositioning<FloatT>, GPS_Solver<FloatT> > solver;
|
1427
1648
|
sbas_t() : space_node(), options(), solver(space_node) {}
|
1428
1649
|
} sbas;
|
1429
1650
|
struct glonass_t {
|
1430
1651
|
GLONASS_SpaceNode<FloatT> space_node;
|
1431
1652
|
GLONASS_SolverOptions<FloatT> options;
|
1432
|
-
GLONASS_SinglePositioning<FloatT> solver;
|
1653
|
+
HookableSolver<GLONASS_SinglePositioning<FloatT>, GPS_Solver<FloatT> > solver;
|
1433
1654
|
glonass_t() : space_node(), options(), solver(space_node) {}
|
1434
1655
|
} glonass;
|
1435
1656
|
SWIG_Object hooks;
|
@@ -1465,6 +1686,9 @@ struct GPS_Solver
|
|
1465
1686
|
= sbas.solver.tropospheric_correction
|
1466
1687
|
= glonass.solver.tropospheric_correction
|
1467
1688
|
= tropospheric;
|
1689
|
+
gps.solver.hook = this;
|
1690
|
+
sbas.solver.hook = this;
|
1691
|
+
glonass.solver.hook = this;
|
1468
1692
|
}
|
1469
1693
|
GPS_SpaceNode<FloatT> &gps_space_node() {return gps.space_node;}
|
1470
1694
|
GPS_SolverOptions<FloatT> &gps_options() {return gps.options;}
|
@@ -1472,28 +1696,44 @@ struct GPS_Solver
|
|
1472
1696
|
SBAS_SolverOptions<FloatT> &sbas_options() {return sbas.options;}
|
1473
1697
|
GLONASS_SpaceNode<FloatT> &glonass_space_node() {return glonass.space_node;}
|
1474
1698
|
GLONASS_SolverOptions<FloatT> &glonass_options() {return glonass.options;}
|
1475
|
-
const base_t &
|
1699
|
+
const base_t &select(
|
1476
1700
|
const typename base_t::prn_t &prn) const {
|
1477
1701
|
if(prn > 0 && prn <= 32){return gps.solver;}
|
1478
1702
|
if(prn >= 120 && prn <= 158){return sbas.solver;}
|
1479
1703
|
if(prn > 192 && prn <= 202){return gps.solver;}
|
1480
1704
|
if(prn > 0x100 && prn <= (0x100 + 24)){return glonass.solver;}
|
1481
|
-
|
1482
|
-
// => relative_property() => select_solver()
|
1483
|
-
// For not supported satellite, call loop prevention is required.
|
1484
|
-
static const base_t dummy;
|
1485
|
-
return dummy;
|
1705
|
+
return *this;
|
1486
1706
|
}
|
1707
|
+
// proxy of virtual functions
|
1708
|
+
typename base_t::relative_property_t relative_property(
|
1709
|
+
const typename base_t::prn_t &prn,
|
1710
|
+
const typename base_t::measurement_t::mapped_type &measurement,
|
1711
|
+
const typename base_t::float_t &receiver_error,
|
1712
|
+
const typename base_t::gps_time_t &time_arrival,
|
1713
|
+
const typename base_t::pos_t &usr_pos,
|
1714
|
+
const typename base_t::xyz_t &usr_vel,
|
1715
|
+
const typename base_t::relative_property_t &orig) const;
|
1487
1716
|
virtual typename base_t::relative_property_t relative_property(
|
1488
1717
|
const typename base_t::prn_t &prn,
|
1489
1718
|
const typename base_t::measurement_t::mapped_type &measurement,
|
1490
1719
|
const typename base_t::float_t &receiver_error,
|
1491
1720
|
const typename base_t::gps_time_t &time_arrival,
|
1492
1721
|
const typename base_t::pos_t &usr_pos,
|
1493
|
-
const typename base_t::xyz_t &usr_vel) const
|
1722
|
+
const typename base_t::xyz_t &usr_vel) const {
|
1723
|
+
return relative_property(
|
1724
|
+
prn, measurement, receiver_error, time_arrival, usr_pos, usr_vel,
|
1725
|
+
super_t::relative_property(
|
1726
|
+
prn, measurement, receiver_error, time_arrival, usr_pos, usr_vel));
|
1727
|
+
}
|
1728
|
+
typename base_t::satellite_t select_satellite(
|
1729
|
+
const typename base_t::prn_t &prn,
|
1730
|
+
const typename base_t::gps_time_t &time,
|
1731
|
+
const typename base_t::satellite_t &orig) const;
|
1494
1732
|
virtual typename base_t::satellite_t select_satellite(
|
1495
1733
|
const typename base_t::prn_t &prn,
|
1496
|
-
const typename base_t::gps_time_t &time) const
|
1734
|
+
const typename base_t::gps_time_t &time) const {
|
1735
|
+
return select_satellite(prn, time, super_t::select_satellite(prn, time));
|
1736
|
+
}
|
1497
1737
|
virtual bool update_position_solution(
|
1498
1738
|
const typename base_t::geometric_matrices_t &geomat,
|
1499
1739
|
typename base_t::user_pvt_t &res) const;
|
@@ -765,9 +765,70 @@ __RINEX_CLK_TEXT__
|
|
765
765
|
[:alpha, :beta].each{|k|
|
766
766
|
puts "Iono #{k}: #{sn.iono_utc.send(k)}"
|
767
767
|
}
|
768
|
+
proc{|raw|
|
769
|
+
expect(raw.size).to eq(10)
|
770
|
+
puts "Raw(IONO): #{raw.collect{|v| "0x%08X" % [v]}.join(', ')}"
|
771
|
+
iono2 = GPS::Ionospheric_UTC_Parameters::parse(raw)
|
772
|
+
[:alpha, :beta].collect{|f|
|
773
|
+
sn.iono_utc.send(f).zip(iono2.send(f))
|
774
|
+
}.flatten(1).zip([-30, -27, -24, -24, 11, 14, 16, 16]).each{|(a, b), sf|
|
775
|
+
expect(a).to be_within((2 ** sf) * 2).of(b)
|
776
|
+
}
|
777
|
+
[
|
778
|
+
[:A1, 2 ** -50], [:A0, 2 ** -30],
|
779
|
+
].each{|k, sf|
|
780
|
+
#p [k, sf.to_f, sn.iono_utc.send(k) - iono2.send(k), sn.iono_utc.send(k), iono2.send(k)]
|
781
|
+
expect(sn.iono_utc.send(k)).to be_within((sf || 1).to_f * 2).of(iono2.send(k))
|
782
|
+
}
|
783
|
+
}.call(sn.iono_utc.dump(t_meas))
|
768
784
|
|
769
785
|
meas.each{|prn, k, v|
|
770
786
|
eph = sn.ephemeris(prn)
|
787
|
+
expect(eph.valid?(t_meas)).to eq(true)
|
788
|
+
sc2rad = 3.1415926535898
|
789
|
+
proc{|raw|
|
790
|
+
expect(raw.size).to eq(30)
|
791
|
+
eph2 = GPS::Ephemeris::new
|
792
|
+
raw.each_slice(10).with_index{|subframe, i|
|
793
|
+
puts "Raw(PRN:#{prn},SF:#{i+1}): #{subframe.collect{|v| "0x%08X" % [v]}.join(', ')}"
|
794
|
+
eph2.parse(subframe)
|
795
|
+
}
|
796
|
+
expect(eph.WN % 1024).to be(eph2.WN % 1024)
|
797
|
+
[
|
798
|
+
#:URA,
|
799
|
+
:SV_health, :iodc,
|
800
|
+
[:t_GD, 2 ** -31], [:t_oc, 2 ** 4], # SF1
|
801
|
+
[:a_f0, 2 ** -31], [:a_f1, 2 ** -43], [:a_f2, 2 ** -55], # SF1
|
802
|
+
:iode, # SF2
|
803
|
+
[:c_rs, 2 ** -5], [:delta_n, sc2rad * 2 ** -43], # SF2
|
804
|
+
[:M0, sc2rad * 2 ** -31], [:c_uc, 2 ** -29], [:e, 2 ** -33], # SF2
|
805
|
+
[:c_us, 2 ** -29], [:sqrt_A, 2 ** -19], [:t_oe, 2 ** 4], # SF2
|
806
|
+
:fit_interval, # SF2
|
807
|
+
[:c_ic, 2 ** -29], [:Omega0, sc2rad * 2 ** -31], [:c_is, 2 ** -29], # SF3
|
808
|
+
[:i0, sc2rad * 2 ** -31], [:c_rc, 2 ** -5], [:omega, sc2rad * 2 ** -31], # SF3
|
809
|
+
[:dot_Omega0, sc2rad * 2 ** -43], [:dot_i0, sc2rad * 2 ** -43], # SF3
|
810
|
+
].each{|k, sf|
|
811
|
+
#p [k, sf.to_f, eph.send(k) - eph2.send(k), eph.send(k), eph2.send(k)]
|
812
|
+
expect(eph.send(k)).to be_within((sf || 1).to_f * 2).of(eph2.send(k))
|
813
|
+
}
|
814
|
+
}.call(eph.dump(t_meas))
|
815
|
+
proc{|raw| # Almanac -> Ephemeris
|
816
|
+
expect(raw.size).to eq(10)
|
817
|
+
puts "Raw(PRN:#{prn},Almanac): #{raw.collect{|v| "0x%08X" % [v]}.join(', ')}"
|
818
|
+
eph2 = GPS::Ephemeris::new
|
819
|
+
eph2.parse_almanac(raw)
|
820
|
+
[
|
821
|
+
:SV_health,
|
822
|
+
[:t_oc, 2 ** 12], [:a_f0, 2 ** -20], [:a_f1, 2 ** -38],
|
823
|
+
[:M0, sc2rad * 2 ** -23], [:e, 2 ** -21],
|
824
|
+
[:sqrt_A, 2 ** -11], [:t_oe, 2 ** 12],
|
825
|
+
[:Omega0, sc2rad * 2 ** -23], [:i0, sc2rad * 2 ** -19],
|
826
|
+
[:omega, sc2rad * 2 ** -23], [:dot_Omega0, sc2rad * 2 ** -38],
|
827
|
+
].each{|k, sf|
|
828
|
+
#p [k, sf.to_f, eph.send(k) - eph2.send(k), eph.send(k), eph2.send(k)]
|
829
|
+
expect(eph.send(k)).to be_within((sf || 1).to_f * 2).of(eph2.send(k))
|
830
|
+
}
|
831
|
+
}.call(eph.dump_almanac(t_meas))
|
771
832
|
puts "XYZ(PRN:#{prn}): #{eph.constellation(t_meas)[0].to_a} (iodc: #{eph.iodc}, iode: #{eph.iode})"
|
772
833
|
}
|
773
834
|
|
@@ -863,6 +924,9 @@ __RINEX_CLK_TEXT__
|
|
863
924
|
}}.not_to raise_error
|
864
925
|
expect(solver.correction[:gps_ionospheric]).to include(:no_correction)
|
865
926
|
expect(solver.correction[:options][:f_10_7]).to eq(10)
|
927
|
+
expect(solver.gps_options.exclude_L2C?).to eq(true) #default
|
928
|
+
solver.gps_options.exclude_L2C = false
|
929
|
+
expect(solver.gps_options.exclude_L2C?).to eq(false)
|
866
930
|
sn.read(input[:rinex_nav])
|
867
931
|
t_meas = GPS::Time::new(1849, 172413)
|
868
932
|
sn.update_all_ephemeris(t_meas)
|
@@ -0,0 +1,84 @@
|
|
1
|
+
#ifndef __BIT_COUNTER__
|
2
|
+
#define __BIT_COUNTER__
|
3
|
+
|
4
|
+
#include <climits>
|
5
|
+
|
6
|
+
using namespace std;
|
7
|
+
|
8
|
+
/**
|
9
|
+
* Counting bits utility
|
10
|
+
*/
|
11
|
+
template <class T>
|
12
|
+
struct BitCounter {
|
13
|
+
|
14
|
+
static const int num_of_bits = CHAR_BIT * sizeof(T);
|
15
|
+
|
16
|
+
private:
|
17
|
+
template <int Interval>
|
18
|
+
static T make_mask(){
|
19
|
+
/*
|
20
|
+
* @see http://www.nminoru.jp/~nminoru/programming/bitcount.html
|
21
|
+
*
|
22
|
+
* 01010101010101010101010101010101 1 32
|
23
|
+
* 00110011001100110011001100110011 2 32
|
24
|
+
* 00001111000011110000111100001111 4 32
|
25
|
+
* 00000000111111110000000011111111 8 32
|
26
|
+
* 00000000000000001111111111111111 16 32
|
27
|
+
*/
|
28
|
+
|
29
|
+
T mask(1);
|
30
|
+
for(int i(1); i < Interval; i++){
|
31
|
+
mask <<= 1;
|
32
|
+
mask |= 0x01;
|
33
|
+
}
|
34
|
+
/*
|
35
|
+
// Equivalent to the following if T is built-in type
|
36
|
+
mask <<= Interval;
|
37
|
+
mask = (mask & (-mask)) - 1;
|
38
|
+
*/
|
39
|
+
|
40
|
+
T res(mask);
|
41
|
+
for(unsigned int i(1); i < num_of_bits / Interval / 2; i++){
|
42
|
+
res <<= Interval;
|
43
|
+
res <<= Interval;
|
44
|
+
res |= mask;
|
45
|
+
}
|
46
|
+
return res;
|
47
|
+
}
|
48
|
+
|
49
|
+
template <int Interval, class U = void>
|
50
|
+
struct count_loop {
|
51
|
+
static T run(T bits){
|
52
|
+
bits = count_loop<(Interval >> 1)>::run(bits);
|
53
|
+
static const T mask(make_mask<Interval>());
|
54
|
+
// bit shift and add operators are required at least
|
55
|
+
return (bits & mask) + ((bits >> Interval) & mask);
|
56
|
+
}
|
57
|
+
};
|
58
|
+
template <class U>
|
59
|
+
struct count_loop<1, U> {
|
60
|
+
static T run(T bits){
|
61
|
+
static const T mask(make_mask<1>());
|
62
|
+
return (bits & mask) + ((bits >> 1) & mask);
|
63
|
+
}
|
64
|
+
};
|
65
|
+
|
66
|
+
public:
|
67
|
+
/**
|
68
|
+
* Count bits in a value
|
69
|
+
* @param results
|
70
|
+
*/
|
71
|
+
static T count(const T &v) {
|
72
|
+
return count_loop<(num_of_bits >> 1)>::run(v);
|
73
|
+
}
|
74
|
+
|
75
|
+
/**
|
76
|
+
* Count rightmost zeros before the first one (Number of trailing zeros)
|
77
|
+
* @param bits results
|
78
|
+
*/
|
79
|
+
static T ntz(const T &v) {
|
80
|
+
return count((T)((~v) & (v - 1)));
|
81
|
+
}
|
82
|
+
};
|
83
|
+
|
84
|
+
#endif /* __BIT_COUNTER__ */
|