gps_pvt 0.5.1 → 0.6.2

data/ext/ninja-scan-light/tool/navigation/RINEX.h

@@ -52,6 +52,7 @@
 #include <cstring>
 #include <limits>
 
+#include "util/text_helper.h"
 #include "GPS.h"
 #include "SBAS.h"
 #include "GLONASS.h"
@@ -61,56 +62,12 @@ class RINEX_Reader {
   public:
     typedef RINEX_Reader<U> self_t;
     typedef std::map<std::string, std::vector<std::string> > header_t;
-    
-    struct src_stream_t : public std::istream {
-      src_stream_t(std::istream &is) : std::istream(is.rdbuf()) {}
-      /**
-       * getline() for multi-platform (in addition to \n, \r\n and \r are supported)
-       *
-       * @see https://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
-       * @see https://en.cppreference.com/w/cpp/io/basic_istream/getline
-       * TODO gcount() mismatch
-       */
-      std::istream& getline(
-          typename std::istream::char_type* s,
-          std::streamsize n){
-        std::streamsize i(1), consumed(0);
-        typename std::istream::sentry se(*this, true);
-        if((bool)se){
-          std::streambuf* sb(this->rdbuf());
-          for(; i < n; ++i){
-            int c(sb->sbumpc());
-            if(c == std::streambuf::traits_type::eof()){
-              this->setstate(std::ios::eofbit);
-              break;
-            }
-            ++consumed;
-            if(c == '\n'){
-              break;
-            }else if(c == '\r'){
-              if(sb->sgetc() == '\n'){
-                sb->sbumpc();
-                ++consumed;
-              }
-              break;
-            }
-            *(s++) = (typename std::istream::char_type)c;
-          }
-        }
-        if(((i == 1) && (consumed == 0)) || (i == n)){
-          this->setstate(std::ios::failbit);
-        }else{
-          *s = '\0';
-        }
-        return *this;
-      }
-    };
 
   protected:
     header_t _header;
-    src_stream_t src;
+    typename TextHelper<>::crlf_stream_t src;
     bool _has_next;
-    
+
   public:
     struct version_type_t {
       int version;
@@ -186,57 +143,8 @@ class RINEX_Reader {
     bool has_next() const {return _has_next;}
 
     template <class T, bool is_integer = std::numeric_limits<T>::is_integer>
-    struct conv_t {
-      static bool d(
-          std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
-        if(str2val){
-          std::stringstream ss(buf.substr(offset, length));
-          ss >> *(T *)value;
-          return (ss.rdstate() & std::ios_base::failbit) == 0;
-        }else{
-          std::stringstream ss;
-          ss << std::setfill(opt == 1 ? '0' : ' ') << std::right << std::setw(length) << *(T *)value;
-          buf.replace(offset, length, ss.str());
-          return true;
-        }
-      }
-      static bool f_pure(
-          std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
-        if(str2val){
-          std::stringstream ss(buf.substr(offset, length));
-          ss >> *(T *)value;
-          return (ss.rdstate() & std::ios_base::failbit) == 0;
-        }else{
-          std::stringstream ss;
-          ss << std::setfill(' ') << std::right << std::setw(length)
-              << std::setprecision(precision) << std::fixed
-              << *(T *)value;
-          buf.replace(offset, length, ss.str());
-          return true;
-        }
-      }
-      static bool f(
-          std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
-        bool res(f_pure(buf, offset, length, value, precision, str2val));
-        if((!str2val) && res){
-          if((*(T *)value) >= 0){
-            if((*(T *)value) < 1){
-              int i(length - precision - 2);
-              // 0.12345 => .12345
-              if(i >= 0){buf[i + offset] = ' ';}
-            }
-          }else{
-            if((*(T *)value) > -1){
-              int i(length - precision - 2);
-              // -0.12345 => -.12345
-              if(i >= 0){buf[i + offset] = '-';}
-              if(--i >= 0){buf[i + offset] = ' ';}
-            }
-          }
-        }
-        return res;
-      }
-      static bool e(
+    struct conv_t : public TextHelper<>::template format_t<T> {
+      static bool e_dot_head(
           std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
         if(str2val){
           std::string s(buf.substr(offset, length));
@@ -272,66 +180,25 @@ class RINEX_Reader {
         }
       }
     };
+
     template <class T>
-    struct conv_t<T, true> {
-      static bool d(
-          std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
-        return conv_t<T, false>::d(buf, offset, length, value, opt, str2val);
-      }
-      static bool f_pure(
-          std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
-        double v(*(T *)value);
-        bool res(
-            conv_t<double, false>::f_pure(buf, offset, length, &v, precision, str2val));
-        *(T *)value = static_cast<T>(v);
-        return res;
-      }
-      static bool f(
-          std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
-        double v(*(T *)value);
-        bool res(
-            conv_t<double, false>::f(buf, offset, length, &v, precision, str2val));
-        *(T *)value = static_cast<T>(v);
-        return res;
-      }
-      static bool e(
+    struct conv_t<T, true> : public TextHelper<>::template format_t<T> {
+      static bool e_dot_head(
           std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
         double v(*(T *)value);
         bool res(
-            conv_t<double, false>::e(buf, offset, length, &v, precision, str2val));
+            conv_t<double, false>::e_dot_head(buf, offset, length, &v, precision, str2val));
         *(T *)value = static_cast<T>(v);
         return res;
       }
     };
 
-    struct convert_item_t {
-      bool (*func)(
-          std::string &buf, const int &offset, const int &length, void *value,
-          const int &opt, const bool &str2val);
-      int offset;
-      int length;
-      int value_offset;
-      int opt;
-    };
-
-    /**
-     * @param recovery if conversion fails, then this functor is invoked.
-     * If recovery is successfully performed, this functor should return true.
-     * The return value of this function reflects it.
-     * @return (bool) if all conversion are successfully performed, true is returned; otherwise false.
-     */
-    static bool convert(
+    typedef typename TextHelper<>::convert_item_t convert_item_t;
+    
+    static inline bool convert(
         const convert_item_t *items, const int &size, const std::string &buf, void *values,
         bool (*recovery)(const int &, const std::string &, void *) = NULL){
-      // str => value
-      bool res(true);
-      for(int i(0); i < size; ++i){
-        if((*items[i].func)(
-            const_cast<std::string &>(buf), items[i].offset, items[i].length, (char *)values + items[i].value_offset,
-            items[i].opt, true)){continue;}
-        res &= (recovery ? (*recovery)(i, buf, values) : false);
-      }
-      return res;
+      return TextHelper<>::str2val(items, size, buf, values, recovery);
     }
     template <int N>
     static inline bool convert(
@@ -979,29 +846,37 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
       int year, month, day;
       FloatT tau_c_neg, tau_GPS; // TODO check tau_GPS polarity
       int leap_sec;
+      int flags;
+      enum {
+        TAU_C_NEG = 0x01,
+        TAU_GPS   = 0x02,
+        LEAP_SEC  = 0x04,
+      };
     };
     static const typename super_t::convert_item_t t_corr_glonass_v2[4];
 
     bool extract_t_corr_glonass_v2(t_corr_glonass_t &t_corr_glonass) const {
-      bool utc, leap;
+      t_corr_glonass.flags = 0;
       super_t::header_t::const_iterator it;
 
-      if(utc = ((it = _header.find("CORR TO SYSTEM TIME")) != _header.end())){
+      if((it = _header.find("CORR TO SYSTEM TIME")) != _header.end()){
         super_t::convert(t_corr_glonass_v2, it->second.front(), &t_corr_glonass);
+        t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG;
       }
 
-      if(leap = ((it = _header.find("LEAP SECONDS")) != _header.end())){
+      if((it = _header.find("LEAP SECONDS")) != _header.end()){
         iono_utc_t iono_utc;
         super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
         t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
+        t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
       }
 
-      return utc && leap;
+      return t_corr_glonass.flags > 0;
     }
 
     bool extract_t_corr_glonass_v3(t_corr_glonass_t &t_corr_glonass) const {
       iono_utc_t iono_utc;
-      bool utc(false), leap(false);
+      t_corr_glonass.flags = 0;
       typedef super_t::header_t::const_iterator it_t;
       typedef super_t::header_t::mapped_type::const_iterator it2_t;
 
@@ -1013,11 +888,12 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
             super_t::convert(utc_v3, *it2, &iono_utc);
             t_corr_glonass.year = t_corr_glonass.month = t_corr_glonass.day = 0;
             t_corr_glonass.tau_c_neg = iono_utc.A0;
+            t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG;
           }else if(it2->find("GLGP") != it2->npos){
             super_t::convert(utc_v3, *it2, &iono_utc);
             t_corr_glonass.tau_GPS = iono_utc.A0;
+            t_corr_glonass.flags |= t_corr_glonass_t::TAU_GPS;
           }
-          utc = true;
         }
       }
 
@@ -1028,10 +904,10 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
           super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
         }
         t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
-        leap = true;
+        t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
       }
 
-      return utc && leap;
+      return t_corr_glonass.flags > 0;
     }
 
     struct space_node_list_t {
@@ -1088,8 +964,11 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
             typename message_glonass_t::eph_t eph0(reader.msg_glonass);
             eph0.tau_c = -t_corr_glonass.tau_c_neg;
             eph0.tau_GPS = t_corr_glonass.tau_GPS;
-            typename GLONASS_SpaceNode<FloatT>
-                ::SatelliteProperties::Ephemeris_with_GPS_Time eph(eph0, t_corr_glonass.leap_sec);
+            typename GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris_with_GPS_Time eph(
+                eph0,
+                (t_corr_glonass.flags & t_corr_glonass_t::LEAP_SEC)
+                   ? t_corr_glonass.leap_sec
+                   : GPS_Time<FloatT>::guess_leap_seconds(reader.msg_glonass.date_tm));
             space_nodes.glonass->satellite(reader.msg_glonass.svid).register_ephemeris(eph);
             res++;
             break;
@@ -1426,10 +1305,10 @@ class RINEX_OBS_Reader : public RINEX_Reader<> {
     {super_t::template conv_t<value_type>::d, offset, length, \
       offsetof(container_type, container_member), 1}
 #define GEN_F2(offset, length, precision, container_type, container_member, value_type) \
-    {super_t::template conv_t<value_type>::f, offset, length, \
+    {super_t::template conv_t<value_type>::f_dot_head, offset, length, \
       offsetof(container_type, container_member), precision}
 #define GEN_E2(offset, length, precision, container_type, container_member, value_type) \
-    {super_t::template conv_t<value_type>::e, offset, length, \
+    {super_t::template conv_t<value_type>::e_dot_head, offset, length, \
       offsetof(container_type, container_member), precision}
 #define GEN_F(offset, length, precision, container_type, container_member) \
     GEN_F2(offset, length, precision, container_type, container_member, FloatT)
@@ -1958,14 +1837,14 @@ class RINEX_Writer {
     static std::string RINEX_Float(
         const FloatT &value, const int width = 19, const int precision = 12){
       std::string s;
-      RINEX_Reader<U>::template conv_t<FloatT>::f(s, 0, width, &const_cast<FloatT &>(value), precision, false);
+      RINEX_Reader<U>::template conv_t<FloatT>::f_dot_head(s, 0, width, &const_cast<FloatT &>(value), precision, false);
       return s;
     }
     template <class FloatT>
     static std::string RINEX_FloatD(
         const FloatT &value, const int width = 19, const int precision = 12){
       std::string s;
-      RINEX_Reader<U>::template conv_t<FloatT>::e(s, 0, width, &const_cast<FloatT &>(value), precision, false);
+      RINEX_Reader<U>::template conv_t<FloatT>::e_dot_head(s, 0, width, &const_cast<FloatT &>(value), precision, false);
       return s;
     }
     template <class T>
@@ -1975,20 +1854,10 @@ class RINEX_Writer {
       return s;
     }
     
-    /**
-     * @return If all conversion are successfully performed, then true; otherwise false;
-     */
     static bool convert(
         const typename RINEX_Reader<U>::convert_item_t *items, const int &size,
         std::string &buf, const void *values){
-      // value => string
-      bool res(true);
-      for(int i(0); i < size; ++i){
-        res &= (*items[i].func)(
-            buf, items[i].offset, items[i].length, (char *)(const_cast<void *>(values)) + items[i].value_offset,
-            items[i].opt, false);
-      }
-      return res;
+      return TextHelper<>::val2str(items, size, buf, values);
     }
     template <int N>
     static inline bool convert(

data/ext/ninja-scan-light/tool/navigation/SBAS.h

@@ -1768,30 +1768,38 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
             return (delta_t < 0) || (delta_t > Timing::values[Timing::GEO_NAVIGATION_DATA].interval);
           }
 
-          constellation_t constellation(
-              const gps_time_t &t_rx, const float_t &pseudo_range = 0,
-              const bool &with_velocity = true) const {
+          float_t clock_error(const gps_time_t &t) const {
+            float_t t_G(-t.interval(WN, 0)); // t_0 is expected to be modified as time of week
+            return a_Gf0 + a_Gf1 * (t_G - t_0); // Eq.(A-45), delay is positive
+          }
 
-            float_t t_G(-t_rx.interval(WN, 0) - (pseudo_range / gps_space_node_t::light_speed));
+          float_t clock_error_dot(const gps_time_t &t) const {
+            return a_Gf1;
+          }
 
-            float_t t(t_G - (a_Gf0 + a_Gf1 * (t_G - t_0))); // Eq.(A-45)
-            float_t t_dot(1.0 - a_Gf1);
+          constellation_t constellation(
+              const gps_time_t &t_tx, const float_t &dt_transit = 0,
+              const bool &with_velocity = true) const {
 
-            float_t delta_t(t - t_0), delta_t2(delta_t * delta_t / 2);
+            float_t delta_t(-t_tx.interval(WN, t_0)), delta_t2(delta_t * delta_t / 2);
 
             constellation_t res = {
               xyz_t(
                   x + dx * delta_t + ddx * delta_t2,
                   y + dy * delta_t + ddy * delta_t2,
-                  z + dz * delta_t + ddz * delta_t2), // Eq. (A-44)
+                  z + dz * delta_t + ddz * delta_t2).after(dt_transit), // Eq. (A-44)
               xyz_t(
-                  (dx + ddx * delta_t) * t_dot,
-                  (dy + ddy * delta_t) * t_dot,
-                  (dz + ddz * delta_t) * t_dot),
+                  dx + ddx * delta_t,
+                  dy + ddy * delta_t,
+                  dz + ddz * delta_t).after(dt_transit),
             };
 
-            // Be careful, Sagnac correction must be performed before geometric distance calculation
-            // @see SBAS_SpaceNode::sagnac_correction
+            /* Sagnac correction, which is expected to perform before geometric distance calculation,
+             * can be skipped by using non-zero dt_transit.
+             * This is because the return values is corrected to be along with the coordinate
+             * at the reception time.
+             * @see SBAS_SpaceNode::sagnac_correction
+             */
 
             return res;
           }

data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h

@@ -114,19 +114,17 @@ class SBAS_SinglePositioning : public SolverBaseT {
           static inline const typename space_node_t::Satellite &sat(const void *ptr) {
             return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
           }
-          static xyz_t position(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).ephemeris().constellation(t, pseudo_range, false).position;
+          static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
+            return sat(ptr).ephemeris().constellation(t_tx, dt_transit, false).position;
           }
-          static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            return sat(ptr).ephemeris().constellation(t, pseudo_range, true).velocity;
+          static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
+            return sat(ptr).ephemeris().constellation(t_tx, dt_transit, true).velocity;
           }
-          static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            // Clock correction is taken into account in position()
-            return 0;
+          static float_t clock_error(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).ephemeris().clock_error(t_tx);
           }
-          static float_t clock_error_dot(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
-            // Clock rate error is taken in account in velocity()
-            return 0;
+          static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
+            return sat(ptr).ephemeris().clock_error_dot(t_tx);
           }
         };
         satellite_t res = {
@@ -257,20 +255,25 @@ class SBAS_SinglePositioning : public SolverBaseT {
 
       range -= receiver_error;
 
+      static const float_t &c(GPS_SpaceNode<float_t>::light_speed);
+
       // Clock error correction
       range += ((range_error.unknown_flag & range_error_t::SATELLITE_CLOCK)
-          ? (sat.clock_error(time_arrival, range) * GPS_SpaceNode<float_t>::light_speed)
+          ? (sat.clock_error(time_arrival - range / c) * c)
           : range_error.value[range_error_t::SATELLITE_CLOCK]);
 
       // TODO WAAS long term clock correction (2.1.1.4.11)
 
       // Calculate satellite position
-      xyz_t sat_pos(sat.position(time_arrival, range));
+      float_t dt_transit(range / c);
+      gps_time_t t_tx(time_arrival - dt_transit);
+      xyz_t sat_pos(sat.position(t_tx, dt_transit));
       float_t geometric_range(usr_pos.xyz.dist(sat_pos));
 
-      // Calculate residual with Sagnac correction (A.4.4.11)
+      // Calculate residual without Sagnac correction (A.4.4.11),
+      // because of the satellite position is calculated in the reception time ECEF.
       res.range_residual = range
-          + space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
+          // + space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
           - geometric_range;
 
       // Setup design matrix
@@ -312,12 +315,12 @@ class SBAS_SinglePositioning : public SolverBaseT {
 
       res.range_corrected = range;
 
-      xyz_t rel_vel(sat.velocity(time_arrival, range) - usr_vel); // Calculate velocity
+      xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
 
       res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
           + res.los_neg[1] * rel_vel.y()
           + res.los_neg[2] * rel_vel.z()
-          + sat.clock_error_dot(time_arrival, range) * GPS_SpaceNode<float_t>::light_speed;
+          + sat.clock_error_dot(t_tx) * c;
 
       return res;
     }