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.
@@ -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
- gps_t() : space_node(), options(), solver(space_node) {}
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);