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.
@@ -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__ */