gps_pvt 0.4.1 → 0.6.0

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,54 +62,12 @@ class RINEX_Reader {
   public:
     typedef RINEX_Reader<U> self_t;
     typedef std::map<std::string, std::vector<std::string> > header_t;
-    
+
   protected:
     header_t _header;
-    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;
-      }
-    } src;
+    typename TextHelper<>::crlf_stream_t src;
     bool _has_next;
-    
+
   public:
     struct version_type_t {
       int version;
@@ -184,49 +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(
-          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;
-          std::string s(ss.str());
-          if((*(T *)value) >= 0){
-            if((*(T *)value) < 1){
-              int i(length - precision - 2);
-              if(i >= 0){s[i] = ' ';}
-            }
-          }else{
-            if((*(T *)value) > -1){
-              int i(length - precision - 2);
-              if(i >= 0){s[i] = '-';}
-              if(--i >= 0){s[i] = ' ';}
-            }
-          }
-          buf.replace(offset, length, s);
-          return true;
-        }
-      }
-      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));
@@ -262,58 +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(
+    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>::f(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;
       }
-      static bool e(
-          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));
-        *(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(
@@ -1408,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)
@@ -1940,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>
@@ -1957,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(

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

@@ -2183,10 +2183,11 @@ template <typename T>
 typename SBAS_SpaceNode<FloatT>::KnownSatellites::list_t
     SBAS_SpaceNode<FloatT>::KnownSatellites::sort(T sorter){
   static const typename SBAS_SpaceNode<FloatT>::RangingCode codes[] = {
+    {120,  145, 01106,    5,   "ASECNA (A-SBAS)"},
     {121,  175, 01241,    5,   "EGNOS (Eutelsat 5WB)"},
     {122,   52, 00267,  143.5, "SPAN (INMARSAT 4F1)"},
     {123,   21, 00232,   31.5, "EGNOS (ASTRA 5B)"},
-    {124,  237, 01617,    0,   "EGNOS (Reserved)"},
+    //{124,  237, 01617,    0,   "(Reserved)"},
     {125,  235, 01076,  -16,   "SDCM (Luch-5A)"},
     {126,  886, 01764,   63.9, "EGNOS (INMARSAT 4F2)"},
     {127,  657, 00717,   55,   "GAGAN (GSAT-8)"},
@@ -2198,16 +2199,30 @@ typename SBAS_SpaceNode<FloatT>::KnownSatellites::list_t
     {133,  603, 01731, -129,   "WAAS (SES-15)"},
     {134,  130, 00706,   91.5, "KASS (MEASAT-3D)"},
     {135,  359, 01216, -125,   "WAAS (Intelsat Galaxy 30)"},
-    {136,  595, 00740,    5,   "EGNOS (SES-5)"},
+    {136,  595, 00740,    5,   "EGNOS (HOTBIRD 13G)"},
     {137,   68, 01007,  127,   "MSAS (QZS-3)"},
     {138,  386, 00450,  107.3, "WAAS (ANIK F1R)"},
+    //{139,  797, 00305,    0,   "MSAS (QZS-7)"},
     {140,  456, 01653,   95,   "SDCM (Luch-5V)"},
     {141,  499, 01411,  167,   "SDCM (Luch-5A)"},
+    //{142,  883, 01644,    0,   "(Unallocated)"},
     {143,  307, 01312,  110.5, "BDSBAS (G3)"},
     {144,  127, 01060,   80,   "BDSBAS (G2)"},
-    {147,  118, 00355,   42.5, "NSAS (NIGCOMSAT-1R)"},
+    //{145,  211, 01560,    0,   "(Unallocated)"},
+    //{146,  121, 00035,    0,   "(Unallocated)"},
+    {147,  118, 00355,    5,   "ASECNA (A-SBAS)"},
     {148,  163, 00335,  -24.8, "ASAL (ALCOMSAT-1)"},
-  }; ///< @see https://www.gps.gov/technical/prn-codes/L1-CA-PRN-code-assignments-2020-Apr.pdf
+    //{149,  628, 01254,    0,   "(Unallocated)"},
+    //{150,  853, 01041,    0,   "EGNOS"},
+    //{151,  484, 00142,    0,   "(Unallocated)"},
+    //{152,  289, 01641,    0,   "(Unallocated)"},
+    //{153,  811, 01504,    0,   "(Unallocated)"},
+    //{154,  202, 00751,    0,   "(Unallocated)"},
+    //{155, 1021, 01774,    0,   "(Unallocated)"},
+    //{156,  463, 00107,    0,   "(Unallocated)"},
+    //{157,  568, 01153,    0,   "(Unallocated)"},
+    //{158,  904, 01542,    0,   "(Unallocated)"},
+  }; ///< @see https://www.gps.gov/technical/prn-codes/L1-CA-PRN-code-assignments-2021-Jun.pdf
   list_t res;
   res.reserve(sizeof(codes) / sizeof(codes[0]));
   for(unsigned int i(0); i < sizeof(codes) / sizeof(codes[0]); ++i){

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

@@ -73,7 +73,6 @@ class SBAS_SinglePositioning : public SolverBaseT {
 
     typedef SBAS_SpaceNode<float_t> space_node_t;
     inheritate_type(gps_time_t);
-    typedef typename space_node_t::Satellite satellite_t;
 
     inheritate_type(xyz_t);
     inheritate_type(enu_t);
@@ -81,6 +80,7 @@ class SBAS_SinglePositioning : public SolverBaseT {
     inheritate_type(pos_t);
 
     typedef typename base_t::measurement_t measurement_t;
+    typedef typename base_t::satellite_t satellite_t;
     typedef typename base_t::range_error_t range_error_t;
     typedef typename base_t::range_corrector_t range_corrector_t;
     typedef typename base_t::range_correction_t range_correction_t;
@@ -92,11 +92,52 @@ class SBAS_SinglePositioning : public SolverBaseT {
         SBAS_SinglePositioning_Options<float_t>, base_t> options_t;
 
   protected:
-    const space_node_t &_space_node;
     SBAS_SinglePositioning_Options<float_t> _options;
 
   public:
-    const space_node_t &space_node() const {return _space_node;}
+    struct satellites_t {
+      const void *impl;
+      satellite_t (*impl_select)(const void *, const prn_t &, const gps_time_t &);
+      inline satellite_t select(const prn_t &prn, const gps_time_t &receiver_time) const {
+        return impl_select(impl, prn, receiver_time);
+      }
+      static satellite_t select_broadcast(
+          const void *ptr, const prn_t &prn, const gps_time_t &receiver_time){
+        const typename space_node_t::satellites_t &sats(
+            reinterpret_cast<const space_node_t *>(ptr)->satellites());
+        const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn));
+        if((it_sat == sats.end()) // has ephemeris?
+            || (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
+          return satellite_t::unavailable();
+        }
+        struct impl_t {
+          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 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 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_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;
+          }
+        };
+        satellite_t res = {
+            &(it_sat->second),
+            impl_t::position, impl_t::velocity,
+            impl_t::clock_error, impl_t::clock_error_dot};
+        return res;
+      }
+      satellites_t(const space_node_t &sn)
+          : impl(&sn), impl_select(select_broadcast) {}
+    } satellites;
 
     struct ionospheric_sbas_t : public range_corrector_t {
       const space_node_t &space_node;
@@ -155,7 +196,8 @@ class SBAS_SinglePositioning : public SolverBaseT {
     }
 
     SBAS_SinglePositioning(const space_node_t &sn)
-        : base_t(), _space_node(sn), _options(available_options(options_t())),
+        : base_t(), _options(available_options(options_t())),
+        satellites(sn),
         ionospheric_sbas(sn), tropospheric_sbas() {
 
       // default ionospheric correction: Broadcasted IGP.
@@ -168,25 +210,17 @@ class SBAS_SinglePositioning : public SolverBaseT {
     ~SBAS_SinglePositioning(){}
 
     /**
-     * Check availability of a satellite with which observation is associated
+     * Select satellite by using PRN and time
      *
      * @param prn satellite number
      * @param receiver_time receiver time
-     * @return (const satellite_t *) If available, const pointer to satellite is returned,
-     * otherwise NULL.
+     * @return (satellite_t) If available, satellite.is_available() returning true is returned.
      */
-    const satellite_t *is_available(
-        const typename space_node_t::satellites_t::key_type &prn,
+    satellite_t select_satellite(
+        const prn_t &prn,
         const gps_time_t &receiver_time) const {
-
-      const typename space_node_t::satellites_t &sats(_space_node.satellites());
-      const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn));
-      if((it_sat == sats.end()) // has ephemeris?
-          || (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
-        return NULL;
-      }
-
-      return &(it_sat->second);
+      if(_options.exclude_prn[prn]){return satellite_t::unavailable();}
+      return satellites.select(prn, receiver_time);
     }
 
     /**
@@ -210,33 +244,28 @@ class SBAS_SinglePositioning : public SolverBaseT {
 
       relative_property_t res = {0};
 
-      if(_options.exclude_prn[prn]){return res;}
-
       float_t range;
       range_error_t range_error;
       if(!this->range(measurement, range, &range_error)){
         return res; // If no range entry, return with weight = 0
       }
 
-      const satellite_t *sat(is_available(prn, time_arrival));
-      if(!sat){return res;} // If satellite is unavailable, return with weight = 0
+      satellite_t sat(select_satellite(prn, time_arrival));
+      if(!sat.is_available()){return res;} // If satellite is unavailable, return with weight = 0
 
       ///< The following procedure is based on Appendix.S with modification
 
       range -= receiver_error;
 
-      // Clock correction will be performed in the following constellation()
-      if(!(range_error.unknown_flag & range_error_t::SATELLITE_CLOCK)){
-        range += range_error.value[range_error_t::SATELLITE_CLOCK];
-      }
+      // 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)
+          : range_error.value[range_error_t::SATELLITE_CLOCK]);
 
       // TODO WAAS long term clock correction (2.1.1.4.11)
 
-      // Calculate satellite position and velocity
-      typename space_node_t::SatelliteProperties::constellation_t sat_pos_vel(
-          sat->ephemeris().constellation(time_arrival, range));
-
-      const xyz_t &sat_pos(sat_pos_vel.position);
+      // Calculate satellite position
+      xyz_t sat_pos(sat.position(time_arrival, range));
       float_t geometric_range(usr_pos.xyz.dist(sat_pos));
 
       // Calculate residual with Sagnac correction (A.4.4.11)
@@ -283,24 +312,15 @@ class SBAS_SinglePositioning : public SolverBaseT {
 
       res.range_corrected = range;
 
-      xyz_t rel_vel(sat_pos_vel.velocity - usr_vel); // Calculate velocity
+      xyz_t rel_vel(sat.velocity(time_arrival, range) - usr_vel); // Calculate velocity
 
-      // Note: clock rate error is already accounted for in constellation()
       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();
+          + res.los_neg[2] * rel_vel.z()
+          + sat.clock_error_dot(time_arrival, range) * GPS_SpaceNode<float_t>::light_speed;
 
       return res;
     }
-
-    xyz_t *satellite_position(
-        const prn_t &prn,
-        const gps_time_t &time,
-        xyz_t &res) const {
-
-      const satellite_t *sat(is_available(prn, time));
-      return sat ? &(res = sat->ephemeris().constellation(time).position) : NULL;
-    }
 };
 
 #endif /* __SBAS_SOLVER_H__ */