gps_pvt 0.1.1 → 0.1.2

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -1,375 +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
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