diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 246737f2c..8b6c011bf 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -58,7 +58,9 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, pvt_output_parameters.pre_2009_file = configuration->property("GNSS-SDR.pre_2009_file", false); // output rate - pvt_output_parameters.output_rate_ms = bc::lcm(20, configuration->property(role + ".output_rate_ms", 500)); + pvt_output_parameters.observable_interval_ms = configuration->property("GNSS-SDR.observable_interval_ms", pvt_output_parameters.observable_interval_ms); + + pvt_output_parameters.output_rate_ms = bc::lcm(static_cast(pvt_output_parameters.observable_interval_ms), configuration->property(role + ".output_rate_ms", 500)); // display rate pvt_output_parameters.display_rate_ms = bc::lcm(pvt_output_parameters.output_rate_ms, configuration->property(role + ".display_rate_ms", 500)); diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 622ba7543..69a0fa936 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -173,6 +173,7 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_nchannels = nchannels; d_type_of_rx = conf_.type_of_receiver; + d_observable_interval_ms = conf_.observable_interval_ms; // GPS Ephemeris data message port in this->message_port_register_in(pmt::mp("telemetry")); @@ -2003,8 +2004,8 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (!d_gnss_observables_map_t0.empty()) { const auto t0_int_ms = static_cast(d_gnss_observables_map_t0.cbegin()->second.RX_time * 1000.0); - const uint32_t adjust_next_20ms = 20 - t0_int_ms % 20; - current_RX_time_ms = t0_int_ms + adjust_next_20ms; + const uint32_t adjust_next_obs_interval_ms = d_observable_interval_ms - t0_int_ms % d_observable_interval_ms; + current_RX_time_ms = t0_int_ms + adjust_next_obs_interval_ms; if (current_RX_time_ms % d_output_rate_ms == 0) { diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 77e40e9b4..be63fe0ff 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -254,6 +254,7 @@ private: uint32_t d_nchannels; uint32_t d_type_of_rx; + uint32_t d_observable_interval_ms; bool d_dump; bool d_dump_mat; diff --git a/src/algorithms/PVT/libs/pvt_conf.cc b/src/algorithms/PVT/libs/pvt_conf.cc index d5951940c..f5c73401a 100644 --- a/src/algorithms/PVT/libs/pvt_conf.cc +++ b/src/algorithms/PVT/libs/pvt_conf.cc @@ -19,6 +19,7 @@ Pvt_Conf::Pvt_Conf() { type_of_receiver = 0U; + observable_interval_ms = 20U; output_rate_ms = 0; display_rate_ms = 0; kml_rate_ms = 1000; diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index 147e5109b..bd92de550 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -51,6 +51,8 @@ public: std::string udp_eph_addresses; uint32_t type_of_receiver; + uint32_t observable_interval_ms; + int32_t output_rate_ms; int32_t display_rate_ms; int32_t kml_rate_ms; diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index 9061ba4bb..e502fa12c 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -31,13 +31,14 @@ HybridObservables::HybridObservables(const ConfigurationInterface* configuration dump_mat_ = configuration->property(role + ".dump_mat", true); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - Obs_Conf conf; + Obs_Conf conf{}; conf.dump = dump_; conf.dump_mat = dump_mat_; conf.dump_filename = dump_filename_; conf.nchannels_in = in_streams_; conf.nchannels_out = out_streams_; + conf.observable_interval_ms = configuration->property("GNSS-SDR.observable_interval_ms", conf.observable_interval_ms); conf.enable_carrier_smoothing = configuration->property(role + ".enable_carrier_smoothing", conf.enable_carrier_smoothing); if (FLAGS_carrier_smoothing_factor == DEFAULT_CARRIER_SMOOTHING_FACTOR) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index a3a55b565..4fbcbb9be 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -26,6 +26,7 @@ #include #include #include +#include // for std::min #include #include // for round #include // for size_t, llabs @@ -117,12 +118,11 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block( } } d_T_rx_TOW_ms = 0U; - d_T_rx_step_ms = 20; // read from config at the adapter GNSS-SDR.observable_interval_ms!! + d_T_rx_step_ms = conf_.observable_interval_ms; d_T_rx_TOW_set = false; d_T_status_report_timer_ms = 0; - // rework - d_Rx_clock_buffer.set_capacity(10); // 10*20 ms = 200 ms of data in buffer - d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer + d_Rx_clock_buffer.set_capacity(std::min(std::max(200U / d_T_rx_step_ms, 3U), 10U)); + d_Rx_clock_buffer.clear(); d_channel_last_pll_lock = std::vector(d_nchannels_out, false); d_channel_last_pseudorange_smooth = std::vector(d_nchannels_out, 0.0); @@ -191,10 +191,10 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg { const auto new_rx_clock_offset_s = boost::any_cast(pmt::any_ref(msg)); d_T_rx_TOW_ms = d_T_rx_TOW_ms - static_cast(round(new_rx_clock_offset_s * 1000.0)); - // align the receiver clock to integer multiple of 20 ms - if (d_T_rx_TOW_ms % 20) + // align the receiver clock to integer multiple of d_T_rx_step_ms + if (d_T_rx_TOW_ms % d_T_rx_step_ms) { - d_T_rx_TOW_ms += 20 - d_T_rx_TOW_ms % 20; + d_T_rx_TOW_ms += d_T_rx_step_ms - d_T_rx_TOW_ms % d_T_rx_step_ms; } // d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer for (uint32_t n = 0; n < d_nchannels_out; n++) @@ -371,7 +371,7 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, uint3 if (nearest_element != -1 and nearest_element != static_cast(d_gnss_synchro_history->size(ch))) { - if ((static_cast(old_abs_diff) / static_cast(d_gnss_synchro_history->get(ch, nearest_element).fs)) < 0.02) + if ((static_cast(old_abs_diff) / static_cast(d_gnss_synchro_history->get(ch, nearest_element).fs)) < static_cast(d_T_rx_step_ms) / 1000.0) { int32_t neighbor_element; if (rx_clock > d_gnss_synchro_history->get(ch, nearest_element).Tracking_sample_counter) @@ -483,10 +483,10 @@ void hybrid_observables_gs::update_TOW(const std::vector &data) } } d_T_rx_TOW_ms = TOW_ref; - // align the receiver clock to integer multiple of 20 ms - if (d_T_rx_TOW_ms % 20) + // align the receiver clock to integer multiple of d_T_rx_step_ms + if (d_T_rx_TOW_ms % d_T_rx_step_ms) { - d_T_rx_TOW_ms += 20 - d_T_rx_TOW_ms % 20; + d_T_rx_TOW_ms += d_T_rx_step_ms - d_T_rx_TOW_ms % d_T_rx_step_ms; } } else diff --git a/src/algorithms/observables/libs/obs_conf.cc b/src/algorithms/observables/libs/obs_conf.cc index 9dbadf9c7..47901434f 100644 --- a/src/algorithms/observables/libs/obs_conf.cc +++ b/src/algorithms/observables/libs/obs_conf.cc @@ -22,8 +22,9 @@ Obs_Conf::Obs_Conf() { dump_filename = std::string("obs_dump.dat"); smoothing_factor = FLAGS_carrier_smoothing_factor; - nchannels_in = 0; - nchannels_out = 0; + nchannels_in = 0U; + nchannels_out = 0U; + observable_interval_ms = 20U; enable_carrier_smoothing = false; dump = false; dump_mat = false; diff --git a/src/algorithms/observables/libs/obs_conf.h b/src/algorithms/observables/libs/obs_conf.h index b3728d688..ff7340fb1 100644 --- a/src/algorithms/observables/libs/obs_conf.h +++ b/src/algorithms/observables/libs/obs_conf.h @@ -36,6 +36,7 @@ public: int32_t smoothing_factor; uint32_t nchannels_in; uint32_t nchannels_out; + uint32_t observable_interval_ms; bool enable_carrier_smoothing; bool dump; bool dump_mat;