gps_pvt 0.9.4 → 0.10.1

data/lib/gps_pvt/asn1/per.rb

@@ -0,0 +1,182 @@
+#!/usr/bin/ruby
+
+module GPS_PVT
+
+# @see ISO/IEC 8825-2:2003(E)
+# @see http://www5d.biglobe.ne.jp/~stssk/asn1/per.html
+module PER
+module Basic_Unaligned
+module Encoder
+class <<self
+  def non_negative_binary_integer2(v, bits) # 10.3
+    "%0#{bits}b" % v
+  end
+  def non_negative_binary_integer(v, align = 1) # 10.3
+    non_negative_binary_integer2(
+        v, (Math::log2(v + 1) / align).ceil * align)
+  end
+  def twos_complement_binary_integer(v) # 10.4
+    if v >= 0 then
+      bits = ((Math::log2(v + 1) + 1) / 8).ceil * 8
+      "%0#{bits}b" % v
+    else
+      bits = ((Math::log2(-v) + 1) / 8).ceil * 8 - 1
+      "1%0#{bits}b" % (v + (1 << bits))
+    end
+  end
+  def constrainted_whole_number2(v, v_min, v_max) # 10.5.6
+    non_negative_binary_integer2(
+        v - v_min,
+        Math::log2(v_max - v_min + 1).ceil)
+  end
+  def constrainted_whole_number(v, v_range) # 10.5.6
+    constrainted_whole_number2(v, *v_range.minmax)
+  end
+  def normally_small_non_negative_whole_number(v, *len_enc) # 10.6
+    if v <= 63 then
+      "0%06b" % v # 10.6.1
+    else
+      "1#{semi_constrained_whole_number(v, 0, *len_enc)}" # 10.6.2
+    end
+  end
+  def semi_constrained_whole_number(v, v_min, *len_enc) # 10.7
+    len_enc = :length_otherwise if len_enc.empty?
+    bf = non_negative_binary_integer(v - v_min, 8).scan(/.{8}/)
+    with_length(bf.size, *len_enc).collect{|len_str, range|
+      len_str + bf[range].join
+    }.join
+  end
+  def unconstrained_whole_number(v, *len_enc) # 10.8
+    len_enc = :length_otherwise if len_enc.empty?
+    bf = twos_complement_binary_integer(v).scan(/.{8}/)
+    with_length(bf.size, *len_enc).collect{|len_str, range|
+      len_str + bf[range].join
+    }.join
+  end
+  def length_constrained_whole_number(len, len_range)
+    if len_range.max < 65536 then # 10.9.4.1
+      (len_range.min == len_range.max) ?
+          "" : 
+          constrainted_whole_number(len, len_range)
+    else
+      length_otherwise(len)
+    end
+  end
+  def length_normally_small_length(len) # 10.9.4.2 -> 10.9.3.4
+    if len <= 64 then
+      normally_small_non_negative_whole_number(len - 1)
+    else
+      len_enc, len_remain = length_otherwise(len)
+      len_enc = "1#{len_enc}"
+      len_remain ? [len_enc, len_remain] : len_enc
+    end
+  end
+  def length_otherwise(len) # 10.9.4.2 -> 10.9.3.5-8
+    if len <= 127 then # 10.9.3.6
+      non_negative_binary_integer2(len, 8)
+    elsif len < 16384 then # 10.9.3.7
+      "10#{non_negative_binary_integer2(len, 14)}"
+    else # 10.9.3.8
+      q, r = len.divmod(16384)
+      q2 = [q, 4].min
+      res = "11#{non_negative_binary_integer2(q2, 6)}"
+      ((r == 0) && (q <= 4)) ? res : [res, len - (q2 * 16384)]
+    end
+  end
+  def with_length(len, *len_enc)
+    Enumerator::new{|y|
+      len_str, len_remain = len_enc[0].kind_of?(Symbol) ? send(len_enc[0], len, *len_enc[1..-1]) : len_enc
+      loop{
+        if len_remain then
+          y << [len_str, -len..-(len_remain+1)]
+        else
+          y << [len_str, -len..-1]
+          break
+        end
+        len_str, len_remain = length_otherwise(len = len_remain) # fragmentation
+      }
+    }
+  end
+end
+end
+module Decoder
+class <<self
+  def non_negative_binary_integer(str, bits) # 10.3
+    str.slice!(0, bits).to_i(2)
+  end
+  def twos_complement_binary_integer(str, bits) # 10.4
+    bits -= 1
+    case str.slice!(0)
+    when '0'; 0 
+    when '1'; -(1 << bits) 
+    end + non_negative_binary_integer(str, bits)
+  end
+  def constrainted_whole_number2(str, v_min, v_max) # 10.5.6
+    non_negative_binary_integer(
+        str,
+        Math::log2(v_max - v_min + 1).ceil) + v_min
+  end
+  def constrainted_whole_number(str, v_range) # 10.5.6
+    constrainted_whole_number2(str, *v_range.minmax)
+  end
+  def normally_small_non_negative_whole_number(str, *len_dec) # 10.6
+    case str.slice!(0)
+    when '0'; str.slice!(0, 6).to_i(2) # 10.6.1
+    when '1'; semi_constrained_whole_number(str, 0, *len_dec) # 10.6.2
+    end
+  end
+  def semi_constrained_whole_number(str, v_min, *len_dec) # 10.7
+    len_dec = :length_otherwise if len_dec.empty?
+    v_str = with_length(str, *len_dec).collect{|len_oct|
+      str.slice!(0, len_oct * 8)
+    }.join
+    non_negative_binary_integer(v_str, v_str.size) + v_min
+  end
+  def unconstrained_whole_number(str, *len_dec) # 10.8
+    len_dec = :length_otherwise if len_dec.empty?
+    v_str = with_length(str, *len_dec).collect{|len_oct|
+      str.slice!(0, len_oct * 8)
+    }.join
+    twos_complement_binary_integer(v_str, v_str.size)
+  end
+  def length_constrained_whole_number(str, len_range)
+    if len_range.max < 65536 then # 10.9.4.1
+      (len_range.min == len_range.max) ?
+          len_range.min :
+          constrainted_whole_number(str, len_range)
+    else
+      length_otherwise(str)
+    end
+  end
+  def length_normally_small_length(str) # 10.9.4.2 -> 10.9.3.4
+    case str.slice!(0)
+    when '0'; str.slice!(0, 6).to_i(2) + 1
+    when '1'; length_otherwise(str)
+    end
+  end
+  def length_otherwise(str) # 10.9.4.2 -> 10.9.3.5-8
+    case str.slice!(0)
+    when '0'; non_negative_binary_integer(str, 7) # 10.9.3.6
+    when '1';
+      case str.slice!(0)
+      when '0'; non_negative_binary_integer(str, 14) # 10.9.3.7
+      when '1'; [non_negative_binary_integer(str, 6) * (1 << 14), true] # 10.9.3.8
+      end
+    end
+  end
+  def with_length(str, *len_dec)
+    Enumerator::new{|y|
+      len, cnt = len_dec[0].kind_of?(Symbol) ? send(len_dec[0], str, *len_dec[1..-1]) : len_dec
+      loop{
+        y << len
+        break unless cnt
+        len, cnt = length_otherwise(str) # fragmentation
+      }
+    }
+  end
+end
+end
+end
+end
+
+end

data/lib/gps_pvt/receiver/agps.rb

@@ -0,0 +1,31 @@
+=begin
+AGPS handler for receiver
+=end
+
+module GPS_PVT
+class Receiver
+  def parse_supl(src, opt = {}, &b)
+    $stderr.print "A-GPS (%s) "%[src]
+    opt = {
+      :interval => 60 * 10, # 10 min.
+    }.merge(opt)
+    require_relative '../supl'
+    src_io = Util::open(src)
+    while data = src_io.get_assisted_data
+      data.ephemeris.each{|eph|
+        target = case eph
+        when GPS::Ephemeris; @solver.gps_space_node
+        when GPS::Ephemeris_GLONASS; @solver.glonass_space_node
+        when GPS::Ephemeris_SBAS; @solver.sbas_space_node
+        else nil
+        end
+        critical{target.register_ephemeris(eph.svid, eph)} if target
+      } if data.respond_to?(:ephemeris)
+      critical{
+        @solver.gps_space_node.update_iono_utc(data.iono_utc)
+      } if data.respond_to?(:iono_utc)
+      sleep(opt[:interval])
+    end
+  end
+end
+end

data/lib/gps_pvt/receiver/extension.rb

@@ -57,4 +57,25 @@ class Receiver
     }
   end
 end
+
+module GPS
+
+[
+  Ionospheric_UTC_Parameters,
+  Ephemeris, Ephemeris_SBAS, Ephemeris_GLONASS,
+].each{|cls|
+  cls.class_eval{
+    proc{|func_list|
+      func_list.select!{|func|
+        (/=$/ !~ func.to_s) && func_list.include?("#{func}=".to_sym)
+      }
+      define_method(:to_hash){
+        Hash[*(func_list.collect{|func|
+          [func, send(func)]
+        }.flatten(1))]
+      }
+    }.call(instance_methods(false))
+  }
+}
+end
 end

data/lib/gps_pvt/receiver/rtcm3.rb

@@ -195,7 +195,7 @@ class Receiver
       when 1020
         params = parsed.params
         eph = GPS::Ephemeris_GLONASS::new
-        params[:F_T] ||= 10 # [m]
+        eph.F_T = 10 # [m], default to be overwritten
         params.each{|k, v|
           next if [:P3, :NA, :N_4].include?(k)
           eph.send("#{k}=".to_sym, v)
@@ -205,6 +205,7 @@ class Receiver
               ? date_src \
               : [(ref_time + 3 * 60 * 60).c_tm(leap_sec)])) # UTC -> Moscow time
         }.call([:N_4, :NA].collect{|k| params[k]})
+        eph.N_T = params[:NA] || eph.NA unless params[:N_T] # N_T is available only for GLONASS-M
         eph.rehash(leap_sec)
         critical{
           @solver.glonass_space_node.register_ephemeris(eph.svid, eph)

data/lib/gps_pvt/receiver.rb

@@ -38,20 +38,21 @@ class Receiver
         ] + (pvt.rel_ENU.to_a rescue [nil] * 3)
       } 
     ]] + [proc{
-      labels = [:g, :p, :h, :v, :t].collect{|k| "#{k}dop".to_sym}
+      labels = [:g, :p, :h, :v, :t].collect{|k| "#{k}dop".to_sym} \
+          + [:h, :v, :t].collect{|k| "#{k}sigma".to_sym}
       [
         labels,
         proc{|pvt|
-          next [nil] * 5 unless pvt.position_solved?
+          next [nil] * 8 unless pvt.position_solved?
           labels.collect{|k| pvt.send(k)}
         }
       ]
     }.call] + [[
-      [:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms],
+      [:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms, :vel_sigma],
       proc{|pvt|
         next [nil] * 4 unless pvt.velocity_solved?
         [:north, :east, :down].collect{|k| pvt.velocity.send(k)} \
-            + [pvt.receiver_error_rate] 
+            + [pvt.receiver_error_rate, pvt.vel_sigma] 
       }
     ]] + [
       [:used_satellites, proc{|pvt| pvt.used_satellites}],
@@ -135,7 +136,7 @@ class Receiver
         opt[:satellites].collect{|prn, label|
           pr, rate, doppler, freq = keys.collect{|k| meas_hash[prn][k] rescue nil}
           freq ||= GPS::SpaceNode.L1_Frequency
-          [pr, rate || ((doppler * GPS::SpaceNode::light_speed / freq) rescue nil)]
+          [pr, rate || ((-doppler * GPS::SpaceNode::light_speed / freq) rescue nil)]
         }
       }
     ]]
@@ -324,25 +325,31 @@ class Receiver
   end
 
   GPS::Measurement.class_eval{
-    proc{
-      key2sym = []
-      GPS::Measurement.constants.each{|k|
-        i = GPS::Measurement.const_get(k)
-        key2sym[i] = k if i.kind_of?(Integer)
-      }
-      define_method(:to_a2){
-        to_a.collect{|prn, k, v| [prn, key2sym[k] || k, v]}
-      }
-      define_method(:to_hash2){
-        Hash[*(to_hash.collect{|prn, k_v|
-          [prn, Hash[*(k_v.collect{|k, v| [key2sym[k] || k, v]}.flatten(1))]]
-        }.flatten(1))]
-      }
-    }.call
     add_orig = instance_method(:add)
     define_method(:add){|prn, key, value|
       add_orig.bind(self).call(prn, key.kind_of?(Symbol) ? GPS::Measurement.const_get(key) : key, value)
     }
+    key2sym = GPS::Measurement.constants.inject([]){|res, k|
+      res[GPS::Measurement.const_get(k)] = k if /^L\d/ =~ k.to_s
+      res
+    }
+    define_method(:to_a2){
+      collect{|prn, k, v| [prn, key2sym[k] || k, v]}
+    }
+    cl_hash2 = Class::new(Hash){
+      define_method(:to_meas){
+        GPS::Measurement::new.tap{|res|
+          each{|prn, k_v|
+            k_v.each{|k, v| res.add(prn, k, v)}
+          }
+        }
+      }
+    }
+    define_method(:to_hash2){
+      cl_hash2::new.tap{|res|
+        each{|prn, k, v| (res[prn] ||= {})[key2sym[k] || k] = v}
+      }
+    }
   }
 
   def run(meas, t_meas, ref_pos = @base_station)
@@ -724,4 +731,5 @@ end
 end
 
 require_relative 'receiver/rtcm3'
+require_relative 'receiver/agps'
 require_relative 'receiver/extension'

data/lib/gps_pvt/rtcm3.rb

@@ -104,12 +104,7 @@ class RTCM3
         57 => 16,
         71 => 8,
         76 => 10,
-        77 => proc{
-          idx2meter = [
-              2.40, 3.40, 4.85, 6.85, 9.65, 13.65, 24.00, 48.00,
-              96.00, 192.00, 384.00, 768.00, 1536.00, 3072.00, 6144.00]
-          [4, proc{|v| (v >= idx2meter.size) ? (idx2meter[-1] * 2) : idx2meter[v]}]
-        }.call, # [m]
+        77 => 4,
         78 => 2,
         79 => num_gen.call(14, Rational(sc2rad, 1 << 43)), # [rad/s]
         81 => unum_gen.call(16, 1 << 4), # [sec]
@@ -137,7 +132,7 @@ class RTCM3
         103 => 1,
         104 => 1,
         105 => 1,
-        106 => [2, proc{|v| [0, 30, 45, 60][v] * 60}], # [s]
+        106 => 2,
         107 => [12, proc{|v|
           hh, mm, ss = [v >> 7, (v & 0x7E) >> 1, (v & 0x1) > 0 ? 30 : 0]
           hh * 3600 + mm * 60 + ss # [sec]
@@ -156,15 +151,13 @@ class RTCM3
         125 => num_sign_gen.call(5, Rational(1, 1 << 30)), # [sec], (M)
         126 => 5, # [day]
         127 => 1, # (M)
-        128 => [4, proc{|v|
-          [1, 2, 2.5, 4, 5, 7, 10, 12, 14, 16, 32, 64, 128, 256, 512, 1024][v]
-        }], # [m] (M)
-        129 => 11, # [day]
+        128 => 4, # (M)
+        129 => invalidate.call(unum_gen.call(11), 0), # [day]
         130 => 2, # 1 => GLONASS-M, (M) fields are active 
         131 => 1,
-        132 => 11, # [day]
+        132 => invalidate.call(unum_gen.call(11), 0), # [day]
         133 => num_sign_gen.call(32, Rational(1, 1 << 31)), # [sec]
-        134 => 5, # [4year], (M)
+        134 => invalidate.call(unum_gen.call(5), 0), # [4year], (M)
         135 => num_sign_gen.call(22, Rational(1, 1 << 30)), # [sec], (M)
         136 => 1, # (M)
         137 => 1,
@@ -247,9 +240,11 @@ class RTCM3
       1013 => [2, 3, 51, 52, 53, 54],
       1019 => [2, 9, (76..79).to_a, 71, (81..103).to_a, 137].flatten, # 488 bits @see Table 3.5-21
       1020 => [2, 38, 40, (104..136).to_a].flatten, # 360 bits @see Table 3.5-21
+      # @see BNC Ntrip client DecodeSBASEphemeris() of RTCM3Decorder.cpp
+      # https://software.rtcm-ntrip.org/browser/ntrip/trunk/BNC/src/RTCM3/RTCM3Decoder.cpp
       1043 => [2] + [:prn, :iodn, :tod, :ura, 
           [:xy] * 2, :z, [:dxy] * 2, :dz, [:ddxy] * 2, :ddz,
-          :agf0, :agf1].flatten.collect{|k| "SBAS_#{k}".to_sym}, # @see BNC Ntrip client RTCM3Decorder.cpp
+          :agf0, :agf1].flatten.collect{|k| "SBAS_#{k}".to_sym},
       1044 => [2, (429..457).to_a].flatten, # 485 bits
       1070..1229 => [2, [:uint, 12], [:uint, 30], 393], # 55 bits part of messages will be overwritten
       1071..1077 => [2, 3, 4, 393, 409, [1, 7], 411, 412, 417, 418, 394, 395], # 169 bits @see Table 3.5-78
@@ -311,7 +306,7 @@ class RTCM3
       end
     end
     module GPS_Ephemeris
-      KEY2IDX = {:svid => 1, :WN => 2, :URA => 3, :dot_i0 => 5, :iode => 6, :t_oc => 7,
+      KEY2IDX = {:svid => 1, :WN => 2, :URA_index => 3, :dot_i0 => 5, :iode => 6, :t_oc => 7,
           :a_f2 => 8, :a_f1 => 9, :a_f0 => 10, :iodc => 11, :c_rs => 12, :delta_n => 13,
           :M0 => 14, :c_uc => 15, :e => 16, :c_us => 17, :sqrt_A => 18, :t_oe => 19, :c_ic => 20,
           :Omega0 => 21, :c_is => 22, :i0 => 23, :c_rc => 24, :omega => 25, :dot_Omega0 => 26,
@@ -319,22 +314,12 @@ class RTCM3
       def params
         # TODO WN is truncated to 0-1023
         res = Hash[*(KEY2IDX.collect{|k, i| [k, self[i][0]]}.flatten(1))]
-        res[:fit_interval] = ((self[29] == 0) ? 4 : case res[:iodc] 
-          when 240..247; 8
-          when 248..255, 496; 14
-          when 497..503; 26
-          when 504..510; 50
-          when 511, 752..756; 74
-          when 757..763; 98
-          when 764..767, 1088..1010; 122
-          when 1011..1020; 146
-          else; 6
-        end) * 60 * 60
+        res[:fit_interval] = (self[29][0] != 0)
         res
       end
     end
     module SBAS_Ephemeris
-      KEY2IDX = {:svid => 1, :iodn => 2, :tod => 3, :URA => 4,
+      KEY2IDX = {:svid => 1, :iodn => 2, :tod => 3, :URA_index => 4,
           :x => 5, :y => 6, :z => 7,
           :dx => 8, :dy => 9, :dz => 10,
           :ddx => 11, :ddy => 12, :ddz => 13,
@@ -349,18 +334,18 @@ class RTCM3
         # TODO insufficient: :n => ?(String4); extra: :P3
         # TODO generate time with t_b, N_T, NA, N_4
         # TODO GPS.i is required to modify to generate EPhemeris_with_GPS_Time
-        k_i =  {:svid => 1, :freq_ch => 2, :P1 => 5, :t_k => 6, :B_n => 7, :P2 => 8, :t_b => 9,
+        k_i =  {:svid => 1, :freq_ch => 2, :P1_index => 5, :t_k => 6, :B_n => 7, :P2 => 8, :t_b => 9,
             :xn_dot => 10, :xn => 11, :xn_ddot => 12,
             :yn_dot => 13, :yn => 14, :yn_ddot => 15,
             :zn_dot => 16, :zn => 17, :zn_ddot => 18,
             :P3 => 19, :gamma_n => 20, :p => 21, :tau_n => 23, :delta_tau_n => 24, :E_n => 25,
-            :P4 => 26, :F_T => 27, :N_T => 28, :M => 29}
+            :P4 => 26, :F_T_index => 27, :N_T => 28, :M => 29}
         k_i.merge!({:NA => 31, :tau_c => 32, :N_4 => 33, :tau_GPS => 34}) if self[30][0] == 1 # check DF131
         res = Hash[*(k_i.collect{|k, i| [k, self[i][0]]}.flatten(1))]
         res.reject!{|k, v|
           case k
           when :N_T; v == 0
-          when :p, :delta_tau_n, :P4, :F_T, :N_4, :tau_GPS; true # TODO sometimes delta_tau_n is valid?
+          when :p, :delta_tau_n, :P4, :F_T_index, :N_4, :tau_GPS; true # TODO sometimes delta_tau_n is valid?
           else; false
           end
         } if (res[:M] != 1) # check DF130
@@ -372,12 +357,12 @@ class RTCM3
           :iode => 6, :c_rs => 7, :delta_n => 8, :M0 => 9, :c_uc => 10, :e => 11,
           :c_us => 12, :sqrt_A => 13, :t_oe => 14, :c_ic => 15, :Omega0 => 16,
           :c_is => 17, :i0 => 18, :c_rc => 19, :omega => 20, :dot_Omega0 => 21,
-          :dot_i0 => 22, :WN => 24, :URA => 25, :SV_health => 26,
+          :dot_i0 => 22, :WN => 24, :URA_index => 25, :SV_health => 26,
           :t_GD => 27, :iodc => 28}
       def params
         # TODO PRN = svid + 192, WN is truncated to 0-1023
         res = Hash[*(KEY2IDX.collect{|k, i| [k, self[i][0]]}.flatten(1))]
-        res[:fit_interval] = (self[29] == 0) ? 2 * 60 * 60 : nil # TODO how to treat fit_interval > 2 hrs
+        res[:fit_interval] = (self[29][0] != 0)
         res
       end
     end