gps_pvt 0.8.5 → 0.9.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.
- checksums.yaml +4 -4
- data/README.md +3 -2
- data/exe/gps2ubx +6 -4
- data/exe/gps_pvt +6 -5
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +615 -84
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +24 -10
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +2 -1
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +1 -1
- data/ext/ninja-scan-light/tool/swig/GPS.i +32 -23
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +2 -0
- data/lib/gps_pvt/ntrip.rb +23 -4
- data/lib/gps_pvt/receiver/rtcm3.rb +110 -0
- data/lib/gps_pvt/receiver.rb +1 -0
- data/lib/gps_pvt/rtcm3.rb +364 -0
- data/lib/gps_pvt/util.rb +44 -0
- data/lib/gps_pvt/version.rb +1 -1
- metadata +4 -2
@@ -371,6 +371,18 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
|
|
371
371
|
}
|
372
372
|
return res;
|
373
373
|
}
|
374
|
+
static void date2raw(const date_t &src, u8_t *N_4_, u16_t *NA_){
|
375
|
+
std::div_t divmod(std::div(src.year - 1996, 4));
|
376
|
+
if(N_4_){*N_4_ = divmod.quot + 1;}
|
377
|
+
if(NA_){
|
378
|
+
*NA_ = src.day_of_year + 1;
|
379
|
+
switch(divmod.rem){
|
380
|
+
case 1: *NA_ += 366; break;
|
381
|
+
case 2: *NA_ += 731; break;
|
382
|
+
case 3: *NA_ += 1096; break;
|
383
|
+
}
|
384
|
+
}
|
385
|
+
}
|
374
386
|
|
375
387
|
operator TimeProperties() const {
|
376
388
|
TimeProperties res;
|
@@ -389,14 +401,7 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
|
|
389
401
|
{TARGET = (s32_t)std::floor(t.TARGET / sf[SF_ ## TARGET] + 0.5);}
|
390
402
|
CONVERT(tau_c);
|
391
403
|
CONVERT(tau_GPS);
|
392
|
-
|
393
|
-
N_4 = divmod.quot + 1;
|
394
|
-
NA = t.date.day_of_year + 1;
|
395
|
-
switch(divmod.rem){
|
396
|
-
case 1: NA += 366; break;
|
397
|
-
case 2: NA += 731; break;
|
398
|
-
case 3: NA += 1096; break;
|
399
|
-
}
|
404
|
+
date2raw(t.date, &N_4, &NA);
|
400
405
|
l_n = (t.l_n ? 1 : 0);
|
401
406
|
#undef CONVERT
|
402
407
|
return *this;
|
@@ -1072,6 +1077,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1072
1077
|
* 3) t_GPS - t_GL = delta_T + tau_GPS (defined in ICD 4.5 Non-immediate info.)
|
1073
1078
|
*/
|
1074
1079
|
struct Ephemeris_with_Time : public Ephemeris, TimeProperties {
|
1080
|
+
typename TimeProperties::date_t t_b_date;
|
1075
1081
|
typedef typename Ephemeris::constellation_t constellation_t;
|
1076
1082
|
constellation_t xa_t_b;
|
1077
1083
|
float_t sidereal_t_b_rad;
|
@@ -1080,8 +1086,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1080
1086
|
static const float_t time_step_max_per_one_integration;
|
1081
1087
|
|
1082
1088
|
void calculate_additional() {
|
1089
|
+
u8_t N_4;
|
1090
|
+
u16_t NA;
|
1091
|
+
TimeProperties::raw_t::date2raw(TimeProperties::date, &N_4, &NA);
|
1092
|
+
// TODO detect large difference between NA and N_T caused by rolling over.
|
1093
|
+
t_b_date = TimeProperties::raw_t::raw2date(N_4, this->N_T);
|
1083
1094
|
sidereal_t_b_rad = TimeProperties::date_t::Greenwich_sidereal_time_deg(
|
1084
|
-
|
1095
|
+
t_b_date.c_tm(),
|
1085
1096
|
(float_t)(this->t_b) / (60 * 60) - 3) / 180 * M_PI;
|
1086
1097
|
constellation_t x_t_b = {
|
1087
1098
|
{this->xn, this->yn, this->zn},
|
@@ -1101,11 +1112,14 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1101
1112
|
t_mt.tm_hour += 3;
|
1102
1113
|
std::mktime(&t_mt); // renormalization
|
1103
1114
|
this->date = TimeProperties::date_t::from_c_tm(t_mt);
|
1115
|
+
u16_t N_T;
|
1116
|
+
TimeProperties::raw_t::date2raw(this->date, NULL, &N_T);
|
1117
|
+
this->N_T = N_T;
|
1104
1118
|
this->t_b = (t_mt.tm_hour * 60 + t_mt.tm_min) * 60 + t_mt.tm_sec;
|
1105
1119
|
calculate_additional();
|
1106
1120
|
}
|
1107
1121
|
std::tm c_tm_utc() const {
|
1108
|
-
std::tm t(
|
1122
|
+
std::tm t(t_b_date.c_tm()); // set date on Moscow time
|
1109
1123
|
(t.tm_sec = (int)(this->t_b)) -= 3 * 60 * 60; // add second on UTC
|
1110
1124
|
std::mktime(&t); // renormalization
|
1111
1125
|
return t;
|
@@ -658,10 +658,11 @@ protected:
|
|
658
658
|
* @param x solution
|
659
659
|
* @return WSSR scalar
|
660
660
|
*/
|
661
|
-
float_t wssr(const matrix_t &x
|
661
|
+
float_t wssr(const matrix_t &x) const {
|
662
662
|
matrix_t v(delta_r - G * x);
|
663
663
|
return (v.transpose() * W * v)(0, 0);
|
664
664
|
}
|
665
|
+
float_t wssr() const {return wssr(least_square());}
|
665
666
|
/**
|
666
667
|
* Calculate weighted square sum of residual (WSSR) based on least square solution
|
667
668
|
* with solution coefficient matrix (S).
|
@@ -1809,7 +1809,7 @@ class RINEX_Writer {
|
|
1809
1809
|
friend std::ostream &operator<<(std::ostream &out, const header_t &header){
|
1810
1810
|
std::stringstream ss;
|
1811
1811
|
ss << std::setfill(' ') << std::left;
|
1812
|
-
for(header_t::const_iterator it(header.begin()), it_end(header.end());
|
1812
|
+
for(typename header_t::const_iterator it(header.begin()), it_end(header.end());
|
1813
1813
|
it != it_end; ++it){
|
1814
1814
|
ss << std::setw(60) << it->second.substr(0, 60)
|
1815
1815
|
<< std::setw(20) << it->first.substr(0, 20)
|
@@ -191,16 +191,19 @@ static std::string inspect_str(const VALUE &v){
|
|
191
191
|
#endif
|
192
192
|
}
|
193
193
|
|
194
|
-
%define
|
195
|
-
%rename(%str(
|
196
|
-
type set_ ##
|
197
|
-
return self->
|
194
|
+
%define MAKE_ACCESSOR2(func_name, target, type)
|
195
|
+
%rename(%str(func_name ## =)) set_ ## func_name;
|
196
|
+
type set_ ## func_name (const type &v) {
|
197
|
+
return self->target= v;
|
198
198
|
}
|
199
|
-
%rename(%str(
|
200
|
-
const type &get_ ##
|
201
|
-
return self->
|
199
|
+
%rename(%str(func_name)) get_ ## func_name;
|
200
|
+
const type &get_ ## func_name () const {
|
201
|
+
return self->target;
|
202
202
|
}
|
203
203
|
%enddef
|
204
|
+
%define MAKE_ACCESSOR(name, type)
|
205
|
+
MAKE_ACCESSOR2(name, name, type)
|
206
|
+
%enddef
|
204
207
|
|
205
208
|
%define MAKE_VECTOR2ARRAY(type)
|
206
209
|
%typemap(out) std::vector<type> {
|
@@ -663,6 +666,14 @@ struct GLONASS_Ephemeris
|
|
663
666
|
raw = *this;
|
664
667
|
has_string = 0x1F;
|
665
668
|
}
|
669
|
+
GLONASS_Ephemeris &rehash(const int &deltaT = 0) {
|
670
|
+
typedef typename GLONASS_SpaceNode<FloatT>::SatelliteProperties prop_t;
|
671
|
+
return *this = GLONASS_Ephemeris(eph_t(
|
672
|
+
typename prop_t::Ephemeris_with_Time(
|
673
|
+
(typename prop_t::Ephemeris)(*this),
|
674
|
+
(typename GLONASS_SpaceNode<FloatT>::TimeProperties)(*this)),
|
675
|
+
deltaT));
|
676
|
+
}
|
666
677
|
};
|
667
678
|
%}
|
668
679
|
%extend GLONASS_Ephemeris {
|
@@ -692,6 +703,15 @@ struct GLONASS_Ephemeris
|
|
692
703
|
|
693
704
|
MAKE_ACCESSOR(tau_c, FloatT);
|
694
705
|
MAKE_ACCESSOR(tau_GPS, FloatT);
|
706
|
+
MAKE_ACCESSOR2(year, date.year, int);
|
707
|
+
MAKE_ACCESSOR2(day_of_year, date.day_of_year, int);
|
708
|
+
|
709
|
+
void set_date(const unsigned int &N_4, const unsigned int &NA) {
|
710
|
+
self->date = GLONASS_SpaceNode<FloatT>::TimeProperties::raw_t::raw2date(N_4, NA);
|
711
|
+
}
|
712
|
+
void set_date(const std::tm &t) {
|
713
|
+
self->date = GLONASS_SpaceNode<FloatT>::TimeProperties::date_t::from_c_tm(t);
|
714
|
+
}
|
695
715
|
|
696
716
|
FloatT frequency_L1() const {
|
697
717
|
return self->L1_frequency();
|
@@ -1149,19 +1169,8 @@ struct GPS_Measurement {
|
|
1149
1169
|
}
|
1150
1170
|
|
1151
1171
|
%extend GPS_SolverOptions_Common {
|
1152
|
-
|
1153
|
-
|
1154
|
-
type set_ ## name (const type &v) {
|
1155
|
-
return self->cast_general()->name = v;
|
1156
|
-
}
|
1157
|
-
%rename(%str(name)) get_ ## name;
|
1158
|
-
const type &get_ ## name () const {
|
1159
|
-
return self->cast_general()->name;
|
1160
|
-
}
|
1161
|
-
%enddef
|
1162
|
-
MAKE_ACCESSOR2(elevation_mask, FloatT);
|
1163
|
-
MAKE_ACCESSOR2(residual_mask, FloatT);
|
1164
|
-
#undef MAKE_ACCESSOR2
|
1172
|
+
MAKE_ACCESSOR2(elevation_mask, cast_general()->elevation_mask, FloatT);
|
1173
|
+
MAKE_ACCESSOR2(residual_mask, cast_general()->residual_mask, FloatT);
|
1165
1174
|
MAKE_VECTOR2ARRAY(int);
|
1166
1175
|
%ignore cast_general;
|
1167
1176
|
}
|
@@ -1321,15 +1330,15 @@ struct HookableSolver : public BaseT {
|
|
1321
1330
|
HookableSolver<
|
1322
1331
|
GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >,
|
1323
1332
|
GPS_Solver<FloatT> >
|
1324
|
-
::HookableSolver
|
1333
|
+
::HookableSolver/*<GPS_SpaceNode<FloatT> >*/(const GPS_SpaceNode<FloatT> &sn) // to avoid out-of-line constructor error
|
1325
1334
|
: GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >(sn), hook(NULL) {}
|
1326
1335
|
template <> template <>
|
1327
1336
|
HookableSolver<SBAS_SinglePositioning<FloatT>, GPS_Solver<FloatT> >
|
1328
|
-
::HookableSolver
|
1337
|
+
::HookableSolver/*<SBAS_SpaceNode<FloatT> >*/(const SBAS_SpaceNode<FloatT> &sn)
|
1329
1338
|
: SBAS_SinglePositioning<FloatT>(sn), hook(NULL) {}
|
1330
1339
|
template <> template <>
|
1331
1340
|
HookableSolver<GLONASS_SinglePositioning<FloatT>, GPS_Solver<FloatT> >
|
1332
|
-
::HookableSolver
|
1341
|
+
::HookableSolver/*<GLONASS_SpaceNode<FloatT> >*/(const GLONASS_SpaceNode<FloatT> &sn)
|
1333
1342
|
: GLONASS_SinglePositioning<FloatT>(sn), hook(NULL) {}
|
1334
1343
|
template <>
|
1335
1344
|
GPS_Solver<FloatT>::base_t::relative_property_t
|
data/lib/gps_pvt/ntrip.rb
CHANGED
@@ -88,8 +88,8 @@ class Ntrip < Net::HTTP
|
|
88
88
|
}
|
89
89
|
req
|
90
90
|
end
|
91
|
-
def get_source_table(header = {})
|
92
|
-
Ntrip.parse_source_table(request(generate_request('/', header)).read_body)
|
91
|
+
def get_source_table(header = {}, &b)
|
92
|
+
(b || proc{|str| Ntrip.parse_source_table(str)}).call(request(generate_request('/', header)).read_body)
|
93
93
|
end
|
94
94
|
def get_data(mount_point, header = {}, &b)
|
95
95
|
request(generate_request("/#{mount_point}", header)){|res|
|
@@ -129,10 +129,16 @@ OpenURI.class_eval{
|
|
129
129
|
def OpenURI.open_ntrip(buf, target, proxy, options) # :nodoc:
|
130
130
|
GPS_PVT::Ntrip.start(target.host, target.port){|ntrip|
|
131
131
|
# get source table
|
132
|
-
tbl = ntrip.get_source_table(options)
|
132
|
+
tbl = ntrip.get_source_table(options){|str| str}
|
133
|
+
if target.root? then
|
134
|
+
buf << tbl
|
135
|
+
buf.io.rewind
|
136
|
+
next
|
137
|
+
end
|
138
|
+
tbl = GPS_PVT::Ntrip::parse_source_table(tbl)
|
133
139
|
|
134
140
|
# check mount point
|
135
|
-
mnt_pt = target.
|
141
|
+
mnt_pt = target.mount_point
|
136
142
|
prop = tbl.mount_points[mnt_pt]
|
137
143
|
raise Net::ProtocolError::new("Mount point(#{mnt_pt}) not found") unless prop
|
138
144
|
|
@@ -154,10 +160,23 @@ OpenURI.class_eval{
|
|
154
160
|
}
|
155
161
|
module URI
|
156
162
|
class Ntrip < HTTP
|
163
|
+
def root
|
164
|
+
res = self.clone
|
165
|
+
res.path = '/'
|
166
|
+
res
|
167
|
+
end
|
168
|
+
def root?; self.path == '/'; end
|
169
|
+
def mount_point
|
170
|
+
self.path.sub(%r|^/|, '')
|
171
|
+
end
|
172
|
+
|
157
173
|
def buffer_open(buf, proxy, options)
|
158
174
|
OpenURI.open_ntrip(buf, self, proxy, options)
|
159
175
|
end
|
160
176
|
include OpenURI::OpenRead
|
177
|
+
def read_source_table(options = {})
|
178
|
+
GPS_PVT::Ntrip::parse_source_table(self.root.read(options))
|
179
|
+
end
|
161
180
|
end
|
162
181
|
if respond_to?(:register_scheme) then
|
163
182
|
register_scheme('NTRIP', Ntrip)
|
@@ -0,0 +1,110 @@
|
|
1
|
+
=begin
|
2
|
+
RTCM3 handler for receiver
|
3
|
+
=end
|
4
|
+
|
5
|
+
module GPS_PVT
|
6
|
+
class Receiver
|
7
|
+
def parse_rtcm3(src, opt = {}, &b)
|
8
|
+
$stderr.print "Reading RTCM3 stream (%s) "%[src]
|
9
|
+
require_relative '../rtcm3'
|
10
|
+
src_io = open(src)
|
11
|
+
rtcm3 = GPS_PVT::RTCM3::new(src_io)
|
12
|
+
ref_time = case (ref_time = opt[:ref_time])
|
13
|
+
when GPS::Time;
|
14
|
+
when Time
|
15
|
+
t_array = ref_time.utc.to_a[0..5].reverse
|
16
|
+
GPS::Time::new(t_array, GPS::Time::guess_leap_seconds(t_array))
|
17
|
+
when nil; GPS::Time::now
|
18
|
+
else; raise "reference time (#{ref_time}) should be GPS::Time or Time"
|
19
|
+
end
|
20
|
+
leap_sec = ref_time.leap_seconds
|
21
|
+
after_run = b || proc{|pvt| puts pvt.to_s if pvt}
|
22
|
+
t_meas, meas = [nil, GPS::Measurement::new]
|
23
|
+
dt_threshold = GPS::Time::Seconds_week / 2
|
24
|
+
|
25
|
+
while packet = rtcm3.read_packet
|
26
|
+
msg_num = packet.message_number
|
27
|
+
parsed = packet.parse
|
28
|
+
case msg_num
|
29
|
+
when 1019, 1044
|
30
|
+
params = parsed.params
|
31
|
+
if msg_num == 1044
|
32
|
+
params[:svid] += 192
|
33
|
+
params[:fit_interval] ||= 2 * 60 * 60
|
34
|
+
end
|
35
|
+
params[:WN] += ((ref_time.week - params[:WN]).to_f / 1024).round * 1024
|
36
|
+
eph = GPS::Ephemeris::new
|
37
|
+
params.each{|k, v| eph.send("#{k}=".to_sym, v)}
|
38
|
+
critical{
|
39
|
+
@solver.gps_space_node.register_ephemeris(eph.svid, eph)
|
40
|
+
}
|
41
|
+
when 1020
|
42
|
+
params = parsed.params
|
43
|
+
eph = GPS::Ephemeris_GLONASS::new
|
44
|
+
params[:F_T] ||= 10 # [m]
|
45
|
+
params.each{|k, v|
|
46
|
+
next if [:P3, :NA, :N_4].include?(k)
|
47
|
+
eph.send("#{k}=".to_sym, v)
|
48
|
+
}
|
49
|
+
proc{|date_src|
|
50
|
+
eph.set_date(*(date_src.all? \
|
51
|
+
? date_src \
|
52
|
+
: [(ref_time + 3 * 60 * 60).c_tm(leap_sec)])) # UTC -> Moscow time
|
53
|
+
}.call([:N_4, :NA].collect{|k| params[k]})
|
54
|
+
eph.rehash(leap_sec)
|
55
|
+
critical{
|
56
|
+
@solver.glonass_space_node.register_ephemeris(eph.svid, eph)
|
57
|
+
}
|
58
|
+
when 1077, 1087, 1097, 1117
|
59
|
+
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
|
69
|
+
[{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
|
70
|
+
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
|
80
|
+
[{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
|
81
|
+
15 => [:L2CM, GPS::SpaceNode.L2_WaveLength],
|
82
|
+
16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, 192]
|
83
|
+
else; [{}, 0]
|
84
|
+
end
|
85
|
+
[:sat_sig, :pseudo_range, :phase_range, :phase_range_rate].collect{|k|
|
86
|
+
ranges[k]
|
87
|
+
}.transpose.each{|(svid, sig), pr, cpr, dr|
|
88
|
+
prefix, len = sig_list[sig]
|
89
|
+
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
|
94
|
+
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
|
98
|
+
}
|
99
|
+
else
|
100
|
+
#p({msg_num => parsed})
|
101
|
+
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
|
107
|
+
end
|
108
|
+
end
|
109
|
+
end
|
110
|
+
end
|