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
@@ -51,6 +51,7 @@
|
|
51
51
|
#endif
|
52
52
|
|
53
53
|
#include "param/vector3.h"
|
54
|
+
#include "param/quaternion.h"
|
54
55
|
#include "param/matrix.h"
|
55
56
|
|
56
57
|
#include "WGS84.h"
|
@@ -130,6 +131,73 @@ class System_3D {
|
|
130
131
|
in >> self[2];
|
131
132
|
return in;
|
132
133
|
}
|
134
|
+
|
135
|
+
|
136
|
+
///< Coordinate rotation
|
137
|
+
struct Rotation {
|
138
|
+
Quaternion<FloatT> q;
|
139
|
+
Rotation() : q(1, 0, 0, 0) {}
|
140
|
+
Rotation &then(const Quaternion<FloatT> &q_) {
|
141
|
+
q *= q_;
|
142
|
+
return *this;
|
143
|
+
}
|
144
|
+
Rotation &then(const Rotation &rot) {
|
145
|
+
return then(rot.q);
|
146
|
+
}
|
147
|
+
Rotation &then_x(const FloatT &rad) {
|
148
|
+
return then(Quaternion<FloatT>(
|
149
|
+
std::cos(rad / 2), std::sin(rad / 2), 0, 0));
|
150
|
+
}
|
151
|
+
Rotation &then_y(const FloatT &rad) {
|
152
|
+
return then(Quaternion<FloatT>(
|
153
|
+
std::cos(rad / 2), 0, std::sin(rad / 2), 0));
|
154
|
+
}
|
155
|
+
Rotation &then_z(const FloatT &rad) {
|
156
|
+
return then(Quaternion<FloatT>(
|
157
|
+
std::cos(rad / 2), 0, 0, std::sin(rad / 2)));
|
158
|
+
}
|
159
|
+
operator Matrix<FloatT>() const {
|
160
|
+
return q.getDCM();
|
161
|
+
}
|
162
|
+
/**
|
163
|
+
* Perform coordinate rotation
|
164
|
+
* For example; application of clockwise 90 degree coordinate rotation along to z axis
|
165
|
+
* to vector(1,0,0) yields to vector(0,-1,0).
|
166
|
+
* This is not identical to vector rotation along with some axis,
|
167
|
+
* which corresponds to vector(0,1,0) of example, i.e.,
|
168
|
+
* z-axis 90 degree rotation of vector(1,0,0).
|
169
|
+
*
|
170
|
+
* @param vec target vector to which rotation is applied.
|
171
|
+
*/
|
172
|
+
void apply(FloatT (&vec)[3]) const {
|
173
|
+
#if 0
|
174
|
+
// as definition: (~q * vec * q).vector();
|
175
|
+
Vector3<FloatT> v(((q.conj() *= Vector3<FloatT>(vec.v)) *= q).vector());
|
176
|
+
vec[0] = v[0];
|
177
|
+
vec[1] = v[1];
|
178
|
+
vec[2] = v[2];
|
179
|
+
#else // optimized version
|
180
|
+
FloatT q0(q[0]), qv[3] = {q[1], q[2], q[3]};
|
181
|
+
FloatT q0_(qv[0] * vec[0] + qv[1] * vec[1] + qv[2] * vec[2]), // -q~.vec (i*) vec
|
182
|
+
qv_[3] = { // q~[0] * vec + q~.vec (e*) vec
|
183
|
+
q0 * vec[0] - qv[1] * vec[2] + qv[2] * vec[1], // polarity checked; q~.vec = -qv
|
184
|
+
q0 * vec[1] - qv[2] * vec[0] + qv[0] * vec[2],
|
185
|
+
q0 * vec[2] - qv[0] * vec[1] + qv[1] * vec[0]};
|
186
|
+
vec[0] = q0_ * qv[0] + q0 * qv_[0] + qv_[1] * qv[2] - qv_[2] * qv[1];
|
187
|
+
vec[1] = q0_ * qv[1] + q0 * qv_[1] + qv_[2] * qv[0] - qv_[0] * qv[2];
|
188
|
+
vec[2] = q0_ * qv[2] + q0 * qv_[2] + qv_[0] * qv[1] - qv_[1] * qv[0];
|
189
|
+
#endif
|
190
|
+
}
|
191
|
+
|
192
|
+
/**
|
193
|
+
* Perform coordinate rotation by invoking apply(vec.v)
|
194
|
+
*
|
195
|
+
* @see apply(FloatT (&vec)[3])
|
196
|
+
*/
|
197
|
+
void apply(System_3D &vec) const {
|
198
|
+
apply(vec.v);
|
199
|
+
}
|
200
|
+
};
|
133
201
|
};
|
134
202
|
|
135
203
|
template <class FloatT, class Earth> class System_XYZ;
|
@@ -186,7 +254,6 @@ class System_XYZ : public System_3D<FloatT> {
|
|
186
254
|
}
|
187
255
|
|
188
256
|
FloatT dist() const {
|
189
|
-
|
190
257
|
return FloatT(std::sqrt(
|
191
258
|
pow2(x()) + pow2(y()) + pow2(z())));
|
192
259
|
}
|
@@ -220,6 +287,19 @@ class System_XYZ : public System_3D<FloatT> {
|
|
220
287
|
std::atan2(y(), x()),
|
221
288
|
(p / std::cos(_lat) - n));
|
222
289
|
}
|
290
|
+
|
291
|
+
/**
|
292
|
+
* Get coordinates in a delayed (i.e., rotated) ECEF frame.
|
293
|
+
*
|
294
|
+
* @param (delay_sec) delayed seconds (both positive (delayed) and negative (hasten) values
|
295
|
+
* are acceptable)
|
296
|
+
* @return (self_t) coordinates
|
297
|
+
*/
|
298
|
+
self_t after(const FloatT &delay_sec) const {
|
299
|
+
FloatT rad(delay_sec * -Earth::Omega_Earth_IAU); // Earth rotation is negative direction (y->x)
|
300
|
+
FloatT crad(std::cos(rad)), srad(std::sin(rad));
|
301
|
+
return self_t(x() * crad - y() * srad, x() * srad + y() * crad, z());
|
302
|
+
}
|
223
303
|
};
|
224
304
|
|
225
305
|
template <class FloatT, class Earth>
|
@@ -22,6 +22,8 @@
|
|
22
22
|
#include "navigation/QZSS.h"
|
23
23
|
#include "navigation/GLONASS.h"
|
24
24
|
#include "navigation/RINEX.h"
|
25
|
+
#include "navigation/SP3.h"
|
26
|
+
#include "navigation/ANTEX.h"
|
25
27
|
|
26
28
|
#include "navigation/GPS_Solver_Base.h"
|
27
29
|
#include "navigation/GPS_Solver.h"
|
@@ -1104,7 +1106,7 @@ struct GPS_RangeCorrector
|
|
1104
1106
|
%ignore glonass;
|
1105
1107
|
%ignore select_solver;
|
1106
1108
|
%ignore relative_property;
|
1107
|
-
%ignore
|
1109
|
+
%ignore select_satellite;
|
1108
1110
|
%ignore update_position_solution;
|
1109
1111
|
%ignore user_correctors_t;
|
1110
1112
|
%ignore user_correctors;
|
@@ -1201,47 +1203,17 @@ struct GPS_RangeCorrector
|
|
1201
1203
|
return super_t::update_position_solution(geomat, res);
|
1202
1204
|
}
|
1203
1205
|
template <>
|
1204
|
-
GPS_Solver<FloatT>::base_t::
|
1206
|
+
GPS_Solver<FloatT>::base_t::satellite_t GPS_Solver<FloatT>::select_satellite(
|
1205
1207
|
const GPS_Solver<FloatT>::base_t::prn_t &prn,
|
1206
|
-
const GPS_Solver<FloatT>::base_t::gps_time_t &time
|
1207
|
-
|
1208
|
-
|
1209
|
-
select_solver(prn).satellite_position(prn, time, buf));
|
1208
|
+
const GPS_Solver<FloatT>::base_t::gps_time_t &time) const {
|
1209
|
+
GPS_Solver<FloatT>::base_t::satellite_t res(
|
1210
|
+
select_solver(prn).select_satellite(prn, time));
|
1210
1211
|
#ifdef SWIGRUBY
|
1211
|
-
|
1212
|
-
static const VALUE key(ID2SYM(rb_intern("
|
1212
|
+
if(!res.is_available()){
|
1213
|
+
static const VALUE key(ID2SYM(rb_intern("relative_property")));
|
1213
1214
|
VALUE hook(rb_hash_lookup(hooks, key));
|
1214
|
-
if(NIL_P(hook)){
|
1215
|
-
|
1216
|
-
SWIG_From(int)(prn), // prn
|
1217
|
-
SWIG_NewPointerObj( // time
|
1218
|
-
new base_t::gps_time_t(time), $descriptor(GPS_Time<FloatT> *), SWIG_POINTER_OWN),
|
1219
|
-
(res // position (internally calculated)
|
1220
|
-
? SWIG_NewPointerObj(res, $descriptor(System_XYZ<FloatT, WGS84> *), 0)
|
1221
|
-
: Qnil)};
|
1222
|
-
VALUE res_hook(proc_call_throw_if_error(hook, sizeof(values) / sizeof(values[0]), values));
|
1223
|
-
if(NIL_P(res_hook)){ // unknown position
|
1224
|
-
res = NULL;
|
1225
|
-
break;
|
1226
|
-
}else if(RB_TYPE_P(res_hook, T_ARRAY) && (RARRAY_LEN(res_hook) == 3)){
|
1227
|
-
int i(0);
|
1228
|
-
for(; i < 3; ++i){
|
1229
|
-
VALUE v(RARRAY_AREF(res_hook, i));
|
1230
|
-
if(!SWIG_IsOK(swig::asval(v, &buf[i]))){break;}
|
1231
|
-
}
|
1232
|
-
if(i == 3){
|
1233
|
-
res = &buf;
|
1234
|
-
break;
|
1235
|
-
}
|
1236
|
-
}else if(SWIG_IsOK(SWIG_ConvertPtr(
|
1237
|
-
res_hook, (void **)&res, $descriptor(System_XYZ<FloatT, WGS84> *), 0))){
|
1238
|
-
res = &(buf = *res);
|
1239
|
-
break;
|
1240
|
-
}
|
1241
|
-
throw std::runtime_error(
|
1242
|
-
std::string("System_XYZ or [d * 3] is expected (d: " %str(FloatT) "), however ")
|
1243
|
-
.append(inspect_str(res_hook)));
|
1244
|
-
}while(false);
|
1215
|
+
if(!NIL_P(hook)){res.impl = this;}
|
1216
|
+
}
|
1245
1217
|
#endif
|
1246
1218
|
return res;
|
1247
1219
|
}
|
@@ -1408,7 +1380,28 @@ struct GPS_Solver
|
|
1408
1380
|
GPS_SpaceNode<FloatT> space_node;
|
1409
1381
|
GPS_SolverOptions<FloatT> options;
|
1410
1382
|
GPS_SinglePositioning<FloatT> solver;
|
1411
|
-
|
1383
|
+
struct ephemeris_proxy_t {
|
1384
|
+
struct item_t {
|
1385
|
+
const void *impl;
|
1386
|
+
typename base_t::satellite_t (*impl_select)(
|
1387
|
+
const void *,
|
1388
|
+
const typename base_t::prn_t &, const typename base_t::gps_time_t &);
|
1389
|
+
} gps, qzss;
|
1390
|
+
static typename base_t::satellite_t forward(
|
1391
|
+
const void *ptr,
|
1392
|
+
const typename base_t::prn_t &prn, const typename base_t::gps_time_t &t){
|
1393
|
+
const ephemeris_proxy_t *proxy(static_cast<const ephemeris_proxy_t *>(ptr));
|
1394
|
+
const item_t &target(((prn >= 193) && (prn <= 202)) ? proxy->qzss : proxy->gps);
|
1395
|
+
return target.impl_select(target.impl, prn, t);
|
1396
|
+
}
|
1397
|
+
ephemeris_proxy_t(GPS_SinglePositioning<FloatT> &solver){
|
1398
|
+
gps.impl = qzss.impl = solver.satellites.impl;
|
1399
|
+
gps.impl_select = qzss.impl_select = solver.satellites.impl_select;
|
1400
|
+
solver.satellites.impl = this;
|
1401
|
+
solver.satellites.impl_select = forward;
|
1402
|
+
}
|
1403
|
+
} ephemeris_proxy;
|
1404
|
+
gps_t() : space_node(), options(), solver(space_node), ephemeris_proxy(solver) {}
|
1412
1405
|
} gps;
|
1413
1406
|
struct sbas_t {
|
1414
1407
|
SBAS_SpaceNode<FloatT> space_node;
|
@@ -1481,10 +1474,9 @@ struct GPS_Solver
|
|
1481
1474
|
const typename base_t::gps_time_t &time_arrival,
|
1482
1475
|
const typename base_t::pos_t &usr_pos,
|
1483
1476
|
const typename base_t::xyz_t &usr_vel) const;
|
1484
|
-
virtual typename base_t::
|
1477
|
+
virtual typename base_t::satellite_t select_satellite(
|
1485
1478
|
const typename base_t::prn_t &prn,
|
1486
|
-
const typename base_t::gps_time_t &time
|
1487
|
-
typename base_t::xyz_t &res) const;
|
1479
|
+
const typename base_t::gps_time_t &time) const;
|
1488
1480
|
virtual bool update_position_solution(
|
1489
1481
|
const typename base_t::geometric_matrices_t &geomat,
|
1490
1482
|
typename base_t::user_pvt_t &res) const;
|
@@ -1667,6 +1659,101 @@ template <class FloatT>
|
|
1667
1659
|
struct RINEX_Observation {};
|
1668
1660
|
}
|
1669
1661
|
|
1662
|
+
%extend SP3 {
|
1663
|
+
%typemap(in,numinputs=0) int count[ANY] (int temp[$1_dim0]) "$1 = temp;"
|
1664
|
+
%typemap(argout) int count[ANY] {
|
1665
|
+
for(int i(0); i < $1_dim0; ++i){
|
1666
|
+
%append_output(SWIG_From(int)($1[i]));
|
1667
|
+
}
|
1668
|
+
}
|
1669
|
+
void satellites(int count[SP3::SYS_SYSTEMS]) const {
|
1670
|
+
typename SP3_Product<FloatT>::satellite_count_t x(self->satellite_count());
|
1671
|
+
count[SP3<FloatT>::SYS_GPS] = x.gps;
|
1672
|
+
count[SP3<FloatT>::SYS_SBAS] = x.sbas;
|
1673
|
+
count[SP3<FloatT>::SYS_QZSS] = x.qzss;
|
1674
|
+
count[SP3<FloatT>::SYS_GLONASS] = x.glonass;
|
1675
|
+
count[SP3<FloatT>::SYS_GALILEO] = x.galileo;
|
1676
|
+
count[SP3<FloatT>::SYS_BEIDOU] = x.beidou;
|
1677
|
+
}
|
1678
|
+
|
1679
|
+
}
|
1680
|
+
%inline {
|
1681
|
+
template <class FloatT>
|
1682
|
+
struct SP3 : public SP3_Product<FloatT> {
|
1683
|
+
int read(const char *fname) {
|
1684
|
+
std::fstream fin(fname, std::ios::in | std::ios::binary);
|
1685
|
+
return SP3_Reader<FloatT>::read_all(fin, *this);
|
1686
|
+
}
|
1687
|
+
enum system_t {
|
1688
|
+
SYS_GPS,
|
1689
|
+
SYS_SBAS,
|
1690
|
+
SYS_QZSS,
|
1691
|
+
SYS_GLONASS,
|
1692
|
+
SYS_GALILEO,
|
1693
|
+
SYS_BEIDOU,
|
1694
|
+
SYS_SYSTEMS,
|
1695
|
+
};
|
1696
|
+
bool push(GPS_Solver<FloatT> &solver, const system_t &sys) const {
|
1697
|
+
switch(sys){
|
1698
|
+
case SYS_GPS:
|
1699
|
+
return SP3_Product<FloatT>::push(
|
1700
|
+
solver.gps.ephemeris_proxy.gps, SP3_Product<FloatT>::SYSTEM_GPS);
|
1701
|
+
case SYS_SBAS:
|
1702
|
+
return SP3_Product<FloatT>::push(
|
1703
|
+
solver.sbas.solver.satellites, SP3_Product<FloatT>::SYSTEM_SBAS);
|
1704
|
+
case SYS_QZSS:
|
1705
|
+
return SP3_Product<FloatT>::push(
|
1706
|
+
solver.gps.ephemeris_proxy.qzss, SP3_Product<FloatT>::SYSTEM_QZSS);
|
1707
|
+
case SYS_GLONASS:
|
1708
|
+
return SP3_Product<FloatT>::push(
|
1709
|
+
solver.glonass.solver.satellites, SP3_Product<FloatT>::SYSTEM_GLONASS);
|
1710
|
+
case SYS_GALILEO:
|
1711
|
+
case SYS_BEIDOU:
|
1712
|
+
default:
|
1713
|
+
break;
|
1714
|
+
}
|
1715
|
+
return false;
|
1716
|
+
}
|
1717
|
+
bool push(GPS_Solver<FloatT> &solver) const {
|
1718
|
+
system_t target[] = {
|
1719
|
+
SYS_GPS,
|
1720
|
+
SYS_SBAS,
|
1721
|
+
SYS_QZSS,
|
1722
|
+
SYS_GLONASS,
|
1723
|
+
//SYS_GALILEO,
|
1724
|
+
//SYS_BEIDOU,
|
1725
|
+
};
|
1726
|
+
for(std::size_t i(0); i < sizeof(target) / sizeof(target[0]); ++i){
|
1727
|
+
if(!push(solver, target[i])){return false;}
|
1728
|
+
}
|
1729
|
+
return true;
|
1730
|
+
}
|
1731
|
+
System_XYZ<FloatT, WGS84> position(
|
1732
|
+
const int &sat_id, const GPS_Time<FloatT> &t) const {
|
1733
|
+
return SP3_Product<FloatT>::select(sat_id, t).position(t);
|
1734
|
+
}
|
1735
|
+
System_XYZ<FloatT, WGS84> velocity(
|
1736
|
+
const int &sat_id, const GPS_Time<FloatT> &t) const {
|
1737
|
+
return SP3_Product<FloatT>::select(sat_id, t).velocity(t);
|
1738
|
+
}
|
1739
|
+
FloatT clock_error(
|
1740
|
+
const int &sat_id, const GPS_Time<FloatT> &t) const {
|
1741
|
+
return SP3_Product<FloatT>::select(sat_id, t).clock_error(t);
|
1742
|
+
}
|
1743
|
+
FloatT clock_error_dot(
|
1744
|
+
const int &sat_id, const GPS_Time<FloatT> &t) const {
|
1745
|
+
return SP3_Product<FloatT>::select(sat_id, t).clock_error_dot(t);
|
1746
|
+
}
|
1747
|
+
int apply_antex(const char *fname) {
|
1748
|
+
ANTEX_Product<FloatT> antex;
|
1749
|
+
std::fstream fin(fname, std::ios::in | std::ios::binary);
|
1750
|
+
int read_items(ANTEX_Reader<FloatT>::read_all(fin, antex));
|
1751
|
+
if(read_items < 0){return read_items;}
|
1752
|
+
return antex.move_to_antenna_position(*this);
|
1753
|
+
}
|
1754
|
+
};
|
1755
|
+
}
|
1756
|
+
|
1670
1757
|
#undef MAKE_ACCESSOR
|
1671
1758
|
#undef MAKE_VECTOR2ARRAY
|
1672
1759
|
#undef MAKE_ARRAY_INPUT
|
@@ -1694,6 +1781,7 @@ struct RINEX_Observation {};
|
|
1694
1781
|
%template(SolverOptions_GLONASS) GLONASS_SolverOptions<type>;
|
1695
1782
|
|
1696
1783
|
%template(RINEX_Observation) RINEX_Observation<type>;
|
1784
|
+
%template(SP3) SP3<type>;
|
1697
1785
|
%enddef
|
1698
1786
|
|
1699
1787
|
CONCRETIZE(double);
|