gps_pvt 0.2.1 → 0.3.3
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 +33 -5
- data/Rakefile +0 -0
- data/exe/gps_pvt +63 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +784 -745
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +723 -436
- data/ext/ninja-scan-light/tool/navigation/GPS.h +15 -44
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +61 -147
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +56 -2
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +2 -2
- data/ext/ninja-scan-light/tool/navigation/SBAS_Solver.h +55 -78
- data/ext/ninja-scan-light/tool/param/bit_array.h +4 -3
- data/ext/ninja-scan-light/tool/swig/GPS.i +255 -63
- data/ext/ninja-scan-light/tool/swig/SylphideMath.i +91 -21
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +25 -5
- data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb +51 -7
- data/gps_pvt.gemspec +63 -0
- data/lib/gps_pvt/receiver.rb +84 -40
- data/lib/gps_pvt/version.rb +1 -1
- metadata +7 -6
@@ -49,11 +49,7 @@ struct SBAS_SinglePositioning_Options : public GPS_Solver_GeneralOptions<FloatT>
|
|
49
49
|
|
50
50
|
typename GPS_Solver_Base<FloatT>::options_t::template exclude_prn_t<120, 158> exclude_prn; // SBAS PRN ranges from 120 to 158
|
51
51
|
SBAS_SinglePositioning_Options()
|
52
|
-
: super_t() {
|
53
|
-
// default: SBAS IGP, then broadcasted Klobuchar.
|
54
|
-
super_t::ionospheric_models[0] = super_t::IONOSPHERIC_SBAS;
|
55
|
-
super_t::ionospheric_models[1] = super_t::IONOSPHERIC_KLOBUCHAR;
|
56
|
-
|
52
|
+
: super_t(), exclude_prn() {
|
57
53
|
exclude_prn.set(true); // SBAS ranging is default off.
|
58
54
|
}
|
59
55
|
};
|
@@ -86,6 +82,8 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
86
82
|
|
87
83
|
typedef typename base_t::measurement_t measurement_t;
|
88
84
|
typedef typename base_t::range_error_t range_error_t;
|
85
|
+
typedef typename base_t::range_corrector_t range_corrector_t;
|
86
|
+
typedef typename base_t::range_correction_t range_correction_t;
|
89
87
|
|
90
88
|
inheritate_type(relative_property_t);
|
91
89
|
#undef inheritate_type
|
@@ -93,8 +91,6 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
93
91
|
typedef typename GPS_Solver_Base<float_t>::options_t::template merge_t<
|
94
92
|
SBAS_SinglePositioning_Options<float_t>, base_t> options_t;
|
95
93
|
|
96
|
-
typedef GPS_SpaceNode<float_t> gps_space_node_t;
|
97
|
-
const gps_space_node_t *space_node_gps; ///< optional
|
98
94
|
protected:
|
99
95
|
const space_node_t &_space_node;
|
100
96
|
SBAS_SinglePositioning_Options<float_t> _options;
|
@@ -102,40 +98,47 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
102
98
|
public:
|
103
99
|
const space_node_t &space_node() const {return _space_node;}
|
104
100
|
|
105
|
-
|
106
|
-
|
107
|
-
|
108
|
-
|
109
|
-
|
110
|
-
|
111
|
-
|
112
|
-
|
113
|
-
|
114
|
-
|
115
|
-
|
116
|
-
|
117
|
-
|
118
|
-
|
119
|
-
|
120
|
-
|
121
|
-
|
122
|
-
|
123
|
-
|
124
|
-
|
125
|
-
|
126
|
-
|
127
|
-
break;
|
128
|
-
default:
|
129
|
-
break;
|
130
|
-
}
|
131
|
-
if(usable){
|
132
|
-
available_models++;
|
133
|
-
}else{
|
134
|
-
opt.ionospheric_models[i] = options_t::IONOSPHERIC_SKIP;
|
101
|
+
struct ionospheric_sbas_t : public range_corrector_t {
|
102
|
+
const space_node_t &space_node;
|
103
|
+
ionospheric_sbas_t(const space_node_t &sn) : range_corrector_t(), space_node(sn) {}
|
104
|
+
bool is_available(const gps_time_t &t) const {
|
105
|
+
return true;
|
106
|
+
}
|
107
|
+
float_t *calculate(
|
108
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
109
|
+
float_t &buf) const {
|
110
|
+
|
111
|
+
// placeholder of checking availability and performing correction
|
112
|
+
typedef typename space_node_t::available_satellites_t sats_t;
|
113
|
+
sats_t sats(space_node.available_satellites(usr_pos.llh.longitude()));
|
114
|
+
|
115
|
+
typename space_node_t::IonosphericGridPoints::PointProperty prop;
|
116
|
+
for(typename sats_t::const_iterator it(sats.begin()); it != sats.end(); ++it){
|
117
|
+
prop = it->second
|
118
|
+
->ionospheric_grid_points().iono_correction(sat_rel_pos, usr_pos.llh);
|
119
|
+
if(prop.is_available()){
|
120
|
+
return &(buf = prop.delay);
|
121
|
+
}
|
122
|
+
break; // TODO The nearest satellite is only checked
|
135
123
|
}
|
124
|
+
return NULL;
|
136
125
|
}
|
137
|
-
|
138
|
-
|
126
|
+
} ionospheric_sbas;
|
127
|
+
|
128
|
+
struct tropospheric_sbas_t : public range_corrector_t {
|
129
|
+
tropospheric_sbas_t() : range_corrector_t() {}
|
130
|
+
bool is_available(const gps_time_t &t) const {
|
131
|
+
return true;
|
132
|
+
}
|
133
|
+
float_t *calculate(
|
134
|
+
const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
|
135
|
+
float_t &buf) const {
|
136
|
+
return &(buf = space_node_t::tropo_correction(
|
137
|
+
t.year(), sat_rel_pos, usr_pos.llh));
|
138
|
+
}
|
139
|
+
} tropospheric_sbas;
|
140
|
+
|
141
|
+
range_correction_t ionospheric_correction, tropospheric_correction;
|
139
142
|
|
140
143
|
options_t available_options() const {
|
141
144
|
return options_t(base_t::available_options(), _options);
|
@@ -143,19 +146,24 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
143
146
|
|
144
147
|
options_t available_options(const options_t &opt_wish) const {
|
145
148
|
SBAS_SinglePositioning_Options<float_t> opt(opt_wish);
|
146
|
-
filter_ionospheric_models(opt);
|
147
149
|
return options_t(base_t::available_options(opt_wish), opt);
|
148
150
|
}
|
149
151
|
|
150
152
|
options_t update_options(const options_t &opt_wish){
|
151
|
-
|
153
|
+
_options = opt_wish;
|
152
154
|
return options_t(base_t::update_options(opt_wish), _options);
|
153
155
|
}
|
154
156
|
|
155
|
-
SBAS_SinglePositioning(const space_node_t &sn
|
156
|
-
: base_t(),
|
157
|
-
|
158
|
-
|
157
|
+
SBAS_SinglePositioning(const space_node_t &sn)
|
158
|
+
: base_t(), _space_node(sn), _options(available_options(options_t())),
|
159
|
+
ionospheric_sbas(sn), tropospheric_sbas() {
|
160
|
+
|
161
|
+
// default ionospheric correction: Broadcasted IGP.
|
162
|
+
ionospheric_correction.push_front(&ionospheric_sbas);
|
163
|
+
|
164
|
+
// default troposheric correction: SBAS
|
165
|
+
tropospheric_correction.push_front(&tropospheric_sbas);
|
166
|
+
}
|
159
167
|
|
160
168
|
~SBAS_SinglePositioning(){}
|
161
169
|
|
@@ -245,43 +253,12 @@ class SBAS_SinglePositioning : public SolverBaseT {
|
|
245
253
|
|
246
254
|
// Tropospheric (2.1.4.10.3, A.4.2.4)
|
247
255
|
res.range_residual += (range_error.unknown_flag & range_error_t::MASK_TROPOSPHERIC)
|
248
|
-
?
|
256
|
+
? tropospheric_correction(time_arrival, usr_pos, relative_pos)
|
249
257
|
: range_error.value[range_error_t::TROPOSPHERIC];
|
250
258
|
|
251
259
|
// Ionospheric (2.1.4.10.2, A.4.4.10.4)
|
252
260
|
if(range_error.unknown_flag & range_error_t::MASK_IONOSPHERIC){
|
253
|
-
|
254
|
-
bool iono_model_hit(false);
|
255
|
-
for(std::size_t i(0); i < sizeof(_options.ionospheric_models) / sizeof(_options.ionospheric_models[0]); ++i){
|
256
|
-
switch(_options.ionospheric_models[i]){
|
257
|
-
case options_t::IONOSPHERIC_KLOBUCHAR: // 20.3.3.5.2.5
|
258
|
-
res.range_residual += space_node_gps->iono_correction(relative_pos, usr_pos.llh, time_arrival);
|
259
|
-
break;
|
260
|
-
case options_t::IONOSPHERIC_SBAS: { // 2.1.4.10.2, A.4.4.10.4
|
261
|
-
typename space_node_t::IonosphericGridPoints::PointProperty prop(
|
262
|
-
sat->ionospheric_grid_points().iono_correction(relative_pos, usr_pos.llh));
|
263
|
-
if(!prop.is_available()){continue;}
|
264
|
-
res.range_residual += prop.delay;
|
265
|
-
break;
|
266
|
-
}
|
267
|
-
case options_t::IONOSPHERIC_NTCM_GL: {
|
268
|
-
// TODO f_10_7 setup, optimization (mag_model etc.)
|
269
|
-
typename gps_space_node_t::pierce_point_res_t pp(gps_space_node_t::pierce_point(relative_pos, usr_pos.llh));
|
270
|
-
res.range_residual -= gps_space_node_t::tec2delay(
|
271
|
-
gps_space_node_t::slant_factor(relative_pos)
|
272
|
-
* NTCM_GL_Generic<float_t>::tec_vert(
|
273
|
-
pp.latitude, pp.longitude,
|
274
|
-
time_arrival.year(), _options.f_10_7));
|
275
|
-
break;
|
276
|
-
}
|
277
|
-
case options_t::IONOSPHERIC_NONE:
|
278
|
-
break;
|
279
|
-
default:
|
280
|
-
continue;
|
281
|
-
}
|
282
|
-
iono_model_hit = true;
|
283
|
-
break;
|
284
|
-
}
|
261
|
+
res.range_residual += ionospheric_correction(time_arrival, usr_pos, relative_pos);
|
285
262
|
}else{
|
286
263
|
res.range_residual += range_error.value[range_error_t::IONOSPHERIC];
|
287
264
|
}
|
@@ -60,6 +60,7 @@ struct const_div_t<denom, denom, log2, 0> {
|
|
60
60
|
template <int MAX_SIZE, class ContainerT = unsigned char>
|
61
61
|
struct BitArray {
|
62
62
|
static const int bits_per_addr = (int)sizeof(ContainerT) * CHAR_BIT;
|
63
|
+
static const int max_size = MAX_SIZE;
|
63
64
|
ContainerT buf[(MAX_SIZE + bits_per_addr - 1) / bits_per_addr];
|
64
65
|
void set(const bool &new_bit = false) {
|
65
66
|
std::memset(buf, (new_bit ? (~0) : 0), sizeof(buf));
|
@@ -128,14 +129,14 @@ struct BitArray {
|
|
128
129
|
std::vector<int> res;
|
129
130
|
int idx(0);
|
130
131
|
static const const_div_t<bits_per_addr> qr(MAX_SIZE);
|
131
|
-
int rem(qr.rem);
|
132
|
-
for(
|
132
|
+
int rem(qr.rem), i(0);
|
133
|
+
for(; i < qr.quot; ++i, idx += bits_per_addr){
|
133
134
|
int idx2(idx);
|
134
135
|
for(ContainerT temp(buf[i]); temp > 0; temp >>= 1, ++idx2){
|
135
136
|
if(temp & 0x1){res.push_back(idx2);}
|
136
137
|
}
|
137
138
|
}
|
138
|
-
for(ContainerT temp(buf[
|
139
|
+
for(ContainerT temp(buf[i]); (temp > 0) && (rem > 0); --rem, ++idx, temp >>= 1){
|
139
140
|
if(temp & 0x1){res.push_back(idx);}
|
140
141
|
}
|
141
142
|
return res;
|
@@ -153,12 +153,26 @@ static std::string inspect_str(const VALUE &v){
|
|
153
153
|
$1 = (TYPE($input) == T_ARRAY) ? 1 : 0;
|
154
154
|
}
|
155
155
|
#endif
|
156
|
+
%ignore canonicalize();
|
157
|
+
%ignore GPS_Time(const int &_week, const float_t &_seconds);
|
158
|
+
%typemap(in, numinputs=0) void *dummy "";
|
159
|
+
GPS_Time(const int &week_, const float_t &seconds_, void *dummy){
|
160
|
+
return &((new GPS_Time<FloatT>(week_, seconds_))->canonicalize());
|
161
|
+
}
|
156
162
|
%apply int *OUTPUT { int *week };
|
157
163
|
%apply FloatT *OUTPUT { FloatT *seconds };
|
158
164
|
void to_a(int *week, FloatT *seconds) const {
|
159
165
|
*week = self->week;
|
160
166
|
*seconds = self->seconds;
|
161
167
|
}
|
168
|
+
#if defined(SWIG)
|
169
|
+
int __cmp__(const GPS_Time<FloatT> &t) const {
|
170
|
+
return ((self->week < t.week) ? -1
|
171
|
+
: ((self->week > t.week) ? 1
|
172
|
+
: (self->seconds < t.seconds ? -1
|
173
|
+
: (self->seconds > t.seconds ? 1 : 0))));
|
174
|
+
}
|
175
|
+
#endif
|
162
176
|
}
|
163
177
|
|
164
178
|
%define MAKE_ACCESSOR(name, type)
|
@@ -413,8 +427,6 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
413
427
|
%append_output(swig::from($1.latitude));
|
414
428
|
%append_output(swig::from($1.longitude));
|
415
429
|
}
|
416
|
-
%ignore iono_correction() const;
|
417
|
-
%ignore tropo_correction() const;
|
418
430
|
int read(const char *fname) {
|
419
431
|
std::fstream fin(fname, std::ios::in | std::ios::binary);
|
420
432
|
typename RINEX_NAV_Reader<FloatT>::space_node_list_t space_nodes = {self};
|
@@ -800,6 +812,7 @@ struct GPS_Measurement {
|
|
800
812
|
L1_RANGE_RATE_SIGMA,
|
801
813
|
L1_SIGNAL_STRENGTH_dBHz,
|
802
814
|
L1_LOCK_SEC,
|
815
|
+
L1_FREQUENCY,
|
803
816
|
ITEMS_PREDEFINED,
|
804
817
|
};
|
805
818
|
void add(const int &prn, const int &key, const FloatT &value){
|
@@ -821,71 +834,15 @@ const type &get_ ## name () const {
|
|
821
834
|
%enddef
|
822
835
|
MAKE_ACCESSOR2(elevation_mask, FloatT);
|
823
836
|
MAKE_ACCESSOR2(residual_mask, FloatT);
|
824
|
-
MAKE_ACCESSOR2(f_10_7, FloatT);
|
825
837
|
#undef MAKE_ACCESSOR2
|
826
838
|
MAKE_VECTOR2ARRAY(int);
|
827
839
|
%ignore cast_base;
|
828
|
-
#ifdef SWIGRUBY
|
829
|
-
%rename("ionospheric_models=") set_ionospheric_models;
|
830
|
-
%rename("ionospheric_models") get_ionospheric_models;
|
831
|
-
#endif
|
832
|
-
%typemap(in) const std::vector<int> &models (std::vector<int> temp) {
|
833
|
-
$1 = &temp;
|
834
|
-
#ifdef SWIGRUBY
|
835
|
-
if(RB_TYPE_P($input, T_ARRAY)){
|
836
|
-
for(int i(0), i_max(RARRAY_LEN($input)); i < i_max; ++i){
|
837
|
-
SWIG_Object obj(RARRAY_AREF($input, i));
|
838
|
-
int v;
|
839
|
-
if(SWIG_IsOK(SWIG_AsVal(int)(obj, &v))){
|
840
|
-
temp.push_back(v);
|
841
|
-
}else{
|
842
|
-
SWIG_exception(SWIG_TypeError, "$*1_ltype is expected");
|
843
|
-
}
|
844
|
-
}
|
845
|
-
}
|
846
|
-
#endif
|
847
|
-
}
|
848
840
|
}
|
849
841
|
%inline %{
|
850
842
|
template <class FloatT>
|
851
843
|
struct GPS_SolverOptions_Common {
|
852
|
-
enum {
|
853
|
-
IONOSPHERIC_KLOBUCHAR,
|
854
|
-
IONOSPHERIC_SBAS,
|
855
|
-
IONOSPHERIC_NTCM_GL,
|
856
|
-
IONOSPHERIC_NONE, // which allows no correction
|
857
|
-
IONOSPHERIC_MODELS,
|
858
|
-
IONOSPHERIC_SKIP = IONOSPHERIC_MODELS, // which means delegating the next slot
|
859
|
-
};
|
860
844
|
virtual GPS_Solver_GeneralOptions<FloatT> *cast_general() = 0;
|
861
845
|
virtual const GPS_Solver_GeneralOptions<FloatT> *cast_general() const = 0;
|
862
|
-
std::vector<int> get_ionospheric_models() const {
|
863
|
-
typedef GPS_Solver_GeneralOptions<FloatT> general_t;
|
864
|
-
const general_t *general(this->cast_general());
|
865
|
-
std::vector<int> res;
|
866
|
-
for(int i(0); i < general_t::IONOSPHERIC_MODELS; ++i){
|
867
|
-
int v((int)(general->ionospheric_models[i]));
|
868
|
-
if(v == general_t::IONOSPHERIC_SKIP){break;}
|
869
|
-
res.push_back(v);
|
870
|
-
}
|
871
|
-
return res;
|
872
|
-
}
|
873
|
-
std::vector<int> set_ionospheric_models(const std::vector<int> &models){
|
874
|
-
typedef GPS_Solver_GeneralOptions<FloatT> general_t;
|
875
|
-
general_t *general(this->cast_general());
|
876
|
-
typedef typename general_t::ionospheric_model_t model_t;
|
877
|
-
for(int i(0), j(0), j_max(models.size()); i < general_t::IONOSPHERIC_MODELS; ++i){
|
878
|
-
model_t v(general_t::IONOSPHERIC_SKIP);
|
879
|
-
if(j < j_max){
|
880
|
-
if((models[j] >= 0) && (models[j] < general_t::IONOSPHERIC_SKIP)){
|
881
|
-
v = (model_t)models[j];
|
882
|
-
}
|
883
|
-
++j;
|
884
|
-
}
|
885
|
-
general->ionospheric_models[i] = v;
|
886
|
-
}
|
887
|
-
return get_ionospheric_models();
|
888
|
-
}
|
889
846
|
};
|
890
847
|
%}
|
891
848
|
|
@@ -927,6 +884,27 @@ struct SBAS_SolverOptions
|
|
927
884
|
};
|
928
885
|
%}
|
929
886
|
|
887
|
+
%header {
|
888
|
+
template <class FloatT>
|
889
|
+
struct GPS_RangeCorrector
|
890
|
+
: public GPS_Solver_Base<FloatT>::range_corrector_t {
|
891
|
+
SWIG_Object callback;
|
892
|
+
GPS_RangeCorrector(const SWIG_Object &callback_)
|
893
|
+
: GPS_Solver_Base<FloatT>::range_corrector_t(),
|
894
|
+
callback(callback_) {}
|
895
|
+
bool is_available(const typename GPS_Solver_Base<FloatT>::gps_time_t &t) const {
|
896
|
+
return false;
|
897
|
+
}
|
898
|
+
FloatT *calculate(
|
899
|
+
const typename GPS_Solver_Base<FloatT>::gps_time_t &t,
|
900
|
+
const typename GPS_Solver_Base<FloatT>::pos_t &usr_pos,
|
901
|
+
const typename GPS_Solver_Base<FloatT>::enu_t &sat_rel_pos,
|
902
|
+
FloatT &buf) const {
|
903
|
+
return NULL;
|
904
|
+
}
|
905
|
+
};
|
906
|
+
}
|
907
|
+
|
930
908
|
%extend GPS_Solver {
|
931
909
|
%ignore super_t;
|
932
910
|
%ignore base_t;
|
@@ -938,6 +916,8 @@ struct SBAS_SolverOptions
|
|
938
916
|
%ignore relative_property;
|
939
917
|
%ignore satellite_position;
|
940
918
|
%ignore update_position_solution;
|
919
|
+
%ignore user_correctors_t;
|
920
|
+
%ignore user_correctors;
|
941
921
|
%immutable hooks;
|
942
922
|
%ignore mark;
|
943
923
|
%fragment("hook"{GPS_Solver<FloatT>}, "header",
|
@@ -1022,7 +1002,9 @@ struct SBAS_SolverOptions
|
|
1022
1002
|
SWIG_NewPointerObj(&geomat_.W,
|
1023
1003
|
$descriptor(Matrix<FloatT, Array2D_Dense<FloatT>, MatrixViewBase<> > *), 0),
|
1024
1004
|
SWIG_NewPointerObj(&geomat_.delta_r,
|
1025
|
-
$descriptor(Matrix<FloatT, Array2D_Dense<FloatT>, MatrixViewBase<> > *), 0)
|
1005
|
+
$descriptor(Matrix<FloatT, Array2D_Dense<FloatT>, MatrixViewBase<> > *), 0),
|
1006
|
+
SWIG_NewPointerObj(&res,
|
1007
|
+
$descriptor(GPS_User_PVT<FloatT> *), 0)};
|
1026
1008
|
proc_call_throw_if_error(hook, sizeof(values) / sizeof(values[0]), values);
|
1027
1009
|
}while(false);
|
1028
1010
|
#endif
|
@@ -1075,6 +1057,152 @@ struct SBAS_SolverOptions
|
|
1075
1057
|
}
|
1076
1058
|
}
|
1077
1059
|
%fragment("hook"{GPS_Solver<FloatT>});
|
1060
|
+
%ignore update_correction;
|
1061
|
+
#ifdef SWIGRUBY
|
1062
|
+
%fragment("correction"{GPS_Solver<FloatT>}, "header",
|
1063
|
+
fragment=SWIG_From_frag(int),
|
1064
|
+
fragment=SWIG_Traits_frag(FloatT)){
|
1065
|
+
template <>
|
1066
|
+
bool GPS_RangeCorrector<FloatT>::is_available(
|
1067
|
+
const typename GPS_Solver_Base<FloatT>::gps_time_t &t) const {
|
1068
|
+
VALUE values[] = {
|
1069
|
+
SWIG_NewPointerObj(
|
1070
|
+
%const_cast(&t, GPS_Time<FloatT> *), $descriptor(GPS_Time<FloatT> *), 0),
|
1071
|
+
};
|
1072
|
+
VALUE res(proc_call_throw_if_error(
|
1073
|
+
callback, sizeof(values) / sizeof(values[0]), values));
|
1074
|
+
return RTEST(res) ? true : false;
|
1075
|
+
}
|
1076
|
+
template <>
|
1077
|
+
FloatT *GPS_RangeCorrector<FloatT>::calculate(
|
1078
|
+
const typename GPS_Solver_Base<FloatT>::gps_time_t &t,
|
1079
|
+
const typename GPS_Solver_Base<FloatT>::pos_t &usr_pos,
|
1080
|
+
const typename GPS_Solver_Base<FloatT>::enu_t &sat_rel_pos,
|
1081
|
+
FloatT &buf) const {
|
1082
|
+
VALUE values[] = {
|
1083
|
+
SWIG_NewPointerObj(
|
1084
|
+
%const_cast(&t, GPS_Time<FloatT> *),
|
1085
|
+
$descriptor(GPS_Time<FloatT> *), 0),
|
1086
|
+
SWIG_NewPointerObj(
|
1087
|
+
(%const_cast(&usr_pos.xyz, System_XYZ<FloatT, WGS84> *)),
|
1088
|
+
$descriptor(System_XYZ<FloatT, WGS84> *), 0),
|
1089
|
+
SWIG_NewPointerObj(
|
1090
|
+
(%const_cast(&sat_rel_pos, System_ENU<FloatT, WGS84> *)),
|
1091
|
+
$descriptor(System_ENU<FloatT, WGS84> *), 0),
|
1092
|
+
};
|
1093
|
+
VALUE res(proc_call_throw_if_error(
|
1094
|
+
callback, sizeof(values) / sizeof(values[0]), values));
|
1095
|
+
return SWIG_IsOK(swig::asval(res, &buf)) ? &buf : NULL;
|
1096
|
+
}
|
1097
|
+
template<>
|
1098
|
+
VALUE GPS_Solver<FloatT>::update_correction(
|
1099
|
+
const bool &update, const VALUE &hash){
|
1100
|
+
typedef range_correction_list_t list_t;
|
1101
|
+
static const VALUE k_root[] = {
|
1102
|
+
ID2SYM(rb_intern("gps_ionospheric")),
|
1103
|
+
ID2SYM(rb_intern("gps_tropospheric")),
|
1104
|
+
ID2SYM(rb_intern("sbas_ionospheric")),
|
1105
|
+
ID2SYM(rb_intern("sbas_tropospheric")),
|
1106
|
+
};
|
1107
|
+
static const VALUE k_opt(ID2SYM(rb_intern("options")));
|
1108
|
+
static const VALUE k_f_10_7(ID2SYM(rb_intern("f_10_7")));
|
1109
|
+
static const VALUE k_known(ID2SYM(rb_intern("known")));
|
1110
|
+
struct {
|
1111
|
+
VALUE sym;
|
1112
|
+
list_t::mapped_type::value_type obj;
|
1113
|
+
} item[] = {
|
1114
|
+
{ID2SYM(rb_intern("no_correction")), &base_t::no_correction},
|
1115
|
+
{ID2SYM(rb_intern("klobuchar")), &this->gps.solver.ionospheric_klobuchar},
|
1116
|
+
{ID2SYM(rb_intern("ntcm_gl")), &this->gps.solver.ionospheric_ntcm_gl},
|
1117
|
+
{ID2SYM(rb_intern("hopfield")), &this->gps.solver.tropospheric_simplified},
|
1118
|
+
{ID2SYM(rb_intern("sbas_igp")), &this->sbas.solver.ionospheric_sbas},
|
1119
|
+
{ID2SYM(rb_intern("sbas_tropo")), &this->sbas.solver.tropospheric_sbas},
|
1120
|
+
};
|
1121
|
+
list_t input;
|
1122
|
+
if(update){
|
1123
|
+
if(!RB_TYPE_P(hash, T_HASH)){
|
1124
|
+
throw std::runtime_error(
|
1125
|
+
std::string("Hash is expected, however ").append(inspect_str(hash)));
|
1126
|
+
}
|
1127
|
+
for(std::size_t i(0); i < sizeof(k_root) / sizeof(k_root[0]); ++i){
|
1128
|
+
VALUE ary = rb_hash_lookup(hash, k_root[i]);
|
1129
|
+
if(NIL_P(ary)){continue;}
|
1130
|
+
if(!RB_TYPE_P(ary, T_ARRAY)){
|
1131
|
+
ary = rb_ary_new_from_values(1, &ary);
|
1132
|
+
}
|
1133
|
+
for(int j(0); j < RARRAY_LEN(ary); ++j){
|
1134
|
+
std::size_t k(0);
|
1135
|
+
VALUE v(rb_ary_entry(ary, j));
|
1136
|
+
for(; k < sizeof(item) / sizeof(item[0]); ++k){
|
1137
|
+
if(v == item[k].sym){break;}
|
1138
|
+
}
|
1139
|
+
if(k >= sizeof(item) / sizeof(item[0])){ // other than symbol
|
1140
|
+
user_correctors.push_back(GPS_RangeCorrector<FloatT>(v));
|
1141
|
+
input[i].push_back(&user_correctors.back());
|
1142
|
+
}else{
|
1143
|
+
input[i].push_back(item[k].obj);
|
1144
|
+
}
|
1145
|
+
}
|
1146
|
+
}
|
1147
|
+
VALUE opt(rb_hash_lookup(hash, k_opt));
|
1148
|
+
if(RB_TYPE_P(opt, T_HASH)){
|
1149
|
+
swig::asval(rb_hash_lookup(opt, k_f_10_7), // ntcm_gl
|
1150
|
+
&this->gps.solver.ionospheric_ntcm_gl.f_10_7);
|
1151
|
+
}
|
1152
|
+
}
|
1153
|
+
list_t output(update_correction(update, input));
|
1154
|
+
VALUE res = rb_hash_new();
|
1155
|
+
for(list_t::const_iterator it(output.begin()), it_end(output.end());
|
1156
|
+
it != it_end; ++it){
|
1157
|
+
VALUE k;
|
1158
|
+
if((it->first < 0) || (it->first >= (int)(sizeof(k_root) / sizeof(k_root[0])))){
|
1159
|
+
k = SWIG_From(int)(it->first);
|
1160
|
+
}else{
|
1161
|
+
k = k_root[it->first];
|
1162
|
+
}
|
1163
|
+
VALUE v = rb_ary_new();
|
1164
|
+
for(list_t::mapped_type::const_iterator
|
1165
|
+
it2(it->second.begin()), it2_end(it->second.end());
|
1166
|
+
it2 != it2_end; ++it2){
|
1167
|
+
std::size_t i(0);
|
1168
|
+
for(; i < sizeof(item) / sizeof(item[0]); ++i){
|
1169
|
+
if(*it2 == item[i].obj){break;}
|
1170
|
+
}
|
1171
|
+
if(i >= sizeof(item) / sizeof(item[0])){ // other than built-in corrector
|
1172
|
+
rb_ary_push(v,
|
1173
|
+
reinterpret_cast<const GPS_RangeCorrector<FloatT> *>(*it2)->callback);
|
1174
|
+
}else{
|
1175
|
+
rb_ary_push(v, item[i].sym);
|
1176
|
+
}
|
1177
|
+
}
|
1178
|
+
rb_hash_aset(res, k, v);
|
1179
|
+
}
|
1180
|
+
{ // common options
|
1181
|
+
VALUE opt = rb_hash_new();
|
1182
|
+
rb_hash_aset(res, k_opt, opt);
|
1183
|
+
rb_hash_aset(opt, k_f_10_7, // ntcm_gl
|
1184
|
+
swig::from(this->gps.solver.ionospheric_ntcm_gl.f_10_7));
|
1185
|
+
}
|
1186
|
+
{ // known models
|
1187
|
+
VALUE ary = rb_ary_new_capa((int)(sizeof(item) / sizeof(item[0])));
|
1188
|
+
for(std::size_t i(0); i < sizeof(item) / sizeof(item[0]); ++i){
|
1189
|
+
rb_ary_push(ary, item[i].sym);
|
1190
|
+
}
|
1191
|
+
rb_hash_aset(res, k_known, ary);
|
1192
|
+
}
|
1193
|
+
return res;
|
1194
|
+
}
|
1195
|
+
}
|
1196
|
+
%fragment("correction"{GPS_Solver<FloatT>});
|
1197
|
+
%rename("correction") get_correction;
|
1198
|
+
%rename("correction=") set_correction;
|
1199
|
+
VALUE get_correction() const {
|
1200
|
+
return const_cast<GPS_Solver<FloatT> *>(self)->update_correction(false, Qnil);
|
1201
|
+
}
|
1202
|
+
VALUE set_correction(VALUE hash){
|
1203
|
+
return self->update_correction(true, hash);
|
1204
|
+
}
|
1205
|
+
#endif
|
1078
1206
|
}
|
1079
1207
|
%inline {
|
1080
1208
|
template <class FloatT>
|
@@ -1097,19 +1225,36 @@ struct GPS_Solver
|
|
1097
1225
|
sbas_t() : space_node(), options(), solver(space_node) {}
|
1098
1226
|
} sbas;
|
1099
1227
|
SWIG_Object hooks;
|
1228
|
+
typedef std::vector<GPS_RangeCorrector<FloatT> > user_correctors_t;
|
1229
|
+
user_correctors_t user_correctors;
|
1100
1230
|
#ifdef SWIGRUBY
|
1101
1231
|
static void mark(void *ptr){
|
1102
1232
|
GPS_Solver<FloatT> *solver = (GPS_Solver<FloatT> *)ptr;
|
1103
|
-
if(solver->hooks == Qnil){return;}
|
1104
1233
|
rb_gc_mark(solver->hooks);
|
1234
|
+
for(typename user_correctors_t::const_iterator
|
1235
|
+
it(solver->user_correctors.begin()), it_end(solver->user_correctors.end());
|
1236
|
+
it != it_end; ++it){
|
1237
|
+
rb_gc_mark(it->callback);
|
1238
|
+
}
|
1105
1239
|
}
|
1106
1240
|
#endif
|
1107
|
-
GPS_Solver() : super_t(),
|
1108
|
-
|
1109
|
-
|
1241
|
+
GPS_Solver() : super_t(),
|
1242
|
+
gps(), sbas(),
|
1243
|
+
hooks(), user_correctors() {
|
1110
1244
|
#ifdef SWIGRUBY
|
1111
1245
|
hooks = rb_hash_new();
|
1112
1246
|
#endif
|
1247
|
+
typename base_t::range_correction_t ionospheric, tropospheric;
|
1248
|
+
ionospheric.push_back(&sbas.solver.ionospheric_sbas);
|
1249
|
+
ionospheric.push_back(&gps.solver.ionospheric_klobuchar);
|
1250
|
+
tropospheric.push_back(&sbas.solver.tropospheric_sbas);
|
1251
|
+
tropospheric.push_back(&gps.solver.tropospheric_simplified);
|
1252
|
+
gps.solver.ionospheric_correction
|
1253
|
+
= sbas.solver.ionospheric_correction
|
1254
|
+
= ionospheric;
|
1255
|
+
gps.solver.tropospheric_correction
|
1256
|
+
= sbas.solver.tropospheric_correction
|
1257
|
+
= tropospheric;
|
1113
1258
|
}
|
1114
1259
|
GPS_SpaceNode<FloatT> &gps_space_node() {return gps.space_node;}
|
1115
1260
|
GPS_SolverOptions<FloatT> &gps_options() {return gps.options;}
|
@@ -1149,6 +1294,53 @@ struct GPS_Solver
|
|
1149
1294
|
const_cast<sbas_t &>(sbas).solver.update_options(sbas.options);
|
1150
1295
|
return super_t::solve().user_pvt(measurement.items, receiver_time);
|
1151
1296
|
}
|
1297
|
+
typedef
|
1298
|
+
std::map<int, std::vector<const typename base_t::range_corrector_t *> >
|
1299
|
+
range_correction_list_t;
|
1300
|
+
range_correction_list_t update_correction(
|
1301
|
+
const bool &update,
|
1302
|
+
const range_correction_list_t &list = range_correction_list_t()){
|
1303
|
+
range_correction_list_t res;
|
1304
|
+
typename base_t::range_correction_t *root[] = {
|
1305
|
+
&gps.solver.ionospheric_correction,
|
1306
|
+
&gps.solver.tropospheric_correction,
|
1307
|
+
&sbas.solver.ionospheric_correction,
|
1308
|
+
&sbas.solver.tropospheric_correction,
|
1309
|
+
};
|
1310
|
+
for(std::size_t i(0); i < sizeof(root) / sizeof(root[0]); ++i){
|
1311
|
+
do{
|
1312
|
+
if(!update){break;}
|
1313
|
+
typename range_correction_list_t::const_iterator it(list.find(i));
|
1314
|
+
if(it == list.end()){break;}
|
1315
|
+
|
1316
|
+
// Remove user defined unused correctors
|
1317
|
+
for(typename base_t::range_correction_t::const_iterator
|
1318
|
+
it2(root[i]->begin()), it2_end(root[i]->end());
|
1319
|
+
it2 != it2_end; ++it2){
|
1320
|
+
for(typename user_correctors_t::const_iterator
|
1321
|
+
it3(user_correctors.begin()), it3_end(user_correctors.end());
|
1322
|
+
it3 != it3_end; ++it3){
|
1323
|
+
if(*it2 != &(*it3)){continue;}
|
1324
|
+
user_correctors.erase(it3);
|
1325
|
+
}
|
1326
|
+
}
|
1327
|
+
|
1328
|
+
root[i]->clear();
|
1329
|
+
for(typename range_correction_list_t::mapped_type::const_iterator
|
1330
|
+
it2(it->second.begin()), it2_end(it->second.end());
|
1331
|
+
it2 != it2_end; ++it2){
|
1332
|
+
root[i]->push_back(*it2);
|
1333
|
+
}
|
1334
|
+
}while(false);
|
1335
|
+
for(typename base_t::range_correction_t::const_iterator
|
1336
|
+
it(root[i]->begin()), it_end(root[i]->end());
|
1337
|
+
it != it_end; ++it){
|
1338
|
+
res[i].push_back(*it);
|
1339
|
+
}
|
1340
|
+
}
|
1341
|
+
return res;
|
1342
|
+
}
|
1343
|
+
SWIG_Object update_correction(const bool &update, const SWIG_Object &hash);
|
1152
1344
|
};
|
1153
1345
|
}
|
1154
1346
|
|