gps_pvt 0.5.0 → 0.6.1
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- checksums.yaml +4 -4
- data/README.md +11 -7
- data/Rakefile +42 -23
- data/exe/gps_pvt +13 -6
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +44 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +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 +41 -154
- 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 +1194 -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/lib/gps_pvt/receiver.rb +24 -0
- data/lib/gps_pvt/util.rb +25 -7
- data/lib/gps_pvt/version.rb +1 -1
- metadata +6 -2
@@ -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(
|
@@ -961,29 +846,37 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
961
846
|
int year, month, day;
|
962
847
|
FloatT tau_c_neg, tau_GPS; // TODO check tau_GPS polarity
|
963
848
|
int leap_sec;
|
849
|
+
int flags;
|
850
|
+
enum {
|
851
|
+
TAU_C_NEG = 0x01,
|
852
|
+
TAU_GPS = 0x02,
|
853
|
+
LEAP_SEC = 0x04,
|
854
|
+
};
|
964
855
|
};
|
965
856
|
static const typename super_t::convert_item_t t_corr_glonass_v2[4];
|
966
857
|
|
967
858
|
bool extract_t_corr_glonass_v2(t_corr_glonass_t &t_corr_glonass) const {
|
968
|
-
|
859
|
+
t_corr_glonass.flags = 0;
|
969
860
|
super_t::header_t::const_iterator it;
|
970
861
|
|
971
|
-
if(
|
862
|
+
if((it = _header.find("CORR TO SYSTEM TIME")) != _header.end()){
|
972
863
|
super_t::convert(t_corr_glonass_v2, it->second.front(), &t_corr_glonass);
|
864
|
+
t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG;
|
973
865
|
}
|
974
866
|
|
975
|
-
if(
|
867
|
+
if((it = _header.find("LEAP SECONDS")) != _header.end()){
|
976
868
|
iono_utc_t iono_utc;
|
977
869
|
super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
|
978
870
|
t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
|
871
|
+
t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
|
979
872
|
}
|
980
873
|
|
981
|
-
return
|
874
|
+
return t_corr_glonass.flags > 0;
|
982
875
|
}
|
983
876
|
|
984
877
|
bool extract_t_corr_glonass_v3(t_corr_glonass_t &t_corr_glonass) const {
|
985
878
|
iono_utc_t iono_utc;
|
986
|
-
|
879
|
+
t_corr_glonass.flags = 0;
|
987
880
|
typedef super_t::header_t::const_iterator it_t;
|
988
881
|
typedef super_t::header_t::mapped_type::const_iterator it2_t;
|
989
882
|
|
@@ -995,11 +888,12 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
995
888
|
super_t::convert(utc_v3, *it2, &iono_utc);
|
996
889
|
t_corr_glonass.year = t_corr_glonass.month = t_corr_glonass.day = 0;
|
997
890
|
t_corr_glonass.tau_c_neg = iono_utc.A0;
|
891
|
+
t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG;
|
998
892
|
}else if(it2->find("GLGP") != it2->npos){
|
999
893
|
super_t::convert(utc_v3, *it2, &iono_utc);
|
1000
894
|
t_corr_glonass.tau_GPS = iono_utc.A0;
|
895
|
+
t_corr_glonass.flags |= t_corr_glonass_t::TAU_GPS;
|
1001
896
|
}
|
1002
|
-
utc = true;
|
1003
897
|
}
|
1004
898
|
}
|
1005
899
|
|
@@ -1010,10 +904,10 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
1010
904
|
super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
|
1011
905
|
}
|
1012
906
|
t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
|
1013
|
-
|
907
|
+
t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
|
1014
908
|
}
|
1015
909
|
|
1016
|
-
return
|
910
|
+
return t_corr_glonass.flags > 0;
|
1017
911
|
}
|
1018
912
|
|
1019
913
|
struct space_node_list_t {
|
@@ -1070,8 +964,11 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
1070
964
|
typename message_glonass_t::eph_t eph0(reader.msg_glonass);
|
1071
965
|
eph0.tau_c = -t_corr_glonass.tau_c_neg;
|
1072
966
|
eph0.tau_GPS = t_corr_glonass.tau_GPS;
|
1073
|
-
typename GLONASS_SpaceNode<FloatT
|
1074
|
-
|
967
|
+
typename GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris_with_GPS_Time eph(
|
968
|
+
eph0,
|
969
|
+
(t_corr_glonass.flags & t_corr_glonass_t::LEAP_SEC)
|
970
|
+
? t_corr_glonass.leap_sec
|
971
|
+
: GPS_Time<FloatT>::guess_leap_seconds(reader.msg_glonass.date_tm));
|
1075
972
|
space_nodes.glonass->satellite(reader.msg_glonass.svid).register_ephemeris(eph);
|
1076
973
|
res++;
|
1077
974
|
break;
|
@@ -1408,10 +1305,10 @@ class RINEX_OBS_Reader : public RINEX_Reader<> {
|
|
1408
1305
|
{super_t::template conv_t<value_type>::d, offset, length, \
|
1409
1306
|
offsetof(container_type, container_member), 1}
|
1410
1307
|
#define GEN_F2(offset, length, precision, container_type, container_member, value_type) \
|
1411
|
-
{super_t::template conv_t<value_type>::
|
1308
|
+
{super_t::template conv_t<value_type>::f_dot_head, offset, length, \
|
1412
1309
|
offsetof(container_type, container_member), precision}
|
1413
1310
|
#define GEN_E2(offset, length, precision, container_type, container_member, value_type) \
|
1414
|
-
{super_t::template conv_t<value_type>::
|
1311
|
+
{super_t::template conv_t<value_type>::e_dot_head, offset, length, \
|
1415
1312
|
offsetof(container_type, container_member), precision}
|
1416
1313
|
#define GEN_F(offset, length, precision, container_type, container_member) \
|
1417
1314
|
GEN_F2(offset, length, precision, container_type, container_member, FloatT)
|
@@ -1940,14 +1837,14 @@ class RINEX_Writer {
|
|
1940
1837
|
static std::string RINEX_Float(
|
1941
1838
|
const FloatT &value, const int width = 19, const int precision = 12){
|
1942
1839
|
std::string s;
|
1943
|
-
RINEX_Reader<U>::template conv_t<FloatT>::
|
1840
|
+
RINEX_Reader<U>::template conv_t<FloatT>::f_dot_head(s, 0, width, &const_cast<FloatT &>(value), precision, false);
|
1944
1841
|
return s;
|
1945
1842
|
}
|
1946
1843
|
template <class FloatT>
|
1947
1844
|
static std::string RINEX_FloatD(
|
1948
1845
|
const FloatT &value, const int width = 19, const int precision = 12){
|
1949
1846
|
std::string s;
|
1950
|
-
RINEX_Reader<U>::template conv_t<FloatT>::
|
1847
|
+
RINEX_Reader<U>::template conv_t<FloatT>::e_dot_head(s, 0, width, &const_cast<FloatT &>(value), precision, false);
|
1951
1848
|
return s;
|
1952
1849
|
}
|
1953
1850
|
template <class T>
|
@@ -1957,20 +1854,10 @@ class RINEX_Writer {
|
|
1957
1854
|
return s;
|
1958
1855
|
}
|
1959
1856
|
|
1960
|
-
/**
|
1961
|
-
* @return If all conversion are successfully performed, then true; otherwise false;
|
1962
|
-
*/
|
1963
1857
|
static bool convert(
|
1964
1858
|
const typename RINEX_Reader<U>::convert_item_t *items, const int &size,
|
1965
1859
|
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;
|
1860
|
+
return TextHelper<>::val2str(items, size, buf, values);
|
1974
1861
|
}
|
1975
1862
|
template <int N>
|
1976
1863
|
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__ */
|