gps2ubx

#!/usr/bin/env ruby

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.

$stderr.puts <<__STRING__
Usage: #{__FILE__} GPS_file ... > as_you_like.ubx
As GPS_file, rinex_obs(*.YYo) and rtcm3 stream are supported.
(YY = last two digit of year)
File format is automatically determined based on its extention described in above parentheses.
If you want to specify its format manually, command options like --rinex_obs=file_name are available.
Supported RINEX versions are 2 and 3.
RXM-RAWX and RXM-SFRBX are included in output UBX if corresponding file(s) is given. 
A file having additional ".gz" or ".Z" extension is recognized as a compressed file.
Major URL such as http(s)://... or ftp://... is acceptable as an input file name.
Ntrip specified in URI as ntrip://(username):(password)@(caster_host):(port)/(mount_point) is also supported, and its format is automatically detected.
Assisted GPS by using SUPL (Secure user plane location) is also supported by using supl://(host) URI. 
__STRING__

options = []
misc_options = {
  :broadcast_data => false,
  :ubx_rawx => false,
  :ubx_nav => false,
  :eph_interval => 60 * 5,
}

# check options and file format
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, :pvt_csv].include?(k) # file type
  options << [$1.to_sym, $']
  nil
}.compact

# Check file existence and extension
files.collect!{|fname, ftype|
  ftype ||= case fname
  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
    when URI::Ntrip; uri.read_format
    when URI::Supl; :supl
    end
    fname = uri
  end
  raise "Format cannot be guessed, use --(format, ex. rinex_obs)=#{fname}" unless ftype
  [fname, ftype]
}

options.reject!{|opt|
  case opt[0]
  when :ubx_rawx, :ubx_nav, :broadcast_data, :eph_interval
    misc_options[opt[0]] = opt[1]
    true
  when :online_ephemeris
    (misc_options[opt[0]] ||= []) << opt[1]
    true
  else
    false
  end
}

rcv = GPS_PVT::Receiver::new(options)

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|
  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 first
files.each{|fname, ftype|
  case ftype
  when :rinex_nav; rcv.parse_rinex_nav(fname)
  end
}

# then, other files
threads = files.collect{|fname, ftype|
  task = case ftype
  when :ubx; proc{rcv.parse_ubx(fname){}}
  when :rinex_obs; proc{rcv.parse_rinex_obs(fname){}}
  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)
  else; task.call; nil
  end
}.compact

outputs = proc{
  tmp = []
  tmp << outputs.pop until outputs.empty?
  tmp
}.call.sort!{|a, b| b[0] <=> a[0]} if threads.empty? # Sort by measurement time

# Time packet for solution of leap seconds
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|
    next nil if t_meas < t_next
    t_next = t_meas + 60 # 1 min. interval
    ubx = tmpl.clone
    t_sec = t_meas.seconds
    t_msec = (t_sec * 1E3).round
    t_nsec = ((t_sec * 1E3 - t_msec) * 1E6).round
    leap = rcv.leap_seconds(t_meas)
    ubx += [
      t_msec, # ITOW  ms  GPS Millisecond time of Week
      t_nsec, # Frac  ns  Nanoseconds remainder of rounded ms above, range -500000 .. 500000
      t_meas.week, # week  -  GPS week (GPS time)
      leap || 0, # LeapS  s  Leap Seconds (GPS-UTC)
      leap ? 0x07 : 0x03, # validity bit field (0x01=ToW, 0x02=WN, 0x04=UTC)
      10000, # TAcc  ns  Time Accuracy Estimate
    ].pack("Vl<vcCV").unpack("C*")
    GPS_PVT::UBX::update(ubx + [0, 0]).pack("C*")
  }
}.call

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)
  }
  proc{|freq| ((freq - freq0) / delta).to_i}
}.call

gen_raw = proc{|t_meas, meas| # Convert to RXM-RAW(0x10)
  ubx = [0xB5, 0x62, 0x02, 0x10, 0, 0]
  ubx += [(t_meas.seconds * 1E3).to_i, t_meas.week].pack("Vv").unpack("C*")
  ubx += [0] * 2
  meas_ubx = meas.collect{|sat, items|
    res = [0] * 24
    setter = proc{|value, offset, len, str, pre_proc|
      array = case value
      when Array; value
      when Symbol
        [items[GPS_PVT::GPS::Measurement.const_get(value)]]
      else
        next nil
      end
      pre_proc.call(array) if pre_proc
      next if array.empty?
      array = array.pack(str).unpack("C*") if str
      res[offset - 8, len] = array
    }
    svid = case sat
    when 1..32, 120..158, 193..202 # GPS, SBAS, QZSS
      sat
    when (0x100 + 1)..(0x100 + 32) # GLONASS
      sat - 0x100 + 64 # => 65..96
    else
      next nil # TODO Galileo, Beidou, ...
    end
    
    qi = 6
    setter.call(:L1_CARRIER_PHASE,
        8, 8, "E", proc{|v| next if v[0]; qi = 4; v.clear})
    setter.call(:L1_PSEUDORANGE,
        16, 8, "E", proc{|v| next if v[0]; qi = 0; v.clear})
    setter.call(:L1_DOPPLER,
        24, 4, "e", proc{|v| next if v[0]; qi = 0; v.clear})
    setter.call([svid, qi], 28, 2)
    setter.call(:L1_SIGNAL_STRENGTH_dBHz,
        30, 1, nil, proc{|v| v.replace(v[0] ? [v[0].to_i] : [])})
    setter.call(:L1_LOCK_SEC,
        31, 1, nil, proc{|v| v.replace(v[0] ? [(v[0] < 0) ? 1 : 0] : [0])})

    res
  }.compact
  ubx[6 + 6] = meas_ubx.size
  ubx += meas_ubx.flatten(1)
  ubx += [0, 0]
  GPS_PVT::UBX::update(ubx).pack("C*")
}

gen_rawx = proc{|t_meas, meas| # Convert to RXM-RAWX(0x15)
  ubx = [0xB5, 0x62, 0x02, 0x15, 0, 0]
  ubx += [t_meas.seconds, t_meas.week].pack("Ev").unpack("C*")
  ubx += [0] * 6
  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
        k = [sig, value].join('_').to_sym
        [items[GPS_PVT::GPS::Measurement.const_get(k)]]
      else
        next nil
      end
      pre_proc.call(array) if pre_proc
      next if array.empty?
      array = array.pack(str).unpack("C*") if str
      res[offset - 16, len] = array
    }
    
    setter.call([sys, svid], 36, 2)

    trk_stat = 0
    setter.call(:PSEUDORANGE,
        16, 8, "E", proc{|v| v[0] ? (trk_stat |= 0x1) : v.clear})
    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(: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(:CARRIER_PHASE,
        24, 8, "E", proc{|v| v[0] ? (trk_stat |= 0x2) : v.clear})
    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(:SIGNAL_STRENGTH_dBHz,
        42, 1, nil, proc{|v| v.replace(v[0] ? [v[0].to_i] : [])})
    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[6 + 13] = 1 # version
  ubx += meas_ubx.flatten
  ubx += [0, 0]
  GPS_PVT::UBX::update(ubx).pack("C*")
}

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 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
  task_dump.call
else
  (threads << Thread::new{loop{
    task_dump.call
  }}).each{|th| th.join}
end