gps_pvt 0.9.4 → 0.10.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.
@@ -0,0 +1,567 @@
1
+ require_relative 'upl/upl'
2
+ require_relative 'GPS'
3
+
4
+ require 'socket'
5
+ require 'openssl'
6
+
7
+ module GPS_PVT
8
+ class SUPL_Client
9
+ include UPL
10
+
11
+ attr_accessor :opts
12
+
13
+ def initialize(host, opts = {})
14
+ @host = host
15
+ @opts = {
16
+ :port => 7275,
17
+ :debug => 0,
18
+ :protocol => [:lpp, :rrlp],
19
+ }.merge(opts)
20
+ end
21
+
22
+ def get_assisted_data
23
+ begin
24
+ @socket = TCPSocket::new(@host, @opts[:port])
25
+ if @opts[:port] == 7275 then
26
+ @socket = OpenSSL::SSL::SSLSocket::new(@socket)
27
+ @socket.connect
28
+ end
29
+ send_supl_start
30
+ recv_supl_response
31
+ send_supl_pos_init
32
+ recv_supl_pos
33
+ ensure
34
+ @socket.close unless @socket.closed?
35
+ end
36
+ end
37
+
38
+ private
39
+
40
+ def send(cmd)
41
+ msg = encode(cmd)
42
+ (iter = proc{|keys, root1, root2, parents|
43
+ keys.each{|k|
44
+ if k.kind_of?(Hash) then
45
+ k.each{|k2, v2|
46
+ iter.call(v2, root1[k2], root2[k2], parents + [k2])
47
+ }
48
+ else
49
+ $stderr.puts [parents + [k], root1[k], root2[k]].inspect unless root1[k] == root2[k]
50
+ end
51
+ }
52
+ }).call(cmd.all_keys - [:length], cmd, decode(msg), []) if @opts[:debug] > 2
53
+ $stderr.puts [
54
+ msg.unpack("C*").collect{|byte| "%02X"%[byte]}.join(' '),
55
+ decode(msg)].inspect if @opts[:debug] > 1
56
+ $stderr.puts "tx(#{msg.size})" if @opts[:debug] > 0
57
+ @socket.write(msg)
58
+ end
59
+
60
+ def receive
61
+ raw = @socket.read(2)
62
+ raw += @socket.read(raw.unpack("n")[0] - 2)
63
+ res = decode(raw)
64
+ $stderr.puts "rx(#{raw.size})" if @opts[:debug] > 0
65
+ $stderr.puts [raw, res] if @opts[:debug] > 1
66
+ res
67
+ end
68
+
69
+ def send_supl_start
70
+ cmd = generate_skeleton(:SUPLSTART, {:version => 2})
71
+ cmd[:sessionID][:setSessionID] = {
72
+ :sessionId => 1,
73
+ :setId => proc{
74
+ # Ex) :msisdn => [0xFF, 0xFF, 0x91, 0x94, 0x48, 0x45, 0x83, 0x98],
75
+ next {:msisdn => @opts[:msisdn]} if @opts[:msisdn]
76
+ # Ex) :imsi => "440109012345678",
77
+ next {:imsi => @opts[:imsi].scan(/(.)(.?)/).collect{|a, b|
78
+ "0x#{a}#{b == '' ? '0' : b}".to_i(16)
79
+ }} if @opts[:imsi]
80
+ require 'resolv'
81
+ remote_ip = Resolv::DNS.new({:nameserver => "resolver1.opendns.com"}) \
82
+ .getresources("myip.opendns.com", Resolv::DNS::Resource::IN::A)[0].address
83
+ {:iPAddress => case remote_ip
84
+ when Resolv::IPv4; {:ipv4Address => remote_ip.address.unpack("C4")}
85
+ when Resolv::IPv6; {:ipv6Address => remote_ip.address.unpack("C16")}
86
+ else; raise
87
+ end}
88
+ }.call
89
+ }
90
+ proc{|cap|
91
+ cap[:posTechnology].keys.each{|k|
92
+ cap[:posTechnology][k] = [:agpsSETBased].include?(k)
93
+ }
94
+ cap[:posTechnology][:"ver2-PosTechnology-extension"] = {
95
+ :gANSSPositionMethods => [
96
+ {
97
+ :ganssId => 1, # SBAS
98
+ :ganssSBASid => [0, 1, 0], # MSAS
99
+ :gANSSPositioningMethodTypes => {:setAssisted => true, :setBased => true, :autonomous => false},
100
+ :gANSSSignals => [1] * 8,
101
+ },
102
+ {
103
+ :ganssId => 3, # QZSS
104
+ :gANSSPositioningMethodTypes => {:setAssisted => true, :setBased => true, :autonomous => false},
105
+ :gANSSSignals => [1] * 8,
106
+ },
107
+ {
108
+ :ganssId => 4, # GLONASS
109
+ :gANSSPositioningMethodTypes => {:setAssisted => true, :setBased => true, :autonomous => false},
110
+ :gANSSSignals => [1] * 8,
111
+ },
112
+ ],
113
+ }
114
+ cap[:prefMethod] = :agpsSETBasedPreferred
115
+ cap[:posProtocol].keys.each{|k|
116
+ cap[:posProtocol][k] = [:rrlp].include?(k)
117
+ }
118
+ cap[:posProtocol][:"ver2-PosProtocol-extension"] = {
119
+ :lpp => @opts[:protocol].include?(:lpp),
120
+ :posProtocolVersionRRLP => {
121
+ :majorVersionField => 17,
122
+ :technicalVersionField => 0,
123
+ :editorialVersionField => 0,
124
+ },
125
+ :posProtocolVersionLPP => {
126
+ :majorVersionField => 17,
127
+ :technicalVersionField => 5,
128
+ :editorialVersionField => 0,
129
+ },
130
+ }
131
+ @capability = cap
132
+ }.call(cmd[:message][:msSUPLSTART][:sETCapabilities])
133
+ proc{|loc|
134
+ loc[:cellInfo] = {
135
+ :gsmCell => {:refMCC => 244, :refMNC => 5, :refLAC => 23010, :refCI => 12720},
136
+ }
137
+ loc[:status] = :current
138
+ @location_id = loc
139
+ }.call(cmd[:message][:msSUPLSTART][:locationId])
140
+
141
+ send(cmd)
142
+ end
143
+
144
+
145
+ def recv_supl_response
146
+ data = receive
147
+ @session_id = data[:sessionID]
148
+ end
149
+
150
+ def send_supl_pos_init
151
+ cmd = generate_skeleton(:SUPLPOSINIT, {:version => 2})
152
+ cmd[:sessionID] = @session_id
153
+ proc{|posinit|
154
+ posinit[:sETCapabilities] = @capability
155
+ posinit[:requestedAssistData] = Hash[*([
156
+ :almanacRequested,
157
+ :utcModelRequested,
158
+ :ionosphericModelRequested,
159
+ :dgpsCorrectionsRequested,
160
+ :referenceLocationRequested,
161
+ :referenceTimeRequested,
162
+ :acquisitionAssistanceRequested,
163
+ :realTimeIntegrityRequested,
164
+ :navigationModelRequested,
165
+ ].collect{|k|
166
+ [k, [
167
+ :utcModelRequested,
168
+ :ionosphericModelRequested,
169
+ :referenceLocationRequested,
170
+ :referenceTimeRequested,
171
+ :acquisitionAssistanceRequested,
172
+ :realTimeIntegrityRequested,
173
+ :navigationModelRequested,
174
+ ].include?(k)]
175
+ }.flatten(1))]
176
+ posinit[:requestedAssistData][:"ver2-RequestedAssistData-extension"] = {
177
+ :ganssRequestedCommonAssistanceDataList => {
178
+ :ganssReferenceTime => true,
179
+ :ganssIonosphericModel => true,
180
+ :ganssAdditionalIonosphericModelForDataID00 => false,
181
+ :ganssAdditionalIonosphericModelForDataID11 => false,
182
+ :ganssEarthOrientationParameters => false,
183
+ :ganssAdditionalIonosphericModelForDataID01 => false,
184
+ },
185
+ :ganssRequestedGenericAssistanceDataList => [
186
+ # SBAS
187
+ {:ganssId => 1, :ganssSBASid => [0, 1, 0], # MSAS
188
+ :ganssRealTimeIntegrity => true, :ganssAlmanac => false,
189
+ :ganssNavigationModelData => {:ganssWeek => 0, :ganssToe => 0, :"t-toeLimit" => 0},
190
+ :ganssReferenceMeasurementInfo => false, :ganssUTCModel => true, :ganssAuxiliaryInformation => false},
191
+ # QZSS
192
+ {:ganssId => 3, :ganssRealTimeIntegrity => true, :ganssAlmanac => false,
193
+ :ganssNavigationModelData => {:ganssWeek => 0, :ganssToe => 0, :"t-toeLimit" => 0},
194
+ :ganssReferenceMeasurementInfo => false, :ganssUTCModel => true, :ganssAuxiliaryInformation => false},
195
+ # GLONASS
196
+ {:ganssId => 4, :ganssRealTimeIntegrity => true, :ganssAlmanac => false,
197
+ :ganssNavigationModelData => {:ganssWeek => 0, :ganssToe => 0, :"t-toeLimit" => 0},
198
+ :ganssReferenceMeasurementInfo => false, :ganssUTCModel => true, :ganssAuxiliaryInformation => false},
199
+ ],
200
+ }
201
+ posinit[:requestedAssistData][:navigationModelData] = {
202
+ :gpsWeek => 0,
203
+ :gpsToe => 0,
204
+ :nSAT => 0,
205
+ :toeLimit => 0,
206
+ } if false
207
+ posinit[:locationId] = @location_id
208
+ }.call(cmd[:message][:msSUPLPOSINIT])
209
+
210
+ send(cmd)
211
+ end
212
+
213
+ def recv_supl_pos
214
+ res = {}
215
+ merge = proc{|src, dst|
216
+ src.each{|k, v|
217
+ case dst[k]
218
+ when Hash; merge.call(v, dst[k])
219
+ when Array; dst[k] += v
220
+ when nil; dst[k] = v
221
+ end
222
+ }
223
+ }
224
+ data = receive
225
+ if data[:message][:msSUPLPOS][:posPayLoad][:"ver2-PosPayLoad-extension"] then
226
+ merge.call(
227
+ data[:message][:msSUPLPOS][:posPayLoad][:"ver2-PosPayLoad-extension"][:lPPPayload].decode[:"lpp-MessageBody"],
228
+ res)
229
+ attach_lpp(res)
230
+ else
231
+ while true
232
+ rrlp_data = data[:message][:msSUPLPOS][:posPayLoad][:rrlpPayload].decode
233
+ merge.call(
234
+ rrlp_data[:component][:assistanceData][:"gps-AssistData"][:controlHeader],
235
+ res)
236
+ break unless (rrlp_data[:component][:assistanceData][:moreAssDataToBeSent] == :moreMessagesOnTheWay)
237
+
238
+ # SUPL-POS + RRLP-assistanceDataAck
239
+ cmd = generate_skeleton(:SUPLPOS, {:version => 2})
240
+ cmd[:sessionID] = @session_id
241
+ cmd[:message][:msSUPLPOS] = {
242
+ :posPayLoad => {:rrlpPayload => {
243
+ :referenceNumber => rrlp_data[:referenceNumber],
244
+ :component => {:assistanceDataAck => nil}
245
+ }}
246
+ }
247
+ send(cmd)
248
+ data = receive
249
+ end
250
+ attach_rrlp(res)
251
+ end
252
+ res
253
+ end
254
+
255
+ def SUPL_Client.correct_gps_week(week, t_gps_ref, width = 1024)
256
+ width_half = width / 2
257
+ cycle, week_rem_ref = t_gps_ref.week.divmod(width)
258
+ week_rem = week % width
259
+ delta = week_rem_ref - week_rem
260
+ if delta > width_half then
261
+ cycle += 1
262
+ elsif delta < -width_half then
263
+ cycle -= 1
264
+ end
265
+ cycle * width + week_rem
266
+ end
267
+
268
+ EPH_KEY_TBL_RRLP = Hash[*({
269
+ :URA_index => :URA,
270
+ :dot_i0 => [:IDot, -43, true],
271
+ #:iode => nil,
272
+ :t_oc => [:Toc, 4],
273
+ :a_f2 => [:AF2, -55],
274
+ :a_f1 => [:AF1, -43],
275
+ :a_f0 => [:AF0, -31],
276
+ :iodc => :IODC,
277
+ :c_rs => [:Crs, -5],
278
+ :delta_n => [:DeltaN, -43, true],
279
+ :M0 => [:M0, -31, true],
280
+ :c_uc => [:Cuc, -29],
281
+ :e => [:E, -33],
282
+ :c_us => [:Cus, -29],
283
+ :sqrt_A => [:APowerHalf, -19],
284
+ :t_oe => [:Toe, 4],
285
+ :c_ic => [:Cic, -29],
286
+ :Omega0 => [:OmegaA0, -31, true],
287
+ :c_is => [:Cis, -29],
288
+ :i0 => [:I0, -31, true],
289
+ :c_rc => [:Crc, -5],
290
+ :omega => [:W, -31, true],
291
+ :dot_Omega0 => [:OmegaADot, -43, true],
292
+ :t_GD => [:Tgd, -31],
293
+ :SV_health => :SVhealth,
294
+ }.collect{|dst_k, (src_k, sf_pow2, sc2rad)|
295
+ sf_pow2 ||= 0
296
+ sf = sf_pow2 < 0 ? Rational(1, 1 << -sf_pow2) : (1 << sf_pow2)
297
+ sf = sf.to_f * GPS::GPS_SC2RAD if sc2rad
298
+ ["#{dst_k}=".to_sym, ["ephem#{src_k}".to_sym, sf]]
299
+ }.flatten(1))]
300
+ =begin
301
+ :ephemCodeOnL2
302
+ :ephemL2Pflag
303
+ :ephemAODA
304
+ =end
305
+
306
+ def attach_rrlp(msg)
307
+ t_gps = proc{
308
+ week_rem, sec008 = [:gpsWeek, :gpsTOW23b].collect{|k| msg[:referenceTime][:gpsTime][k]}
309
+ GPS::Time::new(
310
+ SUPL_Client::correct_gps_week(week_rem, GPS::Time::now),
311
+ Rational(sec008 * 8, 100).to_f)
312
+ }.call
313
+ msg.define_singleton_method(:ref_time){t_gps}
314
+ msg.define_singleton_method(:iono_utc){
315
+ params = GPS::Ionospheric_UTC_Parameters::new
316
+ iono = self[:ionosphericModel]
317
+ a, b = (0..3).collect{|i|
318
+ [iono["alfa#{i}".to_sym], iono["beta#{i}".to_sym]]
319
+ }.transpose
320
+ params.alpha = a.zip([-30, -27, -24, -24]).collect{|v, pow2|
321
+ (v * Rational(1, 1 << -pow2)).to_f
322
+ }
323
+ params.beta = b.zip([11, 14, 16, 16]).collect{|v, pow2|
324
+ v * (1 << pow2)
325
+ }
326
+ utc = self[:utcModel]
327
+ [:A1, :A0, [:t_ot, :Tot], [:WN_t, :WNt], [:delta_t_LS, :DeltaTls],
328
+ [:WN_LSF, :WNlsf], :DN, [:delta_t_LSF, :DeltaTlsf]].each{|k_dst, k_src|
329
+ params.send("#{k_dst}=".to_sym, utc["utc#{k_src || k_dst}".to_sym])
330
+ }
331
+ {:A1 => -50, :A0 => -30, :t_ot => 12}.each{|k, pow2|
332
+ params.send("#{k}=".to_sym,
333
+ params.send(k) * (pow2 < 0 ? Rational(1, 1 << -pow2) : (1 << pow2)))
334
+ }
335
+ params.WN_t = SUPL_Client::correct_gps_week(params.WN_t, t_gps, 256)
336
+ params.WN_LSF = SUPL_Client::correct_gps_week(params.WN_LSF, t_gps, 256)
337
+ params
338
+ }
339
+ msg.define_singleton_method(:ephemeris){
340
+ self[:navigationModel][:navModelList].collect{|v|
341
+ eph = GPS::Ephemeris::new
342
+ eph.svid = v[:satelliteID] + 1
343
+ eph_src = v[:satStatus][:newSatelliteAndModelUC]
344
+ EPH_KEY_TBL_RRLP.each{|dst_k, (src_k, sf)|
345
+ v = sf * eph_src[src_k]
346
+ eph.send(dst_k, v.kind_of?(Rational) ? v.to_f : v)
347
+ }
348
+ eph.WN = t_gps.week
349
+ delta_sec = t_gps.seconds - eph.t_oe
350
+ if delta_sec > GPS::Time::Seconds_week / 2 then
351
+ eph.WN += 1
352
+ elsif delta_sec < -GPS::Time::Seconds_week / 2 then
353
+ eph.WN -= 1
354
+ end
355
+ eph.iode = eph.iodc & 0xFF
356
+ eph.fit_interval = [eph_src[:ephemFitFlag], eph.iodc]
357
+ eph
358
+ }
359
+ }
360
+ msg
361
+ end
362
+
363
+ EPH_KEY_TBL_LPP = Hash[*({
364
+ :URA_index => :URA,
365
+ :dot_i0 => [:IDot, -43, true],
366
+ #:iode => nil,
367
+ :t_oc => [:Toc, 4],
368
+ :a_f2 => [:af2, -55],
369
+ :a_f1 => [:af1, -43],
370
+ :a_f0 => [:af0, -31],
371
+ #:iodc => :IODC,
372
+ :c_rs => [:Crs, -5],
373
+ :delta_n => [:DeltaN, -43, true],
374
+ :M0 => [:M0, -31, true],
375
+ :c_uc => [:Cuc, -29],
376
+ :e => [:E, -33],
377
+ :c_us => [:Cus, -29],
378
+ :sqrt_A => [:APowerHalf, -19],
379
+ :t_oe => [:Toe, 4],
380
+ :c_ic => [:Cic, -29],
381
+ :Omega0 => [:OmegaA0, -31, true],
382
+ :c_is => [:Cis, -29],
383
+ :i0 => [:I0, -31, true],
384
+ :c_rc => [:Crc, -5],
385
+ :omega => [:Omega, -31, true],
386
+ :dot_Omega0 => [:OmegaADot, -43, true],
387
+ :t_GD => [:Tgd, -31],
388
+ }.collect{|dst_k, (src_k, sf_pow2, sc2rad)|
389
+ sf_pow2 ||= 0
390
+ sf = sf_pow2 < 0 ? Rational(1, 1 << -sf_pow2) : (1 << sf_pow2)
391
+ sf = sf.to_f * GPS::GPS_SC2RAD if sc2rad
392
+ ["#{dst_k}=".to_sym, ["nav#{src_k}".to_sym, sf]]
393
+ }.flatten(1))]
394
+ =begin
395
+ :ephemCodeOnL2
396
+ :ephemL2Pflag
397
+ :ephemAODA
398
+ =end
399
+
400
+ EPH_KEY_TBL_LPP_GLO = Hash[*({
401
+ :xn => [:X, Rational(1000, 1 << 11)], :xn_dot => [:Xdot, Rational(1000, 1 << 20)], :xn_ddot => [:Xdotdot, Rational(1000, 1 << 30)],
402
+ :yn => [:Y, Rational(1000, 1 << 11)], :yn_dot => [:Ydot, Rational(1000, 1 << 20)], :yn_ddot => [:Ydotdot, Rational(1000, 1 << 30)],
403
+ :zn => [:Z, Rational(1000, 1 << 11)], :zn_dot => [:Zdot, Rational(1000, 1 << 20)], :zn_ddot => [:Zdotdot, Rational(1000, 1 << 30)],
404
+ :tau_n => [:Tau, Rational(1, 1 << 30)],
405
+ :gamma_n => [:Gamma, Rational(1, 1 << 40)],
406
+ :M => :M, :P1 => [:P1, proc{|v| [0, 30, 45, 60][v] * 60}], :P2 => :P2, :E_n => :En,
407
+ }.collect{|dst_k, (src_k, sf)|
408
+ ["#{dst_k}=".to_sym, ["glo#{src_k}".to_sym, sf || 1]]
409
+ }.flatten(1))]
410
+ =begin
411
+ :has_string=, :raw=, :t_k=,
412
+ :N_T=, :p=, :delta_tau_n=, :P4=,
413
+ :tau_GPS=, :tau_c=, :day_of_year=, :year=, :n=, :freq_ch=
414
+ =end
415
+
416
+ def attach_lpp(msg)
417
+ t_gps = proc{|data|
418
+ raise unless data[:"gnss-TimeID"][:"gnss-id"] == :gps
419
+ dayn, tod, msec = [:"gnss-DayNumber", :"gnss-TimeOfDay", :"gnss-TimeOfDayFrac-msec"].collect{|k|
420
+ data[k]
421
+ }
422
+ week, dow = dayn.divmod(7)
423
+ GPS::Time::new(week, dow * 24 * 3600 + tod + Rational(msec, 1000).to_f)
424
+ }.call(msg[:c1][:provideAssistanceData][:criticalExtensions][:c1] \
425
+ [:"provideAssistanceData-r9"][:"a-gnss-ProvideAssistanceData"] \
426
+ [:"gnss-CommonAssistData"][:"gnss-ReferenceTime"][:"gnss-SystemTime"])
427
+ msg.define_singleton_method(:ref_time){t_gps}
428
+ iono_utc = proc{
429
+ params = GPS::Ionospheric_UTC_Parameters::new
430
+ proc{|data|
431
+ a, b = (0..3).collect{|i|
432
+ [data["alfa#{i}".to_sym], data["beta#{i}".to_sym]]
433
+ }.transpose
434
+ params.alpha = a.zip([-30, -27, -24, -24]).collect{|v, pow2|
435
+ (v * Rational(1, 1 << -pow2)).to_f
436
+ }
437
+ params.beta = b.zip([11, 14, 16, 16]).collect{|v, pow2|
438
+ v * (1 << pow2)
439
+ }
440
+ }.call(msg[:c1][:provideAssistanceData][:criticalExtensions][:c1] \
441
+ [:"provideAssistanceData-r9"][:"a-gnss-ProvideAssistanceData"] \
442
+ [:"gnss-CommonAssistData"][:"gnss-IonosphericModel"][:klobucharModel])
443
+ proc{|data|
444
+ [[:A1], [:A0], [:t_ot, :Tot],
445
+ [:WN_t, :WNt], [:delta_t_LS, :DeltaTlsf],
446
+ [:WN_LSF, :WNlsf], [:DN], [:delta_t_LSF, :DeltaTlsf]].each{|k_dst, k_src|
447
+ params.send("#{k_dst}=".to_sym, data["gnss-Utc-#{k_src || k_dst}".to_sym])
448
+ }
449
+ }.call(msg[:c1][:provideAssistanceData][:criticalExtensions][:c1] \
450
+ [:"provideAssistanceData-r9"][:"a-gnss-ProvideAssistanceData"] \
451
+ [:"gnss-GenericAssistData"].select{|v| v[:"gnss-ID"][:"gnss-id"] == :gps}[0] \
452
+ [:"gnss-UTC-Model"][:utcModel1])
453
+ {:A1 => -50, :A0 => -30, :t_ot => 12}.each{|k, pow2|
454
+ params.send("#{k}=".to_sym,
455
+ params.send(k) * (pow2 < 0 ? Rational(1, 1 << -pow2) : (1 << pow2)))
456
+ }
457
+ params.WN_t = SUPL_Client::correct_gps_week(params.WN_t, t_gps, 256)
458
+ params.WN_LSF = SUPL_Client::correct_gps_week(params.WN_LSF, t_gps, 256)
459
+ params
460
+ }.call
461
+ leap_seconds = iono_utc.delta_t_LS rescue t_gps.leap_seconds
462
+ msg.define_singleton_method(:iono_utc){iono_utc}
463
+ extract_gps_ephemeris = proc{|sat_list, sys|
464
+ offset = {:gps => 1, :qzss => 193}[sys]
465
+ sat_list.collect{|v|
466
+ eph = GPS::Ephemeris::new
467
+ eph.svid = v[:svID][:"satellite-id"] + offset
468
+ eph_src = v[:"gnss-ClockModel"][:"nav-ClockModel"].merge(v[:"gnss-OrbitModel"][:"nav-KeplerianSet"])
469
+ EPH_KEY_TBL_LPP.each{|dst_k, (src_k, sf)|
470
+ v2 = sf * eph_src[src_k]
471
+ eph.send(dst_k, v2.kind_of?(Rational) ? v2.to_f : v2)
472
+ }
473
+ eph.iodc = Integer(v[:iod].join, 2)
474
+ eph.iode = (eph.iodc & 0xFF)
475
+ eph.SV_health = Integer(v[:svHealth][0..5].join, 2)
476
+ eph.WN = t_gps.week
477
+ delta_sec = t_gps.seconds - eph.t_oe
478
+ if delta_sec > GPS::Time::Seconds_week / 2 then
479
+ eph.WN += 1
480
+ elsif delta_sec < -GPS::Time::Seconds_week / 2 then
481
+ eph.WN -= 1
482
+ end
483
+ eph.fit_interval = [eph_src[:navFitFlag], eph.iodc, sys]
484
+ eph
485
+ }
486
+ }
487
+ msg.define_singleton_method(:ephemeris){
488
+ assist_data = self[:c1][:provideAssistanceData][:criticalExtensions][:c1] \
489
+ [:"provideAssistanceData-r9"][:"a-gnss-ProvideAssistanceData"] \
490
+ [:"gnss-GenericAssistData"]
491
+ res = [:gps, :qzss].collect{|k|
492
+ extract_gps_ephemeris.call(
493
+ assist_data.select{|v| v[:"gnss-ID"][:"gnss-id"] == k}[0] \
494
+ [:"gnss-NavigationModel"][:"gnss-SatelliteList"], k)
495
+ }.flatten(1)
496
+ assist_data_glo = assist_data.select{|v| v[:"gnss-ID"][:"gnss-id"] == :glonass}[0]
497
+ utc_params_glo = {
498
+ :tau_c= => [:tauC, Rational(1, 1 << 31)],
499
+ }.collect{|dst_k, (src_k, sf)|
500
+ [dst_k, sf * assist_data_glo[:"gnss-UTC-Model"][:utcModel3][src_k]]
501
+ }
502
+ res += assist_data_glo[:"gnss-NavigationModel"][:"gnss-SatelliteList"].collect{|sat|
503
+ eph = GPS::Ephemeris_GLONASS::new
504
+ eph.svid = sat[:svID][:"satellite-id"] + 1
505
+ eph_src = sat[:"gnss-ClockModel"][:"glonass-ClockModel"].merge(
506
+ sat[:"gnss-OrbitModel"][:"glonass-ECEF"])
507
+ (EPH_KEY_TBL_LPP_GLO.collect{|dst_k, (src_k, sf)|
508
+ v = eph_src[src_k]
509
+ [dst_k, sf.send(sf.kind_of?(Proc) ? :call : :*, case v
510
+ when Array; Integer(v.join, 2)
511
+ when true; 1
512
+ when false; 0
513
+ else; v
514
+ end)]
515
+ } + utc_params_glo).each{|dst_k, v|
516
+ eph.send(dst_k, v.kind_of?(Rational) ? v.to_f : v)
517
+ }
518
+ eph.B_n = sat[:svHealth][0]
519
+ eph.F_T_index = Integer(sat[:svHealth][1..4].join, 2)
520
+ eph.t_b = Integer(sat[:iod][4..-1].join, 2) * 15 * 60
521
+ eph.set_date((t_gps + 3 * 60 * 60).c_tm(leap_seconds)) # UTC -> Moscow time
522
+ eph.N_T = eph.NA
523
+ eph.rehash(leap_seconds)
524
+ eph
525
+ }
526
+ res
527
+ }
528
+ msg
529
+ end
530
+ end
531
+ end
532
+
533
+ require 'open-uri'
534
+
535
+ OpenURI.class_eval{
536
+ def OpenURI.open_supl(buf, target, proxy, options) # :nodoc:
537
+ options[:port] = target.port
538
+ URI.decode_www_form(target.query || "").each{|k, v|
539
+ case k = k.to_sym
540
+ when :protocol
541
+ (options[k] ||= []) << v.to_sym
542
+ end
543
+ }
544
+ buf.instance_eval{
545
+ @io = GPS_PVT::SUPL_Client::new(target.host, options)
546
+ @io.define_singleton_method(:read){
547
+ require 'json'
548
+ JSON::pretty_generate(self.get_assisted_data)
549
+ }
550
+ @io.define_singleton_method(:closed?){true} # For open-uri auto close
551
+ }
552
+ end
553
+ }
554
+ module URI
555
+ class Supl < Generic
556
+ DEFAULT_PORT = 7275
557
+ def buffer_open(buf, proxy, options)
558
+ OpenURI.open_supl(buf, self, proxy, options)
559
+ end
560
+ include OpenURI::OpenRead
561
+ end
562
+ if respond_to?(:register_scheme) then
563
+ register_scheme('SUPL', Supl)
564
+ else
565
+ @@schemes['SUPL'] = Supl
566
+ end
567
+ end
data/lib/gps_pvt/ubx.rb CHANGED
@@ -144,5 +144,20 @@ class UBX
144
144
  nil
145
145
  end
146
146
  end
147
+
148
+ def UBX.gnss_svid(svid_legacy)
149
+ case svid_legacy
150
+ when 1..32; [:GPS, svid_legacy]
151
+ when 120..158; [:SBAS, svid_legacy]
152
+ when 211..246; [:Galileo, svid_legacy - 210]
153
+ when 159..163; [:Beido, svid_legacy - 158]
154
+ when 33..64; [:Beido, svid_legacy - 27]
155
+ #when 173..182 # IMES
156
+ when 193..197; [:QZSS, svid_legacy - 192]
157
+ when 65..96; [:GLONASS, svid_legacy - 64]
158
+ when 255; [:GLONASS, nil]
159
+ else; nil
160
+ end
161
+ end
147
162
  end
148
163
  end