gps_pvt 0.5.1 → 0.6.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 +11 -7
- data/Rakefile +42 -23
- data/exe/gps_pvt +13 -6
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +44 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +8975 -7360
- 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/GPS.h +92 -4
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +9 -1
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +18 -161
- 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 +120 -1
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +503 -0
- data/ext/ninja-scan-light/tool/util/text_helper.h +248 -0
- data/lib/gps_pvt/receiver.rb +24 -0
- data/lib/gps_pvt/util.rb +25 -7
- data/lib/gps_pvt/version.rb +1 -1
- metadata +6 -2
@@ -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__ */
|