supl.rb

require_relative 'upl/upl'
require_relative 'GPS'

require 'socket'
require 'openssl'

module GPS_PVT
class SUPL_Client
  include UPL
  
  attr_accessor :opts
  
  def initialize(host, opts = {})
    @host = host
    @opts = {
      :port => 7275,
      :debug => 0,
      :protocol => [:lpp, :rrlp],
    }.merge(opts)
  end

  def get_assisted_data
    begin
      @socket = TCPSocket::new(@host, @opts[:port])
      if @opts[:port] == 7275 then
        @socket = OpenSSL::SSL::SSLSocket::new(@socket)
        @socket.connect
      end
      send_supl_start
      recv_supl_response
      send_supl_pos_init
      recv_supl_pos
    ensure
      @socket.close unless @socket.closed?
    end
  end
  
  private

  def send(cmd)
    msg = encode(cmd)
    (iter = proc{|keys, root1, root2, parents|
      keys.each{|k|
        if k.kind_of?(Hash) then
          k.each{|k2, v2|
            iter.call(v2, root1[k2], root2[k2], parents + [k2])
          }
        else
          $stderr.puts [parents + [k], root1[k], root2[k]].inspect unless root1[k] == root2[k]
        end
      }
    }).call(cmd.all_keys - [:length], cmd, decode(msg), []) if @opts[:debug] > 2
    $stderr.puts [
        msg.unpack("C*").collect{|byte| "%02X"%[byte]}.join(' '),
        decode(msg)].inspect if @opts[:debug] > 1
    $stderr.puts "tx(#{msg.size})" if @opts[:debug] > 0
    @socket.write(msg)
  end

  def receive
    raw = @socket.read(2)
    raw += @socket.read(raw.unpack("n")[0] - 2)
    res = decode(raw)
    $stderr.puts "rx(#{raw.size})" if @opts[:debug] > 0
    $stderr.puts [raw, res] if @opts[:debug] > 1
    res
  end

  def send_supl_start
    cmd = generate_skeleton(:SUPLSTART, {:version => 2})
    cmd[:sessionID][:setSessionID] = {
      :sessionId => 1,
      :setId => proc{
        # Ex) :msisdn => [0xFF, 0xFF, 0x91, 0x94, 0x48, 0x45, 0x83, 0x98],
        next {:msisdn => @opts[:msisdn]} if @opts[:msisdn]
        # Ex) :imsi => "440109012345678",
        next {:imsi => @opts[:imsi].scan(/(.)(.?)/).collect{|a, b|
          "0x#{a}#{b == '' ? '0' : b}".to_i(16)
        }} if @opts[:imsi]
        require 'resolv'
        remote_ip = Resolv::DNS.new({:nameserver => "resolver1.opendns.com"}) \
            .getresources("myip.opendns.com", Resolv::DNS::Resource::IN::A)[0].address
        {:iPAddress => case remote_ip
        when Resolv::IPv4; {:ipv4Address => remote_ip.address.unpack("C4")}
        when Resolv::IPv6; {:ipv6Address => remote_ip.address.unpack("C16")}
        else; raise
        end}
      }.call
    }
    proc{|cap|
      cap[:posTechnology].keys.each{|k|
        cap[:posTechnology][k] = [:agpsSETBased].include?(k)
      }
      cap[:posTechnology][:"ver2-PosTechnology-extension"] = {
        :gANSSPositionMethods => [
          {
            :ganssId => 1, # SBAS
            :ganssSBASid => [0, 1, 0], # MSAS
            :gANSSPositioningMethodTypes => {:setAssisted => true, :setBased => true, :autonomous => false},
            :gANSSSignals => [1] * 8,
          },
          {
            :ganssId => 3, # QZSS
            :gANSSPositioningMethodTypes => {:setAssisted => true, :setBased => true, :autonomous => false},
            :gANSSSignals => [1] * 8,
          },
          {
            :ganssId => 4, # GLONASS
            :gANSSPositioningMethodTypes => {:setAssisted => true, :setBased => true, :autonomous => false},
            :gANSSSignals => [1] * 8,
          },
        ],
      }
      cap[:prefMethod] = :agpsSETBasedPreferred
      cap[:posProtocol].keys.each{|k|
        cap[:posProtocol][k] = [:rrlp].include?(k)
      }
      cap[:posProtocol][:"ver2-PosProtocol-extension"] = {
        :lpp => @opts[:protocol].include?(:lpp),
        :posProtocolVersionRRLP => {
          :majorVersionField => 17,
          :technicalVersionField => 0,
          :editorialVersionField => 0,
        },
        :posProtocolVersionLPP => {
          :majorVersionField => 17,
          :technicalVersionField => 5,
          :editorialVersionField => 0,
        },
      }
      @capability = cap
    }.call(cmd[:message][:msSUPLSTART][:sETCapabilities])
    proc{|loc|
      loc[:cellInfo] = {
        :gsmCell => {:refMCC => 244, :refMNC => 5, :refLAC => 23010, :refCI => 12720},
      }
      loc[:status] = :current
      @location_id = loc
    }.call(cmd[:message][:msSUPLSTART][:locationId])
      
    send(cmd)
  end
  
  
  def recv_supl_response
    data = receive
    @session_id = data[:sessionID]
  end

  def send_supl_pos_init
    cmd = generate_skeleton(:SUPLPOSINIT, {:version => 2})
    cmd[:sessionID] = @session_id
    proc{|posinit|
      posinit[:sETCapabilities] = @capability
      posinit[:requestedAssistData] = Hash[*([
        :almanacRequested,
        :utcModelRequested,
        :ionosphericModelRequested,
        :dgpsCorrectionsRequested,
        :referenceLocationRequested,
        :referenceTimeRequested,
        :acquisitionAssistanceRequested,
        :realTimeIntegrityRequested,
        :navigationModelRequested,
      ].collect{|k|
        [k, [
              :utcModelRequested,
              :ionosphericModelRequested,
              :referenceLocationRequested,
              :referenceTimeRequested,
              :acquisitionAssistanceRequested,
              :realTimeIntegrityRequested,
              :navigationModelRequested,
            ].include?(k)]
      }.flatten(1))]
      posinit[:requestedAssistData][:"ver2-RequestedAssistData-extension"] = {
        :ganssRequestedCommonAssistanceDataList => {
          :ganssReferenceTime => true,
          :ganssIonosphericModel => true,
          :ganssAdditionalIonosphericModelForDataID00 => false,
          :ganssAdditionalIonosphericModelForDataID11 => false,
          :ganssEarthOrientationParameters => false,
          :ganssAdditionalIonosphericModelForDataID01 => false,
        },
        :ganssRequestedGenericAssistanceDataList => [
          # SBAS
          {:ganssId => 1, :ganssSBASid => [0, 1, 0], # MSAS
              :ganssRealTimeIntegrity => true, :ganssAlmanac => false,
              :ganssNavigationModelData => {:ganssWeek => 0, :ganssToe => 0, :"t-toeLimit" => 0},
              :ganssReferenceMeasurementInfo => false, :ganssUTCModel => true, :ganssAuxiliaryInformation => false},
          # QZSS
          {:ganssId => 3, :ganssRealTimeIntegrity => true, :ganssAlmanac => false,
              :ganssNavigationModelData => {:ganssWeek => 0, :ganssToe => 0, :"t-toeLimit" => 0},
              :ganssReferenceMeasurementInfo => false, :ganssUTCModel => true, :ganssAuxiliaryInformation => false},
          # GLONASS
          {:ganssId => 4, :ganssRealTimeIntegrity => true, :ganssAlmanac => false,
              :ganssNavigationModelData => {:ganssWeek => 0, :ganssToe => 0, :"t-toeLimit" => 0},
              :ganssReferenceMeasurementInfo => false, :ganssUTCModel => true, :ganssAuxiliaryInformation => false},
        ],
      }
      posinit[:requestedAssistData][:navigationModelData] = {
        :gpsWeek => 0,
        :gpsToe => 0,
        :nSAT => 0,
        :toeLimit => 0,
      } if false
      posinit[:locationId] = @location_id
    }.call(cmd[:message][:msSUPLPOSINIT])
    
    send(cmd)
  end

  def recv_supl_pos
    res = {}
    merge = proc{|src, dst|
      src.each{|k, v|
        case dst[k]
        when Hash; merge.call(v, dst[k])
        when Array; dst[k] += v
        when nil; dst[k] = v
        end
      }
    }
    data = receive
    if data[:message][:msSUPLPOS][:posPayLoad][:"ver2-PosPayLoad-extension"] then
      merge.call(
          data[:message][:msSUPLPOS][:posPayLoad][:"ver2-PosPayLoad-extension"][:lPPPayload].decode[:"lpp-MessageBody"],
          res)
      attach_lpp(res)
    else
      while true
        rrlp_data = data[:message][:msSUPLPOS][:posPayLoad][:rrlpPayload].decode
        merge.call(
            rrlp_data[:component][:assistanceData][:"gps-AssistData"][:controlHeader],
            res)
        break unless (rrlp_data[:component][:assistanceData][:moreAssDataToBeSent] == :moreMessagesOnTheWay)
        
        # SUPL-POS + RRLP-assistanceDataAck
        cmd = generate_skeleton(:SUPLPOS, {:version => 2})
        cmd[:sessionID] = @session_id
        cmd[:message][:msSUPLPOS] = {
          :posPayLoad => {:rrlpPayload => {
            :referenceNumber => rrlp_data[:referenceNumber],
            :component => {:assistanceDataAck => nil}
          }}
        }
        send(cmd)
        data = receive
      end
      attach_rrlp(res)
    end
    res
  end
  
  def SUPL_Client.correct_gps_week(week, t_gps_ref, width = 1024)
    width_half = width / 2
    cycle, week_rem_ref = t_gps_ref.week.divmod(width)
    week_rem = week % width
    delta = week_rem_ref - week_rem
    if delta > width_half then
      cycle += 1
    elsif delta < -width_half then
      cycle -= 1
  end
    cycle * width + week_rem
  end
  
  EPH_KEY_TBL_RRLP = Hash[*({
    :URA_index  => :URA,
    :dot_i0     => [:IDot, -43, true],
    #:iode       => nil,
    :t_oc       => [:Toc, 4],
    :a_f2       => [:AF2, -55],
    :a_f1       => [:AF1, -43],
    :a_f0       => [:AF0, -31],
    :iodc       => :IODC,
    :c_rs       => [:Crs, -5],
    :delta_n    => [:DeltaN, -43, true],
    :M0         => [:M0, -31, true],
    :c_uc       => [:Cuc, -29],
    :e          => [:E, -33],
    :c_us       => [:Cus, -29],
    :sqrt_A     => [:APowerHalf, -19],
    :t_oe       => [:Toe, 4],
    :c_ic       => [:Cic, -29],
    :Omega0     => [:OmegaA0, -31, true],
    :c_is       => [:Cis, -29],
    :i0         => [:I0, -31, true],
    :c_rc       => [:Crc, -5],
    :omega      => [:W, -31, true],
    :dot_Omega0 => [:OmegaADot, -43, true],
    :t_GD       => [:Tgd, -31],
    :SV_health  => :SVhealth,
  }.collect{|dst_k, (src_k, sf_pow2, sc2rad)|
    sf_pow2 ||= 0
    sf = sf_pow2 < 0 ? Rational(1, 1 << -sf_pow2) : (1 << sf_pow2)
    sf = sf.to_f * GPS::GPS_SC2RAD if sc2rad
    ["#{dst_k}=".to_sym, ["ephem#{src_k}".to_sym, sf]]
  }.flatten(1))]
=begin
:ephemCodeOnL2 
:ephemL2Pflag
:ephemAODA
=end
  
  def attach_rrlp(msg)
    t_gps = proc{
      week_rem, sec008 = [:gpsWeek, :gpsTOW23b].collect{|k| msg[:referenceTime][:gpsTime][k]}
      GPS::Time::new(
          SUPL_Client::correct_gps_week(week_rem, GPS::Time::now),
          Rational(sec008 * 8, 100).to_f)
    }.call
    msg.define_singleton_method(:ref_time){t_gps}
    msg.define_singleton_method(:iono_utc){
      params = GPS::Ionospheric_UTC_Parameters::new
      iono = self[:ionosphericModel]
      a, b = (0..3).collect{|i|
        [iono["alfa#{i}".to_sym], iono["beta#{i}".to_sym]]
      }.transpose
      params.alpha = a.zip([-30, -27, -24, -24]).collect{|v, pow2|
        (v * Rational(1, 1 << -pow2)).to_f
      }
      params.beta = b.zip([11, 14, 16, 16]).collect{|v, pow2|
        v * (1 << pow2)
      }
      utc = self[:utcModel]
      [:A1, :A0, [:t_ot, :Tot], [:WN_t, :WNt], [:delta_t_LS, :DeltaTls],
          [:WN_LSF, :WNlsf], :DN, [:delta_t_LSF, :DeltaTlsf]].each{|k_dst, k_src|
        params.send("#{k_dst}=".to_sym, utc["utc#{k_src || k_dst}".to_sym])
      }
      {:A1 => -50, :A0 => -30, :t_ot => 12}.each{|k, pow2|
        params.send("#{k}=".to_sym,
            params.send(k) * (pow2 < 0 ? Rational(1, 1 << -pow2) : (1 << pow2)))
      }
      params.WN_t = SUPL_Client::correct_gps_week(params.WN_t, t_gps, 256)
      params.WN_LSF = SUPL_Client::correct_gps_week(params.WN_LSF, t_gps, 256)
      params
    }
    msg.define_singleton_method(:ephemeris){
      self[:navigationModel][:navModelList].collect{|v|
        eph = GPS::Ephemeris::new
        eph.svid = v[:satelliteID] + 1
        eph_src = v[:satStatus][:newSatelliteAndModelUC]
        EPH_KEY_TBL_RRLP.each{|dst_k, (src_k, sf)|
          v = sf * eph_src[src_k]
          eph.send(dst_k, v.kind_of?(Rational) ? v.to_f : v)
        }
        eph.WN = t_gps.week
        delta_sec = t_gps.seconds - eph.t_oe
        if delta_sec > GPS::Time::Seconds_week / 2 then
          eph.WN += 1
        elsif delta_sec < -GPS::Time::Seconds_week / 2 then
          eph.WN -= 1
        end
        eph.iode = eph.iodc & 0xFF
        eph.fit_interval = (eph_src[:ephemFitFlag] != 0)
        eph
      }
    }
    msg
  end

  EPH_KEY_TBL_LPP = Hash[*({
    :URA_index  => :URA,
    :dot_i0     => [:IDot, -43, true],
    #:iode       => nil,
    :t_oc       => [:Toc, 4],
    :a_f2       => [:af2, -55],
    :a_f1       => [:af1, -43],
    :a_f0       => [:af0, -31],
    #:iodc       => :IODC,
    :c_rs       => [:Crs, -5],
    :delta_n    => [:DeltaN, -43, true],
    :M0         => [:M0, -31, true],
    :c_uc       => [:Cuc, -29],
    :e          => [:E, -33],
    :c_us       => [:Cus, -29],
    :sqrt_A     => [:APowerHalf, -19],
    :t_oe       => [:Toe, 4],
    :c_ic       => [:Cic, -29],
    :Omega0     => [:OmegaA0, -31, true],
    :c_is       => [:Cis, -29],
    :i0         => [:I0, -31, true],
    :c_rc       => [:Crc, -5],
    :omega      => [:Omega, -31, true],
    :dot_Omega0 => [:OmegaADot, -43, true],
    :t_GD       => [:Tgd, -31],
  }.collect{|dst_k, (src_k, sf_pow2, sc2rad)|
    sf_pow2 ||= 0
    sf = sf_pow2 < 0 ? Rational(1, 1 << -sf_pow2) : (1 << sf_pow2)
    sf = sf.to_f * GPS::GPS_SC2RAD if sc2rad
    ["#{dst_k}=".to_sym, ["nav#{src_k}".to_sym, sf]]
  }.flatten(1))]
=begin
:ephemCodeOnL2 
:ephemL2Pflag
:ephemAODA
=end
  
  EPH_KEY_TBL_LPP_GLO = Hash[*({
    :xn => [:X, Rational(1000, 1 << 11)], :xn_dot => [:Xdot, Rational(1000, 1 << 20)], :xn_ddot => [:Xdotdot, Rational(1000, 1 << 30)], 
    :yn => [:Y, Rational(1000, 1 << 11)], :yn_dot => [:Ydot, Rational(1000, 1 << 20)], :yn_ddot => [:Ydotdot, Rational(1000, 1 << 30)],
    :zn => [:Z, Rational(1000, 1 << 11)], :zn_dot => [:Zdot, Rational(1000, 1 << 20)], :zn_ddot => [:Zdotdot, Rational(1000, 1 << 30)],
    :tau_n => [:Tau, Rational(1, 1 << 30)],
    :gamma_n => [:Gamma, Rational(1, 1 << 40)],
    :M => :M, :P1_index => :P1, :P2 => :P2, :E_n => :En,
  }.collect{|dst_k, (src_k, sf)|
    ["#{dst_k}=".to_sym, ["glo#{src_k}".to_sym, sf || 1]]
  }.flatten(1))]
=begin
:has_string=, :raw=, :t_k=,
:N_T=, :p=, :delta_tau_n=, :P4=,
:tau_GPS=, :tau_c=, :day_of_year=, :year=, :n=, :freq_ch=
=end
  
  def attach_lpp(msg)
    t_gps = proc{|data|
      raise unless data[:"gnss-TimeID"][:"gnss-id"] == :gps
      dayn, tod, msec = [:"gnss-DayNumber", :"gnss-TimeOfDay", :"gnss-TimeOfDayFrac-msec"].collect{|k|
        data[k]
      }
      week, dow = dayn.divmod(7)
      GPS::Time::new(week, dow * 24 * 3600 + tod + Rational(msec, 1000).to_f)
    }.call(msg[:c1][:provideAssistanceData][:criticalExtensions][:c1] \
        [:"provideAssistanceData-r9"][:"a-gnss-ProvideAssistanceData"] \
        [:"gnss-CommonAssistData"][:"gnss-ReferenceTime"][:"gnss-SystemTime"])
    msg.define_singleton_method(:ref_time){t_gps}
    iono_utc = proc{
      params = GPS::Ionospheric_UTC_Parameters::new
      proc{|data|
        a, b = (0..3).collect{|i|
          [data["alfa#{i}".to_sym], data["beta#{i}".to_sym]]
        }.transpose
        params.alpha = a.zip([-30, -27, -24, -24]).collect{|v, pow2|
          (v * Rational(1, 1 << -pow2)).to_f
        }
        params.beta = b.zip([11, 14, 16, 16]).collect{|v, pow2|
          v * (1 << pow2)
        }
      }.call(msg[:c1][:provideAssistanceData][:criticalExtensions][:c1] \
          [:"provideAssistanceData-r9"][:"a-gnss-ProvideAssistanceData"] \
          [:"gnss-CommonAssistData"][:"gnss-IonosphericModel"][:klobucharModel])
      proc{|data|
        [[:A1], [:A0], [:t_ot, :Tot],
            [:WN_t, :WNt], [:delta_t_LS, :DeltaTlsf],
            [:WN_LSF, :WNlsf], [:DN], [:delta_t_LSF, :DeltaTlsf]].each{|k_dst, k_src|
          params.send("#{k_dst}=".to_sym, data["gnss-Utc-#{k_src || k_dst}".to_sym])
        }
      }.call(msg[:c1][:provideAssistanceData][:criticalExtensions][:c1] \
          [:"provideAssistanceData-r9"][:"a-gnss-ProvideAssistanceData"] \
          [:"gnss-GenericAssistData"].select{|v| v[:"gnss-ID"][:"gnss-id"] == :gps}[0] \
          [:"gnss-UTC-Model"][:utcModel1])
      {:A1 => -50, :A0 => -30, :t_ot => 12}.each{|k, pow2|
        params.send("#{k}=".to_sym,
            params.send(k) * (pow2 < 0 ? Rational(1, 1 << -pow2) : (1 << pow2)))
      }
      params.WN_t = SUPL_Client::correct_gps_week(params.WN_t, t_gps, 256)
      params.WN_LSF = SUPL_Client::correct_gps_week(params.WN_LSF, t_gps, 256)
      params
    }.call
    leap_seconds = iono_utc.delta_t_LS rescue t_gps.leap_seconds
    msg.define_singleton_method(:iono_utc){iono_utc}
    extract_gps_ephemeris = proc{|sat_list, sys|
      offset = {:gps => 1, :qzss => 193}[sys]
      sat_list.collect{|v|
        eph = GPS::Ephemeris::new
        eph.svid = v[:svID][:"satellite-id"] + offset
        eph_src = v[:"gnss-ClockModel"][:"nav-ClockModel"].merge(v[:"gnss-OrbitModel"][:"nav-KeplerianSet"])
        EPH_KEY_TBL_LPP.each{|dst_k, (src_k, sf)|
          v2 = sf * eph_src[src_k]
          eph.send(dst_k, v2.kind_of?(Rational) ? v2.to_f : v2)
        }
        eph.iodc = Integer(v[:iod].join, 2)
        eph.iode = (eph.iodc & 0xFF)
        eph.SV_health = Integer(v[:svHealth][0..5].join, 2)
        eph.WN = t_gps.week
        delta_sec = t_gps.seconds - eph.t_oe
        if delta_sec > GPS::Time::Seconds_week / 2 then
          eph.WN += 1
        elsif delta_sec < -GPS::Time::Seconds_week / 2 then
          eph.WN -= 1
        end
        eph.fit_interval = (eph_src[:navFitFlag] != 0)
        eph
      }
    }
    msg.define_singleton_method(:ephemeris){
      assist_data = self[:c1][:provideAssistanceData][:criticalExtensions][:c1] \
          [:"provideAssistanceData-r9"][:"a-gnss-ProvideAssistanceData"] \
          [:"gnss-GenericAssistData"]
      res = [:gps, :qzss].collect{|k|
        extract_gps_ephemeris.call(
            assist_data.select{|v| v[:"gnss-ID"][:"gnss-id"] == k}[0] \
              [:"gnss-NavigationModel"][:"gnss-SatelliteList"], k)
      }.flatten(1)
      assist_data_glo = assist_data.select{|v| v[:"gnss-ID"][:"gnss-id"] == :glonass}[0]
      utc_params_glo = {
        :tau_c= => [:tauC, Rational(1, 1 << 31)],
      }.collect{|dst_k, (src_k, sf)|
        [dst_k, sf * assist_data_glo[:"gnss-UTC-Model"][:utcModel3][src_k]]
      }
      res += assist_data_glo[:"gnss-NavigationModel"][:"gnss-SatelliteList"].collect{|sat|
        eph = GPS::Ephemeris_GLONASS::new
        eph.svid = sat[:svID][:"satellite-id"] + 1
        eph_src = sat[:"gnss-ClockModel"][:"glonass-ClockModel"].merge(
            sat[:"gnss-OrbitModel"][:"glonass-ECEF"])
        (EPH_KEY_TBL_LPP_GLO.collect{|dst_k, (src_k, sf)|
          v = eph_src[src_k]
          [dst_k, sf.send(sf.kind_of?(Proc) ? :call : :*, case v
              when Array; Integer(v.join, 2)
              when true; 1
              when false; 0
              else; v
              end)]
        } + utc_params_glo).each{|dst_k, v|
          eph.send(dst_k, v.kind_of?(Rational) ? v.to_f : v)
        }
        eph.B_n = sat[:svHealth][0]
        eph.F_T_index = Integer(sat[:svHealth][1..4].join, 2)
        eph.t_b = Integer(sat[:iod][4..-1].join, 2) * 15 * 60
        eph.set_date((t_gps + 3 * 60 * 60).c_tm(leap_seconds)) # UTC -> Moscow time
        eph.N_T = eph.NA
        eph.rehash(leap_seconds)
        eph
      }
      res
    }
    msg
  end
end
end

require 'open-uri'

OpenURI.class_eval{
  def OpenURI.open_supl(buf, target, proxy, options) # :nodoc:
    options[:port] = target.port
    URI.decode_www_form(target.query || "").each{|k, v|
      case k = k.to_sym
      when :protocol
        (options[k] ||= []) << v.to_sym
      end
    }
    buf.instance_eval{
      @io = GPS_PVT::SUPL_Client::new(target.host, options)
      @io.define_singleton_method(:read){
        require 'json'
        JSON::pretty_generate(self.get_assisted_data)
      }
      @io.define_singleton_method(:closed?){true} # For open-uri auto close
    }
  end
}
module URI
  class Supl < Generic
    DEFAULT_PORT = 7275
    def buffer_open(buf, proxy, options)
      OpenURI.open_supl(buf, self, proxy, options)
    end
    include OpenURI::OpenRead
  end
  if respond_to?(:register_scheme) then
    register_scheme('SUPL', Supl)
  else
    @@schemes['SUPL'] = Supl
  end
end