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
@@ -0,0 +1,182 @@
|
|
1
|
+
#!/usr/bin/ruby
|
2
|
+
|
3
|
+
module GPS_PVT
|
4
|
+
|
5
|
+
# @see ISO/IEC 8825-2:2003(E)
|
6
|
+
# @see http://www5d.biglobe.ne.jp/~stssk/asn1/per.html
|
7
|
+
module PER
|
8
|
+
module Basic_Unaligned
|
9
|
+
module Encoder
|
10
|
+
class <<self
|
11
|
+
def non_negative_binary_integer2(v, bits) # 10.3
|
12
|
+
"%0#{bits}b" % v
|
13
|
+
end
|
14
|
+
def non_negative_binary_integer(v, align = 1) # 10.3
|
15
|
+
non_negative_binary_integer2(
|
16
|
+
v, (Math::log2(v + 1) / align).ceil * align)
|
17
|
+
end
|
18
|
+
def twos_complement_binary_integer(v) # 10.4
|
19
|
+
if v >= 0 then
|
20
|
+
bits = ((Math::log2(v + 1) + 1) / 8).ceil * 8
|
21
|
+
"%0#{bits}b" % v
|
22
|
+
else
|
23
|
+
bits = ((Math::log2(-v) + 1) / 8).ceil * 8 - 1
|
24
|
+
"1%0#{bits}b" % (v + (1 << bits))
|
25
|
+
end
|
26
|
+
end
|
27
|
+
def constrainted_whole_number2(v, v_min, v_max) # 10.5.6
|
28
|
+
non_negative_binary_integer2(
|
29
|
+
v - v_min,
|
30
|
+
Math::log2(v_max - v_min + 1).ceil)
|
31
|
+
end
|
32
|
+
def constrainted_whole_number(v, v_range) # 10.5.6
|
33
|
+
constrainted_whole_number2(v, *v_range.minmax)
|
34
|
+
end
|
35
|
+
def normally_small_non_negative_whole_number(v, *len_enc) # 10.6
|
36
|
+
if v <= 63 then
|
37
|
+
"0%06b" % v # 10.6.1
|
38
|
+
else
|
39
|
+
"1#{semi_constrained_whole_number(v, 0, *len_enc)}" # 10.6.2
|
40
|
+
end
|
41
|
+
end
|
42
|
+
def semi_constrained_whole_number(v, v_min, *len_enc) # 10.7
|
43
|
+
len_enc = :length_otherwise if len_enc.empty?
|
44
|
+
bf = non_negative_binary_integer(v - v_min, 8).scan(/.{8}/)
|
45
|
+
with_length(bf.size, *len_enc).collect{|len_str, range|
|
46
|
+
len_str + bf[range].join
|
47
|
+
}.join
|
48
|
+
end
|
49
|
+
def unconstrained_whole_number(v, *len_enc) # 10.8
|
50
|
+
len_enc = :length_otherwise if len_enc.empty?
|
51
|
+
bf = twos_complement_binary_integer(v).scan(/.{8}/)
|
52
|
+
with_length(bf.size, *len_enc).collect{|len_str, range|
|
53
|
+
len_str + bf[range].join
|
54
|
+
}.join
|
55
|
+
end
|
56
|
+
def length_constrained_whole_number(len, len_range)
|
57
|
+
if len_range.max < 65536 then # 10.9.4.1
|
58
|
+
(len_range.min == len_range.max) ?
|
59
|
+
"" :
|
60
|
+
constrainted_whole_number(len, len_range)
|
61
|
+
else
|
62
|
+
length_otherwise(len)
|
63
|
+
end
|
64
|
+
end
|
65
|
+
def length_normally_small_length(len) # 10.9.4.2 -> 10.9.3.4
|
66
|
+
if len <= 64 then
|
67
|
+
normally_small_non_negative_whole_number(len - 1)
|
68
|
+
else
|
69
|
+
len_enc, len_remain = length_otherwise(len)
|
70
|
+
len_enc = "1#{len_enc}"
|
71
|
+
len_remain ? [len_enc, len_remain] : len_enc
|
72
|
+
end
|
73
|
+
end
|
74
|
+
def length_otherwise(len) # 10.9.4.2 -> 10.9.3.5-8
|
75
|
+
if len <= 127 then # 10.9.3.6
|
76
|
+
non_negative_binary_integer2(len, 8)
|
77
|
+
elsif len < 16384 then # 10.9.3.7
|
78
|
+
"10#{non_negative_binary_integer2(len, 14)}"
|
79
|
+
else # 10.9.3.8
|
80
|
+
q, r = len.divmod(16384)
|
81
|
+
q2 = [q, 4].min
|
82
|
+
res = "11#{non_negative_binary_integer2(q2, 6)}"
|
83
|
+
((r == 0) && (q <= 4)) ? res : [res, len - (q2 * 16384)]
|
84
|
+
end
|
85
|
+
end
|
86
|
+
def with_length(len, *len_enc)
|
87
|
+
Enumerator::new{|y|
|
88
|
+
len_str, len_remain = len_enc[0].kind_of?(Symbol) ? send(len_enc[0], len, *len_enc[1..-1]) : len_enc
|
89
|
+
loop{
|
90
|
+
if len_remain then
|
91
|
+
y << [len_str, -len..-(len_remain+1)]
|
92
|
+
else
|
93
|
+
y << [len_str, -len..-1]
|
94
|
+
break
|
95
|
+
end
|
96
|
+
len_str, len_remain = length_otherwise(len = len_remain) # fragmentation
|
97
|
+
}
|
98
|
+
}
|
99
|
+
end
|
100
|
+
end
|
101
|
+
end
|
102
|
+
module Decoder
|
103
|
+
class <<self
|
104
|
+
def non_negative_binary_integer(str, bits) # 10.3
|
105
|
+
str.slice!(0, bits).to_i(2)
|
106
|
+
end
|
107
|
+
def twos_complement_binary_integer(str, bits) # 10.4
|
108
|
+
bits -= 1
|
109
|
+
case str.slice!(0)
|
110
|
+
when '0'; 0
|
111
|
+
when '1'; -(1 << bits)
|
112
|
+
end + non_negative_binary_integer(str, bits)
|
113
|
+
end
|
114
|
+
def constrainted_whole_number2(str, v_min, v_max) # 10.5.6
|
115
|
+
non_negative_binary_integer(
|
116
|
+
str,
|
117
|
+
Math::log2(v_max - v_min + 1).ceil) + v_min
|
118
|
+
end
|
119
|
+
def constrainted_whole_number(str, v_range) # 10.5.6
|
120
|
+
constrainted_whole_number2(str, *v_range.minmax)
|
121
|
+
end
|
122
|
+
def normally_small_non_negative_whole_number(str, *len_dec) # 10.6
|
123
|
+
case str.slice!(0)
|
124
|
+
when '0'; str.slice!(0, 6).to_i(2) # 10.6.1
|
125
|
+
when '1'; semi_constrained_whole_number(str, 0, *len_dec) # 10.6.2
|
126
|
+
end
|
127
|
+
end
|
128
|
+
def semi_constrained_whole_number(str, v_min, *len_dec) # 10.7
|
129
|
+
len_dec = :length_otherwise if len_dec.empty?
|
130
|
+
v_str = with_length(str, *len_dec).collect{|len_oct|
|
131
|
+
str.slice!(0, len_oct * 8)
|
132
|
+
}.join
|
133
|
+
non_negative_binary_integer(v_str, v_str.size) + v_min
|
134
|
+
end
|
135
|
+
def unconstrained_whole_number(str, *len_dec) # 10.8
|
136
|
+
len_dec = :length_otherwise if len_dec.empty?
|
137
|
+
v_str = with_length(str, *len_dec).collect{|len_oct|
|
138
|
+
str.slice!(0, len_oct * 8)
|
139
|
+
}.join
|
140
|
+
twos_complement_binary_integer(v_str, v_str.size)
|
141
|
+
end
|
142
|
+
def length_constrained_whole_number(str, len_range)
|
143
|
+
if len_range.max < 65536 then # 10.9.4.1
|
144
|
+
(len_range.min == len_range.max) ?
|
145
|
+
len_range.min :
|
146
|
+
constrainted_whole_number(str, len_range)
|
147
|
+
else
|
148
|
+
length_otherwise(str)
|
149
|
+
end
|
150
|
+
end
|
151
|
+
def length_normally_small_length(str) # 10.9.4.2 -> 10.9.3.4
|
152
|
+
case str.slice!(0)
|
153
|
+
when '0'; str.slice!(0, 6).to_i(2) + 1
|
154
|
+
when '1'; length_otherwise(str)
|
155
|
+
end
|
156
|
+
end
|
157
|
+
def length_otherwise(str) # 10.9.4.2 -> 10.9.3.5-8
|
158
|
+
case str.slice!(0)
|
159
|
+
when '0'; non_negative_binary_integer(str, 7) # 10.9.3.6
|
160
|
+
when '1';
|
161
|
+
case str.slice!(0)
|
162
|
+
when '0'; non_negative_binary_integer(str, 14) # 10.9.3.7
|
163
|
+
when '1'; [non_negative_binary_integer(str, 6) * (1 << 14), true] # 10.9.3.8
|
164
|
+
end
|
165
|
+
end
|
166
|
+
end
|
167
|
+
def with_length(str, *len_dec)
|
168
|
+
Enumerator::new{|y|
|
169
|
+
len, cnt = len_dec[0].kind_of?(Symbol) ? send(len_dec[0], str, *len_dec[1..-1]) : len_dec
|
170
|
+
loop{
|
171
|
+
y << len
|
172
|
+
break unless cnt
|
173
|
+
len, cnt = length_otherwise(str) # fragmentation
|
174
|
+
}
|
175
|
+
}
|
176
|
+
end
|
177
|
+
end
|
178
|
+
end
|
179
|
+
end
|
180
|
+
end
|
181
|
+
|
182
|
+
end
|
data/lib/gps_pvt/ntrip.rb
CHANGED
@@ -54,7 +54,7 @@ class Ntrip < Net::HTTP
|
|
54
54
|
llh0 = Coordinate::LLH::new(D2R * lat_deg, D2R * lng_deg, 0)
|
55
55
|
collect{|pt, prop|
|
56
56
|
llh = Coordinate::LLH::new(*([:latitude, :longitude].collect{|k| D2R * prop[k].to_f} + [0]))
|
57
|
-
[llh0.xyz.
|
57
|
+
[llh0.xyz.distance(llh.xyz), prop]
|
58
58
|
}.sort{|a, b| a[0] <=> b[0]} # return [distance, property]
|
59
59
|
end
|
60
60
|
end
|
@@ -0,0 +1,31 @@
|
|
1
|
+
=begin
|
2
|
+
AGPS handler for receiver
|
3
|
+
=end
|
4
|
+
|
5
|
+
module GPS_PVT
|
6
|
+
class Receiver
|
7
|
+
def parse_supl(src, opt = {}, &b)
|
8
|
+
$stderr.print "A-GPS (%s) "%[src]
|
9
|
+
opt = {
|
10
|
+
:interval => 60 * 10, # 10 min.
|
11
|
+
}.merge(opt)
|
12
|
+
require_relative '../supl'
|
13
|
+
src_io = Util::open(src)
|
14
|
+
while data = src_io.get_assisted_data
|
15
|
+
data.ephemeris.each{|eph|
|
16
|
+
target = case eph
|
17
|
+
when GPS::Ephemeris; @solver.gps_space_node
|
18
|
+
when GPS::Ephemeris_GLONASS; @solver.glonass_space_node
|
19
|
+
when GPS::Ephemeris_SBAS; @solver.sbas_space_node
|
20
|
+
else nil
|
21
|
+
end
|
22
|
+
critical{target.register_ephemeris(eph.svid, eph)} if target
|
23
|
+
} if data.respond_to?(:ephemeris)
|
24
|
+
critical{
|
25
|
+
@solver.gps_space_node.update_iono_utc(data.iono_utc)
|
26
|
+
} if data.respond_to?(:iono_utc)
|
27
|
+
sleep(opt[:interval])
|
28
|
+
end
|
29
|
+
end
|
30
|
+
end
|
31
|
+
end
|
@@ -57,4 +57,98 @@ class Receiver
|
|
57
57
|
}
|
58
58
|
end
|
59
59
|
end
|
60
|
+
|
61
|
+
module GPS
|
62
|
+
|
63
|
+
# These ephemeris helper functions will be removed
|
64
|
+
# when native functions are available in GPS.i
|
65
|
+
class Ephemeris
|
66
|
+
URA_TABLE = [
|
67
|
+
2.40, 3.40, 4.85, 6.85, 9.65, 13.65, 24.00, 48.00,
|
68
|
+
96.00, 192.00, 384.00, 768.00, 1536.00, 3072.00, 6144.00]
|
69
|
+
def URA_index=(idx)
|
70
|
+
send(:URA=, (idx >= URA_TABLE.size) ? (URA_TABLE[-1] * 2) : (idx < 0 ? -1 : URA_TABLE[idx]))
|
71
|
+
end
|
72
|
+
def URA_index
|
73
|
+
ura = send(:URA)
|
74
|
+
(ura < 0) ? -1 : URA_TABLE.find_index{|v| ura <= v}
|
75
|
+
end
|
76
|
+
proc{
|
77
|
+
orig = instance_method(:fit_interval=)
|
78
|
+
define_method(:fit_interval=){|args|
|
79
|
+
args = case args
|
80
|
+
when Array
|
81
|
+
flag, iodc, sys = args
|
82
|
+
hr = case (sys ||= :GPS)
|
83
|
+
when :GPS, :gps
|
84
|
+
(flag == 0) ? 4 : case iodc
|
85
|
+
when 240..247; 8
|
86
|
+
when 248..255, 496; 14
|
87
|
+
when 497..503; 26
|
88
|
+
when 504..510; 50
|
89
|
+
when 511, 752..756; 74
|
90
|
+
when 757..763; 98
|
91
|
+
when 764..767, 1088..1010; 122
|
92
|
+
when 1011..1020; 146
|
93
|
+
else; 6
|
94
|
+
end
|
95
|
+
when :QZSS, :qzss
|
96
|
+
raise unless flag == 0 # TODO how to treat fit_interval > 2 hrs
|
97
|
+
2
|
98
|
+
else; raise
|
99
|
+
end
|
100
|
+
hr * 60 * 60
|
101
|
+
else
|
102
|
+
args
|
103
|
+
end
|
104
|
+
orig.bind(self).call(args)
|
105
|
+
}
|
106
|
+
}.call
|
107
|
+
end
|
108
|
+
class Ephemeris_SBAS
|
109
|
+
URA_TABLE = [ # Table 2-3 in DO-229E
|
110
|
+
2.0, 2.8, 4.0, 5.7, 8.0, 11.3, 16.0, 32.0,
|
111
|
+
64.0, 128.0, 256.0, 512.0, 1024.0, 2048.0, 4096.0]
|
112
|
+
def URA_index=(idx)
|
113
|
+
send(:URA=, (idx >= URA_TABLE.size) ? (URA_TABLE[-1] * 2) : (idx < 0 ? -1 : URA_TABLE[idx]))
|
114
|
+
end
|
115
|
+
def URA_index
|
116
|
+
ura = send(:URA)
|
117
|
+
(ura < 0) ? -1 : URA_TABLE.find_index{|v| ura <= v}
|
118
|
+
end
|
119
|
+
end
|
120
|
+
class Ephemeris_GLONASS
|
121
|
+
F_T_TABLE = [ # Table 4.4 in ICD 5.1
|
122
|
+
1, 2, 2.5, 4, 5, 7, 10, 12, 14, 16, 32, 64, 128, 256, 512, 1024]
|
123
|
+
def F_T_index=(idx)
|
124
|
+
send(:F_T=, (idx >= F_T_TABLE.size) ? (F_T_TABLE[-1] * 2) : (idx < 0 ? -1 : F_T_TABLE[idx]))
|
125
|
+
end
|
126
|
+
def F_T_index
|
127
|
+
f_t = send(:F_T)
|
128
|
+
(f_t < 0) ? -1 : F_T_TABLE.find_index{|v| f_t <= v}
|
129
|
+
end
|
130
|
+
def NA
|
131
|
+
# based on TimeProperties::date2raw
|
132
|
+
self.day_of_year + [1, 367, 732, 1097][(self.year - 1996) % 4]
|
133
|
+
end
|
134
|
+
end
|
135
|
+
|
136
|
+
[
|
137
|
+
Ionospheric_UTC_Parameters,
|
138
|
+
Ephemeris, Ephemeris_SBAS, Ephemeris_GLONASS,
|
139
|
+
].each{|cls|
|
140
|
+
cls.class_eval{
|
141
|
+
proc{|func_list|
|
142
|
+
func_list.select!{|func|
|
143
|
+
(/=$/ !~ func.to_s) && func_list.include?("#{func}=".to_sym)
|
144
|
+
}
|
145
|
+
define_method(:to_hash){
|
146
|
+
Hash[*(func_list.collect{|func|
|
147
|
+
[func, send(func)]
|
148
|
+
}.flatten(1))]
|
149
|
+
}
|
150
|
+
}.call(instance_methods(false))
|
151
|
+
}
|
152
|
+
}
|
153
|
+
end
|
60
154
|
end
|
@@ -81,7 +81,7 @@ class Receiver
|
|
81
81
|
eph = sn.ephemeris(svid)
|
82
82
|
cache[[sys, svid]] = [if eph.valid?(t) then
|
83
83
|
sv_pos, clk_err = eph.constellation(t).values_at(0, 2)
|
84
|
-
sv_pos.
|
84
|
+
sv_pos.distance(ref_pos) - (clk_err * c_1ms * 1E3)
|
85
85
|
end, t]
|
86
86
|
}
|
87
87
|
}
|
@@ -195,7 +195,7 @@ class Receiver
|
|
195
195
|
when 1020
|
196
196
|
params = parsed.params
|
197
197
|
eph = GPS::Ephemeris_GLONASS::new
|
198
|
-
|
198
|
+
eph.F_T = 10 # [m], default to be overwritten
|
199
199
|
params.each{|k, v|
|
200
200
|
next if [:P3, :NA, :N_4].include?(k)
|
201
201
|
eph.send("#{k}=".to_sym, v)
|
@@ -205,6 +205,7 @@ class Receiver
|
|
205
205
|
? date_src \
|
206
206
|
: [(ref_time + 3 * 60 * 60).c_tm(leap_sec)])) # UTC -> Moscow time
|
207
207
|
}.call([:N_4, :NA].collect{|k| params[k]})
|
208
|
+
eph.N_T = params[:NA] || eph.NA unless params[:N_T] # N_T is available only for GLONASS-M
|
208
209
|
eph.rehash(leap_sec)
|
209
210
|
critical{
|
210
211
|
@solver.glonass_space_node.register_ephemeris(eph.svid, eph)
|
@@ -259,9 +260,9 @@ class Receiver
|
|
259
260
|
sys_svid_list = ranges[:sat_sig].collect{|sat, sig| [sys, (sat + svid_offset) & 0xFF]}
|
260
261
|
restore_ranges.call(t_meas2, sys_svid_list, ranges)
|
261
262
|
item_size = sys_svid_list.size
|
262
|
-
[:sat_sig, :pseudo_range, :phase_range, :phase_range_rate, :cn].collect{|k|
|
263
|
+
[:sat_sig, :pseudo_range, :phase_range, :phase_range_rate, :cn, :halfc_amb].collect{|k|
|
263
264
|
ranges[k] || ([nil] * item_size)
|
264
|
-
}.transpose.each{|(svid, sig), pr, cpr, dr, cn|
|
265
|
+
}.transpose.each{|(svid, sig), pr, cpr, dr, cn, amb|
|
265
266
|
prefix, len = sig_list[sig]
|
266
267
|
next unless prefix
|
267
268
|
proc{
|
@@ -274,6 +275,7 @@ class Receiver
|
|
274
275
|
meas2 << [svid, "#{prefix}_RANGE_RATE".to_sym, dr] if dr
|
275
276
|
meas2 << [svid, "#{prefix}_CARRIER_PHASE".to_sym, cpr / len] if cpr && len
|
276
277
|
meas2 << [svid, "#{prefix}_SIGNAL_STRENGTH_dBHz".to_sym, cn] if cn
|
278
|
+
meas2 << [svid, "#{prefix}_CARRIER_PHASE_AMBIGUITY_SCALE".to_sym, 0.5] if amb && (amb == 1)
|
277
279
|
}
|
278
280
|
else
|
279
281
|
#p({msg_num => parsed})
|
data/lib/gps_pvt/receiver.rb
CHANGED
@@ -135,7 +135,7 @@ class Receiver
|
|
135
135
|
opt[:satellites].collect{|prn, label|
|
136
136
|
pr, rate, doppler, freq = keys.collect{|k| meas_hash[prn][k] rescue nil}
|
137
137
|
freq ||= GPS::SpaceNode.L1_Frequency
|
138
|
-
[pr, rate || ((doppler * GPS::SpaceNode::light_speed / freq) rescue nil)]
|
138
|
+
[pr, rate || ((-doppler * GPS::SpaceNode::light_speed / freq) rescue nil)]
|
139
139
|
}
|
140
140
|
}
|
141
141
|
]]
|
@@ -324,25 +324,31 @@ class Receiver
|
|
324
324
|
end
|
325
325
|
|
326
326
|
GPS::Measurement.class_eval{
|
327
|
-
proc{
|
328
|
-
key2sym = []
|
329
|
-
GPS::Measurement.constants.each{|k|
|
330
|
-
i = GPS::Measurement.const_get(k)
|
331
|
-
key2sym[i] = k if i.kind_of?(Integer)
|
332
|
-
}
|
333
|
-
define_method(:to_a2){
|
334
|
-
to_a.collect{|prn, k, v| [prn, key2sym[k] || k, v]}
|
335
|
-
}
|
336
|
-
define_method(:to_hash2){
|
337
|
-
Hash[*(to_hash.collect{|prn, k_v|
|
338
|
-
[prn, Hash[*(k_v.collect{|k, v| [key2sym[k] || k, v]}.flatten(1))]]
|
339
|
-
}.flatten(1))]
|
340
|
-
}
|
341
|
-
}.call
|
342
327
|
add_orig = instance_method(:add)
|
343
328
|
define_method(:add){|prn, key, value|
|
344
329
|
add_orig.bind(self).call(prn, key.kind_of?(Symbol) ? GPS::Measurement.const_get(key) : key, value)
|
345
330
|
}
|
331
|
+
key2sym = GPS::Measurement.constants.inject([]){|res, k|
|
332
|
+
res[GPS::Measurement.const_get(k)] = k if /^L\d/ =~ k.to_s
|
333
|
+
res
|
334
|
+
}
|
335
|
+
define_method(:to_a2){
|
336
|
+
collect{|prn, k, v| [prn, key2sym[k] || k, v]}
|
337
|
+
}
|
338
|
+
cl_hash2 = Class::new(Hash){
|
339
|
+
define_method(:to_meas){
|
340
|
+
GPS::Measurement::new.tap{|res|
|
341
|
+
each{|prn, k_v|
|
342
|
+
k_v.each{|k, v| res.add(prn, k, v)}
|
343
|
+
}
|
344
|
+
}
|
345
|
+
}
|
346
|
+
}
|
347
|
+
define_method(:to_hash2){
|
348
|
+
cl_hash2::new.tap{|res|
|
349
|
+
each{|prn, k, v| (res[prn] ||= {})[key2sym[k] || k] = v}
|
350
|
+
}
|
351
|
+
}
|
346
352
|
}
|
347
353
|
|
348
354
|
def run(meas, t_meas, ref_pos = @base_station)
|
@@ -509,10 +515,12 @@ class Receiver
|
|
509
515
|
}.each{|k, prop|
|
510
516
|
meas.add(prn, k, loader.call(*prop))
|
511
517
|
}
|
518
|
+
lli = packet[6 + 31 + (i * 24)]
|
512
519
|
# bit 0 of RINEX LLI (loss of lock indicator) shows lost lock
|
513
520
|
# between previous and current observation, which maps negative lock seconds
|
514
|
-
meas.add(prn, :L1_LOCK_SEC,
|
515
|
-
|
521
|
+
meas.add(prn, :L1_LOCK_SEC, (lli & 0x01 == 0x01) ? -1 : 0)
|
522
|
+
# set bit 1 of LLI represents possibility of half cycle ambiguity
|
523
|
+
meas.add(prn, :L1_CARRIER_PHASE_AMBIGUITY_SCALE, 0.5) if (lli & 0x02 == 0x02)
|
516
524
|
}
|
517
525
|
after_run.call(run(meas, t_meas), [meas, t_meas])
|
518
526
|
when [0x02, 0x15] # RXM-RAWX
|
@@ -553,7 +561,13 @@ class Receiver
|
|
553
561
|
}],
|
554
562
|
:DOPPLER => [32, 4, "e"],
|
555
563
|
:DOPPLER_SIGMA => [45, 1, nil, proc{|v| 2E-3 * (1 << (v[0] & 0xF))}],
|
556
|
-
:CARRIER_PHASE => [24, 8, "E", proc{|v|
|
564
|
+
:CARRIER_PHASE => [24, 8, "E", proc{|v|
|
565
|
+
case (trk_stat & 0x6)
|
566
|
+
when 0x6; (trk_stat & 0x8 == 0x8) ? (v + 0.5) : v
|
567
|
+
when 0x2; meas.add(svid, "#{sigid}_CARRIER_PHASE_AMBIGUITY_SCALE".to_sym, 0.5); v
|
568
|
+
else; nil
|
569
|
+
end
|
570
|
+
}],
|
557
571
|
:CARRIER_PHASE_SIGMA => [44, 1, nil, proc{|v|
|
558
572
|
(trk_stat & 0x2 == 0x2) ? (0.004 * (v[0] & 0xF)) : nil
|
559
573
|
}],
|
@@ -629,7 +643,7 @@ class Receiver
|
|
629
643
|
'S' => :SIGNAL_STRENGTH_dBHz,
|
630
644
|
}[type_[0]]]
|
631
645
|
next nil unless sig_obs_type.all?
|
632
|
-
[i, sig_obs_type.join('_').to_sym]
|
646
|
+
[i, sig_obs_type.join('_').to_sym, *sig_obs_type]
|
633
647
|
}.compact]
|
634
648
|
}.flatten(1))]
|
635
649
|
|
@@ -644,8 +658,7 @@ class Receiver
|
|
644
658
|
}.call(item[:header]["GLONASS SLOT / FRQ #"])
|
645
659
|
|
646
660
|
meas = GPS::Measurement::new
|
647
|
-
item[:meas].each{|
|
648
|
-
sys, prn = k
|
661
|
+
item[:meas].each{|(sys, prn), v|
|
649
662
|
case sys
|
650
663
|
when 'G', ' '
|
651
664
|
when 'S'; prn += 100
|
@@ -662,8 +675,11 @@ class Receiver
|
|
662
675
|
else; next
|
663
676
|
end
|
664
677
|
types[sys] = (types[' '] || []) unless types[sys]
|
665
|
-
types[sys].each{|i, type_|
|
666
|
-
|
678
|
+
types[sys].each{|i, type_, sig_type, obs_type|
|
679
|
+
next unless v[i]
|
680
|
+
meas.add(prn, type_, v[i][0])
|
681
|
+
meas.add(prn, "#{sig_type}_CARRIER_PHASE_AMBIGUITY_SCALE".to_sym, 0.5) \
|
682
|
+
if (obs_type == :CARRIER_PHASE) && (v[i][1] & 0x2 == 0x2)
|
667
683
|
}
|
668
684
|
}
|
669
685
|
after_run.call(run(meas, t_meas), [meas, t_meas])
|
@@ -714,4 +730,5 @@ end
|
|
714
730
|
end
|
715
731
|
|
716
732
|
require_relative 'receiver/rtcm3'
|
733
|
+
require_relative 'receiver/agps'
|
717
734
|
require_relative 'receiver/extension'
|
data/lib/gps_pvt/rtcm3.rb
CHANGED
@@ -104,12 +104,7 @@ class RTCM3
|
|
104
104
|
57 => 16,
|
105
105
|
71 => 8,
|
106
106
|
76 => 10,
|
107
|
-
77 =>
|
108
|
-
idx2meter = [
|
109
|
-
2.40, 3.40, 4.85, 6.85, 9.65, 13.65, 24.00, 48.00,
|
110
|
-
96.00, 192.00, 384.00, 768.00, 1536.00, 3072.00, 6144.00]
|
111
|
-
[4, proc{|v| (v >= idx2meter.size) ? (idx2meter[-1] * 2) : idx2meter[v]}]
|
112
|
-
}.call, # [m]
|
107
|
+
77 => 4,
|
113
108
|
78 => 2,
|
114
109
|
79 => num_gen.call(14, Rational(sc2rad, 1 << 43)), # [rad/s]
|
115
110
|
81 => unum_gen.call(16, 1 << 4), # [sec]
|
@@ -156,15 +151,13 @@ class RTCM3
|
|
156
151
|
125 => num_sign_gen.call(5, Rational(1, 1 << 30)), # [sec], (M)
|
157
152
|
126 => 5, # [day]
|
158
153
|
127 => 1, # (M)
|
159
|
-
128 =>
|
160
|
-
|
161
|
-
}], # [m] (M)
|
162
|
-
129 => 11, # [day]
|
154
|
+
128 => 4, # (M)
|
155
|
+
129 => invalidate.call(unum_gen.call(11), 0), # [day]
|
163
156
|
130 => 2, # 1 => GLONASS-M, (M) fields are active
|
164
157
|
131 => 1,
|
165
|
-
132 => 11, # [day]
|
158
|
+
132 => invalidate.call(unum_gen.call(11), 0), # [day]
|
166
159
|
133 => num_sign_gen.call(32, Rational(1, 1 << 31)), # [sec]
|
167
|
-
134 => 5, # [4year], (M)
|
160
|
+
134 => invalidate.call(unum_gen.call(5), 0), # [4year], (M)
|
168
161
|
135 => num_sign_gen.call(22, Rational(1, 1 << 30)), # [sec], (M)
|
169
162
|
136 => 1, # (M)
|
170
163
|
137 => 1,
|
@@ -247,9 +240,11 @@ class RTCM3
|
|
247
240
|
1013 => [2, 3, 51, 52, 53, 54],
|
248
241
|
1019 => [2, 9, (76..79).to_a, 71, (81..103).to_a, 137].flatten, # 488 bits @see Table 3.5-21
|
249
242
|
1020 => [2, 38, 40, (104..136).to_a].flatten, # 360 bits @see Table 3.5-21
|
243
|
+
# @see BNC Ntrip client DecodeSBASEphemeris() of RTCM3Decorder.cpp
|
244
|
+
# https://software.rtcm-ntrip.org/browser/ntrip/trunk/BNC/src/RTCM3/RTCM3Decoder.cpp
|
250
245
|
1043 => [2] + [:prn, :iodn, :tod, :ura,
|
251
246
|
[:xy] * 2, :z, [:dxy] * 2, :dz, [:ddxy] * 2, :ddz,
|
252
|
-
:agf0, :agf1].flatten.collect{|k| "SBAS_#{k}".to_sym},
|
247
|
+
:agf0, :agf1].flatten.collect{|k| "SBAS_#{k}".to_sym},
|
253
248
|
1044 => [2, (429..457).to_a].flatten, # 485 bits
|
254
249
|
1070..1229 => [2, [:uint, 12], [:uint, 30], 393], # 55 bits part of messages will be overwritten
|
255
250
|
1071..1077 => [2, 3, 4, 393, 409, [1, 7], 411, 412, 417, 418, 394, 395], # 169 bits @see Table 3.5-78
|
@@ -311,7 +306,7 @@ class RTCM3
|
|
311
306
|
end
|
312
307
|
end
|
313
308
|
module GPS_Ephemeris
|
314
|
-
KEY2IDX = {:svid => 1, :WN => 2, :
|
309
|
+
KEY2IDX = {:svid => 1, :WN => 2, :URA_index => 3, :dot_i0 => 5, :iode => 6, :t_oc => 7,
|
315
310
|
:a_f2 => 8, :a_f1 => 9, :a_f0 => 10, :iodc => 11, :c_rs => 12, :delta_n => 13,
|
316
311
|
:M0 => 14, :c_uc => 15, :e => 16, :c_us => 17, :sqrt_A => 18, :t_oe => 19, :c_ic => 20,
|
317
312
|
:Omega0 => 21, :c_is => 22, :i0 => 23, :c_rc => 24, :omega => 25, :dot_Omega0 => 26,
|
@@ -319,22 +314,12 @@ class RTCM3
|
|
319
314
|
def params
|
320
315
|
# TODO WN is truncated to 0-1023
|
321
316
|
res = Hash[*(KEY2IDX.collect{|k, i| [k, self[i][0]]}.flatten(1))]
|
322
|
-
res[:fit_interval] =
|
323
|
-
when 240..247; 8
|
324
|
-
when 248..255, 496; 14
|
325
|
-
when 497..503; 26
|
326
|
-
when 504..510; 50
|
327
|
-
when 511, 752..756; 74
|
328
|
-
when 757..763; 98
|
329
|
-
when 764..767, 1088..1010; 122
|
330
|
-
when 1011..1020; 146
|
331
|
-
else; 6
|
332
|
-
end) * 60 * 60
|
317
|
+
res[:fit_interval] = [self[29][0], res[:iodc]]
|
333
318
|
res
|
334
319
|
end
|
335
320
|
end
|
336
321
|
module SBAS_Ephemeris
|
337
|
-
KEY2IDX = {:svid => 1, :iodn => 2, :tod => 3, :
|
322
|
+
KEY2IDX = {:svid => 1, :iodn => 2, :tod => 3, :URA_index => 4,
|
338
323
|
:x => 5, :y => 6, :z => 7,
|
339
324
|
:dx => 8, :dy => 9, :dz => 10,
|
340
325
|
:ddx => 11, :ddy => 12, :ddz => 13,
|
@@ -354,13 +339,13 @@ class RTCM3
|
|
354
339
|
:yn_dot => 13, :yn => 14, :yn_ddot => 15,
|
355
340
|
:zn_dot => 16, :zn => 17, :zn_ddot => 18,
|
356
341
|
:P3 => 19, :gamma_n => 20, :p => 21, :tau_n => 23, :delta_tau_n => 24, :E_n => 25,
|
357
|
-
:P4 => 26, :
|
342
|
+
:P4 => 26, :F_T_index => 27, :N_T => 28, :M => 29}
|
358
343
|
k_i.merge!({:NA => 31, :tau_c => 32, :N_4 => 33, :tau_GPS => 34}) if self[30][0] == 1 # check DF131
|
359
344
|
res = Hash[*(k_i.collect{|k, i| [k, self[i][0]]}.flatten(1))]
|
360
345
|
res.reject!{|k, v|
|
361
346
|
case k
|
362
347
|
when :N_T; v == 0
|
363
|
-
when :p, :delta_tau_n, :P4, :
|
348
|
+
when :p, :delta_tau_n, :P4, :F_T_index, :N_4, :tau_GPS; true # TODO sometimes delta_tau_n is valid?
|
364
349
|
else; false
|
365
350
|
end
|
366
351
|
} if (res[:M] != 1) # check DF130
|
@@ -372,12 +357,12 @@ class RTCM3
|
|
372
357
|
:iode => 6, :c_rs => 7, :delta_n => 8, :M0 => 9, :c_uc => 10, :e => 11,
|
373
358
|
:c_us => 12, :sqrt_A => 13, :t_oe => 14, :c_ic => 15, :Omega0 => 16,
|
374
359
|
:c_is => 17, :i0 => 18, :c_rc => 19, :omega => 20, :dot_Omega0 => 21,
|
375
|
-
:dot_i0 => 22, :WN => 24, :
|
360
|
+
:dot_i0 => 22, :WN => 24, :URA_index => 25, :SV_health => 26,
|
376
361
|
:t_GD => 27, :iodc => 28}
|
377
362
|
def params
|
378
363
|
# TODO PRN = svid + 192, WN is truncated to 0-1023
|
379
364
|
res = Hash[*(KEY2IDX.collect{|k, i| [k, self[i][0]]}.flatten(1))]
|
380
|
-
res[:fit_interval] =
|
365
|
+
res[:fit_interval] = [self[29][0], res[:iodc], :QZSS]
|
381
366
|
res
|
382
367
|
end
|
383
368
|
end
|
@@ -421,6 +406,7 @@ class RTCM3
|
|
421
406
|
}
|
422
407
|
add_proc.call(0)
|
423
408
|
add_proc.call(1)
|
409
|
+
res[:halfc_amb] = self[-ncell, ncell].transpose[0] if self[-1][1] == 420
|
424
410
|
res
|
425
411
|
end
|
426
412
|
end
|
@@ -433,14 +419,16 @@ class RTCM3
|
|
433
419
|
range_rough2 = self[offset + (nsat * 1), nsat] # DF398
|
434
420
|
range_fine = self[offset + (nsat * 2), ncell] # DF400/405
|
435
421
|
phase_fine = self[offset + (nsat * 2) + (ncell * 1), ncell] # DF401/406
|
422
|
+
halfc_amb = self[offset + (nsat * 2) + (ncell * 3), ncell] # DF420
|
436
423
|
cn = self[offset + (nsat * 2) + (ncell * 4), ncell] # DF403/408
|
437
|
-
Hash[*([:sat_sig, :pseudo_range, :phase_range, :cn].zip(
|
424
|
+
Hash[*([:sat_sig, :pseudo_range, :phase_range, :cn, :halfc_amb].zip(
|
438
425
|
[cells] + cells.collect.with_index{|(sat, sig), i|
|
439
426
|
i2 = sats.find_index(sat)
|
440
427
|
rough_ms = (range_rough2[i2][0] + range_rough[i2][0]) rescue nil
|
441
428
|
[(((range_fine[i][0] + rough_ms) * SPEED_OF_LIGHT) rescue nil),
|
442
429
|
(((phase_fine[i][0] + rough_ms) * SPEED_OF_LIGHT) rescue nil),
|
443
|
-
cn[i][0]
|
430
|
+
cn[i][0],
|
431
|
+
halfc_amb[i][0]]
|
444
432
|
}.transpose).flatten(1))]
|
445
433
|
end
|
446
434
|
end
|
@@ -454,16 +442,18 @@ class RTCM3
|
|
454
442
|
delta_rough = self[offset + (nsat * 3), nsat] # DF399
|
455
443
|
range_fine = self[offset + (nsat * 4), ncell] # DF400/405
|
456
444
|
phase_fine = self[offset + (nsat * 4) + (ncell * 1), ncell] # DF401/406
|
445
|
+
halfc_amb = self[offset + (nsat * 4) + (ncell * 3), ncell] # DF420
|
457
446
|
cn = self[offset + (nsat * 4) + (ncell * 4), ncell] # DF403/408
|
458
447
|
delta_fine = self[offset + (nsat * 4) + (ncell * 5), ncell] # DF404
|
459
|
-
Hash[*([:sat_sig, :pseudo_range, :phase_range, :phase_range_rate, :cn].zip(
|
448
|
+
Hash[*([:sat_sig, :pseudo_range, :phase_range, :phase_range_rate, :cn, :halfc_amb].zip(
|
460
449
|
[cells] + cells.collect.with_index{|(sat, sig), i|
|
461
450
|
i2 = sats.find_index(sat)
|
462
451
|
rough_ms = (range_rough2[i2][0] + range_rough[i2][0]) rescue nil
|
463
452
|
[(((range_fine[i][0] + rough_ms) * SPEED_OF_LIGHT) rescue nil),
|
464
453
|
(((phase_fine[i][0] + rough_ms) * SPEED_OF_LIGHT) rescue nil),
|
465
454
|
((delta_fine[i][0] + delta_rough[i2][0]) rescue nil),
|
466
|
-
cn[i][0]
|
455
|
+
cn[i][0],
|
456
|
+
halfc_amb[i][0]]
|
467
457
|
}.transpose).flatten(1))]
|
468
458
|
end
|
469
459
|
end
|