gps_pvt 0.8.0 → 0.8.2

checksums.yaml

@@ -1,7 +1,7 @@
 ---
 SHA256:
-  metadata.gz: 5bccc645ba148e24c0d8b43c1663ed245a6306eedfd376bdccf0f817977d5964
-  data.tar.gz: 13b461cf2ce59b7064f4c69f13969f3e758974ae6dd331bf3628a901f76b3762
+  metadata.gz: b87c77fbb3844eb0fd835ef079b8a379708a6fe24e3de4ceeb6934206573f489
+  data.tar.gz: a280c1aa82bed41319e13069907940a062a073a0b01c95eefa3d3d8c1b32c517
 SHA512:
-  metadata.gz: cebbb365561e4a2a56cadde92d69f03490313d56baa33ed5e14749220385a85813ee91cbdb93cb3980f66d69d95e1a63fb2852fb8441992767b64e7a80558ec5
-  data.tar.gz: 251feae90231587a9e1c720ab20675084573b6ef1fc50429ad04223a1ad269eccc16d0e79cb7f4ffa547be95c1129a83e12e3edb5dc1495d20245cb1afc39693
+  metadata.gz: 761d7e99868e04d93adb48fac0345ff71467068657c544efe857ed4f6206ed8d11698871ad7dc73c232f4bd51094cf13c6752d96fc17307caccb4a2eab8dbe03
+  data.tar.gz: fd70c1d59dbf3885e7a2286726a9c16e0cfbec3097a105bbffde59e3a65ee22eb09f1d0b4032b371faf2d1695b074b0752f37763ef6b5716dd4214fed4566755

data/Rakefile

@@ -45,6 +45,7 @@ namespace :git do
               + (<<-__SPARSE_PATTERNS__).lines.collect{|str| str.chomp.gsub(/^ */, ' ')}.join,
             tool/param/
             tool/util/text_helper.h
+            tool/util/bit_counter.h
             tool/algorithm/integral.h
             tool/algorithm/interpolate.h
             tool/navigation/GPS*

data/exe/gps_pvt

@@ -92,23 +92,7 @@ files.collect!{|fname, ftype|
 rcv = GPS_PVT::Receiver::new(options)
 
 proc{|src|
-  next unless src
-  loader = proc{|t_meas|
-    utc = Time::utc(*t_meas.c_tm)
-    y, yday = [:year, :yday].collect{|f| utc.send(f)}
-    uri = URI::parse(
-        "ftp://gssc.esa.int/gnss/data/daily/" + 
-          "%04d/brdc/BRDC00IGS_R_%04d%03d0000_01D_MN.rnx.gz"%[y, y, yday])
-    rcv.parse_rinex_nav(uri)
-    uri
-  }
-  run_orig = rcv.method(:run)
-  eph_list = {}
-  rcv.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)
-  }
+  rcv.attach_online_ephemeris(src) if src
 }.call(misc_options[:online_ephemeris])
 
 proc{

data/exe/to_ubx

@@ -21,7 +21,9 @@ __STRING__
 
 options = []
 misc_options = {
-  :ubx_rawx => false
+  :broadcast_data => false,
+  :ubx_rawx => false,
+  :eph_interval => 60 * 5,
 }
 
 # check options and file format
@@ -53,7 +55,7 @@ files.collect!{|fname, ftype|
 
 options.reject!{|opt|
   case opt[0]
-  when :ubx_rawx
+  when :ubx_rawx, :online_ephemeris, :broadcast_data, :eph_interval
     misc_options[opt[0]] = opt[1]
     true
   else
@@ -68,6 +70,21 @@ rcv.define_singleton_method(:run){|meas, t_meas, *args|
   obs << [t_meas, meas]
 }
 
+proc{|src|
+  rcv.attach_online_ephemeris(src) if src
+}.call(misc_options[:online_ephemeris])
+
+proc{
+  cache = nil
+  rcv.define_singleton_method(:leap_seconds){|t_meas|
+    cache ||= if self.solver.gps_space_node.is_valid_utc then
+      self.solver.gps_space_node.iono_utc.delta_t_LS
+    else
+      t_meas.leap_seconds
+    end
+  }
+}.call
+
 # parse RINEX NAV
 files.each{|fname, ftype|
   case ftype
@@ -84,6 +101,92 @@ files.each{|fname, ftype|
 
 obs.sort!{|a, b| a[0] <=> b[0]} # Sort by measurement time
 
+# Add dummy before actual first observation in order to notify time to solver
+obs.unshift([obs[0][0] - 1, GPS_PVT::GPS::Measurement::new])
+
+gen_sfrb, gen_sfrbx = proc{
+  cache = {}
+  sfrb_tmpl = [0xB5, 0x62, 0x02, 0x11, 0, 0]
+  sfrb_tmpl += [0, 0] # ch, id
+  sfrbx_tmpl = [0xB5, 0x62, 0x02, 0x13, 0, 0]
+  sfrbx_tmpl += [0, 0, 0, 0, 0, 0, 2, 0] # version = 2
+  iono_utc = rcv.solver.gps_space_node.iono_utc.instance_eval{
+    f_orig = method(:dump)
+    define_singleton_method(:dump){|t_meas|
+      rcv.solver.gps_space_node.is_valid_iono_utc ? f_orig.call(t_meas) : []
+    }
+    self
+  }
+  [proc{|t_meas, meas| # Convert to RXM-SFRB(0x11)
+    meas.collect{|sat, items|
+      t_prv, eph, ch = cache.include?(sat) ? cache[sat] : []
+      next nil if t_prv && (t_meas - t_prv < misc_options[:eph_interval])
+      sfrb = sfrb_tmpl.clone
+      sfrb[6] = (ch ||= (cache.size % 0x100))
+      sfrb[7] = sat
+      res = case sat
+      when 1..32 # GPS
+        next nil unless (eph = rcv.ephemeris(t_meas, :GPS, sat))
+        (eph.dump(t_meas) + iono_utc.dump(t_meas)).each_slice(10).collect{|subframe|
+          ubx = sfrb + subframe.collect{|word|
+            word >> 6
+          }.pack("V*").unpack("C*") + [0, 0]
+          GPS_PVT::UBX::update(ubx)
+        }
+      when 120..158 # SBAS
+        next nil unless (eph = rcv.ephemeris(t_meas, :SBAS, sat))
+        ubx = sfrb + proc{|msg|
+            msg[7] >>= 6
+            msg
+          }.call(eph.dump + [0, 0]).pack("V*").unpack("C*") + [0, 0]
+        GPS_PVT::UBX::update(ubx)
+      else
+        next nil
+      end
+      cache[sat] = [t_meas, eph, ch]
+      res
+    }.compact.flatten.pack("C*")
+  },
+  proc{|t_meas, meas| # Convert to RXM-SFRBX(0x13)
+    meas.collect{|sat, items|
+      t_prv, eph, ch = cache.include?(sat) ? cache[sat] : []
+      next nil if t_prv && (t_meas - t_prv < misc_options[:eph_interval])
+      sfrbx = sfrbx_tmpl.clone
+      
+      res = case sat
+      when 1..32, 193..202 # GPS, QZSS
+        sys = sat <= 32 ? :GPS : :QZSS
+        next nil unless (eph = rcv.ephemeris(t_meas, sys, sat))
+        sfrbx[6..7] = [GPS_PVT::UBX::GNSS_ID[sys], sys == :QZSS ? (sat - 192) : sat] # sys, id
+        sfrbx[10] = 10 # words
+        sfrbx[11] = (ch ||= (cache.size % 0x100)) # ch
+        (eph.dump(t_meas) + iono_utc.dump(t_meas)).each_slice(10).collect{|subframe|
+          GPS_PVT::UBX::update(sfrbx + subframe.pack("V*").unpack("C*") + [0, 0])
+        }
+      when 120..158 # SBAS
+        next nil unless (eph = rcv.ephemeris(t_meas, :SBAS, sat))
+        sfrbx[6..7] = [GPS_PVT::UBX::GNSS_ID[:SBAS], sat] # sys, id
+        sfrbx[10] = 8 # words
+        sfrbx[11] = (ch ||= (cache.size % 0x100)) # ch
+        GPS_PVT::UBX::update(sfrbx + eph.dump.pack("V*").unpack("C*") + [0, 0])
+      when (0x100 + 1)..(0x100 + 32) # GLONASS
+        svid = sat - 0x100
+        next nil unless (eph = rcv.ephemeris(t_meas, :GLONASS, svid))
+        sfrbx[6..7] = [GPS_PVT::UBX::GNSS_ID[:GLONASS], svid] # sys, id
+        sfrbx[10] = 4 # words
+        sfrbx[11] = (ch ||= (cache.size % 0x100)) # ch
+        eph.dump(t_meas).each_slice(3).collect{|str|
+          GPS_PVT::UBX::update(sfrbx + (str + [0]).pack("V*").unpack("C*") + [0, 0])
+        }
+      else
+        next nil
+      end
+      cache[sat] = [t_meas, eph, ch]
+      res
+    }.compact.flatten.pack("C*")
+  }]
+}.call
+
 glonass_freq_ch = proc{
   freq0, delta = [:L1_frequency_base, :L1_frequency_gap].collect{|k|
     GPS_PVT::GPS::SpaceNode_GLONASS.send(k)
@@ -92,7 +195,6 @@ glonass_freq_ch = proc{
 }.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
@@ -142,17 +244,17 @@ gen_raw = proc{|t_meas, meas| # Convert to RXM-RAW(0x10)
 }
 
 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|
+  gen_packet = proc{|sys, svid, sig, 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)]]
+        k = [sig, value].join('_').to_sym
+        [items[GPS_PVT::GPS::Measurement.const_get(k)]]
       else
         next nil
       end
@@ -161,54 +263,82 @@ gen_rawx = proc{|t_meas, meas| # Convert to RXM-RAWX(0x15)
       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)
     
+    setter.call([sys, svid], 36, 2)
+
     trk_stat = 0
-    setter.call(:L1_PSEUDORANGE,
+    setter.call(:PSEUDORANGE,
         16, 8, "E", proc{|v| v[0] ? (trk_stat |= 0x1) : v.clear})
-    setter.call(:L1_PSEUDORANGE_SIGMA,
+    setter.call(: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,
+    setter.call(:DOPPLER, 32, 4, "e") rescue next nil
+    setter.call(: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,
+    setter.call(:CARRIER_PHASE,
         24, 8, "E", proc{|v| v[0] ? (trk_stat |= 0x2) : v.clear})
-    setter.call(:L1_CARRIER_PHASE_SIGMA,
+    setter.call(: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,
+    setter.call(:SIGNAL_STRENGTH_dBHz,
         42, 1, nil, proc{|v| v.replace(v[0] ? [v[0].to_i] : [])})
-    setter.call(:L1_LOCK_SEC,
+    setter.call(:LOCK_SEC,
         40, 2, "v", proc{|v| v.replace(v[0] ? [(v[0] / 1E-3).to_i] : [])})
     setter.call([trk_stat], 46, 1)
+    res.define_singleton_method(:set, &setter)
 
     res
+  }
+  meas_ubx = meas.inject([]){|packets, (sat, items)|
+    case sat
+    when 1..32 # GPS
+      packets << gen_packet.call(0, sat, :L1, items)
+      packets += {:L2CL => 3, :L2CM => 4}.collect{|sig, sigid|
+        next nil unless packet = gen_packet.call(0, sat, sig, items)
+        packet[38 - 16] = sigid
+        packet
+      }
+    when 120..158 # SBAS
+      packets << gen_packet.call(1, sat, :L1, items)
+    when 193..202 # QZSS
+      packets << gen_packet.call(5, sat, :L1, items)
+      packets += {:L2CL => 5, :L2CM => 4}.collect{|sig, sigid|
+        next nil unless packet = gen_packet.call(5, sat, sig, items)
+        packet[38 - 16] = sigid
+        packet
+      }
+    when (0x100 + 1)..(0x100 + 32) # GLONASS
+      packet = gen_packet.call(6, sat - 0x100, :L1, items)
+      packet.set(:FREQUENCY,
+          39, 1, nil,
+          proc{|v| v.replace([(v[0] ? glonass_freq_ch.call(v[0]) : 0) + 7])} ) if packet
+      packets << packet
+    else
+      # TODO Galileo, Beidou, ...
+    end
   }.compact
+  
+  proc{|ls| # leap seconds
+    next unless ls
+    ubx[6 + 10] = ls
+    ubx[6 + 12] |= 0x01
+  }.call(rcv.leap_seconds(t_meas))
   ubx[6 + 11] = meas_ubx.size
-  ubx += meas_ubx.flatten(1)
+  ubx[6 + 13] = 1 # version
+  ubx += meas_ubx.flatten
   ubx += [0, 0]
   GPS_PVT::UBX::update(ubx).pack("C*")
 }
 
-gen = misc_options[:ubx_rawx] ? gen_rawx : gen_raw
+gen_list = []
+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)
 STDOUT.binmode
-obs.each{|*item| print gen.call(*item)}
+obs.each{|t_meas, meas|
+  meas2 = meas.to_hash
+  gen_list.each{|gen| print gen.call(t_meas, meas2)}
+}