gps_pvt 0.1.4 → 0.2.0

data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
  * This file was automatically generated by SWIG (http://www.swig.org).
- * Version 4.0.1
+ * Version 4.0.2
  *
  * This file is not intended to be easily readable and contains a number of
  * coding conventions designed to improve portability and efficiency. Do not make
@@ -1612,6 +1612,8 @@ SWIGRUNTIMEINLINE char *
 SWIG_Ruby_MangleStr(VALUE obj)
 {
   VALUE stype = rb_iv_get(obj, "@__swigtype__");
+  if (NIL_P(stype))
+    return NULL;
   return StringValuePtr(stype);
 }
 
@@ -1876,7 +1878,7 @@ static VALUE mSylphideMath;
 #define SWIG_RUBY_THREAD_END_BLOCK
 
 
-#define SWIGVERSION 0x040001 
+#define SWIGVERSION 0x040002 
 #define SWIG_VERSION SWIGVERSION
 
 
@@ -2259,7 +2261,7 @@ SWIG_ruby_failed(VALUE SWIGUNUSEDPARM(arg1), VALUE SWIGUNUSEDPARM(arg2))
 } 
 
 
-/*@SWIG:/usr/share/swig4.0/ruby/rubyprimtypes.swg,19,%ruby_aux_method@*/
+/*@SWIG:/usr/local/share/swig/4.0.2/ruby/rubyprimtypes.swg,19,%ruby_aux_method@*/
 SWIGINTERN VALUE SWIG_AUX_NUM2DBL(VALUE arg)
 {
   VALUE *args = (VALUE *)arg;
@@ -3089,7 +3091,7 @@ SWIG_From_unsigned_SS_int  (unsigned int value)
 }
 
 
-/*@SWIG:/usr/share/swig4.0/ruby/rubyprimtypes.swg,19,%ruby_aux_method@*/
+/*@SWIG:/usr/local/share/swig/4.0.2/ruby/rubyprimtypes.swg,19,%ruby_aux_method@*/
 SWIGINTERN VALUE SWIG_AUX_NUM2LONG(VALUE arg)
 {
   VALUE *args = (VALUE *)arg;
@@ -3155,7 +3157,7 @@ SWIG_AsVal_bool (VALUE obj, bool *val)
 }
 
 
-/*@SWIG:/usr/share/swig4.0/ruby/rubyprimtypes.swg,19,%ruby_aux_method@*/
+/*@SWIG:/usr/local/share/swig/4.0.2/ruby/rubyprimtypes.swg,19,%ruby_aux_method@*/
 SWIGINTERN VALUE SWIG_AUX_NUM2ULONG(VALUE arg)
 {
   VALUE *args = (VALUE *)arg;

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

@@ -243,7 +243,7 @@ struct GPS_Time {
   GPS_Time &canonicalize(){
     int quot(std::floor(seconds / seconds_week));
     week += quot;
-    seconds -= (seconds_week * quot);
+    seconds -= (quot * (int)seconds_week);
     return *this;
   }
   GPS_Time(const std::tm &t, const float_t &leap_seconds = 0) {
@@ -405,11 +405,12 @@ struct GPS_Time {
     }
 
     // process remaining 4 years
-    int doy_i(0), doy[] = {
+    int doy[] = {
       leap_year ? 366 : 365,
       365, 365, 365,
       is_leap_year(year + 4) ? 366 : 365,
     };
+    std::size_t doy_i(0);
     for(; doy_i < sizeof(doy) / sizeof(doy[0]); ++doy_i){
       if(days <= doy[doy_i]){break;}
       days -= doy[doy_i];
@@ -2051,6 +2052,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
      * @param height_over_ellipsoid
      * @see spherically single layer approach, for example,
      * Eq.(3) of "Ionospheric Range Error Correction Models" by N. Jakowski
+     * @see DO-229D A4.4.10.4 Eq.(A-42) is equivalent
      */
     static float_t slant_factor(
         const enu_t &relative_pos,
@@ -2089,8 +2091,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
       // Elevation and azimuth
       float_t el(relative_pos.elevation()),
              az(relative_pos.azimuth());
-      float_t sc_el(rad2sc(el)),
-             sc_az(rad2sc(az));
+      float_t sc_el(rad2sc(el));
              
       // Pierce point (PP stands for the earth projection of the Pierce point)
       // (the following equation is based on GPS ICD)

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

@@ -43,11 +43,13 @@
 #include <exception>
 
 #include <cmath>
+#include <cstddef>
 
 #include "param/bit_array.h"
 
 #include "GPS.h"
 #include "GPS_Solver_Base.h"
+#include "SBAS.h"
 #include "NTCM.h"
 
 template <class FloatT>
@@ -57,6 +59,7 @@ struct GPS_Solver_GeneralOptions {
 
   enum ionospheric_model_t {
     IONOSPHERIC_KLOBUCHAR,
+    IONOSPHERIC_SBAS,
     IONOSPHERIC_NTCM_GL,
     IONOSPHERIC_NONE, // which allows no correction
     IONOSPHERIC_MODELS,
@@ -66,7 +69,7 @@ struct GPS_Solver_GeneralOptions {
 
   int count_ionospheric_models() const {
     int res(0);
-    for(int i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
+    for(std::size_t i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
       if(ionospheric_models[i] != IONOSPHERIC_SKIP){res++;}
     }
     return res;
@@ -78,7 +81,7 @@ struct GPS_Solver_GeneralOptions {
       : elevation_mask(0), // elevation mask default is 0 [deg]
       residual_mask(30), // range residual mask is 30 [m]
       f_10_7(-1) {
-    for(int i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
+    for(std::size_t i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
       ionospheric_models[i] = IONOSPHERIC_SKIP;
     }
     // default: broadcasted Klobuchar parameters are at least required for solution.
@@ -97,6 +100,22 @@ struct GPS_Solver_GeneralOptions {
     }
     ionospheric_models[index] = model;
 
+    return true;
+  }
+  bool remove_ionospheric_model(const ionospheric_model_t &model) {
+    // shift, then replace
+    int j(0);
+    for(int i(0); i < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]); ++i){
+      if(ionospheric_models[i] == model){continue;}
+      ionospheric_models[j++] = ionospheric_models[i];
+    }
+    if(j == (sizeof(ionospheric_models) / sizeof(ionospheric_models[0]))){
+      return false;
+    }
+    do{
+      ionospheric_models[j++] = IONOSPHERIC_SKIP;
+    }while(j < sizeof(ionospheric_models) / sizeof(ionospheric_models[0]));
+
     return true;
   }
 };
@@ -139,6 +158,8 @@ class GPS_SinglePositioning : public SolverBaseT {
     inheritate_type(llh_t);
     inheritate_type(enu_t);
 
+    typedef SBAS_SpaceNode<float_t> sbas_space_node_t;
+
     inheritate_type(pos_t);
 
     inheritate_type(prn_obs_t);
@@ -151,9 +172,10 @@ class GPS_SinglePositioning : public SolverBaseT {
     inheritate_type(user_pvt_t);
 #undef inheritate_type
 
+    const sbas_space_node_t *space_node_sbas; ///< optional
+
     typedef typename GPS_Solver_Base<float_t>::options_t::template merge_t<
         GPS_SinglePositioning_Options<float_t>, base_t> options_t;
-
   protected:
     const space_node_t &_space_node;
     GPS_SinglePositioning_Options<float_t> _options;
@@ -172,22 +194,28 @@ class GPS_SinglePositioning : public SolverBaseT {
 
       int available_models(0);
 
-      for(int i(0); i < sizeof(opt.ionospheric_models) / sizeof(opt.ionospheric_models[0]); ++i){
+      for(std::size_t i(0);
+          i < sizeof(opt.ionospheric_models) / sizeof(opt.ionospheric_models[0]); ++i){
+        bool usable(false);
         switch(opt.ionospheric_models[i]){
           case options_t::IONOSPHERIC_KLOBUCHAR:
             // check whether Klobuchar parameters alpha and beta have been already received
-            if(_space_node.is_valid_iono()){break;}
-            opt.ionospheric_models[i] = options_t::IONOSPHERIC_SKIP;
-            continue;
+            if(_space_node.is_valid_iono()){usable = true;}
+            break;
+          case options_t::IONOSPHERIC_SBAS:
+            if(space_node_sbas){usable = true;}
+            break;
           case options_t::IONOSPHERIC_NTCM_GL:
-            if(opt.f_10_7 >= 0){break;}
+            if(opt.f_10_7 >= 0){usable = true;}
             // check F10.7 has been already configured
-            opt.ionospheric_models[i] = options_t::IONOSPHERIC_SKIP;
-            continue;
-          case options_t::IONOSPHERIC_SKIP:
-            continue;
+            break;
+          default: break;
+        }
+        if(usable){
+          available_models++;
+        }else{
+          opt.ionospheric_models[i] = options_t::IONOSPHERIC_SKIP;
         }
-        available_models++;
       }
 
       return available_models;
@@ -209,7 +237,9 @@ class GPS_SinglePositioning : public SolverBaseT {
     }
 
     GPS_SinglePositioning(const space_node_t &sn)
-        : base_t(), _space_node(sn), _options() {
+        : base_t(), 
+        _space_node(sn), space_node_sbas(NULL),
+        _options() {
       filter_ionospheric_models(_options);
     }
 
@@ -266,11 +296,28 @@ class GPS_SinglePositioning : public SolverBaseT {
       if(error.unknown_flag & range_error_t::MASK_IONOSPHERIC){
         // Ionospheric model selection, the fall back is no correction
         bool iono_model_hit(false);
-        for(int i(0); i < sizeof(_options.ionospheric_models) / sizeof(_options.ionospheric_models[0]); ++i){
+        for(std::size_t i(0);
+            i < sizeof(_options.ionospheric_models) / sizeof(_options.ionospheric_models[0]);
+            ++i){
           switch(_options.ionospheric_models[i]){
             case options_t::IONOSPHERIC_KLOBUCHAR:
               residual.residual += _space_node.iono_correction(relative_pos, usr_pos.llh, time_arrival);
               break;
+            case options_t::IONOSPHERIC_SBAS: {
+              // placeholder of checking availability and performing correction
+              typedef typename SBAS_SpaceNode<float_t>::available_satellites_t sats_t;
+              sats_t sats(space_node_sbas->available_satellites(usr_pos.llh.longitude()));
+
+              typename SBAS_SpaceNode<float_t>::IonosphericGridPoints::PointProperty prop;
+              for(typename sats_t::const_iterator it(sats.begin()); it != sats.end(); ++it){
+                prop = it->second
+                    ->ionospheric_grid_points().iono_correction(relative_pos, usr_pos.llh);
+                break; // TODO The nearest satellite is only checked
+              }
+              if(!prop.is_available()){continue;}
+              residual.residual += prop.delay;
+              break;
+            }
             case options_t::IONOSPHERIC_NTCM_GL: {
               // TODO f_10_7 setup, optimization (mag_model etc.)
               typename space_node_t::pierce_point_res_t pp(_space_node.pierce_point(relative_pos, usr_pos.llh));

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

@@ -202,9 +202,9 @@ struct GPS_Solver_Base {
   virtual const float_t *rate(
       const typename measurement_t::mapped_type &values, float_t &buf) const {
     const float_t *res;
-    if(res = find_value(values, measurement_items_t::L1_RANGE_RATE, buf)){
+    if((res = find_value(values, measurement_items_t::L1_RANGE_RATE, buf))){
 
-    }else if(res = find_value(values, measurement_items_t::L1_DOPPLER, buf)){
+    }else if((res = find_value(values, measurement_items_t::L1_DOPPLER, buf))){
       // Fall back to doppler
       buf *= -space_node_t::L1_WaveLength();
     }
@@ -214,9 +214,9 @@ struct GPS_Solver_Base {
   virtual const float_t *rate_sigma(
       const typename measurement_t::mapped_type &values, float_t &buf) const {
     const float_t *res;
-    if(res = find_value(values, measurement_items_t::L1_RANGE_RATE_SIGMA, buf)){
+    if((res = find_value(values, measurement_items_t::L1_RANGE_RATE_SIGMA, buf))){
 
-    }else if(res = find_value(values, measurement_items_t::L1_DOPPLER_SIGMA, buf)){
+    }else if((res = find_value(values, measurement_items_t::L1_DOPPLER_SIGMA, buf))){
       // Fall back to doppler
       buf *= space_node_t::L1_WaveLength();
     }
@@ -308,8 +308,9 @@ struct GPS_Solver_Base {
         case ERROR_VELOCITY_INSUFFICIENT_SATELLITES:
         case ERROR_VELOCITY_LS:
           return true;
+        default:
+          return false;
       }
-      return false;
     }
     bool velocity_solved() const {
       return error_code == ERROR_NO;

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

@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2021, M.Naruoka (fenrir)
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * - Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * - Neither the name of the naruoka.org nor the names of its contributors
+ *   may be used to endorse or promote products derived from this software
+ *   without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+ * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef __QZSS_H__
+#define __QZSS_H__
+
+/** @file
+ * @brief QZSS ICD definitions
+ */
+
+#include "GPS.h"
+#include <limits>
+
+template <class FloatT>
+struct QZSS_LNAV_Ephemeris_Raw : public GPS_SpaceNode<FloatT>::Satellite::Ephemeris::raw_t {
+  typedef typename GPS_SpaceNode<FloatT>::Satellite::Ephemeris eph_t;
+  typedef typename eph_t::raw_t super_t;
+
+  operator eph_t() const {
+    eph_t res(super_t::operator eph_t());
+    res.fit_interval = super_t::fit_interval_flag
+        ? (std::numeric_limits<FloatT>::max)()
+        : (2 * 60 * 60); // Fit interval (ICD:4.1.2.4 Subframe 2); should be zero
+    return res;
+  };
+
+  QZSS_LNAV_Ephemeris_Raw &operator=(const eph_t &eph){
+    super_t::operator=(eph);
+    super_t::fit_interval_flag = (eph.fit_interval > 2 * 60 * 60);
+    return *this;
+  }
+};
+
+#endif /* __QZSS_H__ */