gps_pvt 0.9.3 → 0.10.0
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/CHANGELOG.md +159 -3
- data/README.md +4 -3
- data/Rakefile +24 -0
- data/exe/gps2ubx +12 -5
- data/exe/gps_pvt +7 -2
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +53 -19
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +39 -7
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +5210 -2058
- data/ext/gps_pvt/extconf.rb +6 -5
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +1 -1
- data/ext/ninja-scan-light/tool/navigation/GPS.h +6 -2
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +1 -1
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +3 -1
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_MultiFrequency.h +3 -0
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +9 -9
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +1 -1
- data/ext/ninja-scan-light/tool/navigation/coordinate.h +13 -6
- data/ext/ninja-scan-light/tool/param/matrix.h +1020 -247
- data/ext/ninja-scan-light/tool/param/matrix_fixed.h +26 -0
- data/ext/ninja-scan-light/tool/swig/GPS.i +6 -4
- data/ext/ninja-scan-light/tool/swig/SylphideMath.i +139 -36
- data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb +115 -5
- data/gps_pvt.gemspec +3 -1
- data/lib/gps_pvt/asn1/asn1.rb +888 -0
- data/lib/gps_pvt/asn1/asn1.y +903 -0
- data/lib/gps_pvt/asn1/per.rb +182 -0
- data/lib/gps_pvt/ntrip.rb +1 -1
- data/lib/gps_pvt/receiver/agps.rb +31 -0
- data/lib/gps_pvt/receiver/extension.rb +94 -0
- data/lib/gps_pvt/receiver/rtcm3.rb +6 -4
- data/lib/gps_pvt/receiver.rb +41 -24
- data/lib/gps_pvt/rtcm3.rb +24 -34
- data/lib/gps_pvt/supl.rb +567 -0
- data/lib/gps_pvt/ubx.rb +15 -0
- data/lib/gps_pvt/upl/LPP-V17_5_0-Release17.asn +6441 -0
- data/lib/gps_pvt/upl/RRLP-V17_0_0-Release17.asn +2780 -0
- data/lib/gps_pvt/upl/ULP-V2_0_6-20200720-D.asn +2185 -0
- data/lib/gps_pvt/upl/upl.json.gz +0 -0
- data/lib/gps_pvt/upl/upl.rb +99 -0
- data/lib/gps_pvt/util.rb +1 -0
- data/lib/gps_pvt/version.rb +1 -1
- metadata +41 -3
data/lib/gps_pvt/supl.rb
ADDED
@@ -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
|