Add SEM/YUMA almanac support

lib/gps_pvt/receiver.rb

@@ -732,4 +732,5 @@ def attach_rinex_clk(src)
 
 require_relative 'receiver/rtcm3'
 require_relative 'receiver/agps'
+require_relative 'receiver/almanac'
 require_relative 'receiver/extension'

lib/gps_pvt/receiver/almanac.rb

@@ -0,0 +1,137 @@
+=begin
+Additional Almanac handler for receiver
+=end
+
+module GPS_PVT
+class Receiver
+  def correct_week_sem_yuma_almanac(src, week_rem = 0)
+    t_ref = case src.to_s
+    when /www\.navcen\.uscg\.gov\/.*\/(\d{4})\//
+      # ex) https://www.navcen.uscg.gov/sites/default/files/gps/almanac/20XX/(Sem|Yuma)/003.(al3|alm)
+      GPS_PVT::GPS::Time::new(Time::new($1.to_i).to_a.slice(0, 6).reverse)
+    when /www\.navcen\.uscg\.gov\/.*\/current_(sem|yuma)/
+      GPS_PVT::GPS::Time::now
+    else
+      raise
+    end
+    q, rem = t_ref.week.divmod(1024)
+    delta = rem - (week_rem % 1024)
+    if delta <= -512 then
+      q -= 1
+    elsif delta > 512 then
+      q += 1
+    end
+    q * 1024 + week_rem
+  end
+
+  def parse_sem_almanac(src)
+    src_io = open(Util::get_txt(src))
+    raise unless src_io.readline =~ /(\d+)\s+(\S+)/ # line 1
+    num, name = [$1.to_i, $2]
+    raise unless src_io.readline =~ /(\d+)\s+(\d+)/ # line 2
+    week, t_oa = [$1.to_i, $2.to_i]
+    week = correct_week_sem_yuma_almanac(src, week)
+    src_io.readline # line 3
+
+    num.times.each{
+      eph = GPS::Ephemeris::new
+      9.times{|i| # line R-1..9
+        case i
+        when 0, 1, 2, 6, 7
+          # N/A items; 1 => SV reference number, 7 => configuration code
+          k = {0 => :svid, 2 => :URA_index, 6 => :SV_health}[i]
+          v = Integer(src_io.readline)
+          eph.send("#{k}=".to_sym, v) if k
+        when 3..5
+          res = src_io.readline.scan(/[+-]?\d+(?:\.\d+)?(?:E[+-]\d+)?/).collect{|s| Float(s)}
+          raise unless res.size == 3
+          res.zip({
+            3 => [:e, [:i0, GPS::GPS_SC2RAD], [:dot_Omega0, GPS::GPS_SC2RAD]],
+            4 => [:sqrt_A, [:Omega0, GPS::GPS_SC2RAD], [:omega, GPS::GPS_SC2RAD]],
+            5 => [[:M0, GPS::GPS_SC2RAD], :a_f0, :a_f1],
+          }[i]).each{|v, (k, sf)|
+            eph.send("#{k}=".to_sym, sf ? (sf * v) : v)
+          }
+        when 8
+          src_io.readline
+        end
+      }
+      eph.i0 = GPS::GPS_SC2RAD * 0.3 + eph.i0
+      eph.WN = week
+      eph.t_oc = eph.t_oe = t_oa
+      [:iodc, :t_GD, :a_f2, :iode, :c_rs, :delta_n,
+          :c_uc, :c_us, :c_ic, :c_is, :c_rc, :dot_i0, :iode_subframe3].each{|k|
+        eph.send("#{k}=", 0)
+      }
+      critical{@solver.gps_space_node.register_ephemeris(eph.svid, eph)}
+    }
+
+    $stderr.puts "Read SEM Almanac file (%s): %d items."%[src, num]
+  end
+
+  YUMA_ITEMS = [
+    [proc{|s| s.to_i}, {
+      :ID => :svid,
+      :Health => :SV_health,
+      :week => :WN,
+    }],
+    [proc{|s| Float(s)}, {
+      :Eccentricity => :e,
+      "Time of Applicability" => [:t_oc, :t_oe],
+      "Orbital Inclination" => :i0,
+      "Rate of Right Ascen" => :dot_Omega0,
+      'SQRT\(A\)' => :sqrt_A,
+      "Right Ascen at Week" => :Omega0,
+      "Argument of Perigee" => :omega,
+      "Mean Anom" => :M0,
+      "Af0" => :a_f0,
+      "Af1" => :a_f1,
+    }],
+  ].collect{|cnv, key_list|
+    key_list.collect{|k1, k2_list|
+      [/#{k1}[^:]*:/, cnv,
+          *([k2_list].flatten(1).collect{|k2|
+            "#{k2}=".to_sym
+          })]
+    }
+  }.flatten(1)
+
+  def parse_yuma_almanac(src)
+    src_io = open(Util::get_txt(src))
+    num = 0
+
+    idx_line = -1
+    eph, items = nil
+    while !src_io.eof?
+      line = src_io.readline.chomp
+      if idx_line < 0 then
+        if line =~ /^\*{8}/ then
+          eph = GPS::Ephemeris::new
+          items = YUMA_ITEMS.clone
+          idx_line = 0
+        end
+        next
+      end
+      raise unless i = items.index{|re, cnv, *k_list|
+        next false unless re =~ line
+        v = cnv.call($')
+        k_list.each{|k| eph.send(k, v)}
+        true
+      }
+      items.delete_at(i)
+      next unless items.empty?
+
+      [:iodc, :t_GD, :a_f2, :iode, :c_rs, :delta_n,
+          :c_uc, :c_us, :c_ic, :c_is, :c_rc, :dot_i0, :iode_subframe3].each{|k|
+        eph.send("#{k}=", 0)
+      }
+      eph.WN = correct_week_sem_yuma_almanac(src, eph.WN)
+      critical{@solver.gps_space_node.register_ephemeris(eph.svid, eph)}
+      num += 1
+      idx_line = -1
+    end
+
+    $stderr.puts "Read YUMA Almanac file (%s): %d items."%[src, num]
+  end
+end
+end

spec/gps_pvt/almanac_spec.rb

@@ -0,0 +1,75 @@
+# frozen_string_literal: true
+
+require 'rspec'
+
+require 'gps_pvt'
+
+RSpec::describe GPS_PVT::Receiver do
+  let(:src){{
+    :SEM => URI::parse(
+        "https://www.navcen.uscg.gov/sites/default/files/gps/almanac/current_sem.al3"),
+    :YUMA => URI::parse(
+        "https://www.navcen.uscg.gov/sites/default/files/gps/almanac/current_yuma.alm"),
+    :SUPL => URI::parse(
+        "supl://supl.google.com/"),
+    :RTCM3 => URI::parse(
+        "ntrip://test%40example.com:[email protected]:2101/RTCM3EPH"),
+  }}
+  let(:receiver){
+    rcv = described_class.new
+    rcv.define_singleton_method(:hook_new_ephemeris){|*args|}
+    rcv.solver.instance_eval{
+      [:gps_space_node, :sbas_space_node, :glonass_space_node].each{|k|
+        k_var = "@#{k}"
+        instance_variable_set(k_var, send(k))
+        v = instance_variable_get(k_var)
+        define_singleton_method(k){v}
+        orig = v.method(:register_ephemeris)
+        v.define_singleton_method(:register_ephemeris){|svid, eph|
+          rcv.hook_new_ephemeris(k, svid, eph)
+          orig.call(svid, eph)
+        }
+      }
+    }
+    rcv
+  }
+  def cmp(args_src1, args_src2)
+    expect(1).to be_within(10E3).of(1)
+    base_time = GPS_PVT::GPS::Time::now
+    c_light = GPS_PVT::GPS::SpaceNode::light_speed
+    eph_cache = {}
+    rcv = receiver
+    rcv.define_singleton_method(:hook_new_ephemeris){|sn, svid, eph_a|
+      eph_cache[[sn, svid]] = eph_a
+    }
+    rcv.send(*args_src1)
+
+    comarator = proc{|eph_a, eph_b|
+      values = [eph_a, eph_b].collect{|eph|
+        #eph.to_hash.each{|k, v| $stderr.puts [k, v].join(',')}
+        pos, vel, delta_t, delta_dt = eph.constellation(base_time)
+        [pos.to_a, delta_t * c_light, vel.to_a, delta_dt * c_light].flatten
+      }.transpose
+      values[0..3].each{|a, b| # check only in position
+        expect(a).to be_within(10E3).of(b) # < 10 km
+      }
+    }
+
+    rcv.define_singleton_method(:hook_new_ephemeris){|sn, svid, eph_b|
+      next unless eph_a = eph_cache.delete([sn, svid])
+      comarator.call(eph_a, eph_b)
+      Thread::exit if eph_cache.empty?
+    }
+    Timeout::timeout(60){
+      Thread::new{rcv.send(*args_src2)}.join
+    } rescue $stderr.puts "Timeout with #{eph_cache.size} item comparison remained"
+  end
+  it "can use SEM almanac" do
+    cmp([:parse_sem_almanac, src[:SEM]], [:parse_supl, src[:SUPL]])
+    #cmp([:parse_sem_almanac, src[:SEM]], [:parse_rtcm3, src[:RTCM3]])
+  end
+  it "can use YUMA almanac" do
+    cmp([:parse_yuma_almanac, src[:YUMA]], [:parse_supl, src[:SUPL]])
+    #cmp([:parse_yuma_almanac, src[:YUMA]], [:parse_rtcm3, src[:RTCM3]])
+  end
+end