gps_pvt 0.9.1 → 0.9.3

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.
@@ -53,7 +53,7 @@ shared_examples 'Matrix' do
53
53
  [:row_size, :column_size].each{|f|
54
54
  a = a_gen.call
55
55
  a.define_singleton_method(f){-1}
56
- expect{ mat_type::new(a) }.to raise_error(RuntimeError)
56
+ expect{ mat_type::new(a) }.to raise_error(ArgumentError)
57
57
  }
58
58
  end
59
59
  it 'is invoked with I, identity, unit' do
@@ -70,12 +70,12 @@ shared_examples 'Matrix' do
70
70
  end
71
71
  it 'sets its elements with [], [[]], Matrix' do
72
72
  expect( mat_type::new(params[:rc][0], params[:rc][1], compare_with.flatten).to_a ).to eq(compare_with)
73
- expect{ mat_type::new(params[:rc][0], params[:rc][1], compare_with.flatten[0..-2]) }.to raise_error(RuntimeError)
73
+ expect{ mat_type::new(params[:rc][0], params[:rc][1], compare_with.flatten[0..-2]) }.to raise_error(ArgumentError)
74
74
  expect( mat_type::new(params[:rc][0], params[:rc][1], compare_with.flatten + [gen_elm.call]).to_a ).to eq(compare_with)
75
- expect{ mat_type::new(params[:rc][0], params[:rc][1], compare_with.flatten[0..-2] + [nil]) }.to raise_error(RuntimeError)
75
+ expect{ mat_type::new(params[:rc][0], params[:rc][1], compare_with.flatten[0..-2] + [nil]) }.to raise_error(ArgumentError)
76
76
 
77
77
  expect( mat_type::new(params[:rc][0], params[:rc][1], compare_with).to_a ).to eq(compare_with)
78
- expect{ mat_type::new(params[:rc][0], params[:rc][1], compare_with[0..-2]) }.to raise_error(RuntimeError)
78
+ expect{ mat_type::new(params[:rc][0], params[:rc][1], compare_with[0..-2]) }.to raise_error(ArgumentError)
79
79
  expect( mat_type::new(params[:rc][0], params[:rc][1], compare_with + [params[:rc][1].times.map{gen_elm.call}]).to_a ).to eq(compare_with)
80
80
  expect( mat_type::new(compare_with).to_a ).to eq(compare_with)
81
81
 
@@ -83,7 +83,7 @@ shared_examples 'Matrix' do
83
83
  end
84
84
  it 'sets its elements with {}' do
85
85
  expect( mat_type::new(*params[:rc]){|i, j| compare_with[i][j]}.to_a ).to eq(compare_with)
86
- expect{ mat_type::new(*params[:rc]){nil}.to_a }.to raise_error(RuntimeError)
86
+ expect{ mat_type::new(*params[:rc]){nil}.to_a }.to raise_error(ArgumentError)
87
87
  expect{ mat_type::new(compare_with){|i, j| compare_with[i][j]} }.to raise_error(ArgumentError)
88
88
  expect{ mat_type::new(Matrix[*compare_with]){|i, j| compare_with[i][j]} }.to raise_error(ArgumentError)
89
89
 
@@ -372,7 +372,7 @@ shared_examples 'Matrix' do
372
372
  end
373
373
  it 'have +(scalar)' do
374
374
  expect((mat[0] + 1).to_a).to eq((Matrix[*compare_with[0]] + Matrix::unit(params[:rc][0])).to_a)
375
- expect{mat[2] + 1}.to raise_error(RuntimeError)
375
+ expect{mat[2] + 1}.to raise_error(ArgumentError)
376
376
  end
377
377
  it 'have +(mat)' do
378
378
  [[0, 1], [2, 3]].each{|i, j|
@@ -381,7 +381,7 @@ shared_examples 'Matrix' do
381
381
  end
382
382
  it 'have -(scalar)' do
383
383
  expect((mat[0] - 1).to_a).to eq((Matrix[*compare_with[0]] - Matrix::unit(params[:rc][0])).to_a)
384
- expect{mat[2] - 1}.to raise_error(RuntimeError)
384
+ expect{mat[2] - 1}.to raise_error(ArgumentError)
385
385
  end
386
386
  it 'have -(mat)' do
387
387
  [[0, 1], [2, 3]].each{|i, j|
@@ -395,7 +395,7 @@ shared_examples 'Matrix' do
395
395
  end
396
396
  it 'have *(mat)' do
397
397
  expect((mat[0] * mat[1]).to_a).to eq((Matrix[*compare_with[0]] * Matrix[*compare_with[1]]).to_a)
398
- expect{mat[2] * mat[3]}.to raise_error(RuntimeError)
398
+ expect{mat[2] * mat[3]}.to raise_error(ArgumentError)
399
399
  expect((mat[2] * mat[3].t).to_a).to eq((Matrix[*compare_with[2]] * Matrix[*compare_with[3]].t).to_a)
400
400
  end
401
401
  it 'have /(scalar)' do
@@ -18,14 +18,168 @@ class Receiver
18
18
  else; raise "reference time (#{ref_time}) should be GPS::Time or Time"
19
19
  end
20
20
  leap_sec = ref_time.leap_seconds
21
+ ref_pos = opt[:ref_pos] || if src_io.respond_to?(:property) then
22
+ Coordinate::LLH::new(*(src_io.property.values_at(:latitude, :longitude).collect{|v|
23
+ v.to_f / 180 * Math::PI
24
+ } + [0])).xyz
25
+ else
26
+ defined?(@base_station) ? @base_station : nil
27
+ end
21
28
  after_run = b || proc{|pvt| puts pvt.to_s if pvt}
22
- t_meas, meas = [nil, GPS::Measurement::new]
29
+ t_meas, meas = [nil, {}]
30
+ # meas := {msg_num => [[], ...]} due to duplicated observation such as 1074 and 1077
31
+ run_proc = proc{
32
+ meas_ = GPS::Measurement::new
33
+ meas.sort.each{|k, values| # larger msg_num entries have higher priority
34
+ values.each{|prn_k_v| meas_.add(*prn_k_v)}
35
+ }
36
+ pvt = nil
37
+ after_run.call(pvt = run(meas_, t_meas), [meas_, ref_time = t_meas]) if t_meas
38
+ ref_pos = pvt.xyz if pvt && pvt.position_solved?
39
+ t_meas, meas = [nil, {}]
40
+ }
23
41
  dt_threshold = GPS::Time::Seconds_week / 2
42
+ tow2t = proc{|tow_sec|
43
+ dt = tow_sec - ref_time.seconds
44
+ GPS::Time::new(ref_time.week + if dt <= -dt_threshold then; 1
45
+ elsif dt >= dt_threshold then; -1
46
+ else; 0; end, tow_sec)
47
+ }
48
+ utc2t = proc{|utc_tod|
49
+ if t_meas then
50
+ delta = (t_meas.seconds - utc_tod).to_i % (60 * 60 * 24)
51
+ leap_sec = (delta >= (60 * 60 * 12)) ? delta - (60 * 60 * 12) : delta
52
+ t_meas
53
+ else
54
+ ref_dow, ref_tod = ref_time.seconds.divmod(60 * 60 * 24)
55
+ tod = utc_tod + leap_sec
56
+ tod_delta = ref_tod - tod
57
+ if tod_delta > 12 * 60 * 60 then
58
+ ref_dow -= 1
59
+ elsif tod_delta < -12 * 60 * 60 then
60
+ ref_dow += 1
61
+ end
62
+ GPS::Time::new(ref_time.week, 0) + tod + 60 * 60 * 24 * ref_dow
63
+ end
64
+ }
65
+ restore_ranges = proc{
66
+ c_1ms = 299_792.458
67
+ threshold = c_1ms / 10 # 100 us =~ 30 km
68
+ cache = {} # {[sys, svid] => [range, t], ...}
69
+ get_rough = proc{|t, sys_svid_list|
70
+ sn_list = sys_svid_list.collect{|sys, svid|
71
+ case sys
72
+ when :GPS, :QZSS; @solver.gps_space_node
73
+ when :SBAS; @solver.sbas_space_node
74
+ when :GLONASS; @solver.glonass_space_node
75
+ end
76
+ }
77
+ critical{
78
+ sn_list.uniq.compact{|sn| sn.update_all_ephemeris(t)}
79
+ sys_svid_list.zip(sn_list).each{|(sys, svid), sn|
80
+ next unless sn
81
+ eph = sn.ephemeris(svid)
82
+ cache[[sys, svid]] = [if eph.valid?(t) then
83
+ sv_pos, clk_err = eph.constellation(t).values_at(0, 2)
84
+ sv_pos.dist(ref_pos) - (clk_err * c_1ms * 1E3)
85
+ end, t]
86
+ }
87
+ }
88
+ }
89
+ per_kind = proc{|t, sys_svid_list, ranges_rem|
90
+ get_rough.call(t, sys_svid_list.uniq.reject{|sys, svid|
91
+ next true unless sys
92
+ range, t2 = cache[[sys, svid]]
93
+ range && ((t2 - t).abs <= 60)
94
+ })
95
+ ranges_rem.zip(sys_svid_list).collect{|rem_in, (sys, svid)|
96
+ range_ref, t2 = cache[[sys, svid]]
97
+ next nil unless range_ref
98
+ q, rem_ref = range_ref.divmod(c_1ms)
99
+ delta = rem_in - rem_ref
100
+ res = if delta.abs <= threshold then
101
+ q * c_1ms + rem_in
102
+ elsif -delta + c_1ms <= threshold
103
+ (q - 1) * c_1ms + rem_in
104
+ elsif delta + c_1ms <= threshold
105
+ (q + 1) * c_1ms + rem_in
106
+ end
107
+ #p [sys, svid, q, rem_in, rem_ref, res]
108
+ (cache[[sys, svid]] = [res, t])[0]
109
+ }
110
+ }
111
+ proc{|t, sys_svid_list, ranges|
112
+ [
113
+ :pseudo_range, # for MT 1001/3/9/11, MSM1/3
114
+ :phase_range, # for MT 1003/11, MSM2/3
115
+ ].each{|k|
116
+ next if ranges[k]
117
+ k_rem = "#{k}_rem".to_sym
118
+ ranges[k] = per_kind.call(t, sys_svid_list, ranges[k_rem]) if ranges[k_rem]
119
+ }
120
+ }
121
+ }.call
24
122
 
25
123
  while packet = rtcm3.read_packet
26
124
  msg_num = packet.message_number
27
125
  parsed = packet.parse
126
+ t_meas2, meas2 = [nil, []] # per_packet
127
+ add_proc = proc{
128
+ t_meas ||= t_meas2
129
+ meas[msg_num] = meas2 unless meas2.empty?
130
+ }
28
131
  case msg_num
132
+ when 1001..1004
133
+ t_meas2 = tow2t.call(parsed[2][0]) # DF004
134
+ ranges = parsed.ranges
135
+ sys_svid_list = ranges[:sat].collect{|svid|
136
+ case svid
137
+ when 1..32; [:GPS, svid]
138
+ when 40..58; [:SBAS, svid + 80]
139
+ else; [nil, svid]
140
+ end
141
+ }
142
+ restore_ranges.call(t_meas2, sys_svid_list, ranges)
143
+ item_size = sys_svid_list.size
144
+ ([sys_svid_list] + [:pseudo_range, :phase_range, :cn].collect{|k|
145
+ ranges[k] || ([nil] * item_size)
146
+ }).transpose.each{|(sys, svid), pr, cpr, cn|
147
+ next unless sys
148
+ meas2 << [svid, :L1_PSEUDORANGE, pr] if pr
149
+ meas2 << [svid, :L1_CARRIER_PHASE, cpr / GPS::SpaceNode.L1_WaveLength] if cpr
150
+ meas2 << [svid, :L1_SIGNAL_STRENGTH_dBHz, cn] if cn
151
+ }
152
+ when 1009..1012
153
+ t_meas2 = utc2t.call(parsed[2][0] - 60 * 60 * 3) # DF034 UTC(SU)+3hr, time of day[sec]
154
+ ranges = parsed.ranges
155
+ sys_svid_list = ranges[:sat].collect{|svid|
156
+ case svid
157
+ when 1..24; [:GLONASS, svid]
158
+ when 40..58; [:SBAS, svid + 80]
159
+ else; [nil, svid]
160
+ end
161
+ }
162
+ restore_ranges.call(t_meas2, sys_svid_list, ranges)
163
+ item_size = sys_svid_list.size
164
+ ([sys_svid_list] + [:freq_ch, :pseudo_range, :phase_range, :cn].collect{|k|
165
+ ranges[k] || ([nil] * item_size)
166
+ }).transpose.each{|(sys, svid), freq_ch, pr, cpr, cn|
167
+ case sys
168
+ when :GLONASS
169
+ svid += 0x100
170
+ freq = GPS::SpaceNode_GLONASS::L1_frequency(freq_ch)
171
+ len = GPS::SpaceNode_GLONASS.light_speed / freq
172
+ meas2 << [svid, :L1_FREQUENCY, freq]
173
+ meas2 << [svid, :L1_CARRIER_PHASE, cpr / len] if cpr
174
+ when :SBAS
175
+ meas2 << [svid, :L1_CARRIER_PHASE, cpr / GPS::SpaceNode.L1_WaveLength] if cpr
176
+ else; next
177
+ end
178
+ meas2 << [svid, :L1_PSEUDORANGE, pr] if pr
179
+ meas2 << [svid, :L1_SIGNAL_STRENGTH_dBHz, cn] if cn
180
+ }
181
+ when 1013
182
+ leap_sec = parsed[5][0]
29
183
  when 1019, 1044
30
184
  params = parsed.params
31
185
  if msg_num == 1044
@@ -55,55 +209,79 @@ class Receiver
55
209
  critical{
56
210
  @solver.glonass_space_node.register_ephemeris(eph.svid, eph)
57
211
  }
58
- when 1077, 1087, 1097, 1117
212
+ when 1043
213
+ params = parsed.params
214
+ eph = GPS::Ephemeris_SBAS::new
215
+ tod_delta = params[:tod] - (ref_time.seconds % (24 * 60 * 60))
216
+ if tod_delta > (12 * 60 * 60) then
217
+ tod_delta -= (24 * 60 * 60)
218
+ elsif tod_delta < -(12 * 60 * 60) then
219
+ tod_delta += (24 * 60 * 60)
220
+ end
221
+ toe = ref_time + tod_delta
222
+ eph.WN, eph.t_0 = [:week, :seconds].collect{|k| toe.send(k)}
223
+ params.each{|k, v| eph.send("#{k}=".to_sym, v) unless [:iodn, :tod].include?(k)}
224
+ critical{
225
+ @solver.sbas_space_node.register_ephemeris(eph.svid, eph)
226
+ }
227
+ when 1071..1077, 1081..1087, 1101..1107, 1111..1117
59
228
  ranges = parsed.ranges
60
- sig_list, svid_offset = case msg_num
61
- when 1077 # GPS
62
- t_meas ||= proc{ # update time of measurement
63
- t_meas_sec = parsed[2][0] # DF004
64
- dt = t_meas_sec - ref_time.seconds
65
- GPS::Time::new(ref_time.week + if dt <= -dt_threshold then; 1
66
- elsif dt >= dt_threshold then; -1
67
- else; 0; end, t_meas_sec)
68
- }.call
229
+ glonass_freq = nil
230
+ case msg_num / 10 # check time of measurement
231
+ when 107, 110, 111 # GPS, SBAS, QZSS
232
+ t_meas2 = tow2t.call(parsed[2][0]) # DF004
233
+ when 108 # GLONASS
234
+ t_meas2 = utc2t.call(parsed[3][0] - 60 * 60 * 3) # DF034 UTC(SU)+3hr, time of day[sec]
235
+ glonass_freq = critical{
236
+ @solver.glonass_space_node.update_all_ephemeris(t_meas2)
237
+ Hash[*(ranges[:sat_sig].collect{|svid, sig| svid}.uniq.collect{|svid|
238
+ eph = @solver.glonass_space_node.ephemeris(svid)
239
+ next nil unless eph.in_range?(t_meas2)
240
+ [svid, {:L1 => eph.frequency_L1}]
241
+ }.compact.flatten(1))]
242
+ }
243
+ end
244
+ sig_list, sys, svid_offset = case msg_num / 10
245
+ when 107 # GPS
69
246
  [{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
70
247
  15 => [:L2CM, GPS::SpaceNode.L2_WaveLength],
71
- 16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, 0]
72
- when 1087 # GLONASS
73
- proc{
74
- utc = parsed[3][0] - 60 * 60 * 3 # DF034 UTC(SU)+3hr
75
- delta = (t_meas.seconds - utc).to_i % (60 * 60 * 24)
76
- leap_sec = (delta >= (60 * 60 * 12)) ? delta - (60 * 60 * 12) : delta
77
- }.call if t_meas
78
- [{2 => [:L1, GPS::SpaceNode_GLONASS.light_speed / GPS::SpaceNode_GLONASS.L1_frequency_base]}, 0x100]
79
- when 1117 # QZSS
248
+ 16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, :GPS, 0]
249
+ when 108 # GLONASS
250
+ [{2 => [:L1, nil]}, :GLONASS, 0x100]
251
+ when 110 # SBAS
252
+ [{2 => [:L1, GPS::SpaceNode.L1_WaveLength]}, :SBAS, 120]
253
+ when 111 # QZSS
80
254
  [{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
81
255
  15 => [:L2CM, GPS::SpaceNode.L2_WaveLength],
82
- 16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, 192]
83
- else; [{}, 0]
256
+ 16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, :QZSS, 192]
257
+ else; [{}, nil, 0]
84
258
  end
85
- [:sat_sig, :pseudo_range, :phase_range, :phase_range_rate].collect{|k|
86
- ranges[k]
87
- }.transpose.each{|(svid, sig), pr, cpr, dr|
259
+ sys_svid_list = ranges[:sat_sig].collect{|sat, sig| [sys, (sat + svid_offset) & 0xFF]}
260
+ restore_ranges.call(t_meas2, sys_svid_list, ranges)
261
+ item_size = sys_svid_list.size
262
+ [:sat_sig, :pseudo_range, :phase_range, :phase_range_rate, :cn].collect{|k|
263
+ ranges[k] || ([nil] * item_size)
264
+ }.transpose.each{|(svid, sig), pr, cpr, dr, cn|
88
265
  prefix, len = sig_list[sig]
89
266
  next unless prefix
90
- if svid_offset > 0 then
91
- @solver.glonass_space_node.update_all_ephemeris(t_meas)
92
- eph = @solver.glonass_space_node.ephemeris(svid)
93
- end
267
+ proc{
268
+ next unless freq = (glonass_freq[svid] || {})[prefix]
269
+ meas2 << [svid, "#{prefix}_FREQUENCY".to_sym, freq]
270
+ len = GPS::SpaceNode_GLONASS.light_speed / freq
271
+ }.call if glonass_freq
94
272
  svid += svid_offset
95
- meas.add(svid, "#{prefix}_PSEUDORANGE".to_sym, pr) if pr
96
- meas.add(svid, "#{prefix}_RANGE_RATE".to_sym, dr) if dr
97
- meas.add(svid, "#{prefix}_CARRIER_PHASE".to_sym, cpr / len) if cpr
273
+ meas2 << [svid, "#{prefix}_PSEUDORANGE".to_sym, pr] if pr
274
+ meas2 << [svid, "#{prefix}_RANGE_RATE".to_sym, dr] if dr
275
+ meas2 << [svid, "#{prefix}_CARRIER_PHASE".to_sym, cpr / len] if cpr && len
276
+ meas2 << [svid, "#{prefix}_SIGNAL_STRENGTH_dBHz".to_sym, cn] if cn
98
277
  }
99
278
  else
100
279
  #p({msg_num => parsed})
101
280
  end
102
- if (1070..1229).include?(msg_num) &&
103
- (!parsed.more_data? rescue (packet.decode([1], 24 + 54)[0] == 0)) then
104
- after_run.call(run(meas, t_meas), [meas, ref_time = t_meas]) if t_meas
105
- t_meas, meas = [nil, GPS::Measurement::new]
106
- end
281
+
282
+ run_proc.call if t_meas && t_meas2 && ((t_meas - t_meas2).abs > 1E-3) # fallback for incorrect more_data flag
283
+ add_proc.call
284
+ run_proc.call if (1070..1229).include?(msg_num) && (!parsed.more_data?)
107
285
  end
108
286
  end
109
287
  end
@@ -416,6 +416,7 @@ class Receiver
416
416
  opt = options[0] || {}
417
417
  case sys
418
418
  when :GPS, :QZSS
419
+ return unless bcast_data.size == 10 # 8 for QZSS(SAIF)
419
420
  return unless eph = @eph_list[prn]
420
421
  sn = @solver.gps_space_node
421
422
  subframe, iodc_or_iode = eph.parse(bcast_data)