gps_pvt 0.1.7 → 0.2.0

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.
@@ -7,99 +7,122 @@ require_relative 'GPS'
7
7
 
8
8
  module GPS_PVT
9
9
  class Receiver
10
- OUTPUT_PVT_ITEMS = [[
11
- [:week, :itow_rcv, :year, :month, :mday, :hour, :min, :sec],
12
- proc{|pvt|
13
- [:week, :seconds, :c_tm].collect{|f| pvt.receiver_time.send(f)}.flatten
14
- }
15
- ]] + [[
16
- [:receiver_clock_error_meter, :longitude, :latitude, :height, :rel_E, :rel_N, :rel_U],
17
- proc{|pvt|
18
- next [nil] * 7 unless pvt.position_solved?
10
+ def self.pvt_items(opt = {})
11
+ opt = {
12
+ :system => [[:GPS, 1..32]],
13
+ :satellites => (1..32).to_a,
14
+ }.merge(opt)
15
+ [[
16
+ [:week, :itow_rcv, :year, :month, :mday, :hour, :min, :sec],
17
+ proc{|pvt|
18
+ [:week, :seconds, :c_tm].collect{|f| pvt.receiver_time.send(f)}.flatten
19
+ }
20
+ ]] + [[
21
+ [:receiver_clock_error_meter, :longitude, :latitude, :height, :rel_E, :rel_N, :rel_U],
22
+ proc{|pvt|
23
+ next [nil] * 7 unless pvt.position_solved?
24
+ [
25
+ pvt.receiver_error,
26
+ pvt.llh.lng / Math::PI * 180,
27
+ pvt.llh.lat / Math::PI * 180,
28
+ pvt.llh.alt,
29
+ ] + (pvt.rel_ENU.to_a rescue [nil] * 3)
30
+ }
31
+ ]] + [proc{
32
+ labels = [:g, :p, :h, :v, :t].collect{|k| "#{k}dop".to_sym}
19
33
  [
20
- pvt.receiver_error,
21
- pvt.llh.lng / Math::PI * 180,
22
- pvt.llh.lat / Math::PI * 180,
23
- pvt.llh.alt,
24
- ] + (pvt.rel_ENU.to_a rescue [nil] * 3)
25
- }
26
- ]] + [proc{
27
- labels = [:g, :p, :h, :v, :t].collect{|k| "#{k}dop".to_sym}
28
- [
29
- labels,
34
+ labels,
35
+ proc{|pvt|
36
+ next [nil] * 5 unless pvt.position_solved?
37
+ labels.collect{|k| pvt.send(k)}
38
+ }
39
+ ]
40
+ }.call] + [[
41
+ [:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms],
30
42
  proc{|pvt|
31
- next [nil] * 5 unless pvt.position_solved?
32
- labels.collect{|k| pvt.send(k)}
43
+ next [nil] * 4 unless pvt.velocity_solved?
44
+ [:north, :east, :down].collect{|k| pvt.velocity.send(k)} \
45
+ + [pvt.receiver_error_rate]
33
46
  }
34
- ]
35
- }.call] + [[
36
- [:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms],
37
- proc{|pvt|
38
- next [nil] * 4 unless pvt.velocity_solved?
39
- [:north, :east, :down].collect{|k| pvt.velocity.send(k)} \
40
- + [pvt.receiver_error_rate]
41
- }
42
- ]] + [
43
- [:used_satellites, proc{|pvt| pvt.used_satellites}],
44
- [:PRN, proc{|pvt|
45
- ("%32s"%[pvt.used_satellite_list.collect{|i|
46
- 1 << (i - 1)
47
- }.inject(0){|res, v| res | v}.to_s(2)]).scan(/.{8}/).collect{|str|
48
- str.gsub(' ', '0')
49
- }.join('_')
50
- }],
51
- ] + [[
52
- (1..32).collect{|prn|
53
- [:range_residual, :weight, :azimuth, :elevation, :slopeH, :slopeV].collect{|str| "#{str}(#{prn})"}
54
- }.flatten,
55
- proc{|pvt|
56
- next ([nil] * 6 * 32) unless pvt.position_solved?
57
- sats = pvt.used_satellite_list
58
- r, w = [:delta_r, :W].collect{|f| pvt.send(f)}
59
- (1..32).collect{|i|
60
- next ([nil] * 6) unless i2 = sats.index(i)
61
- [r[i2, 0], w[i2, i2]] +
62
- [:azimuth, :elevation].collect{|f|
63
- pvt.send(f)[i] / Math::PI * 180
64
- } + [pvt.slopeH[i], pvt.slopeV[i]]
65
- }.flatten
66
- },
67
- ]] + [[
68
- [:wssr, :wssr_sf, :weight_max,
69
- :slopeH_max, :slopeH_max_PRN, :slopeH_max_elevation,
70
- :slopeV_max, :slopeV_max_PRN, :slopeV_max_elevation],
71
- proc{|pvt|
72
- next [nil] * 9 unless fd = pvt.fd
73
- el_deg = [4, 6].collect{|i| pvt.elevation[fd[i]] / Math::PI * 180}
74
- fd[0..4] + [el_deg[0]] + fd[5..6] + [el_deg[1]]
75
- }
76
- ]] + [[
77
- [:wssr_FDE_min, :wssr_FDE_min_PRN, :wssr_FDE_2nd, :wssr_FDE_2nd_PRN],
78
- proc{|pvt|
79
- [:fde_min, :fde_2nd].collect{|f|
80
- info = pvt.send(f)
81
- next ([nil] * 2) if (!info) || info.empty?
82
- [info[0], info[-3]]
83
- }.flatten
84
- }
85
- ]]
47
+ ]] + [
48
+ [:used_satellites, proc{|pvt| pvt.used_satellites}],
49
+ ] + opt[:system].collect{|sys, range|
50
+ bit_flip = if range.kind_of?(Array) then
51
+ proc{|res, i|
52
+ res[i] = "1" if i = range.index(i)
53
+ res
54
+ }
55
+ else # expect Range
56
+ base_prn = range.min
57
+ proc{|res, i|
58
+ res[i - base_prn] = "1" if range.include?(i)
59
+ res
60
+ }
61
+ end
62
+ ["#{sys}_PRN", proc{|pvt|
63
+ pvt.used_satellite_list.inject("0" * range.size, &bit_flip) \
64
+ .scan(/.{1,8}/).join('_').reverse
65
+ }]
66
+ } + [[
67
+ opt[:satellites].collect{|prn, label|
68
+ [:range_residual, :weight, :azimuth, :elevation, :slopeH, :slopeV].collect{|str|
69
+ "#{str}(#{label || prn})"
70
+ }
71
+ }.flatten,
72
+ proc{|pvt|
73
+ next ([nil] * 6 * opt[:satellites].size) unless pvt.position_solved?
74
+ sats = pvt.used_satellite_list
75
+ r, w = [:delta_r, :W].collect{|f| pvt.send(f)}
76
+ opt[:satellites].collect{|i|
77
+ next ([nil] * 6) unless i2 = sats.index(i)
78
+ [r[i2, 0], w[i2, i2]] +
79
+ [:azimuth, :elevation].collect{|f|
80
+ pvt.send(f)[i] / Math::PI * 180
81
+ } + [pvt.slopeH[i], pvt.slopeV[i]]
82
+ }.flatten
83
+ },
84
+ ]] + [[
85
+ [:wssr, :wssr_sf, :weight_max,
86
+ :slopeH_max, :slopeH_max_PRN, :slopeH_max_elevation,
87
+ :slopeV_max, :slopeV_max_PRN, :slopeV_max_elevation],
88
+ proc{|pvt|
89
+ next [nil] * 9 unless fd = pvt.fd
90
+ el_deg = [4, 6].collect{|i| pvt.elevation[fd[i]] / Math::PI * 180}
91
+ fd[0..4] + [el_deg[0]] + fd[5..6] + [el_deg[1]]
92
+ }
93
+ ]] + [[
94
+ [:wssr_FDE_min, :wssr_FDE_min_PRN, :wssr_FDE_2nd, :wssr_FDE_2nd_PRN],
95
+ proc{|pvt|
96
+ [:fde_min, :fde_2nd].collect{|f|
97
+ info = pvt.send(f)
98
+ next ([nil] * 2) if (!info) || info.empty?
99
+ [info[0], info[-3]]
100
+ }.flatten
101
+ }
102
+ ]]
103
+ end
86
104
 
87
- OUTPUT_MEAS_ITEMS = [[
88
- (1..32).collect{|prn|
89
- [:L1_range, :L1_rate].collect{|str| "#{str}(#{prn})"}
90
- }.flatten,
91
- proc{|meas|
92
- meas_hash = Hash[*(meas.collect{|prn, k, v| [[prn, k], v]}.flatten(1))]
93
- (1..32).collect{|prn|
94
- [:L1_PSEUDORANGE, [:L1_DOPPLER, GPS::SpaceNode.L1_WaveLength]].collect{|k, sf|
95
- meas_hash[[prn, GPS::Measurement.const_get(k)]] * (sf || 1) rescue nil
105
+ def self.meas_items(opt = {})
106
+ opt = {
107
+ :satellites => (1..32).to_a,
108
+ }.merge(opt)
109
+ [[
110
+ opt[:satellites].collect{|prn, label|
111
+ [:L1_range, :L1_rate].collect{|str| "#{str}(#{label || prn})"}
112
+ }.flatten,
113
+ proc{|meas|
114
+ meas_hash = Hash[*(meas.collect{|prn, k, v| [[prn, k], v]}.flatten(1))]
115
+ opt[:satellites].collect{|prn|
116
+ [:L1_PSEUDORANGE, [:L1_DOPPLER, GPS::SpaceNode.L1_WaveLength]].collect{|k, sf|
117
+ meas_hash[[prn, GPS::Measurement.const_get(k)]] * (sf || 1) rescue nil
118
+ }
96
119
  }
97
120
  }
98
- }
99
- ]]
100
-
101
- def self.header
102
- (OUTPUT_PVT_ITEMS + OUTPUT_MEAS_ITEMS).transpose[0].flatten.join(',')
121
+ ]]
122
+ end
123
+
124
+ def header
125
+ (@output[:pvt] + @output[:meas]).transpose[0].flatten.join(',')
103
126
  end
104
127
 
105
128
  attr_accessor :solver
@@ -111,8 +134,17 @@ class Receiver
111
134
  rel_prop[0] = 1 if rel_prop[0] > 0 # weight = 1
112
135
  rel_prop
113
136
  }
137
+ @debug = {}
138
+ output_options = {
139
+ :system => [[:GPS, 1..32], [:QZSS, 193..202]],
140
+ :satellites => (1..32).to_a + (193..202).to_a, # [idx, ...] or [[idx, label], ...] is acceptable
141
+ }
114
142
  options = options.reject{|k, v|
115
143
  case k
144
+ when :debug
145
+ v = v.split(/,/)
146
+ @debug[v[0].upcase.to_sym] = v[1..-1]
147
+ next true
116
148
  when :weight
117
149
  case v.to_sym
118
150
  when :elevation # (same as underneath C++ library)
@@ -156,6 +188,49 @@ class Receiver
156
188
  llh[0..1].collect{|rad| rad / Math::PI * 180} + [llh[2]]
157
189
  }"
158
190
  next true
191
+ when :with, :without
192
+ [v].flatten.each{|spec| # array is acceptable
193
+ sys, svid = case spec
194
+ when Integer
195
+ [nil, spec]
196
+ when /([a-zA-Z]+)(?::(-?\d+))?/
197
+ [$1.upcase.to_sym, (Integre($2) rescue nil)]
198
+ when /-?\d+/
199
+ [nil, $&.to_i]
200
+ else
201
+ next false
202
+ end
203
+ mode = if svid && (svid < 0) then
204
+ svid *= -1
205
+ (k == :with) ? :exclude : :include
206
+ else
207
+ (k == :with) ? :include : :exclude
208
+ end
209
+ if (sys == :GPS) || (sys == :QZSS) \
210
+ || (svid && ((1..32).include?(svid) || (193..202).include?(svid))) then
211
+ [svid || ((1..32).to_a + (193..202).to_a)].flatten.each{
212
+ @solver.gps_options.send(mode, svid)
213
+ }
214
+ elsif (sys == :SBAS) || (svid && (120..158).include?(svid)) then
215
+ prns = [svid || (120..158).to_a].flatten
216
+ unless (i = output_options[:system].index{|sys, range| sys == :SBAS}) then
217
+ i = -1
218
+ output_options[:system] << [:SBAS, []]
219
+ else
220
+ output_options[:system][i].reject!{|prn| prns.include?(prn)}
221
+ end
222
+ output_options[:satellites].reject!{|prn, label| prns.include?(prn)}
223
+ if mode == :include then
224
+ output_options[:system][i][1] += prns
225
+ output_options[:satellites] += prns
226
+ end
227
+ prns.each{|prn| @solver.sbas_options.send(mode, prn)}
228
+ else
229
+ next false
230
+ end
231
+ $stderr.puts "#{mode.capitalize} satellite: #{[sys, svid].compact.join(':')}"
232
+ }
233
+ next true
159
234
  end
160
235
  false
161
236
  }
@@ -164,6 +239,10 @@ class Receiver
164
239
  opt.elevation_mask = 0.0 / 180 * Math::PI # 0 deg
165
240
  opt.residual_mask = 1E4 # 10 km
166
241
  }.call(@solver.gps_options)
242
+ @output = {
243
+ :pvt => Receiver::pvt_items(output_options),
244
+ :meas => Receiver::meas_items(output_options),
245
+ }
167
246
  end
168
247
 
169
248
  GPS::Measurement.class_eval{
@@ -202,10 +281,11 @@ class Receiver
202
281
  pvt.define_singleton_method(:rel_ENU){
203
282
  Coordinate::ENU::relative(xyz, ref_pos)
204
283
  } if (ref_pos && pvt.position_solved?)
284
+ output = @output
205
285
  pvt.define_singleton_method(:to_s){
206
- (OUTPUT_PVT_ITEMS.transpose[1].collect{|task|
286
+ (output[:pvt].transpose[1].collect{|task|
207
287
  task.call(pvt)
208
- } + OUTPUT_MEAS_ITEMS.transpose[1].collect{|task|
288
+ } + output[:meas].transpose[1].collect{|task|
209
289
  task.call(meas)
210
290
  }).flatten.join(',')
211
291
  }
@@ -226,7 +306,7 @@ class Receiver
226
306
  self.instance_variable_set(k, Hash[*(sats.zip(values).flatten(1))])
227
307
  }
228
308
  [:@slopeH, :@slopeV] \
229
- .zip((self.slope_HV_enu.to_a.transpose rescue [nil, nil])) \
309
+ .zip((self.fd ? self.slope_HV_enu.to_a.transpose : [nil, nil])) \
230
310
  .each{|k, values|
231
311
  self.instance_variable_set(k,
232
312
  Hash[*(values ? sats.zip(values).flatten(1) : [])])
@@ -239,30 +319,41 @@ class Receiver
239
319
  }
240
320
 
241
321
  proc{
242
- eph_list = Hash[*(1..32).collect{|prn|
322
+ eph_list = Hash[*((1..32).to_a + (193..202).to_a).collect{|prn|
243
323
  eph = GPS::Ephemeris::new
244
324
  eph.svid = prn
245
325
  [prn, eph]
246
326
  }.flatten(1)]
247
- define_method(:register_ephemeris){|t_meas, prn, bcast_data|
248
- next unless eph = eph_list[prn]
249
- sn = @solver.gps_space_node
250
- subframe, iodc_or_iode = eph.parse(bcast_data)
251
- if iodc_or_iode < 0 then
252
- begin
253
- sn.update_iono_utc(
254
- GPS::Ionospheric_UTC_Parameters::parse(bcast_data))
255
- [:alpha, :beta].each{|k|
256
- $stderr.puts "Iono #{k}: #{sn.iono_utc.send(k)}"
257
- } if false
258
- rescue
327
+ define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data|
328
+ case sys
329
+ when :GPS, :QZSS
330
+ next unless eph = eph_list[prn]
331
+ sn = @solver.gps_space_node
332
+ subframe, iodc_or_iode = eph.parse(bcast_data)
333
+ if iodc_or_iode < 0 then
334
+ begin
335
+ sn.update_iono_utc(
336
+ GPS::Ionospheric_UTC_Parameters::parse(bcast_data))
337
+ [:alpha, :beta].each{|k|
338
+ $stderr.puts "Iono #{k}: #{sn.iono_utc.send(k)}"
339
+ } if false
340
+ rescue
341
+ end
342
+ next
343
+ end
344
+ if t_meas and eph.consistent? then
345
+ eph.WN = ((t_meas.week / 1024).to_i * 1024) + (eph.WN % 1024)
346
+ sn.register_ephemeris(prn, eph)
347
+ eph.invalidate
348
+ end
349
+ when :SBAS
350
+ case @solver.sbas_space_node.decode_message(bcast_data[0..7], prn, t_meas)
351
+ when 26
352
+ ['', "IGP broadcasted by PRN#{prn} @ #{Time::utc(*t_meas.c_tm)}",
353
+ @solver.sbas_space_node.ionospheric_grid_points(prn)].each{|str|
354
+ $stderr.puts str
355
+ } if @debug[:SBAS_IGP]
259
356
  end
260
- next
261
- end
262
- if t_meas and eph.consistent? then
263
- eph.WN = ((t_meas.week / 1024).to_i * 1024) + (eph.WN % 1024)
264
- sn.register_ephemeris(prn, eph)
265
- eph.invalidate
266
357
  end
267
358
  }
268
359
  }.call
@@ -276,6 +367,24 @@ class Receiver
276
367
 
277
368
  after_run = b || proc{|pvt| puts pvt.to_s}
278
369
 
370
+ gnss_serial = proc{|svid, sys|
371
+ if sys then # new numbering
372
+ sys = [:GPS, :SBAS, :Galileo, :BeiDou, :IMES, :QZSS, :GLONASS][sys] if sys.kind_of?(Integer)
373
+ case sys
374
+ when :QZSS; svid += 192
375
+ end
376
+ else # old numbering
377
+ sys = case svid
378
+ when 1..32; :GPS
379
+ when 120..158; :SBAS
380
+ when 193..202; :QZSS
381
+ when 65..96; svid -= 64; :GLONASS
382
+ when 255; :GLONASS
383
+ end
384
+ end
385
+ [sys, svid]
386
+ }
387
+
279
388
  t_meas = nil
280
389
  ubx.each_packet.with_index(1){|packet, i|
281
390
  $stderr.print '.' if i % 1000 == 0
@@ -320,8 +429,11 @@ class Receiver
320
429
  v = post.call(v) if post
321
430
  v
322
431
  }
323
- next unless (gnss = loader.call(36, 1)[0]) == 0
324
- svid = loader.call(37, 1)[0]
432
+ sys, svid = gnss_serial.call(*loader.call(36, 2).reverse)
433
+ case sys
434
+ when :GPS, :QZSS;
435
+ else; next
436
+ end
325
437
  trk_stat = loader.call(46, 1)[0]
326
438
  {
327
439
  :L1_PSEUDORANGE => [16, 8, "E", proc{|v| (trk_stat & 0x1 == 0x1) ? v : nil}],
@@ -343,17 +455,19 @@ class Receiver
343
455
  }
344
456
  after_run.call(run(meas, t_meas), [meas, t_meas])
345
457
  when [0x02, 0x11] # RXM-SFRB
458
+ sys, svid = gnss_serial.call(packet[6 + 1])
346
459
  register_ephemeris(
347
460
  t_meas,
348
- packet[6 + 1],
461
+ sys, svid,
349
462
  packet.slice(6 + 2, 40).each_slice(4).collect{|v|
350
- (v.pack("C*").unpack("V")[0] & 0xFFFFFF) << 6
463
+ res = v.pack("C*").unpack("V")[0]
464
+ (sys == :GPS) ? ((res & 0xFFFFFF) << 6) : res
351
465
  })
352
466
  when [0x02, 0x13] # RXM-SFRBX
353
- next unless (gnss = packet[6]) == 0
467
+ sys, svid = gnss_serial.call(packet[6 + 1], packet[6])
354
468
  register_ephemeris(
355
469
  t_meas,
356
- packet[6 + 1],
470
+ sys, svid,
357
471
  packet.slice(6 + 8, 4 * packet[6 + 4]).each_slice(4).collect{|v|
358
472
  v.pack("C*").unpack("V")[0]
359
473
  })
@@ -375,26 +489,33 @@ class Receiver
375
489
  GPS::RINEX_Observation::read(fname){|item|
376
490
  $stderr.print '.' if (count += 1) % 1000 == 0
377
491
  t_meas = item[:time]
378
-
492
+
493
+ types ||= Hash[*(item[:meas_types].collect{|sys, values|
494
+ [sys, values.collect.with_index{|type_, i|
495
+ case type_
496
+ when "C1", "C1C"
497
+ [i, :L1_PSEUDORANGE]
498
+ when "L1", "L1C"
499
+ [i, :L1_CARRIER_PHASE]
500
+ when "D1", "D1C"
501
+ [i, :L1_DOPPLER]
502
+ when "S1", "S1C"
503
+ [i, :L1_SIGNAL_STRENGTH_dBHz]
504
+ else
505
+ nil
506
+ end
507
+ }.compact]
508
+ }.flatten(1))]
509
+
379
510
  meas = GPS::Measurement::new
380
- types ||= (item[:meas_types]['G'] || item[:meas_types][' ']).collect.with_index{|type_, i|
381
- case type_
382
- when "C1", "C1C"
383
- [i, :L1_PSEUDORANGE]
384
- when "L1", "L1C"
385
- [i, :L1_CARRIER_PHASE]
386
- when "D1", "D1C"
387
- [i, :L1_DOPPLER]
388
- when "S1", "S1C"
389
- [i, :L1_SIGNAL_STRENGTH_dBHz]
390
- else
391
- nil
392
- end
393
- }.compact
394
511
  item[:meas].each{|k, v|
395
512
  sys, prn = k
396
- next unless sys == 'G' # GPS only
397
- types.each{|i, type_|
513
+ case sys
514
+ when 'G', ' '
515
+ when 'J'; prn += 192
516
+ else; next
517
+ end
518
+ (types[sys] || []).each{|i, type_|
398
519
  meas.add(prn, type_, v[i][0]) if v[i]
399
520
  }
400
521
  }
@@ -1,5 +1,5 @@
1
1
  # frozen_string_literal: true
2
2
 
3
3
  module GPS_PVT
4
- VERSION = "0.1.7"
4
+ VERSION = "0.2.0"
5
5
  end
metadata CHANGED
@@ -1,14 +1,14 @@
1
1
  --- !ruby/object:Gem::Specification
2
2
  name: gps_pvt
3
3
  version: !ruby/object:Gem::Version
4
- version: 0.1.7
4
+ version: 0.2.0
5
5
  platform: ruby
6
6
  authors:
7
7
  - fenrir(M.Naruoka)
8
8
  autorequire:
9
9
  bindir: exe
10
10
  cert_chain: []
11
- date: 2022-01-14 00:00:00.000000000 Z
11
+ date: 2022-01-26 00:00:00.000000000 Z
12
12
  dependencies:
13
13
  - !ruby/object:Gem::Dependency
14
14
  name: rake
@@ -69,7 +69,10 @@ files:
69
69
  - ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h
70
70
  - ext/ninja-scan-light/tool/navigation/MagneticField.h
71
71
  - ext/ninja-scan-light/tool/navigation/NTCM.h
72
+ - ext/ninja-scan-light/tool/navigation/QZSS.h
72
73
  - ext/ninja-scan-light/tool/navigation/RINEX.h
74
+ - ext/ninja-scan-light/tool/navigation/SBAS.h
75
+ - ext/ninja-scan-light/tool/navigation/SBAS_Solver.h
73
76
  - ext/ninja-scan-light/tool/navigation/WGS84.h
74
77
  - ext/ninja-scan-light/tool/navigation/coordinate.h
75
78
  - ext/ninja-scan-light/tool/param/bit_array.h