Add velocity sigma to GPS solution outputs

fenrir-naru · Nov 16, 2023 · af3a2d8 · af3a2d8
1 parent aca8df6
commit af3a2d8
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 4 deletions.
diff --git a/tool/INS_GPS/GNSS_Receiver.h b/tool/INS_GPS/GNSS_Receiver.h
@@ -545,6 +545,7 @@ data.gps.solver_options. expr
             << ',' << "v_east"
             << ',' << "v_down"
             << ',' << "receiver_clock_error_dot_ms"
+            << ',' << "vel_sigma"
             << ',' << "used_satellites"
             << ',' << "PRN";
       }
@@ -622,9 +623,10 @@ data.gps.solver_options. expr
         out << ',' << src.user_velocity_enu.north()
             << ',' << src.user_velocity_enu.east()
             << ',' << -src.user_velocity_enu.up()
-            << ',' << src.receiver_error_rate;
+            << ',' << src.receiver_error_rate
+            << ',' << src.sigma_vel.p;
       }else{
-        out << ",,,,";
+        out << ",,,,,";
       }
       if(src.position_solved()){
         out << ',' << src.used_satellites

diff --git a/tool/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb
@@ -53,11 +53,11 @@ def self.pvt_items(opt = {})
         }
       ]
     }.call] + [[
-      [:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms],
+      [:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms, :vel_sigma],
       proc{|pvt|
         next [nil] * 4 unless pvt.velocity_solved?
         [:north, :east, :down].collect{|k| pvt.velocity.send(k)} \
-            + [pvt.receiver_error_rate] 
+            + [pvt.receiver_error_rate, pvt.vel_sigma] 
       }
     ]] + [
       [:used_satellites, proc{|pvt| pvt.used_satellites}],

diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i
@@ -641,6 +641,7 @@ struct GPS_User_PVT
   const FloatT &hsigma() const {return base_t::sigma_pos.h;}
   const FloatT &vsigma() const {return base_t::sigma_pos.v;}
   const FloatT &tsigma() const {return base_t::sigma_pos.t;}
+  const FloatT &vel_sigma() const {return base_t::sigma_vel.p;}
   const unsigned int &used_satellites() const {return base_t::used_satellites;}
   std::vector<int> used_satellite_list() const {return base_t::used_satellite_mask.indices_one();}
   bool position_solved() const {return base_t::position_solved();}