gps_pvt 0.10.3 → 0.10.4

checksums.yaml

@@ -1,7 +1,7 @@
 ---
 SHA256:
-  metadata.gz: 0d6b65fbf88b765e83cf8d2341d42e6919c8b79be5e98d2c493ca23ec5445e8e
-  data.tar.gz: 11bd177fd1cd8ba4ecf5134b64977f6e9a3b0e08cff3a290d1bca6897958f23b
+  metadata.gz: 60413c3add439fa66e0ff908f43f0049ad306fcefca50e2c485bc28790597143
+  data.tar.gz: 788f430151ca35a95abd4f96e4f30239e9f53768014af9ddc5897305b25b7fb9
 SHA512:
-  metadata.gz: e20cfb1dd15fe7ff0ff7320e6acc32d8966c1ad6d67f3203e27da757fb095e512085bdea4e7a9ac90282df8a4ea92235cb94a3b6210f6d1b8938c17f505f9ee3
-  data.tar.gz: 6897b4cdedcfd23edb23bdaf57f6d008f9c2562107dc7ccbf1788e7249637b5a1a7fc79b0af09d68a967aed5d86811947b4f29b2465b4db8835727adf69af4a0
+  metadata.gz: 9c0ddc7d6a4f589cf3b3692eacde62405c78890f20cadf5abde07d189ae35bda83fd0b9e905728e4fecaf111aeee14bfb615166ef72d355fcac1a3368330ee89
+  data.tar.gz: 457d23f7e4032549961e499638403b3d01d681a8051231616ee2dde5183967c119028bd2a4475f2fe1c1e3ae2ffda61729162ced5d30b33113bfe21c161c188e

data/exe/gps2ubx

@@ -3,6 +3,7 @@
 require 'gps_pvt'
 require 'uri'
 require 'gps_pvt/ubx'
+require 'gps_pvt/pvt'
 
 # Convert file(s) to ubx format
 # TODO currently only RINEX observation file is supported.
@@ -25,6 +26,7 @@ options = []
 misc_options = {
   :broadcast_data => false,
   :ubx_rawx => false,
+  :ubx_nav => false,
   :eph_interval => 60 * 5,
 }
 
@@ -32,7 +34,7 @@ misc_options = {
 files = ARGV.collect{|arg|
   next [arg, nil] unless arg =~ /^--([^=]+)=?/
   k, v = [$1.downcase.to_sym, $']
-  next [v, k] if [:rinex_nav, :rinex_obs, :ubx, :rtcm3].include?(k) # file type
+  next [v, k] if [:rinex_nav, :rinex_obs, :ubx, :rtcm3, :pvt_csv].include?(k) # file type
   options << [$1.to_sym, $']
   nil
 }.compact
@@ -43,6 +45,7 @@ files.collect!{|fname, ftype|
   when /\.\d{2}[nhqg](?:\.gz)?$/; :rinex_nav
   when /\.\d{2}o(?:\.gz)?$/; :rinex_obs
   when /\.ubx$/; :ubx
+  when /\.csv$/; misc_options[:ubx_nav] = true; :pvt_csv
   end
   if (!(uri = URI::parse(fname)).instance_of?(URI::Generic) rescue false) then
     ftype ||= case uri
@@ -57,7 +60,7 @@ files.collect!{|fname, ftype|
 
 options.reject!{|opt|
   case opt[0]
-  when :ubx_rawx, :broadcast_data, :eph_interval
+  when :ubx_rawx, :ubx_nav, :broadcast_data, :eph_interval
     misc_options[opt[0]] = opt[1]
     true
   when :online_ephemeris
@@ -70,10 +73,13 @@ options.reject!{|opt|
 
 rcv = GPS_PVT::Receiver::new(options)
 
-obs = Queue::new
-rcv.define_singleton_method(:run){|meas, t_meas, *args|
-  obs << [t_meas, meas]
-  nil
+outputs = Queue::new # [[time, item1, item2, ...], ...]
+rcv.instance_eval{
+  nav_task = misc_options[:ubx_nav] ? method(:run) : proc{}
+  define_singleton_method(:run){|meas, t_meas, *args|
+    outputs << [t_meas, meas, nav_task.call(meas, t_meas, *args)].compact
+    nil
+  }
 }
 
 proc{|src|
@@ -106,6 +112,9 @@ threads = files.collect{|fname, ftype|
   when :rtcm3; proc{rcv.parse_rtcm3(fname){}}
   when :supl; proc{rcv.parse_supl(fname)}
   when :rinex_nav; proc{}
+  when :pvt_csv; proc{GPS_PVT::GPS::PVT_minimal::parse_csv(fname){|t, pvt|
+    outputs << [t, pvt]
+  }}
   end
   case fname
   when URI::Ntrip, URI::Supl; Thread::new(&task)
@@ -113,9 +122,9 @@ threads = files.collect{|fname, ftype|
   end
 }.compact
 
-obs = proc{
+outputs = proc{
   tmp = []
-  tmp << obs.pop until obs.empty?
+  tmp << outputs.pop until outputs.empty?
   tmp
 }.call.sort!{|a, b| b[0] <=> a[0]} if threads.empty? # Sort by measurement time
 
@@ -124,7 +133,7 @@ gen_gpstime = proc{
   tmpl = [0xB5, 0x62, 0x01, 0x20, 16, 0]
   gpst = GPS_PVT::GPS::Time
   t_next = gpst::new(*gpst::leap_second_events.find{|wn, sec, leap| leap == 1}[0..1])
-  proc{|t_meas, meas|
+  proc{|t_meas|
     next nil if t_meas < t_next
     t_next = t_meas + 60 # 1 min. interval
     ubx = tmpl.clone
@@ -374,17 +383,102 @@ gen_rawx = proc{|t_meas, meas| # Convert to RXM-RAWX(0x15)
   GPS_PVT::UBX::update(ubx).pack("C*")
 }
 
-gen_list = []
-gen_list << gen_gpstime unless misc_options[:ubx_rawx]
-gen_list << (misc_options[:ubx_rawx] ? gen_sfrbx : gen_sfrb) if misc_options[:broadcast_data]
-gen_list << (misc_options[:ubx_rawx] ? gen_rawx : gen_raw)
+gen_nav = proc{|t_meas, pvt|
+  t_msec = (t_meas.seconds * 1E3).round
+  packet = []
+  
+  proc{ # Convert to NAV-SOL (0x01-0x06)
+    ubx_sol = [0xB5, 0x62, 0x01, 0x06, 52, 0]
+    ubx_sol += [
+        t_msec,
+        0, # frac
+        t_meas.week, # week
+        if (pvt.position_solved? and pvt.velocity_solved?) then
+          [
+            0x03, # 3D-Fix
+            0x0D, # GPSfixOK, WKNSET, TOWSET
+            pvt.xyz.to_a.collect{|v| (v * 1E2).to_i}, # ECEF_XYZ [cm]
+            (Math::sqrt((pvt.hsigma ** 2) + (pvt.vsigma ** 2)) * 1E2).to_i, # 3D pos accuracy [cm]
+            (pvt.velocity.absolute(pvt.xyz) - pvt.xyz).to_a.collect{|v| (v * 1E2).to_i}, # ECEF_VXYZ [cm/s]
+            (pvt.vel_sigma * 1E2).to_i, # Speed accuracy [cm/s]
+            (pvt.pdop * 1E2).to_i, # PDOP [0.01]
+          ]
+        else
+          [
+            0, # 3D-Fix
+            0x08, # TOWSET
+            [0] * 3, # ECEF_XYZ [cm]
+            0, # 3D pos accuracy [cm]
+            [0] * 3, # ECEF_VXYZ [cm/s]
+            0, # Speed accuracy [cm/s]
+            0, # PDOP
+          ]
+        end,
+        0,
+        pvt.used_satellites,
+        0].flatten.pack('V2vc2l<3Vl<3VvC2V').unpack('C*')
+    packet += GPS_PVT::UBX::update(ubx_sol + [0, 0])
+  }.call
+    
+  # Convert to NAV-POSLLH (0x01-0x02)
+  if pvt.position_solved? then
+    llh = pvt.llh
+    ubx_posllh = [0xB5, 0x62, 0x01, 0x02, 28, 0]
+    ubx_posllh += [
+        t_msec,
+        (llh.lng / Math::PI * 180 * 1E7).to_i, # Longitude [1E-7 deg]
+        (llh.lat / Math::PI * 180 * 1E7).to_i, # Latitude [1E-7 deg]
+        (llh.alt * 1E3).to_i, # WGS-84 altitude [mm]
+        (llh.alt * 1E3).to_i, # mean sea level TODO fix
+        (pvt.hsigma * 1E3).to_i, # HAcc [mm]
+        (pvt.vsigma * 1E3).to_i, # VAcc [mm]
+        ].pack('V*').unpack("C*")
+    packet += GPS_PVT::UBX::update(ubx_posllh + [0, 0])
+  end
+
+  # Convert to NAV-VELNED (0x01-0x12)
+  if pvt.velocity_solved? then
+    vel = pvt.velocity
+    ubx_velned = [0xB5, 0x62, 0x01, 0x12, 36, 0]
+    ubx_velned += [
+        t_msec,
+        (vel.n * 1E2).to_i, # N speed [cm/s]
+        (vel.e * 1E2).to_i, # E speed [cm/s]
+        (vel.d * 1E2).to_i, # D speed [cm/s]
+        (vel.distance * 1E2).to_i, # 3D speed [cm/s]
+        (vel.horizontal * 1E2).to_i, # 2D speed [cm/s]
+        (vel.azimuth / Math::PI * 180 * 1E5).to_i, # Heading [1E-5 deg]
+        (pvt.vel_sigma * 1E2).to_i, # sAcc speed accuracy [cm/s]
+        (2 * 1E5).to_i, # cAcc heading accuracy [1E-5 deg] TODO
+        ].pack('V*').unpack("C*")
+    packet += GPS_PVT::UBX::update(ubx_velned + [0, 0])
+  end
+  
+  (gen_gpstime.call(t_meas) || '') + packet.pack('C*')
+}
+
+gen_list = Hash[*({
+  :Measurement => proc{
+    gen_ary = []
+    gen_ary << gen_gpstime unless misc_options[:ubx_rawx]
+    gen_ary << (misc_options[:ubx_rawx] ? gen_sfrbx : gen_sfrb) if misc_options[:broadcast_data]
+    gen_ary << (misc_options[:ubx_rawx] ? gen_rawx : gen_raw)
+    proc{|t_meas, meas|
+      meas = meas.to_hash
+      gen_ary.collect{|gen| gen.call(t_meas, meas)}.join
+    }
+  }.call,
+  :PVT => gen_nav,
+  :PVT_minimal => gen_nav,
+}.collect{|k, v| [GPS_PVT::GPS.const_get(k), v]}.flatten(1))]
 STDOUT.binmode
 
 task_dump = proc{
-  until obs.empty? do
-    t_meas, meas = obs.pop
-    meas2 = meas.to_hash
-    gen_list.each{|gen| print gen.call(t_meas, meas2)}
+  until outputs.empty? do
+    t_meas, *items = outputs.pop
+    items.each{|item|
+      print gen_list[item.class].call(t_meas, item)
+    }
   end
 }
 if threads.empty? then

data/lib/gps_pvt/pvt.rb

@@ -0,0 +1,83 @@
+require 'gps_pvt/GPS'
+require 'gps_pvt/Coordinate'
+
+module GPS_PVT
+class GPS::PVT_minimal
+  def initialize
+    # GLOBAL POSITIONING SYSTEM STANDARD POSITIONING SERVICE
+    # PERFORMANCE STANDARD 5th edition (Apr. 2020) 3.8
+    @hsigma = 8.0 # 95%
+    @vsigma = 13.0 # 95%
+    @pdop = 6.0 # 98% global
+    @vel_sigma = 0.2 # 85%
+    @use_satellites = 0 # unknown
+  end
+  def position_solved?; @xyz || @llh; end
+  def velocity_solved?; @velocity; end
+  def xyz; @xyz || @llh.xyz; end
+  def llh; @xyz ? @xyz.llh : @llh; end
+  def xyz=(args); @xyz = Coordinate::XYZ::new(*args); end
+  def llh=(args); @llh = Coordinate::LLH::new(*args); end
+  def velocity=(args); @velocity = Coordinate::ENU::new(*args); end
+  attr_reader :velocity
+  attr_accessor :hsigma, :vsigma, :pdop, :vel_sigma, :used_satellites
+  class <<self
+    def parse_csv(csv_fname, &b)
+      require_relative 'util'
+      $stderr.puts "Reading CSV file (%s) "%[csv_fname]
+      io = open(Util::get_txt(csv_fname), 'r')
+      
+      header = io.readline.chomp.split(/ *, */)
+      idx_table = [
+        :week, [:tow, /^i?t(?:ime_)?o(?:f_)?w(?:eek)?/],
+        :year, :month, :mday, :hour, :min, [:sec, /^sec/],
+        [:lng, /^(?:long(?:itude)?|lng)/], [:lat, /^lat(?:itude)/],
+              [:alt, /^(?:alt(?:itude)?|h(?:$|eight|_))/], # assumption: [deg], [deg], [m]
+        :x, :y, :z, # ECEF xyz
+        :hsigma, :vsigma, :pdop,
+        [:vn, /^v(?:el)?_?n(?:orth)?/], [:ve, /^v(?:el)?_?e(?:ast)?/],
+            [:vd, /^v(?:el)?_?d(?:own)?/], [:vu, /^v(?:el)?_?u(?:p)?/],
+        :vx, :vy, :vz, # ECEF xyz
+        [:vel_sigma, /^v(?:el)?_sigma/],
+        [:used_satellites, /^(?:used_)?sat(?:ellite)?s/],
+      ].collect{|k, re|
+        re ||= k.to_s
+        idx = header.find_index{|str| re === str}
+        idx && [k, idx]
+      }.compact
+      enum = Enumerator::new{|y|
+        io.each_line{|line|
+          values = line.chomp.split(/ *, */)
+          items = Hash[*(idx_table.collect{|k, idx|
+            v = values[idx]
+            (v == '') ? nil : [k, (Integer(v) rescue Float(v))]
+          }.compact.flatten(1))]
+          if items.include?(:week) then
+            t = GPS::Time::new(items[:week], items[:tow])
+          else
+            # UTC assumption, thus leap seconds must be added.
+            t = GPS::Time::new([:year, :month, :mday, :hour, :min, :sec].collect{|k| items[k]})
+            t += GPS::Time::guess_leap_seconds(t)
+          end
+          pvt = GPS::PVT_minimal::new
+          if items.include?(:lat) then
+            pvt.llh = ([:lat, :lng].collect{|k| Math::PI / 180 * items[k]} + [items[:alt]])
+          elsif items.include?(:xyz) then
+            pvt.xyz = [:x, :y, :z].collect{|k| items[k]}
+          end
+          if items.include?(:vn) then
+            pvt.velocity = ([:vn, :ve].collect{|k| items[k]} + [items[:vu] || -items[:vd]])
+          elsif items.include?(:vx) then
+            pvt.velocity = Coordinate::ENU::relative_rel(
+                Coordinate::XYZ::new(*([:vx, :vy, :vz].collect{|k| items[k]})),
+                pvt.llh)
+          end
+          [:pdop, :hsigma, :vsigma, :vel_sigma, :used_satellites].each{|k| pvt.send("#{k}=", items[k])}
+          y.yield(t, pvt)
+        }
+      }
+      b ? enum.each{|*res| b.call(*res)} : enum
+    end
+  end
+end
+end

data/lib/gps_pvt/receiver.rb

@@ -50,7 +50,7 @@ class Receiver
     }.call] + [[
       [:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms, :vel_sigma],
       proc{|pvt|
-        next [nil] * 4 unless pvt.velocity_solved?
+        next [nil] * 5 unless pvt.velocity_solved?
         [:north, :east, :down].collect{|k| pvt.velocity.send(k)} \
             + [pvt.receiver_error_rate, pvt.vel_sigma] 
       }

data/lib/gps_pvt/version.rb

@@ -1,7 +1,7 @@
 # frozen_string_literal: true
 
 module GPS_PVT
-  VERSION = "0.10.3"
+  VERSION = "0.10.4"
   
   def GPS_PVT.version_compare(a, b)
     Gem::Version::new(a) <=> Gem::Version::new(b)

metadata

@@ -1,14 +1,14 @@
 --- !ruby/object:Gem::Specification
 name: gps_pvt
 version: !ruby/object:Gem::Version
-  version: 0.10.3
+  version: 0.10.4
 platform: ruby
 authors:
 - fenrir(M.Naruoka)
 autorequire: 
 bindir: exe
 cert_chain: []
-date: 2024-10-24 00:00:00.000000000 Z
+date: 2024-12-15 00:00:00.000000000 Z
 dependencies:
 - !ruby/object:Gem::Dependency
   name: rubyserial
@@ -180,6 +180,7 @@ files:
 - lib/gps_pvt/asn1/asn1.y
 - lib/gps_pvt/asn1/per.rb
 - lib/gps_pvt/ntrip.rb
+- lib/gps_pvt/pvt.rb
 - lib/gps_pvt/receiver.rb
 - lib/gps_pvt/receiver/agps.rb
 - lib/gps_pvt/receiver/almanac.rb