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,747 @@
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 __ANTEX_H__
38
+ #define __ANTEX_H__
39
+
40
+ #include <string>
41
+ #include <ctime>
42
+
43
+ #include <vector>
44
+ #include <map>
45
+
46
+ #include "util/text_helper.h"
47
+ #include "GPS.h"
48
+ #include "SP3.h"
49
+ #include "coordinate.h"
50
+
51
+ template <class FloatT>
52
+ struct ANTEX_Product {
53
+ struct per_freq_t {
54
+ FloatT north_east_up[3];
55
+ std::vector<FloatT> noazi; // TODO
56
+ std::map<FloatT, std::vector<FloatT> > azi; // TODO
57
+ };
58
+ struct antenna_t {
59
+ std::string type, serial;
60
+ GPS_Time<FloatT> valid_from;
61
+ GPS_Time<FloatT> valid_until;
62
+ std::map<std::string, per_freq_t> freq;
63
+ std::map<std::string, per_freq_t> freq_rms;
64
+ bool is_satellite(int *sat_id = NULL) const {
65
+ if(serial.size() != 3){return false;}
66
+ int dummy;
67
+ return SP3_Reader<FloatT>::conv_t::sat_id(
68
+ const_cast<std::string &>(serial), 0, 3, (sat_id ? sat_id : &dummy));
69
+ }
70
+ };
71
+ typedef std::vector<antenna_t> prop_t;
72
+ prop_t prop;
73
+
74
+ struct eci2ecef_opt_t {
75
+ FloatT delta_UT1;
76
+ FloatT x_p, y_p;
77
+ eci2ecef_opt_t() : delta_UT1(0), x_p(0), y_p(0) {}
78
+ };
79
+ static typename System_XYZ<FloatT>::Rotation eci2ecef(
80
+ const GPS_Time<FloatT> &t_gps,
81
+ const eci2ecef_opt_t &opt = eci2ecef_opt_t() ) {
82
+ // ECI -> ECEF
83
+ // @see https://doi.org/10.1007/978-3-540-78522-4_2 Chapter 2
84
+ // X_ECEF = R_M * R_S * R_N * R_P * X_ECI
85
+ // R_M: polar motion, R_S: Earth rotation, R_N: nutation, R_P: precession
86
+
87
+ typedef typename System_XYZ<FloatT>::Rotation rot_t;
88
+
89
+ // pp.20 in IERS Conventions (1996) and Eq.(5.2) in Chapter 5 of IERS Conventions (2010) defines
90
+ // t = (TT - 2000 January ld 12h TT) in days/36525,
91
+ // where t is relative time as it computed with a difference of two Terrestrial Time (TT).
92
+ // Note: TT is approximately TAI + 32.184s = GPS + 19 + 32.184.
93
+ FloatT t_date(t_gps.julian_date_2000()/* + (19.0 + 32.184) / 86400*/); // TODO need offset?
94
+ FloatT t(t_date / 36525), t2(t * t), t3(t2 * t); // century and its powered
95
+
96
+ #define AS2RAD(x) ((x) * (M_PI / 3600 / 180)) // arcseconds to radians
97
+ #define DMS2RAD(d, m, s) AS2RAD((FloatT)(s) + (FloatT)(m) * 60 + (FloatT)(d) * 3600)
98
+ #define DEG2RAD(d) DMS2RAD(d, 0, 0)
99
+ // astronomical arguments
100
+ struct ast_args_t {
101
+ FloatT f[5]; // rad
102
+ ast_args_t(const FloatT &t){
103
+ // @see Eq.(5.43) in Chapter 5 of IERS Conventions (2010)
104
+ // https://www.iers.org/SharedDocs/Publikationen/EN/IERS/Publications/tn/TechnNote36/tn36_043.pdf#page=25
105
+ static const FloatT fc[][5] = {
106
+ // F1(l): Mean Anomaly of the Moon
107
+ {DEG2RAD(134.96340251), AS2RAD(1717915923.2178), AS2RAD( 31.8792), AS2RAD( 0.051635), AS2RAD(-0.00024470)},
108
+ // F2(l'): Mean Anomaly of the Sun
109
+ {DEG2RAD(357.52910918), AS2RAD( 129596581.0481), AS2RAD( -0.5532), AS2RAD( 0.000136), AS2RAD(-0.00001149)},
110
+ // F3(F)
111
+ {DEG2RAD( 93.27209062), AS2RAD(1739527262.8478), AS2RAD(-12.7512), AS2RAD(-0.001037), AS2RAD( 0.00000417)},
112
+ // F4(D): Mean Elongation of the Moon from the Sun
113
+ {DEG2RAD(297.85019547), AS2RAD(1602961601.2090), AS2RAD( -6.3706), AS2RAD( 0.006593), AS2RAD(-0.00003169)},
114
+ // F5(Omega): Mean Longitude of the Ascending Node of the Moon
115
+ {DEG2RAD(125.04455501), AS2RAD( -6962890.2665), AS2RAD( 7.4722), AS2RAD( 0.007702), AS2RAD(-0.00005939)},
116
+ };
117
+ FloatT tt[] = {1, t, std::pow(t, 2), std::pow(t, 3), std::pow(t, 4)};
118
+ for(std::size_t i(0); i < sizeof(f) / sizeof(f[0]); i++){
119
+ f[i] = 0;
120
+ for(std::size_t j(0); j < sizeof(fc[0]) / sizeof(fc[0][0]); ++j){
121
+ f[i] += fc[i][j] * tt[j];
122
+ }
123
+ f[i] = std::fmod(f[i], M_PI * 2);
124
+ }
125
+
126
+ }
127
+ } ast_args(t);
128
+
129
+ // IAU 1976 precession
130
+ FloatT
131
+ zeta (t * AS2RAD(2306.2181) + t2 * AS2RAD(0.30188) + t3 * AS2RAD(0.017998)), // same as RTKlib
132
+ theta(t * AS2RAD(2004.3109) - t2 * AS2RAD(0.42665) - t3 * AS2RAD(0.041833)),
133
+ z (t * AS2RAD(2306.2181) + t2 * AS2RAD(1.09468) + t3 * AS2RAD(0.018203));
134
+
135
+ // Precession=R3(-z)*R2(theta)*R3(-zeta)
136
+ rot_t r_p(rot_t().then_z(-zeta).then_y(theta).then_z(-z));
137
+
138
+ struct nut_iau1980_t {
139
+ FloatT dpsi, deps;
140
+ nut_iau1980_t(const FloatT &t, const FloatT (&f)[5])
141
+ : dpsi(0), deps(0){
142
+ static const struct {
143
+ int arg[5]; // l, l', F, D, Omega
144
+ FloatT period; // (days)
145
+ int lng_A; // longitude(1E-4)
146
+ FloatT lng_At; // [1/century]
147
+ int obl_B; // obliquity(1E-4)
148
+ FloatT obl_Bt; // [1/century]
149
+ } nut[106] = {
150
+ {{ 0, 0, 0, 0, 1},-6798.4, -171996, -174.2, 92025, 8.9},
151
+ {{ 0, 0, 2, -2, 2}, 182.6, -13187, -1.6, 5736, -3.1},
152
+ {{ 0, 0, 2, 0, 2}, 13.7, -2274, -0.2, 977, -0.5},
153
+ {{ 0, 0, 0, 0, 2},-3399.2, 2062, 0.2, -895, 0.5},
154
+ {{ 0, -1, 0, 0, 0}, -365.3, -1426, 3.4, 54, -0.1},
155
+ {{ 1, 0, 0, 0, 0}, 27.6, 712, 0.1, -7, 0.0},
156
+ {{ 0, 1, 2, -2, 2}, 121.7, -517, 1.2, 224, -0.6},
157
+ {{ 0, 0, 2, 0, 1}, 13.6, -386, -0.4, 200, 0.0},
158
+ {{ 1, 0, 2, 0, 2}, 9.1, -301, 0.0, 129, -0.1},
159
+ {{ 0, -1, 2, -2, 2}, 365.2, 217, -0.5, -95, 0.3},
160
+ {{ -1, 0, 0, 2, 0}, 31.8, 158, 0.0, -1, 0.0},
161
+ {{ 0, 0, 2, -2, 1}, 177.8, 129, 0.1, -70, 0.0},
162
+ {{ -1, 0, 2, 0, 2}, 27.1, 123, 0.0, -53, 0.0},
163
+ {{ 1, 0, 0, 0, 1}, 27.7, 63, 0.1, -33, 0.0},
164
+ {{ 0, 0, 0, 2, 0}, 14.8, 63, 0.0, -2, 0.0},
165
+ {{ -1, 0, 2, 2, 2}, 9.6, -59, 0.0, 26, 0.0},
166
+ {{ -1, 0, 0, 0, 1}, -27.4, -58, -0.1, 32, 0.0},
167
+ {{ 1, 0, 2, 0, 1}, 9.1, -51, 0.0, 27, 0.0},
168
+ {{ -2, 0, 0, 2, 0}, -205.9, -48, 0.0, 1, 0.0},
169
+ {{ -2, 0, 2, 0, 1}, 1305.5, 46, 0.0, -24, 0.0},
170
+ {{ 0, 0, 2, 2, 2}, 7.1, -38, 0.0, 16, 0.0},
171
+ {{ 2, 0, 2, 0, 2}, 6.9, -31, 0.0, 13, 0.0},
172
+ {{ 2, 0, 0, 0, 0}, 13.8, 29, 0.0, -1, 0.0},
173
+ {{ 1, 0, 2, -2, 2}, 23.9, 29, 0.0, -12, 0.0},
174
+ {{ 0, 0, 2, 0, 0}, 13.6, 26, 0.0, -1, 0.0},
175
+ {{ 0, 0, 2, -2, 0}, 173.3, -22, 0.0, 0, 0.0},
176
+ {{ -1, 0, 2, 0, 1}, 27.0, 21, 0.0, -10, 0.0},
177
+ {{ 0, 2, 0, 0, 0}, 182.6, 17, -0.1, 0, 0.0},
178
+ {{ 0, 2, 2, -2, 2}, 91.3, -16, 0.1, 7, 0.0},
179
+ {{ -1, 0, 0, 2, 1}, 32.0, 16, 0.0, -8, 0.0},
180
+ {{ 0, 1, 0, 0, 1}, 386.0, -15, 0.0, 9, 0.0},
181
+ {{ 1, 0, 0, -2, 1}, -31.7, -13, 0.0, 7, 0.0},
182
+ {{ 0, -1, 0, 0, 1}, -346.6, -12, 0.0, 6, 0.0},
183
+ {{ 2, 0, -2, 0, 0},-1095.2, 11, 0.0, 0, 0.0},
184
+ {{ -1, 0, 2, 2, 1}, 9.5, -10, 0.0, 5, 0.0},
185
+ {{ 1, 0, 2, 2, 2}, 5.6, -8, 0.0, 3, 0.0},
186
+ {{ 0, -1, 2, 0, 2}, 14.2, -7, 0.0, 3, 0.0},
187
+ {{ 0, 0, 2, 2, 1}, 7.1, -7, 0.0, 3, 0.0},
188
+ {{ 1, 1, 0, -2, 0}, -34.8, -7, 0.0, 0, 0.0},
189
+ {{ 0, 1, 2, 0, 2}, 13.2, 7, 0.0, -3, 0.0},
190
+ {{ -2, 0, 0, 2, 1}, -199.8, -6, 0.0, 3, 0.0},
191
+ {{ 0, 0, 0, 2, 1}, 14.8, -6, 0.0, 3, 0.0},
192
+ {{ 2, 0, 2, -2, 2}, 12.8, 6, 0.0, -3, 0.0},
193
+ {{ 1, 0, 0, 2, 0}, 9.6, 6, 0.0, 0, 0.0},
194
+ {{ 1, 0, 2, -2, 1}, 23.9, 6, 0.0, -3, 0.0},
195
+ {{ 0, 0, 0, -2, 1}, -14.7, -5, 0.0, 3, 0.0},
196
+ {{ 0, -1, 2, -2, 1}, 346.6, -5, 0.0, 3, 0.0},
197
+ {{ 2, 0, 2, 0, 1}, 6.9, -5, 0.0, 3, 0.0},
198
+ {{ 1, -1, 0, 0, 0}, 29.8, 5, 0.0, 0, 0.0},
199
+ {{ 1, 0, 0, -1, 0}, 411.8, -4, 0.0, 0, 0.0},
200
+ {{ 0, 0, 0, 1, 0}, 29.5, -4, 0.0, 0, 0.0},
201
+ {{ 0, 1, 0, -2, 0}, -15.4, -4, 0.0, 0, 0.0},
202
+ {{ 1, 0, -2, 0, 0}, -26.9, 4, 0.0, 0, 0.0},
203
+ {{ 2, 0, 0, -2, 1}, 212.3, 4, 0.0, -2, 0.0},
204
+ {{ 0, 1, 2, -2, 1}, 119.6, 4, 0.0, -2, 0.0},
205
+ {{ 1, 1, 0, 0, 0}, 25.6, -3, 0.0, 0, 0.0},
206
+ {{ 1, -1, 0, -1, 0},-3232.9, -3, 0.0, 0, 0.0},
207
+ {{ -1, -1, 2, 2, 2}, 9.8, -3, 0.0, 1, 0.0},
208
+ {{ 0, -1, 2, 2, 2}, 7.2, -3, 0.0, 1, 0.0},
209
+ {{ 1, -1, 2, 0, 2}, 9.4, -3, 0.0, 1, 0.0},
210
+ {{ 3, 0, 2, 0, 2}, 5.5, -3, 0.0, 1, 0.0},
211
+ {{ -2, 0, 2, 0, 2}, 1615.7, -3, 0.0, 1, 0.0},
212
+ {{ 1, 0, 2, 0, 0}, 9.1, 3, 0.0, 0, 0.0},
213
+ {{ -1, 0, 2, 4, 2}, 5.8, -2, 0.0, 1, 0.0},
214
+ {{ 1, 0, 0, 0, 2}, 27.8, -2, 0.0, 1, 0.0},
215
+ {{ -1, 0, 2, -2, 1}, -32.6, -2, 0.0, 1, 0.0},
216
+ {{ 0, -2, 2, -2, 1}, 6786.3, -2, 0.0, 1, 0.0},
217
+ {{ -2, 0, 0, 0, 1}, -13.7, -2, 0.0, 1, 0.0},
218
+ {{ 2, 0, 0, 0, 1}, 13.8, 2, 0.0, -1, 0.0},
219
+ {{ 3, 0, 0, 0, 0}, 9.2, 2, 0.0, 0, 0.0},
220
+ {{ 1, 1, 2, 0, 2}, 8.9, 2, 0.0, -1, 0.0},
221
+ {{ 0, 0, 2, 1, 2}, 9.3, 2, 0.0, -1, 0.0},
222
+ {{ 1, 0, 0, 2, 1}, 9.6, -1, 0.0, 0, 0.0},
223
+ {{ 1, 0, 2, 2, 1}, 5.6, -1, 0.0, 1, 0.0},
224
+ {{ 1, 1, 0, -2, 1}, -34.7, -1, 0.0, 0, 0.0},
225
+ {{ 0, 1, 0, 2, 0}, 14.2, -1, 0.0, 0, 0.0},
226
+ {{ 0, 1, 2, -2, 0}, 117.5, -1, 0.0, 0, 0.0},
227
+ {{ 0, 1, -2, 2, 0}, -329.8, -1, 0.0, 0, 0.0},
228
+ {{ 1, 0, -2, 2, 0}, 23.8, -1, 0.0, 0, 0.0},
229
+ {{ 1, 0, -2, -2, 0}, -9.5, -1, 0.0, 0, 0.0},
230
+ {{ 1, 0, 2, -2, 0}, 32.8, -1, 0.0, 0, 0.0},
231
+ {{ 1, 0, 0, -4, 0}, -10.1, -1, 0.0, 0, 0.0},
232
+ {{ 2, 0, 0, -4, 0}, -15.9, -1, 0.0, 0, 0.0},
233
+ {{ 0, 0, 2, 4, 2}, 4.8, -1, 0.0, 0, 0.0},
234
+ {{ 0, 0, 2, -1, 2}, 25.4, -1, 0.0, 0, 0.0},
235
+ {{ -2, 0, 2, 4, 2}, 7.3, -1, 0.0, 1, 0.0},
236
+ {{ 2, 0, 2, 2, 2}, 4.7, -1, 0.0, 0, 0.0},
237
+ {{ 0, -1, 2, 0, 1}, 14.2, -1, 0.0, 0, 0.0},
238
+ {{ 0, 0, -2, 0, 1}, -13.6, -1, 0.0, 0, 0.0},
239
+ {{ 0, 0, 4, -2, 2}, 12.7, 1, 0.0, 0, 0.0},
240
+ {{ 0, 1, 0, 0, 2}, 409.2, 1, 0.0, 0, 0.0},
241
+ {{ 1, 1, 2, -2, 2}, 22.5, 1, 0.0, -1, 0.0},
242
+ {{ 3, 0, 2, -2, 2}, 8.7, 1, 0.0, 0, 0.0},
243
+ {{ -2, 0, 2, 2, 2}, 14.6, 1, 0.0, -1, 0.0},
244
+ {{ -1, 0, 0, 0, 2}, -27.3, 1, 0.0, -1, 0.0},
245
+ {{ 0, 0, -2, 2, 1}, -169.0, 1, 0.0, 0, 0.0},
246
+ {{ 0, 1, 2, 0, 1}, 13.1, 1, 0.0, 0, 0.0},
247
+ {{ -1, 0, 4, 0, 2}, 9.1, 1, 0.0, 0, 0.0},
248
+ {{ 2, 1, 0, -2, 0}, 131.7, 1, 0.0, 0, 0.0},
249
+ {{ 2, 0, 0, 2, 0}, 7.1, 1, 0.0, 0, 0.0},
250
+ {{ 2, 0, 2, -2, 1}, 12.8, 1, 0.0, -1, 0.0},
251
+ {{ 2, 0, -2, 0, 1}, -943.2, 1, 0.0, 0, 0.0},
252
+ {{ 1, -1, 0, -2, 0}, -29.3, 1, 0.0, 0, 0.0},
253
+ {{ -1, 0, 0, 1, 1}, -388.3, 1, 0.0, 0, 0.0},
254
+ {{ -1, -1, 0, 2, 1}, 35.0, 1, 0.0, 0, 0.0},
255
+ {{ 0, 1, 0, 1, 0}, 27.3, 1, 0.0, 0, 0.0},
256
+ };
257
+ for(std::size_t i(0); i < sizeof(nut) / sizeof(nut[0]); i++){
258
+ FloatT ang(0);
259
+ for(int j(0); j < 5; j++){
260
+ ang += f[j] * nut[i].arg[j];
261
+ }
262
+ dpsi += (t * nut[i].lng_At + nut[i].lng_A) * std::sin(ang);
263
+ deps += (t * nut[i].obl_Bt + nut[i].obl_B) * std::cos(ang);
264
+ }
265
+ dpsi = AS2RAD(dpsi * 1E-4); // 0.1 mas -> rad
266
+ deps = AS2RAD(deps * 1E-4);
267
+ }
268
+ } nut_iau1980(t, ast_args.f);
269
+
270
+ FloatT eps(AS2RAD(84381.448)
271
+ - t * AS2RAD(46.8150)
272
+ - t2 * AS2RAD( 0.00059)
273
+ + t3 * AS2RAD( 0.001813));
274
+
275
+ // IAU 1980 nutation
276
+ // Nutation=R1(-epsilon-de)*R3(dpsi)*R1(epsilon)
277
+ rot_t r_n(rot_t().then_x(eps).then_z(-nut_iau1980.dpsi).then_x(-eps - nut_iau1980.deps));
278
+
279
+ rot_t r_s;
280
+ #if 1
281
+ { // Greenwich apparent sidereal time (rad)
282
+ // IERS Conventions 1996 Chapter.5 pp.21
283
+ FloatT gmst(
284
+ t_gps.greenwich_mean_sidereal_time_sec_ires1996(opt.delta_UT1)
285
+ * (M_PI * 2 / 86400)); // [rad]
286
+ FloatT gast(gmst // GAST or GST
287
+ + nut_iau1980.dpsi * std::cos(eps)
288
+ + AS2RAD(0.00264) * std::sin(ast_args.f[4])
289
+ + AS2RAD(0.000063) * std::sin(ast_args.f[4] * 2));
290
+ r_s.then_z(gast);
291
+ }
292
+ #else
293
+ // Eq.(5.14) of IERS 2010(Technical Note No.36)
294
+ // TODO t_date will be corrected to make it based on UT1
295
+ r_s.then_z(t_gps.earth_rotation_angle(opt.delta_UT1));
296
+ #endif
297
+
298
+ // Polar motion
299
+ rot_t r_m(rot_t().then_x(-opt.y_p).then_y(-opt.x_p));
300
+
301
+ // eci to ecef transformation matrix
302
+ return r_p.then(r_n).then(r_s).then(r_m);
303
+ #undef DEG2RAD
304
+ #undef DMS2RAD
305
+ #undef AS2RAD
306
+ }
307
+
308
+ static Vector3<FloatT> sun_direction_ecef(
309
+ const GPS_Time<FloatT> &t,
310
+ FloatT *r = NULL){
311
+ // @see https://en.wikipedia.org/wiki/Position_of_the_Sun
312
+ // @see https://astronomy.stackexchange.com/a/37199
313
+ #define DEG2RAD(deg) ((deg) / 180 * M_PI)
314
+ FloatT n(t.julian_date_2000()/* + 19.0 / 86400*/); // GPS_Time -> Julian day -> J2000.0
315
+ FloatT L(DEG2RAD(280.4606184) + DEG2RAD(36000.77005361 / 36525) * n); // mean longitude
316
+ FloatT g(DEG2RAD(357.5277233) + DEG2RAD(35999.05034 / 36525) * n); // mean anomaly
317
+ FloatT lambda(L
318
+ + DEG2RAD(1.914666471) * std::sin(g)
319
+ + DEG2RAD(0.918994643) * std::sin(g * 2)); // ecliptic longitude of the Sun
320
+ FloatT clambda(std::cos(lambda)), slambda(std::sin(lambda));
321
+ FloatT epsilon(DEG2RAD(23.43929) - DEG2RAD(46.8093/3600/36525) * n); // eccentricity
322
+ FloatT cepsilon(std::cos(epsilon)), sepsilon(std::sin(epsilon));
323
+ if(r){
324
+ *r = (1.000140612 - 0.016708617 * std::cos(g) - 0.000139589 * std::cos(g * 2))
325
+ * 1.495978707E11; // AU -> meter
326
+ }
327
+ FloatT dir[] = {clambda, cepsilon * slambda, sepsilon * slambda};
328
+ eci2ecef(t).apply(dir); // ECI -> ECEF
329
+ return Vector3<FloatT>(dir);
330
+ #undef DEG2RAD
331
+ }
332
+
333
+ static System_XYZ<FloatT> sun_position_ecef(const GPS_Time<FloatT> &t){
334
+ FloatT r;
335
+ return System_XYZ<FloatT>(sun_direction_ecef(t, &r) * r);
336
+ }
337
+
338
+ int move_to_antenna_position(
339
+ SP3_Product<FloatT> &sp3,
340
+ const std::map<char, std::string> &freq_table = std::map<char, std::string>()) const {
341
+
342
+ static const struct default_opt_t {
343
+ std::map<char, std::string> freq_table;
344
+ default_opt_t() : freq_table() {
345
+ freq_table['G'] = "G01"; // GPS L1; others are "G02"(L2), "G05"(L5)
346
+ freq_table['R'] = "R01"; // GLONASS G1; other is "R02"(G2)
347
+ freq_table['E'] = "E01"; // Galileo E1; others are "E05"(E5a), "E07"(E5b), "E08"(E5a+E5b), "E06"(E6)
348
+ freq_table['C'] = "C01"; // Compass E1; others are "C02"(E2), "C07"(E5b), "C06"(E6)
349
+ freq_table['J'] = "J01"; // QZSS L1; others are "J02"(L2), "J05"(L5), "J06"(LEX)
350
+ freq_table['S'] = "S01"; // SBAS L1; other is "S05"(L5)
351
+ }
352
+ } default_opt;
353
+
354
+ std::map<char, std::string> freq_table2(default_opt.freq_table);
355
+ freq_table2.insert(freq_table.begin(), freq_table.end());
356
+
357
+ int moved(0);
358
+ for(typename prop_t::const_iterator it_ant(prop.begin()), it_ant_end(prop.end());
359
+ it_ant != it_ant_end; ++it_ant){
360
+ int sat_id;
361
+ if(!it_ant->is_satellite(&sat_id)){continue;} // If not satellite antenna, then skip.
362
+
363
+ typename std::map<std::string, per_freq_t>::const_iterator it_freq(
364
+ it_ant->freq.find(freq_table2[it_ant->serial[0]]));
365
+ if(it_freq == it_ant->freq.end()){continue;} // If target frequency is not registered, then skip.
366
+
367
+ typename SP3_Product<FloatT>::satellites_t::iterator it_sat(sp3.satellites.find(sat_id));
368
+ if(it_sat == sp3.satellites.end()){continue;} // If satellite is not registered, then skip.
369
+
370
+ for(typename SP3_Product<FloatT>::per_satellite_t::history_t::iterator
371
+ it_pos(it_sat->second.pos_history.begin()), it_pos_end(it_sat->second.pos_history.end());
372
+ it_pos != it_pos_end; ++it_pos){ // ascending order of target time
373
+ if(it_pos->first > it_ant->valid_until){break;} // skip if target time of position is after valid_until.
374
+ if(it_pos->first < it_ant->valid_from){continue;} // skip if target time of position is before valid_from.
375
+
376
+ // Calculate satellite frame unit vector in ECEF coordinate
377
+ Vector3<FloatT> z_dir(it_pos->second.xyz / -it_pos->second.xyz.abs());
378
+ Vector3<FloatT> y_dir(z_dir * sun_direction_ecef(it_pos->first));
379
+ y_dir /= y_dir.abs();
380
+ Vector3<FloatT> x_dir(y_dir * z_dir);
381
+
382
+ // correction
383
+ const FloatT (&xyz)[3](it_freq->second.north_east_up);
384
+ Vector3<FloatT> antenna_ecef(x_dir * xyz[0] + y_dir * xyz[1] + z_dir * xyz[2]);
385
+ it_pos->second.xyz += antenna_ecef;
386
+
387
+ ++moved;
388
+ }
389
+ }
390
+
391
+ return moved;
392
+ }
393
+ };
394
+
395
+ template <class FloatT>
396
+ class ANTEX_Reader {
397
+ protected:
398
+ typename TextHelper<>::crlf_stream_t src;
399
+ public:
400
+ ANTEX_Reader(std::istream &in) : src(in) {}
401
+
402
+ bool has_next() {
403
+ return !(src.eof() || src.fail());
404
+ }
405
+
406
+ struct type_serial_t {
407
+ char type[20];
408
+ char serial_or_prn[20];
409
+ char blank_or_code[10];
410
+ char blank_or_cospar[10];
411
+ template <std::size_t N>
412
+ static std::string to_string(const char (&c)[N]) {
413
+ std::string buf(c, N);
414
+ return buf.substr(0, buf.find_last_not_of(' ') + 1);
415
+ }
416
+ };
417
+ struct time_t {
418
+ int year, month, day_of_month, hour, minute;
419
+ FloatT second;
420
+ operator std::tm() const {
421
+ std::tm res = {
422
+ (int)second, minute, hour, day_of_month, month - 1, year - 1900, 0, 0, 0
423
+ };
424
+ std::mktime(&res);
425
+ return res;
426
+ }
427
+ operator GPS_Time<FloatT>() const {
428
+ return GPS_Time<FloatT>(std::tm(*this));
429
+ }
430
+ };
431
+ struct freq_t {
432
+ char freq_name[3];
433
+ };
434
+ struct neu_t {
435
+ FloatT values[3];
436
+ };
437
+
438
+ struct parsed_t {
439
+ enum {
440
+ ANTEX_VERSION_SYST,
441
+ PCV_TYPE_REFANT,
442
+ COMMENT,
443
+ END_OF_HEADER,
444
+ START_OF_ANTENNA,
445
+ TYPE_SERIAL_NO,
446
+ METH_BY_NUM_DATE,
447
+ DAZI,
448
+ ZEN1_ZEN2_DZEN,
449
+ NUM_OF_FREQUENCIES,
450
+ VALID_FROM,
451
+ VALID_UNTIL,
452
+ SINEX_CODE,
453
+ START_OF_FREQUENCY,
454
+ NORTH_EAST_UP,
455
+ NOAZI_VALUES,
456
+ DAZI_VALUES,
457
+ END_OF_FREQUENCY,
458
+ START_OF_FREQ_RMS,
459
+ END_OF_FREQ_RMS,
460
+ END_OF_ANTENNA,
461
+ IGNORABLE,
462
+ UNKNOWN,
463
+ } type;
464
+ union {
465
+ type_serial_t type_serial;
466
+ time_t time;
467
+ freq_t freq;
468
+ neu_t neu;
469
+ } item;
470
+ };
471
+
472
+ static const typename TextHelper<>::convert_item_t type_serial_items[4];
473
+ static const typename TextHelper<>::convert_item_t time_items[6];
474
+ static const typename TextHelper<>::convert_item_t freq_items[1];
475
+ static const typename TextHelper<>::convert_item_t neu_items[3];
476
+
477
+ parsed_t parse_line() {
478
+ parsed_t res = {parsed_t::UNKNOWN, {0}};
479
+
480
+ char buf[0x400] = {0};
481
+ src.getline(buf, sizeof(buf));
482
+ std::string line(buf);
483
+
484
+ if(line.size() == 0){
485
+ res.type = parsed_t::IGNORABLE;
486
+ return res;
487
+ }
488
+ line = line.substr(0, line.find_last_not_of(" ") + 1);
489
+
490
+ if(line.size() > 60){
491
+ std::string label(line.substr(60));
492
+
493
+ if(label.compare("ANTEX VERSION / SYST") == 0){
494
+ res.type = parsed_t::ANTEX_VERSION_SYST;
495
+ return res;
496
+ }
497
+ if(label.compare("PCV TYPE / REFANT") == 0){
498
+ res.type = parsed_t::PCV_TYPE_REFANT;
499
+ return res;
500
+ }
501
+ if(label.compare("COMMENT") == 0){
502
+ res.type = parsed_t::COMMENT;
503
+ return res;
504
+ }
505
+ if(label.compare("END OF HEADER") == 0){
506
+ res.type = parsed_t::END_OF_HEADER;
507
+ return res;
508
+ }
509
+ if(label.compare("START OF ANTENNA") == 0){
510
+ res.type = parsed_t::START_OF_ANTENNA;
511
+ return res;
512
+ }
513
+ if(label.compare("TYPE / SERIAL NO") == 0){
514
+ TextHelper<>::str2val(type_serial_items, line, &res.item);
515
+ res.type = parsed_t::TYPE_SERIAL_NO;
516
+ return res;
517
+ }
518
+ if(label.compare("METH / BY / # / DATE") == 0){
519
+ res.type = parsed_t::METH_BY_NUM_DATE;
520
+ return res;
521
+ }
522
+ if(label.compare("DAZI") == 0){
523
+ res.type = parsed_t::DAZI;
524
+ return res;
525
+ }
526
+ if(label.compare("ZEN1 / ZEN2 / DZEN") == 0){
527
+ res.type = parsed_t::ZEN1_ZEN2_DZEN;
528
+ return res;
529
+ }
530
+ if(label.compare("# OF FREQUENCIES") == 0){
531
+ res.type = parsed_t::NUM_OF_FREQUENCIES;
532
+ return res;
533
+ }
534
+ if(label.compare("VALID FROM") == 0){
535
+ TextHelper<>::str2val(time_items, line, &res.item);
536
+ res.type = parsed_t::VALID_FROM;
537
+ return res;
538
+ }
539
+ if(label.compare("VALID UNTIL") == 0){
540
+ TextHelper<>::str2val(time_items, line, &res.item);
541
+ res.type = parsed_t::VALID_UNTIL;
542
+ return res;
543
+ }
544
+ if(label.compare("SINEX CODE") == 0){
545
+ res.type = parsed_t::SINEX_CODE;
546
+ return res;
547
+ }
548
+ if(label.compare("START OF FREQUENCY") == 0){
549
+ TextHelper<>::str2val(freq_items, line, &res.item);
550
+ res.type = parsed_t::START_OF_FREQUENCY;
551
+ return res;
552
+ }
553
+ if(label.compare("NORTH / EAST / UP") == 0){
554
+ TextHelper<>::str2val(neu_items, line, &res.item);
555
+ res.type = parsed_t::NORTH_EAST_UP;
556
+ return res;
557
+ }
558
+ if(label.compare("END OF FREQUENCY") == 0){
559
+ res.type = parsed_t::END_OF_FREQUENCY;
560
+ return res;
561
+ }
562
+ if(label.compare("START OF FREQ RMS") == 0){
563
+ TextHelper<>::str2val(freq_items, line, &res.item);
564
+ res.type = parsed_t::START_OF_FREQ_RMS;
565
+ return res;
566
+ }
567
+ if(label.compare("END OF FREQ RMS") == 0){
568
+ res.type = parsed_t::END_OF_FREQ_RMS;
569
+ return res;
570
+ }
571
+ if(label.compare("END OF ANTENNA") == 0){
572
+ res.type = parsed_t::END_OF_ANTENNA;
573
+ return res;
574
+ }
575
+ }
576
+ if(line.substr(3, 5).compare("NOAZI") == 0){
577
+ // (Values of a non-azimuth-dependent pattern)
578
+ // or (Rms values of a non-azimuth-dependent pattern)
579
+ res.type = parsed_t::NOAZI_VALUES;
580
+ return res;
581
+ }
582
+ for(double d; !!(std::stringstream(line.substr(0, 8)) >> d); ){
583
+ // (Values of an azimuth-dependent pattern)
584
+ // or (Rms Values of an azimuth-dependent pattern)
585
+ res.type = parsed_t::DAZI_VALUES;
586
+ return res;
587
+ }
588
+
589
+ res.type = parsed_t::UNKNOWN;
590
+ return res;
591
+ }
592
+
593
+ static int read_all(std::istream &in, ANTEX_Product<FloatT> &dst) {
594
+ ANTEX_Reader<FloatT> src(in);
595
+ enum {
596
+ INITIAL_STATE = 1,
597
+ ON_HEADER,
598
+ FREE_STATE,
599
+ ON_ANTENNA,
600
+ ON_FREQUENCY,
601
+ ON_FREQUENCY_RMS,
602
+ } state = INITIAL_STATE;
603
+
604
+ typedef typename ANTEX_Product<FloatT>::antenna_t antenna_t;
605
+ antenna_t *antenna;
606
+ typedef typename ANTEX_Product<FloatT>::per_freq_t per_freq_t;
607
+ per_freq_t *per_freq;
608
+
609
+ while(src.has_next()){
610
+ parsed_t parsed(src.parse_line());
611
+ if(parsed.type == parsed_t::IGNORABLE){continue;}
612
+ switch(state){
613
+ case INITIAL_STATE:
614
+ if(parsed.type != parsed_t::ANTEX_VERSION_SYST){
615
+ return -INITIAL_STATE; // error
616
+ }
617
+ state = ON_HEADER;
618
+ break;
619
+ case ON_HEADER:
620
+ switch(parsed.type){
621
+ case parsed_t::COMMENT: break;
622
+ case parsed_t::PCV_TYPE_REFANT: break;
623
+ case parsed_t::END_OF_HEADER:
624
+ state = FREE_STATE;
625
+ break;
626
+ default: // error
627
+ return -ON_HEADER;
628
+ }
629
+ break;
630
+ case FREE_STATE:
631
+ if(parsed.type != parsed_t::START_OF_ANTENNA){
632
+ return -FREE_STATE; // error
633
+ }
634
+ state = ON_ANTENNA;
635
+ dst.prop.resize(dst.prop.size() + 1);
636
+ antenna = &dst.prop.back();
637
+ break;
638
+ case ON_ANTENNA:
639
+ switch(parsed.type){
640
+ case parsed_t::TYPE_SERIAL_NO: {
641
+ antenna->type = type_serial_t::to_string(parsed.item.type_serial.type);
642
+ antenna->serial = type_serial_t::to_string(parsed.item.type_serial.serial_or_prn);
643
+ antenna->valid_until = GPS_Time<FloatT>::now();
644
+ break;
645
+ }
646
+ case parsed_t::METH_BY_NUM_DATE: break;
647
+ case parsed_t::DAZI: break;
648
+ case parsed_t::ZEN1_ZEN2_DZEN: break;
649
+ case parsed_t::NUM_OF_FREQUENCIES: break;
650
+ case parsed_t::VALID_FROM:
651
+ antenna->valid_from = parsed.item.time;
652
+ break;
653
+ case parsed_t::VALID_UNTIL:
654
+ antenna->valid_until = parsed.item.time;
655
+ break;
656
+ case parsed_t::SINEX_CODE: break;
657
+ case parsed_t::COMMENT: break;
658
+ case parsed_t::START_OF_FREQUENCY:
659
+ state = ON_FREQUENCY;
660
+ per_freq = &antenna->freq[
661
+ std::string(parsed.item.freq.freq_name, sizeof(parsed.item.freq.freq_name))];
662
+ break;
663
+ case parsed_t::START_OF_FREQ_RMS:
664
+ per_freq = &antenna->freq_rms[
665
+ std::string(parsed.item.freq.freq_name, sizeof(parsed.item.freq.freq_name))];
666
+ state = ON_FREQUENCY_RMS;
667
+ break;
668
+ case parsed_t::END_OF_ANTENNA:
669
+ state = FREE_STATE;
670
+ break;
671
+ default: // error
672
+ return -ON_ANTENNA;
673
+ }
674
+ break;
675
+ case ON_FREQUENCY:
676
+ case ON_FREQUENCY_RMS:
677
+ switch(parsed.type){
678
+ case parsed_t::NORTH_EAST_UP:
679
+ for(std::size_t i(0);
680
+ i < sizeof(per_freq->north_east_up) / sizeof(per_freq->north_east_up[0]);
681
+ ++ i){
682
+ per_freq->north_east_up[i] = parsed.item.neu.values[i] * 1E-3; // mm -> m
683
+ }
684
+ break;
685
+ case parsed_t::NOAZI_VALUES: break; // TODO variable size, implement callback of parse_line()?
686
+ case parsed_t::DAZI_VALUES: break; // TODO
687
+ case parsed_t::END_OF_FREQUENCY:
688
+ if(state == ON_FREQUENCY){state = ON_ANTENNA; break;}
689
+ return -state;
690
+ case parsed_t::END_OF_FREQ_RMS:
691
+ if(state != ON_FREQUENCY_RMS){state = ON_ANTENNA; break;}
692
+ return -state;
693
+ default: // error
694
+ return -state;
695
+ }
696
+ break;
697
+ }
698
+ }
699
+ return dst.prop.size();
700
+ }
701
+ };
702
+
703
+ #define GEN_C(offset, length, container_type, container_member) \
704
+ {TextHelper<>::template format_t<char>::c, offset, length, \
705
+ offsetof(container_type, container_member), 0}
706
+ #define GEN_I(offset, length, container_type, container_member) \
707
+ {TextHelper<>::template format_t<int>::d, offset, length, \
708
+ offsetof(container_type, container_member), 0}
709
+ #define GEN_F(offset, length, container_type, container_member, precision) \
710
+ {TextHelper<>::template format_t<FloatT>::f, offset, length, \
711
+ offsetof(container_type, container_member), precision}
712
+
713
+ template <class FloatT>
714
+ const typename TextHelper<>::convert_item_t ANTEX_Reader<FloatT>::type_serial_items[4] = {
715
+ GEN_C( 0, 20, type_serial_t, type), // not null-terminated, following the same
716
+ GEN_C(20, 20, type_serial_t, serial_or_prn),
717
+ GEN_C(40, 10, type_serial_t, blank_or_code),
718
+ GEN_C(50, 10, type_serial_t, blank_or_cospar),
719
+ };
720
+
721
+ template <class FloatT>
722
+ const typename TextHelper<>::convert_item_t ANTEX_Reader<FloatT>::time_items[6] = {
723
+ GEN_I( 0, 6, time_t, year),
724
+ GEN_I( 6, 6, time_t, month),
725
+ GEN_I(12, 6, time_t, day_of_month),
726
+ GEN_I(18, 6, time_t, hour),
727
+ GEN_I(24, 6, time_t, minute),
728
+ GEN_F(30, 13, time_t, second, 7),
729
+ };
730
+
731
+ template <class FloatT>
732
+ const typename TextHelper<>::convert_item_t ANTEX_Reader<FloatT>::freq_items[1] = {
733
+ GEN_C( 3, 3, freq_t, freq_name),
734
+ };
735
+
736
+ template <class FloatT>
737
+ const typename TextHelper<>::convert_item_t ANTEX_Reader<FloatT>::neu_items[3] = {
738
+ GEN_F( 0, 10, neu_t, values[0], 2),
739
+ GEN_F(10, 10, neu_t, values[1], 2),
740
+ GEN_F(20, 10, neu_t, values[2], 2),
741
+ };
742
+
743
+ #undef GEN_C
744
+ #undef GEN_I
745
+ #undef GEN_F
746
+
747
+ #endif /* #define __ANTEX_H__ */