gps_pvt 0.9.2 → 0.9.3

data/lib/gps_pvt/receiver/rtcm3.rb

@@ -18,6 +18,13 @@ class Receiver
     else; raise "reference time (#{ref_time}) should be GPS::Time or Time"
     end
     leap_sec = ref_time.leap_seconds
+    ref_pos = opt[:ref_pos] || if src_io.respond_to?(:property) then
+      Coordinate::LLH::new(*(src_io.property.values_at(:latitude, :longitude).collect{|v|
+        v.to_f / 180 * Math::PI
+      } + [0])).xyz
+    else 
+      defined?(@base_station) ? @base_station : nil
+    end
     after_run = b || proc{|pvt| puts pvt.to_s if pvt}
     t_meas, meas = [nil, {}]
     # meas := {msg_num => [[], ...]} due to duplicated observation such as 1074 and 1077
@@ -26,7 +33,9 @@ class Receiver
       meas.sort.each{|k, values| # larger msg_num entries have higher priority
         values.each{|prn_k_v| meas_.add(*prn_k_v)}
       }
-      after_run.call(run(meas_, t_meas), [meas_, ref_time = t_meas]) if t_meas
+      pvt = nil
+      after_run.call(pvt = run(meas_, t_meas), [meas_, ref_time = t_meas]) if t_meas
+      ref_pos = pvt.xyz if pvt && pvt.position_solved?
       t_meas, meas = [nil, {}]
     }
     dt_threshold = GPS::Time::Seconds_week / 2
@@ -53,6 +62,63 @@ class Receiver
         GPS::Time::new(ref_time.week, 0) + tod + 60 * 60 * 24 * ref_dow
       end
     }
+    restore_ranges = proc{
+      c_1ms = 299_792.458
+      threshold = c_1ms / 10 # 100 us =~ 30 km
+      cache = {} # {[sys, svid] => [range, t], ...}
+      get_rough = proc{|t, sys_svid_list|
+        sn_list = sys_svid_list.collect{|sys, svid|
+          case sys
+          when :GPS, :QZSS; @solver.gps_space_node
+          when :SBAS; @solver.sbas_space_node
+          when :GLONASS; @solver.glonass_space_node
+          end
+        }
+        critical{
+          sn_list.uniq.compact{|sn| sn.update_all_ephemeris(t)}
+          sys_svid_list.zip(sn_list).each{|(sys, svid), sn|
+            next unless sn
+            eph = sn.ephemeris(svid)
+            cache[[sys, svid]] = [if eph.valid?(t) then
+              sv_pos, clk_err = eph.constellation(t).values_at(0, 2)
+              sv_pos.dist(ref_pos) - (clk_err * c_1ms * 1E3)
+            end, t]
+          }
+        }
+      }
+      per_kind = proc{|t, sys_svid_list, ranges_rem|
+        get_rough.call(t, sys_svid_list.uniq.reject{|sys, svid|
+          next true unless sys
+          range, t2 = cache[[sys, svid]]
+          range && ((t2 - t).abs <= 60)
+        })
+        ranges_rem.zip(sys_svid_list).collect{|rem_in, (sys, svid)|
+          range_ref, t2 = cache[[sys, svid]]
+          next nil unless range_ref
+          q, rem_ref = range_ref.divmod(c_1ms)
+          delta = rem_in - rem_ref 
+          res = if delta.abs <= threshold then
+            q * c_1ms + rem_in
+          elsif -delta + c_1ms <= threshold
+            (q - 1) * c_1ms + rem_in
+          elsif delta + c_1ms <= threshold
+            (q + 1) * c_1ms + rem_in
+          end
+          #p [sys, svid, q, rem_in, rem_ref, res]
+          (cache[[sys, svid]] = [res, t])[0]
+        }
+      }
+      proc{|t, sys_svid_list, ranges|
+        [
+          :pseudo_range, # for MT 1001/3/9/11, MSM1/3
+          :phase_range, # for MT 1003/11, MSM2/3
+        ].each{|k|
+          next if ranges[k]
+          k_rem = "#{k}_rem".to_sym
+          ranges[k] = per_kind.call(t, sys_svid_list, ranges[k_rem]) if ranges[k_rem]
+        }
+      }
+    }.call
 
     while packet = rtcm3.read_packet
       msg_num = packet.message_number
@@ -66,15 +132,19 @@ class Receiver
       when 1001..1004
         t_meas2 = tow2t.call(parsed[2][0]) # DF004
         ranges = parsed.ranges
-        item_size = ranges[:sat].size
-        [:sat, :pseudo_range, :phase_range, :cn].collect{|k|
-          ranges[k] || ([nil] * item_size)
-        }.transpose.each{|svid, pr, cpr, cn|
+        sys_svid_list = ranges[:sat].collect{|svid|
           case svid
-          when 1..32; # GPS
-          when 40..58; svid += 80 # SBAS
-          else; next
+          when 1..32; [:GPS, svid]
+          when 40..58; [:SBAS, svid + 80]
+          else; [nil, svid]
           end
+        }
+        restore_ranges.call(t_meas2, sys_svid_list, ranges)
+        item_size = sys_svid_list.size
+        ([sys_svid_list] + [:pseudo_range, :phase_range, :cn].collect{|k|
+          ranges[k] || ([nil] * item_size)
+        }).transpose.each{|(sys, svid), pr, cpr, cn|
+          next unless sys
           meas2 << [svid, :L1_PSEUDORANGE, pr] if pr
           meas2 << [svid, :L1_CARRIER_PHASE, cpr / GPS::SpaceNode.L1_WaveLength] if cpr
           meas2 << [svid, :L1_SIGNAL_STRENGTH_dBHz, cn] if cn
@@ -82,25 +152,34 @@ class Receiver
       when 1009..1012
         t_meas2 = utc2t.call(parsed[2][0] - 60 * 60 * 3) # DF034 UTC(SU)+3hr, time of day[sec]
         ranges = parsed.ranges
-        item_size = ranges[:sat].size
-        [:sat, :freq_ch, :pseudo_range, :phase_range, :cn].collect{|k|
-          ranges[k] || ([nil] * item_size)
-        }.transpose.each{|svid, freq_ch, pr, cpr, cn|
+        sys_svid_list = ranges[:sat].collect{|svid|
           case svid
-          when 1..24 # GLONASS
+          when 1..24; [:GLONASS, svid]
+          when 40..58; [:SBAS, svid + 80]
+          else; [nil, svid]
+          end
+        }
+        restore_ranges.call(t_meas2, sys_svid_list, ranges)
+        item_size = sys_svid_list.size
+        ([sys_svid_list] + [:freq_ch, :pseudo_range, :phase_range, :cn].collect{|k|
+          ranges[k] || ([nil] * item_size)
+        }).transpose.each{|(sys, svid), freq_ch, pr, cpr, cn|
+          case sys
+          when :GLONASS
             svid += 0x100
             freq = GPS::SpaceNode_GLONASS::L1_frequency(freq_ch)
             len = GPS::SpaceNode_GLONASS.light_speed / freq
             meas2 << [svid, :L1_FREQUENCY, freq]
             meas2 << [svid, :L1_CARRIER_PHASE, cpr / len] if cpr
-          when 40..58
-            svid += 80 # SBAS
+          when :SBAS
             meas2 << [svid, :L1_CARRIER_PHASE, cpr / GPS::SpaceNode.L1_WaveLength] if cpr
           else; next
           end
           meas2 << [svid, :L1_PSEUDORANGE, pr] if pr
           meas2 << [svid, :L1_SIGNAL_STRENGTH_dBHz, cn] if cn
         }
+      when 1013
+        leap_sec = parsed[5][0]
       when 1019, 1044
         params = parsed.params
         if msg_num == 1044
@@ -162,22 +241,24 @@ class Receiver
             }.compact.flatten(1))]
           }
         end
-        sig_list, svid_offset = case msg_num / 10
+        sig_list, sys, svid_offset = case msg_num / 10
           when 107 # GPS
             [{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
                 15 => [:L2CM, GPS::SpaceNode.L2_WaveLength],
-                16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, 0]
+                16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, :GPS, 0]
           when 108 # GLONASS
-            [{2 => [:L1, nil]}, 0x100]
+            [{2 => [:L1, nil]}, :GLONASS, 0x100]
           when 110 # SBAS
-            [{2 => [:L1, GPS::SpaceNode.L1_WaveLength]}, 120]
+            [{2 => [:L1, GPS::SpaceNode.L1_WaveLength]}, :SBAS, 120]
           when 111 # QZSS
             [{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
                 15 => [:L2CM, GPS::SpaceNode.L2_WaveLength],
-                16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, 192]
-          else; [{}, 0]
+                16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, :QZSS, 192]
+          else; [{}, nil, 0]
         end
-        item_size = ranges[:sat_sig].size
+        sys_svid_list = ranges[:sat_sig].collect{|sat, sig| [sys, (sat + svid_offset) & 0xFF]}
+        restore_ranges.call(t_meas2, sys_svid_list, ranges)
+        item_size = sys_svid_list.size
         [:sat_sig, :pseudo_range, :phase_range, :phase_range_rate, :cn].collect{|k|
           ranges[k] || ([nil] * item_size)
         }.transpose.each{|(svid, sig), pr, cpr, dr, cn|

data/lib/gps_pvt/receiver.rb

@@ -416,6 +416,7 @@ class Receiver
     opt = options[0] || {}
     case sys
     when :GPS, :QZSS
+      return unless bcast_data.size == 10 # 8 for QZSS(SAIF)
       return unless eph = @eph_list[prn]
       sn = @solver.gps_space_node
       subframe, iodc_or_iode = eph.parse(bcast_data)

data/lib/gps_pvt/rtcm3.rb

@@ -95,6 +95,13 @@ class RTCM3
         48 => invalidate.call(num_gen.call(20, Rational(5, 10000)), 0x80000), # [m]
         49 => 7,
         50 => invalidate.call(unum_gen.call(8, Rational(1, 4)), 0), # [db-Hz]
+        51 => 16,
+        52 => 17,
+        53 => 5,
+        54 => 8,
+        55 => 12,
+        56 => 1,
+        57 => 16,
         71 => 8,
         76 => 10,
         77 => proc{
@@ -237,12 +244,14 @@ class RTCM3
       1001..1004 => (2..8).to_a,
       1005 => [2, 3, 21, 22, 23, 24, 141, 25, 142, [1, 1], 26, 364, 27],
       1009..1012 => [2, 3, 34, 5, 35, 36, 37],
+      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
       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
       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
       1081..1087 => [2, 3, 416, 34, 393, 409, [1, 7], 411, 412, 417, 418, 394, 395], # 169 bits @see Table 3.5-93
       1091..1097 => [2, 3, 248, 393, 409, [1, 7], 411, 412, 417, 418, 394, 395], # 169 bits @see Table 3.5-98
@@ -392,6 +401,29 @@ class RTCM3
       end
       SPEED_OF_LIGHT = 299_792_458
     end
+    module MSM1_2_3
+      include MSM
+      def ranges
+        sats, cells, offset = property.values_at(:sats, :cells, :header_items)
+        nsat, ncell = [sats.size, cells.size]
+        res = {:sat_sig => cells}
+        range_rough = cells.collect{|sat, sig| # DF398
+          self[offset + sats.find_index(sat)][0]
+        }
+        add_proc = proc{|idx_cell|
+          values = self[offset + (nsat * 1) + (ncell * idx_cell), ncell]
+          next if values.empty? 
+          k = {400 => :pseudo_range_rem, 401 => :phase_range_rem}[values[0][1]]
+          next unless k
+          res[k] = values.zip(range_rough).collect{|(v, df), v_base|
+            ((v_base + v) * SPEED_OF_LIGHT) rescue nil
+          }
+        }
+        add_proc.call(0)
+        add_proc.call(1)
+        res
+      end
+    end
     module MSM4_6
       include MSM
       def ranges
@@ -468,6 +500,9 @@ class RTCM3
               1012 => (38..50).to_a,
             }[msg_num]] * nsat).flatten), offset)
         attributes << GLONASS_Observation
+      when 1013
+        add_proc.call(DataFrame.generate_prop(
+            ((55..57).to_a * values[4]).flatten), 24 + mt[:bits_total])
       when 1019
         attributes << GPS_Ephemeris
       when 1020
@@ -492,6 +527,15 @@ class RTCM3
           add_proc.call(msm_sig, offset)
         }
         case msg_num % 10
+        when 1
+          attributes << MSM1_2_3
+          msm_proc.call([398], [400])
+        when 2
+          attributes << MSM1_2_3
+          msm_proc.call([398], [401, 402, 420])
+        when 3
+          attributes << MSM1_2_3
+          msm_proc.call([398], [400, 401, 402, 420])
         when 4
           attributes << MSM4_6
           msm_proc.call([397, 398], [400, 401, 402, 420, 403])

data/lib/gps_pvt/version.rb

@@ -1,7 +1,7 @@
 # frozen_string_literal: true
 
 module GPS_PVT
-  VERSION = "0.9.2"
+  VERSION = "0.9.3"
   
   def GPS_PVT.version_compare(a, b)
     Gem::Version::new(a) <=> Gem::Version::new(b)

metadata

@@ -1,14 +1,14 @@
 --- !ruby/object:Gem::Specification
 name: gps_pvt
 version: !ruby/object:Gem::Version
-  version: 0.9.2
+  version: 0.9.3
 platform: ruby
 authors:
 - fenrir(M.Naruoka)
 autorequire: 
 bindir: exe
 cert_chain: []
-date: 2023-02-13 00:00:00.000000000 Z
+date: 2023-03-07 00:00:00.000000000 Z
 dependencies:
 - !ruby/object:Gem::Dependency
   name: rubyserial