gps_pvt 0.8.0 → 0.8.1

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.
@@ -201,6 +201,11 @@ template <class InputT> \
201
201
  static u ## bits ## _t name(const InputT *buf){ \
202
202
  return parser_t::template bits2num<u ## bits ## _t, InputT>( \
203
203
  buf, offset_bits, length); \
204
+ } \
205
+ template <class BufferT> \
206
+ static void name ## _set(BufferT *dest, const u ## bits ## _t &src){ \
207
+ parser_t::template num2bits<u ## bits ## _t, BufferT>( \
208
+ dest, src, offset_bits, length); \
204
209
  }
205
210
  #define convert_s(bits, offset_bits, length, name) \
206
211
  template <class InputT> \
@@ -208,12 +213,22 @@ static s ## bits ## _t name(const InputT *buf){ \
208
213
  return (s ## bits ## _t)parser_t::template bits2num<u ## bits ## _t, InputT>( \
209
214
  buf, offset_bits) \
210
215
  >> (bits - length); \
216
+ } \
217
+ template <class BufferT> \
218
+ static void name ## _set(BufferT *dest, const s ## bits ## _t &src){ \
219
+ parser_t::template num2bits<u ## bits ## _t, BufferT>( \
220
+ dest, *(u ## bits ## _t *)(&src), offset_bits, length); \
211
221
  }
212
222
  #define convert_u_ch(bits, offset_bits, length, bits_per_ch, name) \
213
223
  template <class InputT> \
214
224
  static u ## bits ## _t name(const InputT *buf, const uint_t &ch){ \
215
225
  return parser_t::template bits2num<u ## bits ## _t>( \
216
226
  buf, offset_bits + (bits_per_ch * ch), length); \
227
+ } \
228
+ template <class BufferT> \
229
+ static void name ## _set(BufferT *dest, const uint_t &ch, const u ## bits ## _t &src){ \
230
+ parser_t::template num2bits<u ## bits ## _t, BufferT>( \
231
+ dest, src, offset_bits + (bits_per_ch * ch), length); \
217
232
  }
218
233
  #define convert_s_ch(bits, offset_bits, length, bits_per_ch, name) \
219
234
  template <class InputT> \
@@ -221,6 +236,11 @@ static s ## bits ## _t name(const InputT *buf, const uint_t &ch){ \
221
236
  return (s ## bits ## _t)parser_t::template bits2num<u ## bits ## _t>( \
222
237
  buf, offset_bits + (bits_per_ch * ch)) \
223
238
  >> (bits - length); \
239
+ } \
240
+ template <class BufferT> \
241
+ static void name ## _set(BufferT *dest, const uint_t &ch, const s ## bits ## _t &src){ \
242
+ parser_t::template num2bits<u ## bits ## _t, BufferT>( \
243
+ dest, *(u ## bits ## _t *)(&src), offset_bits + (bits_per_ch * ch), length); \
224
244
  }
225
245
 
226
246
  /**
@@ -254,8 +274,55 @@ static s ## bits ## _t name(const InputT *buf, const uint_t &ch){ \
254
274
  }
255
275
 
256
276
  convert_u(8, 0, 8, preamble);
277
+ template <class BufferT>
278
+ static void preamble_set2(BufferT *dest, const unsigned int &index = 0){
279
+ static const u8_t preamble_list[] = {0x53, 0x9A, 0xC6};
280
+ preamble_set(dest, preamble_list[index % 3]);
281
+ }
257
282
  convert_u(8, 8, 6, message_type);
258
283
 
284
+ convert_u(32, 226, 24, parity);
285
+ /**
286
+ * update parity bits based on current buffer
287
+ * @param buf buffer
288
+ * @see SBAS MOPS (DO-229) A.4.3.3
289
+ */
290
+ template <class BufferT>
291
+ static void parity_set(BufferT *dst){
292
+ // CRC-24Q
293
+ static const u32_t poly(0x1864CFBu);
294
+ static const struct tbl_t {
295
+ u32_t fw[0x100];
296
+ tbl_t() {
297
+ for(int i(0); i < 0x100; ++i){
298
+ fw[i] = i << 16;
299
+ for(int j(0); j < 8; ++j){
300
+ fw[i] <<= 1;
301
+ if(fw[i] & 0x1000000u){fw[i] ^= poly;}
302
+ }
303
+ }
304
+ }
305
+ } tbl;
306
+ u32_t crc(0);
307
+ u8_t buf;
308
+ int bit_idx(0);
309
+ for(int bytes(226 / 8); bytes > 0; bytes--, bit_idx += 8){
310
+ buf = parser_t::template bits2num<u8_t, BufferT>(dst, bit_idx, 8);
311
+ crc = (((crc << 8) & 0xFFFF00) | buf) ^ tbl.fw[(crc >> 16) & 0xFF];
312
+ }
313
+ buf = parser_t::template bits2num<u8_t, BufferT>(dst, bit_idx, 8);
314
+ for(unsigned char mask(0x80); bit_idx < 226; bit_idx++, mask >>= 1){
315
+ crc <<= 1;
316
+ if(buf & mask){crc |= 1;}
317
+ if(crc & 0x1000000u){crc ^= poly;}
318
+ }
319
+ for(int i(0); i < 3; i++){ // forward operation for padding
320
+ unsigned int cache(tbl.fw[(crc >> 16) & 0xFF]);
321
+ crc = (((crc << 8) ^ cache) & 0xFFFF00) | (cache & 0xFF);
322
+ }
323
+ parity_set(dst, crc);
324
+ }
325
+
259
326
  struct Type1 { ///< @see Fig. A-6 PRN_MASK
260
327
  struct mask_t {
261
328
  u8_t valid;
@@ -1838,6 +1905,18 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
1838
1905
  };
1839
1906
  return res;
1840
1907
  }
1908
+ template <class BufferT>
1909
+ void dump(BufferT *dst){
1910
+ typedef typename DataBlock::Type9 msg_t;
1911
+ #define dump_item(name) msg_t:: name ## _set(dst, name)
1912
+ DataBlock::message_type_set(dst, 9);
1913
+ msg_t::t0_set(dst, t_0); msg_t::ura_set(dst, URA);
1914
+ dump_item(x); dump_item(y); dump_item(z);
1915
+ dump_item(dx); dump_item(dy); dump_item(dz);
1916
+ dump_item(ddx); dump_item(ddy); dump_item(ddz);
1917
+ dump_item(a_Gf0); dump_item(a_Gf1);
1918
+ #undef dump_item
1919
+ }
1841
1920
 
1842
1921
  enum {
1843
1922
  SF_t_0,
@@ -1873,13 +1952,13 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
1873
1952
 
1874
1953
  raw_t &operator=(const Ephemeris &eph){
1875
1954
  #define CONVERT3(TARGET_DST, TARGET_SRC, TARGET_SF) \
1876
- {TARGET_DST = (s32_t)((TARGET_SRC + 0.5 * sf[SF_ ## TARGET_SF]) / sf[SF_ ## TARGET_SF]);}
1877
- #define CONVERT2(TARGET, TARGET_SF) CONVERT3(eph.TARGET, eph.TARGET, TARGET_SF)
1955
+ {TARGET_DST = std::floor((TARGET_SRC / sf[SF_ ## TARGET_SF]) + 0.5);}
1956
+ #define CONVERT2(TARGET, TARGET_SF) CONVERT3(TARGET, eph.TARGET, TARGET_SF)
1878
1957
  #define CONVERT(TARGET) CONVERT2(TARGET, TARGET)
1879
1958
  svid = eph.svid;
1880
1959
 
1881
1960
  URA = URA_index(eph.URA);
1882
- CONVERT3(t_0, std::fmod(t_0, gps_time_t::seconds_day), t_0);
1961
+ CONVERT3(t_0, std::fmod(eph.t_0, gps_time_t::seconds_day), t_0);
1883
1962
  CONVERT2(x, xy); CONVERT2(y, xy); CONVERT(z);
1884
1963
  CONVERT2(dx, dxy); CONVERT2(dy, dxy); CONVERT(dz);
1885
1964
  CONVERT2(ddx, ddxy); CONVERT2(ddy, ddxy); CONVERT(ddz);
@@ -1992,7 +2071,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET_SF]){break;}
1992
2071
  float_t dx, dy, dz; ///< ECEF velocity (m/s)
1993
2072
  float_t t_0; ///< Time of applicability (s)
1994
2073
 
1995
- ///< Up-cast to ephemeris
2074
+ ///< Upgrade to ephemeris
1996
2075
  operator Ephemeris() const {
1997
2076
  Ephemeris converted = {
1998
2077
  prn, // Satellite number
@@ -2006,6 +2085,14 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET_SF]){break;}
2006
2085
  };
2007
2086
  return converted;
2008
2087
  }
2088
+ ///< Downgrade from ephemeris
2089
+ Almanac &operator=(const Ephemeris &eph) {
2090
+ prn = eph.svid;
2091
+ t_0 = eph.t_0;
2092
+ x = eph.x; y = eph.y; z = eph.z;
2093
+ dx = eph.dx; dy = eph.dy; dz = eph.dz;
2094
+ return *this;
2095
+ }
2009
2096
 
2010
2097
  struct raw_t {
2011
2098
  u8_t data_id;
@@ -2028,6 +2115,21 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET_SF]){break;}
2028
2115
  };
2029
2116
  return res;
2030
2117
  }
2118
+ template <class BufferT>
2119
+ void dump(BufferT *dst, const uint_t &ch = 0){
2120
+ typedef typename DataBlock::Type17 msg_t;
2121
+ #define dump_item(name) msg_t:: name ## _set(dst, ch, name)
2122
+ if(ch == 0){
2123
+ DataBlock::message_type_set(dst, 17);
2124
+ msg_t::t0_set(dst, t_0);
2125
+ }
2126
+ msg_t::id_set(dst, ch, data_id);
2127
+ dump_item(prn);
2128
+ msg_t::health_status_set(dst, ch, SV_health);
2129
+ dump_item(x); dump_item(y); dump_item(z);
2130
+ dump_item(dx); dump_item(dy); dump_item(dz);
2131
+ #undef dump_item
2132
+ }
2031
2133
 
2032
2134
  enum {
2033
2135
  SF_xy, SF_z,
@@ -43,25 +43,31 @@
43
43
 
44
44
  #include <vector>
45
45
 
46
- template <int denom, int pow2 = 1, int log2 = 0, int rem2 = denom % pow2>
47
- struct const_div_t : public std::div_t {
48
- const_div_t(const int &num) : std::div_t(std::div(num, denom)) {}
49
- };
50
- template <int denom, int pow2, int log2>
51
- struct const_div_t<denom, pow2, log2, 0> : public const_div_t<denom, (pow2 << 1), log2 + 1> {
52
- const_div_t(const int &num) : const_div_t<denom, (pow2 << 1), log2 + 1>(num) {}
53
- };
54
- template <int denom, int log2>
55
- struct const_div_t<denom, denom, log2, 0> {
56
- int quot, rem;
57
- const_div_t(const int &num) : quot(num >> log2), rem(num & (denom - 1)) {}
58
- };
46
+ #include "util/bit_counter.h"
59
47
 
60
48
  template <int MAX_SIZE, class ContainerT = unsigned char>
61
49
  struct BitArray {
50
+
51
+ template <int denom, int pow2 = 1, int log2 = 0, int rem2 = denom % pow2>
52
+ struct const_div_t : public std::div_t {
53
+ const_div_t(const int &num) : std::div_t(std::div(num, denom)) {}
54
+ };
55
+ template <int denom, int pow2, int log2>
56
+ struct const_div_t<denom, pow2, log2, 0> : public const_div_t<denom, (pow2 << 1), log2 + 1> {
57
+ const_div_t(const int &num) : const_div_t<denom, (pow2 << 1), log2 + 1>(num) {}
58
+ };
59
+ template <int denom, int log2>
60
+ struct const_div_t<denom, denom, log2, 0> {
61
+ int quot, rem;
62
+ const_div_t(const int &num) : quot(num >> log2), rem(num & (denom - 1)) {}
63
+ };
64
+
62
65
  static const int bits_per_addr = (int)sizeof(ContainerT) * CHAR_BIT;
63
66
  static const int max_size = MAX_SIZE;
67
+ static const const_div_t<bits_per_addr> prop;
64
68
  ContainerT buf[(MAX_SIZE + bits_per_addr - 1) / bits_per_addr];
69
+ static const ContainerT mask_rem;
70
+
65
71
  void set(const bool &new_bit = false) {
66
72
  std::memset(buf, (new_bit ? (~0) : 0), sizeof(buf));
67
73
  }
@@ -125,12 +131,35 @@ struct BitArray {
125
131
  return (unsigned int)res;
126
132
  }
127
133
  }
134
+ int ones() const {
135
+ int res(0);
136
+ for(int i(0); i < prop.quot; ++i){
137
+ res += BitCounter<ContainerT>::count(buf[i]);
138
+ }
139
+ if(prop.rem){
140
+ res += BitCounter<ContainerT>::count(
141
+ buf[(sizeof(buf) / sizeof(buf[0])) - 1] & mask_rem);
142
+ }
143
+ return res;
144
+ }
145
+ int least_index_one() const {
146
+ int res(0);
147
+ for(int i(0); prop.quot; ++i){
148
+ int ntz(BitCounter<ContainerT>::ntz(buf[i]));
149
+ res += ntz;
150
+ if(ntz < bits_per_addr){return res;}
151
+ }
152
+ if(prop.rem){
153
+ res += BitCounter<ContainerT>::ntz(
154
+ buf[(sizeof(buf) / sizeof(buf[0])) - 1] & mask_rem);
155
+ }
156
+ return res;
157
+ }
128
158
  std::vector<int> indices_one(const int &offset = 0) const {
129
159
  std::vector<int> res;
130
160
  int idx(offset);
131
- static const const_div_t<bits_per_addr> qr(MAX_SIZE);
132
- int rem(qr.rem), i(0);
133
- for(; i < qr.quot; ++i, idx += bits_per_addr){
161
+ int rem(prop.rem), i(0);
162
+ for(; i < prop.quot; ++i, idx += bits_per_addr){
134
163
  int idx2(idx);
135
164
  for(ContainerT temp(buf[i]); temp > 0; temp >>= 1, ++idx2){
136
165
  if(temp & 0x1){res.push_back(idx2);}
@@ -143,4 +172,12 @@ struct BitArray {
143
172
  }
144
173
  };
145
174
 
175
+ template <int MAX_SIZE, class ContainerT>
176
+ const typename BitArray<MAX_SIZE, ContainerT>::template const_div_t<BitArray<MAX_SIZE, ContainerT>::bits_per_addr>
177
+ BitArray<MAX_SIZE, ContainerT>::prop
178
+ = typename BitArray<MAX_SIZE, ContainerT>::template const_div_t<BitArray<MAX_SIZE, ContainerT>::bits_per_addr>(MAX_SIZE);
179
+
180
+ template <int MAX_SIZE, class ContainerT>
181
+ const ContainerT BitArray<MAX_SIZE, ContainerT>::mask_rem = ((ContainerT)0x1 << prop.rem) - 1;
182
+
146
183
  #endif /* __BIT_ARRAY_H__ */
@@ -273,6 +273,25 @@ struct GPS_Ionospheric_UTC_Parameters : public GPS_SpaceNode<FloatT>::Ionospheri
273
273
  (typename GPS_SpaceNode<FloatT>::Ionospheric_UTC_Parameters &)res = raw;
274
274
  return res;
275
275
  }
276
+ %typemap(in,numinputs=0) unsigned int buf_brdc[ANY] (unsigned int temp[$1_dim0] = {0}) "$1 = temp;"
277
+ %typemap(argout) unsigned int buf_brdc[ANY] {
278
+ for(int i(0); i < $1_dim0; ++i){
279
+ %append_output(SWIG_From(unsigned int)(($1)[i]));
280
+ }
281
+ }
282
+ /**
283
+ * Return broadcasted raw data related to ionospheric and UTC parameters.
284
+ * @param buf_brdc pointer to store raw data of subframe 4 page 18.
285
+ * Each 30bit length word (MSB 2 bits are padding) is stored in each successive address of the pointer.
286
+ * @param t GPS time at broadcasting
287
+ */
288
+ void dump(unsigned int buf_brdc[10], const GPS_Time<FloatT> &t){
289
+ typedef typename GPS_SpaceNode<FloatT>
290
+ ::BroadcastedMessage<unsigned int, 30> dump_t;
291
+ dump_t::how_set(buf_brdc, t);
292
+ GPS_SpaceNode<FloatT>::Ionospheric_UTC_Parameters::raw_t raw;
293
+ (raw = *self).dump<2, 0>(buf_brdc);
294
+ }
276
295
  }
277
296
 
278
297
  %inline %{
@@ -287,6 +306,11 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
287
306
  || (this->iode != this->iode_subframe3)
288
307
  || ((this->iodc & 0xFF) != this->iode));
289
308
  }
309
+ bool is_valid(const GPS_Time<FloatT> &t) const {
310
+ return is_consistent() && GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::is_valid(t);
311
+ }
312
+ GPS_Time<FloatT> t_clock() const {return GPS_Time<FloatT>(this->WN, this->t_oc);}
313
+ GPS_Time<FloatT> t_ephemeris() const {return GPS_Time<FloatT>(this->WN, this->t_oe);}
290
314
  GPS_Ephemeris() : GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris() {
291
315
  invalidate();
292
316
  }
@@ -303,7 +327,7 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
303
327
  MAKE_ACCESSOR(svid, unsigned int);
304
328
 
305
329
  MAKE_ACCESSOR(WN, unsigned int);
306
- MAKE_ACCESSOR(URA, int);
330
+ MAKE_ACCESSOR(URA, FloatT);
307
331
  MAKE_ACCESSOR(SV_health, unsigned int);
308
332
  MAKE_ACCESSOR(iodc, int);
309
333
  MAKE_ACCESSOR(t_GD, FloatT);
@@ -384,6 +408,83 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
384
408
  break;
385
409
  }
386
410
  }
411
+ %apply unsigned int buf_brdc[ANY] {
412
+ unsigned int buf_sf1[10], unsigned int buf_sf2[10], unsigned int buf_sf3[10]};
413
+ /**
414
+ * Return broadcasted raw data of ephemeris data.
415
+ * @param buf_sf1 pointer to store raw data of subframe 1.
416
+ * Each 30bit length word (MSB 2 bits are padding) is stored in each successive address of the pointer.
417
+ * @param buf_sf2 pointer to store raw data of subframe 2. Its structue is same as sf1.
418
+ * @param buf_sf3 pointer to store raw data of subframe 3. Its structue is same as sf1.
419
+ * @param t GPS time at broadcasting
420
+ */
421
+ void dump(
422
+ unsigned int buf_sf1[10], unsigned int buf_sf2[10], unsigned int buf_sf3[10],
423
+ const GPS_Time<FloatT> &t){
424
+ typedef typename GPS_SpaceNode<FloatT>
425
+ ::BroadcastedMessage<unsigned int, 30> dump_t;
426
+ GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t raw;
427
+ raw = *self;
428
+ unsigned int *buf[10] = {buf_sf1, buf_sf2, buf_sf3};
429
+ for(int i(0); i < 3; ++i){
430
+ dump_t::how_set(buf[i], t);
431
+ raw.dump<2, 0>(buf[i], i + 1);
432
+ }
433
+ }
434
+ int parse_almanac(const unsigned int buf[10]){
435
+ typedef GPS_SpaceNode<FloatT>::BroadcastedMessage<unsigned int, 30> parse_t;
436
+ switch(parse_t::subframe_id(buf)){
437
+ case 4:
438
+ case 5:
439
+ break;
440
+ default: return -1;
441
+ }
442
+ typedef GPS_SpaceNode<FloatT>::SatelliteProperties::Almanac almanac_t;
443
+ almanac_t::raw_t raw;
444
+ switch(parse_t::data_id(buf)){
445
+ case 1: // GPS
446
+ raw.update<2, 0>(buf);
447
+ if((raw.svid < 1) || (raw.svid > 32)){return -1;}
448
+ break;
449
+ case 3: // QZSS
450
+ reinterpret_cast<QZSS_SpaceNode<FloatT>::SatelliteProperties::Almanac::raw_t &>(raw)
451
+ .update<2, 0>(buf);
452
+ if((raw.svid < 193) || (raw.svid > 202)){return -1;}
453
+ break;
454
+ default:
455
+ return -1;
456
+ }
457
+ almanac_t almanac;
458
+ *self = (GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris)(almanac = raw);
459
+ return self->svid;
460
+ }
461
+ /**
462
+ * Return broadcasted raw data of almanac data.
463
+ * @param buf_brdc pointer to store raw data of subframe 4 or 5.
464
+ * Each 30bit (MSB 2 bits are padding) length word is stored in each successive address of the pointer.
465
+ * @param t GPS time at broadcasting
466
+ * @param qzss_subframe if 4 or 5, this ephemeris is treated as QZSS, otherwise GPS (default).
467
+ */
468
+ void dump_almanac(
469
+ unsigned int buf_brdc[10], const GPS_Time<FloatT> &t,
470
+ const unsigned int &qzss_subframe = 0){
471
+ typedef typename GPS_SpaceNode<FloatT>
472
+ ::BroadcastedMessage<unsigned int, 30> dump_t;
473
+ dump_t::how_set(buf_brdc, t);
474
+ typedef GPS_SpaceNode<FloatT>::SatelliteProperties::Almanac almanac_t;
475
+ almanac_t almanac;
476
+ almanac_t::raw_t raw;
477
+ raw = (almanac = *self);
478
+ switch(qzss_subframe){
479
+ case 4:
480
+ case 5:
481
+ reinterpret_cast<QZSS_SpaceNode<FloatT>::SatelliteProperties::Almanac::raw_t &>(raw)
482
+ .dump<2, 0>(buf_brdc, qzss_subframe);
483
+ break;
484
+ default:
485
+ raw.dump<2, 0>(buf_brdc);
486
+ }
487
+ }
387
488
  %typemap(out) constellation_res_t {
388
489
  %append_output(SWIG_NewPointerObj((new System_XYZ<FloatT, WGS84>($1.position)),
389
490
  $descriptor(System_XYZ<FloatT, WGS84> *), SWIG_POINTER_OWN));
@@ -402,6 +503,7 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
402
503
  }
403
504
  #if defined(SWIGRUBY)
404
505
  %rename("consistent?") is_consistent;
506
+ %rename("valid?") is_valid;
405
507
  #endif
406
508
  }
407
509
 
@@ -444,6 +546,10 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
444
546
  SBAS_Ephemeris() : SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris() {}
445
547
  SBAS_Ephemeris(const typename SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris &eph)
446
548
  : SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris(eph) {}
549
+ bool is_valid(const GPS_Time<FloatT> &t) const {
550
+ return SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::is_valid(t);
551
+ }
552
+ GPS_Time<FloatT> t_applicable() const {return GPS_Time<FloatT>(this->WN, this->t_0);}
447
553
  };
448
554
  %}
449
555
  %extend SBAS_Ephemeris {
@@ -457,6 +563,18 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
457
563
  MAKE_ACCESSOR(ddx, FloatT); MAKE_ACCESSOR(ddy, FloatT); MAKE_ACCESSOR(ddz, FloatT);
458
564
  MAKE_ACCESSOR(a_Gf0, FloatT);
459
565
  MAKE_ACCESSOR(a_Gf1, FloatT);
566
+ /**
567
+ * Return broadcasted raw data of SBAS ephemeris
568
+ * @param buf_brdc pointer to store raw data of Type 9 message.
569
+ * 250 of 256 bits are effective, LSB 6 bits of last word are padding.
570
+ * @param preamble_idx index of preamble, 0(0x53), 1(0x9A), or 2(0xC6)
571
+ */
572
+ void dump(unsigned int buf_brdc[8], const unsigned int &preamble_idx = 0){
573
+ SBAS_SpaceNode<FloatT>::DataBlock::preamble_set2(buf_brdc, preamble_idx);
574
+ SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t raw;
575
+ (raw = *self).dump(buf_brdc);
576
+ SBAS_SpaceNode<FloatT>::DataBlock::parity_set(buf_brdc);
577
+ }
460
578
  typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
461
579
  const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0,
462
580
  const bool &with_velocity = true) const {
@@ -466,6 +584,9 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
466
584
  pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot(t_tx)};
467
585
  return res;
468
586
  }
587
+ #if defined(SWIGRUBY)
588
+ %rename("valid?") is_valid;
589
+ #endif
469
590
  }
470
591
 
471
592
  %extend SBAS_SpaceNode {
@@ -519,6 +640,9 @@ struct GLONASS_Ephemeris
519
640
  bool is_consistent() const {
520
641
  return has_string == 0x1F;
521
642
  }
643
+ bool is_valid(const GPS_Time<FloatT> &t) const {
644
+ return is_consistent() && eph_t::is_valid(t);
645
+ }
522
646
  GLONASS_Ephemeris() : eph_t() {
523
647
  invalidate();
524
648
  }
@@ -607,6 +731,31 @@ struct GLONASS_Ephemeris
607
731
  self->has_string = has_string;
608
732
  return updated;
609
733
  }
734
+ %apply unsigned int buf_brdc[ANY] {
735
+ unsigned int buf_str1[3], unsigned int buf_str2[3], unsigned int buf_str3[3],
736
+ unsigned int buf_str4[3], unsigned int buf_str5[3]};
737
+ /**
738
+ * Return broadcasted raw data of GLONASS ephemeris data.
739
+ * @param buf_str1 pointer to store raw data of string 1.
740
+ * 85bit length data (LSB 11 bits are padding) is stored in successive address of the pointer.
741
+ * @param buf_str2 pointer to store raw data of string 2. Its structue is same as str1.
742
+ * @param buf_str3 pointer to store raw data of string 3. Its structue is same as str1.
743
+ * @param buf_str4 pointer to store raw data of string 4. Its structue is same as str1.
744
+ * @param buf_str5 pointer to store raw data of string 5. Its structue is same as str1.
745
+ * @param t GPS time at broadcasting
746
+ */
747
+ void dump(
748
+ unsigned int buf_str1[3], unsigned int buf_str2[3], unsigned int buf_str3[3],
749
+ unsigned int buf_str4[3], unsigned int buf_str5[3],
750
+ const GPS_Time<FloatT> &t){
751
+ typename GLONASS_Ephemeris<FloatT>::eph_t::raw_t raw;
752
+ raw = *self;
753
+ unsigned int *buf[4] = {buf_str1, buf_str2, buf_str3, buf_str4};
754
+ for(int i(0); i < 4; ++i){
755
+ raw.GLONASS_Ephemeris<FloatT>::Ephemeris::raw_t::dump<0, 0>(buf[i], i + 1);
756
+ }
757
+ raw.GLONASS_Ephemeris<FloatT>::TimeProperties::raw_t::dump<0, 0>(buf_str5);
758
+ }
610
759
  typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
611
760
  const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0) const {
612
761
  typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t pv(
@@ -617,6 +766,7 @@ struct GLONASS_Ephemeris
617
766
  }
618
767
  #if defined(SWIGRUBY)
619
768
  %rename("consistent?") is_consistent;
769
+ %rename("valid?") is_valid;
620
770
  %rename("in_range?") is_in_range;
621
771
  #endif
622
772
  bool is_in_range(const GPS_Time<FloatT> &t) const {
@@ -1081,6 +1231,43 @@ struct GPS_RangeCorrector
1081
1231
  };
1082
1232
  }
1083
1233
 
1234
+ %{
1235
+ template <class BaseT, class HookT>
1236
+ struct HookableSolver : public BaseT {
1237
+ typedef BaseT base_t;
1238
+ HookT *hook;
1239
+ HookableSolver(const BaseT &base) : BaseT(base), hook(NULL) {}
1240
+ virtual typename base_t::relative_property_t relative_property(
1241
+ const typename base_t::prn_t &prn,
1242
+ const typename base_t::measurement_t::mapped_type &measurement,
1243
+ const typename base_t::float_t &receiver_error,
1244
+ const typename base_t::gps_time_t &time_arrival,
1245
+ const typename base_t::pos_t &usr_pos,
1246
+ const typename base_t::xyz_t &usr_vel) const {
1247
+ typename base_t::relative_property_t res(
1248
+ base_t::relative_property(
1249
+ prn, measurement, receiver_error, time_arrival,
1250
+ usr_pos, usr_vel));
1251
+ if(hook){
1252
+ res = hook->relative_property(
1253
+ prn, measurement, receiver_error, time_arrival,
1254
+ usr_pos, usr_vel,
1255
+ res);
1256
+ }
1257
+ return res;
1258
+ }
1259
+ virtual typename base_t::satellite_t select_satellite(
1260
+ const typename base_t::prn_t &prn,
1261
+ const typename base_t::gps_time_t &time) const {
1262
+ typename base_t::satellite_t res(base_t::select_satellite(prn, time));
1263
+ if(hook){
1264
+ res = hook->select_satellite(prn, time, res);
1265
+ }
1266
+ return res;
1267
+ }
1268
+ };
1269
+ %}
1270
+
1084
1271
  %extend GPS_Solver {
1085
1272
  %ignore super_t;
1086
1273
  %ignore base_t;
@@ -1090,7 +1277,7 @@ struct GPS_RangeCorrector
1090
1277
  %ignore sbas;
1091
1278
  %ignore glonass_t;
1092
1279
  %ignore glonass;
1093
- %ignore select_solver;
1280
+ %ignore select;
1094
1281
  %ignore relative_property;
1095
1282
  %ignore select_satellite;
1096
1283
  %ignore update_position_solution;
@@ -1110,14 +1297,12 @@ struct GPS_RangeCorrector
1110
1297
  const GPS_Solver<FloatT>::base_t::float_t &receiver_error,
1111
1298
  const GPS_Solver<FloatT>::base_t::gps_time_t &time_arrival,
1112
1299
  const GPS_Solver<FloatT>::base_t::pos_t &usr_pos,
1113
- const GPS_Solver<FloatT>::base_t::xyz_t &usr_vel) const {
1300
+ const GPS_Solver<FloatT>::base_t::xyz_t &usr_vel,
1301
+ const GPS_Solver<FloatT>::base_t::relative_property_t &res_orig) const {
1114
1302
  union {
1115
1303
  base_t::relative_property_t prop;
1116
1304
  FloatT values[7];
1117
- } res = {
1118
- select_solver(prn).relative_property(
1119
- prn, measurement, receiver_error, time_arrival,
1120
- usr_pos, usr_vel)};
1305
+ } res = {res_orig};
1121
1306
  #ifdef SWIGRUBY
1122
1307
  do{
1123
1308
  static const VALUE key(ID2SYM(rb_intern("relative_property")));
@@ -1197,9 +1382,9 @@ struct GPS_RangeCorrector
1197
1382
  template <>
1198
1383
  GPS_Solver<FloatT>::base_t::satellite_t GPS_Solver<FloatT>::select_satellite(
1199
1384
  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));
1385
+ const GPS_Solver<FloatT>::base_t::gps_time_t &time,
1386
+ const GPS_Solver<FloatT>::base_t::satellite_t &res_orig) const {
1387
+ GPS_Solver<FloatT>::base_t::satellite_t res(res_orig);
1203
1388
  #ifdef SWIGRUBY
1204
1389
  if(!res.is_available()){
1205
1390
  static const VALUE key(ID2SYM(rb_intern("relative_property")));
@@ -1396,7 +1581,7 @@ struct GPS_Solver
1396
1581
  struct gps_t {
1397
1582
  GPS_SpaceNode<FloatT> space_node;
1398
1583
  GPS_SolverOptions<FloatT> options;
1399
- GPS_SinglePositioning<FloatT> solver;
1584
+ HookableSolver<GPS_SinglePositioning<FloatT>, GPS_Solver<FloatT> > solver;
1400
1585
  struct ephemeris_proxy_t {
1401
1586
  struct item_t {
1402
1587
  const void *impl;
@@ -1418,19 +1603,19 @@ struct GPS_Solver
1418
1603
  solver.satellites.impl_select = forward;
1419
1604
  }
1420
1605
  } ephemeris_proxy;
1421
- gps_t() : space_node(), options(), solver(space_node), ephemeris_proxy(solver) {}
1606
+ gps_t() : space_node(), options(), solver(GPS_SinglePositioning<FloatT>(space_node)), ephemeris_proxy(solver) {}
1422
1607
  } gps;
1423
1608
  struct sbas_t {
1424
1609
  SBAS_SpaceNode<FloatT> space_node;
1425
1610
  SBAS_SolverOptions<FloatT> options;
1426
- SBAS_SinglePositioning<FloatT> solver;
1427
- sbas_t() : space_node(), options(), solver(space_node) {}
1611
+ HookableSolver<SBAS_SinglePositioning<FloatT>, GPS_Solver<FloatT> > solver;
1612
+ sbas_t() : space_node(), options(), solver(SBAS_SinglePositioning<FloatT>(space_node)) {}
1428
1613
  } sbas;
1429
1614
  struct glonass_t {
1430
1615
  GLONASS_SpaceNode<FloatT> space_node;
1431
1616
  GLONASS_SolverOptions<FloatT> options;
1432
- GLONASS_SinglePositioning<FloatT> solver;
1433
- glonass_t() : space_node(), options(), solver(space_node) {}
1617
+ HookableSolver<GLONASS_SinglePositioning<FloatT>, GPS_Solver<FloatT> > solver;
1618
+ glonass_t() : space_node(), options(), solver(GLONASS_SinglePositioning<FloatT>(space_node)) {}
1434
1619
  } glonass;
1435
1620
  SWIG_Object hooks;
1436
1621
  typedef std::vector<GPS_RangeCorrector<FloatT> > user_correctors_t;
@@ -1465,6 +1650,9 @@ struct GPS_Solver
1465
1650
  = sbas.solver.tropospheric_correction
1466
1651
  = glonass.solver.tropospheric_correction
1467
1652
  = tropospheric;
1653
+ gps.solver.hook = this;
1654
+ sbas.solver.hook = this;
1655
+ glonass.solver.hook = this;
1468
1656
  }
1469
1657
  GPS_SpaceNode<FloatT> &gps_space_node() {return gps.space_node;}
1470
1658
  GPS_SolverOptions<FloatT> &gps_options() {return gps.options;}
@@ -1472,28 +1660,44 @@ struct GPS_Solver
1472
1660
  SBAS_SolverOptions<FloatT> &sbas_options() {return sbas.options;}
1473
1661
  GLONASS_SpaceNode<FloatT> &glonass_space_node() {return glonass.space_node;}
1474
1662
  GLONASS_SolverOptions<FloatT> &glonass_options() {return glonass.options;}
1475
- const base_t &select_solver(
1663
+ const base_t &select(
1476
1664
  const typename base_t::prn_t &prn) const {
1477
1665
  if(prn > 0 && prn <= 32){return gps.solver;}
1478
1666
  if(prn >= 120 && prn <= 158){return sbas.solver;}
1479
1667
  if(prn > 192 && prn <= 202){return gps.solver;}
1480
1668
  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;
1669
+ return *this;
1486
1670
  }
1671
+ // proxy of virtual functions
1672
+ typename base_t::relative_property_t relative_property(
1673
+ const typename base_t::prn_t &prn,
1674
+ const typename base_t::measurement_t::mapped_type &measurement,
1675
+ const typename base_t::float_t &receiver_error,
1676
+ const typename base_t::gps_time_t &time_arrival,
1677
+ const typename base_t::pos_t &usr_pos,
1678
+ const typename base_t::xyz_t &usr_vel,
1679
+ const typename base_t::relative_property_t &orig) const;
1487
1680
  virtual typename base_t::relative_property_t relative_property(
1488
1681
  const typename base_t::prn_t &prn,
1489
1682
  const typename base_t::measurement_t::mapped_type &measurement,
1490
1683
  const typename base_t::float_t &receiver_error,
1491
1684
  const typename base_t::gps_time_t &time_arrival,
1492
1685
  const typename base_t::pos_t &usr_pos,
1493
- const typename base_t::xyz_t &usr_vel) const;
1686
+ const typename base_t::xyz_t &usr_vel) const {
1687
+ return relative_property(
1688
+ prn, measurement, receiver_error, time_arrival, usr_pos, usr_vel,
1689
+ super_t::relative_property(
1690
+ prn, measurement, receiver_error, time_arrival, usr_pos, usr_vel));
1691
+ }
1692
+ typename base_t::satellite_t select_satellite(
1693
+ const typename base_t::prn_t &prn,
1694
+ const typename base_t::gps_time_t &time,
1695
+ const typename base_t::satellite_t &orig) const;
1494
1696
  virtual typename base_t::satellite_t select_satellite(
1495
1697
  const typename base_t::prn_t &prn,
1496
- const typename base_t::gps_time_t &time) const;
1698
+ const typename base_t::gps_time_t &time) const {
1699
+ return select_satellite(prn, time, super_t::select_satellite(prn, time));
1700
+ }
1497
1701
  virtual bool update_position_solution(
1498
1702
  const typename base_t::geometric_matrices_t &geomat,
1499
1703
  typename base_t::user_pvt_t &res) const;