gps_pvt 0.4.1 → 0.6.0

data/ext/ninja-scan-light/tool/navigation/coordinate.h

@@ -51,6 +51,7 @@
 #endif
 
 #include "param/vector3.h"
+#include "param/quaternion.h"
 #include "param/matrix.h"
 
 #include "WGS84.h"
@@ -130,6 +131,73 @@ class System_3D {
       in >> self[2];
       return in;
     }
+
+
+    ///< Coordinate rotation
+    struct Rotation {
+      Quaternion<FloatT> q;
+      Rotation() : q(1, 0, 0, 0) {}
+      Rotation &then(const Quaternion<FloatT> &q_) {
+        q *= q_;
+        return *this;
+      }
+      Rotation &then(const Rotation &rot) {
+        return then(rot.q);
+      }
+      Rotation &then_x(const FloatT &rad) {
+        return then(Quaternion<FloatT>(
+            std::cos(rad / 2), std::sin(rad / 2), 0, 0));
+      }
+      Rotation &then_y(const FloatT &rad) {
+        return then(Quaternion<FloatT>(
+            std::cos(rad / 2), 0, std::sin(rad / 2), 0));
+      }
+      Rotation &then_z(const FloatT &rad) {
+        return then(Quaternion<FloatT>(
+            std::cos(rad / 2), 0, 0, std::sin(rad / 2)));
+      }
+      operator Matrix<FloatT>() const {
+        return q.getDCM();
+      }
+      /**
+       * Perform coordinate rotation
+       * For example; application of clockwise 90 degree coordinate rotation along to z axis
+       * to vector(1,0,0) yields to vector(0,-1,0).
+       * This is not identical to vector rotation along with some axis,
+       * which corresponds to vector(0,1,0) of example, i.e.,
+       * z-axis 90 degree rotation of vector(1,0,0).
+       *
+       * @param vec target vector to which rotation is applied.
+       */
+      void apply(FloatT (&vec)[3]) const {
+#if 0
+        // as definition: (~q * vec * q).vector();
+        Vector3<FloatT> v(((q.conj() *= Vector3<FloatT>(vec.v)) *= q).vector());
+        vec[0] = v[0];
+        vec[1] = v[1];
+        vec[2] = v[2];
+#else // optimized version
+        FloatT q0(q[0]), qv[3] = {q[1], q[2], q[3]};
+        FloatT q0_(qv[0] * vec[0] + qv[1] * vec[1] + qv[2] * vec[2]), // -q~.vec (i*) vec
+            qv_[3] = { // q~[0] * vec + q~.vec (e*) vec
+              q0 * vec[0] - qv[1] * vec[2] + qv[2] * vec[1], // polarity checked; q~.vec = -qv
+              q0 * vec[1] - qv[2] * vec[0] + qv[0] * vec[2],
+              q0 * vec[2] - qv[0] * vec[1] + qv[1] * vec[0]};
+        vec[0] = q0_ * qv[0] + q0 * qv_[0] + qv_[1] * qv[2] - qv_[2] * qv[1];
+        vec[1] = q0_ * qv[1] + q0 * qv_[1] + qv_[2] * qv[0] - qv_[0] * qv[2];
+        vec[2] = q0_ * qv[2] + q0 * qv_[2] + qv_[0] * qv[1] - qv_[1] * qv[0];
+#endif
+      }
+
+      /**
+       * Perform coordinate rotation by invoking apply(vec.v)
+       *
+       * @see apply(FloatT (&vec)[3])
+       */
+      void apply(System_3D &vec) const {
+        apply(vec.v);
+      }
+    };
 };
 
 template <class FloatT, class Earth> class System_XYZ;
@@ -186,7 +254,6 @@ class System_XYZ : public System_3D<FloatT> {
     }
     
     FloatT dist() const {
-      
       return FloatT(std::sqrt(
           pow2(x()) + pow2(y()) + pow2(z())));
     }
@@ -220,6 +287,19 @@ class System_XYZ : public System_3D<FloatT> {
           std::atan2(y(), x()),
           (p / std::cos(_lat) - n));
     }
+
+    /**
+     * Get coordinates in a delayed (i.e., rotated) ECEF frame.
+     *
+     * @param (delay_sec) delayed seconds (both positive (delayed) and negative (hasten) values
+     * are acceptable)
+     * @return (self_t) coordinates
+     */
+    self_t after(const FloatT &delay_sec) const {
+      FloatT rad(delay_sec * -Earth::Omega_Earth_IAU); // Earth rotation is negative direction (y->x)
+      FloatT crad(std::cos(rad)), srad(std::sin(rad));
+      return self_t(x() * crad - y() * srad, x() * srad + y() * crad, z());
+    }
 };
 
 template <class FloatT, class Earth>

data/ext/ninja-scan-light/tool/swig/GPS.i

@@ -22,6 +22,8 @@
 #include "navigation/QZSS.h"
 #include "navigation/GLONASS.h"
 #include "navigation/RINEX.h"
+#include "navigation/SP3.h"
+#include "navigation/ANTEX.h"
 
 #include "navigation/GPS_Solver_Base.h"
 #include "navigation/GPS_Solver.h"
@@ -1104,7 +1106,7 @@ struct GPS_RangeCorrector
   %ignore glonass;
   %ignore select_solver;
   %ignore relative_property;
-  %ignore satellite_position;
+  %ignore select_satellite;
   %ignore update_position_solution;
   %ignore user_correctors_t;
   %ignore user_correctors;
@@ -1201,47 +1203,17 @@ struct GPS_RangeCorrector
       return super_t::update_position_solution(geomat, res);
     }
     template <>
-    GPS_Solver<FloatT>::base_t::xyz_t *GPS_Solver<FloatT>::satellite_position(
+    GPS_Solver<FloatT>::base_t::satellite_t GPS_Solver<FloatT>::select_satellite(
         const GPS_Solver<FloatT>::base_t::prn_t &prn,
-        const GPS_Solver<FloatT>::base_t::gps_time_t &time,
-        GPS_Solver<FloatT>::base_t::xyz_t &buf) const {
-      GPS_Solver<FloatT>::base_t::xyz_t *res(
-          select_solver(prn).satellite_position(prn, time, buf));
+        const GPS_Solver<FloatT>::base_t::gps_time_t &time) const {
+      GPS_Solver<FloatT>::base_t::satellite_t res(
+          select_solver(prn).select_satellite(prn, time));
 #ifdef SWIGRUBY
-      do{
-        static const VALUE key(ID2SYM(rb_intern("satellite_position")));
+      if(!res.is_available()){
+        static const VALUE key(ID2SYM(rb_intern("relative_property")));
         VALUE hook(rb_hash_lookup(hooks, key));
-        if(NIL_P(hook)){break;}
-        VALUE values[] = {
-            SWIG_From(int)(prn), // prn
-            SWIG_NewPointerObj( // time
-              new base_t::gps_time_t(time), $descriptor(GPS_Time<FloatT> *), SWIG_POINTER_OWN),
-            (res // position (internally calculated)
-              ? SWIG_NewPointerObj(res, $descriptor(System_XYZ<FloatT, WGS84> *), 0) 
-              : Qnil)};
-        VALUE res_hook(proc_call_throw_if_error(hook, sizeof(values) / sizeof(values[0]), values));
-        if(NIL_P(res_hook)){ // unknown position
-          res = NULL;
-          break;
-        }else if(RB_TYPE_P(res_hook, T_ARRAY) && (RARRAY_LEN(res_hook) == 3)){
-          int i(0);
-          for(; i < 3; ++i){
-            VALUE v(RARRAY_AREF(res_hook, i));
-            if(!SWIG_IsOK(swig::asval(v, &buf[i]))){break;}
-          }
-          if(i == 3){
-            res = &buf;
-            break;
-          }
-        }else if(SWIG_IsOK(SWIG_ConvertPtr(
-            res_hook, (void **)&res, $descriptor(System_XYZ<FloatT, WGS84> *), 0))){
-          res = &(buf = *res);
-          break;
-        }
-        throw std::runtime_error(
-            std::string("System_XYZ or [d * 3] is expected (d: " %str(FloatT) "), however ")
-              .append(inspect_str(res_hook)));
-      }while(false);
+        if(!NIL_P(hook)){res.impl = this;}
+      }
 #endif
       return res;
     }
@@ -1408,7 +1380,28 @@ struct GPS_Solver
     GPS_SpaceNode<FloatT> space_node;
     GPS_SolverOptions<FloatT> options;
     GPS_SinglePositioning<FloatT> solver;
-    gps_t() : space_node(), options(), solver(space_node) {}
+    struct ephemeris_proxy_t {
+      struct item_t {
+        const void *impl;
+        typename base_t::satellite_t (*impl_select)(
+            const void *,
+            const typename base_t::prn_t &, const typename base_t::gps_time_t &);
+      } gps, qzss;
+      static typename base_t::satellite_t forward(
+          const void *ptr,
+          const typename base_t::prn_t &prn, const typename base_t::gps_time_t &t){
+        const ephemeris_proxy_t *proxy(static_cast<const ephemeris_proxy_t *>(ptr));
+        const item_t &target(((prn >= 193) && (prn <= 202)) ? proxy->qzss : proxy->gps);
+        return target.impl_select(target.impl, prn, t);
+      }
+      ephemeris_proxy_t(GPS_SinglePositioning<FloatT> &solver){
+        gps.impl = qzss.impl = solver.satellites.impl;
+        gps.impl_select = qzss.impl_select = solver.satellites.impl_select;
+        solver.satellites.impl = this;
+        solver.satellites.impl_select = forward;
+      }
+    } ephemeris_proxy;
+    gps_t() : space_node(), options(), solver(space_node), ephemeris_proxy(solver) {}
   } gps;
   struct sbas_t {
     SBAS_SpaceNode<FloatT> space_node;
@@ -1481,10 +1474,9 @@ struct GPS_Solver
       const typename base_t::gps_time_t &time_arrival,
       const typename base_t::pos_t &usr_pos,
       const typename base_t::xyz_t &usr_vel) const;
-  virtual typename base_t::xyz_t *satellite_position(
+  virtual typename base_t::satellite_t select_satellite(
       const typename base_t::prn_t &prn,
-      const typename base_t::gps_time_t &time,
-      typename base_t::xyz_t &res) const;
+      const typename base_t::gps_time_t &time) const;
   virtual bool update_position_solution(
       const typename base_t::geometric_matrices_t &geomat,
       typename base_t::user_pvt_t &res) const;
@@ -1667,6 +1659,101 @@ template <class FloatT>
 struct RINEX_Observation {};
 }
 
+%extend SP3 {
+  %typemap(in,numinputs=0) int count[ANY] (int temp[$1_dim0]) "$1 = temp;"
+  %typemap(argout) int count[ANY] {
+    for(int i(0); i < $1_dim0; ++i){
+      %append_output(SWIG_From(int)($1[i]));
+    }
+  }
+  void satellites(int count[SP3::SYS_SYSTEMS]) const {
+    typename SP3_Product<FloatT>::satellite_count_t x(self->satellite_count());
+    count[SP3<FloatT>::SYS_GPS] = x.gps;
+    count[SP3<FloatT>::SYS_SBAS] = x.sbas;
+    count[SP3<FloatT>::SYS_QZSS] = x.qzss;
+    count[SP3<FloatT>::SYS_GLONASS] = x.glonass;
+    count[SP3<FloatT>::SYS_GALILEO] = x.galileo;
+    count[SP3<FloatT>::SYS_BEIDOU] = x.beidou;
+  }
+
+} 
+%inline {
+template <class FloatT>
+struct SP3 : public SP3_Product<FloatT> {
+  int read(const char *fname) {
+    std::fstream fin(fname, std::ios::in | std::ios::binary);
+    return SP3_Reader<FloatT>::read_all(fin, *this);
+  }
+  enum system_t {
+    SYS_GPS,
+    SYS_SBAS,
+    SYS_QZSS,
+    SYS_GLONASS,
+    SYS_GALILEO,
+    SYS_BEIDOU,
+    SYS_SYSTEMS,
+  };
+  bool push(GPS_Solver<FloatT> &solver, const system_t &sys) const {
+    switch(sys){
+      case SYS_GPS:
+        return SP3_Product<FloatT>::push(
+            solver.gps.ephemeris_proxy.gps, SP3_Product<FloatT>::SYSTEM_GPS);
+      case SYS_SBAS:
+        return SP3_Product<FloatT>::push(
+            solver.sbas.solver.satellites, SP3_Product<FloatT>::SYSTEM_SBAS);
+      case SYS_QZSS:
+        return SP3_Product<FloatT>::push(
+            solver.gps.ephemeris_proxy.qzss, SP3_Product<FloatT>::SYSTEM_QZSS);
+      case SYS_GLONASS:
+        return SP3_Product<FloatT>::push(
+            solver.glonass.solver.satellites, SP3_Product<FloatT>::SYSTEM_GLONASS);
+      case SYS_GALILEO:
+      case SYS_BEIDOU:
+      default:
+        break;
+    }
+    return false;
+  }
+  bool push(GPS_Solver<FloatT> &solver) const {
+    system_t target[] = {
+      SYS_GPS,
+      SYS_SBAS,
+      SYS_QZSS,
+      SYS_GLONASS,
+      //SYS_GALILEO,
+      //SYS_BEIDOU,
+    };
+    for(std::size_t i(0); i < sizeof(target) / sizeof(target[0]); ++i){
+      if(!push(solver, target[i])){return false;}
+    }
+    return true;
+  }
+  System_XYZ<FloatT, WGS84> position(
+      const int &sat_id, const GPS_Time<FloatT> &t) const {
+    return SP3_Product<FloatT>::select(sat_id, t).position(t);
+  }
+  System_XYZ<FloatT, WGS84> velocity(
+      const int &sat_id, const GPS_Time<FloatT> &t) const {
+    return SP3_Product<FloatT>::select(sat_id, t).velocity(t);
+  }
+  FloatT clock_error(
+      const int &sat_id, const GPS_Time<FloatT> &t) const {
+    return SP3_Product<FloatT>::select(sat_id, t).clock_error(t);
+  }
+  FloatT clock_error_dot(
+      const int &sat_id, const GPS_Time<FloatT> &t) const {
+    return SP3_Product<FloatT>::select(sat_id, t).clock_error_dot(t);
+  }
+  int apply_antex(const char *fname) {
+    ANTEX_Product<FloatT> antex;
+    std::fstream fin(fname, std::ios::in | std::ios::binary);
+    int read_items(ANTEX_Reader<FloatT>::read_all(fin, antex));
+    if(read_items < 0){return read_items;}
+    return antex.move_to_antenna_position(*this);
+  }
+};
+}
+
 #undef MAKE_ACCESSOR
 #undef MAKE_VECTOR2ARRAY
 #undef MAKE_ARRAY_INPUT
@@ -1694,6 +1781,7 @@ struct RINEX_Observation {};
 %template(SolverOptions_GLONASS) GLONASS_SolverOptions<type>;
 
 %template(RINEX_Observation) RINEX_Observation<type>;
+%template(SP3) SP3<type>;
 %enddef
 
 CONCRETIZE(double);