gps_pvt 0.7.2 → 0.8.1

data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb

@@ -765,9 +765,70 @@ __RINEX_CLK_TEXT__
       [:alpha, :beta].each{|k|
         puts "Iono #{k}: #{sn.iono_utc.send(k)}"
       }
+      proc{|raw|
+        expect(raw.size).to eq(10)
+        puts "Raw(IONO): #{raw.collect{|v| "0x%08X" % [v]}.join(', ')}"
+        iono2 = GPS::Ionospheric_UTC_Parameters::parse(raw)
+        [:alpha, :beta].collect{|f|
+          sn.iono_utc.send(f).zip(iono2.send(f))
+        }.flatten(1).zip([-30, -27, -24, -24, 11, 14, 16, 16]).each{|(a, b), sf|
+          expect(a).to be_within((2 ** sf) * 2).of(b)
+        }
+        [
+          [:A1, 2 ** -50], [:A0, 2 ** -30],
+        ].each{|k, sf|
+          #p [k, sf.to_f, sn.iono_utc.send(k) - iono2.send(k), sn.iono_utc.send(k), iono2.send(k)]
+          expect(sn.iono_utc.send(k)).to be_within((sf || 1).to_f * 2).of(iono2.send(k))
+        }
+      }.call(sn.iono_utc.dump(t_meas))
       
       meas.each{|prn, k, v|
         eph = sn.ephemeris(prn)
+        expect(eph.valid?(t_meas)).to eq(true)
+        sc2rad = 3.1415926535898
+        proc{|raw|
+          expect(raw.size).to eq(30)
+          eph2 = GPS::Ephemeris::new
+          raw.each_slice(10).with_index{|subframe, i|
+            puts "Raw(PRN:#{prn},SF:#{i+1}): #{subframe.collect{|v| "0x%08X" % [v]}.join(', ')}"
+            eph2.parse(subframe)
+          }
+          expect(eph.WN % 1024).to be(eph2.WN % 1024)
+          [
+            #:URA,
+            :SV_health, :iodc,
+            [:t_GD, 2 ** -31], [:t_oc, 2 ** 4], # SF1
+            [:a_f0, 2 ** -31], [:a_f1, 2 ** -43], [:a_f2, 2 ** -55], # SF1
+            :iode, # SF2
+            [:c_rs, 2 ** -5], [:delta_n, sc2rad * 2 ** -43], # SF2
+            [:M0, sc2rad * 2 ** -31], [:c_uc, 2 ** -29], [:e, 2 ** -33], # SF2
+            [:c_us, 2 ** -29], [:sqrt_A, 2 ** -19], [:t_oe, 2 ** 4], # SF2
+            :fit_interval, # SF2
+            [:c_ic, 2 ** -29], [:Omega0, sc2rad * 2 ** -31], [:c_is, 2 ** -29], # SF3
+            [:i0, sc2rad * 2 ** -31], [:c_rc, 2 ** -5], [:omega, sc2rad * 2 ** -31], # SF3
+            [:dot_Omega0, sc2rad * 2 ** -43], [:dot_i0, sc2rad * 2 ** -43], # SF3
+          ].each{|k, sf|
+            #p [k, sf.to_f, eph.send(k) - eph2.send(k), eph.send(k), eph2.send(k)]
+            expect(eph.send(k)).to be_within((sf || 1).to_f * 2).of(eph2.send(k))
+          }
+        }.call(eph.dump(t_meas))
+        proc{|raw| # Almanac -> Ephemeris
+          expect(raw.size).to eq(10)
+          puts "Raw(PRN:#{prn},Almanac): #{raw.collect{|v| "0x%08X" % [v]}.join(', ')}"
+          eph2 = GPS::Ephemeris::new
+          eph2.parse_almanac(raw)
+          [
+            :SV_health,
+            [:t_oc, 2 ** 12], [:a_f0, 2 ** -20], [:a_f1, 2 ** -38], 
+            [:M0, sc2rad * 2 ** -23], [:e, 2 ** -21],
+            [:sqrt_A, 2 ** -11], [:t_oe, 2 ** 12],
+            [:Omega0, sc2rad * 2 ** -23], [:i0, sc2rad * 2 ** -19],
+            [:omega, sc2rad * 2 ** -23], [:dot_Omega0, sc2rad * 2 ** -38],
+          ].each{|k, sf|
+            #p [k, sf.to_f, eph.send(k) - eph2.send(k), eph.send(k), eph2.send(k)]
+            expect(eph.send(k)).to be_within((sf || 1).to_f * 2).of(eph2.send(k))
+          }
+        }.call(eph.dump_almanac(t_meas))
         puts "XYZ(PRN:#{prn}): #{eph.constellation(t_meas)[0].to_a} (iodc: #{eph.iodc}, iode: #{eph.iode})"
       }
       

data/ext/ninja-scan-light/tool/util/bit_counter.h

@@ -0,0 +1,84 @@
+#ifndef __BIT_COUNTER__
+#define __BIT_COUNTER__
+
+#include <climits>
+
+using namespace std;
+
+/**
+ * Counting bits utility
+ */
+template <class T>
+struct BitCounter {
+
+  static const int num_of_bits = CHAR_BIT * sizeof(T);
+
+  private:
+    template <int Interval>
+    static T make_mask(){
+      /*
+       * @see http://www.nminoru.jp/~nminoru/programming/bitcount.html
+       * 
+       * 01010101010101010101010101010101  1 32
+       * 00110011001100110011001100110011  2 32
+       * 00001111000011110000111100001111  4 32
+       * 00000000111111110000000011111111  8 32
+       * 00000000000000001111111111111111 16 32 
+       */
+
+      T mask(1);
+      for(int i(1); i < Interval; i++){
+        mask <<= 1;
+        mask |= 0x01;
+      }
+      /*
+      // Equivalent to the following if T is built-in type
+      mask <<= Interval;
+      mask = (mask & (-mask)) - 1;
+      */ 
+
+      T res(mask);
+      for(unsigned int i(1); i < num_of_bits / Interval / 2; i++){
+        res <<= Interval;
+        res <<= Interval;
+        res |= mask;
+      }
+      return res;
+    }
+
+    template <int Interval, class U = void>
+    struct count_loop {
+      static T run(T bits){
+        bits = count_loop<(Interval >> 1)>::run(bits);
+        static const T mask(make_mask<Interval>());
+        // bit shift and add operators are required at least
+        return (bits & mask) + ((bits >> Interval) & mask);
+      }
+    };
+    template <class U>
+    struct count_loop<1, U> {
+      static T run(T bits){
+        static const T mask(make_mask<1>());
+        return (bits & mask) + ((bits >> 1) & mask);
+      }
+    };
+
+  public:
+    /**
+     * Count bits in a value
+     * @param results
+     */
+    static T count(const T &v) {
+      return count_loop<(num_of_bits >> 1)>::run(v);
+    }
+
+    /**
+     * Count rightmost zeros before the first one (Number of trailing zeros)
+     * @param bits results
+     */
+    static T ntz(const T &v) {
+      return count((T)((~v) & (v - 1)));
+    }
+};
+
+#endif /* __BIT_COUNTER__ */

data/gps_pvt.gemspec

@@ -58,6 +58,7 @@ Gem::Specification.new do |spec|
 
   # Uncomment to register a new dependency of your gem
   # spec.add_dependency "example-gem", "~> 1.0"
+  spec.add_dependency "rubyserial"
   spec.add_development_dependency "rake"
   spec.add_development_dependency "rake-compiler"
 

data/lib/gps_pvt/receiver/extension.rb

@@ -0,0 +1,51 @@
+=begin
+Receiver extension
+=end
+
+module GPS_PVT
+class Receiver
+  
+  # shortcut to access ephemeris registered in receiver 
+  def ephemeris(t, sys, prn)
+    eph = case sys
+    when :GPS, :QZSS
+      critical{
+        @solver.gps_space_node.update_all_ephemeris(t)
+        @solver.gps_space_node.ephemeris(prn)
+      }
+    when :SBAS
+      critical{
+        @solver.sbas_space_node.update_all_ephemeris(t)
+        @solver.sbas_space_node.ephemeris(prn)
+      }
+    when :GLONASS
+      critical{
+        @solver.glonass_space_node.update_all_ephemeris(t)
+        @solver.glonass_space_node.ephemeris(prn)
+      }
+    else
+      return nil
+    end
+    return (eph.valid?(t) ? eph : nil)
+  end
+
+  def attach_online_ephemeris(uri_template = nil)
+    if (!uri_template) || (uri_template =~ /^\s*$/) then
+      uri_template = "ftp://gssc.esa.int/gnss/data/daily/%Y/brdc/BRDC00IGS_R_%Y%j0000_01D_MN.rnx.gz"
+    end
+    loader = proc{|t_meas|
+      utc = Time::utc(*t_meas.c_tm)
+      uri = URI::parse(utc.strftime(uri_template))
+      self.parse_rinex_nav(uri)
+      uri
+    }
+    run_orig = self.method(:run)
+    eph_list = {}
+    self.define_singleton_method(:run){|meas, t_meas, *args|
+      w_d = [t_meas.week, (t_meas.seconds.to_i / 86400)]
+      eph_list[w_d] ||= loader.call(t_meas)
+      run_orig.call(meas, t_meas, *args)
+    }
+  end
+end
+end

data/lib/gps_pvt/receiver.rb

@@ -3,7 +3,7 @@ Receiver class to be an top level interface to a user
 (The origin is ninja-scan-light/tool/misc/receiver_debug.rb)
 =end
 
-require_relative 'GPS'
+require 'gps_pvt/GPS' # in case GPS.so is generated under ext/gps_pvt
 require_relative 'util'
 
 module GPS_PVT
@@ -154,6 +154,7 @@ class Receiver
       :skip_exclusion => true, # default is to skip fault exclusion calculation
     }
     @debug = {}
+    @semaphore = Mutex::new
     solver_opts = [:gps_options, :sbas_options, :glonass_options].collect{|target|
       @solver.send(target)
     }
@@ -298,6 +299,24 @@ class Receiver
       :meas => Receiver::meas_items(output_options),
     }
   end
+  
+  def critical(&b)
+    begin
+      @semaphore.synchronize{b.call}
+    rescue ThreadError # recovery from deadlock
+      b.call
+    end
+  end
+  
+  class << self
+    def make_critical(fname)
+      f_orig = instance_method(fname)
+      define_method(fname){|*args, &b|
+        critical{f_orig.bind(self).call(*args, &b)}
+      }
+    end
+    private :make_critical
+  end
 
   GPS::Measurement.class_eval{
     proc{
@@ -331,7 +350,7 @@ class Receiver
 =end
 
     #@solver.gps_space_node.update_all_ephemeris(t_meas) # internally called in the following solver.solve
-    pvt = @solver.solve(meas, t_meas)
+    pvt = critical{@solver.solve(meas, t_meas)}
     pvt.define_singleton_method(:rel_ENU){
       Coordinate::ENU::relative(xyz, ref_pos)
     } if (ref_pos && pvt.position_solved?)
@@ -378,60 +397,59 @@ class Receiver
     }
   }
   
-  proc{
-    eph_list = Hash[*((1..32).to_a + (193..202).to_a).collect{|prn|
+  def register_ephemeris(t_meas, sys, prn, bcast_data, *options)
+    @eph_list ||= Hash[*((1..32).to_a + (193..202).to_a).collect{|prn|
       eph = GPS::Ephemeris::new
       eph.svid = prn
       [prn, eph]
     }.flatten(1)]
-    eph_glonass_list = Hash[*(1..24).collect{|num|
+    @eph_glonass_list ||= Hash[*(1..24).collect{|num|
       eph = GPS::Ephemeris_GLONASS::new
       eph.svid = num
       [num, eph]
     }.flatten(1)]
-    define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data, *options|
-      opt = options[0] || {}
-      case sys
-      when :GPS, :QZSS
-        next unless eph = eph_list[prn]
-        sn = @solver.gps_space_node
-        subframe, iodc_or_iode = eph.parse(bcast_data)
-        if iodc_or_iode < 0 then
-          begin
-            sn.update_iono_utc(
-                GPS::Ionospheric_UTC_Parameters::parse(bcast_data))
-            [:alpha, :beta].each{|k|
-              $stderr.puts "Iono #{k}: #{sn.iono_utc.send(k)}"
-            } if false
-          rescue
-          end
-          next
+    opt = options[0] || {}
+    case sys
+    when :GPS, :QZSS
+      return unless eph = @eph_list[prn]
+      sn = @solver.gps_space_node
+      subframe, iodc_or_iode = eph.parse(bcast_data)
+      if iodc_or_iode < 0 then
+        begin
+          sn.update_iono_utc(
+              GPS::Ionospheric_UTC_Parameters::parse(bcast_data))
+          [:alpha, :beta].each{|k|
+            $stderr.puts "Iono #{k}: #{sn.iono_utc.send(k)}"
+          } if false
+        rescue
         end
-        if t_meas and eph.consistent? then
-          eph.WN = ((t_meas.week / 1024).to_i * 1024) + (eph.WN % 1024)
-          sn.register_ephemeris(prn, eph)
-          eph.invalidate
-        end
-      when :SBAS
-        case @solver.sbas_space_node.decode_message(bcast_data[0..7], prn, t_meas)
-        when 26
-          ['', "IGP broadcasted by PRN#{prn} @ #{Time::utc(*t_meas.c_tm)}",
-              @solver.sbas_space_node.ionospheric_grid_points(prn)].each{|str|
-            $stderr.puts str
-          } if @debug[:SBAS_IGP] 
-        end if t_meas
-      when :GLONASS
-        next unless eph = eph_glonass_list[prn]
-        leap_sec = @solver.gps_space_node.is_valid_utc ? 
-            @solver.gps_space_node.iono_utc.delta_t_LS :
-            GPS::Time::guess_leap_seconds(t_meas)
-        next unless eph.parse(bcast_data[0..3], leap_sec)
-        eph.freq_ch = opt[:freq_ch] || 0
-        @solver.glonass_space_node.register_ephemeris(prn, eph)
+        return
+      end
+      if t_meas and eph.consistent? then
+        eph.WN = ((t_meas.week / 1024).to_i * 1024) + (eph.WN % 1024)
+        sn.register_ephemeris(prn, eph)
         eph.invalidate
       end
-    }
-  }.call
+    when :SBAS
+      case @solver.sbas_space_node.decode_message(bcast_data[0..7], prn, t_meas)
+      when 26
+        ['', "IGP broadcasted by PRN#{prn} @ #{Time::utc(*t_meas.c_tm)}",
+            @solver.sbas_space_node.ionospheric_grid_points(prn)].each{|str|
+          $stderr.puts str
+        } if @debug[:SBAS_IGP]
+      end if t_meas
+    when :GLONASS
+      return unless eph = @eph_glonass_list[prn]
+      leap_sec = @solver.gps_space_node.is_valid_utc ? 
+          @solver.gps_space_node.iono_utc.delta_t_LS :
+          GPS::Time::guess_leap_seconds(t_meas)
+      return unless eph.parse(bcast_data[0..3], leap_sec)
+      eph.freq_ch = opt[:freq_ch] || 0
+      @solver.glonass_space_node.register_ephemeris(prn, eph)
+      eph.invalidate
+    end
+  end
+  make_critical :register_ephemeris
   
   def parse_ubx(ubx_fname, &b)
     $stderr.print "Reading UBX file (%s) "%[ubx_fname]
@@ -505,6 +523,8 @@ class Receiver
             v
           }
           sys, svid = gnss_serial.call(*loader.call(36, 2).reverse)
+          # sigID check to restrict signal to L1 if version(>0); @see UBX-18010854
+          next if (packet[6 + 13] != 0) && (loader.call(38, 1, "C") != 0)
           case sys
           when :GPS, :SBAS, :QZSS;
           when :GLONASS
@@ -538,10 +558,13 @@ class Receiver
         register_ephemeris(
             t_meas,
             sys, svid,
-            packet.slice(6 + 2, 40).each_slice(4).collect{|v|
-              res = v.pack("C*").unpack("V")[0]
-              (sys == :GPS) ? ((res & 0xFFFFFF) << 6) : res
-            })
+            proc{|data|
+              case sys # adjust padding
+              when :GPS; data.collect!{|v| (v & 0xFFFFFF) << 6}
+              when :SBAS; data[7] <<= 6
+              end
+              data
+            }.call(packet.slice(6 + 2, 40).pack("C*").unpack("V*")))
       when [0x02, 0x13] # RXM-SFRBX
         sys, svid = gnss_serial.call(packet[6 + 1], packet[6])
         opt = {}
@@ -549,9 +572,7 @@ class Receiver
         register_ephemeris(
             t_meas,
             sys, svid,
-            packet.slice(6 + 8, 4 * packet[6 + 4]).each_slice(4).collect{|v|
-              v.pack("C*").unpack("V")[0]
-            }, opt)
+            packet.slice(6 + 8, 4 * packet[6 + 4]).pack("C*").unpack("V*"), opt)
       end
     }
     $stderr.puts ", found packets are %s"%[ubx_kind.inspect]
@@ -564,7 +585,7 @@ class Receiver
       @solver.sbas_space_node,
       @solver.glonass_space_node,
     ].inject(0){|res, sn|
-      loaded_items = sn.send(:read, fname)
+      loaded_items = critical{sn.send(:read, fname)}
       raise "Format error! (Not RINEX) #{src}" if loaded_items < 0
       res + loaded_items
     }
@@ -648,7 +669,7 @@ class Receiver
       next unless /^SYS_(?!SYSTEMS)(.*)/ =~ sys.to_s
       idx, sys_name = [@sp3.class.const_get(sys), $1]
       next unless sats[idx] > 0
-      next unless @sp3.push(@solver, idx)
+      next unless critical{@sp3.push(@solver, idx)}
       $stderr.puts "Change ephemeris source of #{sys_name} to SP3" 
     }
   end
@@ -656,7 +677,7 @@ class Receiver
   def attach_antex(src)
     fname = Util::get_txt(src)
     raise "Specify SP3 before ANTEX application!" unless @sp3
-    applied_items = @sp3.apply_antex(fname)
+    applied_items = critical{@sp3.apply_antex(fname)}
     raise "Format error! (Not ANTEX) #{src}" unless applied_items >= 0
     $stderr.puts "SP3 correction with ANTEX file (%s): %d items have been processed."%[src, applied_items]
   end
@@ -672,9 +693,11 @@ class Receiver
       next unless /^SYS_(?!SYSTEMS)(.*)/ =~ sys.to_s
       idx, sys_name = [@clk.class.const_get(sys), $1]
       next unless sats[idx] > 0
-      next unless @clk.push(@solver, idx)
+      next unless critical{@clk.push(@solver, idx)}
       $stderr.puts "Change clock error source of #{sys_name} to RINEX clock" 
     }
   end
 end
 end
+
+require_relative 'receiver/extension'

data/lib/gps_pvt/util.rb

@@ -1,7 +1,39 @@
-require 'open-uri'
 require 'tempfile'
 require 'uri'
 
+proc{
+  # port[:baudrate], baudrate default is 115200
+  Serial.class_eval{
+    const_set(:SPEC, 
+        if RubySerial::ON_WINDOWS then
+          %r{^(?:\\\\.\\)?(COM\d+)(?::(\d+))?$}
+        elsif RubySerial::ON_LINUX then
+          %r{^(/dev/tty[^:]+)(?::(\d+))?$}
+        else
+          nil
+        end)
+  }
+  Serial.class_eval{
+    read_orig = instance_method(:read)
+    define_method(:read){|len|
+      buf = ''
+      f = read_orig.bind(self)
+      buf += f.call(len - buf.size) while buf.size < len
+      buf
+    }
+    def eof?; false; end
+  }
+  Kernel.instance_eval{
+    open_orig = method(:open)
+    define_method(:open){|*args, &b|
+      return open_orig.call(*args, &b) unless Serial::SPEC =~ args[0]
+      Serial::new($1, $2 ? $2.to_i : 115200)
+    }
+  }
+}.call if require 'rubyserial'
+
+require 'open-uri'
+
 module GPS_PVT
 module Util
   class << self

data/lib/gps_pvt/version.rb

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

metadata

@@ -1,15 +1,29 @@
 --- !ruby/object:Gem::Specification
 name: gps_pvt
 version: !ruby/object:Gem::Version
-  version: 0.7.2
+  version: 0.8.1
 platform: ruby
 authors:
 - fenrir(M.Naruoka)
 autorequire: 
 bindir: exe
 cert_chain: []
-date: 2022-09-07 00:00:00.000000000 Z
+date: 2022-11-17 00:00:00.000000000 Z
 dependencies:
+- !ruby/object:Gem::Dependency
+  name: rubyserial
+  requirement: !ruby/object:Gem::Requirement
+    requirements:
+    - - ">="
+      - !ruby/object:Gem::Version
+        version: '0'
+  type: :runtime
+  prerelease: false
+  version_requirements: !ruby/object:Gem::Requirement
+    requirements:
+    - - ">="
+      - !ruby/object:Gem::Version
+        version: '0'
 - !ruby/object:Gem::Dependency
   name: rake
   requirement: !ruby/object:Gem::Requirement
@@ -98,11 +112,13 @@ 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
+- ext/ninja-scan-light/tool/util/bit_counter.h
 - ext/ninja-scan-light/tool/util/text_helper.h
 - gps_pvt.gemspec
 - gps_pvt.rbs
 - lib/gps_pvt.rb
 - lib/gps_pvt/receiver.rb
+- lib/gps_pvt/receiver/extension.rb
 - lib/gps_pvt/ubx.rb
 - lib/gps_pvt/util.rb
 - lib/gps_pvt/version.rb