gps_pvt 0.4.1 → 0.6.0
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/README.md +11 -6
- data/Rakefile +42 -23
- data/exe/gps_pvt +47 -8
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +44 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +9198 -7614
- data/ext/ninja-scan-light/tool/algorithm/interpolate.h +121 -0
- data/ext/ninja-scan-light/tool/navigation/ANTEX.h +747 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +63 -43
- data/ext/ninja-scan-light/tool/navigation/GPS.h +92 -4
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +58 -34
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +50 -17
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +19 -144
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +19 -4
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +63 -43
- data/ext/ninja-scan-light/tool/navigation/SP3.h +1178 -0
- data/ext/ninja-scan-light/tool/navigation/coordinate.h +81 -1
- data/ext/ninja-scan-light/tool/swig/GPS.i +131 -43
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +503 -16
- data/ext/ninja-scan-light/tool/util/text_helper.h +248 -0
- data/{sig/gps_pvt.rbs → gps_pvt.rbs} +0 -0
- data/lib/gps_pvt/receiver.rb +32 -5
- data/lib/gps_pvt/util.rb +50 -0
- data/lib/gps_pvt/version.rb +1 -1
- metadata +8 -3
@@ -0,0 +1,1178 @@
|
|
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 SP3 Reader/Writer, support ver.D
|
34
|
+
*
|
35
|
+
*/
|
36
|
+
|
37
|
+
#ifndef __SP3_H__
|
38
|
+
#define __SP3_H__
|
39
|
+
|
40
|
+
#include <ctime>
|
41
|
+
#include <cstring>
|
42
|
+
#include <map>
|
43
|
+
#include <set>
|
44
|
+
#include <algorithm>
|
45
|
+
#include <stdexcept>
|
46
|
+
|
47
|
+
#include "util/text_helper.h"
|
48
|
+
#include "GPS.h"
|
49
|
+
#include "GPS_Solver_Base.h"
|
50
|
+
|
51
|
+
#include "param/vector3.h"
|
52
|
+
#include "algorithm/interpolate.h"
|
53
|
+
|
54
|
+
template <class FloatT>
|
55
|
+
struct SP3_Product {
|
56
|
+
struct prop_t {
|
57
|
+
Vector3<FloatT> xyz;
|
58
|
+
FloatT clk;
|
59
|
+
};
|
60
|
+
struct per_satellite_t {
|
61
|
+
typedef std::map<GPS_Time<FloatT>, prop_t> history_t;
|
62
|
+
history_t pos_history;
|
63
|
+
history_t vel_history;
|
64
|
+
|
65
|
+
static const struct interpolate_cnd_t {
|
66
|
+
std::size_t max_epochs; ///< maximum number of epochs used for interpolation
|
67
|
+
FloatT max_delta_t; ///< maximum acceptable absolute time difference between t and epoch
|
68
|
+
} interpolate_cnd_default;
|
69
|
+
|
70
|
+
mutable struct {
|
71
|
+
struct target_t {
|
72
|
+
|
73
|
+
typedef typename std::vector<std::pair<GPS_Time<FloatT>, prop_t> > buf_t;
|
74
|
+
buf_t buf;
|
75
|
+
GPS_Time<FloatT> t0;
|
76
|
+
std::vector<FloatT> dt;
|
77
|
+
bool updated_full_cnd;
|
78
|
+
|
79
|
+
target_t() : t0(0, 0), updated_full_cnd(false) {}
|
80
|
+
|
81
|
+
/**
|
82
|
+
* update interpolation source
|
83
|
+
* @param force_update If true, update is forcibly performed irrespective of current state
|
84
|
+
* @param cnd condition for source data selection
|
85
|
+
*/
|
86
|
+
target_t &update(
|
87
|
+
const GPS_Time<FloatT> &t, const history_t &history,
|
88
|
+
const bool &force_update = false,
|
89
|
+
const interpolate_cnd_t &cnd = interpolate_cnd_default){
|
90
|
+
|
91
|
+
FloatT t_diff(t0 - t);
|
92
|
+
if((!force_update)
|
93
|
+
&& ((std::abs(t_diff) <= 10)
|
94
|
+
|| ((dt.size() >= 2)
|
95
|
+
&& (std::abs(t_diff + dt[0]) <= std::abs(t_diff + dt[1])))) ){
|
96
|
+
return *this;
|
97
|
+
}
|
98
|
+
|
99
|
+
// If the 1st and 2nd nearest epochs are changed, then recalculate interpolation targets.
|
100
|
+
struct {
|
101
|
+
const GPS_Time<FloatT> &t_base;
|
102
|
+
bool operator()(
|
103
|
+
const typename history_t::value_type &rhs,
|
104
|
+
const typename history_t::value_type &lhs) const {
|
105
|
+
return std::abs(rhs.first - t_base) < std::abs(lhs.first - t_base);
|
106
|
+
}
|
107
|
+
} cmp = {(t0 = t)};
|
108
|
+
|
109
|
+
buf.resize(cnd.max_epochs);
|
110
|
+
dt.clear();
|
111
|
+
// extract t where t0-dt <= t <= t0+dt, then sort by ascending order of |t-t0|
|
112
|
+
for(typename buf_t::const_iterator
|
113
|
+
it(buf.begin()),
|
114
|
+
it_end(std::partial_sort_copy(
|
115
|
+
history.lower_bound(t - cnd.max_delta_t),
|
116
|
+
history.upper_bound(t + cnd.max_delta_t),
|
117
|
+
buf.begin(), buf.end(), cmp));
|
118
|
+
it != it_end; ++it){
|
119
|
+
dt.push_back(it->first - t0);
|
120
|
+
}
|
121
|
+
updated_full_cnd = (dt.size() >= cnd.max_epochs);
|
122
|
+
|
123
|
+
return *this;
|
124
|
+
}
|
125
|
+
|
126
|
+
template <class Ty, class Ty_Array>
|
127
|
+
Ty interpolate(
|
128
|
+
const GPS_Time<FloatT> &t, const Ty_Array &y,
|
129
|
+
Ty *derivative = NULL) const {
|
130
|
+
int order(dt.size() - 1);
|
131
|
+
do{
|
132
|
+
if(order > 0){break;}
|
133
|
+
if((order == 0) && (!derivative)){return y[0];}
|
134
|
+
throw std::range_error("Insufficient records for interpolation");
|
135
|
+
}while(false);
|
136
|
+
std::vector<Ty> y_buf(order), dy_buf(order);
|
137
|
+
interpolate_Neville(
|
138
|
+
dt, y, t - t0, y_buf, order,
|
139
|
+
&dy_buf, derivative ? 1 : 0);
|
140
|
+
if(derivative){*derivative = dy_buf[0];}
|
141
|
+
return y_buf[0];
|
142
|
+
}
|
143
|
+
Vector3<FloatT> interpolate_xyz(
|
144
|
+
const GPS_Time<FloatT> &t,
|
145
|
+
Vector3<FloatT> *derivative = NULL) const {
|
146
|
+
struct second_iterator : public buf_t::const_iterator {
|
147
|
+
second_iterator(const typename buf_t::const_iterator &it)
|
148
|
+
: buf_t::const_iterator(it) {}
|
149
|
+
const Vector3<FloatT> &operator[](const int &n) const {
|
150
|
+
return buf_t::const_iterator::operator[](n).second.xyz;
|
151
|
+
}
|
152
|
+
} xyz(buf.begin());
|
153
|
+
return interpolate(t, xyz, derivative);
|
154
|
+
}
|
155
|
+
FloatT interpolate_clk(
|
156
|
+
const GPS_Time<FloatT> &t,
|
157
|
+
FloatT *derivative = NULL) const {
|
158
|
+
struct second_iterator : public buf_t::const_iterator {
|
159
|
+
second_iterator(const typename buf_t::const_iterator &it)
|
160
|
+
: buf_t::const_iterator(it) {}
|
161
|
+
const FloatT &operator[](const int &n) const {
|
162
|
+
return buf_t::const_iterator::operator[](n).second.clk;
|
163
|
+
}
|
164
|
+
} clk(buf.begin());
|
165
|
+
return interpolate(t, clk, derivative);
|
166
|
+
}
|
167
|
+
} pos_clk, vel_rate;
|
168
|
+
} subset;
|
169
|
+
|
170
|
+
/**
|
171
|
+
* Precheck whether interpolate can be fully performed or not
|
172
|
+
*
|
173
|
+
* @return (bool) If interpolation condition is fully satisfied, then true is returned.
|
174
|
+
*/
|
175
|
+
bool precheck(const GPS_Time<FloatT> &t) const {
|
176
|
+
// Only position/clock is checked, because velocity/rate can be calculated based on pos/clk.
|
177
|
+
subset.pos_clk.update(t, pos_history);
|
178
|
+
return subset.pos_clk.updated_full_cnd;
|
179
|
+
}
|
180
|
+
|
181
|
+
typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t
|
182
|
+
constellation(
|
183
|
+
const GPS_Time<FloatT> &t,
|
184
|
+
bool with_velocity = true) const {
|
185
|
+
typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t res;
|
186
|
+
if(with_velocity){
|
187
|
+
try{
|
188
|
+
res.velocity = subset.vel_rate.update(t, vel_history).interpolate_xyz(t);
|
189
|
+
with_velocity = false;
|
190
|
+
}catch(std::range_error &){}
|
191
|
+
}
|
192
|
+
Vector3<FloatT> vel;
|
193
|
+
res.position = subset.pos_clk.update(t, pos_history)
|
194
|
+
.interpolate_xyz(t, with_velocity ? &vel : NULL); // velocity fallback to use position
|
195
|
+
if(with_velocity){res.velocity = vel;}
|
196
|
+
return res;
|
197
|
+
}
|
198
|
+
typename GPS_SpaceNode<FloatT>::xyz_t position(
|
199
|
+
const GPS_Time<FloatT> &t) const {
|
200
|
+
return constellation(t, false).position;
|
201
|
+
}
|
202
|
+
typename GPS_SpaceNode<FloatT>::xyz_t velocity(
|
203
|
+
const GPS_Time<FloatT> &t) const {
|
204
|
+
return constellation(t, true).velocity;
|
205
|
+
}
|
206
|
+
FloatT clock_error(const GPS_Time<FloatT> &t) const {
|
207
|
+
return subset.pos_clk.update(t, pos_history).interpolate_clk(t);
|
208
|
+
}
|
209
|
+
FloatT clock_error_dot(const GPS_Time<FloatT> &t) const {
|
210
|
+
try{
|
211
|
+
return subset.vel_rate.update(t, vel_history).interpolate_clk(t);
|
212
|
+
}catch(std::range_error &){
|
213
|
+
FloatT res;
|
214
|
+
subset.pos_clk.update(t, pos_history).interpolate_clk(t, &res);
|
215
|
+
return res;
|
216
|
+
}
|
217
|
+
}
|
218
|
+
|
219
|
+
operator typename GPS_Solver_Base<FloatT>::satellite_t() const {
|
220
|
+
typedef typename GPS_Solver_Base<FloatT>::gps_time_t gt_t;
|
221
|
+
typedef typename GPS_Solver_Base<FloatT>::float_t float_t;
|
222
|
+
typedef typename GPS_Solver_Base<FloatT>::xyz_t xyz_t;
|
223
|
+
struct impl_t {
|
224
|
+
static inline const per_satellite_t &sat(const void *ptr) {
|
225
|
+
return *reinterpret_cast<const per_satellite_t *>(ptr);
|
226
|
+
}
|
227
|
+
static inline float_t pr2sec(const float_t &pr){
|
228
|
+
return pr / GPS_SpaceNode<FloatT>::light_speed;
|
229
|
+
}
|
230
|
+
static xyz_t position(const void *ptr, const gt_t &t, const float_t &pr) {
|
231
|
+
float_t delta_t(pr2sec(pr));
|
232
|
+
return sat(ptr).position(t - delta_t).after(delta_t);
|
233
|
+
}
|
234
|
+
static xyz_t velocity(const void *ptr, const gt_t &t, const float_t &pr) {
|
235
|
+
float_t delta_t(pr2sec(pr));
|
236
|
+
return sat(ptr).velocity(t - delta_t).after(delta_t);
|
237
|
+
}
|
238
|
+
static float_t clock_error(const void *ptr, const gt_t &t, const float_t &pr) {
|
239
|
+
return sat(ptr).clock_error(t - pr2sec(pr));
|
240
|
+
}
|
241
|
+
static float_t clock_error_dot(const void *ptr, const gt_t &t, const float_t &pr) {
|
242
|
+
return sat(ptr).clock_error_dot(t - pr2sec(pr));
|
243
|
+
}
|
244
|
+
};
|
245
|
+
typename GPS_Solver_Base<FloatT>::satellite_t res = {
|
246
|
+
this,
|
247
|
+
impl_t::position, impl_t::velocity,
|
248
|
+
impl_t::clock_error, impl_t::clock_error_dot
|
249
|
+
};
|
250
|
+
return res;
|
251
|
+
}
|
252
|
+
};
|
253
|
+
typedef std::map<int, per_satellite_t> satellites_t;
|
254
|
+
satellites_t satellites;
|
255
|
+
|
256
|
+
typedef std::set<GPS_Time<FloatT> > epochs_t;
|
257
|
+
epochs_t epochs() const {
|
258
|
+
epochs_t res;
|
259
|
+
struct time_iterator : public per_satellite_t::history_t::const_iterator {
|
260
|
+
time_iterator(
|
261
|
+
const typename per_satellite_t::history_t::const_iterator &it)
|
262
|
+
: per_satellite_t::history_t::const_iterator(it) {}
|
263
|
+
GPS_Time<FloatT> operator*() const {
|
264
|
+
return (*this)->first;
|
265
|
+
}
|
266
|
+
};
|
267
|
+
for(typename satellites_t::const_iterator
|
268
|
+
it(satellites.begin()), it_end(satellites.end());
|
269
|
+
it != it_end; ++it){
|
270
|
+
res.insert(
|
271
|
+
time_iterator(it->second.pos_history.begin()),
|
272
|
+
time_iterator(it->second.pos_history.end()));
|
273
|
+
res.insert(
|
274
|
+
time_iterator(it->second.vel_history.begin()),
|
275
|
+
time_iterator(it->second.vel_history.end()));
|
276
|
+
}
|
277
|
+
return res;
|
278
|
+
}
|
279
|
+
|
280
|
+
bool has_position() const {
|
281
|
+
for(typename satellites_t::const_iterator
|
282
|
+
it(satellites.begin()), it_end(satellites.end());
|
283
|
+
it != it_end; ++it){
|
284
|
+
if(!it->second.pos_history.empty()){return true;};
|
285
|
+
}
|
286
|
+
return false;
|
287
|
+
}
|
288
|
+
bool has_velocity() const {
|
289
|
+
for(typename satellites_t::const_iterator
|
290
|
+
it(satellites.begin()), it_end(satellites.end());
|
291
|
+
it != it_end; ++it){
|
292
|
+
if(!it->second.vel_history.empty()){return true;};
|
293
|
+
}
|
294
|
+
return false;
|
295
|
+
}
|
296
|
+
|
297
|
+
typename GPS_Solver_Base<FloatT>::satellite_t select(
|
298
|
+
const int &sat_id, const GPS_Time<FloatT> &t) const {
|
299
|
+
do{
|
300
|
+
typename satellites_t::const_iterator it(satellites.find(sat_id));
|
301
|
+
if(it == satellites.end()){break;}
|
302
|
+
if(!it->second.precheck(t)){break;}
|
303
|
+
return it->second;
|
304
|
+
}while(false);
|
305
|
+
return GPS_Solver_Base<FloatT>::satellite_t::unavailable();
|
306
|
+
}
|
307
|
+
|
308
|
+
enum system_t {
|
309
|
+
SYSTEM_GPS = (int)'\0' << 8,
|
310
|
+
SYSTEM_SBAS = SYSTEM_GPS,
|
311
|
+
SYSTEM_QZSS = SYSTEM_GPS,
|
312
|
+
SYSTEM_GLONASS = (int)'R' << 8,
|
313
|
+
SYSTEM_LEO = (int)'L' << 8,
|
314
|
+
SYSTEM_GALILEO = (int)'E' << 8,
|
315
|
+
SYSTEM_BEIDOU = (int)'C' << 8,
|
316
|
+
SYSTEM_IRNSS = (int)'I' << 8,
|
317
|
+
};
|
318
|
+
|
319
|
+
#define gen_func(sys) \
|
320
|
+
static typename GPS_Solver_Base<FloatT>::satellite_t select_ ## sys( \
|
321
|
+
const void *ptr, const int &prn, const GPS_Time<FloatT> &receiver_time){ \
|
322
|
+
return reinterpret_cast<const SP3_Product<FloatT> *>(ptr) \
|
323
|
+
->select(prn + SYSTEM_ ## sys, receiver_time); \
|
324
|
+
}
|
325
|
+
gen_func(GPS);
|
326
|
+
gen_func(GLONASS);
|
327
|
+
gen_func(LEO);
|
328
|
+
gen_func(GALILEO);
|
329
|
+
gen_func(BEIDOU);
|
330
|
+
gen_func(IRNSS);
|
331
|
+
#undef gen_fun
|
332
|
+
|
333
|
+
/**
|
334
|
+
* push SP3 product to satellite selector
|
335
|
+
*
|
336
|
+
* @param slct satellite selector having impl and impl_select members
|
337
|
+
* @param sys target system, default is GPS
|
338
|
+
* @return (bool) If push is successfully performed, true will be returned.
|
339
|
+
*/
|
340
|
+
template <class SelectorT>
|
341
|
+
bool push(SelectorT &slct, const system_t &sys = SYSTEM_GPS) const {
|
342
|
+
switch(sys){
|
343
|
+
case SYSTEM_GPS: // SBAS and QZSS are identically treated as GPS.
|
344
|
+
//case SYSTEM_SBAS:
|
345
|
+
//case SYSTEM_QZSS:
|
346
|
+
slct.impl_select = select_GPS; break;
|
347
|
+
case SYSTEM_GLONASS: slct.impl_select = select_GLONASS; break;
|
348
|
+
case SYSTEM_LEO: slct.impl_select = select_LEO; break;
|
349
|
+
case SYSTEM_GALILEO: slct.impl_select = select_GALILEO; break;
|
350
|
+
case SYSTEM_BEIDOU: slct.impl_select = select_BEIDOU; break;
|
351
|
+
case SYSTEM_IRNSS: slct.impl_select = select_IRNSS; break;
|
352
|
+
default: return false;
|
353
|
+
}
|
354
|
+
slct.impl = this;
|
355
|
+
return true;
|
356
|
+
}
|
357
|
+
|
358
|
+
struct satellite_count_t {
|
359
|
+
int gps, sbas, qzss, glonass, leo, galileo, beidou, irnss, unknown;
|
360
|
+
};
|
361
|
+
satellite_count_t satellite_count() const {
|
362
|
+
satellite_count_t res = {0};
|
363
|
+
for(typename satellites_t::const_iterator
|
364
|
+
it(satellites.begin()), it_end(satellites.end());
|
365
|
+
it != it_end; ++it){
|
366
|
+
switch(it->first & 0xFF00){
|
367
|
+
case SYSTEM_GPS: {
|
368
|
+
int id(it->first & 0xFF);
|
369
|
+
if(id < 100){++res.gps;}
|
370
|
+
else if(id < 192){++res.sbas;}
|
371
|
+
else{++res.qzss;}
|
372
|
+
break;
|
373
|
+
}
|
374
|
+
case SYSTEM_GLONASS: ++res.glonass; break;
|
375
|
+
case SYSTEM_LEO: ++res.leo; break;
|
376
|
+
case SYSTEM_GALILEO: ++res.galileo; break;
|
377
|
+
case SYSTEM_BEIDOU: ++res.beidou; break;
|
378
|
+
case SYSTEM_IRNSS: ++res.irnss; break;
|
379
|
+
default: ++res.unknown; break;
|
380
|
+
}
|
381
|
+
}
|
382
|
+
return res;
|
383
|
+
}
|
384
|
+
};
|
385
|
+
|
386
|
+
template <class FloatT>
|
387
|
+
const typename SP3_Product<FloatT>::per_satellite_t::interpolate_cnd_t
|
388
|
+
SP3_Product<FloatT>::per_satellite_t::interpolate_cnd_default = {
|
389
|
+
9, // max_epochs
|
390
|
+
60 * 60 * 2, // max_delta_t, default is 2 hr = 15 min interval records; (2 hr * 2 / (9 - 1) = 15 min)
|
391
|
+
};
|
392
|
+
|
393
|
+
template <class FloatT>
|
394
|
+
class SP3_Reader {
|
395
|
+
protected:
|
396
|
+
typename TextHelper<>::crlf_stream_t src;
|
397
|
+
public:
|
398
|
+
struct l1_t {
|
399
|
+
char version_symbol[2];
|
400
|
+
char pos_or_vel_flag[1];
|
401
|
+
int year_start;
|
402
|
+
int month_start;
|
403
|
+
int day_of_month_st;
|
404
|
+
int hour_start;
|
405
|
+
int minute_start;
|
406
|
+
FloatT second_start;
|
407
|
+
int number_of_epochs;
|
408
|
+
char data_used[5];
|
409
|
+
char coordinate_sys[5];
|
410
|
+
char orbit_type[3];
|
411
|
+
char agency[4];
|
412
|
+
l1_t &operator=(const GPS_Time<FloatT> &t) {
|
413
|
+
std::tm t_(t.c_tm());
|
414
|
+
year_start = t_.tm_year + 1900;
|
415
|
+
month_start = t_.tm_mon + 1;
|
416
|
+
day_of_month_st = t_.tm_mday;
|
417
|
+
hour_start = t_.tm_hour;
|
418
|
+
minute_start = t_.tm_min;
|
419
|
+
second_start = (t.seconds - (int)t.seconds) + t_.tm_sec;
|
420
|
+
return *this;
|
421
|
+
}
|
422
|
+
};
|
423
|
+
struct l2_t {
|
424
|
+
char symbols[2];
|
425
|
+
int gps_week;
|
426
|
+
FloatT seconds_of_week;
|
427
|
+
FloatT epoch_interval;
|
428
|
+
int mod_jul_day_st;
|
429
|
+
FloatT fractional_day;
|
430
|
+
};
|
431
|
+
struct l3_11_t {
|
432
|
+
char symbols[2];
|
433
|
+
int number_of_sats;
|
434
|
+
int sat_id[17];
|
435
|
+
};
|
436
|
+
struct l12_20_t {
|
437
|
+
char symbols[2];
|
438
|
+
int sat_accuracy[17];
|
439
|
+
};
|
440
|
+
struct l21_22_t {
|
441
|
+
char symbols[2];
|
442
|
+
char file_type[2];
|
443
|
+
char _2_characters[2];
|
444
|
+
char time_system[3];
|
445
|
+
char _3_characters[3];
|
446
|
+
char _4_characters[4][4];
|
447
|
+
char _5_characters[4][5];
|
448
|
+
};
|
449
|
+
struct l23_24_t {
|
450
|
+
char symbols[2];
|
451
|
+
FloatT base_for_pos_vel;
|
452
|
+
FloatT base_for_clk_rate;
|
453
|
+
FloatT _14_column_float;
|
454
|
+
FloatT _18_column_float;
|
455
|
+
};
|
456
|
+
struct l25_26_t {
|
457
|
+
char symbols[2];
|
458
|
+
int _4_column_int[4];
|
459
|
+
int _6_column_int[4];
|
460
|
+
int _9_column_int;
|
461
|
+
};
|
462
|
+
struct comment_t {
|
463
|
+
char symbols[2];
|
464
|
+
char comment[77];
|
465
|
+
};
|
466
|
+
struct epoch_t {
|
467
|
+
char symbols[2];
|
468
|
+
int year_start;
|
469
|
+
int month_start;
|
470
|
+
int day_of_month_st;
|
471
|
+
int hour_start;
|
472
|
+
int minute_start;
|
473
|
+
FloatT second_start;
|
474
|
+
std::tm c_tm() const {
|
475
|
+
std::tm res = {
|
476
|
+
(int)second_start,
|
477
|
+
minute_start,
|
478
|
+
hour_start,
|
479
|
+
day_of_month_st,
|
480
|
+
month_start - 1,
|
481
|
+
year_start - 1900,
|
482
|
+
};
|
483
|
+
std::mktime(&res);
|
484
|
+
return res;
|
485
|
+
}
|
486
|
+
operator GPS_Time<FloatT>() const {
|
487
|
+
return GPS_Time<FloatT>(c_tm(), second_start - (int)second_start);
|
488
|
+
}
|
489
|
+
epoch_t &operator=(const GPS_Time<FloatT> &t) {
|
490
|
+
std::tm t_(t.c_tm());
|
491
|
+
year_start = t_.tm_year + 1900;
|
492
|
+
month_start = t_.tm_mon + 1;
|
493
|
+
day_of_month_st = t_.tm_mday;
|
494
|
+
hour_start = t_.tm_hour;
|
495
|
+
minute_start = t_.tm_min;
|
496
|
+
second_start = (t.seconds - (int)t.seconds) + t_.tm_sec;
|
497
|
+
return *this;
|
498
|
+
}
|
499
|
+
};
|
500
|
+
struct position_clock_t {
|
501
|
+
char symbol[1];
|
502
|
+
int vehicle_id;
|
503
|
+
FloatT coordinate_km[3];
|
504
|
+
FloatT clock_us;
|
505
|
+
bool has_sdev;
|
506
|
+
int sdev_b_n_mm[3];
|
507
|
+
int c_sdev_b_n_psec;
|
508
|
+
char clock_event_flag[1];
|
509
|
+
char clock_pred_flag[1];
|
510
|
+
char maneuver_flag[1];
|
511
|
+
char orbit_pred_flag[1];
|
512
|
+
};
|
513
|
+
struct position_clock_correlation_t {
|
514
|
+
char symbols[2];
|
515
|
+
int sdev_mm[3];
|
516
|
+
int clk_sdev_psec;
|
517
|
+
int xy_correlation;
|
518
|
+
int xz_correlation;
|
519
|
+
int xc_correlation;
|
520
|
+
int yz_correlation;
|
521
|
+
int yc_correlation;
|
522
|
+
int zc_correlation;
|
523
|
+
};
|
524
|
+
struct velocity_rate_t {
|
525
|
+
char symbol[1];
|
526
|
+
int vehicle_id;
|
527
|
+
FloatT velocity_dm_s[3];
|
528
|
+
FloatT clock_rate_chg_100ps_s; // 10^-4 microseconds/second = 100 ps/s
|
529
|
+
bool has_sdev;
|
530
|
+
int vel_sdev_100nm_s[3];
|
531
|
+
int clkrate_sdev_10_4_ps_s; // 10^-4 ps/s
|
532
|
+
};
|
533
|
+
struct velocity_rate_correlation_t {
|
534
|
+
char symbols[2];
|
535
|
+
int vel_sdev_100nm_s[3];
|
536
|
+
int clkrate_sdev_10_4_ps_s;
|
537
|
+
int xy_correlation;
|
538
|
+
int xz_correlation;
|
539
|
+
int xc_correlation;
|
540
|
+
int yz_correlation;
|
541
|
+
int yc_correlation;
|
542
|
+
int zc_correlation;
|
543
|
+
};
|
544
|
+
struct parsed_t {
|
545
|
+
enum {
|
546
|
+
UNKNOWN,
|
547
|
+
L1,
|
548
|
+
L2,
|
549
|
+
L3_11,
|
550
|
+
L12_20,
|
551
|
+
L21_22,
|
552
|
+
L23_24,
|
553
|
+
L25_26,
|
554
|
+
COMMENT,
|
555
|
+
EPOCH,
|
556
|
+
POSITION_CLOCK,
|
557
|
+
POSITION_CLOCK_CORRELATION,
|
558
|
+
VELOCITY_RATE,
|
559
|
+
VELOCITY_RATE_CORRELATION,
|
560
|
+
PARSED_ITEMS,
|
561
|
+
} type;
|
562
|
+
union {
|
563
|
+
struct l1_t l1;
|
564
|
+
struct l2_t l2;
|
565
|
+
struct l3_11_t l3_11;
|
566
|
+
struct l12_20_t l12_20;
|
567
|
+
struct l21_22_t l21_22;
|
568
|
+
struct l23_24_t l23_24;
|
569
|
+
struct l25_26_t l25_26;
|
570
|
+
struct comment_t comment;
|
571
|
+
struct epoch_t epoch;
|
572
|
+
struct position_clock_t position_clock;
|
573
|
+
struct position_clock_correlation_t position_clock_correlation;
|
574
|
+
struct velocity_rate_t velocity_rate;
|
575
|
+
struct velocity_rate_correlation_t velocity_rate_correlation;
|
576
|
+
} item;
|
577
|
+
};
|
578
|
+
|
579
|
+
static const typename TextHelper<>::convert_item_t
|
580
|
+
l1_items[13],
|
581
|
+
l2_items[6],
|
582
|
+
l3_11_items[19],
|
583
|
+
l12_20_items[18],
|
584
|
+
l21_22_items[13],
|
585
|
+
l23_24_items[5],
|
586
|
+
l25_26_items[10],
|
587
|
+
comment_items[2],
|
588
|
+
epoch_items[7],
|
589
|
+
position_clock_items[14],
|
590
|
+
position_clock_correlation_items[11],
|
591
|
+
velocity_rate_items[10],
|
592
|
+
velocity_rate_correlation_items[11];
|
593
|
+
|
594
|
+
SP3_Reader(std::istream &in) : src(in) {}
|
595
|
+
|
596
|
+
bool has_next() {
|
597
|
+
return !(src.eof() || src.fail());
|
598
|
+
}
|
599
|
+
|
600
|
+
parsed_t parse_line() {
|
601
|
+
parsed_t res = {parsed_t::UNKNOWN, {0}};
|
602
|
+
|
603
|
+
char buf[0x100] = {0};
|
604
|
+
src.getline(buf, sizeof(buf));
|
605
|
+
std::string line(buf);
|
606
|
+
|
607
|
+
switch(buf[0]){
|
608
|
+
case '#':
|
609
|
+
switch(buf[1]){
|
610
|
+
case '#':
|
611
|
+
TextHelper<>::str2val(l2_items, line, &res.item);
|
612
|
+
res.type = parsed_t::L2;
|
613
|
+
break;
|
614
|
+
default:
|
615
|
+
TextHelper<>::str2val(l1_items, line, &res.item);
|
616
|
+
res.type = parsed_t::L1;
|
617
|
+
break;
|
618
|
+
}
|
619
|
+
break;
|
620
|
+
case '+':
|
621
|
+
switch(buf[1]){
|
622
|
+
case ' ':
|
623
|
+
TextHelper<>::str2val(l3_11_items, line, &res.item);
|
624
|
+
res.type = parsed_t::L3_11;
|
625
|
+
break;
|
626
|
+
case '+':
|
627
|
+
TextHelper<>::str2val(l12_20_items, line, &res.item);
|
628
|
+
res.type = parsed_t::L12_20;
|
629
|
+
break;
|
630
|
+
}
|
631
|
+
break;
|
632
|
+
case '%':
|
633
|
+
switch(buf[1]){
|
634
|
+
case 'c':
|
635
|
+
TextHelper<>::str2val(l21_22_items, line, &res.item);
|
636
|
+
res.type = parsed_t::L21_22;
|
637
|
+
break;
|
638
|
+
case 'f':
|
639
|
+
TextHelper<>::str2val(l23_24_items, line, &res.item);
|
640
|
+
res.type = parsed_t::L23_24;
|
641
|
+
break;
|
642
|
+
case 'i':
|
643
|
+
TextHelper<>::str2val(l25_26_items, line, &res.item);
|
644
|
+
res.type = parsed_t::L25_26;
|
645
|
+
break;
|
646
|
+
}
|
647
|
+
break;
|
648
|
+
case '/':
|
649
|
+
res.type = parsed_t::COMMENT; // TODO
|
650
|
+
break;
|
651
|
+
case '*':
|
652
|
+
TextHelper<>::str2val(epoch_items, line, &res.item);
|
653
|
+
res.type = parsed_t::EPOCH;
|
654
|
+
break;
|
655
|
+
case 'P':
|
656
|
+
res.item.position_clock.has_sdev = (line.length() > 60);
|
657
|
+
TextHelper<>::str2val(
|
658
|
+
position_clock_items,
|
659
|
+
(res.item.position_clock.has_sdev ? 14 : 6),
|
660
|
+
line, &res.item);
|
661
|
+
res.type = parsed_t::POSITION_CLOCK;
|
662
|
+
break;
|
663
|
+
case 'V':
|
664
|
+
res.item.velocity_rate.has_sdev = (line.length() > 60);
|
665
|
+
TextHelper<>::str2val(
|
666
|
+
velocity_rate_items,
|
667
|
+
(res.item.velocity_rate.has_sdev ? 10 : 6),
|
668
|
+
line, &res.item);
|
669
|
+
res.type = parsed_t::VELOCITY_RATE;
|
670
|
+
break;
|
671
|
+
case 'E':
|
672
|
+
switch(buf[1]){
|
673
|
+
case 'P':
|
674
|
+
TextHelper<>::str2val(position_clock_correlation_items, line, &res.item);
|
675
|
+
res.type = parsed_t::POSITION_CLOCK_CORRELATION;
|
676
|
+
break;
|
677
|
+
case 'V':
|
678
|
+
TextHelper<>::str2val(velocity_rate_correlation_items, line, &res.item);
|
679
|
+
res.type = parsed_t::VELOCITY_RATE_CORRELATION;
|
680
|
+
break;
|
681
|
+
}
|
682
|
+
break;
|
683
|
+
}
|
684
|
+
|
685
|
+
return res;
|
686
|
+
}
|
687
|
+
|
688
|
+
struct conv_t {
|
689
|
+
/**
|
690
|
+
* @param value satellite identifier. For GPS, SBAS, and QZSS, it is PRN number.
|
691
|
+
* For other satellite systems, it is ((prefix_char << 8) | satellite_number)
|
692
|
+
* like 20993 = ((82 << 8) | 1) indicating R01 (because 'R' = 82.
|
693
|
+
*/
|
694
|
+
static bool sat_id(
|
695
|
+
std::string &buf, const int &offset, const int &length, void *value,
|
696
|
+
const int &opt = 0, const bool &str2val = true){
|
697
|
+
// format: a letter followed by a 2-digit integer between 01 and 99.
|
698
|
+
if(str2val){
|
699
|
+
if(!TextHelper<>::template format_t<int>::d(
|
700
|
+
buf, offset + 1, length - 1, value, opt, true)){
|
701
|
+
return false;
|
702
|
+
}
|
703
|
+
switch(buf[offset]){
|
704
|
+
case 'G': break; // GPS
|
705
|
+
case 'S': // Satellite-Based Augmentation System (SBAS) satellites
|
706
|
+
*static_cast<int *>(value) += 100; break;
|
707
|
+
case 'J': // QZSS, nn=PRN-192, ex) J01 => PRN=193
|
708
|
+
*static_cast<int *>(value) += 192; break;
|
709
|
+
case 'R': // GLONASS
|
710
|
+
case 'L': // Low-Earth Orbiting (LEO) satellites
|
711
|
+
case 'E': // Galileo
|
712
|
+
case 'C': // BeiDou
|
713
|
+
case 'I': // IRNSS
|
714
|
+
*static_cast<int *>(value) += ((int)buf[offset] << 8); break;
|
715
|
+
case ' ':
|
716
|
+
*static_cast<int *>(value) = 0; break; // TODO
|
717
|
+
default:
|
718
|
+
return false; // unsupported
|
719
|
+
}
|
720
|
+
return true;
|
721
|
+
}else{
|
722
|
+
int digit2(*static_cast<int *>(value));
|
723
|
+
if(digit2 < 0){return false;}
|
724
|
+
char prefix((char)((digit2 >> 8) & 0xFF));
|
725
|
+
do{
|
726
|
+
if(digit2 == 0){
|
727
|
+
prefix = ' ';
|
728
|
+
break;
|
729
|
+
}
|
730
|
+
if(digit2 <= 32){ // GPS
|
731
|
+
prefix = 'G';
|
732
|
+
break;
|
733
|
+
}
|
734
|
+
if(digit2 < 120){return false;}
|
735
|
+
if(digit2 <= 158){ // SBAS
|
736
|
+
prefix = 'S';
|
737
|
+
digit2 -= 100;
|
738
|
+
break;
|
739
|
+
}
|
740
|
+
if(digit2 < 193){return false;}
|
741
|
+
if(digit2 <= 206){ // QZSS
|
742
|
+
prefix = 'J';
|
743
|
+
digit2 -= 192;
|
744
|
+
break;
|
745
|
+
}
|
746
|
+
switch(prefix){
|
747
|
+
case 'R': // GLONASS
|
748
|
+
case 'L': // Low-Earth Orbiting (LEO) satellites
|
749
|
+
case 'E': // Galileo
|
750
|
+
case 'C': // BeiDou
|
751
|
+
case 'I': // IRNSS
|
752
|
+
digit2 &= 0xFF;
|
753
|
+
break;
|
754
|
+
default:
|
755
|
+
return false; // unsupported
|
756
|
+
}
|
757
|
+
}while(false);
|
758
|
+
buf[offset] = prefix;
|
759
|
+
return TextHelper<>::template format_t<int>::d(
|
760
|
+
buf, offset + 1, length - 1, &digit2, digit2 > 0 ? 1 : 0, false);
|
761
|
+
}
|
762
|
+
}
|
763
|
+
};
|
764
|
+
|
765
|
+
static int read_all(std::istream &in, SP3_Product<FloatT> &dst) {
|
766
|
+
SP3_Reader<FloatT> src(in);
|
767
|
+
typedef SP3_Product<FloatT> dst_t;
|
768
|
+
int res(0);
|
769
|
+
struct buf_t {
|
770
|
+
dst_t &dst;
|
771
|
+
int &res;
|
772
|
+
epoch_t epoch;
|
773
|
+
struct pv_t {
|
774
|
+
position_clock_t pos;
|
775
|
+
velocity_rate_t vel;
|
776
|
+
pv_t(){pos.symbol[0] = vel.symbol[0] = 0;}
|
777
|
+
};
|
778
|
+
typedef std::map<int, pv_t> entries_t;
|
779
|
+
entries_t entries;
|
780
|
+
buf_t(dst_t &dst_, int &res_) : dst(dst_), res(res_), entries(){
|
781
|
+
epoch.symbols[0] = 0;
|
782
|
+
}
|
783
|
+
void flush(){
|
784
|
+
if(!epoch.symbols[0]){return;}
|
785
|
+
GPS_Time<FloatT> gpst(epoch);
|
786
|
+
for(typename entries_t::const_iterator it(entries.begin()), it_end(entries.end());
|
787
|
+
it != it_end; ++it){
|
788
|
+
if(it->second.pos.symbol[0]){
|
789
|
+
typename dst_t::prop_t prop = {
|
790
|
+
Vector3<FloatT>(it->second.pos.coordinate_km) * 1E3,
|
791
|
+
it->second.pos.clock_us * 1E-6,
|
792
|
+
};
|
793
|
+
dst.satellites[it->first].pos_history.insert(std::make_pair(gpst, prop));
|
794
|
+
}
|
795
|
+
if(it->second.vel.symbol[0]){
|
796
|
+
typename dst_t::prop_t prop = {
|
797
|
+
Vector3<FloatT>(it->second.vel.velocity_dm_s) * 1E-1,
|
798
|
+
it->second.vel.clock_rate_chg_100ps_s * 1E-10,
|
799
|
+
};
|
800
|
+
dst.satellites[it->first].vel_history.insert(std::make_pair(gpst, prop));
|
801
|
+
}
|
802
|
+
++res;
|
803
|
+
}
|
804
|
+
entries.clear();
|
805
|
+
}
|
806
|
+
} buf(dst, res);
|
807
|
+
while(src.has_next()){
|
808
|
+
parsed_t parsed(src.parse_line());
|
809
|
+
switch(parsed.type){
|
810
|
+
case parsed_t::EPOCH:
|
811
|
+
buf.flush();
|
812
|
+
buf.epoch = parsed.item.epoch;
|
813
|
+
break;
|
814
|
+
case parsed_t::POSITION_CLOCK: {
|
815
|
+
int sat_id(parsed.item.position_clock.vehicle_id);
|
816
|
+
buf.entries[sat_id].pos = parsed.item.position_clock;
|
817
|
+
break;
|
818
|
+
}
|
819
|
+
case parsed_t::VELOCITY_RATE: {
|
820
|
+
int sat_id(parsed.item.velocity_rate.vehicle_id);
|
821
|
+
buf.entries[sat_id].vel = parsed.item.velocity_rate;
|
822
|
+
break;
|
823
|
+
}
|
824
|
+
default: break;
|
825
|
+
}
|
826
|
+
}
|
827
|
+
buf.flush();
|
828
|
+
return res;
|
829
|
+
}
|
830
|
+
};
|
831
|
+
|
832
|
+
#define GEN_C(offset, length, container_type, container_member) \
|
833
|
+
{TextHelper<>::template format_t<char>::c, offset, length, \
|
834
|
+
offsetof(container_type, container_member), 0}
|
835
|
+
#define GEN_I(offset, length, container_type, container_member) \
|
836
|
+
{TextHelper<>::template format_t<int>::d, offset, length, \
|
837
|
+
offsetof(container_type, container_member), 0}
|
838
|
+
#define GEN_I2(offset, length, container_type, container_member) \
|
839
|
+
{TextHelper<>::template format_t<int>::d_blank, offset, length, \
|
840
|
+
offsetof(container_type, container_member), 0}
|
841
|
+
#define GEN_E(offset, length, container_type, container_member, precision) \
|
842
|
+
{TextHelper<>::template format_t<FloatT>::f, offset, length, \
|
843
|
+
offsetof(container_type, container_member), precision}
|
844
|
+
#define GEN_sat(offset, length, container_type, container_member) \
|
845
|
+
{SP3_Reader<FloatT>::conv_t::sat_id, offset, length, \
|
846
|
+
offsetof(container_type, container_member)}
|
847
|
+
|
848
|
+
template <class FloatT>
|
849
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::l1_items[13] = {
|
850
|
+
GEN_C(0, 2, l1_t, version_symbol),
|
851
|
+
GEN_C(2, 1, l1_t, pos_or_vel_flag),
|
852
|
+
GEN_I(3, 4, l1_t, year_start),
|
853
|
+
GEN_I(8, 2, l1_t, month_start),
|
854
|
+
GEN_I(11, 2, l1_t, day_of_month_st),
|
855
|
+
GEN_I(14, 2, l1_t, hour_start),
|
856
|
+
GEN_I(17, 2, l1_t, minute_start),
|
857
|
+
GEN_E(20, 11, l1_t, second_start, 8),
|
858
|
+
GEN_I(32, 7, l1_t, number_of_epochs),
|
859
|
+
GEN_C(40, 5, l1_t, data_used),
|
860
|
+
GEN_C(46, 5, l1_t, coordinate_sys),
|
861
|
+
GEN_C(52, 3, l1_t, orbit_type),
|
862
|
+
GEN_C(56, 4, l1_t, agency),
|
863
|
+
};
|
864
|
+
template <class FloatT>
|
865
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::l2_items[6] = {
|
866
|
+
GEN_C(0, 2, l2_t, symbols),
|
867
|
+
GEN_I(3, 4, l2_t, gps_week),
|
868
|
+
GEN_E(8, 15, l2_t, seconds_of_week, 8),
|
869
|
+
GEN_E(24, 14, l2_t, epoch_interval, 8),
|
870
|
+
GEN_I(39, 5, l2_t, mod_jul_day_st),
|
871
|
+
GEN_E(45, 15, l2_t, fractional_day, 13),
|
872
|
+
};
|
873
|
+
template <class FloatT>
|
874
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::l3_11_items[19] = {
|
875
|
+
GEN_C(0, 2, l3_11_t, symbols),
|
876
|
+
GEN_I2(3, 3, l3_11_t, number_of_sats),
|
877
|
+
GEN_sat(9, 3, l3_11_t, sat_id[0]),
|
878
|
+
GEN_sat(12, 3, l3_11_t, sat_id[1]),
|
879
|
+
GEN_sat(15, 3, l3_11_t, sat_id[2]),
|
880
|
+
GEN_sat(18, 3, l3_11_t, sat_id[3]),
|
881
|
+
GEN_sat(21, 3, l3_11_t, sat_id[4]),
|
882
|
+
GEN_sat(24, 3, l3_11_t, sat_id[5]),
|
883
|
+
GEN_sat(27, 3, l3_11_t, sat_id[6]),
|
884
|
+
GEN_sat(30, 3, l3_11_t, sat_id[7]),
|
885
|
+
GEN_sat(33, 3, l3_11_t, sat_id[8]),
|
886
|
+
GEN_sat(36, 3, l3_11_t, sat_id[9]),
|
887
|
+
GEN_sat(39, 3, l3_11_t, sat_id[10]),
|
888
|
+
GEN_sat(42, 3, l3_11_t, sat_id[11]),
|
889
|
+
GEN_sat(45, 3, l3_11_t, sat_id[12]),
|
890
|
+
GEN_sat(48, 3, l3_11_t, sat_id[13]),
|
891
|
+
GEN_sat(51, 3, l3_11_t, sat_id[14]),
|
892
|
+
GEN_sat(54, 3, l3_11_t, sat_id[15]),
|
893
|
+
GEN_sat(57, 3, l3_11_t, sat_id[16]),
|
894
|
+
};
|
895
|
+
template <class FloatT>
|
896
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::l12_20_items[18] = {
|
897
|
+
GEN_C(0, 2, l12_20_t, symbols),
|
898
|
+
GEN_I(9, 3, l12_20_t, sat_accuracy[0]),
|
899
|
+
GEN_I(12, 3, l12_20_t, sat_accuracy[1]),
|
900
|
+
GEN_I(15, 3, l12_20_t, sat_accuracy[2]),
|
901
|
+
GEN_I(18, 3, l12_20_t, sat_accuracy[3]),
|
902
|
+
GEN_I(21, 3, l12_20_t, sat_accuracy[4]),
|
903
|
+
GEN_I(24, 3, l12_20_t, sat_accuracy[5]),
|
904
|
+
GEN_I(27, 3, l12_20_t, sat_accuracy[6]),
|
905
|
+
GEN_I(30, 3, l12_20_t, sat_accuracy[7]),
|
906
|
+
GEN_I(33, 3, l12_20_t, sat_accuracy[8]),
|
907
|
+
GEN_I(36, 3, l12_20_t, sat_accuracy[9]),
|
908
|
+
GEN_I(39, 3, l12_20_t, sat_accuracy[10]),
|
909
|
+
GEN_I(42, 3, l12_20_t, sat_accuracy[11]),
|
910
|
+
GEN_I(45, 3, l12_20_t, sat_accuracy[12]),
|
911
|
+
GEN_I(48, 3, l12_20_t, sat_accuracy[13]),
|
912
|
+
GEN_I(51, 3, l12_20_t, sat_accuracy[14]),
|
913
|
+
GEN_I(54, 3, l12_20_t, sat_accuracy[15]),
|
914
|
+
GEN_I(57, 3, l12_20_t, sat_accuracy[16]),
|
915
|
+
};
|
916
|
+
template <class FloatT>
|
917
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::l21_22_items[13] = {
|
918
|
+
GEN_C(0, 2, l21_22_t, symbols),
|
919
|
+
GEN_C(3, 2, l21_22_t, file_type),
|
920
|
+
GEN_C(6, 2, l21_22_t, _2_characters),
|
921
|
+
GEN_C(9, 3, l21_22_t, time_system),
|
922
|
+
GEN_C(13, 3, l21_22_t, _3_characters),
|
923
|
+
GEN_C(17, 4, l21_22_t, _4_characters[0]),
|
924
|
+
GEN_C(22, 4, l21_22_t, _4_characters[1]),
|
925
|
+
GEN_C(27, 4, l21_22_t, _4_characters[2]),
|
926
|
+
GEN_C(32, 4, l21_22_t, _4_characters[3]),
|
927
|
+
GEN_C(37, 5, l21_22_t, _5_characters[0]),
|
928
|
+
GEN_C(43, 5, l21_22_t, _5_characters[1]),
|
929
|
+
GEN_C(49, 5, l21_22_t, _5_characters[2]),
|
930
|
+
GEN_C(55, 5, l21_22_t, _5_characters[3]),
|
931
|
+
};
|
932
|
+
template <class FloatT>
|
933
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::l23_24_items[5] = {
|
934
|
+
GEN_C(0, 2, l23_24_t, symbols),
|
935
|
+
GEN_E(3, 10, l23_24_t, base_for_pos_vel, 7),
|
936
|
+
GEN_E(14, 12, l23_24_t, base_for_clk_rate, 9),
|
937
|
+
GEN_E(27, 14, l23_24_t, _14_column_float, 11),
|
938
|
+
GEN_E(42, 18, l23_24_t, _18_column_float, 15),
|
939
|
+
};
|
940
|
+
template <class FloatT>
|
941
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::l25_26_items[10] = {
|
942
|
+
GEN_C(0, 2, l25_26_t, symbols),
|
943
|
+
GEN_I(3, 4, l25_26_t, _4_column_int[0]),
|
944
|
+
GEN_I(8, 4, l25_26_t, _4_column_int[1]),
|
945
|
+
GEN_I(13, 4, l25_26_t, _4_column_int[2]),
|
946
|
+
GEN_I(18, 4, l25_26_t, _4_column_int[3]),
|
947
|
+
GEN_I(23, 6, l25_26_t, _6_column_int[0]),
|
948
|
+
GEN_I(30, 6, l25_26_t, _6_column_int[1]),
|
949
|
+
GEN_I(37, 6, l25_26_t, _6_column_int[2]),
|
950
|
+
GEN_I(44, 6, l25_26_t, _6_column_int[3]),
|
951
|
+
GEN_I(51, 9, l25_26_t, _9_column_int),
|
952
|
+
};
|
953
|
+
template <class FloatT>
|
954
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::comment_items[2] = {
|
955
|
+
GEN_C(0, 2, comment_t, symbols),
|
956
|
+
GEN_C(3, 57, comment_t, comment),
|
957
|
+
};
|
958
|
+
template <class FloatT>
|
959
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::epoch_items[7] = {
|
960
|
+
GEN_C(0, 2, epoch_t, symbols),
|
961
|
+
GEN_I(3, 4, epoch_t, year_start),
|
962
|
+
GEN_I(8, 2, epoch_t, month_start),
|
963
|
+
GEN_I(11, 2, epoch_t, day_of_month_st),
|
964
|
+
GEN_I(14, 2, epoch_t, hour_start),
|
965
|
+
GEN_I(17, 2, epoch_t, minute_start),
|
966
|
+
GEN_E(20, 11, epoch_t, second_start, 8),
|
967
|
+
};
|
968
|
+
template <class FloatT>
|
969
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::position_clock_items[14] = {
|
970
|
+
GEN_C(0, 1, position_clock_t, symbol),
|
971
|
+
GEN_sat(1, 3, position_clock_t, vehicle_id),
|
972
|
+
GEN_E(4, 14, position_clock_t, coordinate_km[0], 6),
|
973
|
+
GEN_E(18, 14, position_clock_t, coordinate_km[1], 6),
|
974
|
+
GEN_E(32, 14, position_clock_t, coordinate_km[2], 6),
|
975
|
+
GEN_E(46, 14, position_clock_t, clock_us, 6),
|
976
|
+
GEN_I(61, 2, position_clock_t, sdev_b_n_mm[0]),
|
977
|
+
GEN_I(64, 2, position_clock_t, sdev_b_n_mm[1]),
|
978
|
+
GEN_I(67, 2, position_clock_t, sdev_b_n_mm[2]),
|
979
|
+
GEN_I(70, 3, position_clock_t, c_sdev_b_n_psec),
|
980
|
+
GEN_C(74, 1, position_clock_t, clock_event_flag),
|
981
|
+
GEN_C(75, 1, position_clock_t, clock_pred_flag),
|
982
|
+
GEN_C(78, 1, position_clock_t, maneuver_flag),
|
983
|
+
GEN_C(79, 1, position_clock_t, orbit_pred_flag),
|
984
|
+
};
|
985
|
+
template <class FloatT>
|
986
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::position_clock_correlation_items[11] = {
|
987
|
+
GEN_C(0, 2, position_clock_correlation_t, symbols),
|
988
|
+
GEN_I(4, 4, position_clock_correlation_t, sdev_mm[0]),
|
989
|
+
GEN_I(9, 4, position_clock_correlation_t, sdev_mm[1]),
|
990
|
+
GEN_I(14, 4, position_clock_correlation_t, sdev_mm[2]),
|
991
|
+
GEN_I(19, 7, position_clock_correlation_t, clk_sdev_psec),
|
992
|
+
GEN_I(27, 8, position_clock_correlation_t, xy_correlation),
|
993
|
+
GEN_I(36, 8, position_clock_correlation_t, xz_correlation),
|
994
|
+
GEN_I(45, 8, position_clock_correlation_t, xc_correlation),
|
995
|
+
GEN_I(54, 8, position_clock_correlation_t, yz_correlation),
|
996
|
+
GEN_I(63, 8, position_clock_correlation_t, yc_correlation),
|
997
|
+
GEN_I(72, 8, position_clock_correlation_t, zc_correlation),
|
998
|
+
};
|
999
|
+
template <class FloatT>
|
1000
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::velocity_rate_items[10] = {
|
1001
|
+
GEN_C(0, 1, velocity_rate_t, symbol),
|
1002
|
+
GEN_sat(1, 3, velocity_rate_t, vehicle_id),
|
1003
|
+
GEN_E(4, 14, velocity_rate_t, velocity_dm_s[0], 6),
|
1004
|
+
GEN_E(18, 14, velocity_rate_t, velocity_dm_s[1], 6),
|
1005
|
+
GEN_E(32, 14, velocity_rate_t, velocity_dm_s[2], 6),
|
1006
|
+
GEN_E(46, 14, velocity_rate_t, clock_rate_chg_100ps_s, 6),
|
1007
|
+
GEN_I(61, 2, velocity_rate_t, vel_sdev_100nm_s[0]),
|
1008
|
+
GEN_I(64, 2, velocity_rate_t, vel_sdev_100nm_s[1]),
|
1009
|
+
GEN_I(67, 2, velocity_rate_t, vel_sdev_100nm_s[2]),
|
1010
|
+
GEN_I(70, 3, velocity_rate_t, clkrate_sdev_10_4_ps_s),
|
1011
|
+
};
|
1012
|
+
template <class FloatT>
|
1013
|
+
const typename TextHelper<>::convert_item_t SP3_Reader<FloatT>::velocity_rate_correlation_items[11] = {
|
1014
|
+
GEN_C(0, 2, velocity_rate_correlation_t, symbols),
|
1015
|
+
GEN_I(4, 4, velocity_rate_correlation_t, vel_sdev_100nm_s[0]),
|
1016
|
+
GEN_I(9, 4, velocity_rate_correlation_t, vel_sdev_100nm_s[1]),
|
1017
|
+
GEN_I(14, 4, velocity_rate_correlation_t, vel_sdev_100nm_s[2]),
|
1018
|
+
GEN_I(19, 7, velocity_rate_correlation_t, clkrate_sdev_10_4_ps_s),
|
1019
|
+
GEN_I(27, 8, velocity_rate_correlation_t, xy_correlation),
|
1020
|
+
GEN_I(36, 8, velocity_rate_correlation_t, xz_correlation),
|
1021
|
+
GEN_I(45, 8, velocity_rate_correlation_t, xc_correlation),
|
1022
|
+
GEN_I(54, 8, velocity_rate_correlation_t, yz_correlation),
|
1023
|
+
GEN_I(63, 8, velocity_rate_correlation_t, yc_correlation),
|
1024
|
+
GEN_I(72, 8, velocity_rate_correlation_t, zc_correlation),
|
1025
|
+
};
|
1026
|
+
|
1027
|
+
#undef GEN_C
|
1028
|
+
#undef GEN_I
|
1029
|
+
#undef GEN_I2
|
1030
|
+
#undef GEN_E
|
1031
|
+
#undef GEN_sat
|
1032
|
+
|
1033
|
+
template <class FloatT>
|
1034
|
+
class SP3_Writer {
|
1035
|
+
public:
|
1036
|
+
typedef SP3_Reader<FloatT> reader_t;
|
1037
|
+
static std::string print_line(const typename reader_t::parsed_t &parsed){
|
1038
|
+
static const struct {
|
1039
|
+
const typename TextHelper<>::convert_item_t *items;
|
1040
|
+
int items_num;
|
1041
|
+
const char *symbol;
|
1042
|
+
} table[reader_t::parsed_t::PARSED_ITEMS] = {
|
1043
|
+
{0}, // UNKNOWN
|
1044
|
+
#define MAKE_ENTRY(k, str) \
|
1045
|
+
{reader_t::k, sizeof(reader_t::k) / sizeof(reader_t::k[0]), str}
|
1046
|
+
MAKE_ENTRY(l1_items, "#"), // L1
|
1047
|
+
MAKE_ENTRY(l2_items, "##"), // L2
|
1048
|
+
MAKE_ENTRY(l3_11_items, "+ "), // L3_11
|
1049
|
+
MAKE_ENTRY(l12_20_items, "++"), // L12_20
|
1050
|
+
MAKE_ENTRY(l21_22_items, "%c"), // L21_22
|
1051
|
+
MAKE_ENTRY(l23_24_items, "%f"), // L23_24
|
1052
|
+
MAKE_ENTRY(l25_26_items, "%i"), // L25_26
|
1053
|
+
MAKE_ENTRY(comment_items, "/"), // COMMENT
|
1054
|
+
MAKE_ENTRY(epoch_items, "*"), // EPOCH
|
1055
|
+
MAKE_ENTRY(position_clock_items, "P"), // POSITION_CLOCK
|
1056
|
+
MAKE_ENTRY(position_clock_correlation_items, "EP"), // POSITION_CLOCK_CORRELATION
|
1057
|
+
MAKE_ENTRY(velocity_rate_items, "V"), // VELOCITY_RATE
|
1058
|
+
MAKE_ENTRY(velocity_rate_correlation_items, "EV"), // VELOCITY_RATE_CORRELATION
|
1059
|
+
#undef MAKE_ENTRY
|
1060
|
+
};
|
1061
|
+
|
1062
|
+
int res_length(60);
|
1063
|
+
int items_num(table[parsed.type].items_num);
|
1064
|
+
switch(parsed.type){
|
1065
|
+
case reader_t::parsed_t::POSITION_CLOCK:
|
1066
|
+
if(parsed.item.position_clock.has_sdev){res_length = 80;}
|
1067
|
+
else{items_num = 6;}
|
1068
|
+
break;
|
1069
|
+
case reader_t::parsed_t::VELOCITY_RATE:
|
1070
|
+
if(parsed.item.velocity_rate.has_sdev){res_length = 80;}
|
1071
|
+
else{items_num = 6;}
|
1072
|
+
break;
|
1073
|
+
case reader_t::parsed_t::POSITION_CLOCK_CORRELATION:
|
1074
|
+
case reader_t::parsed_t::VELOCITY_RATE_CORRELATION:
|
1075
|
+
res_length = 80;
|
1076
|
+
break;
|
1077
|
+
default:
|
1078
|
+
break;
|
1079
|
+
}
|
1080
|
+
std::string res(res_length, ' ');
|
1081
|
+
if((items_num <= 0)
|
1082
|
+
|| (!TextHelper<>::val2str(table[parsed.type].items, items_num, res, &parsed.item))){
|
1083
|
+
return std::string();
|
1084
|
+
}
|
1085
|
+
res.replace(0, std::strlen(table[parsed.type].symbol), table[parsed.type].symbol);
|
1086
|
+
return res;
|
1087
|
+
}
|
1088
|
+
|
1089
|
+
static void dump_default(
|
1090
|
+
std::ostream &out, typename reader_t::parsed_t &item){
|
1091
|
+
out << print_line(item) << std::endl;
|
1092
|
+
}
|
1093
|
+
|
1094
|
+
static int write_all(
|
1095
|
+
std::ostream &out, const SP3_Product<FloatT> &src,
|
1096
|
+
void (*dump)(
|
1097
|
+
std::ostream &, typename reader_t::parsed_t &) = dump_default) {
|
1098
|
+
typedef SP3_Product<FloatT> src_t;
|
1099
|
+
typedef typename src_t::epochs_t epochs_t;
|
1100
|
+
epochs_t epochs(src.epochs());
|
1101
|
+
if(epochs.empty()){return 0;}
|
1102
|
+
{ // 1st line
|
1103
|
+
typename reader_t::parsed_t header = {reader_t::parsed_t::L1};
|
1104
|
+
header.item.l1 = *epochs.begin();
|
1105
|
+
header.item.l1.number_of_epochs = epochs.size();
|
1106
|
+
header.item.l1.pos_or_vel_flag[0] = src.has_velocity() ? 'V' : 'P';
|
1107
|
+
dump(out, header);
|
1108
|
+
}
|
1109
|
+
{ // 2nd line
|
1110
|
+
typename reader_t::parsed_t first_epoch = {reader_t::parsed_t::L2};
|
1111
|
+
GPS_Time<FloatT> t0(*epochs.begin());
|
1112
|
+
first_epoch.item.l2.gps_week = t0.week;
|
1113
|
+
first_epoch.item.l2.seconds_of_week = t0.seconds;
|
1114
|
+
first_epoch.item.l2.epoch_interval
|
1115
|
+
= (epochs.size() > 1) ? (*(++(epochs.begin())) - t0) : 0;
|
1116
|
+
FloatT day_of_week(t0.seconds / 86400);
|
1117
|
+
first_epoch.item.l2.mod_jul_day_st
|
1118
|
+
= 44244 + (t0.week * 7) + (int)day_of_week;
|
1119
|
+
first_epoch.item.l2.fractional_day
|
1120
|
+
= day_of_week - (int)day_of_week;
|
1121
|
+
dump(out, first_epoch);
|
1122
|
+
}
|
1123
|
+
typedef typename src_t::satellites_t sats_t;
|
1124
|
+
{ // 3rd line
|
1125
|
+
int sats(src.satellites.size());
|
1126
|
+
typename reader_t::parsed_t sat_list = {reader_t::parsed_t::L3_11};
|
1127
|
+
sat_list.item.l3_11.number_of_sats = sats;
|
1128
|
+
typename sats_t::const_iterator
|
1129
|
+
it(src.satellites.begin()), it_end(src.satellites.end());
|
1130
|
+
int lines((sats + 16) / 17);
|
1131
|
+
for(int i(lines < 5 ? 5 : lines); i > 0; --i){
|
1132
|
+
for(int j(0); j < 17; ++j){
|
1133
|
+
sat_list.item.l3_11.sat_id[j] = ((it == it_end) ? 0 : (it++)->first);
|
1134
|
+
}
|
1135
|
+
dump(out, sat_list);
|
1136
|
+
sat_list.item.l3_11.number_of_sats = 0;
|
1137
|
+
}
|
1138
|
+
}
|
1139
|
+
typename reader_t::parsed_t
|
1140
|
+
pos = {reader_t::parsed_t::POSITION_CLOCK},
|
1141
|
+
vel = {reader_t::parsed_t::VELOCITY_RATE},
|
1142
|
+
time = {reader_t::parsed_t::EPOCH};
|
1143
|
+
pos.item.position_clock.has_sdev = false;
|
1144
|
+
vel.item.velocity_rate.has_sdev = false;
|
1145
|
+
int entries(0);
|
1146
|
+
for(typename epochs_t::const_iterator it(epochs.begin()), it_end(epochs.end());
|
1147
|
+
it != it_end; ++it){
|
1148
|
+
time.item.epoch = *it;
|
1149
|
+
dump(out, time);
|
1150
|
+
for(typename sats_t::const_iterator
|
1151
|
+
it2(src.satellites.begin()), it2_end(src.satellites.end());
|
1152
|
+
it2 != it2_end; ++it2){
|
1153
|
+
typename src_t::per_satellite_t::history_t::const_iterator it_entry;
|
1154
|
+
if((it_entry = it2->second.pos_history.find(*it))
|
1155
|
+
== it2->second.pos_history.end()){continue;}
|
1156
|
+
++entries;
|
1157
|
+
pos.item.position_clock.vehicle_id = it2->first;
|
1158
|
+
for(int i(0); i < 3; ++i){
|
1159
|
+
pos.item.position_clock.coordinate_km[i] = it_entry->second.xyz[i] * 1E-3;
|
1160
|
+
}
|
1161
|
+
pos.item.position_clock.clock_us = it_entry->second.clk * 1E6;
|
1162
|
+
dump(out, pos);
|
1163
|
+
if((it_entry = it2->second.vel_history.find(*it))
|
1164
|
+
== it2->second.vel_history.end()){continue;}
|
1165
|
+
vel.item.velocity_rate.vehicle_id = it2->first;
|
1166
|
+
for(int i(0); i < 3; ++i){
|
1167
|
+
vel.item.velocity_rate.velocity_dm_s[i] = it_entry->second.xyz[i] * 1E1;
|
1168
|
+
}
|
1169
|
+
vel.item.velocity_rate.clock_rate_chg_100ps_s = it_entry->second.clk * 1E10;
|
1170
|
+
dump(out, vel);
|
1171
|
+
}
|
1172
|
+
}
|
1173
|
+
return entries;
|
1174
|
+
}
|
1175
|
+
};
|
1176
|
+
|
1177
|
+
|
1178
|
+
#endif /* #define __SP3_H__ */
|