gps_pvt 0.1.1
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +7 -0
- data/.rspec +3 -0
- data/CHANGELOG.md +5 -0
- data/CODE_OF_CONDUCT.md +84 -0
- data/Gemfile +10 -0
- data/README.md +86 -0
- data/Rakefile +86 -0
- data/bin/console +15 -0
- data/bin/setup +8 -0
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +6613 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +16019 -0
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +21050 -0
- data/ext/gps_pvt/extconf.rb +70 -0
- data/ext/ninja-scan-light/tool/navigation/EGM.h +2971 -0
- data/ext/ninja-scan-light/tool/navigation/GPS.h +2432 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +479 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +1081 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_MultiFrequency.h +199 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h +210 -0
- data/ext/ninja-scan-light/tool/navigation/MagneticField.h +928 -0
- data/ext/ninja-scan-light/tool/navigation/NTCM.h +211 -0
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +1781 -0
- data/ext/ninja-scan-light/tool/navigation/WGS84.h +186 -0
- data/ext/ninja-scan-light/tool/navigation/coordinate.h +406 -0
- data/ext/ninja-scan-light/tool/param/bit_array.h +145 -0
- data/ext/ninja-scan-light/tool/param/complex.h +558 -0
- data/ext/ninja-scan-light/tool/param/matrix.h +4049 -0
- data/ext/ninja-scan-light/tool/param/matrix_fixed.h +665 -0
- data/ext/ninja-scan-light/tool/param/matrix_special.h +562 -0
- data/ext/ninja-scan-light/tool/param/quaternion.h +765 -0
- data/ext/ninja-scan-light/tool/param/vector3.h +651 -0
- data/ext/ninja-scan-light/tool/swig/Coordinate.i +177 -0
- data/ext/ninja-scan-light/tool/swig/GPS.i +1102 -0
- data/ext/ninja-scan-light/tool/swig/SylphideMath.i +1234 -0
- data/ext/ninja-scan-light/tool/swig/extconf.rb +5 -0
- data/ext/ninja-scan-light/tool/swig/makefile +53 -0
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +417 -0
- data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb +489 -0
- data/gps_pvt.gemspec +57 -0
- data/lib/gps_pvt/receiver.rb +375 -0
- data/lib/gps_pvt/ubx.rb +148 -0
- data/lib/gps_pvt/version.rb +5 -0
- data/lib/gps_pvt.rb +9 -0
- data/sig/gps_pvt.rbs +4 -0
- metadata +117 -0
@@ -0,0 +1,375 @@
|
|
1
|
+
#!/usr/bin/ruby
|
2
|
+
|
3
|
+
=begin
|
4
|
+
Receiver class to be an top level interface to a user
|
5
|
+
(The origin is ninja-scan-light/tool/misc/receiver_debug.rb)
|
6
|
+
=end
|
7
|
+
|
8
|
+
require_relative 'GPS'
|
9
|
+
|
10
|
+
module GPS_PVT
|
11
|
+
class Receiver
|
12
|
+
OUTPUT_PVT_ITEMS = [[
|
13
|
+
[:week, :itow_rcv, :year, :month, :mday, :hour, :min, :sec],
|
14
|
+
proc{|pvt|
|
15
|
+
[:week, :seconds, :c_tm].collect{|f| pvt.receiver_time.send(f)}.flatten
|
16
|
+
}
|
17
|
+
]] + [[
|
18
|
+
[:receiver_clock_error_meter, :longitude, :latitude, :height],
|
19
|
+
proc{|pvt|
|
20
|
+
next [nil] * 4 unless pvt.position_solved?
|
21
|
+
[
|
22
|
+
pvt.receiver_error,
|
23
|
+
pvt.llh.lng / Math::PI * 180,
|
24
|
+
pvt.llh.lat / Math::PI * 180,
|
25
|
+
pvt.llh.alt,
|
26
|
+
]
|
27
|
+
}
|
28
|
+
]] + [proc{
|
29
|
+
labels = [:g, :p, :h, :v, :t].collect{|k| "#{k}dop".to_sym}
|
30
|
+
[
|
31
|
+
labels,
|
32
|
+
proc{|pvt|
|
33
|
+
next [nil] * 5 unless pvt.position_solved?
|
34
|
+
labels.collect{|k| pvt.send(k)}
|
35
|
+
}
|
36
|
+
]
|
37
|
+
}.call] + [[
|
38
|
+
[:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms],
|
39
|
+
proc{|pvt|
|
40
|
+
next [nil] * 4 unless pvt.velocity_solved?
|
41
|
+
[:north, :east, :down].collect{|k| pvt.velocity.send(k)} \
|
42
|
+
+ [pvt.receiver_error_rate]
|
43
|
+
}
|
44
|
+
]] + [
|
45
|
+
[:used_satellites, proc{|pvt| pvt.used_satellites}],
|
46
|
+
[:PRN, proc{|pvt|
|
47
|
+
("%32s"%[pvt.used_satellite_list.collect{|i|
|
48
|
+
1 << (i - 1)
|
49
|
+
}.inject(0){|res, v| res | v}.to_s(2)]).scan(/.{8}/).collect{|str|
|
50
|
+
str.gsub(' ', '0')
|
51
|
+
}.join('_')
|
52
|
+
}],
|
53
|
+
] + [[
|
54
|
+
(1..32).collect{|prn|
|
55
|
+
[:range_residual, :weight, :azimuth, :elevation, :slopeH, :slopeV].collect{|str| "#{str}(#{prn})"}
|
56
|
+
}.flatten,
|
57
|
+
proc{|pvt|
|
58
|
+
next ([nil] * 6 * 32) unless pvt.position_solved?
|
59
|
+
sats = pvt.used_satellite_list
|
60
|
+
r, w = [:delta_r, :W].collect{|f| pvt.send(f)}
|
61
|
+
(1..32).collect{|i|
|
62
|
+
next ([nil] * 6) unless i2 = sats.index(i)
|
63
|
+
[r[i2, 0], w[i2, i2]] +
|
64
|
+
[:azimuth, :elevation].collect{|f|
|
65
|
+
pvt.send(f)[i] / Math::PI * 180
|
66
|
+
} + [pvt.slopeH[i], pvt.slopeV[i]]
|
67
|
+
}.flatten
|
68
|
+
},
|
69
|
+
]] + [[
|
70
|
+
[:wssr, :wssr_sf, :weight_max,
|
71
|
+
:slopeH_max, :slopeH_max_PRN, :slopeH_max_elevation,
|
72
|
+
:slopeV_max, :slopeV_max_PRN, :slopeV_max_elevation],
|
73
|
+
proc{|pvt|
|
74
|
+
next [nil] * 9 unless fd = pvt.fd
|
75
|
+
el_deg = [4, 6].collect{|i| pvt.elevation[fd[i]] / Math::PI * 180}
|
76
|
+
fd[0..4] + [el_deg[0]] + fd[5..6] + [el_deg[1]]
|
77
|
+
}
|
78
|
+
]] + [[
|
79
|
+
[:wssr_FDE_min, :wssr_FDE_min_PRN, :wssr_FDE_2nd, :wssr_FDE_2nd_PRN],
|
80
|
+
proc{|pvt|
|
81
|
+
[:fde_min, :fde_2nd].collect{|f|
|
82
|
+
info = pvt.send(f)
|
83
|
+
next ([nil] * 2) if (!info) || info.empty?
|
84
|
+
[info[0], info[-3]]
|
85
|
+
}.flatten
|
86
|
+
}
|
87
|
+
]]
|
88
|
+
|
89
|
+
OUTPUT_MEAS_ITEMS = [[
|
90
|
+
(1..32).collect{|prn|
|
91
|
+
[:L1_range, :L1_rate].collect{|str| "#{str}(#{prn})"}
|
92
|
+
}.flatten,
|
93
|
+
proc{|meas|
|
94
|
+
meas_hash = Hash[*(meas.collect{|prn, k, v| [[prn, k], v]}.flatten(1))]
|
95
|
+
(1..32).collect{|prn|
|
96
|
+
[:L1_PSEUDORANGE, [:L1_DOPPLER, GPS::SpaceNode.L1_WaveLength]].collect{|k, sf|
|
97
|
+
meas_hash[[prn, GPS::Measurement.const_get(k)]] * (sf || 1) rescue nil
|
98
|
+
}
|
99
|
+
}
|
100
|
+
}
|
101
|
+
]]
|
102
|
+
|
103
|
+
def self.header
|
104
|
+
(OUTPUT_PVT_ITEMS + OUTPUT_MEAS_ITEMS).transpose[0].flatten.join(',')
|
105
|
+
end
|
106
|
+
|
107
|
+
attr_accessor :solver
|
108
|
+
|
109
|
+
def initialize(options = {})
|
110
|
+
@solver = GPS::Solver::new
|
111
|
+
@solver.hooks[:relative_property] = proc{|prn, rel_prop, rcv_e, t_arv, usr_pos, usr_vel|
|
112
|
+
rel_prop[0] = 1 if rel_prop[0] > 0 # weight = 1
|
113
|
+
rel_prop
|
114
|
+
}
|
115
|
+
options = options.reject{|k, v|
|
116
|
+
case k
|
117
|
+
when :weight
|
118
|
+
case v.to_sym
|
119
|
+
when :elevation # (same as underneath C++ library)
|
120
|
+
@solver.hooks[:relative_property] = proc{|prn, rel_prop, rcv_e, t_arv, usr_pos, usr_vel|
|
121
|
+
if rel_prop[0] > 0 then
|
122
|
+
elv = Coordinate::ENU::relative_rel(
|
123
|
+
Coordinate::XYZ::new(*rel_prop[4..6]), usr_pos).elevation
|
124
|
+
rel_prop[0] = (Math::sin(elv)/0.8)**2
|
125
|
+
end
|
126
|
+
rel_prop
|
127
|
+
}
|
128
|
+
next true
|
129
|
+
when :identical # same as default
|
130
|
+
next true
|
131
|
+
end
|
132
|
+
end
|
133
|
+
false
|
134
|
+
}
|
135
|
+
raise "Unknown receiver options: #{options.inspect}" unless options.empty?
|
136
|
+
proc{|opt|
|
137
|
+
opt.elevation_mask = 0.0 / 180 * Math::PI # 0 deg
|
138
|
+
opt.residual_mask = 1E4 # 10 km
|
139
|
+
}.call(@solver.gps_options)
|
140
|
+
end
|
141
|
+
|
142
|
+
def run(meas, t_meas)
|
143
|
+
#$stderr.puts "Measurement time: #{t_meas.to_a} (a.k.a #{"%d/%d/%d %d:%d:%d UTC"%[*t_meas.c_tm]})"
|
144
|
+
sn = @solver.gps_space_node
|
145
|
+
sn.update_all_ephemeris(t_meas)
|
146
|
+
|
147
|
+
meas.to_a.collect{|prn, k, v| prn}.uniq.each{|prn|
|
148
|
+
eph = sn.ephemeris(prn)
|
149
|
+
$stderr.puts "XYZ(PRN:#{prn}): #{eph.constellation(t_meas)[0].to_a} (iodc: #{eph.iodc}, iode: #{eph.iode})"
|
150
|
+
} if false
|
151
|
+
|
152
|
+
pvt = @solver.solve(meas, t_meas)
|
153
|
+
pvt.define_singleton_method(:to_s){
|
154
|
+
(OUTPUT_PVT_ITEMS.transpose[1].collect{|task|
|
155
|
+
task.call(pvt)
|
156
|
+
} + OUTPUT_MEAS_ITEMS.transpose[1].collect{|task|
|
157
|
+
task.call(meas)
|
158
|
+
}).flatten.join(',')
|
159
|
+
}
|
160
|
+
pvt
|
161
|
+
end
|
162
|
+
|
163
|
+
GPS::PVT.class_eval{
|
164
|
+
define_method(:post_solution){|target|
|
165
|
+
sats, az, el = proc{|g|
|
166
|
+
self.used_satellite_list.collect.with_index{|prn, i|
|
167
|
+
# G_enu is measured in the direction from satellite to user positions
|
168
|
+
[prn,
|
169
|
+
Math::atan2(-g[i, 0], -g[i, 1]),
|
170
|
+
Math::asin(-g[i, 2])]
|
171
|
+
}.transpose
|
172
|
+
}.call(self.G_enu) rescue [[], [], []]
|
173
|
+
[[:@azimuth, az], [:@elevation, el]].each{|k, values|
|
174
|
+
self.instance_variable_set(k, Hash[*(sats.zip(values).flatten(1))])
|
175
|
+
}
|
176
|
+
[:@slopeH, :@slopeV] \
|
177
|
+
.zip((self.slope_HV_enu.to_a.transpose rescue [nil, nil])) \
|
178
|
+
.each{|k, values|
|
179
|
+
self.instance_variable_set(k,
|
180
|
+
Hash[*(values ? sats.zip(values).flatten(1) : [])])
|
181
|
+
}
|
182
|
+
instance_variable_get(target)
|
183
|
+
}
|
184
|
+
[:azimuth, :elevation, :slopeH, :slopeV].each{|k|
|
185
|
+
eval("define_method(:#{k}){@#{k} || self.post_solution(:@#{k})}")
|
186
|
+
}
|
187
|
+
}
|
188
|
+
|
189
|
+
proc{
|
190
|
+
eph_list = Hash[*(1..32).collect{|prn|
|
191
|
+
eph = GPS::Ephemeris::new
|
192
|
+
eph.svid = prn
|
193
|
+
[prn, eph]
|
194
|
+
}.flatten(1)]
|
195
|
+
define_method(:register_ephemeris){|t_meas, prn, bcast_data|
|
196
|
+
next unless eph = eph_list[prn]
|
197
|
+
sn = @solver.gps_space_node
|
198
|
+
subframe, iodc_or_iode = eph.parse(bcast_data)
|
199
|
+
if iodc_or_iode < 0 then
|
200
|
+
begin
|
201
|
+
sn.update_iono_utc(
|
202
|
+
GPS::Ionospheric_UTC_Parameters::parse(bcast_data))
|
203
|
+
[:alpha, :beta].each{|k|
|
204
|
+
$stderr.puts "Iono #{k}: #{sn.iono_utc.send(k)}"
|
205
|
+
} if false
|
206
|
+
rescue
|
207
|
+
end
|
208
|
+
next
|
209
|
+
end
|
210
|
+
if t_meas and eph.consistent? then
|
211
|
+
eph.WN = ((t_meas.week / 1024).to_i * 1024) + (eph.WN % 1024)
|
212
|
+
sn.register_ephemeris(prn, eph)
|
213
|
+
eph.invalidate
|
214
|
+
end
|
215
|
+
}
|
216
|
+
}.call
|
217
|
+
|
218
|
+
def parse_ubx(ubx_fname, &b)
|
219
|
+
$stderr.print "Reading UBX file (%s) "%[ubx_fname]
|
220
|
+
require_relative 'ubx'
|
221
|
+
|
222
|
+
ubx = UBX::new(open(ubx_fname))
|
223
|
+
ubx_kind = Hash::new(0)
|
224
|
+
|
225
|
+
after_run = b || proc{|pvt| puts pvt.to_s}
|
226
|
+
|
227
|
+
t_meas = nil
|
228
|
+
ubx.each_packet.with_index(1){|packet, i|
|
229
|
+
$stderr.print '.' if i % 1000 == 0
|
230
|
+
ubx_kind[packet[2..3]] += 1
|
231
|
+
case packet[2..3]
|
232
|
+
when [0x02, 0x10] # RXM-RAW
|
233
|
+
msec, week = [[0, 4, "V"], [4, 2, "v"]].collect{|offset, len, str|
|
234
|
+
packet.slice(6 + offset, len).pack("C*").unpack(str)[0]
|
235
|
+
}
|
236
|
+
t_meas = GPS::Time::new(week, msec.to_f / 1000)
|
237
|
+
meas = GPS::Measurement::new
|
238
|
+
packet[6 + 6].times{|i|
|
239
|
+
loader = proc{|offset, len, str|
|
240
|
+
ary = packet.slice(6 + offset + (i * 24), len)
|
241
|
+
str ? ary.pack("C*").unpack(str)[0] : ary
|
242
|
+
}
|
243
|
+
prn = loader.call(28, 1)[0]
|
244
|
+
{
|
245
|
+
:L1_PSEUDORANGE => [16, 8, "E"],
|
246
|
+
:L1_DOPPLER => [24, 4, "e"],
|
247
|
+
}.each{|k, prop|
|
248
|
+
meas.add(prn, GPS::Measurement.const_get(k), loader.call(*prop))
|
249
|
+
}
|
250
|
+
}
|
251
|
+
after_run.call(run(meas, t_meas), [meas, t_meas])
|
252
|
+
when [0x02, 0x15] # RXM-RAWX
|
253
|
+
sec, week = [[0, 8, "E"], [8, 2, "v"]].collect{|offset, len, str|
|
254
|
+
packet.slice(6 + offset, len).pack("C*").unpack(str)[0]
|
255
|
+
}
|
256
|
+
t_meas = GPS::Time::new(week, sec)
|
257
|
+
meas = GPS::Measurement::new
|
258
|
+
packet[6 + 11].times{|i|
|
259
|
+
loader = proc{|offset, len, str, post|
|
260
|
+
v = packet.slice(6 + offset + (i * 32), len)
|
261
|
+
v = str ? v.pack("C*").unpack(str)[0] : v
|
262
|
+
v = post.call(v) if post
|
263
|
+
v
|
264
|
+
}
|
265
|
+
next unless (gnss = loader.call(36, 1)[0]) == 0
|
266
|
+
svid = loader.call(37, 1)[0]
|
267
|
+
trk_stat = loader.call(46, 1)[0]
|
268
|
+
{
|
269
|
+
:L1_PSEUDORANGE => [16, 8, "E", proc{|v| (trk_stat & 0x1 == 0x1) ? v : nil}],
|
270
|
+
:L1_PSEUDORANGE_SIGMA => [43, 1, nil, proc{|v|
|
271
|
+
(trk_stat & 0x1 == 0x1) ? (1E-2 * (v[0] & 0xF)) : nil
|
272
|
+
}],
|
273
|
+
:L1_DOPPLER => [32, 4, "e"],
|
274
|
+
:L1_DOPPLER_SIGMA => [45, 1, nil, proc{|v| 2E-3 * (v[0] & 0xF)}],
|
275
|
+
}.each{|k, prop|
|
276
|
+
next unless v = loader.call(*prop)
|
277
|
+
meas.add(svid, GPS::Measurement.const_get(k), v)
|
278
|
+
}
|
279
|
+
}
|
280
|
+
after_run.call(run(meas, t_meas), [meas, t_meas])
|
281
|
+
when [0x02, 0x11] # RXM-SFRB
|
282
|
+
register_ephemeris(
|
283
|
+
t_meas,
|
284
|
+
packet[6 + 1],
|
285
|
+
packet.slice(6 + 2, 40).each_slice(4).collect{|v|
|
286
|
+
(v.pack("C*").unpack("V")[0] & 0xFFFFFF) << 6
|
287
|
+
})
|
288
|
+
when [0x02, 0x13] # RXM-SFRBX
|
289
|
+
next unless (gnss = packet[6]) == 0
|
290
|
+
register_ephemeris(
|
291
|
+
t_meas,
|
292
|
+
packet[6 + 1],
|
293
|
+
packet.slice(6 + 8, 4 * packet[6 + 4]).each_slice(4).collect{|v|
|
294
|
+
v.pack("C*").unpack("V")[0]
|
295
|
+
})
|
296
|
+
end
|
297
|
+
}
|
298
|
+
$stderr.puts ", found packets are %s"%[ubx_kind.inspect]
|
299
|
+
end
|
300
|
+
|
301
|
+
def parse_rinex_nav(fname)
|
302
|
+
$stderr.puts "Read RINEX NAV file (%s): %d items."%[
|
303
|
+
fname, @solver.gps_space_node.read(fname)]
|
304
|
+
end
|
305
|
+
|
306
|
+
def parse_rinex_obs(fname, &b)
|
307
|
+
after_run = b || proc{|pvt| puts pvt.to_s}
|
308
|
+
$stderr.print "Reading RINEX observation file (%s)"%[fname]
|
309
|
+
types = nil
|
310
|
+
count = 0
|
311
|
+
GPS::RINEX_Observation::read(fname){|item|
|
312
|
+
$stderr.print '.' if (count += 1) % 1000 == 0
|
313
|
+
t_meas = item[:time]
|
314
|
+
|
315
|
+
meas = GPS::Measurement::new
|
316
|
+
types ||= (item[:meas_types]['G'] || item[:meas_types][' ']).collect.with_index{|type_, i|
|
317
|
+
case type_
|
318
|
+
when "C1", "C1C"
|
319
|
+
[i, GPS::Measurement::L1_PSEUDORANGE]
|
320
|
+
when "D1", "D1C"
|
321
|
+
[i, GPS::Measurement::L1_DOPPLER]
|
322
|
+
else
|
323
|
+
nil
|
324
|
+
end
|
325
|
+
}.compact
|
326
|
+
item[:meas].each{|k, v|
|
327
|
+
sys, prn = k
|
328
|
+
next unless sys == 'G' # GPS only
|
329
|
+
types.each{|i, type_|
|
330
|
+
meas.add(prn, type_, v[i][0]) if v[i]
|
331
|
+
}
|
332
|
+
}
|
333
|
+
after_run.call(run(meas, t_meas), [meas, t_meas])
|
334
|
+
}
|
335
|
+
$stderr.puts ", %d epochs."%[count]
|
336
|
+
end
|
337
|
+
end
|
338
|
+
end
|
339
|
+
|
340
|
+
if __FILE__ == $0 then
|
341
|
+
# runnable quick example to solve PVT by using RINEX NAV/OBS or u-blox ubx
|
342
|
+
options = {}
|
343
|
+
|
344
|
+
# check options
|
345
|
+
ARGV.reject!{|arg|
|
346
|
+
next false unless arg =~ /^--([^=]+)=?/
|
347
|
+
options[$1.to_sym] = $'
|
348
|
+
true
|
349
|
+
}
|
350
|
+
|
351
|
+
# Check file existence
|
352
|
+
ARGV.each{|arg|
|
353
|
+
raise "File not found: #{arg}" unless File::exist?(arg)
|
354
|
+
}
|
355
|
+
|
356
|
+
rcv = GPS_PVT::Receiver::new(options)
|
357
|
+
|
358
|
+
puts GPS_PVT::Receiver::header
|
359
|
+
|
360
|
+
# parse RINEX NAV
|
361
|
+
ARGV.reject!{|arg|
|
362
|
+
next false unless arg =~ /\.\d{2}n$/
|
363
|
+
rcv.parse_rinex_nav(arg)
|
364
|
+
}
|
365
|
+
|
366
|
+
# other files
|
367
|
+
ARGV.each{|arg|
|
368
|
+
case arg
|
369
|
+
when /\.ubx$/
|
370
|
+
rcv.parse_ubx(arg)
|
371
|
+
when /\.\d{2}o$/
|
372
|
+
rcv.parse_rinex_obs(arg)
|
373
|
+
end
|
374
|
+
}
|
375
|
+
end
|
data/lib/gps_pvt/ubx.rb
ADDED
@@ -0,0 +1,148 @@
|
|
1
|
+
# U-blox file utilities
|
2
|
+
# The origin is ninja-scan-light/tool/misc/ubx.rb
|
3
|
+
|
4
|
+
# Copyright (c) 2016, M.Naruoka (fenrir)
|
5
|
+
# All rights reserved.
|
6
|
+
#
|
7
|
+
# Redistribution and use in source and binary forms, with or without modification,
|
8
|
+
# are permitted provided that the following conditions are met:
|
9
|
+
#
|
10
|
+
# - Redistributions of source code must retain the above copyright notice,
|
11
|
+
# this list of conditions and the following disclaimer.
|
12
|
+
# - Redistributions in binary form must reproduce the above copyright notice,
|
13
|
+
# this list of conditions and the following disclaimer in the documentation
|
14
|
+
# and/or other materials provided with the distribution.
|
15
|
+
# - Neither the name of the naruoka.org nor the names of its contributors
|
16
|
+
# may be used to endorse or promote products derived from this software
|
17
|
+
# without specific prior written permission.
|
18
|
+
#
|
19
|
+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
20
|
+
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
21
|
+
# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
22
|
+
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
|
23
|
+
# BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
|
24
|
+
# OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
25
|
+
# PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
26
|
+
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
27
|
+
# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
28
|
+
# STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
29
|
+
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
30
|
+
# EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
31
|
+
|
32
|
+
module GPS_PVT
|
33
|
+
class UBX
|
34
|
+
def initialize(io)
|
35
|
+
@io = io
|
36
|
+
@buf = []
|
37
|
+
end
|
38
|
+
def UBX.checksum(packet, range = 2..-3)
|
39
|
+
ck_a, ck_b = [0, 0]
|
40
|
+
packet[range].each{|b|
|
41
|
+
ck_a += b
|
42
|
+
ck_b += ck_a
|
43
|
+
}
|
44
|
+
ck_a &= 0xFF
|
45
|
+
ck_b &= 0xFF
|
46
|
+
[ck_a, ck_b]
|
47
|
+
end
|
48
|
+
def UBX.update_checksum(packet)
|
49
|
+
packet[-2..-1] = checksum(packet)
|
50
|
+
packet
|
51
|
+
end
|
52
|
+
def UBX.update_size(packet, size = nil)
|
53
|
+
size ||= packet.size - 8
|
54
|
+
size = size.divmod(0x100)
|
55
|
+
packet[4] = size[1]
|
56
|
+
packet[5] = size[0]
|
57
|
+
packet
|
58
|
+
end
|
59
|
+
def UBX.update(packet)
|
60
|
+
[:update_size, :update_checksum].inject(packet){|arg, f|
|
61
|
+
UBX.send(f, arg)
|
62
|
+
}
|
63
|
+
end
|
64
|
+
def read_packet
|
65
|
+
while !@io.eof?
|
66
|
+
if @buf.size < 8 then
|
67
|
+
@buf += @io.read(8 - @buf.size).unpack('C*')
|
68
|
+
return nil if @buf.size < 8
|
69
|
+
end
|
70
|
+
|
71
|
+
if @buf[0] != 0xB5 then
|
72
|
+
@buf.shift
|
73
|
+
next
|
74
|
+
elsif @buf[1] != 0x62 then
|
75
|
+
@buf = @buf[2..-1]
|
76
|
+
next
|
77
|
+
end
|
78
|
+
|
79
|
+
len = (@buf[5] << 8) + @buf[4]
|
80
|
+
if @buf.size < len + 8 then
|
81
|
+
@buf += @io.read(len + 8 - @buf.size).unpack('C*')
|
82
|
+
return nil if @buf.size < len + 8
|
83
|
+
end
|
84
|
+
|
85
|
+
ck_a, ck_b = UBX::checksum(@buf, 2..(len + 5))
|
86
|
+
if (@buf[len + 6] != ck_a) || (@buf[len + 7] != ck_b) then
|
87
|
+
@buf = @buf[2..-1]
|
88
|
+
next
|
89
|
+
end
|
90
|
+
|
91
|
+
packet = @buf[0..(len + 7)]
|
92
|
+
@buf = @buf[(len + 8)..-1]
|
93
|
+
|
94
|
+
return packet
|
95
|
+
end
|
96
|
+
return nil
|
97
|
+
end
|
98
|
+
|
99
|
+
def each_packet(&b)
|
100
|
+
res = Enumerator::new{|y|
|
101
|
+
while packet = read_packet
|
102
|
+
y << packet
|
103
|
+
end
|
104
|
+
}
|
105
|
+
b ? res.each(&b) : res
|
106
|
+
end
|
107
|
+
|
108
|
+
def read_packets
|
109
|
+
each_packet.to_a
|
110
|
+
end
|
111
|
+
|
112
|
+
GNSS_ID = {
|
113
|
+
:GPS => 0,
|
114
|
+
:SBAS => 1,
|
115
|
+
:Galileo => 2,
|
116
|
+
:BeiDou => 3,
|
117
|
+
:QZSS => 5,
|
118
|
+
:GLONASS => 6,
|
119
|
+
}
|
120
|
+
|
121
|
+
SIGNAL_ID = {
|
122
|
+
:GPS => {:L1CA => 0, :L2CL => 3, :L2CM => 4},
|
123
|
+
:SBAS => {:L1CA => 0},
|
124
|
+
:Galileo => {:E1C => 0, :E1B => 1, :E5_bI => 5, :E5_bQ => 6},
|
125
|
+
:BeiDou => {:B1I_D1 => 0, :B1I_D2 => 1, :B2I_D1 => 2, :B2I_D2 => 3},
|
126
|
+
:QZSS => {:L1CA => 0, :L2CL => 4, :L2CM => 5},
|
127
|
+
:GLONASS => {:L1OF => 0, :L2OF => 2},
|
128
|
+
}
|
129
|
+
|
130
|
+
def UBX.svid(id, gnss = :GPS)
|
131
|
+
gnss = GNSS_ID[gnss] if gnss.kind_of?(Symbol)
|
132
|
+
case gnss
|
133
|
+
when GNSS_ID[:GPS], GNSS_ID[:SBAS]
|
134
|
+
id
|
135
|
+
when GNSS_ID[:Galileo]
|
136
|
+
id + 210
|
137
|
+
when GNSS_ID[:Beido]
|
138
|
+
(id < 6) ? (id + 158) : (id + 27)
|
139
|
+
when GNSS_ID[:QZSS]
|
140
|
+
id + 192
|
141
|
+
when GNSS_ID[:GLONASS]
|
142
|
+
id + 64
|
143
|
+
else
|
144
|
+
nil
|
145
|
+
end
|
146
|
+
end
|
147
|
+
end
|
148
|
+
end
|
data/lib/gps_pvt.rb
ADDED
data/sig/gps_pvt.rbs
ADDED
metadata
ADDED
@@ -0,0 +1,117 @@
|
|
1
|
+
--- !ruby/object:Gem::Specification
|
2
|
+
name: gps_pvt
|
3
|
+
version: !ruby/object:Gem::Version
|
4
|
+
version: 0.1.1
|
5
|
+
platform: ruby
|
6
|
+
authors:
|
7
|
+
- fenrir(M.Naruoka)
|
8
|
+
autorequire:
|
9
|
+
bindir: exe
|
10
|
+
cert_chain: []
|
11
|
+
date: 2021-12-20 00:00:00.000000000 Z
|
12
|
+
dependencies:
|
13
|
+
- !ruby/object:Gem::Dependency
|
14
|
+
name: rake
|
15
|
+
requirement: !ruby/object:Gem::Requirement
|
16
|
+
requirements:
|
17
|
+
- - ">="
|
18
|
+
- !ruby/object:Gem::Version
|
19
|
+
version: '0'
|
20
|
+
type: :development
|
21
|
+
prerelease: false
|
22
|
+
version_requirements: !ruby/object:Gem::Requirement
|
23
|
+
requirements:
|
24
|
+
- - ">="
|
25
|
+
- !ruby/object:Gem::Version
|
26
|
+
version: '0'
|
27
|
+
- !ruby/object:Gem::Dependency
|
28
|
+
name: rake-compiler
|
29
|
+
requirement: !ruby/object:Gem::Requirement
|
30
|
+
requirements:
|
31
|
+
- - ">="
|
32
|
+
- !ruby/object:Gem::Version
|
33
|
+
version: '0'
|
34
|
+
type: :development
|
35
|
+
prerelease: false
|
36
|
+
version_requirements: !ruby/object:Gem::Requirement
|
37
|
+
requirements:
|
38
|
+
- - ">="
|
39
|
+
- !ruby/object:Gem::Version
|
40
|
+
version: '0'
|
41
|
+
description: This module calculate PVT by using raw observation obtained from a GPS
|
42
|
+
receiver
|
43
|
+
email:
|
44
|
+
- fenrir.naru@gmail.com
|
45
|
+
executables: []
|
46
|
+
extensions:
|
47
|
+
- ext/gps_pvt/extconf.rb
|
48
|
+
extra_rdoc_files: []
|
49
|
+
files:
|
50
|
+
- ".rspec"
|
51
|
+
- CHANGELOG.md
|
52
|
+
- CODE_OF_CONDUCT.md
|
53
|
+
- Gemfile
|
54
|
+
- README.md
|
55
|
+
- Rakefile
|
56
|
+
- bin/console
|
57
|
+
- bin/setup
|
58
|
+
- ext/gps_pvt/Coordinate/Coordinate_wrap.cxx
|
59
|
+
- ext/gps_pvt/GPS/GPS_wrap.cxx
|
60
|
+
- ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx
|
61
|
+
- ext/gps_pvt/extconf.rb
|
62
|
+
- ext/ninja-scan-light/tool/navigation/EGM.h
|
63
|
+
- ext/ninja-scan-light/tool/navigation/GPS.h
|
64
|
+
- ext/ninja-scan-light/tool/navigation/GPS_Solver.h
|
65
|
+
- ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h
|
66
|
+
- ext/ninja-scan-light/tool/navigation/GPS_Solver_MultiFrequency.h
|
67
|
+
- ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h
|
68
|
+
- ext/ninja-scan-light/tool/navigation/MagneticField.h
|
69
|
+
- ext/ninja-scan-light/tool/navigation/NTCM.h
|
70
|
+
- ext/ninja-scan-light/tool/navigation/RINEX.h
|
71
|
+
- ext/ninja-scan-light/tool/navigation/WGS84.h
|
72
|
+
- ext/ninja-scan-light/tool/navigation/coordinate.h
|
73
|
+
- ext/ninja-scan-light/tool/param/bit_array.h
|
74
|
+
- ext/ninja-scan-light/tool/param/complex.h
|
75
|
+
- ext/ninja-scan-light/tool/param/matrix.h
|
76
|
+
- ext/ninja-scan-light/tool/param/matrix_fixed.h
|
77
|
+
- ext/ninja-scan-light/tool/param/matrix_special.h
|
78
|
+
- ext/ninja-scan-light/tool/param/quaternion.h
|
79
|
+
- ext/ninja-scan-light/tool/param/vector3.h
|
80
|
+
- ext/ninja-scan-light/tool/swig/Coordinate.i
|
81
|
+
- ext/ninja-scan-light/tool/swig/GPS.i
|
82
|
+
- ext/ninja-scan-light/tool/swig/SylphideMath.i
|
83
|
+
- ext/ninja-scan-light/tool/swig/extconf.rb
|
84
|
+
- ext/ninja-scan-light/tool/swig/makefile
|
85
|
+
- ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb
|
86
|
+
- ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb
|
87
|
+
- gps_pvt.gemspec
|
88
|
+
- lib/gps_pvt.rb
|
89
|
+
- lib/gps_pvt/receiver.rb
|
90
|
+
- lib/gps_pvt/ubx.rb
|
91
|
+
- lib/gps_pvt/version.rb
|
92
|
+
- sig/gps_pvt.rbs
|
93
|
+
homepage: https://github.com/fenrir-naru/gps_pvt
|
94
|
+
licenses: []
|
95
|
+
metadata:
|
96
|
+
homepage_uri: https://github.com/fenrir-naru/gps_pvt
|
97
|
+
source_code_uri: https://github.com/fenrir-naru/gps_pvt
|
98
|
+
post_install_message:
|
99
|
+
rdoc_options: []
|
100
|
+
require_paths:
|
101
|
+
- lib
|
102
|
+
required_ruby_version: !ruby/object:Gem::Requirement
|
103
|
+
requirements:
|
104
|
+
- - ">="
|
105
|
+
- !ruby/object:Gem::Version
|
106
|
+
version: 2.6.0
|
107
|
+
required_rubygems_version: !ruby/object:Gem::Requirement
|
108
|
+
requirements:
|
109
|
+
- - ">="
|
110
|
+
- !ruby/object:Gem::Version
|
111
|
+
version: '0'
|
112
|
+
requirements: []
|
113
|
+
rubygems_version: 3.1.2
|
114
|
+
signing_key:
|
115
|
+
specification_version: 4
|
116
|
+
summary: GPS position, velocity, and time (PVT) solver
|
117
|
+
test_files: []
|