gps_pvt 0.2.3 → 0.4.0

data/lib/gps_pvt/receiver.rb

@@ -7,15 +7,22 @@ require_relative 'GPS'
 
 module GPS_PVT
 class Receiver
+
+  GPS::Time.send(:define_method, :utc){ # send as work around of old Ruby
+    res = c_tm(GPS::Time::guess_leap_seconds(self))
+    res[-1] += (seconds % 1)
+    res
+  }
+
   def self.pvt_items(opt = {})
     opt = {
       :system => [[:GPS, 1..32]],
       :satellites => (1..32).to_a,
     }.merge(opt)
     [[
-      [:week, :itow_rcv, :year, :month, :mday, :hour, :min, :sec],
+      [:week, :itow_rcv, :year, :month, :mday, :hour, :min, :sec_rcv_UTC],
       proc{|pvt|
-        [:week, :seconds, :c_tm].collect{|f| pvt.receiver_time.send(f)}.flatten
+        [:week, :seconds, :utc].collect{|f| pvt.receiver_time.send(f)}.flatten
       }
     ]] + [[
       [:receiver_clock_error_meter, :longitude, :latitude, :height, :rel_E, :rel_N, :rel_U],
@@ -47,23 +54,31 @@ class Receiver
     ]] + [
       [:used_satellites, proc{|pvt| pvt.used_satellites}],
     ] + opt[:system].collect{|sys, range|
-      bit_flip = if range.kind_of?(Array) then
-        proc{|res, i|
+      range = range.kind_of?(Array) ? proc{
+        # check whether inputs can be converted to Range
+        next nil if range.empty?
+        a, b = range.minmax
+        ((b - a) == (range.length - 1)) ? (a..b) : range
+      }.call : range
+      next nil unless range
+      bit_flip, label = case range
+      when Array
+        [proc{|res, i|
           res[i] = "1" if i = range.index(i)
           res
-        }
-      else # expect Range
+        }, range.collect{|pen| pen & 0xFF}.reverse.join('+')]
+      when Range
         base_prn = range.min
-        proc{|res, i|
+        [proc{|res, i|
           res[i - base_prn] = "1" if range.include?(i)
           res
-        }
+        }, [:max, :min].collect{|f| range.send(f) & 0xFF}.join('..')]
       end
-      ["#{sys}_PRN", proc{|pvt|
+      ["#{sys}_PRN(#{label})", proc{|pvt|
         pvt.used_satellite_list.inject("0" * range.size, &bit_flip) \
             .scan(/.{1,8}/).join('_').reverse
       }]
-    } + [[
+    }.compact + [[
       opt[:satellites].collect{|prn, label|
         [:range_residual, :weight, :azimuth, :elevation, :slopeH, :slopeV].collect{|str|
           "#{str}(#{label || prn})"
@@ -138,8 +153,11 @@ class Receiver
       rel_prop
     }
     @debug = {}
-    [:gps_options, :sbas_options].each{|target|
-      opt = @solver.send(target) # default solver options
+    solver_opts = [:gps_options, :sbas_options, :glonass_options].collect{|target|
+      @solver.send(target)
+    }
+    solver_opts.each{|opt|
+      # default solver options
       opt.elevation_mask = 0.0 / 180 * Math::PI # 0 deg (use satellite over horizon)
       opt.residual_mask = 1E4 # 10 km (without residual filter, practically)
     }
@@ -168,6 +186,13 @@ class Receiver
         when :identical # same as default
           next true
         end
+      when :elevation_mask_deg
+        raise "Unknown elevation mask angle: #{v}" unless elv_deg = (Float(v) rescue nil)
+        $stderr.puts "Elevation mask: #{elv_deg} deg"
+        solver_opts.each{|opt|
+          opt.elevation_mask = elv_deg / 180 * Math::PI # 0 deg (use satellite over horizon)
+        }
+        next true
       when :base_station
         crd, sys = v.split(/ *, */).collect.with_index{|item, i|
           case item
@@ -219,12 +244,14 @@ class Receiver
               i = -1
               output_options[:system] << [sys_target, []]
             else
-              output_options[:system][i].reject!{|prn| prns.include?(prn)}
+              output_options[:system][i][1].reject!{|prn| prns.include?(prn)}
             end
             output_options[:satellites].reject!{|prn, label| prns.include?(prn)}
             if mode == :include then
               output_options[:system][i][1] += prns
+              output_options[:system][i][1].sort!
               output_options[:satellites] += (labels ? prns.zip(labels) : prns)
+              output_options[:satellites].sort!{|a, b| [a].flatten[0] <=> [b].flatten[0]}
             end
           }
           check_sys_svid = proc{|sys_target, range_in_sys, offset|
@@ -241,6 +268,11 @@ class Receiver
             prns.each{|prn| @solver.sbas_options.send(mode, prn)}
           elsif check_sys_svid.call(:QZSS, 193..202) then
             [svid || (193..202).to_a].flatten.each{|prn| @solver.gps_options.send(mode, prn)}
+          elsif check_sys_svid.call(:GLONASS, 1..24, 0x100) then
+            prns = [svid || (1..24).to_a].flatten.collect{|i| (i & 0xFF) + 0x100}
+            labels = prns.collect{|prn| "GLONASS:#{prn & 0xFF}"}
+            update_output.call(:GLONASS, prns, labels)
+            prns.each{|prn| @solver.glonass_options.send(mode, prn & 0xFF)}
           else
             raise "Unknown satellite: #{spec}"
           end
@@ -317,15 +349,21 @@ class Receiver
       [[:@azimuth, az], [:@elevation, el]].each{|k, values|
         self.instance_variable_set(k, Hash[*(sats.zip(values).flatten(1))])
       }
+      mat_S = self.S
       [:@slopeH, :@slopeV] \
-          .zip((self.fd ? self.slope_HV_enu.to_a.transpose : [nil, nil])) \
+          .zip((self.fd ? self.slope_HV_enu(mat_S).to_a.transpose : [nil, nil])) \
           .each{|k, values|
         self.instance_variable_set(k,
             Hash[*(values ? sats.zip(values).flatten(1) : [])])
       }
+      # If a design matrix G has columns larger than 4, 
+      # other states excluding position and time are estimated.
+      @other_state = self.position_solved? \
+          ? (mat_S * self.delta_r.partial(self.used_satellites, 1, 0, 0)).transpose.to_a[0][4..-1] \
+          : []
       instance_variable_get(target)
     }
-    [:azimuth, :elevation, :slopeH, :slopeV].each{|k|
+    [:azimuth, :elevation, :slopeH, :slopeV, :other_state].each{|k|
       eval("define_method(:#{k}){@#{k} || self.post_solution(:@#{k})}")
     }
   }
@@ -336,7 +374,13 @@ class Receiver
       eph.svid = prn
       [prn, eph]
     }.flatten(1)]
-    define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data|
+    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]
@@ -366,6 +410,15 @@ class Receiver
             $stderr.puts str
           } if @debug[:SBAS_IGP] 
         end
+      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)
+        eph.invalidate
       end
     }
   }.call
@@ -377,7 +430,7 @@ class Receiver
     ubx = UBX::new(open(ubx_fname))  
     ubx_kind = Hash::new(0)
     
-    after_run = b || proc{|pvt| puts pvt.to_s}
+    after_run = b || proc{|pvt| puts pvt.to_s if pvt}
     
     gnss_serial = proc{|svid, sys|
       if sys then # new numbering
@@ -443,7 +496,11 @@ class Receiver
           }
           sys, svid = gnss_serial.call(*loader.call(36, 2).reverse)
           case sys
-          when :GPS, :QZSS; 
+          when :GPS, :SBAS, :QZSS;
+          when :GLONASS
+            svid += 0x100
+            meas.add(svid, :L1_FREQUENCY, 
+                GPS::SpaceNode_GLONASS::L1_frequency(loader.call(39, 1, "C") - 7))
           else; next
           end
           trk_stat = loader.call(46, 1)[0]
@@ -477,12 +534,14 @@ class Receiver
             })
       when [0x02, 0x13] # RXM-SFRBX
         sys, svid = gnss_serial.call(packet[6 + 1], packet[6])
+        opt = {}
+        opt[:freq_ch] = packet[6 + 3] - 7 if sys == :GLONASS
         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)
       end
     }
     $stderr.puts ", found packets are %s"%[ubx_kind.inspect]
@@ -492,6 +551,7 @@ class Receiver
     items = [
       @solver.gps_space_node,
       @solver.sbas_space_node,
+      @solver.glonass_space_node,
     ].inject(0){|res, sn|
       loaded_items = sn.send(:read, fname)
       raise "Format error! (Not RINEX) #{fname}" if loaded_items < 0
@@ -501,9 +561,10 @@ class Receiver
   end
   
   def parse_rinex_obs(fname, &b)
-    after_run = b || proc{|pvt| puts pvt.to_s}
+    after_run = b || proc{|pvt| puts pvt.to_s if pvt}
     $stderr.print "Reading RINEX observation file (%s)"%[fname]
     types = nil
+    glonass_freq = nil
     count = 0
     GPS::RINEX_Observation::read(fname){|item|
       $stderr.print '.' if (count += 1) % 1000 == 0
@@ -526,12 +587,32 @@ class Receiver
         }.compact]
       }.flatten(1))]
 
+      glonass_freq ||= proc{|spec|
+        # frequency channels described in observation file
+        next {} unless spec
+        Hash[*(spec.collect{|line|
+          line[4..-1].scan(/R(\d{2}).([\s+-]\d)./).collect{|prn, ch|
+            [prn.to_i, GPS::SpaceNode_GLONASS::L1_frequency(ch.to_i)]
+          }
+        }.flatten(2))]
+      }.call(item[:header]["GLONASS SLOT / FRQ #"])
+
       meas = GPS::Measurement::new
       item[:meas].each{|k, v|
         sys, prn = k
         case sys
         when 'G', ' '
+        when 'S'; prn += 100
         when 'J'; prn += 192
+        when 'R'
+          freq = (glonass_freq[prn] ||= proc{|sn|
+            # frequency channels saved with ephemeris
+            sn.update_all_ephemeris(t_meas)
+            next nil unless sn.ephemeris(prn).in_range?(t_meas)
+            sn.ephemeris(prn).frequency_L1
+          }.call(@solver.glonass_space_node))
+          prn += 0x100
+          meas.add(prn, :L1_FREQUENCY, freq) if freq
         else; next
         end
         types[sys] = (types[' '] || []) unless types[sys]

data/lib/gps_pvt/version.rb

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

metadata

@@ -1,14 +1,14 @@
 --- !ruby/object:Gem::Specification
 name: gps_pvt
 version: !ruby/object:Gem::Version
-  version: 0.2.3
+  version: 0.4.0
 platform: ruby
 authors:
 - fenrir(M.Naruoka)
-autorequire:
+autorequire: 
 bindir: exe
 cert_chain: []
-date: 2022-02-22 00:00:00.000000000 Z
+date: 2022-03-17 00:00:00.000000000 Z
 dependencies:
 - !ruby/object:Gem::Dependency
   name: rake
@@ -61,7 +61,10 @@ files:
 - ext/gps_pvt/GPS/GPS_wrap.cxx
 - ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx
 - ext/gps_pvt/extconf.rb
+- ext/ninja-scan-light/tool/algorithm/integral.h
 - ext/ninja-scan-light/tool/navigation/EGM.h
+- ext/ninja-scan-light/tool/navigation/GLONASS.h
+- ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h
 - ext/ninja-scan-light/tool/navigation/GPS.h
 - ext/ninja-scan-light/tool/navigation/GPS_Solver.h
 - ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h
@@ -100,7 +103,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
@@ -115,8 +118,8 @@ required_rubygems_version: !ruby/object:Gem::Requirement
     - !ruby/object:Gem::Version
       version: '0'
 requirements: []
-rubygems_version: 3.3.7
-signing_key:
+rubygems_version: 3.1.2
+signing_key: 
 specification_version: 4
 summary: GPS position, velocity, and time (PVT) solver
 test_files: []