From a7147702bc2d2ddd9c61b1e90a8f991886bade68 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 30 Jun 2022 10:38:29 +0200 Subject: [PATCH] Bug fix: PVT does not resolve position anymore after a loss of samples event --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 25 +++++++++++++++---- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 3 ++- .../gnuradio_blocks/hybrid_observables_gs.cc | 20 ++++++++++++++- .../gnuradio_blocks/hybrid_observables_gs.h | 1 + 4 files changed, 42 insertions(+), 7 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 9ec599576..6a4f82616 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -166,11 +166,13 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_flag_monitor_pvt_enabled(conf_.monitor_enabled), d_flag_monitor_ephemeris_enabled(conf_.monitor_ephemeris_enabled), d_show_local_time_zone(conf_.show_local_time_zone), - d_waiting_obs_block_rx_clock_offset_correction_msg(false), + d_timestamp_rx_clock_offset_correction_msg_ms(0LL), d_enable_rx_clock_correction(conf_.enable_rx_clock_correction), d_an_printer_enabled(conf_.an_output_enabled), d_log_timetag(conf_.log_source_timetag) { + //debug + d_pvt_errors_counter = 0; // Send feedback message to observables block with the receiver clock offset this->message_port_register_out(pmt::mp("pvt_to_observables")); // Experimental: VLT commands from PVT to tracking channels @@ -2168,18 +2170,18 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (fabs(Rx_clock_offset_s) * 1000.0 > d_max_obs_block_rx_clock_offset_ms) { - if (!d_waiting_obs_block_rx_clock_offset_correction_msg) + //check if the message was just sent to not duplicate it while it is being applied + if ((d_local_counter_ms - d_timestamp_rx_clock_offset_correction_msg_ms) > 1000) { this->message_port_pub(pmt::mp("pvt_to_observables"), pmt::make_any(Rx_clock_offset_s)); - d_waiting_obs_block_rx_clock_offset_correction_msg = true; - LOG(INFO) << "Sent clock offset correction to observables: " << Rx_clock_offset_s << "[s]"; + d_timestamp_rx_clock_offset_correction_msg_ms = d_local_counter_ms; + LOG(INFO) << "PVT: Sent clock offset correction to observables: " << Rx_clock_offset_s << "[s]"; } } else { if (d_enable_rx_clock_correction == true) { - d_waiting_obs_block_rx_clock_offset_correction_msg = false; d_gnss_observables_map_t0 = d_gnss_observables_map_t1; apply_rx_clock_offset(d_gnss_observables_map, Rx_clock_offset_s); d_gnss_observables_map_t1 = d_gnss_observables_map; @@ -2209,6 +2211,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } else { + d_pvt_errors_counter = 0; d_rx_time = d_gnss_observables_map.begin()->second.RX_time; current_RX_time_ms = static_cast(d_rx_time * 1000.0); if (current_RX_time_ms % d_output_rate_ms == 0) @@ -2221,6 +2224,18 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } } } + else + { + //sanity check: If the PVT solver is getting 100 consecutive errors, send a reset command to observables block + d_pvt_errors_counter++; + if (d_pvt_errors_counter >= 100) + { + int command = 1; + this->message_port_pub(pmt::mp("pvt_to_observables"), pmt::make_any(command)); + LOG(INFO) << "PVT: Number of consecutive position solver error reached, Sent reset to observables."; + d_pvt_errors_counter = 0; + } + } // compute on the fly PVT solution if (flag_compute_pvt_output == true) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index bfd53db63..c9a627e72 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -282,7 +282,8 @@ private: bool d_flag_monitor_pvt_enabled; bool d_flag_monitor_ephemeris_enabled; bool d_show_local_time_zone; - bool d_waiting_obs_block_rx_clock_offset_correction_msg; + uint64_t d_timestamp_rx_clock_offset_correction_msg_ms; + uint32_t d_pvt_errors_counter; bool d_enable_rx_clock_correction; bool d_enable_has_messages; bool d_an_printer_enabled; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index 82b86fc9e..0778dd5c3 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -226,6 +226,25 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg LOG(INFO) << "Corrected new RX Time offset: " << static_cast(round(new_rx_clock_offset_s * 1000.0)) << "[ms]"; } + if (pmt::any_ref(msg).type().hash_code() == d_int_type_hash_code) + { + const auto command_from_pvt = wht::any_cast(pmt::any_ref(msg)); + switch (command_from_pvt) + { + case 1: //reset TOW + d_T_rx_TOW_ms = 0; + d_last_rx_clock_round20ms_error = 0; + d_T_rx_TOW_set = false; + for (uint32_t n = 0; n < d_nchannels_out; n++) + { + d_gnss_synchro_history->clear(n); + } + LOG(INFO) << "Received reset observables TOW command from PVT"; + break; + default: + break; + } + } } catch (const wht::bad_any_cast &e) { @@ -786,7 +805,6 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) } epoch_data[n] = interpolated_gnss_synchro; } - if (d_T_rx_TOW_set) { update_TOW(epoch_data); diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index a6966b5d6..5c4863c7c 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -70,6 +70,7 @@ private: explicit hybrid_observables_gs(const Obs_Conf& conf_); const size_t d_double_type_hash_code = typeid(double).hash_code(); + const size_t d_int_type_hash_code = typeid(int).hash_code(); void msg_handler_pvt_to_observables(const pmt::pmt_t& msg); double compute_T_rx_s(const Gnss_Synchro& a) const;