gps_pvt 0.6.2 → 0.6.3

checksums.yaml

@@ -1,7 +1,7 @@
 ---
 SHA256:
-  metadata.gz: 8a67291f7ea2d3146cace6c9e5f9c3262d89c1bd22534d6c147265c6189089c8
-  data.tar.gz: 211daa142bb6282341367f0ccd375d66a0083e6de01ec7841ac4833795c3a6cc
+  metadata.gz: f9ab8f442240a2b92bfcb975ec6cb72000ec7d257bf7d5f41e0765905b9fa020
+  data.tar.gz: 52ed9e2141f4e0bc111da34be34b28414557fd02f003c3fbff84db571135ad1a
 SHA512:
-  metadata.gz: 80a279b8018856c9d0d73b311c44efd97ce2794dc12353f10462a1b1b65aedc177ae6284fc324feb1980a02da2c2127ab8649058f840fc8279340f1152af3f62
-  data.tar.gz: ce708fe585b811a99a8e0e42862df7bcac023d44859b17ea08b9e6816425845b5ab0a31b207b79a870ca6d52dc43313b00eb67a5b38746ace843c54264ceca1e
+  metadata.gz: 63cc7efd29e6e576c15380bd43550b427015f57d99a545b3373ef829d496e87df799feed2b6499c2bf336619a81bb193ddd1c350990cc85ee7c52a7aadf0d264
+  data.tar.gz: 7911e374e8d1f33f283be0fd7a59262bd1bd2d0315d631456e37659b4729812c260f3f65495bb6de851e8ac7246f148d47129080717aeccc715cc1138e17e158

data/exe/to_ubx

@@ -0,0 +1,213 @@
+#!/usr/bin/env ruby
+
+require 'gps_pvt'
+require 'uri'
+require 'gps_pvt/ubx'
+
+# Convert file(s) to ubx format
+# TODO currently only RINEX observation file is supported.
+
+$stderr.puts <<__STRING__
+Usage: #{__FILE__} GPS_file ... > as_you_like.ubx
+As GPS_file, only rinex_obs(*.YYo) is currently supported.
+(YY = last two digit of year)
+File format is automatically determined based on its extention described in above parentheses.
+If you want to specify its format manually, command options like --rinex_obs=file_name are available.
+Supported RINEX versions are 2 and 3.
+RXM-RAWX and RXM-SFRBX are included in output UBX if corresponding file(s) is given. 
+A file having additional ".gz" or ".Z" extension is recognized as a compressed file.
+Major URL such as http(s)://... or ftp://... is acceptable as an input file name. 
+__STRING__
+
+options = []
+misc_options = {
+  :ubx_rawx => false
+}
+
+# check options and file format
+files = ARGV.collect{|arg|
+  next [arg, nil] unless arg =~ /^--([^=]+)=?/
+  k, v = [$1.downcase.to_sym, $']
+  next [v, k] if [:rinex_nav, :rinex_obs].include?(k) # file type
+  options << [$1.to_sym, $']
+  nil
+}.compact
+
+# Check file existence and extension
+files.collect!{|fname, ftype|
+  ftype ||= case fname
+  when /\.\d{2}[nhqg](?:\.gz)?$/; :rinex_nav
+  when /\.\d{2}o(?:\.gz)?$/; :rinex_obs
+  else
+    raise "Format cannot be guessed, use --(format, ex. rinex_obs)=#{fname}"
+  end
+  fname = proc{
+    next fname if File::exist?(fname)
+    if uri = URI::parse(fname) and !uri.instance_of?(URI::Generic) then
+      next uri
+    end
+    raise "File not found: #{fname}"
+  }.call
+  [fname, ftype]
+}
+
+options.reject!{|opt|
+  case opt[0]
+  when :ubx_rawx
+    misc_options[opt[0]] = opt[1]
+    true
+  else
+    false
+  end
+}
+
+rcv = GPS_PVT::Receiver::new(options)
+
+obs = []
+rcv.define_singleton_method(:run){|meas, t_meas, *args|
+  obs << [t_meas, meas]
+}
+
+# parse RINEX NAV
+files.each{|fname, ftype|
+  case ftype
+  when :rinex_nav; rcv.parse_rinex_nav(fname)
+  end
+}
+
+# other files
+files.each{|fname, ftype|
+  case ftype
+  when :rinex_obs; rcv.parse_rinex_obs(fname){}
+  end
+}
+
+obs.sort!{|a, b| a[0] <=> b[0]} # Sort by measurement time
+
+glonass_freq_ch = proc{
+  freq0, delta = [:L1_frequency_base, :L1_frequency_gap].collect{|k|
+    GPS_PVT::GPS::SpaceNode_GLONASS.send(k)
+  }
+  proc{|freq| ((freq - freq0) / delta).to_i}
+}.call
+
+gen_raw = proc{|t_meas, meas| # Convert to RXM-RAW(0x10)
+  meas = meas.to_hash
+  ubx = [0xB5, 0x62, 0x02, 0x10, 0, 0]
+  ubx += [(t_meas.seconds * 1E3).to_i, t_meas.week].pack("Vv").unpack("C*")
+  ubx += [0] * 2
+  meas_ubx = meas.collect{|sat, items|
+    res = [0] * 24
+    setter = proc{|value, offset, len, str, pre_proc|
+      array = case value
+      when Array; value
+      when Symbol
+        [items[GPS_PVT::GPS::Measurement.const_get(value)]]
+      else
+        next nil
+      end
+      pre_proc.call(array) if pre_proc
+      next if array.empty?
+      array = array.pack(str).unpack("C*") if str
+      res[offset - 8, len] = array
+    }
+    svid = case sat
+    when 1..32, 120..158, 193..202 # GPS, SBAS, QZSS
+      sat
+    when (0x100 + 1)..(0x100 + 32) # GLONASS
+      sat - 0x100 + 64 # => 65..96
+    else
+      next nil # TODO Galileo, Beidou, ...
+    end
+    
+    qi = 6
+    setter.call(:L1_CARRIER_PHASE,
+        8, 8, "E", proc{|v| next if v[0]; qi = 4; v.clear})
+    setter.call(:L1_PSEUDORANGE,
+        16, 8, "E", proc{|v| next if v[0]; qi = 0; v.clear})
+    setter.call(:L1_DOPPLER,
+        24, 4, "e", proc{|v| next if v[0]; qi = 0; v.clear})
+    setter.call([svid, qi], 28, 2)
+    setter.call(:L1_SIGNAL_STRENGTH_dBHz,
+        30, 1, nil, proc{|v| v.replace(v[0] ? [v[0].to_i] : [])})
+    setter.call(:L1_LOCK_SEC,
+        31, 1, nil, proc{|v| v.replace(v[0] ? [(v[0] < 0) ? 1 : 0] : [0])})
+
+    res
+  }.compact
+  ubx[6 + 6] = meas_ubx.size
+  ubx += meas_ubx.flatten(1)
+  ubx += [0, 0]
+  GPS_PVT::UBX::update(ubx).pack("C*")
+}
+
+gen_rawx = proc{|t_meas, meas| # Convert to RXM-RAWX(0x15)
+  meas = meas.to_hash
+  ubx = [0xB5, 0x62, 0x02, 0x15, 0, 0]
+  ubx += [t_meas.seconds, t_meas.week].pack("Ev").unpack("C*")
+  ubx += [0] * 6
+  meas_ubx = meas.collect{|sat, items|
+    res = [0] * 32
+    setter = proc{|value, offset, len, str, pre_proc|
+      array = case value
+      when Array; value
+      when Symbol
+        [items[GPS_PVT::GPS::Measurement.const_get(value)]]
+      else
+        next nil
+      end
+      pre_proc.call(array) if pre_proc
+      next if array.empty?
+      array = array.pack(str).unpack("C*") if str
+      res[offset - 16, len] = array
+    }
+    sys, svid = case sat
+    when 1..32 # GPS
+      [0, sat]
+    when 120..158 # SBAS
+      [1, sat]
+    when 193..202 # QZSS
+      [5, sat]
+    when (0x100 + 1)..(0x100 + 32) # GLONASS
+      setter.call(:L1_FREQUENCY,
+          39, 1, nil, proc{|v| v.replace(v[0] ? glonass_freq_ch.call(v[0]) : [7])})
+      [6, sat - 0x100]
+    else
+      next nil # TODO Galileo, Beidou, ...
+    end
+    setter.call([sys, svid], 36, 2)
+    
+    trk_stat = 0
+    setter.call(:L1_PSEUDORANGE,
+        16, 8, "E", proc{|v| v[0] ? (trk_stat |= 0x1) : v.clear})
+    setter.call(:L1_PSEUDORANGE_SIGMA,
+        43, 1, nil, proc{|v|
+          b = (Math::log2(v[0] / 1E-2).to_i & 0xF) rescue 0x8
+          v.replace((trk_stat & 0x1 == 0x1) ? [b] : [])
+        })
+    setter.call(:L1_DOPPLER, 32, 4, "e")
+    setter.call(:L1_DOPPLER_SIGMA,
+        45, 1, nil, proc{|v| v.replace(v[0] ? [Math::log2(v[0] / 2E-3).to_i & 0xF] : [0x8])})
+    setter.call(:L1_CARRIER_PHASE,
+        24, 8, "E", proc{|v| v[0] ? (trk_stat |= 0x2) : v.clear})
+    setter.call(:L1_CARRIER_PHASE_SIGMA,
+        44, 1, nil, proc{|v|
+          b = ((v[0] / 0.004).to_i & 0xF) rescue 0x8
+          v.replace((trk_stat & 0x2 == 0x2) ? [b] : [])
+        })
+    setter.call(:L1_SIGNAL_STRENGTH_dBHz,
+        42, 1, nil, proc{|v| v.replace(v[0] ? [v[0].to_i] : [])})
+    setter.call(:L1_LOCK_SEC,
+        40, 2, "v", proc{|v| v.replace(v[0] ? [(v[0] / 1E-3).to_i] : [])})
+    setter.call([trk_stat], 46, 1)
+
+    res
+  }.compact
+  ubx[6 + 11] = meas_ubx.size
+  ubx += meas_ubx.flatten(1)
+  ubx += [0, 0]
+  GPS_PVT::UBX::update(ubx).pack("C*")
+}
+
+gen = misc_options[:ubx_rawx] ? gen_rawx : gen_raw
+obs.each{|*item| print gen.call(*item)}

data/ext/ninja-scan-light/tool/algorithm/interpolate.h

@@ -34,6 +34,9 @@
 
 #include <cstddef>
 #include <vector>
+#include <map>
+#include <algorithm>
+#include <stdexcept>
 
 /*
  * perform Neville's interpolation (with derivative)
@@ -118,4 +121,107 @@ Ty interpolate_Neville(
   return interpolate_Neville(x_given, y_given, x, y_buf, n)[0];
 }
 
+template <class Tx, class Ty, class Tx_delta = Tx>
+struct InterpolatableSet {
+  struct condition_t {
+    std::size_t max_x_size; ///< maximum number of data used for interpolation
+    Tx_delta max_dx_range; ///< maximum acceptable absolute difference between x and x0
+  };
+
+  typedef typename std::vector<std::pair<Tx, Ty> > buffer_t;
+  buffer_t buf;
+  Tx x0, x_lower, x_upper;
+  std::vector<Tx_delta> dx;
+  bool ready;
+
+  InterpolatableSet() : x0(), dx(), ready(false) {}
+
+  /**
+   * update interpolation source
+   * @param force_update If true, update is forcibly performed irrespective of current state
+   * @param cnd condition for source data selection
+   */
+  InterpolatableSet &update(
+      const Tx &x, const std::map<Tx, Ty> &xy_table,
+      const condition_t &cnd,
+      const bool &force_update = false){
+
+    do{
+      if(force_update){break;}
+      if(dx.size() <= 2){break;}
+      if(x < x_lower){break;}
+      if(x > x_upper){break;}
+      return *this;
+    }while(false);
+
+    // If the 1st and 2nd nearest items are changed, then recalculate interpolation targets.
+    struct {
+      const Tx &x_base;
+      bool operator()(
+          const typename std::map<Tx, Ty>::value_type &rhs,
+          const typename std::map<Tx, Ty>::value_type &lhs) const {
+        return std::abs(rhs.first - x_base) < std::abs(lhs.first - x_base);
+      }
+    } cmp = {(x0 = x)};
+
+    buf.resize(cnd.max_x_size);
+    dx.clear();
+    // extract x where x0-dx_range <= x <= x0+dx_range, then sort by ascending order of |x-x0|
+    for(typename buffer_t::const_iterator
+          it(buf.begin()),
+          it_end(std::partial_sort_copy(
+            xy_table.lower_bound(x - cnd.max_dx_range),
+            xy_table.upper_bound(x + cnd.max_dx_range),
+            buf.begin(), buf.end(), cmp));
+        it != it_end; ++it){
+      dx.push_back(it->first - x0);
+    }
+    if(dx.size() >= 2){ // calculate a necessary condition to update samples for new x
+      // According to Nth nearest points, x range in which the order of extracted samples
+      // is retained can be calculated.
+      bool ascending(buf[0].first <= buf[1].first);
+      Tx &xa(ascending ? x_lower : x_upper), &xb(ascending ? x_upper : x_lower);
+      xa = x0;
+      for(std::size_t i(2); i < buf.size(); ++i){
+        if(ascending ? (dx[1] <= dx[i]) : (dx[1] >= dx[i])){continue;}
+        xa = x0 + (dx[1] + dx[i]) / 2;
+        break;
+      }
+      xb = x0 + (dx[0] + dx[1]) / 2;
+    }
+    ready = (dx.size() >= cnd.max_x_size);
+
+    return *this;
+  }
+
+  template <class Ty2, class Ty2_Iterator>
+  Ty2 interpolate2(
+      const Tx &x, const Ty2_Iterator &y,
+      Ty2 *derivative = NULL) const {
+    int order(dx.size() - 1);
+    do{
+      if(order > 0){break;}
+      if((order == 0) && (!derivative)){return y[0];}
+      throw std::range_error("Insufficient records for interpolation");
+    }while(false);
+    std::vector<Ty2> y_buf(order), dy_buf(order);
+    interpolate_Neville(
+        dx, y, x - x0, y_buf, order,
+        &dy_buf, derivative ? 1 : 0);
+    if(derivative){*derivative = dy_buf[0];}
+    return y_buf[0];
+  }
+
+  Ty interpolate(const Tx &x, Ty *derivative = NULL) const {
+    struct second_iterator : public buffer_t::const_iterator {
+      second_iterator(const typename buffer_t::const_iterator &it)
+          : buffer_t::const_iterator(it) {}
+      const Ty &operator[](const int &n) const {
+        return buffer_t::const_iterator::operator[](n).second;
+      }
+    } y_it(buf.begin());
+    return interpolate2(x, y_it, derivative);
+  }
+};
+
 #endif /* __INTERPOLATE_H__ */

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

@@ -41,7 +41,6 @@
 #include <cstring>
 #include <map>
 #include <set>
-#include <algorithm>
 #include <stdexcept>
 
 #include "util/text_helper.h"
@@ -61,22 +60,15 @@ struct SP3_Product {
     typedef std::map<GPS_Time<FloatT>, prop_t> history_t;
     history_t pos_history;
     history_t vel_history;
-
-    static const struct interpolate_cnd_t {
-      std::size_t max_epochs; ///< maximum number of epochs used for interpolation
-      FloatT max_delta_t; ///< maximum acceptable absolute time difference between t and epoch
-    } interpolate_cnd_default;
+    
+    typedef InterpolatableSet<GPS_Time<FloatT>, prop_t, FloatT> interpolator_t;
+    typedef typename interpolator_t::condition_t interpolate_cnd_t;
+    static const interpolate_cnd_t interpolate_cnd_default;
 
     mutable struct {
-      struct target_t {
-
-        typedef typename std::vector<std::pair<GPS_Time<FloatT>, prop_t> > buf_t;
-        buf_t buf;
-        GPS_Time<FloatT> t0;
-        std::vector<FloatT> dt;
-        bool updated_full_cnd;
+      struct target_t : public interpolator_t {
 
-        target_t() : t0(0, 0), updated_full_cnd(false) {}
+        target_t() : interpolator_t() {}
 
         /**
          * update interpolation source
@@ -88,58 +80,13 @@ struct SP3_Product {
             const bool &force_update = false,
             const interpolate_cnd_t &cnd = interpolate_cnd_default){
 
-          FloatT t_diff(t0 - t);
-          if((!force_update)
-              && ((std::abs(t_diff) <= 10)
-                || ((dt.size() >= 2)
-                  && (std::abs(t_diff + dt[0]) <= std::abs(t_diff + dt[1])))) ){
-            return *this;
-          }
-
-          // If the 1st and 2nd nearest epochs are changed, then recalculate interpolation targets.
-          struct {
-            const GPS_Time<FloatT> &t_base;
-            bool operator()(
-                const typename history_t::value_type &rhs,
-                const typename history_t::value_type &lhs) const {
-              return std::abs(rhs.first - t_base) < std::abs(lhs.first - t_base);
-            }
-          } cmp = {(t0 = t)};
-
-          buf.resize(cnd.max_epochs);
-          dt.clear();
-          // extract t where t0-dt <= t <= t0+dt, then sort by ascending order of |t-t0|
-          for(typename buf_t::const_iterator
-                it(buf.begin()),
-                it_end(std::partial_sort_copy(
-                  history.lower_bound(t - cnd.max_delta_t),
-                  history.upper_bound(t + cnd.max_delta_t),
-                  buf.begin(), buf.end(), cmp));
-              it != it_end; ++it){
-            dt.push_back(it->first - t0);
-          }
-          updated_full_cnd = (dt.size() >= cnd.max_epochs);
-
+          if((!force_update) && (std::abs(t - interpolator_t::x0) <= 10)){return *this;}
+          interpolator_t::update(t, history, cnd, force_update);
           return *this;
         }
 
-        template <class Ty, class Ty_Array>
-        Ty interpolate(
-            const GPS_Time<FloatT> &t, const Ty_Array &y,
-            Ty *derivative = NULL) const {
-          int order(dt.size() - 1);
-          do{
-            if(order > 0){break;}
-            if((order == 0) && (!derivative)){return y[0];}
-            throw std::range_error("Insufficient records for interpolation");
-          }while(false);
-          std::vector<Ty> y_buf(order), dy_buf(order);
-          interpolate_Neville(
-              dt, y, t - t0, y_buf, order,
-              &dy_buf, derivative ? 1 : 0);
-          if(derivative){*derivative = dy_buf[0];}
-          return y_buf[0];
-        }
+        typedef typename interpolator_t::buffer_t buf_t;
+
         Vector3<FloatT> interpolate_xyz(
             const GPS_Time<FloatT> &t,
             Vector3<FloatT> *derivative = NULL) const {
@@ -149,8 +96,8 @@ struct SP3_Product {
             const Vector3<FloatT> &operator[](const int &n) const {
               return buf_t::const_iterator::operator[](n).second.xyz;
             }
-          } xyz(buf.begin());
-          return interpolate(t, xyz, derivative);
+          } xyz(interpolator_t::buf.begin());
+          return interpolator_t::interpolate2(t, xyz, derivative);
         }
         FloatT interpolate_clk(
             const GPS_Time<FloatT> &t,
@@ -161,8 +108,8 @@ struct SP3_Product {
             const FloatT &operator[](const int &n) const {
               return buf_t::const_iterator::operator[](n).second.clk;
             }
-          } clk(buf.begin());
-          return interpolate(t, clk, derivative);
+          } clk(interpolator_t::buf.begin());
+          return interpolator_t::interpolate2(t, clk, derivative);
         }
       } pos_clk, vel_rate;
     } subset;
@@ -175,7 +122,7 @@ struct SP3_Product {
     bool precheck(const GPS_Time<FloatT> &t) const {
       // Only position/clock is checked, because velocity/rate can be calculated based on pos/clk.
       subset.pos_clk.update(t, pos_history);
-      return subset.pos_clk.updated_full_cnd;
+      return subset.pos_clk.ready;
     }
 
     typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t
@@ -381,8 +328,11 @@ static typename GPS_Solver_Base<FloatT>::satellite_t select_ ## sys( \
 template <class FloatT>
 const typename SP3_Product<FloatT>::per_satellite_t::interpolate_cnd_t
     SP3_Product<FloatT>::per_satellite_t::interpolate_cnd_default = {
-  9, // max_epochs
-  60 * 60 * 2, // max_delta_t, default is 2 hr = 15 min interval records; (2 hr * 2 / (9 - 1) = 15 min)
+  9, // maximum number of epochs used for interpolation
+  /* maximum acceptable absolute time difference between t and epoch,
+   * default is 2 hr = 15 min interval records; (2 hr * 2 / (9 - 1) = 15 min)
+   */
+  60 * 60 * 2,
 };
 
 template <class FloatT>

data/gps_pvt.gemspec

@@ -53,6 +53,9 @@ Gem::Specification.new do |spec|
     }.flatten
   }.call
 
+  spec.rdoc_options << '--exclude=ext/ninja-scan-light'
+  spec.extra_rdoc_files = []
+
   # Uncomment to register a new dependency of your gem
   # spec.add_dependency "example-gem", "~> 1.0"
   spec.add_development_dependency "rake"

data/lib/gps_pvt/receiver.rb

@@ -410,7 +410,7 @@ class Receiver
               @solver.sbas_space_node.ionospheric_grid_points(prn)].each{|str|
             $stderr.puts str
           } if @debug[:SBAS_IGP] 
-        end
+        end if t_meas
       when :GLONASS
         next unless eph = eph_glonass_list[prn]
         leap_sec = @solver.gps_space_node.is_valid_utc ? 
@@ -508,10 +508,10 @@ class Receiver
           {
             :L1_PSEUDORANGE => [16, 8, "E", proc{|v| (trk_stat & 0x1 == 0x1) ? v : nil}],
             :L1_PSEUDORANGE_SIGMA => [43, 1, nil, proc{|v|
-              (trk_stat & 0x1 == 0x1) ? (1E-2 * (v[0] & 0xF)) : nil
+              (trk_stat & 0x1 == 0x1) ? (1E-2 * (1 << (v[0] & 0xF))) : nil
             }],
             :L1_DOPPLER => [32, 4, "e"],
-            :L1_DOPPLER_SIGMA => [45, 1, nil, proc{|v| 2E-3 * (v[0] & 0xF)}],
+            :L1_DOPPLER_SIGMA => [45, 1, nil, proc{|v| 2E-3 * (1 << (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

data/lib/gps_pvt/version.rb

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

metadata

@@ -1,14 +1,14 @@
 --- !ruby/object:Gem::Specification
 name: gps_pvt
 version: !ruby/object:Gem::Version
-  version: 0.6.2
+  version: 0.6.3
 platform: ruby
 authors:
 - fenrir(M.Naruoka)
 autorequire: 
 bindir: exe
 cert_chain: []
-date: 2022-08-03 00:00:00.000000000 Z
+date: 2022-08-19 00:00:00.000000000 Z
 dependencies:
 - !ruby/object:Gem::Dependency
   name: rake
@@ -44,6 +44,7 @@ email:
 - fenrir.naru@gmail.com
 executables:
 - gps_pvt
+- to_ubx
 extensions:
 - ext/gps_pvt/extconf.rb
 extra_rdoc_files: []
@@ -57,6 +58,7 @@ files:
 - bin/console
 - bin/setup
 - exe/gps_pvt
+- exe/to_ubx
 - ext/gps_pvt/Coordinate/Coordinate_wrap.cxx
 - ext/gps_pvt/GPS/GPS_wrap.cxx
 - ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx
@@ -109,7 +111,8 @@ metadata:
   homepage_uri: https://github.com/fenrir-naru/gps_pvt
   source_code_uri: https://github.com/fenrir-naru/gps_pvt
 post_install_message: 
-rdoc_options: []
+rdoc_options:
+- "--exclude=ext/ninja-scan-light"
 require_paths:
 - lib
 required_ruby_version: !ruby/object:Gem::Requirement