gps_pvt 0.8.5 → 0.9.0

data/ext/ninja-scan-light/tool/navigation/GLONASS.h

@@ -371,6 +371,18 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
           }
           return res;
         }
+        static void date2raw(const date_t &src, u8_t *N_4_, u16_t *NA_){
+          std::div_t divmod(std::div(src.year - 1996, 4));
+          if(N_4_){*N_4_ = divmod.quot + 1;}
+          if(NA_){
+            *NA_ = src.day_of_year + 1;
+            switch(divmod.rem){
+              case 1: *NA_ += 366; break;
+              case 2: *NA_ += 731; break;
+              case 3: *NA_ += 1096; break;
+            }
+          }
+        }
 
         operator TimeProperties() const {
           TimeProperties res;
@@ -389,14 +401,7 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
 {TARGET = (s32_t)std::floor(t.TARGET / sf[SF_ ## TARGET] + 0.5);}
           CONVERT(tau_c);
           CONVERT(tau_GPS);
-          std::div_t divmod(std::div(t.date.year - 1996, 4));
-          N_4 = divmod.quot + 1;
-          NA = t.date.day_of_year + 1;
-          switch(divmod.rem){
-            case 1: NA += 366; break;
-            case 2: NA += 731; break;
-            case 3: NA += 1096; break;
-          }
+          date2raw(t.date, &N_4, &NA);
           l_n = (t.l_n ? 1 : 0);
 #undef CONVERT
           return *this;
@@ -1072,6 +1077,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
          * 3) t_GPS - t_GL = delta_T + tau_GPS (defined in ICD 4.5 Non-immediate info.)
          */
         struct Ephemeris_with_Time : public Ephemeris, TimeProperties {
+          typename TimeProperties::date_t t_b_date;
           typedef typename Ephemeris::constellation_t constellation_t;
           constellation_t xa_t_b;
           float_t sidereal_t_b_rad;
@@ -1080,8 +1086,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
           static const float_t time_step_max_per_one_integration;
 
           void calculate_additional() {
+            u8_t N_4;
+            u16_t NA;
+            TimeProperties::raw_t::date2raw(TimeProperties::date, &N_4, &NA);
+            // TODO detect large difference between NA and N_T caused by rolling over.
+            t_b_date = TimeProperties::raw_t::raw2date(N_4, this->N_T);
             sidereal_t_b_rad = TimeProperties::date_t::Greenwich_sidereal_time_deg(
-                TimeProperties::date.c_tm(),
+                t_b_date.c_tm(),
                 (float_t)(this->t_b) / (60 * 60) - 3) / 180 * M_PI;
             constellation_t x_t_b = {
               {this->xn, this->yn, this->zn},
@@ -1101,11 +1112,14 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
             t_mt.tm_hour += 3;
             std::mktime(&t_mt); // renormalization
             this->date = TimeProperties::date_t::from_c_tm(t_mt);
+            u16_t N_T;
+            TimeProperties::raw_t::date2raw(this->date, NULL, &N_T);
+            this->N_T = N_T;
             this->t_b = (t_mt.tm_hour * 60 + t_mt.tm_min) * 60 + t_mt.tm_sec;
             calculate_additional();
           }
           std::tm c_tm_utc() const {
-            std::tm t(TimeProperties::date.c_tm()); // set date on Moscow time
+            std::tm t(t_b_date.c_tm()); // set date on Moscow time
             (t.tm_sec = (int)(this->t_b)) -= 3 * 60 * 60; // add second on UTC
             std::mktime(&t); // renormalization
             return t;

data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h

@@ -658,10 +658,11 @@ protected:
      * @param x solution
      * @return WSSR scalar
      */
-    float_t wssr(const matrix_t &x = least_square()) const {
+    float_t wssr(const matrix_t &x) const {
       matrix_t v(delta_r - G * x);
       return (v.transpose() * W * v)(0, 0);
     }
+    float_t wssr() const {return wssr(least_square());}
     /**
      * Calculate weighted square sum of residual (WSSR) based on least square solution
      * with solution coefficient matrix (S).

data/ext/ninja-scan-light/tool/navigation/RINEX.h

@@ -1809,7 +1809,7 @@ class RINEX_Writer {
       friend std::ostream &operator<<(std::ostream &out, const header_t &header){
         std::stringstream ss;
         ss << std::setfill(' ') << std::left;
-        for(header_t::const_iterator it(header.begin()), it_end(header.end());
+        for(typename header_t::const_iterator it(header.begin()), it_end(header.end());
             it != it_end; ++it){
           ss << std::setw(60) << it->second.substr(0, 60)
               << std::setw(20) << it->first.substr(0, 20)

data/ext/ninja-scan-light/tool/swig/GPS.i

@@ -191,16 +191,19 @@ static std::string inspect_str(const VALUE &v){
 #endif
 }
 
-%define MAKE_ACCESSOR(name, type)
-%rename(%str(name ## =)) set_ ## name;
-type set_ ## name (const type &v) {
-  return self->name = v;
+%define MAKE_ACCESSOR2(func_name, target, type)
+%rename(%str(func_name ## =)) set_ ## func_name;
+type set_ ## func_name (const type &v) {
+  return self->target= v;
 }
-%rename(%str(name)) get_ ## name;
-const type &get_ ## name () const {
-  return self->name;
+%rename(%str(func_name)) get_ ## func_name;
+const type &get_ ## func_name () const {
+  return self->target;
 }
 %enddef
+%define MAKE_ACCESSOR(name, type)
+MAKE_ACCESSOR2(name, name, type)
+%enddef
 
 %define MAKE_VECTOR2ARRAY(type)
 %typemap(out) std::vector<type> {
@@ -663,6 +666,14 @@ struct GLONASS_Ephemeris
     raw = *this;
     has_string = 0x1F;
   }
+  GLONASS_Ephemeris &rehash(const int &deltaT = 0) {
+    typedef typename GLONASS_SpaceNode<FloatT>::SatelliteProperties prop_t;
+    return *this = GLONASS_Ephemeris(eph_t(
+        typename prop_t::Ephemeris_with_Time(
+          (typename prop_t::Ephemeris)(*this),
+          (typename GLONASS_SpaceNode<FloatT>::TimeProperties)(*this)),
+        deltaT));
+  }
 };
 %}
 %extend GLONASS_Ephemeris {
@@ -692,6 +703,15 @@ struct GLONASS_Ephemeris
 
   MAKE_ACCESSOR(tau_c, FloatT);
   MAKE_ACCESSOR(tau_GPS, FloatT);
+  MAKE_ACCESSOR2(year, date.year, int);
+  MAKE_ACCESSOR2(day_of_year, date.day_of_year, int);
+  
+  void set_date(const unsigned int &N_4, const unsigned int &NA) {
+    self->date = GLONASS_SpaceNode<FloatT>::TimeProperties::raw_t::raw2date(N_4, NA);
+  }
+  void set_date(const std::tm &t) {
+    self->date = GLONASS_SpaceNode<FloatT>::TimeProperties::date_t::from_c_tm(t);
+  }
   
   FloatT frequency_L1() const {
     return self->L1_frequency();
@@ -1149,19 +1169,8 @@ struct GPS_Measurement {
 }
 
 %extend GPS_SolverOptions_Common {
-%define MAKE_ACCESSOR2(name, type)
-%rename(%str(name ## =)) set_ ## name;
-type set_ ## name (const type &v) {
-  return self->cast_general()->name = v;
-}
-%rename(%str(name)) get_ ## name;
-const type &get_ ## name () const {
-  return self->cast_general()->name;
-}
-%enddef
-  MAKE_ACCESSOR2(elevation_mask, FloatT);
-  MAKE_ACCESSOR2(residual_mask, FloatT);
-#undef MAKE_ACCESSOR2
+  MAKE_ACCESSOR2(elevation_mask, cast_general()->elevation_mask, FloatT);
+  MAKE_ACCESSOR2(residual_mask, cast_general()->residual_mask, FloatT);
   MAKE_VECTOR2ARRAY(int);
   %ignore cast_general;
 }
@@ -1321,15 +1330,15 @@ struct HookableSolver : public BaseT {
     HookableSolver<
           GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >,
           GPS_Solver<FloatT> >
-        ::HookableSolver<GPS_SpaceNode<FloatT> >(const GPS_SpaceNode<FloatT> &sn)
+        ::HookableSolver/*<GPS_SpaceNode<FloatT> >*/(const GPS_SpaceNode<FloatT> &sn) // to avoid out-of-line constructor error
           : GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >(sn), hook(NULL) {}
     template <> template <>
     HookableSolver<SBAS_SinglePositioning<FloatT>, GPS_Solver<FloatT> >
-        ::HookableSolver<SBAS_SpaceNode<FloatT> >(const SBAS_SpaceNode<FloatT> &sn)
+        ::HookableSolver/*<SBAS_SpaceNode<FloatT> >*/(const SBAS_SpaceNode<FloatT> &sn)
           : SBAS_SinglePositioning<FloatT>(sn), hook(NULL) {}
     template <> template <>
     HookableSolver<GLONASS_SinglePositioning<FloatT>, GPS_Solver<FloatT> >
-        ::HookableSolver<GLONASS_SpaceNode<FloatT> >(const GLONASS_SpaceNode<FloatT> &sn)
+        ::HookableSolver/*<GLONASS_SpaceNode<FloatT> >*/(const GLONASS_SpaceNode<FloatT> &sn)
           : GLONASS_SinglePositioning<FloatT>(sn), hook(NULL) {}
     template <>
     GPS_Solver<FloatT>::base_t::relative_property_t

data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb

@@ -906,6 +906,8 @@ __RINEX_CLK_TEXT__
         }
         puts
       }
+      expect(solver.gps_options.elevation_mask).to eq(0)
+      expect(solver.gps_options.residual_mask).to be > 0
     end
     
     it 'can be modified through hooks' do

data/lib/gps_pvt/ntrip.rb

@@ -88,8 +88,8 @@ class Ntrip < Net::HTTP
     }
     req
   end
-  def get_source_table(header = {})
-    Ntrip.parse_source_table(request(generate_request('/', header)).read_body)
+  def get_source_table(header = {}, &b)
+    (b || proc{|str| Ntrip.parse_source_table(str)}).call(request(generate_request('/', header)).read_body)
   end
   def get_data(mount_point, header = {}, &b)
     request(generate_request("/#{mount_point}", header)){|res|
@@ -129,10 +129,16 @@ OpenURI.class_eval{
   def OpenURI.open_ntrip(buf, target, proxy, options) # :nodoc:
     GPS_PVT::Ntrip.start(target.host, target.port){|ntrip|
       # get source table
-      tbl = ntrip.get_source_table(options)
+      tbl = ntrip.get_source_table(options){|str| str}
+      if target.root? then
+        buf << tbl
+        buf.io.rewind
+        next
+      end
+      tbl = GPS_PVT::Ntrip::parse_source_table(tbl)
       
       # check mount point
-      mnt_pt = target.path.sub(%r|^/|, '')
+      mnt_pt = target.mount_point
       prop = tbl.mount_points[mnt_pt]
       raise Net::ProtocolError::new("Mount point(#{mnt_pt}) not found") unless prop
       
@@ -154,10 +160,23 @@ OpenURI.class_eval{
 }
 module URI
   class Ntrip < HTTP
+    def root
+      res = self.clone
+      res.path = '/'
+      res
+    end
+    def root?; self.path == '/'; end
+    def mount_point
+      self.path.sub(%r|^/|, '')
+    end
+
     def buffer_open(buf, proxy, options)
       OpenURI.open_ntrip(buf, self, proxy, options)
     end
     include OpenURI::OpenRead
+    def read_source_table(options = {})
+      GPS_PVT::Ntrip::parse_source_table(self.root.read(options))
+    end
   end
   if respond_to?(:register_scheme) then
     register_scheme('NTRIP', Ntrip)

data/lib/gps_pvt/receiver/rtcm3.rb

@@ -0,0 +1,110 @@
+=begin
+RTCM3 handler  for receiver
+=end
+
+module GPS_PVT
+class Receiver
+  def parse_rtcm3(src, opt = {}, &b)
+    $stderr.print "Reading RTCM3 stream (%s) "%[src]
+    require_relative '../rtcm3'
+    src_io = open(src)
+    rtcm3 = GPS_PVT::RTCM3::new(src_io)
+    ref_time = case (ref_time = opt[:ref_time])
+    when GPS::Time; 
+    when Time
+      t_array = ref_time.utc.to_a[0..5].reverse
+      GPS::Time::new(t_array, GPS::Time::guess_leap_seconds(t_array))
+    when nil; GPS::Time::now
+    else; raise "reference time (#{ref_time}) should be GPS::Time or Time"
+    end
+    leap_sec = ref_time.leap_seconds
+    after_run = b || proc{|pvt| puts pvt.to_s if pvt}
+    t_meas, meas = [nil, GPS::Measurement::new]
+    dt_threshold = GPS::Time::Seconds_week / 2
+
+    while packet = rtcm3.read_packet
+      msg_num = packet.message_number
+      parsed = packet.parse
+      case msg_num
+      when 1019, 1044
+        params = parsed.params
+        if msg_num == 1044
+          params[:svid] += 192
+          params[:fit_interval] ||= 2 * 60 * 60
+        end
+        params[:WN] += ((ref_time.week - params[:WN]).to_f / 1024).round * 1024
+        eph = GPS::Ephemeris::new
+        params.each{|k, v| eph.send("#{k}=".to_sym, v)}
+        critical{
+          @solver.gps_space_node.register_ephemeris(eph.svid, eph)
+        }
+      when 1020
+        params = parsed.params
+        eph = GPS::Ephemeris_GLONASS::new
+        params[:F_T] ||= 10 # [m]
+        params.each{|k, v|
+          next if [:P3, :NA, :N_4].include?(k)
+          eph.send("#{k}=".to_sym, v)
+        }
+        proc{|date_src|
+          eph.set_date(*(date_src.all? \
+              ? date_src \
+              : [(ref_time + 3 * 60 * 60).c_tm(leap_sec)])) # UTC -> Moscow time
+        }.call([:N_4, :NA].collect{|k| params[k]})
+        eph.rehash(leap_sec)
+        critical{
+          @solver.glonass_space_node.register_ephemeris(eph.svid, eph)
+        }
+      when 1077, 1087, 1097, 1117
+        ranges = parsed.ranges
+        sig_list, svid_offset = case msg_num
+          when 1077 # GPS
+            t_meas ||= proc{ # update time of measurement
+              t_meas_sec = parsed[2][0] # DF004
+              dt = t_meas_sec - ref_time.seconds 
+              GPS::Time::new(ref_time.week + if dt <= -dt_threshold then; 1
+                  elsif dt >= dt_threshold then; -1
+                  else; 0; end, t_meas_sec)
+            }.call
+            [{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
+                15 => [:L2CM, GPS::SpaceNode.L2_WaveLength],
+                16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, 0]
+          when 1087 # GLONASS
+            proc{
+              utc = parsed[3][0] - 60 * 60 * 3 # DF034 UTC(SU)+3hr
+              delta = (t_meas.seconds - utc).to_i % (60 * 60 * 24)
+              leap_sec = (delta >= (60 * 60 * 12)) ? delta - (60 * 60 * 12) : delta
+            }.call if t_meas
+            [{2 => [:L1, GPS::SpaceNode_GLONASS.light_speed / GPS::SpaceNode_GLONASS.L1_frequency_base]}, 0x100]
+          when 1117 # QZSS
+            [{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
+                15 => [:L2CM, GPS::SpaceNode.L2_WaveLength],
+                16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, 192]
+          else; [{}, 0]
+        end
+        [:sat_sig, :pseudo_range, :phase_range, :phase_range_rate].collect{|k|
+          ranges[k]
+        }.transpose.each{|(svid, sig), pr, cpr, dr|
+          prefix, len = sig_list[sig]
+          next unless prefix
+          if svid_offset > 0 then
+            @solver.glonass_space_node.update_all_ephemeris(t_meas)
+            eph = @solver.glonass_space_node.ephemeris(svid)
+          end
+          svid += svid_offset
+          meas.add(svid, "#{prefix}_PSEUDORANGE".to_sym, pr) if pr
+          meas.add(svid, "#{prefix}_RANGE_RATE".to_sym, dr) if dr
+          meas.add(svid, "#{prefix}_CARRIER_PHASE".to_sym, cpr / len) if cpr
+        }
+      else
+        #p({msg_num => parsed})
+      end
+      if (1070..1229).include?(msg_num) && 
+          (!parsed.more_data? rescue (packet.decode([1], 24 + 54)[0] == 0)) then
+        after_run.call(run(meas, t_meas), [meas, ref_time = t_meas]) if t_meas
+        t_meas, meas = [nil, GPS::Measurement::new]
+      end
+    end
+  end
+end
+end

data/lib/gps_pvt/receiver.rb

@@ -712,4 +712,5 @@ class Receiver
 end
 end
 
+require_relative 'receiver/rtcm3'
 require_relative 'receiver/extension'