gps_pvt 0.3.3 → 0.5.0
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/README.md +4 -3
- data/Rakefile +2 -0
- data/exe/gps_pvt +37 -5
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +5595 -131
- data/ext/ninja-scan-light/tool/algorithm/integral.h +91 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +1329 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +306 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +7 -0
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +389 -4
- data/ext/ninja-scan-light/tool/swig/GPS.i +217 -6
- data/{sig/gps_pvt.rbs → gps_pvt.rbs} +0 -0
- data/lib/gps_pvt/receiver.rb +68 -16
- data/lib/gps_pvt/util.rb +32 -0
- data/lib/gps_pvt/version.rb +1 -1
- metadata +7 -3
@@ -0,0 +1,1329 @@
|
|
1
|
+
/*
|
2
|
+
* Copyright (c) 2016, 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
|
+
#ifndef __GLONASS_H__
|
33
|
+
#define __GLONASS_H__
|
34
|
+
|
35
|
+
/** @file
|
36
|
+
* @brief GLONASS ICD definitions
|
37
|
+
*/
|
38
|
+
|
39
|
+
|
40
|
+
#include <ctime>
|
41
|
+
#define _USE_MATH_DEFINES
|
42
|
+
#include <cmath>
|
43
|
+
#include <climits>
|
44
|
+
#include <cstdlib>
|
45
|
+
#include <ctime>
|
46
|
+
|
47
|
+
#include "GPS.h"
|
48
|
+
#include "algorithm/integral.h"
|
49
|
+
|
50
|
+
template <class FloatT = double>
|
51
|
+
class GLONASS_SpaceNode {
|
52
|
+
public:
|
53
|
+
typedef FloatT float_t;
|
54
|
+
|
55
|
+
static const float_t light_speed;
|
56
|
+
static const float_t L1_frequency_base;
|
57
|
+
static const float_t L1_frequency_gap;
|
58
|
+
static const float_t L2_frequency_base;
|
59
|
+
static const float_t L2_frequency_gap;
|
60
|
+
|
61
|
+
public:
|
62
|
+
typedef GLONASS_SpaceNode<float_t> self_t;
|
63
|
+
|
64
|
+
typedef unsigned char u8_t;
|
65
|
+
typedef signed char s8_t;
|
66
|
+
typedef unsigned short u16_t;
|
67
|
+
typedef signed short s16_t;
|
68
|
+
typedef unsigned int u32_t;
|
69
|
+
typedef signed int s32_t;
|
70
|
+
|
71
|
+
typedef int int_t;
|
72
|
+
typedef unsigned int uint_t;
|
73
|
+
|
74
|
+
static float_t L1_frequency(const int_t &freq_ch) {
|
75
|
+
return L1_frequency_base + L1_frequency_gap * freq_ch;
|
76
|
+
}
|
77
|
+
static float_t L2_frequency(const int_t &freq_ch) {
|
78
|
+
return L2_frequency_base + L2_frequency_gap * freq_ch;
|
79
|
+
}
|
80
|
+
|
81
|
+
typedef typename GPS_SpaceNode<float_t>::DataParser DataParser;
|
82
|
+
|
83
|
+
template <class InputT,
|
84
|
+
int EffectiveBits = sizeof(InputT) * CHAR_BIT,
|
85
|
+
int PaddingBits_MSB = (int)sizeof(InputT) * CHAR_BIT - EffectiveBits>
|
86
|
+
struct BroadcastedMessage : public DataParser {
|
87
|
+
#define convert_u(bits, offset_bits, length, name) \
|
88
|
+
static u ## bits ## _t name(const InputT *buf){ \
|
89
|
+
return \
|
90
|
+
DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
|
91
|
+
buf, offset_bits, length); \
|
92
|
+
}
|
93
|
+
#define convert_s(bits, offset_bits, length, name) \
|
94
|
+
static s ## bits ## _t name(const InputT *buf){ \
|
95
|
+
u ## bits ## _t temp( \
|
96
|
+
DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
|
97
|
+
buf, offset_bits, length)); \
|
98
|
+
static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); /* MSB is sign bit */ \
|
99
|
+
return (temp & mask) ? ((s ## bits ## _t)-1 * (temp & (mask - 1))) : temp; \
|
100
|
+
}
|
101
|
+
convert_u( 8, 1, 4, m);
|
102
|
+
convert_u( 8, 77, 8, KX);
|
103
|
+
|
104
|
+
struct String1 {
|
105
|
+
convert_u( 8, 7, 2, P1);
|
106
|
+
convert_u(16, 9, 12, t_k);
|
107
|
+
convert_s(32, 21, 24, xn_dot);
|
108
|
+
convert_s( 8, 45, 5, xn_ddot);
|
109
|
+
convert_s(32, 50, 27, xn);
|
110
|
+
};
|
111
|
+
struct String2 {
|
112
|
+
convert_u( 8, 5, 3, B_n);
|
113
|
+
convert_u( 8, 8, 1, P2);
|
114
|
+
convert_u( 8, 9, 7, t_b);
|
115
|
+
convert_s(32, 21, 24, yn_dot);
|
116
|
+
convert_s( 8, 45, 5, yn_ddot);
|
117
|
+
convert_s(32, 50, 27, yn);
|
118
|
+
};
|
119
|
+
struct String3 {
|
120
|
+
convert_u( 8, 5, 1, P3);
|
121
|
+
convert_s(16, 6, 11, gamma_n);
|
122
|
+
convert_u( 8, 18, 2, p);
|
123
|
+
convert_u( 8, 20, 1, l_n);
|
124
|
+
convert_s(32, 21, 24, zn_dot);
|
125
|
+
convert_s( 8, 45, 5, zn_ddot);
|
126
|
+
convert_s(32, 50, 27, zn);
|
127
|
+
};
|
128
|
+
struct String4 {
|
129
|
+
convert_s(32, 5, 22, tau_n);
|
130
|
+
convert_u( 8, 27, 5, delta_tau_n);
|
131
|
+
convert_u( 8, 32, 5, E_n);
|
132
|
+
convert_u( 8, 51, 1, P4);
|
133
|
+
convert_u( 8, 52, 4, F_T);
|
134
|
+
convert_u(16, 59, 11, N_T);
|
135
|
+
convert_u( 8, 70, 5, n);
|
136
|
+
convert_u( 8, 75, 2, M);
|
137
|
+
};
|
138
|
+
struct String5_Alnamac {
|
139
|
+
convert_u(16, 5, 11, NA);
|
140
|
+
convert_s(32, 16, 32, tau_c);
|
141
|
+
convert_u( 8, 49, 5, N_4);
|
142
|
+
convert_s(32, 54, 22, tau_GPS);
|
143
|
+
convert_u( 8, 76, 1, l_n);
|
144
|
+
};
|
145
|
+
|
146
|
+
struct String6_Alnamac { // String 6; same as 8, 10, 12, 14
|
147
|
+
convert_u( 8, 5, 1, C_n);
|
148
|
+
convert_u( 8, 6, 2, M_n);
|
149
|
+
convert_u( 8, 8, 5, nA);
|
150
|
+
convert_s(16, 13, 10, tauA_n);
|
151
|
+
convert_s(32, 23, 21, lambdaA_n);
|
152
|
+
convert_s(32, 44, 18, delta_iA_n);
|
153
|
+
convert_u(16, 62, 15, epsilonA_n);
|
154
|
+
};
|
155
|
+
struct String7_Alnamac { // String 7; same as 9, 11, 13, 15
|
156
|
+
convert_s(16, 5, 16, omegaA_n);
|
157
|
+
convert_u(32, 21, 21, tA_lambda_n);
|
158
|
+
convert_s(32, 42, 22, delta_TA_n);
|
159
|
+
convert_s( 8, 64, 7, delta_TA_dot_n);
|
160
|
+
convert_u( 8, 71, 5, HA_n);
|
161
|
+
convert_u( 8, 76, 1, l_n);
|
162
|
+
};
|
163
|
+
|
164
|
+
struct Frame5_String14 {
|
165
|
+
convert_s(16, 5, 11, B1);
|
166
|
+
convert_s(16, 16, 10, B2);
|
167
|
+
convert_u( 8, 26, 2, KP);
|
168
|
+
};
|
169
|
+
#undef convert_s
|
170
|
+
#undef convert_u
|
171
|
+
};
|
172
|
+
|
173
|
+
struct TimeProperties {
|
174
|
+
float_t tau_c; ///< GLONASS time scale correction to UTC(SU) time [s]
|
175
|
+
float_t tau_GPS; ///< fractional part of time difference from GLONASS time to GPS time [s], integer part should be derived from another source
|
176
|
+
struct date_t {
|
177
|
+
int_t year; // 20XX
|
178
|
+
int_t day_of_year; // Jan. 1st = 0
|
179
|
+
static const int_t days_m[12];
|
180
|
+
/**
|
181
|
+
* @return (std::tm)
|
182
|
+
*/
|
183
|
+
std::tm c_tm() const {
|
184
|
+
std::tm res = {0};
|
185
|
+
res.tm_year = year - 1900;
|
186
|
+
res.tm_yday = res.tm_mday = day_of_year; // tm_yday ranges [0, 365]
|
187
|
+
do{
|
188
|
+
if(res.tm_mday < days_m[0]){break;} // tm_mon ranges [0, 11], Check January
|
189
|
+
res.tm_mday -= days_m[0];
|
190
|
+
++res.tm_mon;
|
191
|
+
if((year % 400 == 0) || ((year % 4 == 0) && (year % 100 != 0))){ // is leap year?
|
192
|
+
if(res.tm_mday < (days_m[1] + 1)){break;} // Check February in leap year
|
193
|
+
res.tm_mday -= (days_m[1] + 1);
|
194
|
+
++res.tm_mon;
|
195
|
+
}
|
196
|
+
for(; res.tm_mon < (int)(sizeof(days_m) / sizeof(days_m[0])); ++res.tm_mon){
|
197
|
+
if(res.tm_mday < days_m[res.tm_mon]){break;}
|
198
|
+
res.tm_mday -= days_m[res.tm_mon];
|
199
|
+
}
|
200
|
+
}while(false);
|
201
|
+
++res.tm_mday; // tm_mday ranges [1, 31]
|
202
|
+
{
|
203
|
+
/* day of week according to (modified) Sakamoto's methods
|
204
|
+
* @see https://en.wikipedia.org/wiki/Determination_of_the_day_of_the_week#Sakamoto's_methods
|
205
|
+
*/
|
206
|
+
int_t y(year - 1); // year/1/1
|
207
|
+
res.tm_wday = (y + y/4 - y/100 + y/400 + day_of_year + 1) % 7;
|
208
|
+
}
|
209
|
+
return res;
|
210
|
+
}
|
211
|
+
static date_t from_c_tm(const std::tm &date){
|
212
|
+
static const struct days_t {
|
213
|
+
int_t sum_m[sizeof(days_m) / sizeof(days_m[0])];
|
214
|
+
days_t(){
|
215
|
+
sum_m[0] = 0;
|
216
|
+
for(unsigned int i(1); i < sizeof(days_m) / sizeof(days_m[0]); ++i){
|
217
|
+
sum_m[i] = sum_m[i - 1] + days_m[i - 1];
|
218
|
+
}
|
219
|
+
}
|
220
|
+
} days;
|
221
|
+
date_t res = {date.tm_year + 1900, days.sum_m[date.tm_mon] + date.tm_mday - 1};
|
222
|
+
if((date.tm_mon >= 2)
|
223
|
+
&& ((res.year % 400 == 0) || ((res.year % 4 == 0) && (res.year % 100 != 0)))){
|
224
|
+
++res.day_of_year; // leap year
|
225
|
+
}
|
226
|
+
return res;
|
227
|
+
}
|
228
|
+
static float_t julian_day(const std::tm &date){
|
229
|
+
// (5.48) of "Preliminary Orbit Determination" by Curtis, Howard D
|
230
|
+
// @see original Boulet(1991)
|
231
|
+
int y(date.tm_year + 1900), m(date.tm_mon + 1);
|
232
|
+
return 1721013.5 // expect all argument of (int) to be >= 0
|
233
|
+
+ 367 * y
|
234
|
+
- (int)(7 * (y + (int)((m + 9) / 12)) / 4)
|
235
|
+
+ (int)(275 * m / 9)
|
236
|
+
+ date.tm_mday;
|
237
|
+
}
|
238
|
+
static float_t Greenwich_sidereal_time_deg(
|
239
|
+
const std::tm &date, const float_t &ut_hr = 0){
|
240
|
+
// @see "Preliminary Orbit Determination" by Curtis, Howard D
|
241
|
+
float_t T0((julian_day(date) - 2451545) / 36525); // (5.49)
|
242
|
+
float_t theta_G0_deg( // (5.50)
|
243
|
+
100.4606184
|
244
|
+
+ 36000.77004 * T0
|
245
|
+
+ 0.000387933 * std::pow(T0, 2)
|
246
|
+
- 2.583E-8 * std::pow(T0, 3));
|
247
|
+
return theta_G0_deg + 360.98564724 * ut_hr / 24; // (5.51)
|
248
|
+
}
|
249
|
+
} date;
|
250
|
+
bool l_n; // health flag; 0 - healthy, 1 - malfunction
|
251
|
+
|
252
|
+
struct raw_t {
|
253
|
+
s32_t tau_c;
|
254
|
+
s32_t tau_GPS;
|
255
|
+
u8_t N_4;
|
256
|
+
u16_t NA;
|
257
|
+
bool l_n;
|
258
|
+
#define fetch_item(name) name = BroadcastedMessage< \
|
259
|
+
InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> \
|
260
|
+
:: String5_Alnamac :: name (src)
|
261
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
|
262
|
+
void update_string5(const InputT *src){
|
263
|
+
fetch_item(tau_c);
|
264
|
+
fetch_item(tau_GPS);
|
265
|
+
fetch_item(N_4);
|
266
|
+
fetch_item(NA);
|
267
|
+
fetch_item(l_n);
|
268
|
+
}
|
269
|
+
#undef fetch_item
|
270
|
+
|
271
|
+
enum {
|
272
|
+
SF_tau_c, SF_tau_GPS,
|
273
|
+
SF_NUM,
|
274
|
+
};
|
275
|
+
static const float_t sf[SF_NUM];
|
276
|
+
|
277
|
+
static date_t raw2date(const u8_t &N_4_, const u16_t &NA_) {
|
278
|
+
// @see A.3.1.3 (ver.5.1)
|
279
|
+
date_t res;
|
280
|
+
res.year = 1996 + 4 * (N_4_ - 1);
|
281
|
+
if(NA_ <= 366){
|
282
|
+
res.day_of_year = NA_ - 1;
|
283
|
+
}else if(NA_ <= 731){
|
284
|
+
res.year += 1;
|
285
|
+
res.day_of_year = NA_ - 367;
|
286
|
+
}else if(NA_ <= 1096){
|
287
|
+
res.year += 2;
|
288
|
+
res.day_of_year = NA_ - 732;
|
289
|
+
}else if(NA_ <= 1461){
|
290
|
+
res.year += 3;
|
291
|
+
res.day_of_year = NA_ - 1097;
|
292
|
+
}
|
293
|
+
return res;
|
294
|
+
}
|
295
|
+
|
296
|
+
operator TimeProperties() const {
|
297
|
+
TimeProperties res;
|
298
|
+
#define CONVERT(TARGET) \
|
299
|
+
{res.TARGET = sf[SF_ ## TARGET] * TARGET;}
|
300
|
+
CONVERT(tau_c);
|
301
|
+
CONVERT(tau_GPS);
|
302
|
+
res.date = raw2date(N_4, NA);
|
303
|
+
res.l_n = (l_n > 0);
|
304
|
+
#undef CONVERT
|
305
|
+
return res;
|
306
|
+
}
|
307
|
+
|
308
|
+
raw_t &operator=(const TimeProperties &t){
|
309
|
+
#define CONVERT(TARGET) \
|
310
|
+
{TARGET = (s32_t)((t.TARGET + 0.5 * sf[SF_ ## TARGET]) / sf[SF_ ## TARGET]);}
|
311
|
+
CONVERT(tau_c);
|
312
|
+
CONVERT(tau_GPS);
|
313
|
+
std::div_t divmod(std::div(t.date.year - 1996, 4));
|
314
|
+
N_4 = divmod.quot + 1;
|
315
|
+
NA = t.date.day_of_year + 1;
|
316
|
+
switch(divmod.rem){
|
317
|
+
case 1: NA += 366; break;
|
318
|
+
case 2: NA += 731; break;
|
319
|
+
case 3: NA += 1096; break;
|
320
|
+
}
|
321
|
+
l_n = (t.l_n ? 1 : 0);
|
322
|
+
#undef CONVERT
|
323
|
+
return *this;
|
324
|
+
}
|
325
|
+
};
|
326
|
+
|
327
|
+
bool is_equivalent(const TimeProperties &t) const {
|
328
|
+
do{
|
329
|
+
#define CHECK(TARGET) if(TARGET != t.TARGET){break;}
|
330
|
+
CHECK(l_n);
|
331
|
+
CHECK(date.year);
|
332
|
+
CHECK(date.day_of_year);
|
333
|
+
#undef CHECK
|
334
|
+
#define CHECK(TARGET) \
|
335
|
+
if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
336
|
+
CHECK(tau_c);
|
337
|
+
CHECK(tau_GPS);
|
338
|
+
#undef CHECK
|
339
|
+
return true;
|
340
|
+
}while(false);
|
341
|
+
return false;
|
342
|
+
}
|
343
|
+
};
|
344
|
+
|
345
|
+
class SatelliteProperties {
|
346
|
+
public:
|
347
|
+
struct Ephemeris {
|
348
|
+
uint_t svid;
|
349
|
+
int_t freq_ch;
|
350
|
+
uint_t t_k; ///< time referenced to the beginning of the frame from current day [s]
|
351
|
+
uint_t t_b; ///< base time of ephemeris parameters in UTC(SU) + 3hr [s]
|
352
|
+
uint_t M; ///< satellite type; 0 - GLONASS, 1 - GLONASS-M
|
353
|
+
float_t gamma_n; ///< gamma_n(t_c = t_b) = d/dt (t_c - t_n) [dimensionless]
|
354
|
+
float_t tau_n; ///< tau_n(t_c = t_b) = t_c (GLONASS time) - t_n(n-th satellite time) [s]
|
355
|
+
float_t xn, yn, zn; // [m]
|
356
|
+
float_t xn_dot, yn_dot, zn_dot; // [m/s]
|
357
|
+
float_t xn_ddot, yn_ddot, zn_ddot; // [m/s^2]
|
358
|
+
uint_t B_n; // health flag
|
359
|
+
uint_t p; // satellite operation mode
|
360
|
+
uint_t N_T; // current date counting up from leap year Jan. 1st [days]
|
361
|
+
float_t F_T; // user range accuracy at t_b [m]
|
362
|
+
uint_t n; // time difference between L1 and L2
|
363
|
+
float_t delta_tau_n;
|
364
|
+
uint_t E_n; // [days]
|
365
|
+
uint_t P1; // [s] time interval of two adjacent t_b; 0 - 0, 1 - 30, 2 - 45, 3 - 60 mins
|
366
|
+
bool P2;
|
367
|
+
//bool P3; // flag for alnamac; 1 - five, 0 - four
|
368
|
+
bool P4; // flag for ephemeris; 1 - uploaded by the control segment
|
369
|
+
bool l_n; // health flag; 0 - healthy, 1 - malfunction
|
370
|
+
|
371
|
+
float_t L1_frequency() const {
|
372
|
+
return GLONASS_SpaceNode::L1_frequency(freq_ch);
|
373
|
+
}
|
374
|
+
|
375
|
+
float_t L2_frequency() const {
|
376
|
+
return GLONASS_SpaceNode::L2_frequency(freq_ch);
|
377
|
+
}
|
378
|
+
|
379
|
+
struct constellation_t { // TODO make it to be a subclass of System_XYZ<float_t, PZ90>
|
380
|
+
float_t position[3];
|
381
|
+
float_t velocity[3];
|
382
|
+
float_t pos_abs2() const {
|
383
|
+
float_t res(0);
|
384
|
+
for(int i(0); i < 3; ++i){
|
385
|
+
res += (position[i] * position[i]);
|
386
|
+
}
|
387
|
+
return res;
|
388
|
+
}
|
389
|
+
constellation_t operator+(const constellation_t &another) const { // for RK4
|
390
|
+
constellation_t res(*this);
|
391
|
+
for(int i(0); i < 3; ++i){
|
392
|
+
res.position[i] += another.position[i];
|
393
|
+
res.velocity[i] += another.velocity[i];
|
394
|
+
}
|
395
|
+
return res;
|
396
|
+
}
|
397
|
+
constellation_t operator*(const float_t &sf) const { // for RK4
|
398
|
+
constellation_t res(*this);
|
399
|
+
for(int i(0); i < 3; ++i){
|
400
|
+
res.position[i] *= sf;
|
401
|
+
res.velocity[i] *= sf;
|
402
|
+
}
|
403
|
+
return res;
|
404
|
+
}
|
405
|
+
constellation_t operator/(const float_t &sf) const { // for RK4
|
406
|
+
return operator*(((float_t)1)/sf);
|
407
|
+
}
|
408
|
+
// TODO constant definitions should be moved to PZ-90(.02, .11)
|
409
|
+
static const float_t omega_E;
|
410
|
+
constellation_t abs_corrdinate(const float_t &sidereal_time_in_rad){
|
411
|
+
// @see Appendix.A PZ-90 to O-X0Y0Z0
|
412
|
+
float_t crad(std::cos(sidereal_time_in_rad)), srad(std::sin(sidereal_time_in_rad));
|
413
|
+
float_t
|
414
|
+
x0(position[0] * crad - position[1] * srad),
|
415
|
+
y0(position[0] * srad + position[1] * crad);
|
416
|
+
constellation_t res = {
|
417
|
+
{x0, y0, position[2]},
|
418
|
+
{
|
419
|
+
velocity[0] * crad - velocity[1] * srad - omega_E * y0,
|
420
|
+
velocity[0] * srad + velocity[1] * crad + omega_E * x0,
|
421
|
+
velocity[2]
|
422
|
+
}
|
423
|
+
};
|
424
|
+
return res;
|
425
|
+
}
|
426
|
+
constellation_t rel_corrdinate(const float_t &sidereal_time_in_rad){
|
427
|
+
// @see Appendix.A O-X0Y0Z0 to PZ-90
|
428
|
+
float_t crad(std::cos(sidereal_time_in_rad)), srad(std::sin(sidereal_time_in_rad));
|
429
|
+
float_t
|
430
|
+
x( position[0] * crad + position[1] * srad),
|
431
|
+
y(-position[0] * srad + position[1] * crad);
|
432
|
+
constellation_t res = {
|
433
|
+
{x, y, position[2]},
|
434
|
+
{
|
435
|
+
velocity[0] * crad + velocity[1] * srad + omega_E * y,
|
436
|
+
-velocity[0] * srad + velocity[1] * crad - omega_E * x,
|
437
|
+
velocity[2]
|
438
|
+
}
|
439
|
+
};
|
440
|
+
return res;
|
441
|
+
}
|
442
|
+
operator typename GPS_SpaceNode<float_t>::SatelliteProperties
|
443
|
+
::constellation_t() const {
|
444
|
+
#if 1
|
445
|
+
// @see https://gssc.esa.int/navipedia/index.php/Reference_Frames_in_GNSS
|
446
|
+
// PZ90.11 -> WGS84 (starting from 3:00 pm on December 31, 2013)
|
447
|
+
typename GPS_SpaceNode<float_t>::SatelliteProperties::constellation_t res = {
|
448
|
+
typename GPS_SpaceNode<float_t>::xyz_t(
|
449
|
+
position[0] + 0.003, position[1] + 0.001, position[2] + 0.001),
|
450
|
+
typename GPS_SpaceNode<float_t>::xyz_t(
|
451
|
+
velocity[0], velocity[1], velocity[2]),
|
452
|
+
};
|
453
|
+
return res;
|
454
|
+
#else
|
455
|
+
// @see https://www.gsi.go.jp/common/000070971.pdf
|
456
|
+
// @see (originally) Federal Air Navigation Authority (FANA), Aeronautical Information Circular
|
457
|
+
// of the Russian Federation, 12 February 2009, Russia.
|
458
|
+
// PZ90.02 -> WGS84
|
459
|
+
typename GPS_SpaceNode<float_t>::SatelliteProperties::constellation_t res = {
|
460
|
+
typename GPS_SpaceNode<float_t>::xyz_t(
|
461
|
+
position[0] - 0.36, position[1] + 0.08, position[2] + 0.18),
|
462
|
+
typename GPS_SpaceNode<float_t>::xyz_t(
|
463
|
+
velocity[0], velocity[1], velocity[2]),
|
464
|
+
};
|
465
|
+
return res;
|
466
|
+
#endif
|
467
|
+
}
|
468
|
+
};
|
469
|
+
|
470
|
+
struct eccentric_anomaly_t {
|
471
|
+
float_t E_k;
|
472
|
+
float_t snu_k, cnu_k;
|
473
|
+
eccentric_anomaly_t(const float_t &g_k, const float_t &e_k){
|
474
|
+
static const float_t delta_limit(1E-12);
|
475
|
+
for(int loop(0); loop < 10; loop++){
|
476
|
+
float_t E_k2(g_k + e_k * std::sin(E_k));
|
477
|
+
if(std::abs(E_k2 - E_k) < delta_limit){break;}
|
478
|
+
E_k = E_k2;
|
479
|
+
}
|
480
|
+
float_t sE_k(std::sin(E_k)), cE_k(std::cos(E_k)), denom(-e_k * cE_k + 1);
|
481
|
+
snu_k = std::sqrt(-std::pow(e_k, 2) + 1) * sE_k / denom;
|
482
|
+
cnu_k = (cE_k - e_k) / denom;
|
483
|
+
}
|
484
|
+
};
|
485
|
+
|
486
|
+
struct lunar_solar_perturbations_t {
|
487
|
+
struct arg_t {
|
488
|
+
float_t xi, eta, zeta, r;
|
489
|
+
} m, s;
|
490
|
+
struct constatnt_t {
|
491
|
+
float_t
|
492
|
+
a_m, a_s, // [m]
|
493
|
+
e_m, e_s,
|
494
|
+
i_m;
|
495
|
+
};
|
496
|
+
struct var_t {
|
497
|
+
float_t
|
498
|
+
epsilon,
|
499
|
+
g_m, Omega_m, Gamma_m,
|
500
|
+
g_s, omega_s;
|
501
|
+
};
|
502
|
+
|
503
|
+
static lunar_solar_perturbations_t setup(
|
504
|
+
const constatnt_t &C, const var_t &var){
|
505
|
+
|
506
|
+
lunar_solar_perturbations_t res;
|
507
|
+
|
508
|
+
static const float_t
|
509
|
+
sf_ci_m(-std::cos(C.i_m) + 1), si_m(std::sin(C.i_m)),
|
510
|
+
cepsilon(std::cos(var.epsilon)), sepsilon(std::sin(var.epsilon));
|
511
|
+
|
512
|
+
{ // ICD ver.4 and 5.1 Eq.(3) lunar (m) corresponding to Appendix.J.1 in CDMA ICD
|
513
|
+
// (ICD4) float_t Omega_m(Omega_om + Omega_1m * T);
|
514
|
+
float_t sOmega_m(std::sin(var.Omega_m)), cOmega_m(std::cos(var.Omega_m));
|
515
|
+
|
516
|
+
float_t xi_ast(-std::pow(cOmega_m, 2) * sf_ci_m + 1);
|
517
|
+
float_t eta_ast(sOmega_m * si_m);
|
518
|
+
float_t zeta_ast(cOmega_m * si_m);
|
519
|
+
|
520
|
+
float_t xi_11(sOmega_m * cOmega_m * sf_ci_m);
|
521
|
+
float_t xi_12(-std::pow(sOmega_m, 2) * sf_ci_m + 1);
|
522
|
+
|
523
|
+
float_t eta_11(xi_ast * cepsilon - zeta_ast * sepsilon);
|
524
|
+
float_t eta_12(xi_11 * cepsilon + eta_ast * sepsilon);
|
525
|
+
|
526
|
+
float_t zeta_11(xi_ast * sepsilon + zeta_ast * cepsilon);
|
527
|
+
float_t zeta_12(zeta_11 * sepsilon - eta_ast * cepsilon);
|
528
|
+
|
529
|
+
// (ICD4) float_t Gamma_m(Gamma_o + Gamma_1 * T);
|
530
|
+
float_t sGamma(std::sin(var.Gamma_m)), cGamma(std::cos(var.Gamma_m));
|
531
|
+
|
532
|
+
// (ICD4) eccentric_anomaly_t E_m(g_om + g_1m * T, C.e_m);
|
533
|
+
eccentric_anomaly_t E_m(var.g_m, C.e_m);
|
534
|
+
|
535
|
+
float_t
|
536
|
+
srad(E_m.snu_k * cGamma + E_m.cnu_k * sGamma),
|
537
|
+
crad(E_m.cnu_k * cGamma - E_m.snu_k * sGamma);
|
538
|
+
res.m.xi = srad * xi_11 + crad * xi_12;
|
539
|
+
res.m.eta = srad * eta_11 + crad * eta_12;
|
540
|
+
res.m.zeta = srad * zeta_11 + crad * zeta_12;
|
541
|
+
res.m.r = C.a_m * (-C.e_m * std::cos(E_m.E_k) + 1);
|
542
|
+
}
|
543
|
+
|
544
|
+
{ // ICD ver.4 and 5.1 Eq.(3) solar (s) corresponding to Appendix.S in CDMA ICD
|
545
|
+
// (ICD4) float_t omega_s(dms2rad(281, 13, (15.00 + (6189.03 * T))));
|
546
|
+
float_t co(std::cos(var.omega_s)), so(std::sin(var.omega_s));
|
547
|
+
|
548
|
+
// (ICD4) eccentric_anomaly_t E_s(g_os + g_1s * T, C.e_s);
|
549
|
+
eccentric_anomaly_t E_s(var.g_s, C.e_s);
|
550
|
+
|
551
|
+
float_t
|
552
|
+
srad(E_s.snu_k * co + E_s.cnu_k * so), // = sin(nu_s + omega_s)
|
553
|
+
crad(E_s.cnu_k * co - E_s.snu_k * so); // = cos(nu_s + omega_s)
|
554
|
+
|
555
|
+
res.s.xi = crad;
|
556
|
+
res.s.eta = srad * cepsilon;
|
557
|
+
res.s.zeta = srad * sepsilon;
|
558
|
+
res.s.r = C.a_s * (-C.e_s * std::cos(E_s.E_k) + 1);
|
559
|
+
}
|
560
|
+
return res;
|
561
|
+
}
|
562
|
+
/**
|
563
|
+
* @param days Sum of days from the epoch at 00 hours Moscow Time (MT) on 1st January 1975
|
564
|
+
* to the epoch at 00 hours MT of current date within which the instant t_e is.
|
565
|
+
* @param t_e reference time of ephemeris parameters (in Julian centuries of 36525 ephemeris days);
|
566
|
+
*/
|
567
|
+
static lunar_solar_perturbations_t base1975(
|
568
|
+
const float_t &days, const float_t &t_e) {
|
569
|
+
#define dms2rad(deg, min, sec) \
|
570
|
+
(M_PI / 180 * ((float_t)sec / 3600 + (float_t)min / 60 + deg))
|
571
|
+
static const constatnt_t C = {
|
572
|
+
3.84385243E8, // a_m // [m]
|
573
|
+
1.49598E11, // a_s // [m]
|
574
|
+
0.054900489, // e_m
|
575
|
+
0.016719, // e_s
|
576
|
+
dms2rad(5, 8, 43.4), // i_m // 0.08980398 rad
|
577
|
+
};
|
578
|
+
static const float_t
|
579
|
+
epsilon(dms2rad(23, 26, 33)),
|
580
|
+
g_om(dms2rad(-63, 53, 43.41)),
|
581
|
+
g_1m(dms2rad(477198, 50, 56.79)),
|
582
|
+
Omega_om(dms2rad(259, 10, 59.79)),
|
583
|
+
Omega_1m(dms2rad(-1934, 8, 31.23)),
|
584
|
+
Gamma_o(dms2rad(-334, 19, 46.40)),
|
585
|
+
Gamma_1(dms2rad(4069, 2, 2.52)),
|
586
|
+
g_os(dms2rad(358, 28, 33.04)),
|
587
|
+
g_1s(dms2rad(0, 0, 129596579.10));
|
588
|
+
|
589
|
+
float_t T((27392.375 + days + t_e / 86400) / 36525);
|
590
|
+
// 27392.375 equals to interval days between 1900/1/1 0:0:0(GMT) to 1975/1/1 0:0:0(MT)
|
591
|
+
var_t var = {
|
592
|
+
epsilon, // epsilon
|
593
|
+
g_om + g_1m * T, // g_m
|
594
|
+
Omega_om + Omega_1m * T, // Omega_m
|
595
|
+
Gamma_o + Gamma_1 * T, // Gamma_m
|
596
|
+
g_os + g_1s * T, // g_s
|
597
|
+
dms2rad(281, 13, (15.00 + (6189.03 * T))), // omega_s
|
598
|
+
};
|
599
|
+
return setup(C, var);
|
600
|
+
#undef dms2rad
|
601
|
+
}
|
602
|
+
static lunar_solar_perturbations_t base2000(
|
603
|
+
const float_t &jd0, const float_t &t_b) {
|
604
|
+
static const constatnt_t C = {
|
605
|
+
3.84385243E8, // a_m // [m]
|
606
|
+
1.49598E11, // a_s // [m]
|
607
|
+
0.054900489, // e_m
|
608
|
+
0.016719, // e_s
|
609
|
+
0.0898041080, // i_m
|
610
|
+
};
|
611
|
+
|
612
|
+
float_t T((jd0 + ((t_b - 10800) / 86400) - 2451545.0) / 36525);
|
613
|
+
// 2451545.0 equals to Julian date for 2000/1/1 0:0:0(UTC)
|
614
|
+
float_t T2(std::pow(T, 2));
|
615
|
+
|
616
|
+
var_t var = {
|
617
|
+
0.4090926006 - 0.0002270711 * T, // epsilon
|
618
|
+
2.3555557435 + 8328.6914257190 * T + 0.0001545547 * T2, // g_m (q_m)
|
619
|
+
2.1824391966 - 33.7570459536 * T + 0.0000362262 * T2, // Omega_m
|
620
|
+
1.4547885346 + 71.0176852437 * T - 0.0001801481 * T2, // Gamma_m
|
621
|
+
6.2400601269 + 628.3019551714 * T - 0.0000026820 * T2, // g_s (q_s)
|
622
|
+
-7.6281824375 + 0.0300101976 * T + 0.0000079741 * T2, // omega_s
|
623
|
+
};
|
624
|
+
return setup(C, var);
|
625
|
+
}
|
626
|
+
|
627
|
+
};
|
628
|
+
|
629
|
+
struct differential_t {
|
630
|
+
float_t J_x_a, J_y_a, J_z_a;
|
631
|
+
differential_t() : J_x_a(0), J_y_a(0), J_z_a(0) {}
|
632
|
+
differential_t(const float_t &J_x, const float_t &J_y, const float_t &J_z)
|
633
|
+
: J_x_a(J_x), J_y_a(J_y), J_z_a(J_z) {}
|
634
|
+
/**
|
635
|
+
* @param sidereal_time_in_rad TODO calculate from t_b and UTC
|
636
|
+
*/
|
637
|
+
differential_t(const Ephemeris &eph, const float_t &sidereal_time_in_rad){
|
638
|
+
float_t crad(std::cos(sidereal_time_in_rad)), srad(std::sin(sidereal_time_in_rad));
|
639
|
+
J_x_a = eph.xn_ddot * crad - eph.yn_ddot * srad;
|
640
|
+
J_y_a = eph.xn_ddot * srad + eph.yn_ddot * crad;
|
641
|
+
J_z_a = eph.zn_ddot;
|
642
|
+
}
|
643
|
+
constellation_t operator()(const float_t &t, const constellation_t &x) const {
|
644
|
+
// @see APPENDIX 3 EXAMPLES OF ALGORITHMS FOR CALCULATION OF COORDINATES AND VELOCITY
|
645
|
+
float_t r2(x.pos_abs2()), r(std::sqrt(r2));
|
646
|
+
static const float_t
|
647
|
+
a_e(6378136), mu(398600.44E9), C_20(1082625.75E-9); // values are obtained from ver.5.1; TODO move to PZ-90
|
648
|
+
float_t mu_bar(mu / r2),
|
649
|
+
x_a_bar(x.position[0] / r), y_a_bar(x.position[1] / r), z_a_bar(x.position[2] / r),
|
650
|
+
sf_z_a_bar(z_a_bar * z_a_bar * -5 + 1),
|
651
|
+
rho2(a_e * a_e / r2);
|
652
|
+
constellation_t res = {
|
653
|
+
{x.velocity[0], x.velocity[1], x.velocity[2]},
|
654
|
+
{ // @see ver.5.1 Eq.(1)
|
655
|
+
((-C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * x_a_bar + J_x_a,
|
656
|
+
((-C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * y_a_bar + J_y_a,
|
657
|
+
((-C_20 * rho2 * (sf_z_a_bar + 2) * 3 / 2) - 1) * mu_bar * z_a_bar + J_z_a,
|
658
|
+
}
|
659
|
+
};
|
660
|
+
#if 0
|
661
|
+
// If integration is performed in PZ-90, then these additional term is required,
|
662
|
+
// This is derived form simplified algorithm in CDMA ICD, (neither ver 5.1 nor ver.4 ICDs use it)
|
663
|
+
static const float_t omega_E(7.2921151467E-5), omega_E2(omega_E * omega_E);
|
664
|
+
res.velocity[0] += omega_E2 * x.position[0] + omega_E * 2 * x.velocity[1];
|
665
|
+
res.velocity[1] += omega_E2 * x.position[1] - omega_E * 2 * x.velocity[0];
|
666
|
+
#endif
|
667
|
+
return res;
|
668
|
+
}
|
669
|
+
};
|
670
|
+
|
671
|
+
struct differential2_t {
|
672
|
+
lunar_solar_perturbations_t J_src;
|
673
|
+
static void equation2(
|
674
|
+
const float_t &mu, const typename lunar_solar_perturbations_t::arg_t &arg,
|
675
|
+
const float_t (&position)[3],
|
676
|
+
float_t (&J)[3]){
|
677
|
+
// Eq.(2)
|
678
|
+
float_t
|
679
|
+
mu_bar(mu / std::pow(arg.r, 2)),
|
680
|
+
x_a_bar(position[0] / arg.r),
|
681
|
+
y_a_bar(position[1] / arg.r),
|
682
|
+
z_a_bar(position[2] / arg.r),
|
683
|
+
delta_x(arg.xi - x_a_bar),
|
684
|
+
delta_y(arg.eta - y_a_bar),
|
685
|
+
delta_z(arg.zeta - z_a_bar),
|
686
|
+
Delta3(std::pow(
|
687
|
+
std::pow(delta_x, 2) + std::pow(delta_y, 2) + std::pow(delta_z, 2), 1.5));
|
688
|
+
J[0] = mu_bar * (delta_x / Delta3 - arg.xi);
|
689
|
+
J[1] = mu_bar * (delta_y / Delta3 - arg.eta);
|
690
|
+
J[2] = mu_bar * (delta_z / Delta3 - arg.zeta);
|
691
|
+
}
|
692
|
+
void calculate_Jm(const float_t (&position)[3], float_t (&J)[3]) const {
|
693
|
+
equation2(4902.835E9, J_src.m, position, J); // for lunar (m); mu in [m/s]
|
694
|
+
}
|
695
|
+
void calculate_Js(const float_t (&position)[3], float_t (&J)[3]) const {
|
696
|
+
equation2(0.1325263E21, J_src.s, position, J); // for solar (s); mu in [m/s]
|
697
|
+
}
|
698
|
+
constellation_t operator()(const float_t &t, const constellation_t &x) const {
|
699
|
+
// @see APPENDIX 3 EXAMPLES OF ALGORITHMS FOR CALCULATION OF COORDINATES AND VELOCITY
|
700
|
+
float_t J_am[3], J_as[3];
|
701
|
+
calculate_Jm(x.position, J_am);
|
702
|
+
calculate_Js(x.position, J_as);
|
703
|
+
return differential_t(J_am[0] + J_as[0], J_am[1] + J_as[1], J_am[2] + J_as[2])(t, x);
|
704
|
+
}
|
705
|
+
};
|
706
|
+
|
707
|
+
u8_t F_T_index() const;
|
708
|
+
|
709
|
+
u8_t P1_index() const {
|
710
|
+
if(P1 > 45 * 60){
|
711
|
+
return 3;
|
712
|
+
}else if(P1 > 30 * 60){
|
713
|
+
return 2;
|
714
|
+
}else if(P1 > 0){
|
715
|
+
return 1;
|
716
|
+
}else{
|
717
|
+
return 0;
|
718
|
+
}
|
719
|
+
}
|
720
|
+
|
721
|
+
struct raw_t {
|
722
|
+
u8_t svid;
|
723
|
+
|
724
|
+
// String1
|
725
|
+
u8_t P1;
|
726
|
+
u16_t t_k;
|
727
|
+
s32_t xn_dot; ///< SF: 2^-20 [km/s]
|
728
|
+
s8_t xn_ddot; ///< SF: 2^-30 [km/s^2]
|
729
|
+
s32_t xn; ///< SF: 2^-11 [km]
|
730
|
+
|
731
|
+
// String2
|
732
|
+
u8_t B_n;
|
733
|
+
u8_t P2;
|
734
|
+
u8_t t_b; ///< SF: 15
|
735
|
+
s32_t yn_dot;
|
736
|
+
s8_t yn_ddot;
|
737
|
+
s32_t yn;
|
738
|
+
|
739
|
+
// String3
|
740
|
+
u8_t P3;
|
741
|
+
s16_t gamma_n; ///< SF: 2^-40
|
742
|
+
u8_t p;
|
743
|
+
u8_t l_n;
|
744
|
+
s32_t zn_dot;
|
745
|
+
s8_t zn_ddot;
|
746
|
+
s32_t zn;
|
747
|
+
|
748
|
+
// String4
|
749
|
+
s32_t tau_n; ///< SF: 2^-30
|
750
|
+
u8_t delta_tau_n; ///< SF: 2^-30
|
751
|
+
u8_t E_n;
|
752
|
+
u8_t P4;
|
753
|
+
u8_t F_T;
|
754
|
+
u16_t N_T; ///< [days]
|
755
|
+
u8_t n;
|
756
|
+
u8_t M;
|
757
|
+
|
758
|
+
#define fetch_item(num, name) name = BroadcastedMessage< \
|
759
|
+
InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> \
|
760
|
+
:: String ## num :: name (src)
|
761
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
|
762
|
+
void update_string1(const InputT *src){
|
763
|
+
fetch_item(1, P1);
|
764
|
+
fetch_item(1, t_k);
|
765
|
+
fetch_item(1, xn_dot);
|
766
|
+
fetch_item(1, xn_ddot);
|
767
|
+
fetch_item(1, xn);
|
768
|
+
}
|
769
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
|
770
|
+
void update_string2(const InputT *src){
|
771
|
+
// String2
|
772
|
+
fetch_item(2, B_n);
|
773
|
+
fetch_item(2, P2);
|
774
|
+
fetch_item(2, t_b);
|
775
|
+
fetch_item(2, yn_dot);
|
776
|
+
fetch_item(2, yn_ddot);
|
777
|
+
fetch_item(2, yn);
|
778
|
+
}
|
779
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
|
780
|
+
void update_string3(const InputT *src){
|
781
|
+
fetch_item(3, P3);
|
782
|
+
fetch_item(3, gamma_n);
|
783
|
+
fetch_item(3, p);
|
784
|
+
fetch_item(3, l_n);
|
785
|
+
fetch_item(3, zn_dot);
|
786
|
+
fetch_item(3, zn_ddot);
|
787
|
+
fetch_item(3, zn);
|
788
|
+
}
|
789
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
|
790
|
+
void update_string4(const InputT *src){
|
791
|
+
fetch_item(4, tau_n);
|
792
|
+
fetch_item(4, delta_tau_n);
|
793
|
+
fetch_item(4, E_n);
|
794
|
+
fetch_item(4, P4);
|
795
|
+
fetch_item(4, F_T);
|
796
|
+
fetch_item(4, N_T);
|
797
|
+
fetch_item(4, n);
|
798
|
+
fetch_item(4, M);
|
799
|
+
}
|
800
|
+
#undef fetch_item
|
801
|
+
|
802
|
+
enum {
|
803
|
+
SF_xn, SF_yn = SF_xn, SF_zn = SF_xn,
|
804
|
+
SF_xn_dot, SF_yn_dot = SF_xn_dot, SF_zn_dot = SF_xn_dot,
|
805
|
+
SF_xn_ddot, SF_yn_ddot = SF_xn_ddot, SF_zn_ddot = SF_xn_ddot,
|
806
|
+
SF_t_b,
|
807
|
+
SF_gamma_n,
|
808
|
+
SF_tau_n,
|
809
|
+
SF_delta_tau_n,
|
810
|
+
SF_NUM,
|
811
|
+
};
|
812
|
+
static const float_t sf[SF_NUM];
|
813
|
+
|
814
|
+
static const float_t F_T_table[15]; ///< @see Table 4.4
|
815
|
+
|
816
|
+
static const float_t F_T_value(const u8_t &F_T_){
|
817
|
+
if(F_T_ >= (sizeof(F_T_table) / sizeof(F_T_table[0]))){
|
818
|
+
return -1; // not used
|
819
|
+
}else{
|
820
|
+
return F_T_table[F_T_];
|
821
|
+
}
|
822
|
+
}
|
823
|
+
|
824
|
+
static const uint_t P1_value(const u8_t &P1_){
|
825
|
+
switch(P1_){
|
826
|
+
case 1: return 30 * 60;
|
827
|
+
case 2: return 45 * 60;
|
828
|
+
case 3: return 60 * 60;
|
829
|
+
case 0: default: return 0;
|
830
|
+
}
|
831
|
+
}
|
832
|
+
|
833
|
+
operator Ephemeris() const {
|
834
|
+
Ephemeris res;
|
835
|
+
#define CONVERT(TARGET) \
|
836
|
+
{res.TARGET = sf[SF_ ## TARGET] * TARGET;}
|
837
|
+
res.svid = svid;
|
838
|
+
res.t_k = (((t_k >> 7) & 0x1F) * 3600) // hour
|
839
|
+
+ (((t_k >> 1) & 0x3F) * 60) // min
|
840
|
+
+ ((t_k & 0x1) ? 30 : 0); // sec
|
841
|
+
CONVERT(t_b);
|
842
|
+
res.M = M;
|
843
|
+
CONVERT(gamma_n);
|
844
|
+
CONVERT(tau_n);
|
845
|
+
CONVERT(xn); CONVERT(xn_dot); CONVERT(xn_ddot);
|
846
|
+
CONVERT(yn); CONVERT(yn_dot); CONVERT(yn_ddot);
|
847
|
+
CONVERT(zn); CONVERT(zn_dot); CONVERT(zn_ddot);
|
848
|
+
res.B_n = B_n;
|
849
|
+
res.p = p;
|
850
|
+
res.N_T = N_T;
|
851
|
+
|
852
|
+
res.F_T = F_T_value(F_T);
|
853
|
+
res.n = n;
|
854
|
+
CONVERT(delta_tau_n);
|
855
|
+
res.E_n = E_n;
|
856
|
+
res.P1 = P1_value(P1);
|
857
|
+
res.P2 = (P2 > 0);
|
858
|
+
//res.P3 = (P3 > 0);
|
859
|
+
res.P4 = (P4 > 0);
|
860
|
+
res.l_n = (l_n > 0);
|
861
|
+
#undef CONVERT
|
862
|
+
return res;
|
863
|
+
}
|
864
|
+
raw_t &operator=(const Ephemeris &eph){
|
865
|
+
// TODO: m?
|
866
|
+
#define CONVERT(TARGET) \
|
867
|
+
{TARGET = (s32_t)((eph.TARGET + 0.5 * sf[SF_ ## TARGET]) / sf[SF_ ## TARGET]);}
|
868
|
+
svid = eph.svid;
|
869
|
+
{ // t_k
|
870
|
+
std::div_t minutes(div(eph.t_k, 60));
|
871
|
+
std::div_t hrs(div(minutes.quot, 60));
|
872
|
+
t_k = (((hrs.quot << 6) + minutes.quot) << 1) + (minutes.rem > 0 ? 1 : 0);
|
873
|
+
}
|
874
|
+
CONVERT(t_b);
|
875
|
+
M = eph.M;
|
876
|
+
CONVERT(gamma_n);
|
877
|
+
CONVERT(tau_n);
|
878
|
+
CONVERT(xn); CONVERT(xn_dot); CONVERT(xn_ddot);
|
879
|
+
CONVERT(yn); CONVERT(yn_dot); CONVERT(yn_ddot);
|
880
|
+
CONVERT(zn); CONVERT(zn_dot); CONVERT(zn_ddot);
|
881
|
+
B_n = eph.B_n;
|
882
|
+
p = eph.p;
|
883
|
+
N_T = eph.N_T;
|
884
|
+
F_T = eph.F_T_index();
|
885
|
+
n = eph.n;
|
886
|
+
CONVERT(delta_tau_n);
|
887
|
+
E_n = eph.E_n;
|
888
|
+
if(eph.P1 > 45 * 60){
|
889
|
+
P1 = 3;
|
890
|
+
}else if(eph.P1 > 30 * 60){
|
891
|
+
P1 = 2;
|
892
|
+
}else if(eph.P1 > 0){
|
893
|
+
P1 = 1;
|
894
|
+
}else{
|
895
|
+
P1 = 0;
|
896
|
+
}
|
897
|
+
P2 = (eph.P2 ? 1 : 0);
|
898
|
+
//P3 = (eph.P3 ? 1 : 0);
|
899
|
+
P4 = (eph.P4 ? 1 : 0);
|
900
|
+
l_n = (eph.l_n ? 1 : 0);
|
901
|
+
#undef CONVERT
|
902
|
+
return *this;
|
903
|
+
}
|
904
|
+
};
|
905
|
+
|
906
|
+
bool is_equivalent(const Ephemeris &eph) const {
|
907
|
+
do{
|
908
|
+
#define CHECK(TARGET) if(TARGET != eph.TARGET){break;}
|
909
|
+
//CHECK(t_k); // t_k is just a reference time
|
910
|
+
CHECK(t_b);
|
911
|
+
CHECK(M);
|
912
|
+
CHECK(B_n);
|
913
|
+
CHECK(p);
|
914
|
+
CHECK(N_T);
|
915
|
+
CHECK(F_T_index());
|
916
|
+
CHECK(n);
|
917
|
+
CHECK(E_n);
|
918
|
+
CHECK(P1);
|
919
|
+
CHECK(P2);
|
920
|
+
//CHECK(P3);
|
921
|
+
CHECK(P4);
|
922
|
+
CHECK(l_n);
|
923
|
+
#undef CHECK
|
924
|
+
#define CHECK(TARGET) \
|
925
|
+
if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
926
|
+
CHECK(gamma_n);
|
927
|
+
CHECK(tau_n);
|
928
|
+
CHECK(xn); CHECK(xn_dot); CHECK(xn_ddot);
|
929
|
+
CHECK(yn); CHECK(yn_dot); CHECK(yn_ddot);
|
930
|
+
CHECK(zn); CHECK(zn_dot); CHECK(zn_ddot);
|
931
|
+
CHECK(delta_tau_n);
|
932
|
+
#undef CHECK
|
933
|
+
return true;
|
934
|
+
}while(false);
|
935
|
+
return false;
|
936
|
+
}
|
937
|
+
};
|
938
|
+
|
939
|
+
/**
|
940
|
+
*
|
941
|
+
* Relationship of time systems
|
942
|
+
* 1) t_GL = t_UTC + 3hr (defined in ICD 3.3.3 GLONASS Time)
|
943
|
+
* 2) t_UTC + 3hr = t_rcv + tau_c + tau_n - gamma_n * (t_rcv - t_b) (defined in ICD 3.3.3 GLONASS Time, also in RINEX spec.)
|
944
|
+
* where tau_c: system wise, (-tau_c = GLUT in RINEX)
|
945
|
+
* ta_n, gamm_n: per satellite
|
946
|
+
* t_b along to UTC + 3hr (a.k.a, t_GL)
|
947
|
+
* 3) t_GPS - t_GL = delta_T + tau_GPS (defined in ICD 4.5 Non-immediate info.)
|
948
|
+
*/
|
949
|
+
struct Ephemeris_with_Time : public Ephemeris, TimeProperties {
|
950
|
+
typedef typename Ephemeris::constellation_t constellation_t;
|
951
|
+
constellation_t xa_t_b;
|
952
|
+
float_t sidereal_t_b_rad;
|
953
|
+
typename Ephemeris::differential_t eq_of_motion;
|
954
|
+
|
955
|
+
static const float_t time_step_max_per_one_integration;
|
956
|
+
|
957
|
+
void calculate_additional() {
|
958
|
+
sidereal_t_b_rad = TimeProperties::date_t::Greenwich_sidereal_time_deg(
|
959
|
+
TimeProperties::date.c_tm(),
|
960
|
+
(float_t)(this->t_b) / (60 * 60) - 3) / 180 * M_PI;
|
961
|
+
constellation_t x_t_b = {
|
962
|
+
{this->xn, this->yn, this->zn},
|
963
|
+
{this->xn_dot, this->yn_dot, this->zn_dot},
|
964
|
+
};
|
965
|
+
xa_t_b = x_t_b.abs_corrdinate(sidereal_t_b_rad); // PZ-90 => O-XYZ
|
966
|
+
eq_of_motion = typename Ephemeris::differential_t((*this), sidereal_t_b_rad);
|
967
|
+
}
|
968
|
+
Ephemeris_with_Time(const Ephemeris &eph, const TimeProperties &t_prop)
|
969
|
+
: Ephemeris(eph), TimeProperties(t_prop) {
|
970
|
+
calculate_additional();
|
971
|
+
}
|
972
|
+
Ephemeris_with_Time(const Ephemeris &eph, const std::tm &t_utc)
|
973
|
+
: Ephemeris(eph) {
|
974
|
+
this->tau_c = this->tau_GPS = 0;
|
975
|
+
std::tm t_mt(t_utc); // calculate Moscow time
|
976
|
+
t_mt.tm_hour += 3;
|
977
|
+
std::mktime(&t_mt); // renormalization
|
978
|
+
this->date = TimeProperties::date_t::from_c_tm(t_mt);
|
979
|
+
this->t_b = (t_mt.tm_hour * 60 + t_mt.tm_min) * 60 + t_mt.tm_sec;
|
980
|
+
calculate_additional();
|
981
|
+
}
|
982
|
+
std::tm c_tm_utc() const {
|
983
|
+
std::tm t(TimeProperties::date.c_tm()); // set date on Moscow time
|
984
|
+
(t.tm_sec = (int)(this->t_b)) -= 3 * 60 * 60; // add second on UTC
|
985
|
+
std::mktime(&t); // renormalization
|
986
|
+
return t;
|
987
|
+
}
|
988
|
+
struct raw_t : public Ephemeris::raw_t, TimeProperties::raw_t {
|
989
|
+
operator Ephemeris_with_Time() const {
|
990
|
+
return Ephemeris_with_Time((Ephemeris)(*this), (TimeProperties)(*this));
|
991
|
+
}
|
992
|
+
raw_t &operator=(const Ephemeris_with_Time &eph){
|
993
|
+
(typename Ephemeris::raw_t &)(*this) = eph;
|
994
|
+
(typename TimeProperties::raw_t &)(*this) = eph;
|
995
|
+
return *this;
|
996
|
+
}
|
997
|
+
};
|
998
|
+
bool is_equivalent(const Ephemeris_with_Time &eph) const {
|
999
|
+
return Ephemeris::is_equivalent(eph) && TimeProperties::is_equivalent(eph);
|
1000
|
+
}
|
1001
|
+
float_t calculate_clock_error(float_t delta_t, const float_t &pseudo_range = 0) const {
|
1002
|
+
delta_t -= pseudo_range / light_speed;
|
1003
|
+
return -TimeProperties::tau_c - Ephemeris::tau_n + Ephemeris::gamma_n * delta_t;
|
1004
|
+
}
|
1005
|
+
/**
|
1006
|
+
* @param t_arrival_onboard signal arrival time in onboard time scale,
|
1007
|
+
* which is along to glonass time scale (UTC + 3 hr) but is shifted in gradually changing length.
|
1008
|
+
*/
|
1009
|
+
float_t clock_error(
|
1010
|
+
const float_t &t_arrival_onboard, const float_t &pseudo_range = 0) const {
|
1011
|
+
return calculate_clock_error(t_arrival_onboard - Ephemeris::t_b, pseudo_range);
|
1012
|
+
}
|
1013
|
+
/**
|
1014
|
+
* Calculate absolute constellation(t) based on constellation(t_0).
|
1015
|
+
* t_0 is a time around t_b, and is used to calculate
|
1016
|
+
* @param t_step time interval from t_0 to t
|
1017
|
+
* @param times number of integration steps (assume times >=0)
|
1018
|
+
* @param xa_t_0 constellation(t_0)
|
1019
|
+
* @param t_0_from_t_b time interval from t_b to t_0 (= t_0 - t_b)
|
1020
|
+
*/
|
1021
|
+
constellation_t constellation_abs(
|
1022
|
+
const float_t &t_step,
|
1023
|
+
int times,
|
1024
|
+
const constellation_t &xa_t_0, const float_t &t_0_from_t_b) const {
|
1025
|
+
|
1026
|
+
constellation_t res(xa_t_0);
|
1027
|
+
float_t delta_t_itg(t_0_from_t_b); // accumulative time of integration
|
1028
|
+
for(; times > 0; --times, delta_t_itg += t_step){
|
1029
|
+
res = nextByRK4(eq_of_motion, delta_t_itg, res, t_step);
|
1030
|
+
}
|
1031
|
+
return res;
|
1032
|
+
}
|
1033
|
+
/**
|
1034
|
+
* Calculate absolute constellation(t) based on constellation(t_0).
|
1035
|
+
* t_0 is a time around t_b, and is used to calculate
|
1036
|
+
* an intermediate result specified with the 2nd argument.
|
1037
|
+
* This method is useful to calculate constellation effectively
|
1038
|
+
* by using a cache.
|
1039
|
+
* @param delta_t time interval from t_0 to t
|
1040
|
+
* @param xa_t_0 constellation(t_0)
|
1041
|
+
* @param t_0_from_t_b time interval from t_b to t_0 (= t_0 - t_b)
|
1042
|
+
*/
|
1043
|
+
constellation_t constellation_abs(
|
1044
|
+
const float_t &delta_t,
|
1045
|
+
const constellation_t &xa_t_0, const float_t &t_0_from_t_b) const {
|
1046
|
+
|
1047
|
+
float_t t_step_max(delta_t >= 0
|
1048
|
+
? time_step_max_per_one_integration
|
1049
|
+
: -time_step_max_per_one_integration);
|
1050
|
+
int n(std::floor(delta_t / t_step_max));
|
1051
|
+
float_t delta_t_steps(t_step_max * n), delta_t_remain(delta_t - delta_t_steps);
|
1052
|
+
|
1053
|
+
// To perform time integration from t_0 to (t_0 + delta_t),
|
1054
|
+
// n-times integration with t_step_max,
|
1055
|
+
// then, one-time integration with delta_t_remain are conducted.
|
1056
|
+
return constellation_abs(
|
1057
|
+
delta_t_remain, 1,
|
1058
|
+
constellation_abs(t_step_max, n, xa_t_0, t_0_from_t_b),
|
1059
|
+
t_0_from_t_b + delta_t_steps);
|
1060
|
+
}
|
1061
|
+
/**
|
1062
|
+
* Calculate constellation(t) based on constellation(t_b).
|
1063
|
+
* @param delta_t time interval from t_0 to t
|
1064
|
+
* @param pseudo_range measured pusedo_range to correct delta_t, default is 0.
|
1065
|
+
*/
|
1066
|
+
constellation_t calculate_constellation(
|
1067
|
+
const float_t &delta_t, const float_t &pseudo_range = 0) const {
|
1068
|
+
|
1069
|
+
float_t delta_t_to_transmit(delta_t - pseudo_range / light_speed);
|
1070
|
+
constellation_t res(
|
1071
|
+
constellation_abs(delta_t_to_transmit, xa_t_b, float_t(0)));
|
1072
|
+
|
1073
|
+
return res.rel_corrdinate(
|
1074
|
+
sidereal_t_b_rad + (constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02
|
1075
|
+
}
|
1076
|
+
/**
|
1077
|
+
* @param t_arrival_glonass signal arrival time in glonass time scale,
|
1078
|
+
* which is the corrected onboard time by removing clock error.
|
1079
|
+
*/
|
1080
|
+
constellation_t constellation(
|
1081
|
+
const float_t &t_arrival_glonass, const float_t &pseudo_range = 0) const {
|
1082
|
+
return calculate_constellation(
|
1083
|
+
t_arrival_glonass - Ephemeris::t_b, pseudo_range); // measure in UTC + 3hr scale
|
1084
|
+
}
|
1085
|
+
};
|
1086
|
+
|
1087
|
+
struct Ephemeris_with_GPS_Time : public Ephemeris_with_Time {
|
1088
|
+
GPS_Time<float_t> t_b_gps;
|
1089
|
+
Ephemeris_with_GPS_Time()
|
1090
|
+
: Ephemeris_with_Time(Ephemeris(), GPS_Time<float_t>(0, 0).c_tm()),
|
1091
|
+
t_b_gps(0, 0) {}
|
1092
|
+
/**
|
1093
|
+
*
|
1094
|
+
* @param deltaT integer part of difference of GPS and GLONASS time scales.
|
1095
|
+
* This is often identical to the leap seconds because GLONASS time base is UTC
|
1096
|
+
*/
|
1097
|
+
Ephemeris_with_GPS_Time(const Ephemeris_with_Time &eph, const int_t &deltaT = 0)
|
1098
|
+
: Ephemeris_with_Time(eph),
|
1099
|
+
t_b_gps(GPS_Time<float_t>(eph.c_tm_utc()) // in UTC scale
|
1100
|
+
+ Ephemeris_with_Time::tau_GPS // in (GPS - delta_T) scale (delta_T is integer), -tau_GPS = GLGP in RINEX
|
1101
|
+
+ deltaT) {
|
1102
|
+
}
|
1103
|
+
GPS_Time<float_t> base_time() const {
|
1104
|
+
return t_b_gps;
|
1105
|
+
}
|
1106
|
+
bool is_valid(const GPS_Time<float_t> &t) const {
|
1107
|
+
return std::abs(t_b_gps.interval(t)) <= 60 * 60; // 1 hour
|
1108
|
+
}
|
1109
|
+
using Ephemeris_with_Time::clock_error;
|
1110
|
+
float_t clock_error(
|
1111
|
+
const GPS_Time<float_t> &t_arrival_onboard, const float_t &pseudo_range = 0) const {
|
1112
|
+
return Ephemeris_with_Time::calculate_clock_error(t_arrival_onboard - t_b_gps, pseudo_range);
|
1113
|
+
}
|
1114
|
+
using Ephemeris_with_Time::constellation;
|
1115
|
+
typename Ephemeris_with_Time::constellation_t constellation(
|
1116
|
+
const GPS_Time<float_t> &t_arrival_gps, const float_t &pseudo_range = 0) const {
|
1117
|
+
return this->calculate_constellation(t_arrival_gps - t_b_gps, pseudo_range);
|
1118
|
+
}
|
1119
|
+
};
|
1120
|
+
};
|
1121
|
+
|
1122
|
+
struct Satellite : public SatelliteProperties {
|
1123
|
+
public:
|
1124
|
+
typedef typename SatelliteProperties::Ephemeris_with_GPS_Time eph_t;
|
1125
|
+
|
1126
|
+
struct eph_cached_t : public eph_t {
|
1127
|
+
mutable typename eph_t::constellation_t xa_t_0;
|
1128
|
+
mutable float_t t_0_from_t_b;
|
1129
|
+
eph_cached_t() : eph_t(), xa_t_0(eph_t::xa_t_b), t_0_from_t_b(0) {}
|
1130
|
+
eph_cached_t(const eph_t &eph) : eph_t(eph), xa_t_0(eph.xa_t_b), t_0_from_t_b(0) {}
|
1131
|
+
using eph_t::constellation;
|
1132
|
+
typename eph_t::constellation_t constellation(
|
1133
|
+
const GPS_Time<float_t> &t_arrival_gps, const float_t &pseudo_range = 0) const {
|
1134
|
+
float_t delta_t(t_arrival_gps - eph_t::t_b_gps);
|
1135
|
+
float_t delta_t_transmit_from_t_0(delta_t - pseudo_range / light_speed - t_0_from_t_b);
|
1136
|
+
|
1137
|
+
float_t t_step_max(delta_t_transmit_from_t_0 >= 0
|
1138
|
+
? eph_t::time_step_max_per_one_integration
|
1139
|
+
: -eph_t::time_step_max_per_one_integration);
|
1140
|
+
|
1141
|
+
int big_steps(std::floor(delta_t_transmit_from_t_0 / t_step_max));
|
1142
|
+
if(big_steps > 0){ // perform integration and update cache
|
1143
|
+
xa_t_0 = eph_t::constellation_abs(t_step_max, big_steps, xa_t_0, t_0_from_t_b);
|
1144
|
+
float_t delta_t_updated(t_step_max * big_steps);
|
1145
|
+
t_0_from_t_b += delta_t_updated;
|
1146
|
+
delta_t_transmit_from_t_0 -= delta_t_updated;
|
1147
|
+
}
|
1148
|
+
|
1149
|
+
return eph_t::constellation_abs(delta_t_transmit_from_t_0, 1, xa_t_0, t_0_from_t_b)
|
1150
|
+
.rel_corrdinate(
|
1151
|
+
eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02
|
1152
|
+
}
|
1153
|
+
};
|
1154
|
+
|
1155
|
+
typedef typename GPS_SpaceNode<float_t>::template PropertyHistory<eph_cached_t> eph_list_t;
|
1156
|
+
protected:
|
1157
|
+
eph_list_t eph_history;
|
1158
|
+
public:
|
1159
|
+
Satellite() : eph_history() {
|
1160
|
+
// TODO setup first ephemeris as invalid one
|
1161
|
+
// eph_t &eph_current(const_cast<eph_t &>(eph_history.current()));
|
1162
|
+
}
|
1163
|
+
|
1164
|
+
template <class Functor>
|
1165
|
+
void each_ephemeris(
|
1166
|
+
Functor &functor,
|
1167
|
+
const typename eph_list_t::each_mode_t &mode = eph_list_t::EACH_ALL) const {
|
1168
|
+
eph_history.each(functor, mode);
|
1169
|
+
}
|
1170
|
+
|
1171
|
+
void register_ephemeris(const eph_t &eph, const int &priority_delta = 1){
|
1172
|
+
eph_history.add(eph_cached_t(eph), priority_delta);
|
1173
|
+
}
|
1174
|
+
|
1175
|
+
void merge(const Satellite &another, const bool &keep_original = true){
|
1176
|
+
eph_history.merge(another.eph_history, keep_original);
|
1177
|
+
}
|
1178
|
+
|
1179
|
+
const eph_t &ephemeris() const {
|
1180
|
+
return eph_history.current();
|
1181
|
+
}
|
1182
|
+
|
1183
|
+
/**
|
1184
|
+
* Select appropriate ephemeris within registered ones.
|
1185
|
+
*
|
1186
|
+
* @param target_time time at measurement
|
1187
|
+
* @return if true, appropriate ephemeris is selected, otherwise, not selected.
|
1188
|
+
*/
|
1189
|
+
bool select_ephemeris(const GPS_Time<float_t> &target_time){
|
1190
|
+
return eph_history.select(target_time, &eph_t::is_valid);
|
1191
|
+
}
|
1192
|
+
|
1193
|
+
float_t clock_error(const GPS_Time<float_t> &t, const float_t &pseudo_range = 0) const{
|
1194
|
+
return ephemeris().clock_error(t, pseudo_range);
|
1195
|
+
}
|
1196
|
+
|
1197
|
+
typename GPS_SpaceNode<float_t>::SatelliteProperties::constellation_t constellation(
|
1198
|
+
const GPS_Time<float_t> &t, const float_t &pseudo_range = 0) const {
|
1199
|
+
return (typename GPS_SpaceNode<float_t>::SatelliteProperties::constellation_t)(
|
1200
|
+
eph_history.current().constellation(t, pseudo_range));
|
1201
|
+
}
|
1202
|
+
|
1203
|
+
typename GPS_SpaceNode<float_t>::xyz_t position(const GPS_Time<float_t> &t, const float_t &pseudo_range = 0) const {
|
1204
|
+
return constellation(t, pseudo_range).position;
|
1205
|
+
}
|
1206
|
+
|
1207
|
+
typename GPS_SpaceNode<float_t>::xyz_t velocity(const GPS_Time<float_t> &t, const float_t &pseudo_range = 0) const {
|
1208
|
+
return constellation(t, pseudo_range).velocity;
|
1209
|
+
}
|
1210
|
+
};
|
1211
|
+
public:
|
1212
|
+
typedef std::map<int, Satellite> satellites_t;
|
1213
|
+
protected:
|
1214
|
+
satellites_t _satellites;
|
1215
|
+
public:
|
1216
|
+
GLONASS_SpaceNode()
|
1217
|
+
: _satellites() {}
|
1218
|
+
~GLONASS_SpaceNode(){
|
1219
|
+
_satellites.clear();
|
1220
|
+
}
|
1221
|
+
const satellites_t &satellites() const {
|
1222
|
+
return _satellites;
|
1223
|
+
}
|
1224
|
+
Satellite &satellite(const int &prn) {
|
1225
|
+
return _satellites[prn];
|
1226
|
+
}
|
1227
|
+
bool has_satellite(const int &prn) const {
|
1228
|
+
return _satellites.find(prn) != _satellites.end();
|
1229
|
+
}
|
1230
|
+
void update_all_ephemeris(const GPS_Time<float_t> &target_time) {
|
1231
|
+
for(typename satellites_t::iterator it(_satellites.begin()), it_end(_satellites.end());
|
1232
|
+
it != it_end; ++it){
|
1233
|
+
it->second.select_ephemeris(target_time);
|
1234
|
+
}
|
1235
|
+
}
|
1236
|
+
typename Satellite::eph_t latest_ephemeris() const {
|
1237
|
+
struct {
|
1238
|
+
typename Satellite::eph_t res;
|
1239
|
+
void operator()(const typename Satellite::eph_t &eph){
|
1240
|
+
if(res.t_b_gps < eph.t_b_gps){res = eph;}
|
1241
|
+
}
|
1242
|
+
} functor;
|
1243
|
+
for(typename satellites_t::const_iterator
|
1244
|
+
it(_satellites.begin()), it_end(_satellites.end());
|
1245
|
+
it != it_end; ++it){
|
1246
|
+
it->second.each_ephemeris(functor, Satellite::eph_list_t::EACH_NO_REDUNDANT);
|
1247
|
+
}
|
1248
|
+
return functor.res;
|
1249
|
+
}
|
1250
|
+
};
|
1251
|
+
|
1252
|
+
template <class FloatT>
|
1253
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t GLONASS_SpaceNode<FloatT>::light_speed
|
1254
|
+
= 299792458; // [m/s]
|
1255
|
+
|
1256
|
+
template <class FloatT>
|
1257
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t GLONASS_SpaceNode<FloatT>::L1_frequency_base
|
1258
|
+
= 1602E6; // [Hz]
|
1259
|
+
|
1260
|
+
template <class FloatT>
|
1261
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t GLONASS_SpaceNode<FloatT>::L1_frequency_gap
|
1262
|
+
= 562.5E3; // [Hz]
|
1263
|
+
|
1264
|
+
template <class FloatT>
|
1265
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t GLONASS_SpaceNode<FloatT>::L2_frequency_base
|
1266
|
+
= 1246E6; // [Hz]
|
1267
|
+
|
1268
|
+
template <class FloatT>
|
1269
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t GLONASS_SpaceNode<FloatT>::L2_frequency_gap
|
1270
|
+
= 437.5E3; // [Hz]
|
1271
|
+
|
1272
|
+
template <class FloatT>
|
1273
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::constellation_t::omega_E
|
1274
|
+
= 0.7292115E-4; // [s^-1]
|
1275
|
+
|
1276
|
+
template <class FloatT>
|
1277
|
+
const typename GLONASS_SpaceNode<FloatT>::int_t GLONASS_SpaceNode<FloatT>::TimeProperties::date_t::days_m[] = {
|
1278
|
+
31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31
|
1279
|
+
};
|
1280
|
+
|
1281
|
+
#define POWER_2(n) \
|
1282
|
+
(((n) >= 0) \
|
1283
|
+
? (float_t)(1 << (n >= 0 ? n : 0)) \
|
1284
|
+
: (((float_t)1) / (1 << (-(n) >= 30 ? 30 : -(n > 0 ? 0 : n))) \
|
1285
|
+
/ (1 << (-(n) >= 30 ? (-(n) - 30) : 0))))
|
1286
|
+
|
1287
|
+
template <class FloatT>
|
1288
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t GLONASS_SpaceNode<FloatT>::TimeProperties::raw_t::sf[] = {
|
1289
|
+
POWER_2(-31), // tau_c [s]
|
1290
|
+
POWER_2(-30) /* * 60 * 60 * 24*/, // tau_GPS [s], unit is described as [day] in ICD, however, it may be wrong.
|
1291
|
+
};
|
1292
|
+
|
1293
|
+
template <class FloatT>
|
1294
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t::sf[] = {
|
1295
|
+
POWER_2(-11) * 1E3, // x
|
1296
|
+
POWER_2(-20) * 1E3, // x_dot
|
1297
|
+
POWER_2(-30) * 1E3, // x_ddot
|
1298
|
+
15 * 60, // t_b
|
1299
|
+
POWER_2(-40), // gamma_n
|
1300
|
+
POWER_2(-30), // tau_n
|
1301
|
+
POWER_2(-30), // delta_tau_n
|
1302
|
+
};
|
1303
|
+
|
1304
|
+
#undef POWER_2
|
1305
|
+
|
1306
|
+
template <class FloatT>
|
1307
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t::F_T_table[] = {
|
1308
|
+
1, 2, 2.5, 4, 5, 7, 10, 12, 14, 16, 32, 64, 128, 256, 512,
|
1309
|
+
};
|
1310
|
+
|
1311
|
+
template <class FloatT>
|
1312
|
+
typename GLONASS_SpaceNode<FloatT>::u8_t GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::F_T_index() const {
|
1313
|
+
if(F_T <= 0){ // invalid value
|
1314
|
+
return sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]);
|
1315
|
+
}
|
1316
|
+
u8_t res(0);
|
1317
|
+
while(res < (sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]))){
|
1318
|
+
if(F_T <= raw_t::F_T_table[res]){break;}
|
1319
|
+
++res;
|
1320
|
+
}
|
1321
|
+
return res;
|
1322
|
+
}
|
1323
|
+
|
1324
|
+
template <class FloatT>
|
1325
|
+
const typename GLONASS_SpaceNode<FloatT>::float_t
|
1326
|
+
GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris_with_Time
|
1327
|
+
::time_step_max_per_one_integration = 60;
|
1328
|
+
|
1329
|
+
#endif /* __GLONASS_H__ */
|