gps_pvt 0.5.1 → 0.6.0
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 +8975 -7360
- 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/GPS.h +92 -4
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +9 -1
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +18 -161
- 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 +120 -1
- 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
@@ -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"
|
@@ -1378,7 +1380,28 @@ struct GPS_Solver
|
|
1378
1380
|
GPS_SpaceNode<FloatT> space_node;
|
1379
1381
|
GPS_SolverOptions<FloatT> options;
|
1380
1382
|
GPS_SinglePositioning<FloatT> solver;
|
1381
|
-
|
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) {}
|
1382
1405
|
} gps;
|
1383
1406
|
struct sbas_t {
|
1384
1407
|
SBAS_SpaceNode<FloatT> space_node;
|
@@ -1636,6 +1659,101 @@ template <class FloatT>
|
|
1636
1659
|
struct RINEX_Observation {};
|
1637
1660
|
}
|
1638
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
|
+
|
1639
1757
|
#undef MAKE_ACCESSOR
|
1640
1758
|
#undef MAKE_VECTOR2ARRAY
|
1641
1759
|
#undef MAKE_ARRAY_INPUT
|
@@ -1663,6 +1781,7 @@ struct RINEX_Observation {};
|
|
1663
1781
|
%template(SolverOptions_GLONASS) GLONASS_SolverOptions<type>;
|
1664
1782
|
|
1665
1783
|
%template(RINEX_Observation) RINEX_Observation<type>;
|
1784
|
+
%template(SP3) SP3<type>;
|
1666
1785
|
%enddef
|
1667
1786
|
|
1668
1787
|
CONCRETIZE(double);
|