gps_pvt 0.5.1 → 0.6.0

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

@@ -452,7 +452,7 @@ struct GPS_Time {
     struct {
       int week;
       float_t seconds;
-    } uncorrected; // to work around of "incomplete type" error within g++
+    } uncorrected, corrected; // to work around of "incomplete type" error within g++
     leap_second_event_t(
         const int &year, const int &month, const int &day,
         const int &leap)
@@ -462,9 +462,11 @@ struct GPS_Time {
       t.tm_year = tm_year;
       t.tm_mon = tm_mon;
       t.tm_mday = tm_mday;
-      GPS_Time t_gps(t);
-      uncorrected.week = t_gps.week;
-      uncorrected.seconds = t_gps.seconds;
+      GPS_Time t_gps_uc(t), t_gps(t_gps_uc + leap);
+      uncorrected.week = t_gps_uc.week;
+      uncorrected.seconds = t_gps_uc.seconds;
+      corrected.week = t_gps.week;
+      corrected.seconds = t_gps.seconds;
     }
   };
   static const leap_second_event_t leap_second_events[];
@@ -484,6 +486,92 @@ struct GPS_Time {
     }
     return 0;
   }
+
+  int leap_seconds() const {
+    // Treat *this as (normal) GPS time, to which leap seconds are added when it is converted from std::tm
+    for(const leap_second_event_t *i(&leap_second_events[0]); i->leap_seconds > 0; ++i){
+      if(*this >= GPS_Time(i->corrected.week, i->corrected.seconds)){return i->leap_seconds;}
+    }
+    return 0;
+  }
+  float_t julian_date() const {
+    struct conv_t {
+      static int ymd2jd(const int &year, const int &month, const int &day){
+        // @see https://en.wikipedia.org/wiki/Julian_day#Converting_Gregorian_calendar_date_to_Julian_Day_Number
+        return std::div(1461 * (year + 4800 + std::div(month - 14, 12).quot), 4).quot
+            + std::div(367 * (month - 2 - 12 * std::div((month - 14), 12).quot), 12).quot
+            - std::div(3 * std::div((year + 4900 + std::div(month - 14, 12).quot), 100).quot, 4).quot
+            + day - 32075;
+      }
+    };
+    // origin of Julian day is "noon (not midnight)" BC4713/1/1
+    static const float_t t0(conv_t::ymd2jd(1980, 1, 6) - 0.5);
+    // GPS Time is advanced by leap seconds compared with UTC.
+    // The following calculation is incorrect for a day in which a leap second is inserted or truncated.
+    // @see https://en.wikipedia.org/wiki/Julian_day#Julian_date_calculation
+    return t0 + week * 7 + (seconds - leap_seconds()) / seconds_day;
+  }
+  float_t julian_date_2000() const {
+    static const std::tm tm_2000 = {0, 0, 12, 1, 0, 2000 - 1900};
+    static const float_t jd2000(GPS_Time(tm_2000, 13).julian_date());
+    return julian_date() - jd2000;
+  }
+  std::tm utc() const {return c_tm(leap_seconds());}
+
+  float_t greenwich_mean_sidereal_time_sec_ires1996(const float_t &delta_ut1 = float_t(0)) const {
+    float_t jd2000(julian_date_2000() + delta_ut1 / seconds_day);
+    float_t jd2000_day(float_t(0.5) + std::floor(jd2000 - 0.5)); // +/-0.5, +/-1.5, ...
+
+    // @see Chapter 2 of Orbits(978-3540785217) by Xu Guochang
+    // @see Chapter 5 of IERS Conventions (1996) https://www.iers.org/IERS/EN/Publications/TechnicalNotes/tn21.html
+    float_t jc(jd2000_day / 36525), // Julian century
+        jc2(std::pow(jc, 2)), jc3(std::pow(jc, 3));
+    float_t gmst0(24110.54841 // = 6h 41m 50.54841s
+        + jc * 8640184.812866
+        + jc2 * 0.093104
+        - jc3 * 6.2E-6);
+    // ratio of universal to sidereal time as given by Aoki et al. (1982)
+    float_t r(1.002737909350795 // 7.2921158553E-5 = 1.002737909350795 / 86400 * 2pi
+        + jc * 5.9006E-11 // 4.3E-15 = 5.9E-11 / 86400 * 2pi
+        - jc2 * 5.9E-15); // in seconds
+    return gmst0 + r * (jd2000 - jd2000_day) * seconds_day;
+  }
+
+  /**
+   * ERA (Earth rotation rate) defined in Sec. 5.5.3 in IERS 2010(Technical Note No.36)
+   */
+  float_t earth_rotation_angle(const float_t &delta_ut1 = float_t(0),
+      const float_t &scale_factor = float_t(M_PI * 2)) const {
+    float_t jd2000(julian_date_2000() + delta_ut1 / seconds_day);
+    return (jd2000 * 1.00273781191135448 + 0.7790572732640) * scale_factor; // Eq.(5.14)
+  }
+
+  float_t greenwich_mean_sidereal_time_sec_ires2010(const float_t &delta_ut1 = float_t(0)) const {
+    float_t era(earth_rotation_angle(delta_ut1, seconds_day));
+
+    float_t t(julian_date_2000() / 36525);
+    // @see Eq.(5.32) Chapter 5 of IERS Conventions (2010)
+#define AS2SEC(as) (((float_t)seconds_day / (360 * 3600)) * (as))
+    return era
+        + AS2SEC(0.014506)
+        + t * AS2SEC(4612.156534)
+        + std::pow(t, 2) * AS2SEC(1.3915817)
+        - std::pow(t, 3) * AS2SEC(0.00000044)
+        - std::pow(t, 4) * AS2SEC(0.000029956)
+        - std::pow(t, 5) * AS2SEC(0.0000000368);
+#undef AS2SEC
+  }
+
+  /**
+   * Calculate Greenwich mean sidereal time (GMST) in seconds
+   * Internally, greenwich_mean_sidereal_time_sec_ired2010() is called.
+   * @param delta_ut1 time difference of UTC and UT1; UT1 = UTC + delta_UT1,
+   * @return GMST in seconds. 86400 seconds correspond to one rotation
+   * @see greenwich_mean_sidereal_time_sec_ires2010()
+   */
+  float_t greenwich_mean_sidereal_time_sec(const float_t &delta_ut1 = float_t(0)) const {
+    return greenwich_mean_sidereal_time_sec_ires2010(delta_ut1);
+  }
 };
 
 template <class FloatT>

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

@@ -248,7 +248,15 @@ struct GPS_Solver_Base {
       return impl_clock_error_dot(impl, t, pseudo_range);
     }
     static const satellite_t &unavailable() {
-      static const satellite_t res = {NULL};
+      struct impl_t {
+        static xyz_t v3(const void *, const gps_time_t &, const float_t &){
+          return xyz_t(0, 0, 0);
+        }
+        static float_t v(const void *, const gps_time_t &, const float_t &){
+          return float_t(0);
+        }
+      };
+      static const satellite_t res = {NULL, impl_t::v3, impl_t::v3, impl_t::v, impl_t::v};
       return res;
     }
   };

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(
@@ -1426,10 +1293,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 +1825,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 +1842,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(