Finish position averaging implementation

Finish incomplete position averaging implementation.

Configuration parameter:

uint32_t PVT.averaging_depth = 1 - Position averaging window size.
Setting it to a value <=1 disables position averaging.

Signed-off-by: Vladislav P <vladisslav2011@gmail.com>

src/algorithms/PVT/adapters/rtklib_pvt.cc

@@ -74,6 +74,10 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
     // display rate
     pvt_output_parameters.display_rate_ms = bc::lcm(pvt_output_parameters.output_rate_ms, configuration->property(role + ".display_rate_ms", 500));
 
+    // averaging
+    pvt_output_parameters.averaging_depth = configuration->property(role + ".averaging_depth", 1);
+    pvt_output_parameters.averaging_depth = pvt_output_parameters.averaging_depth ? pvt_output_parameters.averaging_depth : 1;
+
     // NMEA Printer settings
     pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false);
     pvt_output_parameters.nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename);

src/algorithms/PVT/adapters/rtklib_pvt.h

@@ -169,6 +169,7 @@ class Gps_Ephemeris;
  *  .show_local_time_zone - (false)
  *  .enable_rx_clock_correction - (false)
  *  .max_clock_offset_ms - (40)
+ *  .averaging_depth - position averaging window size (1)
  */
 class Rtklib_Pvt : public PvtInterface
 {

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc

@@ -527,12 +527,15 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
             d_local_time_str = std::string(" ") + time_zone_abrv + " (UTC " + utc_diff_str.substr(0, 3) + ":" + utc_diff_str.substr(3, 2) + ")";
         }
 
+    // averaging
+    d_averaging_depth = conf_.averaging_depth;
+
     if (d_enable_rx_clock_correction == true)
         {
             // setup two PVT solvers: internal solver for rx clock and user solver
             // user PVT solver
             d_user_pvt_solver = std::make_shared<Rtklib_Solver>(rtk, dump_ls_pvt_filename, d_type_of_rx, d_dump, d_dump_mat, d_use_e6_for_pvt);
-            d_user_pvt_solver->set_averaging_depth(1);
+            d_user_pvt_solver->set_averaging_depth(d_averaging_depth);
             d_user_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
 
             // internal PVT solver, mainly used to estimate the receiver clock
@@ -546,7 +549,7 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
         {
             // only one solver, customized by the user options
             d_internal_pvt_solver = std::make_shared<Rtklib_Solver>(rtk, dump_ls_pvt_filename, d_type_of_rx, d_dump, d_dump_mat, d_use_e6_for_pvt);
-            d_internal_pvt_solver->set_averaging_depth(1);
+            d_internal_pvt_solver->set_averaging_depth(d_averaging_depth);
             d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
             d_user_pvt_solver = d_internal_pvt_solver;
         }
@@ -1764,35 +1767,17 @@ bool rtklib_pvt_gs::get_latest_PVT(double* longitude_deg,
     double* course_over_ground_deg,
     time_t* UTC_time) const
 {
-    if (d_enable_rx_clock_correction == true)
+    if (d_user_pvt_solver->is_valid_position())
         {
-            if (d_user_pvt_solver->is_valid_position())
-                {
-                    *latitude_deg = d_user_pvt_solver->get_latitude();
-                    *longitude_deg = d_user_pvt_solver->get_longitude();
-                    *height_m = d_user_pvt_solver->get_height();
-                    *ground_speed_kmh = d_user_pvt_solver->get_speed_over_ground() * 3600.0 / 1000.0;
-                    *course_over_ground_deg = d_user_pvt_solver->get_course_over_ground();
-                    *UTC_time = convert_to_time_t(d_user_pvt_solver->get_position_UTC_time());
-
-                    return true;
-                }
-        }
-    else
-        {
-            if (d_internal_pvt_solver->is_valid_position())
-                {
-                    *latitude_deg = d_internal_pvt_solver->get_latitude();
-                    *longitude_deg = d_internal_pvt_solver->get_longitude();
-                    *height_m = d_internal_pvt_solver->get_height();
-                    *ground_speed_kmh = d_internal_pvt_solver->get_speed_over_ground() * 3600.0 / 1000.0;
-                    *course_over_ground_deg = d_internal_pvt_solver->get_course_over_ground();
-                    *UTC_time = convert_to_time_t(d_internal_pvt_solver->get_position_UTC_time());
-
-                    return true;
-                }
-        }
+            *latitude_deg = d_user_pvt_solver->get_latitude();
+            *longitude_deg = d_user_pvt_solver->get_longitude();
+            *height_m = d_user_pvt_solver->get_height();
+            *ground_speed_kmh = d_user_pvt_solver->get_speed_over_ground() * 3600.0 / 1000.0;
+            *course_over_ground_deg = d_user_pvt_solver->get_course_over_ground();
+            *UTC_time = convert_to_time_t(d_user_pvt_solver->get_position_UTC_time());
 
+            return true;
+        }
     return false;
 }
 
@@ -2245,7 +2230,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
                     // compute on the fly PVT solution
                     if (flag_compute_pvt_output == true)
                         {
-                            flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false);
+                            flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, true);
                         }
 
                     if (flag_pvt_valid == true)

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h

@@ -270,6 +270,7 @@ class rtklib_pvt_gs : public gr::sync_block
     uint32_t d_type_of_rx;
     uint32_t d_observable_interval_ms;
     uint32_t d_pvt_errors_counter;
+    uint32_t d_averaging_depth;
 
     bool d_dump;
     bool d_dump_mat;

src/algorithms/PVT/libs/pvt_conf.h

@@ -52,6 +52,7 @@ class Pvt_Conf
 
     uint32_t type_of_receiver = 0;
     uint32_t observable_interval_ms = 20;
+    uint32_t averaging_depth = 1;
 
     int32_t output_rate_ms = 0;
     int32_t display_rate_ms = 0;

src/algorithms/PVT/libs/pvt_solution.cc

@@ -85,49 +85,39 @@ void Pvt_Solution::perform_pos_averaging()
 {
     // MOVING AVERAGE PVT
     bool avg = d_flag_averaging;
-    if (avg == true)
+    if (avg == true && d_averaging_depth > 1)
         {
-            if (d_hist_longitude_d.size() == static_cast<unsigned int>(d_averaging_depth))
+            if (d_hist_longitude_d.size() == d_averaging_depth)
                 {
                     // Pop oldest value
                     d_hist_longitude_d.pop_back();
                     d_hist_latitude_d.pop_back();
                     d_hist_height_m.pop_back();
-                    // Push new values
-                    d_hist_longitude_d.push_front(d_longitude_d);
-                    d_hist_latitude_d.push_front(d_latitude_d);
-                    d_hist_height_m.push_front(d_height_m);
-
-                    d_avg_latitude_d = 0.0;
-                    d_avg_longitude_d = 0.0;
-                    d_avg_height_m = 0.0;
-                    for (size_t i = 0; i < d_hist_longitude_d.size(); i++)
-                        {
-                            d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
-                            d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
-                            d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
-                        }
-                    d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth);
-                    d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
-                    d_avg_height_m = d_avg_height_m / static_cast<double>(d_averaging_depth);
-                    d_valid_position = true;
                 }
-            else
+            // Push new values
+            d_hist_longitude_d.push_front(d_longitude_d);
+            d_hist_latitude_d.push_front(d_latitude_d);
+            d_hist_height_m.push_front(d_height_m);
+
+            d_avg_latitude_d = 0.0;
+            d_avg_longitude_d = 0.0;
+            d_avg_height_m = 0.0;
+            for (size_t i = 0; i < d_hist_longitude_d.size(); i++)
                 {
-                    // int current_depth=d_hist_longitude_d.size();
-                    // Push new values
-                    d_hist_longitude_d.push_front(d_longitude_d);
-                    d_hist_latitude_d.push_front(d_latitude_d);
-                    d_hist_height_m.push_front(d_height_m);
-
-                    d_avg_latitude_d = d_latitude_d;
-                    d_avg_longitude_d = d_longitude_d;
-                    d_avg_height_m = d_height_m;
-                    d_valid_position = false;
+                    d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
+                    d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
+                    d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
                 }
+            d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_hist_longitude_d.size());
+            d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_hist_longitude_d.size());
+            d_avg_height_m = d_avg_height_m / static_cast<double>(d_hist_longitude_d.size());
+            d_valid_position = true;
         }
     else
         {
+            d_avg_latitude_d = d_latitude_d;
+            d_avg_longitude_d = d_longitude_d;
+            d_avg_height_m = d_height_m;
             d_valid_position = true;
         }
 }
@@ -159,19 +149,19 @@ void Pvt_Solution::set_clock_drift_ppm(double clock_drift_ppm)
 
 double Pvt_Solution::get_latitude() const
 {
-    return d_latitude_d;
+    return d_avg_latitude_d;
 }
 
 
 double Pvt_Solution::get_longitude() const
 {
-    return d_longitude_d;
+    return d_avg_longitude_d;
 }
 
 
 double Pvt_Solution::get_height() const
 {
-    return d_height_m;
+    return d_avg_height_m;
 }
 
 

src/algorithms/PVT/libs/pvt_solution.h

@@ -114,8 +114,8 @@ class Pvt_Solution
     double d_avg_longitude_d{0.0};  // Averaged longitude in degrees
     double d_avg_height_m{0.0};     // Averaged height [m]
 
-    int d_averaging_depth{0};     // Length of averaging window
-    int d_valid_observations{0};  // Number of valid observations in this epoch
+    unsigned d_averaging_depth{0};  // Length of averaging window
+    int d_valid_observations{0};    // Number of valid observations in this epoch
 
     bool d_pre_2009_file{false};  // Flag to correct week rollover in post processing mode for signals older than 2009
     bool d_valid_position{false};

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -1235,5 +1235,9 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                         }
                 }
         }
-    return this->is_valid_position();
+    if (is_valid_position())
+        {
+            perform_pos_averaging();
+        }
+    return is_valid_position();
 }