gps_pvt 0.1.4 → 0.2.0

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

@@ -15,16 +15,19 @@
 #include <iostream>
 #include <fstream>
 #include <exception>
+#include <sstream>
 
 #include "navigation/GPS.h"
+#include "navigation/SBAS.h"
+#include "navigation/QZSS.h"
 #include "navigation/RINEX.h"
 
 #include "navigation/GPS_Solver_Base.h"
 #include "navigation/GPS_Solver.h"
 #include "navigation/GPS_Solver_RAIM.h"
+#include "navigation/SBAS_Solver.h"
 
 #if defined(__cplusplus) && (__cplusplus < 201103L)
-#include <sstream>
 namespace std {
 template <class T>
 inline std::string to_string(const T &value){
@@ -180,6 +183,26 @@ const type &get_ ## name () const {
   }
 }
 %enddef
+#if defined(SWIGRUBY)
+%define MAKE_ARRAY_INPUT(type, arg_name, f_conv)
+%typemap(typecheck,precedence=SWIG_TYPECHECK_POINTER) type arg_name[ANY] {
+  $1 = RB_TYPE_P($input, T_ARRAY) ? 1 : 0;
+}
+%typemap(in) type arg_name[ANY] ($*1_ltype temp[$1_dim0]) {
+  if(!(RB_TYPE_P($input, T_ARRAY) && (RARRAY_LEN($input) == $1_dim0))){
+    SWIG_exception(SWIG_TypeError, "array[$1_dim0] is expected");
+  }
+  for(int i(0); i < $1_dim0; ++i){
+    if(!SWIG_IsOK(f_conv(RARRAY_AREF($input, i), &temp[i]))){
+      SWIG_exception(SWIG_TypeError, "$*1_ltype is expected");
+    }
+  }
+  $1 = temp;
+}
+%enddef
+#else
+#define MAKE_ARRAY_INPUT(type, arg_name, f_conv)
+#endif
 
 %inline %{
 template <class FloatT>
@@ -195,34 +218,8 @@ struct GPS_Ionospheric_UTC_Parameters : public GPS_SpaceNode<FloatT>::Ionospheri
       %append_output(swig::from($1[i]));
     }
   }
-  %typemap(in) const FloatT values[4] (FloatT temp[4]) {
-#ifdef SWIGRUBY
-    if(!(RB_TYPE_P($input, T_ARRAY) && (RARRAY_LEN($input) == 4))){
-      SWIG_exception(SWIG_TypeError, "array[4] is expected");
-    }
-    for(int i(0); i < 4; ++i){
-      SWIG_Object obj(RARRAY_AREF($input, i));
-      if(!SWIG_IsOK(swig::asval(obj, &temp[i]))){
-        SWIG_exception(SWIG_TypeError, "$*1_ltype is expected");
-      }
-    }
-#endif
-    $1 = temp;
-  }
-  %typemap(in) const unsigned int *buf (unsigned int temp[10]) {
-#ifdef SWIGRUBY
-    if((!RB_TYPE_P($input, T_ARRAY))
-        || (RARRAY_LEN($input) < sizeof(temp) / sizeof(temp[0]))){
-      SWIG_exception(SWIG_TypeError, "array is expected, or too short array");
-    }
-    $1 = temp;
-    for(int i(0); i < sizeof(temp) / sizeof(temp[0]); ++i){
-      if(!SWIG_IsOK(SWIG_AsVal(unsigned int)(RARRAY_AREF($input, i), &($1[i])))){
-        SWIG_exception(SWIG_TypeError, "$*1_ltype is expected");
-      }
-    }
-#endif
-  }
+  MAKE_ARRAY_INPUT(const FloatT, values, swig::asval);
+  MAKE_ARRAY_INPUT(const unsigned int, buf, SWIG_AsVal(unsigned int));
   %rename("alpha=") set_alpha;
   void set_alpha(const FloatT values[4]){
     for(int i(0); i < 4; ++i){
@@ -255,7 +252,7 @@ struct GPS_Ionospheric_UTC_Parameters : public GPS_SpaceNode<FloatT>::Ionospheri
   MAKE_ACCESSOR(WN_LSF, unsigned int);
   MAKE_ACCESSOR(DN, unsigned int);
   MAKE_ACCESSOR(delta_t_LSF, int);
-  static GPS_Ionospheric_UTC_Parameters<FloatT> parse(const unsigned int *buf){
+  static GPS_Ionospheric_UTC_Parameters<FloatT> parse(const unsigned int buf[10]){
     typedef typename GPS_SpaceNode<FloatT>
         ::BroadcastedMessage<unsigned int, 30> parser_t;
     if((parser_t::subframe_id(buf) != 4) || (parser_t::sv_page_id(buf) != 56)){
@@ -321,22 +318,10 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
   MAKE_ACCESSOR(omega, FloatT);
   MAKE_ACCESSOR(dot_Omega0, FloatT);
   MAKE_ACCESSOR(dot_i0, FloatT);
-  %typemap(in) const unsigned int *buf (unsigned int temp[10]) {
-#ifdef SWIGRUBY
-    if((!RB_TYPE_P($input, T_ARRAY))
-        || (RARRAY_LEN($input) < sizeof(temp) / sizeof(temp[0]))){
-      SWIG_exception(SWIG_TypeError, "array is expected, or too short array");
-    }
-    $1 = temp;
-    for(int i(0); i < sizeof(temp) / sizeof(temp[0]); ++i){
-      if(!SWIG_IsOK(SWIG_AsVal(unsigned int)(RARRAY_AREF($input, i), &($1[i])))){
-        SWIG_exception(SWIG_TypeError, "$*1_ltype is expected");
-      }
-    }
-#endif
-  }
+  
+  MAKE_ARRAY_INPUT(const unsigned int, buf, SWIG_AsVal(unsigned int));
   %apply int *OUTPUT { int *subframe_no, int *iodc_or_iode };
-  void parse(const unsigned int *buf, int *subframe_no, int *iodc_or_iode){
+  void parse(const unsigned int buf[10], int *subframe_no, int *iodc_or_iode){
     typedef typename GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris eph_t;
     typename eph_t::raw_t raw;
     eph_t eph;
@@ -432,11 +417,84 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
   %ignore tropo_correction() const;
   int read(const char *fname) {
     std::fstream fin(fname, std::ios::in | std::ios::binary);
-    return RINEX_NAV_Reader<FloatT>::read_all(fin, *self);
+    typename RINEX_NAV_Reader<FloatT>::space_node_list_t space_nodes = {
+      self, // GPS
+      NULL, // SBAS
+      self, // QZSS
+    };
+    return RINEX_NAV_Reader<FloatT>::read_all(fin, space_nodes);
   }
 }
 
 %include navigation/GPS.h
+%include navigation/QZSS.h
+
+%inline %{
+template <class FloatT>
+struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris {
+  SBAS_Ephemeris() : SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris() {}
+  SBAS_Ephemeris(const typename SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris &eph) 
+      : SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris(eph) {}
+};
+%}
+%extend SBAS_Ephemeris {
+  MAKE_ACCESSOR(svid, unsigned int);
+          
+  MAKE_ACCESSOR(WN, unsigned int);
+  MAKE_ACCESSOR(t_0, FloatT);
+  MAKE_ACCESSOR(URA, int);
+  MAKE_ACCESSOR(  x, FloatT); MAKE_ACCESSOR(  y, FloatT); MAKE_ACCESSOR(  z, FloatT);
+  MAKE_ACCESSOR( dx, FloatT); MAKE_ACCESSOR( dy, FloatT); MAKE_ACCESSOR( dz, FloatT);
+  MAKE_ACCESSOR(ddx, FloatT); MAKE_ACCESSOR(ddy, FloatT); MAKE_ACCESSOR(ddz, FloatT);
+  MAKE_ACCESSOR(a_Gf0, FloatT);
+  MAKE_ACCESSOR(a_Gf1, FloatT);
+  %apply GPS_Ephemeris::System_XYZ<FloatT, WGS84> & { System_XYZ<FloatT, WGS84> & }; // TODO ineffective?
+  void constellation(
+      System_XYZ<FloatT, WGS84> &position, System_XYZ<FloatT, WGS84> &velocity,
+      const GPS_Time<FloatT> &t, const FloatT &pseudo_range = 0,
+      const bool &with_velocity = true) const {
+    typename SBAS_SpaceNode<FloatT>::SatelliteProperties::constellation_t res(
+        self->constellation(t, pseudo_range, with_velocity));
+    position = res.position;
+    velocity = res.velocity;
+  }
+}
+
+%extend SBAS_SpaceNode {
+  %fragment(SWIG_Traits_frag(FloatT));
+  %ignore satellites() const;
+  %ignore satellite(const int &);
+  %ignore available_satellites(const FloatT &lng_deg) const;
+  void register_ephemeris(
+      const int &prn, const SBAS_Ephemeris<FloatT> &eph,
+      const int &priority_delta = 1){
+    self->satellite(prn).register_ephemeris(eph, priority_delta);
+  }
+  SBAS_Ephemeris<FloatT> ephemeris(const int &prn) const {
+    return SBAS_Ephemeris<FloatT>(
+        %const_cast(self, SBAS_SpaceNode<FloatT> *)->satellite(prn).ephemeris());
+  }
+  int read(const char *fname) {
+    std::fstream fin(fname, std::ios::in | std::ios::binary);
+    RINEX_NAV_Reader<FloatT>::space_node_list_t space_nodes = {NULL, self};
+    return RINEX_NAV_Reader<FloatT>::read_all(fin, space_nodes);
+  }
+  MAKE_ARRAY_INPUT(const unsigned int, buf, SWIG_AsVal(unsigned int));
+  int decode_message(const unsigned int buf[8],
+      const int &prn, const GPS_Time<FloatT> &t_reception,
+      const bool &LNAV_VNAV_LP_LPV_approach = false){
+    return static_cast<int>(
+        self->decode_message(buf, prn, t_reception, LNAV_VNAV_LP_LPV_approach));
+  }
+  std::string ionospheric_grid_points(const int &prn) const {
+    std::ostringstream ss;
+    ss << %const_cast(self, SBAS_SpaceNode<FloatT> *)->satellite(prn)
+      .ionospheric_grid_points();
+    return ss.str();
+  }
+}
+
+%include navigation/SBAS.h
 
 %extend GPS_User_PVT {
   %ignore solver_t;
@@ -746,12 +804,23 @@ struct GPS_Measurement {
 };
 }
 
-%extend GPS_SolverOptions {
-  %ignore base_t;
-  MAKE_ACCESSOR(elevation_mask, FloatT);
-  MAKE_ACCESSOR(residual_mask, FloatT);
-  MAKE_ACCESSOR(f_10_7, FloatT);
+%extend GPS_SolverOptions_Common {
+%define MAKE_ACCESSOR2(name, type)
+%rename(%str(name ## =)) set_ ## name;
+type set_ ## name (const type &v) {
+  return self->cast_general()->name = v;
+}
+%rename(%str(name)) get_ ## name;
+const type &get_ ## name () const {
+  return self->cast_general()->name;
+}
+%enddef
+  MAKE_ACCESSOR2(elevation_mask, FloatT);
+  MAKE_ACCESSOR2(residual_mask, FloatT);
+  MAKE_ACCESSOR2(f_10_7, FloatT);
+#undef MAKE_ACCESSOR2
   MAKE_VECTOR2ARRAY(int);
+  %ignore cast_base;
 #ifdef SWIGRUBY
   %rename("ionospheric_models=") set_ionospheric_models;
   %rename("ionospheric_models") get_ionospheric_models;
@@ -775,49 +844,92 @@ struct GPS_Measurement {
 }
 %inline %{
 template <class FloatT>
-struct GPS_SolverOptions : public GPS_SinglePositioning<FloatT>::options_t {
-  typedef typename GPS_SinglePositioning<FloatT>::options_t base_t;
-  void exclude(const int &prn){base_t::exclude_prn.set(prn);}
-  void include(const int &prn){base_t::exclude_prn.reset(prn);}
-  std::vector<int> excluded() const {return base_t::exclude_prn.excluded();}
+struct GPS_SolverOptions_Common {
   enum {
     IONOSPHERIC_KLOBUCHAR,
+    IONOSPHERIC_SBAS,
     IONOSPHERIC_NTCM_GL,
     IONOSPHERIC_NONE, // which allows no correction
     IONOSPHERIC_MODELS,
     IONOSPHERIC_SKIP = IONOSPHERIC_MODELS, // which means delegating the next slot
   };
+  virtual GPS_Solver_GeneralOptions<FloatT> *cast_general() = 0;
+  virtual const GPS_Solver_GeneralOptions<FloatT> *cast_general() const = 0;
   std::vector<int> get_ionospheric_models() const {
+    typedef GPS_Solver_GeneralOptions<FloatT> general_t;
+    const general_t *general(this->cast_general());
     std::vector<int> res;
-    for(int i(0); i < base_t::IONOSPHERIC_MODELS; ++i){
-      int v((int)(base_t::ionospheric_models[i]));
-      if(v == base_t::IONOSPHERIC_SKIP){break;}
+    for(int i(0); i < general_t::IONOSPHERIC_MODELS; ++i){
+      int v((int)(general->ionospheric_models[i]));
+      if(v == general_t::IONOSPHERIC_SKIP){break;}
       res.push_back(v);
     }
     return res;
   }
   std::vector<int> set_ionospheric_models(const std::vector<int> &models){
-    typedef typename base_t::ionospheric_model_t model_t;
-    for(int i(0), j(0), j_max(models.size()); i < base_t::IONOSPHERIC_MODELS; ++i){
-      model_t v(base_t::IONOSPHERIC_SKIP);
+    typedef GPS_Solver_GeneralOptions<FloatT> general_t;
+    general_t *general(this->cast_general());
+    typedef typename general_t::ionospheric_model_t model_t;
+    for(int i(0), j(0), j_max(models.size()); i < general_t::IONOSPHERIC_MODELS; ++i){
+      model_t v(general_t::IONOSPHERIC_SKIP);
       if(j < j_max){
-        if((models[j] >= 0) && (models[j] < base_t::IONOSPHERIC_SKIP)){
+        if((models[j] >= 0) && (models[j] < general_t::IONOSPHERIC_SKIP)){
           v = (model_t)models[j];
         }
         ++j;
       }
-      base_t::ionospheric_models[i] = v;
+      general->ionospheric_models[i] = v;
     }
     return get_ionospheric_models();
   }
 };
 %}
 
+%extend GPS_SolverOptions {
+  %ignore base_t;
+  %ignore cast_general;
+  MAKE_VECTOR2ARRAY(int);
+}
+%inline %{
+template <class FloatT>
+struct GPS_SolverOptions 
+    : public GPS_SinglePositioning<FloatT>::options_t, 
+    GPS_SolverOptions_Common<FloatT> {
+  typedef typename GPS_SinglePositioning<FloatT>::options_t base_t;
+  void exclude(const int &prn){base_t::exclude_prn.set(prn);}
+  void include(const int &prn){base_t::exclude_prn.reset(prn);}
+  std::vector<int> excluded() const {return base_t::exclude_prn.excluded();}
+  GPS_Solver_GeneralOptions<FloatT> *cast_general(){return this;}
+  const GPS_Solver_GeneralOptions<FloatT> *cast_general() const {return this;}
+};
+%}
+
+%extend SBAS_SolverOptions {
+  %ignore base_t;
+  %ignore cast_general;
+  MAKE_VECTOR2ARRAY(int);
+}
+%inline %{
+template <class FloatT>
+struct SBAS_SolverOptions 
+    : public SBAS_SinglePositioning<FloatT>::options_t,
+    GPS_SolverOptions_Common<FloatT> {
+  typedef typename SBAS_SinglePositioning<FloatT>::options_t base_t;
+  void exclude(const int &prn){base_t::exclude_prn.set(prn);}
+  void include(const int &prn){base_t::exclude_prn.reset(prn);}
+  std::vector<int> excluded() const {return base_t::exclude_prn.excluded();}
+  GPS_Solver_GeneralOptions<FloatT> *cast_general(){return this;}
+  const GPS_Solver_GeneralOptions<FloatT> *cast_general() const {return this;}
+};
+%}
+
 %extend GPS_Solver {
   %ignore super_t;
   %ignore base_t;
   %ignore gps_t;
   %ignore gps;
+  %ignore sbas_t;
+  %ignore sbas;
   %ignore select_solver;
   %ignore relative_property;
   %ignore satellite_position;
@@ -927,6 +1039,12 @@ struct GPS_Solver
     GPS_SinglePositioning<FloatT> solver;
     gps_t() : space_node(), options(), solver(space_node) {}
   } gps;
+  struct sbas_t {
+    SBAS_SpaceNode<FloatT> space_node;
+    SBAS_SolverOptions<FloatT> options;
+    SBAS_SinglePositioning<FloatT> solver;
+    sbas_t() : space_node(), options(), solver(space_node) {}
+  } sbas;
   SWIG_Object hooks;
 #ifdef SWIGRUBY
   static void mark(void *ptr){
@@ -935,16 +1053,22 @@ struct GPS_Solver
     rb_gc_mark(solver->hooks);
   }
 #endif
-  GPS_Solver() : super_t(), gps(), hooks() {
+  GPS_Solver() : super_t(), gps(), sbas(), hooks() {
+    gps.solver.space_node_sbas = &sbas.space_node;
+    sbas.solver.space_node_gps = &gps.space_node;
 #ifdef SWIGRUBY
     hooks = rb_hash_new();
 #endif
   }
   GPS_SpaceNode<FloatT> &gps_space_node() {return gps.space_node;}
   GPS_SolverOptions<FloatT> &gps_options() {return gps.options;}
+  SBAS_SpaceNode<FloatT> &sbas_space_node() {return sbas.space_node;}
+  SBAS_SolverOptions<FloatT> &sbas_options() {return sbas.options;}
   const base_t &select_solver(
       const typename base_t::prn_t &prn) const {
     if(prn > 0 && prn <= 32){return gps.solver;}
+    if(prn >= 120 && prn <= 158){return sbas.solver;}
+    if(prn > 192 && prn <= 202){return gps.solver;}
     return *this;
   }
   virtual typename base_t::relative_property_t relative_property(
@@ -968,6 +1092,8 @@ struct GPS_Solver
       const GPS_Time<FloatT> &receiver_time) const {
     const_cast<gps_t &>(gps).space_node.update_all_ephemeris(receiver_time);
     const_cast<gps_t &>(gps).solver.update_options(gps.options);
+    const_cast<sbas_t &>(sbas).space_node.update_all_ephemeris(receiver_time);
+    const_cast<sbas_t &>(sbas).solver.update_options(sbas.options);
     return super_t::solve().user_pvt(measurement.items, receiver_time);
   }
 };
@@ -1091,6 +1217,7 @@ struct RINEX_Observation {};
 
 #undef MAKE_ACCESSOR
 #undef MAKE_VECTOR2ARRAY
+#undef MAKE_ARRAY_INPUT
 
 %define CONCRETIZE(type)
 %template(Time) GPS_Time<type>;
@@ -1099,12 +1226,17 @@ struct RINEX_Observation {};
 %template(Ephemeris) GPS_Ephemeris<type>;
 %template(PVT) GPS_User_PVT<type>;
 %template(Measurement) GPS_Measurement<type>;
+%template(SolverOptionsCommon) GPS_SolverOptions_Common<type>;
 %template(SolverOptions) GPS_SolverOptions<type>;
 #if defined(SWIGRUBY)
 %markfunc GPS_Solver<type> "GPS_Solver<type>::mark";
 #endif
 %template(Solver) GPS_Solver<type>;
 
+%template(Ephemeris_SBAS) SBAS_Ephemeris<type>;
+%template(SpaceNode_SBAS) SBAS_SpaceNode<type>;
+%template(SolverOptions_SBAS) SBAS_SolverOptions<type>;
+
 %template(RINEX_Observation) RINEX_Observation<type>;
 %enddef