gps_pvt 0.1.3 → 0.1.7

checksums.yaml

@@ -1,7 +1,7 @@
 ---
 SHA256:
-  metadata.gz: 58f4afe48f4ffe54827ee4cdcefa0f1f05b34f933ec58ba8eeeaf271bca1efec
-  data.tar.gz: f0f606b68eeac175d84c6b9befdef25a3e1ca61bac1b2de078cff9c0e36073b0
+  metadata.gz: b05eabd6a7d771c772d6146722153e6ae2e9292ed9f41892e8285a2aa7d2f4ee
+  data.tar.gz: e8566da7fc64f0f330771ee8420bef47dfd819ae703d5ef46fe0bcf310ddf11f
 SHA512:
-  metadata.gz: f21951df16944c44a9ff009307860bbb4c815ff5b5ebabac61853c22c4d85b3f61db463754c0775f63fc301b2f7cb425bd46c9f32e9e6f653a86d2f92faa02fc
-  data.tar.gz: d51d26bd9fbcfcd28dfb2a660c965bca1d9b697936d53ebba87a4c79014bf0bb7b0fb5447e8a33aecf258eca0c8344cee5545e0798b0a500c3bd3ec1cd35e359
+  metadata.gz: 56054b607dc23efa2826f02ab117fc07080e447a3775215ea642aa4e77f481929ec0f8d66a4634c873cb453c9a5e9fc36fe73c528c361e50aaa844ebf385506f
+  data.tar.gz: 4d47da0ba28881434c03ef1eb270c5d3e20dc17e5abbb222bb076ae2c2c8295a973b38764e7616e6f4adcd102904ed21389397a52ec3f91853c938e98464248e

data/README.md

@@ -2,7 +2,7 @@
 
 GPS_PVT is a Ruby GPS (Global positioning system) PVT (position, velocity and time) solver. It accepts RINEX NAV and OBS files in addition to u-blox ubx format. Its significant features are easy to use with highly flexibility to customize internal solver behavior such as weight for each available satellite.
 
-The PVT solution is obtained with a stand alone positioning (i.e. neither differential nor kinematic) with least square. Its main internal codes are derived from ones of [ninja-scan-light](https://github.com/fenrir-naru/ninja-scan-light) having capability to calculate tightly-coupled GNSS/INS integrated solution. These codes are written by C++, and wrapped by [SWIG](http://www.swig.org/).
+The PVT solution is obtained with a stand alone positioning (i.e. neither differential nor kinematic) with application of least square to each snapshot. Its main internal codes are derived from ones of [ninja-scan-light](https://github.com/fenrir-naru/ninja-scan-light) having capability to calculate tightly-coupled GNSS/INS integrated solution. These codes are written by C++, and wrapped by [SWIG](http://www.swig.org/).
 
 [![Gem Version](https://badge.fury.io/rb/gps_pvt.svg)](https://badge.fury.io/rb/gps_pvt)
 [![Ruby](https://github.com/fenrir-naru/gps_pvt/actions/workflows/main.yml/badge.svg)](https://github.com/fenrir-naru/gps_pvt/actions/workflows/main.yml)
@@ -46,7 +46,8 @@ receiver.parse_rinex_obs(rinex_obs_file)
 
 # Or precise control of outputs
 receiver.parse_rinex_obs(rinex_obs_file){|pvt, meas| # per epoch
-  meas # => measurement, array of [prn, key, value]; key is represented by GPS_PVT::GPS::Measurement::L1_PSEUDORANGE
+  meas.to_a # => measurement, array of [prn, key, value]; key is represented by GPS_PVT::GPS::Measurement::L1_PSEUDORANGE; instead of .to_a, .to_hash returns {prn => {key => value, ...}, ...}
+  
   pvt # => PVT solution, all properties are shown by pvt.methods
   # for example
   if(pvt.position_solved?){
@@ -59,6 +60,9 @@ receiver.parse_rinex_obs(rinex_obs_file){|pvt, meas| # per epoch
       pvt.receiver_error_rate # clock error rate in m/s
     }
     pvt.used_satellite_list # array of used, i.e., visible and weight>0, satellite
+    pvt.azimuth # azimuth angle [rad] to used satellites in Hash {prn => value, ...}
+    pvt.elevation # elevation angle [rad]
+
     pvt.G # design matrix in Earth-centered-Earth-fixed (ECEF); .to_a returns double array converted from matrix. its row corresponds to one of used_satellite_list
     pvt.G_enu # design matrix in East-North-Up (ENU)
     pvt.W # weight for each satellite
@@ -68,16 +72,34 @@ receiver.parse_rinex_obs(rinex_obs_file){|pvt, meas| # per epoch
 }
 
 # Customize solution
-receiver.solver.options.exclude(prn) # Exclude satellite; the default is to use every satellite if visible
-receiver.solver.options.include(prn) # Discard previous setting of exclusion
+receiver.solver.gps_options.exclude(prn) # Exclude satellite; the default is to use every satellite if visible
+receiver.solver.gps_options.include(prn) # Discard previous setting of exclusion
+receiver.solver.gps_options.elevation_mask = Math::PI / 180 * 10 # example 10 [deg] elevation mask
 receiver.solver.hooks[:relative_property] = proc{|prn, rel_prop, rcv_e, t_arv, usr_pos, usr_vel|
   # control weight per satellite per iteration
   weight, range_c, range_r, rate_rel_neg, *los_neg = rel_prop # relative property
-  weight = 1 # default; same weight
+  # rcv_e, t_arv, usr_pos, usr_vel are temporary solution of 
+  # receiver clock error [m], time of arrival [s], user position and velocity in ECEF, respectively.
+  
+  weight = 1 # same as default; identical weight for each visible satellite
   # or weight based on elevation
   # elv = GPS_PVT::Coordinate::ENU::relative_rel(GPS_PVT::Coordinate::XYZ::new(*los_neg), usr_pos).elevation
   # weight = (Math::sin(elv)/0.8)**2
-  [weight, range_c, range_r, rate_rel_neg] + los_neg
+  
+  [weight, range_c, range_r, rate_rel_neg] + los_neg # must return relative property
+}
+
+# Dynamic customization of weight for each epoch
+(class << receiver; self; end).instance_eval{ # do before parse_XXX
+  alias_method(:run_orig, :run)
+  define_method(:run){|meas, t_meas, &b|
+    meas # observation, same as the 2nd argument of parse_XXX
+    receiver.solver.hooks[:relative_property] = proc{|prn, rel_prop, rcv_e, t_arv, usr_pos, usr_vel|
+      # Do something based on meas, t_meas.
+      rel_prop
+    }
+    run_orig(meas, t_meas, &b)
+  }
 }
 ```
 

data/Rakefile

@@ -69,12 +69,15 @@ task :swig do
       sh [:make, :clean, wrapper,
           "BUILD_DIR=#{out_dir}",
           "SWIGFLAGS='-c++ -ruby -prefix \"GPS_PVT::\"#{" -D__MINGW__" if ENV["MSYSTEM"]}'"].join(' ')
-      lines = open(wrapper, 'r').read.lines.collect{|line|
-        line.sub(/rb_require\(\"([^\"]+)\"\)/){ # from camel to underscore downcase style
-          "rb_require(\"#{$1.sub('GPS_PVT', 'gps_pvt')}\")"
+      open(wrapper, 'r+'){|io|
+        lines = io.read.lines.collect{|line|
+          line.sub(/rb_require\(\"([^\"]+)\"\)/){ # from camel to underscore downcase style
+            "rb_require(\"#{$1.sub('GPS_PVT', 'gps_pvt')}\")"
+          }
         }
+        io.rewind
+        io.write(lines.join)
       }
-      open(wrapper, 'w').write(lines.join)
     }
   }
 end

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

@@ -243,7 +243,7 @@ struct GPS_Time {
   GPS_Time &canonicalize(){
     int quot(std::floor(seconds / seconds_week));
     week += quot;
-    seconds -= (seconds_week * quot);
+    seconds -= (quot * (int)seconds_week);
     return *this;
   }
   GPS_Time(const std::tm &t, const float_t &leap_seconds = 0) {

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

@@ -50,6 +50,7 @@
 #include <algorithm>
 #include <cstddef>
 #include <cstring>
+#include <limits>
 
 #include "GPS.h"
 
@@ -180,22 +181,27 @@ class RINEX_Reader {
     const header_t &header() const {return const_cast<self_t *>(this)->header();}
     bool has_next() const {return _has_next;}
 
-    template <class T>
+    template <class T, bool is_integer = std::numeric_limits<T>::is_integer>
     struct conv_t {
-      static void d(
+      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(buf.substr(offset, length)) >> *(T *)value;
+          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 void f(
+      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(buf.substr(offset, length)) >> *(T *)value;
+          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)
@@ -215,9 +221,10 @@ class RINEX_Reader {
             }
           }
           buf.replace(offset, length, s);
+          return true;
         }
       }
-      static void e(
+      static bool e(
           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));
@@ -225,7 +232,9 @@ class RINEX_Reader {
           if(pos != std::string::npos){
             s.replace(pos, 1, "E");
           }
-          std::stringstream(s) >> *(T *)value;
+          std::stringstream ss(s);
+          ss >> *(T *)value;
+          return (ss.rdstate() & std::ios_base::failbit) == 0;
         }else{
           int w((std::max)(length, precision + 6)); // parentheses of std::max mitigates error C2589 under Windows VC
 
@@ -247,12 +256,36 @@ class RINEX_Reader {
           ss.str("");
           ss << std::setfill(' ') << std::right << std::setw(w) << s;
           buf.replace(offset, length, ss.str());
+          return true;
         }
       }
     };
+    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(
+          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(
+          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 {
-      void (*func)(
+      bool (*func)(
           std::string &buf, const int &offset, const int &length, void *value,
           const int &opt, const bool &str2val);
       int offset;
@@ -261,17 +294,30 @@ class RINEX_Reader {
       int opt;
     };
 
-    static void convert(const convert_item_t *items, const int &size, const std::string &buf, void *values){
+    /**
+     * @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(
+        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){
-        (*items[i].func)(
+        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);
+            items[i].opt, true)){continue;}
+        res &= (recovery ? (*recovery)(i, buf, values) : false);
       }
+      return res;
     }
     template <int N>
-    static inline void convert(const convert_item_t (&items)[N], const std::string &buf, void *values){
-      convert(items, N, buf, values);
+    static inline bool convert(
+        const convert_item_t (&items)[N], const std::string &buf, void *values,
+        bool (*recovery)(const int &, const std::string &, void *) = NULL){
+      return convert(items, N, buf, values, recovery);
     }
 };
 
@@ -404,7 +450,6 @@ struct RINEX_NAV {
     std::tm t_oc_tm;
     int t_oc_year4, t_oc_year2, t_oc_mon12;
     FloatT t_oc_sec;
-    FloatT iodc_f, iode_f; // originally int type
     FloatT t_oe_WN;
     FloatT ura_meter;
     FloatT SV_health_f;
@@ -420,7 +465,6 @@ struct RINEX_NAV {
         t_oc_year2(t_oc_tm.tm_year % 100),
         t_oc_mon12(t_oc_tm.tm_mon + 1),
         t_oc_sec(std::fmod(eph.t_oc, 60)),
-        iodc_f(eph.iodc), iode_f(eph.iode),
         t_oe_WN(eph.WN),
         ura_meter(ephemeris_t::URA_meter(eph.URA)),
         SV_health_f(((eph.SV_health & 0x20) && (eph.SV_health & 0x1F == 0)) ? 1 : eph.SV_health),
@@ -434,9 +478,6 @@ struct RINEX_NAV {
       eph.WN = t_oc.week;
       eph.t_oc = t_oc.seconds;
 
-      eph.iodc = iodc_f;
-      eph.iode = iode_f;
-
       eph.URA = ephemeris_t::URA_index(ura_meter); // meter to index
 
       /* @see ftp://igs.org/pub/data/format/rinex210.txt
@@ -478,13 +519,15 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
     static const typename super_t::convert_item_t eph7_v2[2], eph7_v3[2];
 
   protected:
+    typename super_t::version_type_t::sat_system_t sys_of_msg;
     message_t msg;
     
-    void seek_next_v2() {
+    void seek_next_v2_gps() {
       char buf[256];
 
-      for(int i = 0; (i < 8) && (super_t::src.good()); i++){
-        if(super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+      for(int i(0); i < 8; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
         std::string line(buf);
 
         switch(i){
@@ -505,25 +548,43 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
         }
       }
       msg.update();
+      sys_of_msg = super_t::version_type_t::SYS_GPS;
       super_t::_has_next = true;
     }
 
-    void seek_next_v3() {
+    void seek_next_v2_glonass() {
       char buf[256];
 
-      for(int i = 0; (i < 8) && (super_t::src.good()); i++){
-        if(super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+      for(int i(0); i < 4; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+      }
+      //sys_of_msg = super_t::version_type_t::SYS_GLONASS; // TODO currently not implemented
+      sys_of_msg = super_t::version_type_t::SYS_UNKNOWN;
+      super_t::_has_next = true;
+    }
+
+    void seek_next_v2() {
+      switch(super_t::version_type.sat_system){
+        case super_t::version_type_t::SYS_GPS: seek_next_v2_gps(); return;
+        case super_t::version_type_t::SYS_GLONASS: seek_next_v2_glonass(); return;
+        default: break;
+      }
+    }
+
+    template <std::size_t N>
+    void seek_next_v3_gps(char (&buf)[N]) {
+      super_t::convert(eph0_v3, std::string(buf), &msg);
+      msg.t_oc_tm.tm_year = msg.t_oc_year4 - 1900; // tm_year base is 1900
+      msg.t_oc_tm.tm_mon = msg.t_oc_mon12 - 1; // month [0, 11]
+      msg.t_oc_sec = msg.t_oc_tm.tm_sec;
+
+      for(int i(1); i < 8; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
         std::string line(buf);
 
         switch(i){
-          case 0: {
-            // if(line_data[0] != 'G'){} // TODO check GPS before parsing
-            super_t::convert(eph0_v3, line, &msg);
-            msg.t_oc_tm.tm_year = msg.t_oc_year4 - 1900; // tm_year base is 1900
-            msg.t_oc_tm.tm_mon = msg.t_oc_mon12 - 1; // month [0, 11]
-            msg.t_oc_sec = msg.t_oc_tm.tm_sec;
-            break;
-          }
           case 1: super_t::convert(eph1_v3, line, &msg); break;
           case 2: super_t::convert(eph2_v3, line, &msg); break;
           case 3: super_t::convert(eph3_v3, line, &msg); break;
@@ -534,10 +595,42 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
         }
       }
       msg.update();
+      sys_of_msg = super_t::version_type_t::SYS_GPS;
       super_t::_has_next = true;
     }
 
+    template <std::size_t N>
+    void seek_next_v3_not_implemented(char (&buf)[N], const int &lines) {
+      for(int i(1); i < lines; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+      }
+      sys_of_msg = super_t::version_type_t::SYS_UNKNOWN;
+      super_t::_has_next = true;
+    }
+
+    void seek_next_v3() {
+      char buf[256];
+
+      while(super_t::src.good()
+          && (!super_t::src.getline(buf, sizeof(buf)).fail())){
+
+        switch(buf[0]){
+          case 'G': seek_next_v3_gps(buf); return; // GPS
+          case 'E': seek_next_v3_not_implemented(buf, 8); return; // Galileo
+          case 'R': seek_next_v3_not_implemented(buf, 4); return; // Glonass
+          case 'J': seek_next_v3_not_implemented(buf, 8); return; // QZSS
+          case 'C': seek_next_v3_not_implemented(buf, 8); return; // Beido
+          case 'S': seek_next_v3_not_implemented(buf, 4); return; // SBAS
+          case 'T': seek_next_v3_not_implemented(buf, 8); return; // IRNSS
+          default: break;
+        }
+      }
+    }
+
     void seek_next() {
+      super_t::_has_next = false;
+      sys_of_msg = super_t::version_type_t::SYS_UNKNOWN;
       super_t::version_type.version >= 300 ? seek_next_v3() : seek_next_v2();
     }
 
@@ -547,11 +640,8 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
     }
     ~RINEX_NAV_Reader(){}
     
-    typename space_node_t::Satellite::Ephemeris next() {
-      typename space_node_t::Satellite::Ephemeris current(msg.eph);
-      super_t::_has_next = false;
+    void next() {
       seek_next();
-      return current;
     }
     
     static const typename super_t::convert_item_t iono_alpha_v2[4];
@@ -623,6 +713,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
       }
 
       if((it = _header.find("LEAP SECONDS")) != _header.end()){
+        iono_utc.delta_t_LSF = iono_utc.WN_LSF = iono_utc.DN = 0;
         if(version_type.version >= 301){
           super_t::convert(utc_leap_v301, it->second.front(), &iono_utc);
         }else{
@@ -635,19 +726,38 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
       return alpha && beta && utc && leap;
     }
 
-    static int read_all(std::istream &in, space_node_t &space_node){
+    struct space_node_list_t {
+      space_node_t *gps;
+    };
+
+    static int read_all(std::istream &in, space_node_list_t &space_nodes = {0}){
       int res(-1);
       RINEX_NAV_Reader reader(in);
-      (reader.version_type.version >= 300)
-          ? reader.extract_iono_utc_v3(space_node)
-          : reader.extract_iono_utc_v2(space_node);
+      if(space_nodes.gps){
+        (reader.version_type.version >= 300)
+            ? reader.extract_iono_utc_v3(*space_nodes.gps)
+            : reader.extract_iono_utc_v2(*space_nodes.gps);
+      }
       res++;
-      for(; reader.has_next(); ++res){
-        typename space_node_t::Satellite::Ephemeris eph(reader.next());
-        space_node.satellite(eph.svid).register_ephemeris(eph);
+      for(; reader.has_next(); reader.next()){
+        switch(reader.sys_of_msg){
+          case super_t::version_type_t::SYS_GPS:
+            if(!space_nodes.gps){break;}
+            space_nodes.gps->satellite(reader.msg.eph.svid).register_ephemeris(reader.msg.eph);
+            res++;
+            break;
+          default: break;
+        }
       }
       return res;
     }
+
+    static int read_all(std::istream &in, space_node_t &space_node){
+      space_node_list_t list = {
+        &space_node,
+      };
+      return read_all(in, list);
+    }
 };
 
 template <class FloatT>
@@ -964,12 +1074,16 @@ class RINEX_OBS_Reader : public RINEX_Reader<> {
 #define GEN_I(offset, length, container_type, container_member, value_type) \
     {super_t::template conv_t<value_type>::d, offset, length, \
       offsetof(container_type, container_member), 1}
-#define GEN_F(offset, length, precision, container_type, container_member) \
-    {super_t::template conv_t<FloatT>::f, offset, length, \
+#define GEN_F2(offset, length, precision, container_type, container_member, value_type) \
+    {super_t::template conv_t<value_type>::f, offset, length, \
       offsetof(container_type, container_member), precision}
-#define GEN_E(offset, length, precision, container_type, container_member) \
-    {super_t::template conv_t<FloatT>::e, offset, length, \
+#define GEN_E2(offset, length, precision, container_type, container_member, value_type) \
+    {super_t::template conv_t<value_type>::e, 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)
+#define GEN_E(offset, length, precision, container_type, container_member) \
+    GEN_E2(offset, length, precision, container_type, container_member, FloatT)
 
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph0_v2[] = {
@@ -1000,17 +1114,17 @@ const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>
 
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph1_v2[] = {
-  GEN_E( 3, 19, 12, message_t, iode_f),
-  GEN_E(22, 19, 12, message_t, eph.c_rs),
-  GEN_E(41, 19, 12, message_t, eph.delta_n),
-  GEN_E(60, 19, 12, message_t, eph.M0),
+  GEN_E2( 3, 19, 12, message_t, eph.iode, int),
+  GEN_E (22, 19, 12, message_t, eph.c_rs),
+  GEN_E (41, 19, 12, message_t, eph.delta_n),
+  GEN_E (60, 19, 12, message_t, eph.M0),
 };
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph1_v3[] = {
-  GEN_E( 4, 19, 12, message_t, iode_f),
-  GEN_E(23, 19, 12, message_t, eph.c_rs),
-  GEN_E(42, 19, 12, message_t, eph.delta_n),
-  GEN_E(61, 19, 12, message_t, eph.M0),
+  GEN_E2( 4, 19, 12, message_t, eph.iode, int),
+  GEN_E (23, 19, 12, message_t, eph.c_rs),
+  GEN_E (42, 19, 12, message_t, eph.delta_n),
+  GEN_E (61, 19, 12, message_t, eph.M0),
 };
 
 template <class FloatT>
@@ -1075,17 +1189,17 @@ const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>
 
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph6_v2[] = {
-  GEN_E( 3, 19, 12, message_t, ura_meter),
-  GEN_E(22, 19, 12, message_t, SV_health_f),
-  GEN_E(41, 19, 12, message_t, eph.t_GD),
-  GEN_E(60, 19, 12, message_t, iodc_f),
+  GEN_E ( 3, 19, 12, message_t, ura_meter),
+  GEN_E (22, 19, 12, message_t, SV_health_f),
+  GEN_E (41, 19, 12, message_t, eph.t_GD),
+  GEN_E2(60, 19, 12, message_t, eph.iodc, int),
 };
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph6_v3[] = {
-  GEN_E( 4, 19, 12, message_t, ura_meter),
-  GEN_E(23, 19, 12, message_t, SV_health_f),
-  GEN_E(42, 19, 12, message_t, eph.t_GD),
-  GEN_E(61, 19, 12, message_t, iodc_f),
+  GEN_E ( 4, 19, 12, message_t, ura_meter),
+  GEN_E (23, 19, 12, message_t, SV_health_f),
+  GEN_E (42, 19, 12, message_t, eph.t_GD),
+  GEN_E2(61, 19, 12, message_t, eph.iodc, int),
 };
 
 template <class FloatT>
@@ -1224,7 +1338,7 @@ class RINEX_Writer {
           for(typename super_t::reverse_iterator it(header.rbegin()), it_end(header.rend());
               it != it_end; ++it){
             if(it->first != key){continue;}
-            it_tail = it.base() - 1;
+            it_tail = it.base(); // it_tail points to the next element of an element having the smae key
             break;
           }
         }
@@ -1235,14 +1349,16 @@ class RINEX_Writer {
             const typename super_t::value_type::second_type &value){
           if(it_head == header.end()){
             header.push_back(typename super_t::value_type(key, value));
-            it_head = it_tail = header.rbegin().base();
+            it_tail = header.end(); // in case of invalidation
+            it_head = it_tail - 1;
             return *this;
           }
           it_head->second = value;
-          for(; it_tail != it_head; --it_tail){
-            if(it_tail->first == key){continue;}
+          for(--it_tail; it_tail != it_head; --it_tail){
+            if(it_tail->first != key){continue;}
             header.erase(it_tail);
           }
+          it_tail = it_head + 1;
           return *this;
         }
         /**
@@ -1250,14 +1366,24 @@ class RINEX_Writer {
          */
         bracket_accessor_t &operator<<(
             const typename super_t::value_type::second_type &value){
-          if(it_tail == header.end()){
-            header.push_back(typename super_t::value_type(key, value));
-            it_head = it_tail = header.rbegin().base();
-            return *this;
-          }
-          header.insert(++it_tail, typename super_t::value_type(key, value));
+          super_t::size_type i_head(it_head - header.begin()), i_tail(it_tail - header.begin());
+          header.insert(it_tail, typename super_t::value_type(key, value));
+          it_head = header.begin() + i_head;  // in case of invalidation
+          it_tail = header.begin() + i_tail + 1;
           return *this;
         }
+        unsigned int entries() const {
+          return it_tail - it_head;
+        }
+        typename super_t::iterator find(
+            const typename super_t::value_type::second_type &value) const {
+          for(typename super_t::iterator it(it_head); it != it_tail; ++it){
+            if(it->second.find(value) != super_t::value_type::second_type::npos){
+              return it;
+            }
+          }
+          return header.end();
+        }
       };
       bracket_accessor_t operator[]( // mimic of std::map::operator[]=
           const typename super_t::value_type::first_type &key){
@@ -1336,20 +1462,25 @@ class RINEX_Writer {
       return s;
     }
     
-    static void convert(
+    /**
+     * @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){
-        (*items[i].func)(
+        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;
     }
     template <int N>
-    static inline void convert(
+    static inline bool convert(
         const typename RINEX_Reader<U>::convert_item_t (&items)[N], std::string &buf, const void *values){
-      convert(items, N, buf, values);
+      return convert(items, N, buf, values);
     }
 
     void pgm_runby_date(
@@ -1426,6 +1557,9 @@ class RINEX_NAV_Writer : public RINEX_Writer<> {
       std::string s(60, ' ');
       if(super_t::_version_type.version >= 301){
         super_t::convert(reader_t::utc_leap_v301, s, &space_node.iono_utc());
+        if(space_node.iono_utc().WN_LSF == 0){
+          s.replace(6, 18, 18, ' ');
+        }
       }else{
         super_t::convert(reader_t::utc_leap_v2, s, &space_node.iono_utc());
       }
@@ -1475,16 +1609,6 @@ class RINEX_NAV_Writer : public RINEX_Writer<> {
       return *this;
     }
 
-  protected:
-    struct WriteAllFunctor {
-      RINEX_NAV_Writer &w;
-      int &counter;
-      void operator()(const typename space_node_t::Satellite::Ephemeris &eph) {
-        w << message_t(eph);
-        counter++;
-      }
-    };
-
   public:
     void set_version(
         const int &version,
@@ -1493,31 +1617,67 @@ class RINEX_NAV_Writer : public RINEX_Writer<> {
           version, super_t::version_type_t::FTYPE_NAVIGATION, sys));
     }
 
-    int write_all(const space_node_t &space_node, const int &version = 304){
+    int write_all(
+        const typename reader_t::space_node_list_t &space_nodes,
+        const int &version = 304){
       int res(-1);
-      set_version(version);
-      if(space_node.is_valid_iono_utc()){
-        iono_alpha(space_node);
-        iono_beta(space_node);
-        utc_params(space_node);
-        leap_seconds(space_node);
+      int systems(0);
+      set_version(version, super_t::version_type_t::SYS_UNKNOWN);
+      if(space_nodes.gps && space_nodes.gps->is_valid_iono_utc()){
+        ++systems;
+        set_version(version, super_t::version_type_t::SYS_GPS);
+        switch(version / 100){
+          case 2:
+            if(_header["ION ALPHA"].entries() == 0){iono_alpha(*space_nodes.gps);}
+            if(_header["ION BETA"].entries() == 0){iono_beta(*space_nodes.gps);}
+            if(_header["DELTA-UTC: A0,A1,T,W"].entries() == 0){utc_params(*space_nodes.gps);}
+            if(_header["LEAP SECONDS"].entries() == 0){leap_seconds(*space_nodes.gps);}
+            break;
+          case 3:
+            if(_header["IONOSPHERIC CORR"].find("GPSA") == _header.end()){iono_alpha(*space_nodes.gps);}
+            if(_header["IONOSPHERIC CORR"].find("GPSB") == _header.end()){iono_beta(*space_nodes.gps);}
+            if(_header["TIME SYSTEM CORR"].find("GPUT") == _header.end()){utc_params(*space_nodes.gps);}
+            if(_header["LEAP SECONDS"].entries() == 0){leap_seconds(*space_nodes.gps);}
+            break;
+        }
+      }
+      if(systems > 1){
+        set_version(version, super_t::version_type_t::SYS_MIXED);
       }
       super_t::dist << header();
       res++;
 
-      WriteAllFunctor functor = {*this, res};
-      for(typename space_node_t::satellites_t::const_iterator
-            it(space_node.satellites().begin()), it_end(space_node.satellites().end());
-          it != it_end; ++it){
-        it->second.each_ephemeris(
-            functor,
-            space_node_t::Satellite::eph_list_t::EACH_ALL_INVERTED);
+      struct {
+        RINEX_NAV_Writer &w;
+        int &counter;
+        void operator()(const typename space_node_t::Satellite::Ephemeris &eph) {
+          w << message_t(eph);
+          counter++;
+        }
+      } functor = {*this, res};
+      if(space_nodes.gps){
+        for(typename space_node_t::satellites_t::const_iterator
+              it(space_nodes.gps->satellites().begin()), it_end(space_nodes.gps->satellites().end());
+            it != it_end; ++it){
+          it->second.each_ephemeris(
+              functor,
+              space_node_t::Satellite::eph_list_t::EACH_ALL_INVERTED);
+        }
       }
       return res;
     }
+    static int write_all(
+        std::ostream &out,
+        const typename reader_t::space_node_list_t &space_nodes,
+        const int &version = 304){
+      return RINEX_NAV_Writer(out).write_all(space_nodes, version);
+    }
+    int write_all(const space_node_t &space_node, const int &version = 304){
+      const typename reader_t::space_node_list_t list = {&const_cast<space_node_t &>(space_node)};
+      return write_all(list, version);
+    }
     static int write_all(std::ostream &out, const space_node_t &space_node, const int &version = 304){
-      RINEX_NAV_Writer writer(out);
-      return writer.write_all(space_node, version);
+      return RINEX_NAV_Writer(out).write_all(space_node, version);
     }
 };
 template <class FloatT>

data/lib/gps_pvt/receiver.rb

@@ -13,15 +13,15 @@ class Receiver
       [:week, :seconds, :c_tm].collect{|f| pvt.receiver_time.send(f)}.flatten
     }
   ]] + [[
-    [:receiver_clock_error_meter, :longitude, :latitude, :height],
+    [:receiver_clock_error_meter, :longitude, :latitude, :height, :rel_E, :rel_N, :rel_U],
     proc{|pvt|
-      next [nil] * 4 unless pvt.position_solved? 
+      next [nil] * 7 unless pvt.position_solved?
       [
         pvt.receiver_error,
         pvt.llh.lng / Math::PI * 180,
         pvt.llh.lat / Math::PI * 180,
         pvt.llh.alt,
-      ]
+      ] + (pvt.rel_ENU.to_a rescue [nil] * 3)
     } 
   ]] + [proc{
     labels = [:g, :p, :h, :v, :t].collect{|k| "#{k}dop".to_sym}
@@ -103,6 +103,7 @@ class Receiver
   end
     
   attr_accessor :solver
+  attr_accessor :base_station
 
   def initialize(options = {})
     @solver = GPS::Solver::new
@@ -127,6 +128,34 @@ class Receiver
         when :identical # same as default
           next true
         end
+      when :base_station
+        crd, sys = v.split(/ *, */).collect.with_index{|item, i|
+          case item
+          when /^([\+-]?\d+\.?\d*)([XYZNEDU]?)$/ # ex) meter[X], degree[N]
+            [$1.to_f, ($2 + "XY?"[i])[0]]
+          when /^([\+-]?\d+)_(?:(\d+)_(\d+\.?\d*)|(\d+\.?\d*))([NE])$/ # ex) deg_min_secN
+            [$1.to_f + ($2 || $4).to_f / 60 + ($3 || 0).to_f / 3600, $5]
+          else
+            raise "Unknown coordinate spec.: #{item}"
+          end
+        }.transpose
+        raise "Unknown base station: #{v}" if crd.size != 3
+        @base_station = case (sys = sys.join.to_sym)
+        when :XYZ, :XY?
+          Coordinate::XYZ::new(*crd)
+        when :NED, :ENU, :NE?, :EN? # :NE? => :NEU, :EN? => :ENU
+          (0..1).each{|i| crd[i] *= (Math::PI / 180)}
+          ([:NED, :NE?].include?(sys) ?
+              Coordinate::LLH::new(crd[0], crd[1], crd[2] * (:NED == sys ? -1 : 1)) :
+              Coordinate::LLH::new(crd[1], crd[0], crd[2])).xyz
+        else
+          raise "Unknown coordinate system: #{sys}"
+        end
+        $stderr.puts "Base station (LLH): #{
+          llh = @base_station.llh.to_a
+          llh[0..1].collect{|rad| rad / Math::PI * 180} + [llh[2]]
+        }"
+        next true
       end
       false
     }
@@ -137,17 +166,42 @@ class Receiver
     }.call(@solver.gps_options)
   end
 
-  def run(meas, t_meas)
-    #$stderr.puts "Measurement time: #{t_meas.to_a} (a.k.a #{"%d/%d/%d %d:%d:%d UTC"%[*t_meas.c_tm]})"
-    sn = @solver.gps_space_node
-    sn.update_all_ephemeris(t_meas)
+  GPS::Measurement.class_eval{
+    proc{
+      key2sym = []
+      GPS::Measurement.constants.each{|k|
+        i = GPS::Measurement.const_get(k)
+        key2sym[i] = k if i.kind_of?(Integer)
+      }
+      define_method(:to_a2){
+        to_a.collect{|prn, k, v| [prn, key2sym[k] || k, v]}
+      }
+      define_method(:to_hash2){
+        Hash[*(to_hash.collect{|prn, k_v|
+          [prn, Hash[*(k_v.collect{|k, v| [key2sym[k] || k, v]}.flatten(1))]]
+        }.flatten(1))]
+      }
+    }.call
+    alias_method(:add_orig, :add)
+    define_method(:add){|prn, key, value|
+      add_orig(prn, key.kind_of?(Symbol) ? GPS::Measurement.const_get(key) : key, value)
+    }
+  }
 
+  def run(meas, t_meas, ref_pos = @base_station)
+=begin
+    $stderr.puts "Measurement time: #{t_meas.to_a} (a.k.a #{"%d/%d/%d %d:%d:%d UTC"%[*t_meas.c_tm]})"
     meas.to_a.collect{|prn, k, v| prn}.uniq.each{|prn|
-      eph = sn.ephemeris(prn)
+      eph = @solver.gps_space_node.ephemeris(prn)
       $stderr.puts "XYZ(PRN:#{prn}): #{eph.constellation(t_meas)[0].to_a} (iodc: #{eph.iodc}, iode: #{eph.iode})"
-    } if false 
+    }
+=end
 
+    #@solver.gps_space_node.update_all_ephemeris(t_meas) # internally called in the following solver.solve
     pvt = @solver.solve(meas, t_meas)
+    pvt.define_singleton_method(:rel_ENU){
+      Coordinate::ENU::relative(xyz, ref_pos)
+    } if (ref_pos && pvt.position_solved?)
     pvt.define_singleton_method(:to_s){
       (OUTPUT_PVT_ITEMS.transpose[1].collect{|task|
         task.call(pvt)
@@ -242,9 +296,15 @@ class Receiver
           {
             :L1_PSEUDORANGE => [16, 8, "E"],
             :L1_DOPPLER => [24, 4, "e"],
+            :L1_CARRIER_PHASE => [8, 8, "E"],
+            :L1_SIGNAL_STRENGTH_dBHz => [30, 1, "c"],
           }.each{|k, prop|
-            meas.add(prn, GPS::Measurement.const_get(k), loader.call(*prop))
+            meas.add(prn, k, loader.call(*prop))
           }
+          # bit 0 of RINEX LLI (loss of lock indicator) shows lost lock
+          # between previous and current observation, which maps negative lock seconds
+          meas.add(prn, :L1_LOCK_SEC,
+              (packet[6 + 31 + (i * 24)] & 0x01 == 0x01) ? -1 : 0)
         }
         after_run.call(run(meas, t_meas), [meas, t_meas])
       when [0x02, 0x15] # RXM-RAWX
@@ -270,9 +330,15 @@ class Receiver
             }],
             :L1_DOPPLER => [32, 4, "e"],
             :L1_DOPPLER_SIGMA => [45, 1, nil, proc{|v| 2E-3 * (v[0] & 0xF)}],
+            :L1_CARRIER_PHASE => [24, 8, "E", proc{|v| (trk_stat & 0x2 == 0x2) ? v : nil}],
+            :L1_CARRIER_PHASE_SIGMA => [44, 1, nil, proc{|v|
+              (trk_stat & 0x2 == 0x2) ? (0.004 * (v[0] & 0xF)) : nil
+            }],
+            :L1_SIGNAL_STRENGTH_dBHz => [42, 1, "C"],
+            :L1_LOCK_SEC => [40, 2, "v", proc{|v| 1E-3 * v}],
           }.each{|k, prop|
             next unless v = loader.call(*prop)
-            meas.add(svid, GPS::Measurement.const_get(k), v)
+            meas.add(svid, k, v)
           }
         }
         after_run.call(run(meas, t_meas), [meas, t_meas])
@@ -314,9 +380,13 @@ class Receiver
       types ||= (item[:meas_types]['G'] || item[:meas_types][' ']).collect.with_index{|type_, i|
         case type_
         when "C1", "C1C"
-          [i, GPS::Measurement::L1_PSEUDORANGE]
+          [i, :L1_PSEUDORANGE]
+        when "L1", "L1C"
+          [i, :L1_CARRIER_PHASE]
         when "D1", "D1C"
-          [i, GPS::Measurement::L1_DOPPLER]
+          [i, :L1_DOPPLER]
+        when "S1", "S1C"
+          [i, :L1_SIGNAL_STRENGTH_dBHz]
         else
           nil 
         end

data/lib/gps_pvt/version.rb

@@ -1,5 +1,5 @@
 # frozen_string_literal: true
 
 module GPS_PVT
-  VERSION = "0.1.3"
+  VERSION = "0.1.7"
 end

metadata

@@ -1,14 +1,14 @@
 --- !ruby/object:Gem::Specification
 name: gps_pvt
 version: !ruby/object:Gem::Version
-  version: 0.1.3
+  version: 0.1.7
 platform: ruby
 authors:
 - fenrir(M.Naruoka)
-autorequire: 
+autorequire:
 bindir: exe
 cert_chain: []
-date: 2021-12-21 00:00:00.000000000 Z
+date: 2022-01-14 00:00:00.000000000 Z
 dependencies:
 - !ruby/object:Gem::Dependency
   name: rake
@@ -86,7 +86,6 @@ files:
 - ext/ninja-scan-light/tool/swig/makefile
 - ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb
 - ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb
-- gps_pvt.gemspec
 - lib/gps_pvt.rb
 - lib/gps_pvt/receiver.rb
 - lib/gps_pvt/ubx.rb
@@ -97,7 +96,7 @@ licenses: []
 metadata:
   homepage_uri: https://github.com/fenrir-naru/gps_pvt
   source_code_uri: https://github.com/fenrir-naru/gps_pvt
-post_install_message: 
+post_install_message:
 rdoc_options: []
 require_paths:
 - lib
@@ -112,8 +111,8 @@ required_rubygems_version: !ruby/object:Gem::Requirement
     - !ruby/object:Gem::Version
       version: '0'
 requirements: []
-rubygems_version: 3.1.2
-signing_key: 
+rubygems_version: 3.0.3
+signing_key:
 specification_version: 4
 summary: GPS position, velocity, and time (PVT) solver
 test_files: []

data/gps_pvt.gemspec

@@ -1,57 +0,0 @@
-# frozen_string_literal: true
-
-require_relative "lib/gps_pvt/version"
-
-Gem::Specification.new do |spec|
-  spec.name = "gps_pvt"
-  spec.version = GPS_PVT::VERSION
-  spec.authors = ["fenrir(M.Naruoka)"]
-  spec.email = ["fenrir.naru@gmail.com"]
-
-  spec.summary = "GPS position, velocity, and time (PVT) solver"
-  spec.description = "This module calculate PVT by using raw observation obtained from a GPS receiver"
-  spec.homepage = "https://github.com/fenrir-naru/gps_pvt"
-  spec.required_ruby_version = ">= 2.3.0"
-
-  #spec.metadata["allowed_push_host"] = "TODO: Set to your gem server 'https://example.com'"
-
-  spec.metadata["homepage_uri"] = spec.homepage
-  spec.metadata["source_code_uri"] = spec.homepage
-  #spec.metadata["changelog_uri"] = "TODO: Put your gem's CHANGELOG.md URL here."
-    
-  spec.extensions = ["ext/gps_pvt/extconf.rb"]
-
-  # Specify which files should be added to the gem when it is released.
-  # The `git ls-files -z` loads the files in the RubyGem that have been added into git.
-  spec.files = Dir.chdir(File.expand_path(__dir__)) do
-    `git ls-files -z`.split("\x0").reject do |f|
-      (f == __FILE__) || f.match(%r{\A(?:(?:test|spec|features)/|\.(?:git|travis|circleci)|appveyor)})
-    end
-  end
-  spec.bindir = "exe"
-  spec.executables = spec.files.grep(%r{\Aexe/}) { |f| File.basename(f) }
-  spec.require_paths = ["lib"]
-  
-  spec.files += proc{
-    require 'pathname'
-    base_dir = Pathname::new(File::absolute_path(File.dirname(__FILE__)))
-    # get an array of submodule dirs by executing 'pwd' inside each submodule
-    `git submodule --quiet foreach pwd`.split($/).collect{|dir|
-      # issue git ls-files in submodule's directory
-      `git -C #{dir} ls-files -v`.split($/).collect{|f|
-        next nil unless f =~ /^H */ # consider git sparse checkout
-        # get relative path
-        f = Pathname::new(File::join(dir, $'))
-        (f.relative? ? f : f.relative_path_from(base_dir)).to_s
-      }.compact
-    }.flatten
-  }.call
-
-  # Uncomment to register a new dependency of your gem
-  # spec.add_dependency "example-gem", "~> 1.0"
-  spec.add_development_dependency "rake"
-  spec.add_development_dependency "rake-compiler"
-
-  # For more information and examples about making a new gem, checkout our
-  # guide at: https://bundler.io/guides/creating_gem.html
-end