gps_pvt 0.5.1 → 0.6.2
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 +9015 -7695
- 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.h +37 -35
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +16 -13
- data/ext/ninja-scan-light/tool/navigation/GPS.h +121 -29
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +19 -12
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +39 -11
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +40 -171
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +21 -13
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +19 -16
- data/ext/ninja-scan-light/tool/navigation/SP3.h +1189 -0
- data/ext/ninja-scan-light/tool/navigation/coordinate.h +81 -1
- data/ext/ninja-scan-light/tool/swig/GPS.i +153 -40
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +503 -0
- 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,56 +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
|
-
struct src_stream_t : public std::istream {
|
66
|
-
src_stream_t(std::istream &is) : std::istream(is.rdbuf()) {}
|
67
|
-
/**
|
68
|
-
* getline() for multi-platform (in addition to \n, \r\n and \r are supported)
|
69
|
-
*
|
70
|
-
* @see https://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
|
71
|
-
* @see https://en.cppreference.com/w/cpp/io/basic_istream/getline
|
72
|
-
* TODO gcount() mismatch
|
73
|
-
*/
|
74
|
-
std::istream& getline(
|
75
|
-
typename std::istream::char_type* s,
|
76
|
-
std::streamsize n){
|
77
|
-
std::streamsize i(1), consumed(0);
|
78
|
-
typename std::istream::sentry se(*this, true);
|
79
|
-
if((bool)se){
|
80
|
-
std::streambuf* sb(this->rdbuf());
|
81
|
-
for(; i < n; ++i){
|
82
|
-
int c(sb->sbumpc());
|
83
|
-
if(c == std::streambuf::traits_type::eof()){
|
84
|
-
this->setstate(std::ios::eofbit);
|
85
|
-
break;
|
86
|
-
}
|
87
|
-
++consumed;
|
88
|
-
if(c == '\n'){
|
89
|
-
break;
|
90
|
-
}else if(c == '\r'){
|
91
|
-
if(sb->sgetc() == '\n'){
|
92
|
-
sb->sbumpc();
|
93
|
-
++consumed;
|
94
|
-
}
|
95
|
-
break;
|
96
|
-
}
|
97
|
-
*(s++) = (typename std::istream::char_type)c;
|
98
|
-
}
|
99
|
-
}
|
100
|
-
if(((i == 1) && (consumed == 0)) || (i == n)){
|
101
|
-
this->setstate(std::ios::failbit);
|
102
|
-
}else{
|
103
|
-
*s = '\0';
|
104
|
-
}
|
105
|
-
return *this;
|
106
|
-
}
|
107
|
-
};
|
108
65
|
|
109
66
|
protected:
|
110
67
|
header_t _header;
|
111
|
-
|
68
|
+
typename TextHelper<>::crlf_stream_t src;
|
112
69
|
bool _has_next;
|
113
|
-
|
70
|
+
|
114
71
|
public:
|
115
72
|
struct version_type_t {
|
116
73
|
int version;
|
@@ -186,57 +143,8 @@ class RINEX_Reader {
|
|
186
143
|
bool has_next() const {return _has_next;}
|
187
144
|
|
188
145
|
template <class T, bool is_integer = std::numeric_limits<T>::is_integer>
|
189
|
-
struct conv_t {
|
190
|
-
static bool
|
191
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
|
192
|
-
if(str2val){
|
193
|
-
std::stringstream ss(buf.substr(offset, length));
|
194
|
-
ss >> *(T *)value;
|
195
|
-
return (ss.rdstate() & std::ios_base::failbit) == 0;
|
196
|
-
}else{
|
197
|
-
std::stringstream ss;
|
198
|
-
ss << std::setfill(opt == 1 ? '0' : ' ') << std::right << std::setw(length) << *(T *)value;
|
199
|
-
buf.replace(offset, length, ss.str());
|
200
|
-
return true;
|
201
|
-
}
|
202
|
-
}
|
203
|
-
static bool f_pure(
|
204
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
205
|
-
if(str2val){
|
206
|
-
std::stringstream ss(buf.substr(offset, length));
|
207
|
-
ss >> *(T *)value;
|
208
|
-
return (ss.rdstate() & std::ios_base::failbit) == 0;
|
209
|
-
}else{
|
210
|
-
std::stringstream ss;
|
211
|
-
ss << std::setfill(' ') << std::right << std::setw(length)
|
212
|
-
<< std::setprecision(precision) << std::fixed
|
213
|
-
<< *(T *)value;
|
214
|
-
buf.replace(offset, length, ss.str());
|
215
|
-
return true;
|
216
|
-
}
|
217
|
-
}
|
218
|
-
static bool f(
|
219
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
220
|
-
bool res(f_pure(buf, offset, length, value, precision, str2val));
|
221
|
-
if((!str2val) && res){
|
222
|
-
if((*(T *)value) >= 0){
|
223
|
-
if((*(T *)value) < 1){
|
224
|
-
int i(length - precision - 2);
|
225
|
-
// 0.12345 => .12345
|
226
|
-
if(i >= 0){buf[i + offset] = ' ';}
|
227
|
-
}
|
228
|
-
}else{
|
229
|
-
if((*(T *)value) > -1){
|
230
|
-
int i(length - precision - 2);
|
231
|
-
// -0.12345 => -.12345
|
232
|
-
if(i >= 0){buf[i + offset] = '-';}
|
233
|
-
if(--i >= 0){buf[i + offset] = ' ';}
|
234
|
-
}
|
235
|
-
}
|
236
|
-
}
|
237
|
-
return res;
|
238
|
-
}
|
239
|
-
static bool e(
|
146
|
+
struct conv_t : public TextHelper<>::template format_t<T> {
|
147
|
+
static bool e_dot_head(
|
240
148
|
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
241
149
|
if(str2val){
|
242
150
|
std::string s(buf.substr(offset, length));
|
@@ -272,66 +180,25 @@ class RINEX_Reader {
|
|
272
180
|
}
|
273
181
|
}
|
274
182
|
};
|
183
|
+
|
275
184
|
template <class T>
|
276
|
-
struct conv_t<T, true> {
|
277
|
-
static bool
|
278
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
|
279
|
-
return conv_t<T, false>::d(buf, offset, length, value, opt, str2val);
|
280
|
-
}
|
281
|
-
static bool f_pure(
|
282
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
283
|
-
double v(*(T *)value);
|
284
|
-
bool res(
|
285
|
-
conv_t<double, false>::f_pure(buf, offset, length, &v, precision, str2val));
|
286
|
-
*(T *)value = static_cast<T>(v);
|
287
|
-
return res;
|
288
|
-
}
|
289
|
-
static bool f(
|
290
|
-
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
291
|
-
double v(*(T *)value);
|
292
|
-
bool res(
|
293
|
-
conv_t<double, false>::f(buf, offset, length, &v, precision, str2val));
|
294
|
-
*(T *)value = static_cast<T>(v);
|
295
|
-
return res;
|
296
|
-
}
|
297
|
-
static bool e(
|
185
|
+
struct conv_t<T, true> : public TextHelper<>::template format_t<T> {
|
186
|
+
static bool e_dot_head(
|
298
187
|
std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
|
299
188
|
double v(*(T *)value);
|
300
189
|
bool res(
|
301
|
-
conv_t<double, false>::
|
190
|
+
conv_t<double, false>::e_dot_head(buf, offset, length, &v, precision, str2val));
|
302
191
|
*(T *)value = static_cast<T>(v);
|
303
192
|
return res;
|
304
193
|
}
|
305
194
|
};
|
306
195
|
|
307
|
-
|
308
|
-
|
309
|
-
|
310
|
-
const int &opt, const bool &str2val);
|
311
|
-
int offset;
|
312
|
-
int length;
|
313
|
-
int value_offset;
|
314
|
-
int opt;
|
315
|
-
};
|
316
|
-
|
317
|
-
/**
|
318
|
-
* @param recovery if conversion fails, then this functor is invoked.
|
319
|
-
* If recovery is successfully performed, this functor should return true.
|
320
|
-
* The return value of this function reflects it.
|
321
|
-
* @return (bool) if all conversion are successfully performed, true is returned; otherwise false.
|
322
|
-
*/
|
323
|
-
static bool convert(
|
196
|
+
typedef typename TextHelper<>::convert_item_t convert_item_t;
|
197
|
+
|
198
|
+
static inline bool convert(
|
324
199
|
const convert_item_t *items, const int &size, const std::string &buf, void *values,
|
325
200
|
bool (*recovery)(const int &, const std::string &, void *) = NULL){
|
326
|
-
|
327
|
-
bool res(true);
|
328
|
-
for(int i(0); i < size; ++i){
|
329
|
-
if((*items[i].func)(
|
330
|
-
const_cast<std::string &>(buf), items[i].offset, items[i].length, (char *)values + items[i].value_offset,
|
331
|
-
items[i].opt, true)){continue;}
|
332
|
-
res &= (recovery ? (*recovery)(i, buf, values) : false);
|
333
|
-
}
|
334
|
-
return res;
|
201
|
+
return TextHelper<>::str2val(items, size, buf, values, recovery);
|
335
202
|
}
|
336
203
|
template <int N>
|
337
204
|
static inline bool convert(
|
@@ -979,29 +846,37 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
979
846
|
int year, month, day;
|
980
847
|
FloatT tau_c_neg, tau_GPS; // TODO check tau_GPS polarity
|
981
848
|
int leap_sec;
|
849
|
+
int flags;
|
850
|
+
enum {
|
851
|
+
TAU_C_NEG = 0x01,
|
852
|
+
TAU_GPS = 0x02,
|
853
|
+
LEAP_SEC = 0x04,
|
854
|
+
};
|
982
855
|
};
|
983
856
|
static const typename super_t::convert_item_t t_corr_glonass_v2[4];
|
984
857
|
|
985
858
|
bool extract_t_corr_glonass_v2(t_corr_glonass_t &t_corr_glonass) const {
|
986
|
-
|
859
|
+
t_corr_glonass.flags = 0;
|
987
860
|
super_t::header_t::const_iterator it;
|
988
861
|
|
989
|
-
if(
|
862
|
+
if((it = _header.find("CORR TO SYSTEM TIME")) != _header.end()){
|
990
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;
|
991
865
|
}
|
992
866
|
|
993
|
-
if(
|
867
|
+
if((it = _header.find("LEAP SECONDS")) != _header.end()){
|
994
868
|
iono_utc_t iono_utc;
|
995
869
|
super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
|
996
870
|
t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
|
871
|
+
t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
|
997
872
|
}
|
998
873
|
|
999
|
-
return
|
874
|
+
return t_corr_glonass.flags > 0;
|
1000
875
|
}
|
1001
876
|
|
1002
877
|
bool extract_t_corr_glonass_v3(t_corr_glonass_t &t_corr_glonass) const {
|
1003
878
|
iono_utc_t iono_utc;
|
1004
|
-
|
879
|
+
t_corr_glonass.flags = 0;
|
1005
880
|
typedef super_t::header_t::const_iterator it_t;
|
1006
881
|
typedef super_t::header_t::mapped_type::const_iterator it2_t;
|
1007
882
|
|
@@ -1013,11 +888,12 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
1013
888
|
super_t::convert(utc_v3, *it2, &iono_utc);
|
1014
889
|
t_corr_glonass.year = t_corr_glonass.month = t_corr_glonass.day = 0;
|
1015
890
|
t_corr_glonass.tau_c_neg = iono_utc.A0;
|
891
|
+
t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG;
|
1016
892
|
}else if(it2->find("GLGP") != it2->npos){
|
1017
893
|
super_t::convert(utc_v3, *it2, &iono_utc);
|
1018
894
|
t_corr_glonass.tau_GPS = iono_utc.A0;
|
895
|
+
t_corr_glonass.flags |= t_corr_glonass_t::TAU_GPS;
|
1019
896
|
}
|
1020
|
-
utc = true;
|
1021
897
|
}
|
1022
898
|
}
|
1023
899
|
|
@@ -1028,10 +904,10 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
1028
904
|
super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
|
1029
905
|
}
|
1030
906
|
t_corr_glonass.leap_sec = iono_utc.delta_t_LS;
|
1031
|
-
|
907
|
+
t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC;
|
1032
908
|
}
|
1033
909
|
|
1034
|
-
return
|
910
|
+
return t_corr_glonass.flags > 0;
|
1035
911
|
}
|
1036
912
|
|
1037
913
|
struct space_node_list_t {
|
@@ -1088,8 +964,11 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
|
|
1088
964
|
typename message_glonass_t::eph_t eph0(reader.msg_glonass);
|
1089
965
|
eph0.tau_c = -t_corr_glonass.tau_c_neg;
|
1090
966
|
eph0.tau_GPS = t_corr_glonass.tau_GPS;
|
1091
|
-
typename GLONASS_SpaceNode<FloatT
|
1092
|
-
|
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));
|
1093
972
|
space_nodes.glonass->satellite(reader.msg_glonass.svid).register_ephemeris(eph);
|
1094
973
|
res++;
|
1095
974
|
break;
|
@@ -1426,10 +1305,10 @@ class RINEX_OBS_Reader : public RINEX_Reader<> {
|
|
1426
1305
|
{super_t::template conv_t<value_type>::d, offset, length, \
|
1427
1306
|
offsetof(container_type, container_member), 1}
|
1428
1307
|
#define GEN_F2(offset, length, precision, container_type, container_member, value_type) \
|
1429
|
-
{super_t::template conv_t<value_type>::
|
1308
|
+
{super_t::template conv_t<value_type>::f_dot_head, offset, length, \
|
1430
1309
|
offsetof(container_type, container_member), precision}
|
1431
1310
|
#define GEN_E2(offset, length, precision, container_type, container_member, value_type) \
|
1432
|
-
{super_t::template conv_t<value_type>::
|
1311
|
+
{super_t::template conv_t<value_type>::e_dot_head, offset, length, \
|
1433
1312
|
offsetof(container_type, container_member), precision}
|
1434
1313
|
#define GEN_F(offset, length, precision, container_type, container_member) \
|
1435
1314
|
GEN_F2(offset, length, precision, container_type, container_member, FloatT)
|
@@ -1958,14 +1837,14 @@ class RINEX_Writer {
|
|
1958
1837
|
static std::string RINEX_Float(
|
1959
1838
|
const FloatT &value, const int width = 19, const int precision = 12){
|
1960
1839
|
std::string s;
|
1961
|
-
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);
|
1962
1841
|
return s;
|
1963
1842
|
}
|
1964
1843
|
template <class FloatT>
|
1965
1844
|
static std::string RINEX_FloatD(
|
1966
1845
|
const FloatT &value, const int width = 19, const int precision = 12){
|
1967
1846
|
std::string s;
|
1968
|
-
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);
|
1969
1848
|
return s;
|
1970
1849
|
}
|
1971
1850
|
template <class T>
|
@@ -1975,20 +1854,10 @@ class RINEX_Writer {
|
|
1975
1854
|
return s;
|
1976
1855
|
}
|
1977
1856
|
|
1978
|
-
/**
|
1979
|
-
* @return If all conversion are successfully performed, then true; otherwise false;
|
1980
|
-
*/
|
1981
1857
|
static bool convert(
|
1982
1858
|
const typename RINEX_Reader<U>::convert_item_t *items, const int &size,
|
1983
1859
|
std::string &buf, const void *values){
|
1984
|
-
|
1985
|
-
bool res(true);
|
1986
|
-
for(int i(0); i < size; ++i){
|
1987
|
-
res &= (*items[i].func)(
|
1988
|
-
buf, items[i].offset, items[i].length, (char *)(const_cast<void *>(values)) + items[i].value_offset,
|
1989
|
-
items[i].opt, false);
|
1990
|
-
}
|
1991
|
-
return res;
|
1860
|
+
return TextHelper<>::val2str(items, size, buf, values);
|
1992
1861
|
}
|
1993
1862
|
template <int N>
|
1994
1863
|
static inline bool convert(
|
@@ -1768,30 +1768,38 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1768
1768
|
return (delta_t < 0) || (delta_t > Timing::values[Timing::GEO_NAVIGATION_DATA].interval);
|
1769
1769
|
}
|
1770
1770
|
|
1771
|
-
|
1772
|
-
|
1773
|
-
|
1771
|
+
float_t clock_error(const gps_time_t &t) const {
|
1772
|
+
float_t t_G(-t.interval(WN, 0)); // t_0 is expected to be modified as time of week
|
1773
|
+
return a_Gf0 + a_Gf1 * (t_G - t_0); // Eq.(A-45), delay is positive
|
1774
|
+
}
|
1774
1775
|
|
1775
|
-
|
1776
|
+
float_t clock_error_dot(const gps_time_t &t) const {
|
1777
|
+
return a_Gf1;
|
1778
|
+
}
|
1776
1779
|
|
1777
|
-
|
1778
|
-
|
1780
|
+
constellation_t constellation(
|
1781
|
+
const gps_time_t &t_tx, const float_t &dt_transit = 0,
|
1782
|
+
const bool &with_velocity = true) const {
|
1779
1783
|
|
1780
|
-
float_t delta_t(
|
1784
|
+
float_t delta_t(-t_tx.interval(WN, t_0)), delta_t2(delta_t * delta_t / 2);
|
1781
1785
|
|
1782
1786
|
constellation_t res = {
|
1783
1787
|
xyz_t(
|
1784
1788
|
x + dx * delta_t + ddx * delta_t2,
|
1785
1789
|
y + dy * delta_t + ddy * delta_t2,
|
1786
|
-
z + dz * delta_t + ddz * delta_t2), // Eq. (A-44)
|
1790
|
+
z + dz * delta_t + ddz * delta_t2).after(dt_transit), // Eq. (A-44)
|
1787
1791
|
xyz_t(
|
1788
|
-
|
1789
|
-
|
1790
|
-
|
1792
|
+
dx + ddx * delta_t,
|
1793
|
+
dy + ddy * delta_t,
|
1794
|
+
dz + ddz * delta_t).after(dt_transit),
|
1791
1795
|
};
|
1792
1796
|
|
1793
|
-
|
1794
|
-
|
1797
|
+
/* Sagnac correction, which is expected to perform before geometric distance calculation,
|
1798
|
+
* can be skipped by using non-zero dt_transit.
|
1799
|
+
* This is because the return values is corrected to be along with the coordinate
|
1800
|
+
* at the reception time.
|
1801
|
+
* @see SBAS_SpaceNode::sagnac_correction
|
1802
|
+
*/
|
1795
1803
|
|
1796
1804
|
return res;
|
1797
1805
|
}
|
@@ -114,19 +114,17 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
114
114
|
static inline const typename space_node_t::Satellite &sat(const void *ptr) {
|
115
115
|
return *reinterpret_cast<const typename space_node_t::Satellite *>(ptr);
|
116
116
|
}
|
117
|
-
static xyz_t position(const void *ptr, const gps_time_t &
|
118
|
-
return sat(ptr).ephemeris().constellation(
|
117
|
+
static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
|
118
|
+
return sat(ptr).ephemeris().constellation(t_tx, dt_transit, false).position;
|
119
119
|
}
|
120
|
-
static xyz_t velocity(const void *ptr, const gps_time_t &
|
121
|
-
return sat(ptr).ephemeris().constellation(
|
120
|
+
static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) {
|
121
|
+
return sat(ptr).ephemeris().constellation(t_tx, dt_transit, true).velocity;
|
122
122
|
}
|
123
|
-
static float_t clock_error(const void *ptr, const gps_time_t &
|
124
|
-
|
125
|
-
return 0;
|
123
|
+
static float_t clock_error(const void *ptr, const gps_time_t &t_tx) {
|
124
|
+
return sat(ptr).ephemeris().clock_error(t_tx);
|
126
125
|
}
|
127
|
-
static float_t clock_error_dot(const void *ptr, const gps_time_t &
|
128
|
-
|
129
|
-
return 0;
|
126
|
+
static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) {
|
127
|
+
return sat(ptr).ephemeris().clock_error_dot(t_tx);
|
130
128
|
}
|
131
129
|
};
|
132
130
|
satellite_t res = {
|
@@ -257,20 +255,25 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
257
255
|
|
258
256
|
range -= receiver_error;
|
259
257
|
|
258
|
+
static const float_t &c(GPS_SpaceNode<float_t>::light_speed);
|
259
|
+
|
260
260
|
// Clock error correction
|
261
261
|
range += ((range_error.unknown_flag & range_error_t::SATELLITE_CLOCK)
|
262
|
-
? (sat.clock_error(time_arrival
|
262
|
+
? (sat.clock_error(time_arrival - range / c) * c)
|
263
263
|
: range_error.value[range_error_t::SATELLITE_CLOCK]);
|
264
264
|
|
265
265
|
// TODO WAAS long term clock correction (2.1.1.4.11)
|
266
266
|
|
267
267
|
// Calculate satellite position
|
268
|
-
|
268
|
+
float_t dt_transit(range / c);
|
269
|
+
gps_time_t t_tx(time_arrival - dt_transit);
|
270
|
+
xyz_t sat_pos(sat.position(t_tx, dt_transit));
|
269
271
|
float_t geometric_range(usr_pos.xyz.dist(sat_pos));
|
270
272
|
|
271
|
-
// Calculate residual
|
273
|
+
// Calculate residual without Sagnac correction (A.4.4.11),
|
274
|
+
// because of the satellite position is calculated in the reception time ECEF.
|
272
275
|
res.range_residual = range
|
273
|
-
+ space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
|
276
|
+
// + space_node_t::sagnac_correction(sat_pos, usr_pos.xyz)
|
274
277
|
- geometric_range;
|
275
278
|
|
276
279
|
// Setup design matrix
|
@@ -312,12 +315,12 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
312
315
|
|
313
316
|
res.range_corrected = range;
|
314
317
|
|
315
|
-
xyz_t rel_vel(sat.velocity(
|
318
|
+
xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity
|
316
319
|
|
317
320
|
res.rate_relative_neg = res.los_neg[0] * rel_vel.x()
|
318
321
|
+ res.los_neg[1] * rel_vel.y()
|
319
322
|
+ res.los_neg[2] * rel_vel.z()
|
320
|
-
+ sat.clock_error_dot(
|
323
|
+
+ sat.clock_error_dot(t_tx) * c;
|
321
324
|
|
322
325
|
return res;
|
323
326
|
}
|