gps_pvt 0.4.1 → 0.6.0
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/README.md +11 -6
- data/Rakefile +42 -23
- data/exe/gps_pvt +47 -8
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +44 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +9198 -7614
- data/ext/ninja-scan-light/tool/algorithm/interpolate.h +121 -0
- data/ext/ninja-scan-light/tool/navigation/ANTEX.h +747 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +63 -43
- data/ext/ninja-scan-light/tool/navigation/GPS.h +92 -4
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +58 -34
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +50 -17
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +19 -144
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +19 -4
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +63 -43
- data/ext/ninja-scan-light/tool/navigation/SP3.h +1178 -0
- data/ext/ninja-scan-light/tool/navigation/coordinate.h +81 -1
- data/ext/ninja-scan-light/tool/swig/GPS.i +131 -43
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +503 -16
- data/ext/ninja-scan-light/tool/util/text_helper.h +248 -0
- data/{sig/gps_pvt.rbs → gps_pvt.rbs} +0 -0
- data/lib/gps_pvt/receiver.rb +32 -5
- data/lib/gps_pvt/util.rb +50 -0
- data/lib/gps_pvt/version.rb +1 -1
- metadata +8 -3
@@ -52,6 +52,7 @@
|
|
52
52
|
#include <cstring>
|
53
53
|
#include <limits>
|
54
54
|
|
55
|
+
#include "util/text_helper.h"
|
55
56
|
#include "GPS.h"
|
56
57
|
#include "SBAS.h"
|
57
58
|
#include "GLONASS.h"
|
@@ -61,54 +62,12 @@ class RINEX_Reader {
|
|
61
62
|
public:
|
62
63
|
typedef RINEX_Reader<U> self_t;
|
63
64
|
typedef std::map<std::string, std::vector<std::string> > header_t;
|
64
|
-
|
65
|
+
|
65
66
|
protected:
|
66
67
|
header_t _header;
|
67
|
-
|
68
|
-
src_stream_t(std::istream &is) : std::istream(is.rdbuf()) {}
|
69
|
-
/**
|
70
|
-
* getline() for multi-platform (in addition to \n, \r\n and \r are supported)
|
71
|
-
*
|
72
|
-
* @see https://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
|
73
|
-
* @see https://en.cppreference.com/w/cpp/io/basic_istream/getline
|
74
|
-
* TODO gcount() mismatch
|
75
|
-
*/
|
76
|
-
std::istream& getline(
|
77
|
-
typename std::istream::char_type* s,
|
78
|
-
std::streamsize n){
|
79
|
-
std::streamsize i(1), consumed(0);
|
80
|
-
typename std::istream::sentry se(*this, true);
|
81
|
-
if((bool)se){
|
82
|
-
std::streambuf* sb(this->rdbuf());
|
83
|
-
for(; i < n; ++i){
|
84
|
-
int c(sb->sbumpc());
|
85
|
-
if(c == std::streambuf::traits_type::eof()){
|
86
|
-
this->setstate(std::ios::eofbit);
|
87
|
-
break;
|
88
|
-
}
|
89
|
-
++consumed;
|
90
|
-
if(c == '\n'){
|
91
|
-
break;
|
92
|
-
}else if(c == '\r'){
|
93
|
-
if(sb->sgetc() == '\n'){
|
94
|
-
sb->sbumpc();
|
95
|
-
++consumed;
|
96
|
-
}
|
97
|
-
break;
|
98
|
-
}
|
99
|
-
*(s++) = (typename std::istream::char_type)c;
|
100
|
-
}
|
101
|
-
}
|
102
|
-
if(((i == 1) && (consumed == 0)) || (i == n)){
|
103
|
-
this->setstate(std::ios::failbit);
|
104
|
-
}else{
|
105
|
-
*s = '\0';
|
106
|
-
}
|
107
|
-
return *this;
|
108
|
-
}
|
109
|
-
} src;
|
68
|
+
typename TextHelper<>::crlf_stream_t src;
|
110
69
|
bool _has_next;
|
111
|
-
|
70
|
+
|
112
71
|
public:
|
113
72
|
struct version_type_t {
|
114
73
|
int version;
|
@@ -184,49 +143,8 @@ class RINEX_Reader {
|
|
184
143
|
bool has_next() const {return _has_next;}
|
185
144
|
|
186
145
|
template <class T, bool is_integer = std::numeric_limits<T>::is_integer>
|
187
|
-
struct conv_t {
|
188
|
-
static bool
|
189
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
|
190
|
-
if(str2val){
|
191
|
-
std::stringstream ss(buf.substr(offset, length));
|
192
|
-
ss >> *(T *)value;
|
193
|
-
return (ss.rdstate() & std::ios_base::failbit) == 0;
|
194
|
-
}else{
|
195
|
-
std::stringstream ss;
|
196
|
-
ss << std::setfill(opt == 1 ? '0' : ' ') << std::right << std::setw(length) << *(T *)value;
|
197
|
-
buf.replace(offset, length, ss.str());
|
198
|
-
return true;
|
199
|
-
}
|
200
|
-
}
|
201
|
-
static bool f(
|
202
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
203
|
-
if(str2val){
|
204
|
-
std::stringstream ss(buf.substr(offset, length));
|
205
|
-
ss >> *(T *)value;
|
206
|
-
return (ss.rdstate() & std::ios_base::failbit) == 0;
|
207
|
-
}else{
|
208
|
-
std::stringstream ss;
|
209
|
-
ss << std::setfill(' ') << std::right << std::setw(length)
|
210
|
-
<< std::setprecision(precision) << std::fixed
|
211
|
-
<< *(T *)value;
|
212
|
-
std::string s(ss.str());
|
213
|
-
if((*(T *)value) >= 0){
|
214
|
-
if((*(T *)value) < 1){
|
215
|
-
int i(length - precision - 2);
|
216
|
-
if(i >= 0){s[i] = ' ';}
|
217
|
-
}
|
218
|
-
}else{
|
219
|
-
if((*(T *)value) > -1){
|
220
|
-
int i(length - precision - 2);
|
221
|
-
if(i >= 0){s[i] = '-';}
|
222
|
-
if(--i >= 0){s[i] = ' ';}
|
223
|
-
}
|
224
|
-
}
|
225
|
-
buf.replace(offset, length, s);
|
226
|
-
return true;
|
227
|
-
}
|
228
|
-
}
|
229
|
-
static bool e(
|
146
|
+
struct conv_t : public TextHelper<>::template format_t<T> {
|
147
|
+
static bool e_dot_head(
|
230
148
|
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
231
149
|
if(str2val){
|
232
150
|
std::string s(buf.substr(offset, length));
|
@@ -262,58 +180,25 @@ class RINEX_Reader {
|
|
262
180
|
}
|
263
181
|
}
|
264
182
|
};
|
183
|
+
|
265
184
|
template <class T>
|
266
|
-
struct conv_t<T, true> {
|
267
|
-
static bool
|
268
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
|
269
|
-
return conv_t<T, false>::d(buf, offset, length, value, opt, str2val);
|
270
|
-
}
|
271
|
-
static bool f(
|
185
|
+
struct conv_t<T, true> : public TextHelper<>::template format_t<T> {
|
186
|
+
static bool e_dot_head(
|
272
187
|
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
273
188
|
double v(*(T *)value);
|
274
189
|
bool res(
|
275
|
-
conv_t<double, false>::
|
190
|
+
conv_t<double, false>::e_dot_head(buf, offset, length, &v, precision, str2val));
|
276
191
|
*(T *)value = static_cast<T>(v);
|
277
192
|
return res;
|
278
193
|
}
|
279
|
-
static bool e(
|
280
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
281
|
-
double v(*(T *)value);
|
282
|
-
bool res(
|
283
|
-
conv_t<double, false>::e(buf, offset, length, &v, precision, str2val));
|
284
|
-
*(T *)value = static_cast<T>(v);
|
285
|
-
return res;
|
286
|
-
}
|
287
|
-
};
|
288
|
-
|
289
|
-
struct convert_item_t {
|
290
|
-
bool (*func)(
|
291
|
-
std::string &buf, const int &offset, const int &length, void *value,
|
292
|
-
const int &opt, const bool &str2val);
|
293
|
-
int offset;
|
294
|
-
int length;
|
295
|
-
int value_offset;
|
296
|
-
int opt;
|
297
194
|
};
|
298
195
|
|
299
|
-
|
300
|
-
|
301
|
-
|
302
|
-
* The return value of this function reflects it.
|
303
|
-
* @return (bool) if all conversion are successfully performed, true is returned; otherwise false.
|
304
|
-
*/
|
305
|
-
static bool convert(
|
196
|
+
typedef typename TextHelper<>::convert_item_t convert_item_t;
|
197
|
+
|
198
|
+
static inline bool convert(
|
306
199
|
const convert_item_t *items, const int &size, const std::string &buf, void *values,
|
307
200
|
bool (*recovery)(const int &, const std::string &, void *) = NULL){
|
308
|
-
|
309
|
-
bool res(true);
|
310
|
-
for(int i(0); i < size; ++i){
|
311
|
-
if((*items[i].func)(
|
312
|
-
const_cast<std::string &>(buf), items[i].offset, items[i].length, (char *)values + items[i].value_offset,
|
313
|
-
items[i].opt, true)){continue;}
|
314
|
-
res &= (recovery ? (*recovery)(i, buf, values) : false);
|
315
|
-
}
|
316
|
-
return res;
|
201
|
+
return TextHelper<>::str2val(items, size, buf, values, recovery);
|
317
202
|
}
|
318
203
|
template <int N>
|
319
204
|
static inline bool convert(
|
@@ -1408,10 +1293,10 @@ class RINEX_OBS_Reader : public RINEX_Reader<> {
|
|
1408
1293
|
{super_t::template conv_t<value_type>::d, offset, length, \
|
1409
1294
|
offsetof(container_type, container_member), 1}
|
1410
1295
|
#define GEN_F2(offset, length, precision, container_type, container_member, value_type) \
|
1411
|
-
{super_t::template conv_t<value_type>::
|
1296
|
+
{super_t::template conv_t<value_type>::f_dot_head, offset, length, \
|
1412
1297
|
offsetof(container_type, container_member), precision}
|
1413
1298
|
#define GEN_E2(offset, length, precision, container_type, container_member, value_type) \
|
1414
|
-
{super_t::template conv_t<value_type>::
|
1299
|
+
{super_t::template conv_t<value_type>::e_dot_head, offset, length, \
|
1415
1300
|
offsetof(container_type, container_member), precision}
|
1416
1301
|
#define GEN_F(offset, length, precision, container_type, container_member) \
|
1417
1302
|
GEN_F2(offset, length, precision, container_type, container_member, FloatT)
|
@@ -1940,14 +1825,14 @@ class RINEX_Writer {
|
|
1940
1825
|
static std::string RINEX_Float(
|
1941
1826
|
const FloatT &value, const int width = 19, const int precision = 12){
|
1942
1827
|
std::string s;
|
1943
|
-
RINEX_Reader<U>::template conv_t<FloatT>::
|
1828
|
+
RINEX_Reader<U>::template conv_t<FloatT>::f_dot_head(s, 0, width, &const_cast<FloatT &>(value), precision, false);
|
1944
1829
|
return s;
|
1945
1830
|
}
|
1946
1831
|
template <class FloatT>
|
1947
1832
|
static std::string RINEX_FloatD(
|
1948
1833
|
const FloatT &value, const int width = 19, const int precision = 12){
|
1949
1834
|
std::string s;
|
1950
|
-
RINEX_Reader<U>::template conv_t<FloatT>::
|
1835
|
+
RINEX_Reader<U>::template conv_t<FloatT>::e_dot_head(s, 0, width, &const_cast<FloatT &>(value), precision, false);
|
1951
1836
|
return s;
|
1952
1837
|
}
|
1953
1838
|
template <class T>
|
@@ -1957,20 +1842,10 @@ class RINEX_Writer {
|
|
1957
1842
|
return s;
|
1958
1843
|
}
|
1959
1844
|
|
1960
|
-
/**
|
1961
|
-
* @return If all conversion are successfully performed, then true; otherwise false;
|
1962
|
-
*/
|
1963
1845
|
static bool convert(
|
1964
1846
|
const typename RINEX_Reader<U>::convert_item_t *items, const int &size,
|
1965
1847
|
std::string &buf, const void *values){
|
1966
|
-
|
1967
|
-
bool res(true);
|
1968
|
-
for(int i(0); i < size; ++i){
|
1969
|
-
res &= (*items[i].func)(
|
1970
|
-
buf, items[i].offset, items[i].length, (char *)(const_cast<void *>(values)) + items[i].value_offset,
|
1971
|
-
items[i].opt, false);
|
1972
|
-
}
|
1973
|
-
return res;
|
1848
|
+
return TextHelper<>::val2str(items, size, buf, values);
|
1974
1849
|
}
|
1975
1850
|
template <int N>
|
1976
1851
|
static inline bool convert(
|
@@ -2183,10 +2183,11 @@ template <typename T>
|
|
2183
2183
|
typename SBAS_SpaceNode<FloatT>::KnownSatellites::list_t
|
2184
2184
|
SBAS_SpaceNode<FloatT>::KnownSatellites::sort(T sorter){
|
2185
2185
|
static const typename SBAS_SpaceNode<FloatT>::RangingCode codes[] = {
|
2186
|
+
{120, 145, 01106, 5, "ASECNA (A-SBAS)"},
|
2186
2187
|
{121, 175, 01241, 5, "EGNOS (Eutelsat 5WB)"},
|
2187
2188
|
{122, 52, 00267, 143.5, "SPAN (INMARSAT 4F1)"},
|
2188
2189
|
{123, 21, 00232, 31.5, "EGNOS (ASTRA 5B)"},
|
2189
|
-
{124, 237, 01617, 0, "
|
2190
|
+
//{124, 237, 01617, 0, "(Reserved)"},
|
2190
2191
|
{125, 235, 01076, -16, "SDCM (Luch-5A)"},
|
2191
2192
|
{126, 886, 01764, 63.9, "EGNOS (INMARSAT 4F2)"},
|
2192
2193
|
{127, 657, 00717, 55, "GAGAN (GSAT-8)"},
|
@@ -2198,16 +2199,30 @@ typename SBAS_SpaceNode<FloatT>::KnownSatellites::list_t
|
|
2198
2199
|
{133, 603, 01731, -129, "WAAS (SES-15)"},
|
2199
2200
|
{134, 130, 00706, 91.5, "KASS (MEASAT-3D)"},
|
2200
2201
|
{135, 359, 01216, -125, "WAAS (Intelsat Galaxy 30)"},
|
2201
|
-
{136, 595, 00740, 5, "EGNOS (
|
2202
|
+
{136, 595, 00740, 5, "EGNOS (HOTBIRD 13G)"},
|
2202
2203
|
{137, 68, 01007, 127, "MSAS (QZS-3)"},
|
2203
2204
|
{138, 386, 00450, 107.3, "WAAS (ANIK F1R)"},
|
2205
|
+
//{139, 797, 00305, 0, "MSAS (QZS-7)"},
|
2204
2206
|
{140, 456, 01653, 95, "SDCM (Luch-5V)"},
|
2205
2207
|
{141, 499, 01411, 167, "SDCM (Luch-5A)"},
|
2208
|
+
//{142, 883, 01644, 0, "(Unallocated)"},
|
2206
2209
|
{143, 307, 01312, 110.5, "BDSBAS (G3)"},
|
2207
2210
|
{144, 127, 01060, 80, "BDSBAS (G2)"},
|
2208
|
-
{
|
2211
|
+
//{145, 211, 01560, 0, "(Unallocated)"},
|
2212
|
+
//{146, 121, 00035, 0, "(Unallocated)"},
|
2213
|
+
{147, 118, 00355, 5, "ASECNA (A-SBAS)"},
|
2209
2214
|
{148, 163, 00335, -24.8, "ASAL (ALCOMSAT-1)"},
|
2210
|
-
}
|
2215
|
+
//{149, 628, 01254, 0, "(Unallocated)"},
|
2216
|
+
//{150, 853, 01041, 0, "EGNOS"},
|
2217
|
+
//{151, 484, 00142, 0, "(Unallocated)"},
|
2218
|
+
//{152, 289, 01641, 0, "(Unallocated)"},
|
2219
|
+
//{153, 811, 01504, 0, "(Unallocated)"},
|
2220
|
+
//{154, 202, 00751, 0, "(Unallocated)"},
|
2221
|
+
//{155, 1021, 01774, 0, "(Unallocated)"},
|
2222
|
+
//{156, 463, 00107, 0, "(Unallocated)"},
|
2223
|
+
//{157, 568, 01153, 0, "(Unallocated)"},
|
2224
|
+
//{158, 904, 01542, 0, "(Unallocated)"},
|
2225
|
+
}; ///< @see https://www.gps.gov/technical/prn-codes/L1-CA-PRN-code-assignments-2021-Jun.pdf
|
2211
2226
|
list_t res;
|
2212
2227
|
res.reserve(sizeof(codes) / sizeof(codes[0]));
|
2213
2228
|
for(unsigned int i(0); i < sizeof(codes) / sizeof(codes[0]); ++i){
|
@@ -73,7 +73,6 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
73
73
|
|
74
74
|
typedef SBAS_SpaceNode<float_t> space_node_t;
|
75
75
|
inheritate_type(gps_time_t);
|
76
|
-
typedef typename space_node_t::Satellite satellite_t;
|
77
76
|
|
78
77
|
inheritate_type(xyz_t);
|
79
78
|
inheritate_type(enu_t);
|
@@ -81,6 +80,7 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
81
80
|
inheritate_type(pos_t);
|
82
81
|
|
83
82
|
typedef typename base_t::measurement_t measurement_t;
|
83
|
+
typedef typename base_t::satellite_t satellite_t;
|
84
84
|
typedef typename base_t::range_error_t range_error_t;
|
85
85
|
typedef typename base_t::range_corrector_t range_corrector_t;
|
86
86
|
typedef typename base_t::range_correction_t range_correction_t;
|
@@ -92,11 +92,52 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
92
92
|
SBAS_SinglePositioning_Options<float_t>, base_t> options_t;
|
93
93
|
|
94
94
|
protected:
|
95
|
-
const space_node_t &_space_node;
|
96
95
|
SBAS_SinglePositioning_Options<float_t> _options;
|
97
96
|
|
98
97
|
public:
|
99
|
-
|
98
|
+
struct satellites_t {
|
99
|
+
const void *impl;
|
100
|
+
satellite_t (*impl_select)(const void *, const prn_t &, const gps_time_t &);
|
101
|
+
inline satellite_t select(const prn_t &prn, const gps_time_t &receiver_time) const {
|
102
|
+
return impl_select(impl, prn, receiver_time);
|
103
|
+
}
|
104
|
+
static satellite_t select_broadcast(
|
105
|
+
const void *ptr, const prn_t &prn, const gps_time_t &receiver_time){
|
106
|
+
const typename space_node_t::satellites_t &sats(
|
107
|
+
reinterpret_cast<const space_node_t *>(ptr)->satellites());
|
108
|
+
const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn));
|
109
|
+
if((it_sat == sats.end()) // has ephemeris?
|
110
|
+
|| (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
|
111
|
+
return satellite_t::unavailable();
|
112
|
+
}
|
113
|
+
struct impl_t {
|
114
|
+
static inline const typename space_node_t::Satellite &sat(const void *ptr) {
|
115
|
+
return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
|
116
|
+
}
|
117
|
+
static xyz_t position(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
118
|
+
return sat(ptr).ephemeris().constellation(t, pseudo_range, false).position;
|
119
|
+
}
|
120
|
+
static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
121
|
+
return sat(ptr).ephemeris().constellation(t, pseudo_range, true).velocity;
|
122
|
+
}
|
123
|
+
static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
124
|
+
// Clock correction is taken into account in position()
|
125
|
+
return 0;
|
126
|
+
}
|
127
|
+
static float_t clock_error_dot(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) {
|
128
|
+
// Clock rate error is taken in account in velocity()
|
129
|
+
return 0;
|
130
|
+
}
|
131
|
+
};
|
132
|
+
satellite_t res = {
|
133
|
+
&(it_sat->second),
|
134
|
+
impl_t::position, impl_t::velocity,
|
135
|
+
impl_t::clock_error, impl_t::clock_error_dot};
|
136
|
+
return res;
|
137
|
+
}
|
138
|
+
satellites_t(const space_node_t &sn)
|
139
|
+
: impl(&sn), impl_select(select_broadcast) {}
|
140
|
+
} satellites;
|
100
141
|
|
101
142
|
struct ionospheric_sbas_t : public range_corrector_t {
|
102
143
|
const space_node_t &space_node;
|
@@ -155,7 +196,8 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
155
196
|
}
|
156
197
|
|
157
198
|
SBAS_SinglePositioning(const space_node_t &sn)
|
158
|
-
: base_t(),
|
199
|
+
: base_t(), _options(available_options(options_t())),
|
200
|
+
satellites(sn),
|
159
201
|
ionospheric_sbas(sn), tropospheric_sbas() {
|
160
202
|
|
161
203
|
// default ionospheric correction: Broadcasted IGP.
|
@@ -168,25 +210,17 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
168
210
|
~SBAS_SinglePositioning(){}
|
169
211
|
|
170
212
|
/**
|
171
|
-
*
|
213
|
+
* Select satellite by using PRN and time
|
172
214
|
*
|
173
215
|
* @param prn satellite number
|
174
216
|
* @param receiver_time receiver time
|
175
|
-
* @return (
|
176
|
-
* otherwise NULL.
|
217
|
+
* @return (satellite_t) If available, satellite.is_available() returning true is returned.
|
177
218
|
*/
|
178
|
-
|
179
|
-
const
|
219
|
+
satellite_t select_satellite(
|
220
|
+
const prn_t &prn,
|
180
221
|
const gps_time_t &receiver_time) const {
|
181
|
-
|
182
|
-
|
183
|
-
const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn));
|
184
|
-
if((it_sat == sats.end()) // has ephemeris?
|
185
|
-
|| (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris?
|
186
|
-
return NULL;
|
187
|
-
}
|
188
|
-
|
189
|
-
return &(it_sat->second);
|
222
|
+
if(_options.exclude_prn[prn]){return satellite_t::unavailable();}
|
223
|
+
return satellites.select(prn, receiver_time);
|
190
224
|
}
|
191
225
|
|
192
226
|
/**
|
@@ -210,33 +244,28 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
210
244
|
|
211
245
|
relative_property_t res = {0};
|
212
246
|
|
213
|
-
if(_options.exclude_prn[prn]){return res;}
|
214
|
-
|
215
247
|
float_t range;
|
216
248
|
range_error_t range_error;
|
217
249
|
if(!this->range(measurement, range, &range_error)){
|
218
250
|
return res; // If no range entry, return with weight = 0
|
219
251
|
}
|
220
252
|
|
221
|
-
|
222
|
-
if(!sat){return res;} // If satellite is unavailable, return with weight = 0
|
253
|
+
satellite_t sat(select_satellite(prn, time_arrival));
|
254
|
+
if(!sat.is_available()){return res;} // If satellite is unavailable, return with weight = 0
|
223
255
|
|
224
256
|
///< The following procedure is based on Appendix.S with modification
|
225
257
|
|
226
258
|
range -= receiver_error;
|
227
259
|
|
228
|
-
// Clock correction
|
229
|
-
|
230
|
-
|
231
|
-
|
260
|
+
// Clock error correction
|
261
|
+
range += ((range_error.unknown_flag & range_error_t::SATELLITE_CLOCK)
|
262
|
+
? (sat.clock_error(time_arrival, range) * GPS_SpaceNode<float_t>::light_speed)
|
263
|
+
: range_error.value[range_error_t::SATELLITE_CLOCK]);
|
232
264
|
|
233
265
|
// TODO WAAS long term clock correction (2.1.1.4.11)
|
234
266
|
|
235
|
-
// Calculate satellite position
|
236
|
-
|
237
|
-
sat->ephemeris().constellation(time_arrival, range));
|
238
|
-
|
239
|
-
const xyz_t &sat_pos(sat_pos_vel.position);
|
267
|
+
// Calculate satellite position
|
268
|
+
xyz_t sat_pos(sat.position(time_arrival, range));
|
240
269
|
float_t geometric_range(usr_pos.xyz.dist(sat_pos));
|
241
270
|
|
242
271
|
// Calculate residual with Sagnac correction (A.4.4.11)
|
@@ -283,24 +312,15 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
283
312
|
|
284
313
|
res.range_corrected = range;
|
285
314
|
|
286
|
-
xyz_t rel_vel(
|
315
|
+
xyz_t rel_vel(sat.velocity(time_arrival, range) - usr_vel); // Calculate velocity
|
287
316
|
|
288
|
-
// Note: clock rate error is already accounted for in constellation()
|
289
317
|
res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
|
290
318
|
+ res.los_neg[1] * rel_vel.y()
|
291
|
-
+ res.los_neg[2] * rel_vel.z()
|
319
|
+
+ res.los_neg[2] * rel_vel.z()
|
320
|
+
+ sat.clock_error_dot(time_arrival, range) * GPS_SpaceNode<float_t>::light_speed;
|
292
321
|
|
293
322
|
return res;
|
294
323
|
}
|
295
|
-
|
296
|
-
xyz_t *satellite_position(
|
297
|
-
const prn_t &prn,
|
298
|
-
const gps_time_t &time,
|
299
|
-
xyz_t &res) const {
|
300
|
-
|
301
|
-
const satellite_t *sat(is_available(prn, time));
|
302
|
-
return sat ? &(res = sat->ephemeris().constellation(time).position) : NULL;
|
303
|
-
}
|
304
324
|
};
|
305
325
|
|
306
326
|
#endif /* __SBAS_SOLVER_H__ */
|