gps_pvt 0.6.4 → 0.7.2
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 +2 -1
- data/Rakefile +1 -0
- data/exe/gps_pvt +4 -2
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +993 -296
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +2 -3
- data/ext/ninja-scan-light/tool/navigation/ANTEX.h +1 -1
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +4 -11
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +25 -13
- data/ext/ninja-scan-light/tool/navigation/GPS.h +21 -18
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +42 -25
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +44 -23
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h +29 -1
- data/ext/ninja-scan-light/tool/navigation/MagneticField.h +4 -3
- data/ext/ninja-scan-light/tool/navigation/NTCM.h +1 -1
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +31 -20
- data/ext/ninja-scan-light/tool/navigation/RINEX_Clock.h +458 -0
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +34 -6
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +26 -14
- data/ext/ninja-scan-light/tool/navigation/SP3.h +51 -43
- data/ext/ninja-scan-light/tool/param/bit_array.h +2 -2
- data/ext/ninja-scan-light/tool/param/quaternion.h +2 -2
- data/ext/ninja-scan-light/tool/param/vector3.h +2 -2
- data/ext/ninja-scan-light/tool/swig/GPS.i +124 -52
- data/ext/ninja-scan-light/tool/swig/SylphideMath.i +1 -2
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +134 -13
- data/lib/gps_pvt/receiver.rb +32 -7
- data/lib/gps_pvt/version.rb +1 -1
- metadata +3 -2
@@ -76,6 +76,7 @@ class RINEX_Reader {
|
|
76
76
|
FTYPE_OBSERVATION,
|
77
77
|
FTYPE_NAVIGATION,
|
78
78
|
FTYPE_METEOROLOGICAL,
|
79
|
+
FTYPE_CLOCK,
|
79
80
|
} file_type;
|
80
81
|
enum sat_system_t {
|
81
82
|
SYS_UNKNOWN = 0,
|
@@ -89,6 +90,19 @@ class RINEX_Reader {
|
|
89
90
|
SYS_TRANSIT,
|
90
91
|
SYS_MIXED,
|
91
92
|
} sat_system;
|
93
|
+
static sat_system_t char2sys_v3(const char &c){
|
94
|
+
switch(c){
|
95
|
+
case 'G': return SYS_GPS;
|
96
|
+
case 'R': return SYS_GLONASS;
|
97
|
+
case 'E': return SYS_GALILEO;
|
98
|
+
case 'J': return SYS_QZSS;
|
99
|
+
case 'C': return SYS_BDS;
|
100
|
+
case 'I': return SYS_IRNSS;
|
101
|
+
case 'S': return SYS_SBAS;
|
102
|
+
case 'M': return SYS_MIXED;
|
103
|
+
}
|
104
|
+
return SYS_UNKNOWN;
|
105
|
+
}
|
92
106
|
version_type_t(
|
93
107
|
const int &ver = 0, const file_type_t &ftype = FTYPE_UNKNOWN, const sat_system_t &sys = SYS_UNKNOWN)
|
94
108
|
: version(ver), file_type(ftype), sat_system(sys) {}
|
@@ -105,7 +119,7 @@ class RINEX_Reader {
|
|
105
119
|
: src(in), _has_next(false),
|
106
120
|
version_type() {
|
107
121
|
if(src.fail()){return;}
|
108
|
-
|
122
|
+
|
109
123
|
char buf[256];
|
110
124
|
|
111
125
|
// Read header
|
@@ -135,7 +149,13 @@ class RINEX_Reader {
|
|
135
149
|
version_type.parse(it->second.front());
|
136
150
|
break;
|
137
151
|
}
|
138
|
-
|
152
|
+
}
|
153
|
+
RINEX_Reader(
|
154
|
+
std::istream &in,
|
155
|
+
void (RINEX_Reader::*read_header)())
|
156
|
+
: src(in), _has_next(false),
|
157
|
+
version_type() {
|
158
|
+
(this->*read_header)();
|
139
159
|
}
|
140
160
|
virtual ~RINEX_Reader(){_header.clear();}
|
141
161
|
header_t &header() {return _header;}
|
@@ -235,6 +255,7 @@ void RINEX_Reader<U>::version_type_t::parse(const std::string &buf){
|
|
235
255
|
case 'H': file_type = FTYPE_NAVIGATION;
|
236
256
|
sat_system = SYS_SBAS; // TODO: Is geostational as SBAS?
|
237
257
|
break;
|
258
|
+
case 'C': file_type = FTYPE_CLOCK; break;
|
238
259
|
}
|
239
260
|
break;
|
240
261
|
case 3:
|
@@ -242,18 +263,12 @@ void RINEX_Reader<U>::version_type_t::parse(const std::string &buf){
|
|
242
263
|
case 'O': file_type = FTYPE_OBSERVATION; break;
|
243
264
|
case 'N': file_type = FTYPE_NAVIGATION; break;
|
244
265
|
case 'M': file_type = FTYPE_METEOROLOGICAL; break;
|
266
|
+
case 'C': file_type = FTYPE_CLOCK; break;
|
245
267
|
}
|
246
|
-
if((file_type == FTYPE_OBSERVATION)
|
247
|
-
|
248
|
-
|
249
|
-
|
250
|
-
case 'E': sat_system = SYS_GALILEO; break;
|
251
|
-
case 'J': sat_system = SYS_QZSS; break;
|
252
|
-
case 'C': sat_system = SYS_BDS; break;
|
253
|
-
case 'I': sat_system = SYS_IRNSS; break;
|
254
|
-
case 'S': sat_system = SYS_SBAS; break;
|
255
|
-
case 'M': sat_system = SYS_MIXED; break;
|
256
|
-
}
|
268
|
+
if((file_type == FTYPE_OBSERVATION)
|
269
|
+
|| (file_type == FTYPE_NAVIGATION)
|
270
|
+
|| (file_type == FTYPE_CLOCK)){
|
271
|
+
sat_system = char2sys_v3(buf[40]);
|
257
272
|
}
|
258
273
|
break;
|
259
274
|
}
|
@@ -306,7 +321,7 @@ void RINEX_Reader<U>::version_type_t::dump(std::string &buf) const {
|
|
306
321
|
switch(sat_system){
|
307
322
|
case SYS_GPS: sys_str = "G: GPS"; break;
|
308
323
|
case SYS_GLONASS: sys_str = "R: GLONASS"; break;
|
309
|
-
case SYS_GALILEO: sys_str = "
|
324
|
+
case SYS_GALILEO: sys_str = "E: GALILEO"; break;
|
310
325
|
case SYS_QZSS: sys_str = "Q: QZSS"; break;
|
311
326
|
case SYS_BDS: sys_str = "B: BDS"; break;
|
312
327
|
case SYS_IRNSS: sys_str = "I: IRNSS"; break;
|
@@ -338,7 +353,6 @@ struct RINEX_NAV {
|
|
338
353
|
int t_oc_year4, t_oc_year2, t_oc_mon12;
|
339
354
|
FloatT t_oc_sec;
|
340
355
|
FloatT t_oe_WN;
|
341
|
-
FloatT ura_meter;
|
342
356
|
FloatT t_ot; ///< Transmitting time [s]
|
343
357
|
FloatT fit_interval_hr;
|
344
358
|
FloatT dummy;
|
@@ -352,7 +366,6 @@ struct RINEX_NAV {
|
|
352
366
|
t_oc_mon12(t_oc_tm.tm_mon + 1),
|
353
367
|
t_oc_sec(std::fmod(eph.t_oc, 60)),
|
354
368
|
t_oe_WN(eph.WN),
|
355
|
-
ura_meter(ephemeris_t::URA_meter(eph.URA)),
|
356
369
|
t_ot(0), // TODO
|
357
370
|
fit_interval_hr(eph.fit_interval / (60 * 60)),
|
358
371
|
dummy(0) {
|
@@ -363,8 +376,6 @@ struct RINEX_NAV {
|
|
363
376
|
eph.WN = t_oc.week;
|
364
377
|
eph.t_oc = t_oc.seconds;
|
365
378
|
|
366
|
-
eph.URA = ephemeris_t::URA_index(ura_meter); // meter to index
|
367
|
-
|
368
379
|
/* @see ftp://igs.org/pub/data/format/rinex210.txt
|
369
380
|
* 6.7 Satellite Health
|
370
381
|
* RINEX Value: 0 Health OK
|
@@ -1419,14 +1430,14 @@ const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>
|
|
1419
1430
|
|
1420
1431
|
template <class FloatT>
|
1421
1432
|
const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph6_v2[] = {
|
1422
|
-
GEN_E ( 3, 19, 12, message_t,
|
1433
|
+
GEN_E ( 3, 19, 12, message_t, eph.URA),
|
1423
1434
|
GEN_E2(22, 19, 12, message_t, eph.SV_health, unsigned int),
|
1424
1435
|
GEN_E (41, 19, 12, message_t, eph.t_GD),
|
1425
1436
|
GEN_E2(60, 19, 12, message_t, eph.iodc, int),
|
1426
1437
|
};
|
1427
1438
|
template <class FloatT>
|
1428
1439
|
const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph6_v3[] = {
|
1429
|
-
GEN_E ( 4, 19, 12, message_t,
|
1440
|
+
GEN_E ( 4, 19, 12, message_t, eph.URA),
|
1430
1441
|
GEN_E2(23, 19, 12, message_t, eph.SV_health, unsigned int),
|
1431
1442
|
GEN_E (42, 19, 12, message_t, eph.t_GD),
|
1432
1443
|
GEN_E2(61, 19, 12, message_t, eph.iodc, int),
|
@@ -0,0 +1,458 @@
|
|
1
|
+
/*
|
2
|
+
* Copyright (c) 2022, M.Naruoka (fenrir)
|
3
|
+
* All rights reserved.
|
4
|
+
*
|
5
|
+
* Redistribution and use in source and binary forms, with or without modification,
|
6
|
+
* are permitted provided that the following conditions are met:
|
7
|
+
*
|
8
|
+
* - Redistributions of source code must retain the above copyright notice,
|
9
|
+
* this list of conditions and the following disclaimer.
|
10
|
+
* - Redistributions in binary form must reproduce the above copyright notice,
|
11
|
+
* this list of conditions and the following disclaimer in the documentation
|
12
|
+
* and/or other materials provided with the distribution.
|
13
|
+
* - Neither the name of the naruoka.org nor the names of its contributors
|
14
|
+
* may be used to endorse or promote products derived from this software
|
15
|
+
* without specific prior written permission.
|
16
|
+
*
|
17
|
+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
18
|
+
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
19
|
+
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
20
|
+
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
|
21
|
+
* BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
|
22
|
+
* OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
23
|
+
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
24
|
+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
25
|
+
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
26
|
+
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
27
|
+
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
28
|
+
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
29
|
+
*
|
30
|
+
*/
|
31
|
+
|
32
|
+
/** @file
|
33
|
+
* @brief Loader of RINEX Clock extension, support versions are 3 including 3.04
|
34
|
+
*/
|
35
|
+
|
36
|
+
#ifndef __RINEX_CLOCK__
|
37
|
+
#define __RINEX_CLOCK__
|
38
|
+
|
39
|
+
#include <cstring>
|
40
|
+
#include <cctype>
|
41
|
+
#include <string>
|
42
|
+
|
43
|
+
#include "RINEX.h"
|
44
|
+
#include "GPS.h"
|
45
|
+
#include "algorithm/interpolate.h"
|
46
|
+
#include "GPS_Solver_Base.h"
|
47
|
+
|
48
|
+
template <class FloatT>
|
49
|
+
struct RINEX_CLK {
|
50
|
+
struct record_t {
|
51
|
+
char type[2], name[9];
|
52
|
+
std::tm epoch;
|
53
|
+
int epoch_year4, epoch_mon12;
|
54
|
+
FloatT epoch_sec;
|
55
|
+
int items_followed;
|
56
|
+
bool valid[6];
|
57
|
+
union {
|
58
|
+
FloatT array[6];
|
59
|
+
struct {
|
60
|
+
FloatT bias, bias_sigma, rate, rate_sigma, acc, acc_sigma;
|
61
|
+
} v;
|
62
|
+
} values;
|
63
|
+
|
64
|
+
GPS_Time<FloatT> get_epoch() const {
|
65
|
+
return GPS_Time<FloatT>(epoch) + (epoch_sec - epoch.tm_sec);
|
66
|
+
}
|
67
|
+
record_t &set_epoch(const GPS_Time<FloatT> &t){
|
68
|
+
epoch = t.c_tm();
|
69
|
+
epoch_year4 = epoch.tm_year + 1900;
|
70
|
+
epoch_mon12 = epoch.tm_mon + 1;
|
71
|
+
epoch_sec = std::fmod(t.seconds, 60);
|
72
|
+
return *this;
|
73
|
+
}
|
74
|
+
|
75
|
+
std::string get_name() const {
|
76
|
+
std::string res;
|
77
|
+
for(std::size_t i(0); i < sizeof(name); ++i){
|
78
|
+
if(!std::isgraph(name[i])){break;}
|
79
|
+
res.push_back(name[i]);
|
80
|
+
}
|
81
|
+
return res;
|
82
|
+
}
|
83
|
+
record_t &set_name(const std::string &str){
|
84
|
+
std::memset(name, ' ', sizeof(name));
|
85
|
+
str.copy(name, str.size() > sizeof(name) ? sizeof(name) : str.size());
|
86
|
+
return *this;
|
87
|
+
}
|
88
|
+
};
|
89
|
+
|
90
|
+
struct per_node_t {
|
91
|
+
std::map<GPS_Time<FloatT>, FloatT> bias;
|
92
|
+
typedef InterpolatableSet<GPS_Time<FloatT>, FloatT, FloatT> interpolator_t;
|
93
|
+
static const typename interpolator_t::condition_t interpolator_cnd_default;
|
94
|
+
mutable struct {
|
95
|
+
interpolator_t bias;
|
96
|
+
} subset;
|
97
|
+
bool precheck(const GPS_Time<FloatT> &t) const {
|
98
|
+
subset.bias.update(t, bias, interpolator_cnd_default);
|
99
|
+
return subset.bias.ready;
|
100
|
+
}
|
101
|
+
FloatT clock_error(const GPS_Time<FloatT> &t) const {
|
102
|
+
return subset.bias.update(t, bias, interpolator_cnd_default).interpolate(t);
|
103
|
+
}
|
104
|
+
FloatT clock_error_dot(const GPS_Time<FloatT> &t) const {
|
105
|
+
FloatT res;
|
106
|
+
subset.bias.update(t, bias, interpolator_cnd_default).interpolate(t, &res);
|
107
|
+
return res;
|
108
|
+
}
|
109
|
+
};
|
110
|
+
struct satellites_t {
|
111
|
+
typedef std::map<int, per_node_t> buf_t; // prn => per_node_t
|
112
|
+
buf_t buf;
|
113
|
+
|
114
|
+
enum system_t {
|
115
|
+
SYSTEM_GPS,
|
116
|
+
SYSTEM_SBAS,
|
117
|
+
SYSTEM_QZSS,
|
118
|
+
SYSTEM_GLONASS,
|
119
|
+
SYSTEM_GALILEO,
|
120
|
+
SYSTEM_BEIDOU,
|
121
|
+
SYSTEM_IRNSS,
|
122
|
+
NUM_OF_SYSTEMS,
|
123
|
+
};
|
124
|
+
|
125
|
+
static const int offset_list[NUM_OF_SYSTEMS];
|
126
|
+
|
127
|
+
protected:
|
128
|
+
|
129
|
+
typedef typename GPS_Solver_Base<FloatT>::satellite_t sat_t;
|
130
|
+
|
131
|
+
mutable struct per_system_t {
|
132
|
+
struct {
|
133
|
+
const satellites_t *sats;
|
134
|
+
} clk_rate;
|
135
|
+
struct {
|
136
|
+
const void *impl;
|
137
|
+
sat_t (*impl_select)(const void *, const int &, const GPS_Time<FloatT> &);
|
138
|
+
} pos_vel;
|
139
|
+
system_t sys;
|
140
|
+
} per_system[NUM_OF_SYSTEMS];
|
141
|
+
|
142
|
+
static sat_t select(
|
143
|
+
const void *ptr, const int &prn, const GPS_Time<FloatT> &receiver_time){
|
144
|
+
const per_system_t *ptr_sys(reinterpret_cast<const per_system_t *>(ptr));
|
145
|
+
|
146
|
+
// retrieve position/velocity supplier
|
147
|
+
sat_t res(
|
148
|
+
ptr_sys->pos_vel.impl_select(ptr_sys->pos_vel.impl, prn, receiver_time));
|
149
|
+
|
150
|
+
int sat_id((prn & 0xFF) + offset_list[ptr_sys->sys]);
|
151
|
+
|
152
|
+
// check clock_error/clock_error_dot availability
|
153
|
+
typename buf_t::const_iterator it(ptr_sys->clk_rate.sats->buf.find(sat_id));
|
154
|
+
if((it == ptr_sys->clk_rate.sats->buf.end()) || !it->second.precheck(receiver_time)){
|
155
|
+
// unavailable case
|
156
|
+
static const sat_t &unavailable(sat_t::unavailable());
|
157
|
+
res.impl_t = unavailable.impl_t;
|
158
|
+
res.impl_clock_error = unavailable.impl_clock_error;
|
159
|
+
res.impl_clock_error_dot = unavailable.impl_clock_error_dot;
|
160
|
+
return res;
|
161
|
+
}
|
162
|
+
|
163
|
+
struct impl_t {
|
164
|
+
static inline const per_node_t &sat(const void *ptr) {
|
165
|
+
return *reinterpret_cast<const per_node_t *>(ptr);
|
166
|
+
}
|
167
|
+
static typename GPS_Solver_Base<FloatT>::float_t clock_error(
|
168
|
+
const void *ptr, const GPS_Time<FloatT> &t_tx) {
|
169
|
+
return sat(ptr).clock_error(t_tx);
|
170
|
+
}
|
171
|
+
static typename GPS_Solver_Base<FloatT>::float_t clock_error_dot(
|
172
|
+
const void *ptr, const GPS_Time<FloatT> &t_tx) {
|
173
|
+
return sat(ptr).clock_error_dot(t_tx);
|
174
|
+
}
|
175
|
+
};
|
176
|
+
res.impl_t = &(it->second);
|
177
|
+
res.impl_clock_error = impl_t::clock_error;
|
178
|
+
res.impl_clock_error_dot = impl_t::clock_error_dot;
|
179
|
+
return res;
|
180
|
+
}
|
181
|
+
|
182
|
+
public:
|
183
|
+
|
184
|
+
template <class SelectorT>
|
185
|
+
bool push(SelectorT &slct, const system_t &sys = SYSTEM_GPS) const {
|
186
|
+
if(sys >= NUM_OF_SYSTEMS){return false;}
|
187
|
+
per_system[sys].clk_rate.sats = this;
|
188
|
+
per_system[sys].sys = sys;
|
189
|
+
if(slct.impl_select != select){
|
190
|
+
per_system[sys].pos_vel.impl = slct.impl;
|
191
|
+
per_system[sys].pos_vel.impl_select = slct.impl_select;
|
192
|
+
}
|
193
|
+
slct.impl_select = select;
|
194
|
+
slct.impl = &per_system[sys];
|
195
|
+
return true;
|
196
|
+
}
|
197
|
+
|
198
|
+
struct count_t {
|
199
|
+
int gps, sbas, qzss, glonass, galileo, beidou, irnss, unknown;
|
200
|
+
};
|
201
|
+
count_t count() const {
|
202
|
+
count_t res = {0};
|
203
|
+
for(typename buf_t::const_iterator it(buf.begin()), it_end(buf.end());
|
204
|
+
it != it_end; ++it){
|
205
|
+
switch((char)(it->first >> 8)){
|
206
|
+
case '\0': {
|
207
|
+
int id(it->first & 0xFF);
|
208
|
+
if(id < 100){++res.gps;}
|
209
|
+
else if(id < 192){++res.sbas;}
|
210
|
+
else{++res.qzss;}
|
211
|
+
break;
|
212
|
+
}
|
213
|
+
case 'R': ++res.glonass; break;
|
214
|
+
case 'E': ++res.galileo; break;
|
215
|
+
case 'C': ++res.beidou; break;
|
216
|
+
case 'I': ++res.irnss; break;
|
217
|
+
default: ++res.unknown; break;
|
218
|
+
}
|
219
|
+
}
|
220
|
+
return res;
|
221
|
+
}
|
222
|
+
};
|
223
|
+
typedef std::map<std::string, per_node_t> collection_t; // satellites + receivers, name => per_node_t
|
224
|
+
};
|
225
|
+
|
226
|
+
template <class FloatT>
|
227
|
+
const typename RINEX_CLK<FloatT>::per_node_t::interpolator_t::condition_t
|
228
|
+
RINEX_CLK<FloatT>::per_node_t::interpolator_cnd_default = {
|
229
|
+
5, // max_x_size
|
230
|
+
60 * 60 * 2, // subset.cnd.max_dx_range
|
231
|
+
};
|
232
|
+
|
233
|
+
template <class FloatT>
|
234
|
+
const int RINEX_CLK<FloatT>::satellites_t::offset_list[NUM_OF_SYSTEMS] = {
|
235
|
+
0, 0, 0, // GPS, SBAS, QZSS
|
236
|
+
(int)'R' << 8, // GLONASS
|
237
|
+
(int)'E' << 8, // GALILEO
|
238
|
+
(int)'C' << 8, // BEIDOU
|
239
|
+
(int)'I' << 8, // IRNSS,
|
240
|
+
};
|
241
|
+
|
242
|
+
template <class FloatT = double>
|
243
|
+
class RINEX_CLK_Reader : public RINEX_Reader<> {
|
244
|
+
protected:
|
245
|
+
typedef RINEX_Reader<> super_t;
|
246
|
+
public:
|
247
|
+
typedef typename RINEX_CLK<FloatT>::record_t record_t;
|
248
|
+
static const typename super_t::convert_item_t head_v3[7];
|
249
|
+
static const typename super_t::convert_item_t head_v304[7];
|
250
|
+
|
251
|
+
protected:
|
252
|
+
record_t record;
|
253
|
+
|
254
|
+
void read_header(){
|
255
|
+
if(super_t::src.fail()){return;}
|
256
|
+
|
257
|
+
char buf[256];
|
258
|
+
int label_offset(60);
|
259
|
+
|
260
|
+
// Read header
|
261
|
+
for(int line(0); !super_t::src.eof(); ++line){
|
262
|
+
super_t::src.getline(buf, sizeof(buf));
|
263
|
+
|
264
|
+
std::string content(buf);
|
265
|
+
|
266
|
+
if(line == 0){ // Check version and type extraction
|
267
|
+
switch(content.find("RINEX VERSION / TYPE")){
|
268
|
+
case 60:
|
269
|
+
super_t::version_type.parse(content);
|
270
|
+
break;
|
271
|
+
case 65: {
|
272
|
+
double temp(0);
|
273
|
+
super_t::conv_t<double>::f(content, 0, 4, &temp, 2);
|
274
|
+
super_t::version_type.version = (int)(temp * 100);
|
275
|
+
if(super_t::version_type.version != 304){return;}
|
276
|
+
if(content[21] != 'C'){return;}
|
277
|
+
super_t::version_type.file_type
|
278
|
+
= super_t::version_type_t::FTYPE_CLOCK;
|
279
|
+
super_t::version_type.sat_system
|
280
|
+
= super_t::version_type_t::char2sys_v3(content[42]);
|
281
|
+
label_offset = 65;
|
282
|
+
break;
|
283
|
+
}
|
284
|
+
default: // includes std::string::npos:
|
285
|
+
return;
|
286
|
+
}
|
287
|
+
}
|
288
|
+
|
289
|
+
std::string label(content, label_offset, 20);
|
290
|
+
{
|
291
|
+
int real_length(label.find_last_not_of(' ') + 1);
|
292
|
+
if(real_length < (int)label.length()){
|
293
|
+
label = label.substr(0, real_length);
|
294
|
+
}
|
295
|
+
}
|
296
|
+
|
297
|
+
if(label.find("END OF HEADER") == 0){break;}
|
298
|
+
|
299
|
+
_header[label].push_back(content.substr(0, label_offset));
|
300
|
+
}
|
301
|
+
}
|
302
|
+
|
303
|
+
public:
|
304
|
+
void seek_next() {
|
305
|
+
super_t::_has_next = false;
|
306
|
+
|
307
|
+
char buf[1024];
|
308
|
+
std::string str;
|
309
|
+
bool v304(super_t::version_type.version == 304);
|
310
|
+
while(true){
|
311
|
+
if(super_t::src.getline(buf, sizeof(buf)).fail()){return;}
|
312
|
+
str.assign(buf);
|
313
|
+
if(str.size() < (v304 ? 45 : 37)){continue;}
|
314
|
+
str.copy(record.type, 2);
|
315
|
+
if((std::memcmp(record.type, "AR", 2) == 0)
|
316
|
+
|| (std::memcmp(record.type, "AS", 2) == 0)
|
317
|
+
|| (std::memcmp(record.type, "CR", 2) == 0)
|
318
|
+
|| (std::memcmp(record.type, "DR", 2) == 0)
|
319
|
+
|| (std::memcmp(record.type, "MS", 2) == 0)){
|
320
|
+
break;
|
321
|
+
}
|
322
|
+
}
|
323
|
+
std::memset(record.name, ' ', sizeof(record.name));
|
324
|
+
str.copy(record.name, v304 ? 9 : 4, 3);
|
325
|
+
v304 ? super_t::convert(head_v304, str, &record) : super_t::convert(head_v3, str, &record);
|
326
|
+
record.epoch.tm_year = record.epoch_year4 - 1900; // greater than 1980
|
327
|
+
record.epoch.tm_mon = record.epoch_mon12 - 1; // month [0, 11]
|
328
|
+
record.epoch.tm_sec = (int)record.epoch_sec;
|
329
|
+
|
330
|
+
for(int i(0), j(2), k(v304 ? 45 : 40); i < 6; ++i, ++j, k += (v304 ? 21 : 20)){
|
331
|
+
record.valid[i] = false;
|
332
|
+
if(i >= record.items_followed){continue;}
|
333
|
+
if(j % 4 == 0){
|
334
|
+
j = 0; k = 3;
|
335
|
+
if(super_t::src.getline(buf, sizeof(buf)).fail()){break;}
|
336
|
+
str.assign(buf);
|
337
|
+
}
|
338
|
+
if(!super_t::template conv_t<FloatT>::e_dot_head(
|
339
|
+
str, k, 19, &record.values.array[i], 12)){break;}
|
340
|
+
record.valid[i] = true;
|
341
|
+
}
|
342
|
+
|
343
|
+
super_t::_has_next = true;
|
344
|
+
}
|
345
|
+
|
346
|
+
RINEX_CLK_Reader(std::istream &in)
|
347
|
+
: super_t(in, static_cast<void(super_t::*)()>(
|
348
|
+
&RINEX_CLK_Reader::read_header)) {
|
349
|
+
seek_next();
|
350
|
+
}
|
351
|
+
~RINEX_CLK_Reader(){}
|
352
|
+
|
353
|
+
void next() {
|
354
|
+
seek_next();
|
355
|
+
}
|
356
|
+
|
357
|
+
protected:
|
358
|
+
static int read_all(
|
359
|
+
std::istream &in,
|
360
|
+
void *buf, bool (*callback)(void *, const record_t &)){
|
361
|
+
RINEX_CLK_Reader reader(in);
|
362
|
+
if(reader.version_type.file_type != version_type_t::FTYPE_CLOCK){
|
363
|
+
return -1;
|
364
|
+
}
|
365
|
+
int res(0);
|
366
|
+
for(; reader.has_next(); reader.next()){
|
367
|
+
if(!reader.record.valid[0]){continue;}
|
368
|
+
if(!callback(buf, reader.record)){continue;}
|
369
|
+
++res;
|
370
|
+
}
|
371
|
+
return res;
|
372
|
+
}
|
373
|
+
|
374
|
+
public:
|
375
|
+
static int read_all(
|
376
|
+
std::istream &in,
|
377
|
+
typename RINEX_CLK<FloatT>::satellites_t &dst){
|
378
|
+
struct func_t {
|
379
|
+
static bool add(void *ptr, const record_t &record){
|
380
|
+
typename RINEX_CLK<FloatT>::satellites_t &dst(
|
381
|
+
*reinterpret_cast<typename RINEX_CLK<FloatT>::satellites_t *>(ptr));
|
382
|
+
if((record.type[0] != 'A') || (record.type[1] != 'S')){return false;}
|
383
|
+
int offset(0);
|
384
|
+
char sys(record.name[0]);
|
385
|
+
switch(sys){
|
386
|
+
case 'G': break; // GPS
|
387
|
+
case 'R': offset = (int)'R' << 8; break; // GLONASS
|
388
|
+
case 'E': offset = (int)'E' << 8; break; // Galileo
|
389
|
+
case 'C': offset = (int)'C' << 8; break; // BeiDou
|
390
|
+
case 'J': offset = 192; break; // QZSS
|
391
|
+
case 'S': offset = 100; break; // SBAS
|
392
|
+
case 'I': offset = (int)'I' << 8; break; // IRNSS
|
393
|
+
default: return false;
|
394
|
+
}
|
395
|
+
if(record.name[1] < '0' || record.name[1] > '9'
|
396
|
+
|| record.name[2] < '0' || record.name[2] > '9'){return false;}
|
397
|
+
int idx((record.name[1] - '0') * 10 + (record.name[2] - '0') + offset);
|
398
|
+
dst.buf[idx].bias[record.get_epoch()] = record.values.v.bias;
|
399
|
+
return true;
|
400
|
+
}
|
401
|
+
};
|
402
|
+
return read_all(in, &dst, func_t::add);
|
403
|
+
}
|
404
|
+
|
405
|
+
static int read_all(
|
406
|
+
std::istream &in,
|
407
|
+
typename RINEX_CLK<FloatT>::collection_t &dst){
|
408
|
+
struct func_t {
|
409
|
+
static bool add(void *ptr, const record_t &record){
|
410
|
+
typename RINEX_CLK<FloatT>::collection_t &dst(
|
411
|
+
*reinterpret_cast<typename RINEX_CLK<FloatT>::collection_t *>(ptr));
|
412
|
+
dst[record.get_name()].bias[record.get_epoch()] = record.values.v.bias;
|
413
|
+
return true;
|
414
|
+
}
|
415
|
+
};
|
416
|
+
return read_all(in, &dst, func_t::add);
|
417
|
+
}
|
418
|
+
};
|
419
|
+
|
420
|
+
#define GEN_D(offset, length, container_type, container_member, value_type) \
|
421
|
+
{super_t::template conv_t<value_type>::d, offset, length, \
|
422
|
+
offsetof(container_type, container_member)}
|
423
|
+
#define GEN_I(offset, length, container_type, container_member, value_type) \
|
424
|
+
{super_t::template conv_t<value_type>::d, offset, length, \
|
425
|
+
offsetof(container_type, container_member), 1}
|
426
|
+
#define GEN_F2(offset, length, precision, container_type, container_member, value_type) \
|
427
|
+
{super_t::template conv_t<value_type>::f_dot_head, offset, length, \
|
428
|
+
offsetof(container_type, container_member), precision}
|
429
|
+
#define GEN_F(offset, length, precision, container_type, container_member) \
|
430
|
+
GEN_F2(offset, length, precision, container_type, container_member, FloatT)
|
431
|
+
|
432
|
+
template <class FloatT>
|
433
|
+
const typename RINEX_CLK_Reader<FloatT>::convert_item_t RINEX_CLK_Reader<FloatT>::head_v3[] = {
|
434
|
+
GEN_I( 8, 4, record_t, epoch_year4, int),
|
435
|
+
GEN_I(13, 2, record_t, epoch_mon12, int),
|
436
|
+
GEN_I(16, 2, record_t, epoch.tm_mday, int),
|
437
|
+
GEN_I(19, 2, record_t, epoch.tm_hour, int),
|
438
|
+
GEN_I(22, 2, record_t, epoch.tm_min, int),
|
439
|
+
GEN_F(24, 10, 6, record_t, epoch_sec),
|
440
|
+
GEN_D(34, 3, record_t, items_followed, int),
|
441
|
+
};
|
442
|
+
|
443
|
+
template <class FloatT>
|
444
|
+
const typename RINEX_CLK_Reader<FloatT>::convert_item_t RINEX_CLK_Reader<FloatT>::head_v304[] = {
|
445
|
+
GEN_I(13, 4, record_t, epoch_year4, int),
|
446
|
+
GEN_I(18, 2, record_t, epoch_mon12, int),
|
447
|
+
GEN_I(21, 2, record_t, epoch.tm_mday, int),
|
448
|
+
GEN_I(24, 2, record_t, epoch.tm_hour, int),
|
449
|
+
GEN_I(27, 2, record_t, epoch.tm_min, int),
|
450
|
+
GEN_F(29, 9, 6, record_t, epoch_sec),
|
451
|
+
GEN_D(40, 2, record_t, items_followed, int),
|
452
|
+
};
|
453
|
+
|
454
|
+
#undef GEN_D
|
455
|
+
#undef GEN_I
|
456
|
+
#undef GEN_F
|
457
|
+
|
458
|
+
#endif /* __RINEX_CLOCK__ */
|
@@ -1387,16 +1387,16 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1387
1387
|
|
1388
1388
|
// the first line
|
1389
1389
|
out << " ";
|
1390
|
-
for(
|
1390
|
+
for(std::size_t j(0); j < sizeof(igp.properties[0]) / sizeof(igp.properties[0][0]); ++j){
|
1391
1391
|
int lng(position_index_t::idx2lng(j));
|
1392
1392
|
out << (lng == 0 ? '0' : (lng % 90 == 0 ? '|' : (lng % 30 == 0 ? '+' : ' ')));
|
1393
1393
|
}
|
1394
1394
|
out << std::endl;
|
1395
1395
|
|
1396
|
-
for(
|
1396
|
+
for(std::size_t i(0); i < sizeof(igp.properties) / sizeof(igp.properties[0]); ++i){
|
1397
1397
|
out.width(4);
|
1398
1398
|
out << position_index_t::idx2lat(i) << ' ';
|
1399
|
-
for(
|
1399
|
+
for(std::size_t j(0); j < sizeof(igp.properties[0]) / sizeof(igp.properties[0][0]); ++j){
|
1400
1400
|
if(!igp.properties[i][j].is_available()){
|
1401
1401
|
out << ' ';
|
1402
1402
|
}else{
|
@@ -1711,7 +1711,7 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1711
1711
|
uint_t WN; ///< Week number
|
1712
1712
|
|
1713
1713
|
float_t t_0; ///< Time of applicability (s) <= time of a week
|
1714
|
-
|
1714
|
+
float_t URA; ///< User range accuracy (m)
|
1715
1715
|
float_t x, y, z; ///< ECEF position (m)
|
1716
1716
|
float_t dx, dy, dz; ///< ECEF velocity (m/s)
|
1717
1717
|
float_t ddx, ddy, ddz; ///< ECEF acceleration (m/s^2)
|
@@ -1804,6 +1804,15 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1804
1804
|
return res;
|
1805
1805
|
}
|
1806
1806
|
|
1807
|
+
static const float_t URA_table[15];
|
1808
|
+
|
1809
|
+
inline static float_t URA_meter(const int_t &index){
|
1810
|
+
return gps_space_node_t::SatelliteProperties::Ephemeris::URA_meter(index, URA_table);
|
1811
|
+
}
|
1812
|
+
inline static int_t URA_index(const float_t &meter){
|
1813
|
+
return gps_space_node_t::SatelliteProperties::Ephemeris::URA_index(meter, URA_table);
|
1814
|
+
}
|
1815
|
+
|
1807
1816
|
struct raw_t {
|
1808
1817
|
u8_t svid; ///< Satellite number
|
1809
1818
|
|
@@ -1850,7 +1859,7 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1850
1859
|
converted.svid = svid;
|
1851
1860
|
converted.WN = 0; // Week number (must be configured later) @see adjust_time
|
1852
1861
|
|
1853
|
-
converted.URA = URA;
|
1862
|
+
converted.URA = URA_meter(URA);
|
1854
1863
|
CONVERT(t_0); // Time of a day => time of a week (must be configured later) @see adjust_time
|
1855
1864
|
CONVERT2(x, xy); CONVERT2(y, xy); CONVERT(z);
|
1856
1865
|
CONVERT2(dx, dxy); CONVERT2(dy, dxy); CONVERT(dz);
|
@@ -1869,7 +1878,7 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1869
1878
|
#define CONVERT(TARGET) CONVERT2(TARGET, TARGET)
|
1870
1879
|
svid = eph.svid;
|
1871
1880
|
|
1872
|
-
URA = eph.URA;
|
1881
|
+
URA = URA_index(eph.URA);
|
1873
1882
|
CONVERT3(t_0, std::fmod(t_0, gps_time_t::seconds_day), t_0);
|
1874
1883
|
CONVERT2(x, xy); CONVERT2(y, xy); CONVERT(z);
|
1875
1884
|
CONVERT2(dx, dxy); CONVERT2(dy, dxy); CONVERT(dz);
|
@@ -2350,4 +2359,23 @@ const typename SBAS_SpaceNode<FloatT>::float_t SBAS_SpaceNode<FloatT>::UTC_Param
|
|
2350
2359
|
|
2351
2360
|
#undef POWER_2
|
2352
2361
|
|
2362
|
+
template <class FloatT>
|
2363
|
+
const typename SBAS_SpaceNode<FloatT>::float_t SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::URA_table[] = {
|
2364
|
+
2,
|
2365
|
+
2.8,
|
2366
|
+
4,
|
2367
|
+
5.7,
|
2368
|
+
8,
|
2369
|
+
11.3,
|
2370
|
+
16,
|
2371
|
+
32,
|
2372
|
+
64,
|
2373
|
+
128,
|
2374
|
+
256,
|
2375
|
+
512,
|
2376
|
+
1024,
|
2377
|
+
2048,
|
2378
|
+
4096,
|
2379
|
+
};
|
2380
|
+
|
2353
2381
|
#endif /* __SBAS_H__ */
|