diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index 0bfef7185..86944492e 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -659,6 +659,22 @@ void hybrid_observables_gs::set_tag_timestamp_in_sdr_timeframe(const std::vector } } +void hybrid_observables_gs::propagate_extra_data(const std::vector &data) +{ + if (d_extra_data_tags.empty()) + { + return; + } + + do + { + auto &tag = d_extra_data_tags.front(); + add_item_tag(0, this->nitems_written(0) + 1, tag.key, tag.value); + d_extra_data_tags.pop(); + } + while (!d_extra_data_tags.empty()); +} + int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, @@ -673,9 +689,17 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) { d_Rx_clock_buffer.push_back(in[d_nchannels_in - 1][0].Tracking_sample_counter); - // time tags std::vector tags_vec; - this->get_tags_in_range(tags_vec, d_nchannels_in - 1, this->nitems_read(d_nchannels_in - 1), this->nitems_read(d_nchannels_in - 1) + 1); + // extra data tags + get_tags_in_range(tags_vec, d_nchannels_in - 1, this->nitems_read(d_nchannels_in - 1), this->nitems_read(d_nchannels_in - 1) + 1, pmt::mp("extra_data")); + // std::cout << "OBS (" << std::to_string(tags_vec.size()) << ")" << std::endl; + for (const auto &tag : tags_vec) + { + d_extra_data_tags.emplace(tag); + } + // time tags + tags_vec.clear(); + this->get_tags_in_range(tags_vec, d_nchannels_in - 1, this->nitems_read(d_nchannels_in - 1), this->nitems_read(d_nchannels_in - 1) + 1, pmt::mp("timetag")); for (const auto &it : tags_vec) { try @@ -762,7 +786,6 @@ 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++) { @@ -799,6 +822,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) { compute_pranges(epoch_data); set_tag_timestamp_in_sdr_timeframe(epoch_data, d_Rx_clock_buffer.front()); + propagate_extra_data(epoch_data); } // Carrier smoothing (optional) @@ -810,12 +834,6 @@ 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 f23e67795..81f9d61d5 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -79,6 +79,9 @@ private: void smooth_pseudoranges(std::vector& data); void set_tag_timestamp_in_sdr_timeframe(const std::vector& data, uint64_t rx_clock); + + void propagate_extra_data(const std::vector &data); + int32_t save_matfile() const; Obs_Conf d_conf; @@ -90,6 +93,8 @@ private: std::vector> d_SourceTagTimestamps; std::queue d_TimeChannelTagTimestamps; + std::queue d_extra_data_tags; + std::vector d_channel_last_pll_lock; std::vector d_channel_last_pseudorange_smooth; std::vector d_channel_last_carrier_phase_rads;