diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index ddadd7242b..1b3b17e84c 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/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); diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h index 57fb22c4c6..adc6d14c52 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.h +++ b/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 { diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 61f141ee84..390ee3fe46 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/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(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(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) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index cb8a2053bd..253fa48535 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/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; diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index 34852c9c89..2fc6d2d54c 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/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; diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index 7ebeb03243..8ad48126f4 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/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(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(d_averaging_depth); - d_avg_longitude_d = d_avg_longitude_d / static_cast(d_averaging_depth); - d_avg_height_m = d_avg_height_m / static_cast(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(d_hist_longitude_d.size()); + d_avg_longitude_d = d_avg_longitude_d / static_cast(d_hist_longitude_d.size()); + d_avg_height_m = d_avg_height_m / static_cast(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; } diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index 3d97645378..9e86826065 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/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}; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index bab77a0241..b0334abf46 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1235,5 +1235,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } } } - return this->is_valid_position(); + if (is_valid_position()) + { + perform_pos_averaging(); + } + return is_valid_position(); }