gps_pvt 0.5.1 → 0.6.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.
@@ -52,6 +52,7 @@
52
52
  #include <cstring>
53
53
  #include <limits>
54
54
 
55
+ #include "util/text_helper.h"
55
56
  #include "GPS.h"
56
57
  #include "SBAS.h"
57
58
  #include "GLONASS.h"
@@ -61,56 +62,12 @@ class RINEX_Reader {
61
62
  public:
62
63
  typedef RINEX_Reader<U> self_t;
63
64
  typedef std::map<std::string, std::vector<std::string> > header_t;
64
-
65
- struct src_stream_t : public std::istream {
66
- src_stream_t(std::istream &is) : std::istream(is.rdbuf()) {}
67
- /**
68
- * getline() for multi-platform (in addition to \n, \r\n and \r are supported)
69
- *
70
- * @see https://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
71
- * @see https://en.cppreference.com/w/cpp/io/basic_istream/getline
72
- * TODO gcount() mismatch
73
- */
74
- std::istream& getline(
75
- typename std::istream::char_type* s,
76
- std::streamsize n){
77
- std::streamsize i(1), consumed(0);
78
- typename std::istream::sentry se(*this, true);
79
- if((bool)se){
80
- std::streambuf* sb(this->rdbuf());
81
- for(; i < n; ++i){
82
- int c(sb->sbumpc());
83
- if(c == std::streambuf::traits_type::eof()){
84
- this->setstate(std::ios::eofbit);
85
- break;
86
- }
87
- ++consumed;
88
- if(c == '\n'){
89
- break;
90
- }else if(c == '\r'){
91
- if(sb->sgetc() == '\n'){
92
- sb->sbumpc();
93
- ++consumed;
94
- }
95
- break;
96
- }
97
- *(s++) = (typename std::istream::char_type)c;
98
- }
99
- }
100
- if(((i == 1) && (consumed == 0)) || (i == n)){
101
- this->setstate(std::ios::failbit);
102
- }else{
103
- *s = '\0';
104
- }
105
- return *this;
106
- }
107
- };
108
65
 
109
66
  protected:
110
67
  header_t _header;
111
- src_stream_t src;
68
+ typename TextHelper<>::crlf_stream_t src;
112
69
  bool _has_next;
113
-
70
+
114
71
  public:
115
72
  struct version_type_t {
116
73
  int version;
@@ -186,57 +143,8 @@ class RINEX_Reader {
186
143
  bool has_next() const {return _has_next;}
187
144
 
188
145
  template <class T, bool is_integer = std::numeric_limits<T>::is_integer>
189
- struct conv_t {
190
- static bool d(
191
- std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
192
- if(str2val){
193
- std::stringstream ss(buf.substr(offset, length));
194
- ss >> *(T *)value;
195
- return (ss.rdstate() & std::ios_base::failbit) == 0;
196
- }else{
197
- std::stringstream ss;
198
- ss << std::setfill(opt == 1 ? '0' : ' ') << std::right << std::setw(length) << *(T *)value;
199
- buf.replace(offset, length, ss.str());
200
- return true;
201
- }
202
- }
203
- static bool f_pure(
204
- std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
205
- if(str2val){
206
- std::stringstream ss(buf.substr(offset, length));
207
- ss >> *(T *)value;
208
- return (ss.rdstate() & std::ios_base::failbit) == 0;
209
- }else{
210
- std::stringstream ss;
211
- ss << std::setfill(' ') << std::right << std::setw(length)
212
- << std::setprecision(precision) << std::fixed
213
- << *(T *)value;
214
- buf.replace(offset, length, ss.str());
215
- return true;
216
- }
217
- }
218
- static bool f(
219
- std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
220
- bool res(f_pure(buf, offset, length, value, precision, str2val));
221
- if((!str2val) && res){
222
- if((*(T *)value) >= 0){
223
- if((*(T *)value) < 1){
224
- int i(length - precision - 2);
225
- // 0.12345 => .12345
226
- if(i >= 0){buf[i + offset] = ' ';}
227
- }
228
- }else{
229
- if((*(T *)value) > -1){
230
- int i(length - precision - 2);
231
- // -0.12345 => -.12345
232
- if(i >= 0){buf[i + offset] = '-';}
233
- if(--i >= 0){buf[i + offset] = ' ';}
234
- }
235
- }
236
- }
237
- return res;
238
- }
239
- static bool e(
146
+ struct conv_t : public TextHelper<>::template format_t<T> {
147
+ static bool e_dot_head(
240
148
  std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
241
149
  if(str2val){
242
150
  std::string s(buf.substr(offset, length));
@@ -272,66 +180,25 @@ class RINEX_Reader {
272
180
  }
273
181
  }
274
182
  };
183
+
275
184
  template <class T>
276
- struct conv_t<T, true> {
277
- static bool d(
278
- std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
279
- return conv_t<T, false>::d(buf, offset, length, value, opt, str2val);
280
- }
281
- static bool f_pure(
282
- std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
283
- double v(*(T *)value);
284
- bool res(
285
- conv_t<double, false>::f_pure(buf, offset, length, &v, precision, str2val));
286
- *(T *)value = static_cast<T>(v);
287
- return res;
288
- }
289
- static bool f(
290
- std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
291
- double v(*(T *)value);
292
- bool res(
293
- conv_t<double, false>::f(buf, offset, length, &v, precision, str2val));
294
- *(T *)value = static_cast<T>(v);
295
- return res;
296
- }
297
- static bool e(
185
+ struct conv_t<T, true> : public TextHelper<>::template format_t<T> {
186
+ static bool e_dot_head(
298
187
  std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
299
188
  double v(*(T *)value);
300
189
  bool res(
301
- conv_t<double, false>::e(buf, offset, length, &v, precision, str2val));
190
+ conv_t<double, false>::e_dot_head(buf, offset, length, &v, precision, str2val));
302
191
  *(T *)value = static_cast<T>(v);
303
192
  return res;
304
193
  }
305
194
  };
306
195
 
307
- struct convert_item_t {
308
- bool (*func)(
309
- std::string &buf, const int &offset, const int &length, void *value,
310
- const int &opt, const bool &str2val);
311
- int offset;
312
- int length;
313
- int value_offset;
314
- int opt;
315
- };
316
-
317
- /**
318
- * @param recovery if conversion fails, then this functor is invoked.
319
- * If recovery is successfully performed, this functor should return true.
320
- * The return value of this function reflects it.
321
- * @return (bool) if all conversion are successfully performed, true is returned; otherwise false.
322
- */
323
- static bool convert(
196
+ typedef typename TextHelper<>::convert_item_t convert_item_t;
197
+
198
+ static inline bool convert(
324
199
  const convert_item_t *items, const int &size, const std::string &buf, void *values,
325
200
  bool (*recovery)(const int &, const std::string &, void *) = NULL){
326
- // str => value
327
- bool res(true);
328
- for(int i(0); i < size; ++i){
329
- if((*items[i].func)(
330
- const_cast<std::string &>(buf), items[i].offset, items[i].length, (char *)values + items[i].value_offset,
331
- items[i].opt, true)){continue;}
332
- res &= (recovery ? (*recovery)(i, buf, values) : false);
333
- }
334
- return res;
201
+ return TextHelper<>::str2val(items, size, buf, values, recovery);
335
202
  }
336
203
  template <int N>
337
204
  static inline bool convert(
@@ -979,29 +846,37 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
979
846
  int year, month, day;
980
847
  FloatT tau_c_neg, tau_GPS; // TODO check tau_GPS polarity
981
848
  int leap_sec;
849
+ int flags;
850
+ enum {
851
+ TAU_C_NEG = 0x01,
852
+ TAU_GPS = 0x02,
853
+ LEAP_SEC = 0x04,
854
+ };
982
855
  };
983
856
  static const typename super_t::convert_item_t t_corr_glonass_v2[4];
984
857
 
985
858
  bool extract_t_corr_glonass_v2(t_corr_glonass_t &t_corr_glonass) const {
986
- bool utc, leap;
859
+ t_corr_glonass.flags = 0;
987
860
  super_t::header_t::const_iterator it;
988
861
 
989
- if(utc = ((it = _header.find("CORR TO SYSTEM TIME")) != _header.end())){
862
+ if((it = _header.find("CORR TO SYSTEM TIME")) != _header.end()){
990
863
  super_t::convert(t_corr_glonass_v2, it->second.front(), &t_corr_glonass);
864
+ t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG;
991
865
  }
992
866
 
993
- if(leap = ((it = _header.find("LEAP SECONDS")) != _header.end())){
867
+ if((it = _header.find("LEAP SECONDS")) != _header.end()){
994
868
  iono_utc_t iono_utc;
995
869
  super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
996
870
  t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
871
+ t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
997
872
  }
998
873
 
999
- return utc && leap;
874
+ return t_corr_glonass.flags > 0;
1000
875
  }
1001
876
 
1002
877
  bool extract_t_corr_glonass_v3(t_corr_glonass_t &t_corr_glonass) const {
1003
878
  iono_utc_t iono_utc;
1004
- bool utc(false), leap(false);
879
+ t_corr_glonass.flags = 0;
1005
880
  typedef super_t::header_t::const_iterator it_t;
1006
881
  typedef super_t::header_t::mapped_type::const_iterator it2_t;
1007
882
 
@@ -1013,11 +888,12 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
1013
888
  super_t::convert(utc_v3, *it2, &iono_utc);
1014
889
  t_corr_glonass.year = t_corr_glonass.month = t_corr_glonass.day = 0;
1015
890
  t_corr_glonass.tau_c_neg = iono_utc.A0;
891
+ t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG;
1016
892
  }else if(it2->find("GLGP") != it2->npos){
1017
893
  super_t::convert(utc_v3, *it2, &iono_utc);
1018
894
  t_corr_glonass.tau_GPS = iono_utc.A0;
895
+ t_corr_glonass.flags |= t_corr_glonass_t::TAU_GPS;
1019
896
  }
1020
- utc = true;
1021
897
  }
1022
898
  }
1023
899
 
@@ -1028,10 +904,10 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
1028
904
  super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
1029
905
  }
1030
906
  t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
1031
- leap = true;
907
+ t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
1032
908
  }
1033
909
 
1034
- return utc && leap;
910
+ return t_corr_glonass.flags > 0;
1035
911
  }
1036
912
 
1037
913
  struct space_node_list_t {
@@ -1088,8 +964,11 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
1088
964
  typename message_glonass_t::eph_t eph0(reader.msg_glonass);
1089
965
  eph0.tau_c = -t_corr_glonass.tau_c_neg;
1090
966
  eph0.tau_GPS = t_corr_glonass.tau_GPS;
1091
- typename GLONASS_SpaceNode<FloatT>
1092
- ::SatelliteProperties::Ephemeris_with_GPS_Time eph(eph0, t_corr_glonass.leap_sec);
967
+ typename GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris_with_GPS_Time eph(
968
+ eph0,
969
+ (t_corr_glonass.flags & t_corr_glonass_t::LEAP_SEC)
970
+ ? t_corr_glonass.leap_sec
971
+ : GPS_Time<FloatT>::guess_leap_seconds(reader.msg_glonass.date_tm));
1093
972
  space_nodes.glonass->satellite(reader.msg_glonass.svid).register_ephemeris(eph);
1094
973
  res++;
1095
974
  break;
@@ -1426,10 +1305,10 @@ class RINEX_OBS_Reader : public RINEX_Reader<> {
1426
1305
  {super_t::template conv_t<value_type>::d, offset, length, \
1427
1306
  offsetof(container_type, container_member), 1}
1428
1307
  #define GEN_F2(offset, length, precision, container_type, container_member, value_type) \
1429
- {super_t::template conv_t<value_type>::f, offset, length, \
1308
+ {super_t::template conv_t<value_type>::f_dot_head, offset, length, \
1430
1309
  offsetof(container_type, container_member), precision}
1431
1310
  #define GEN_E2(offset, length, precision, container_type, container_member, value_type) \
1432
- {super_t::template conv_t<value_type>::e, offset, length, \
1311
+ {super_t::template conv_t<value_type>::e_dot_head, offset, length, \
1433
1312
  offsetof(container_type, container_member), precision}
1434
1313
  #define GEN_F(offset, length, precision, container_type, container_member) \
1435
1314
  GEN_F2(offset, length, precision, container_type, container_member, FloatT)
@@ -1958,14 +1837,14 @@ class RINEX_Writer {
1958
1837
  static std::string RINEX_Float(
1959
1838
  const FloatT &value, const int width = 19, const int precision = 12){
1960
1839
  std::string s;
1961
- RINEX_Reader<U>::template conv_t<FloatT>::f(s, 0, width, &const_cast<FloatT &>(value), precision, false);
1840
+ RINEX_Reader<U>::template conv_t<FloatT>::f_dot_head(s, 0, width, &const_cast<FloatT &>(value), precision, false);
1962
1841
  return s;
1963
1842
  }
1964
1843
  template <class FloatT>
1965
1844
  static std::string RINEX_FloatD(
1966
1845
  const FloatT &value, const int width = 19, const int precision = 12){
1967
1846
  std::string s;
1968
- RINEX_Reader<U>::template conv_t<FloatT>::e(s, 0, width, &const_cast<FloatT &>(value), precision, false);
1847
+ RINEX_Reader<U>::template conv_t<FloatT>::e_dot_head(s, 0, width, &const_cast<FloatT &>(value), precision, false);
1969
1848
  return s;
1970
1849
  }
1971
1850
  template <class T>
@@ -1975,20 +1854,10 @@ class RINEX_Writer {
1975
1854
  return s;
1976
1855
  }
1977
1856
 
1978
- /**
1979
- * @return If all conversion are successfully performed, then true; otherwise false;
1980
- */
1981
1857
  static bool convert(
1982
1858
  const typename RINEX_Reader<U>::convert_item_t *items, const int &size,
1983
1859
  std::string &buf, const void *values){
1984
- // value => string
1985
- bool res(true);
1986
- for(int i(0); i < size; ++i){
1987
- res &= (*items[i].func)(
1988
- buf, items[i].offset, items[i].length, (char *)(const_cast<void *>(values)) + items[i].value_offset,
1989
- items[i].opt, false);
1990
- }
1991
- return res;
1860
+ return TextHelper<>::val2str(items, size, buf, values);
1992
1861
  }
1993
1862
  template <int N>
1994
1863
  static inline bool convert(
@@ -1768,30 +1768,38 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
1768
1768
  return (delta_t < 0) || (delta_t > Timing::values[Timing::GEO_NAVIGATION_DATA].interval);
1769
1769
  }
1770
1770
 
1771
- constellation_t constellation(
1772
- const gps_time_t &t_rx, const float_t &pseudo_range = 0,
1773
- const bool &with_velocity = true) const {
1771
+ float_t clock_error(const gps_time_t &t) const {
1772
+ float_t t_G(-t.interval(WN, 0)); // t_0 is expected to be modified as time of week
1773
+ return a_Gf0 + a_Gf1 * (t_G - t_0); // Eq.(A-45), delay is positive
1774
+ }
1774
1775
 
1775
- float_t t_G(-t_rx.interval(WN, 0) - (pseudo_range / gps_space_node_t::light_speed));
1776
+ float_t clock_error_dot(const gps_time_t &t) const {
1777
+ return a_Gf1;
1778
+ }
1776
1779
 
1777
- float_t t(t_G - (a_Gf0 + a_Gf1 * (t_G - t_0))); // Eq.(A-45)
1778
- float_t t_dot(1.0 - a_Gf1);
1780
+ constellation_t constellation(
1781
+ const gps_time_t &t_tx, const float_t &dt_transit = 0,
1782
+ const bool &with_velocity = true) const {
1779
1783
 
1780
- float_t delta_t(t - t_0), delta_t2(delta_t * delta_t / 2);
1784
+ float_t delta_t(-t_tx.interval(WN, t_0)), delta_t2(delta_t * delta_t / 2);
1781
1785
 
1782
1786
  constellation_t res = {
1783
1787
  xyz_t(
1784
1788
  x + dx * delta_t + ddx * delta_t2,
1785
1789
  y + dy * delta_t + ddy * delta_t2,
1786
- z + dz * delta_t + ddz * delta_t2), // Eq. (A-44)
1790
+ z + dz * delta_t + ddz * delta_t2).after(dt_transit), // Eq. (A-44)
1787
1791
  xyz_t(
1788
- (dx + ddx * delta_t) * t_dot,
1789
- (dy + ddy * delta_t) * t_dot,
1790
- (dz + ddz * delta_t) * t_dot),
1792
+ dx + ddx * delta_t,
1793
+ dy + ddy * delta_t,
1794
+ dz + ddz * delta_t).after(dt_transit),
1791
1795
  };
1792
1796
 
1793
- // Be careful, Sagnac correction must be performed before geometric distance calculation
1794
- // @see SBAS_SpaceNode::sagnac_correction
1797
+ /* Sagnac correction, which is expected to perform before geometric distance calculation,
1798
+ * can be skipped by using non-zero dt_transit.
1799
+ * This is because the return values is corrected to be along with the coordinate
1800
+ * at the reception time.
1801
+ * @see SBAS_SpaceNode::sagnac_correction
1802
+ */
1795
1803
 
1796
1804
  return res;
1797
1805
  }
@@ -114,19 +114,17 @@ class SBAS_SinglePositioning : public SolverBaseT {
114
114
  static inline const typename space_node_t::Satellite &sat(const void *ptr) {
115
115
  return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
116
116
  }
117
- static xyz_t position(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
118
- return sat(ptr).ephemeris().constellation(t, pseudo_range, false).position;
117
+ static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
118
+ return sat(ptr).ephemeris().constellation(t_tx, dt_transit, false).position;
119
119
  }
120
- static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
121
- return sat(ptr).ephemeris().constellation(t, pseudo_range, true).velocity;
120
+ static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
121
+ return sat(ptr).ephemeris().constellation(t_tx, dt_transit, true).velocity;
122
122
  }
123
- static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
124
- // Clock correction is taken into account in position()
125
- return 0;
123
+ static float_t clock_error(const void *ptr, const gps_time_t &t_tx) {
124
+ return sat(ptr).ephemeris().clock_error(t_tx);
126
125
  }
127
- static float_t clock_error_dot(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
128
- // Clock rate error is taken in account in velocity()
129
- return 0;
126
+ static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
127
+ return sat(ptr).ephemeris().clock_error_dot(t_tx);
130
128
  }
131
129
  };
132
130
  satellite_t res = {
@@ -257,20 +255,25 @@ class SBAS_SinglePositioning : public SolverBaseT {
257
255
 
258
256
  range -= receiver_error;
259
257
 
258
+ static const float_t &c(GPS_SpaceNode<float_t>::light_speed);
259
+
260
260
  // Clock error correction
261
261
  range += ((range_error.unknown_flag & range_error_t::SATELLITE_CLOCK)
262
- ? (sat.clock_error(time_arrival, range) * GPS_SpaceNode<float_t>::light_speed)
262
+ ? (sat.clock_error(time_arrival - range / c) * c)
263
263
  : range_error.value[range_error_t::SATELLITE_CLOCK]);
264
264
 
265
265
  // TODO WAAS long term clock correction (2.1.1.4.11)
266
266
 
267
267
  // Calculate satellite position
268
- xyz_t sat_pos(sat.position(time_arrival, range));
268
+ float_t dt_transit(range / c);
269
+ gps_time_t t_tx(time_arrival - dt_transit);
270
+ xyz_t sat_pos(sat.position(t_tx, dt_transit));
269
271
  float_t geometric_range(usr_pos.xyz.dist(sat_pos));
270
272
 
271
- // Calculate residual with Sagnac correction (A.4.4.11)
273
+ // Calculate residual without Sagnac correction (A.4.4.11),
274
+ // because of the satellite position is calculated in the reception time ECEF.
272
275
  res.range_residual = range
273
- + space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
276
+ // + space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
274
277
  - geometric_range;
275
278
 
276
279
  // Setup design matrix
@@ -312,12 +315,12 @@ class SBAS_SinglePositioning : public SolverBaseT {
312
315
 
313
316
  res.range_corrected = range;
314
317
 
315
- xyz_t rel_vel(sat.velocity(time_arrival, range) - usr_vel); // Calculate velocity
318
+ xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
316
319
 
317
320
  res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
318
321
  + res.los_neg[1] * rel_vel.y()
319
322
  + res.los_neg[2] * rel_vel.z()
320
- + sat.clock_error_dot(time_arrival, range) * GPS_SpaceNode<float_t>::light_speed;
323
+ + sat.clock_error_dot(t_tx) * c;
321
324
 
322
325
  return res;
323
326
  }