diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index 48a1b1c14..0bfef7185 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -70,6 +70,7 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) d_smooth_filter_M(static_cast(conf_.smoothing_factor)), d_T_rx_step_s(static_cast(conf_.observable_interval_ms) / 1000.0), d_last_rx_clock_round20ms_error(0.0), + d_ref_channel(0U), d_T_rx_TOW_ms(0U), d_T_rx_step_ms(conf_.observable_interval_ms), d_T_status_report_timer_ms(0), @@ -225,6 +226,7 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg { case 1: // reset TOW d_T_rx_TOW_ms = 0; + d_ref_channel = 0; d_last_rx_clock_round20ms_error = 0; d_T_rx_TOW_set = false; for (uint32_t n = 0; n < d_nchannels_out; n++) @@ -508,17 +510,20 @@ void hybrid_observables_gs::update_TOW(const std::vector &data) { // int32_t TOW_ref = std::numeric_limits::max(); uint32_t TOW_ref = 0U; + uint32_t ref_ch = 0U; for (it = data.cbegin(); it != data.cend(); it++) { if (it->Flag_valid_word) { if (it->TOW_at_current_symbol_ms > TOW_ref) { + ref_ch = it->Channel_ID; TOW_ref = it->TOW_at_current_symbol_ms; d_T_rx_TOW_set = true; } } } + d_ref_channel = ref_ch; d_T_rx_TOW_ms = TOW_ref; // align the receiver clock to integer multiple of d_T_rx_step_ms if (d_T_rx_TOW_ms % d_T_rx_step_ms) @@ -756,6 +761,9 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) { std::vector epoch_data(d_nchannels_out); int32_t n_valid = 0; + std::vector tags{}; + get_tags_in_range(tags, d_ref_channel, nitems_read(d_ref_channel) - ninput_items[d_ref_channel], nitems_read(d_ref_channel), pmt::mp("extra_data")); + for (uint32_t n = 0; n < d_nchannels_out; n++) { Gnss_Synchro interpolated_gnss_synchro{}; @@ -802,6 +810,12 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) // output the observables set to the PVT block for (uint32_t n = 0; n < d_nchannels_out; n++) { + for (const auto &tag : tags) + { + add_item_tag(n, this->nitems_written(n) + 1, tag.key, tag.value); + // std::cout << "[ED TAG (TRK)] [" << std::to_string(tag.offset) << "] "; + // std::cout << std::endl; + } out[n][0] = epoch_data[n]; } // report channel status every second diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index 96a83b733..f23e67795 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -102,6 +102,7 @@ private: double d_T_rx_step_s; double d_last_rx_clock_round20ms_error; + uint32_t d_ref_channel; uint32_t d_T_rx_TOW_ms; uint32_t d_T_rx_step_ms; uint32_t d_T_status_report_timer_ms;