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.
@@ -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, int);
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 cast_base;
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 select_solver;
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) const {
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) const {
1201
- GPS_Solver<FloatT>::base_t::satellite_t res(
1202
- select_solver(prn).select_satellite(prn, time));
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
- GPS_SinglePositioning<FloatT> solver;
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 &select_solver(
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
- // call order: base_t::solve => this returned by select()
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__ */