From 91f826b5cad8b6c21ac5eb102ba51dcec295f466 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 3 Dec 2021 12:56:20 +0100 Subject: [PATCH] Adding timetag propagation from source to telemetry decoders (currently supported by Galileo telemetry decoder) --- .../signal_source/libs/gnss_sdr_timestamp.cc | 15 ++-- .../signal_source/libs/gnss_sdr_timestamp.h | 2 + .../galileo_telemetry_decoder_gs.cc | 81 +++++++++++++++++++ .../galileo_telemetry_decoder_gs.h | 3 + .../gnuradio_blocks/dll_pll_veml_tracking.cc | 25 +++++- 5 files changed, 117 insertions(+), 9 deletions(-) diff --git a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc index c0a385fa0..d28f6499c 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc +++ b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc @@ -32,6 +32,7 @@ Gnss_Sdr_Timestamp::Gnss_Sdr_Timestamp(size_t sizeof_stream_item, d_timefile(timestamp_file), d_clock_offset_ms(clock_offset_ms) { + d_fraction_ms_offset = modf(d_clock_offset_ms, &d_integer_ms_offset); // optional clockoffset parameter to convert UTC timestamps to GPS time in some receiver's configuration get_next_timetag = true; next_timetag_samplecount = 0; } @@ -95,26 +96,26 @@ int Gnss_Sdr_Timestamp::work(int noutput_items, { if (read_next_timetag() == false) { - //std::cout << "End of TimeTag file reached!\n"; - //return 0; //todo: find why return -1 does not stop gnss-sdr! + // std::cout << "End of TimeTag file reached!\n"; + // return 0; // todo: find why return -1 does not stop gnss-sdr! } get_next_timetag = false; } for (size_t ch = 0; ch < output_items.size(); ch++) { std::memcpy(output_items[ch], input_items[ch], noutput_items * input_signature()->sizeof_stream_item(ch)); - uint64_t bytes_to_samples = 2; //todo: improve this.. hardcoded 2 bytes -> 1 complex sample! + uint64_t bytes_to_samples = 2; // todo: improve this.. hardcoded 2 bytes -> 1 complex sample! int64_t diff_samplecount = uint64diff(this->nitems_written(ch), next_timetag_samplecount * bytes_to_samples); - //std::cout << "diff_samplecount: " << diff_samplecount << ", noutput_items: " << noutput_items << "\n"; + // std::cout << "diff_samplecount: " << diff_samplecount << ", noutput_items: " << noutput_items << "\n"; if (diff_samplecount <= noutput_items and std::labs(diff_samplecount) <= noutput_items) { const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); - tmp_obj->tow_ms = next_timetag.tow_ms; + tmp_obj->tow_ms = next_timetag.tow_ms + static_cast(d_integer_ms_offset); tmp_obj->week = next_timetag.week; - tmp_obj->tow_ms_fraction = d_clock_offset_ms; //optional clockoffset parameter to convert UTC timestamps to GPS time in some receiver's configuration + tmp_obj->tow_ms_fraction = d_fraction_ms_offset; tmp_obj->rx_time = 0; add_item_tag(ch, this->nitems_written(ch) - diff_samplecount, pmt::mp("timetag"), pmt::make_any(tmp_obj)); - //std::cout << "[" << this->nitems_written(ch) - diff_samplecount << "] Sent TimeTag SC: " << next_timetag_samplecount * bytes_to_samples << ", Week: " << next_timetag.week << ", TOW: " << next_timetag.tow_ms << " [ms] \n"; + // std::cout << "[" << this->nitems_written(ch) - diff_samplecount << "] Sent TimeTag SC: " << next_timetag_samplecount * bytes_to_samples << ", Week: " << next_timetag.week << ", TOW: " << next_timetag.tow_ms << " [ms] \n"; get_next_timetag = true; } } diff --git a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h index 6904a7c0d..2bd9e1f98 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h +++ b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h @@ -66,6 +66,8 @@ private: std::fstream d_timefilestream; GnssTime next_timetag; double d_clock_offset_ms; + double d_integer_ms_offset; + double d_fraction_ms_offset; uint64_t next_timetag_samplecount; bool get_next_timetag; }; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 81e4cd6bc..6c1ba9911 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -685,6 +685,51 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( } } d_sample_counter++; // count for the processed symbols + + // ******* Time Tags from signal source (optional feature) ******* + std::vector tags_vec; + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + 1); // telemetry decoder consumes symbols one-by-one + if (!tags_vec.empty()) + { + for (std::vector::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it) + { + try + { + if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + const std::shared_ptr timetag = boost::any_cast>(pmt::any_ref(it->value)); + // std::cout << "Old tow: " << d_current_timetag.tow_ms << " new tow: " << timetag->tow_ms << "\n"; + d_current_timetag = *timetag; + d_valid_timetag = true; + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + } + } + else + { + if (d_valid_timetag == true) + { + // propagate timetag to current symbol + // todo: tag rx_time is set only in the time channel. The tracking tag does not have valid rx_time (it is not required since it is associated to the current symbol) + // d_current_timetag.rx_time+=d_PRN_code_period_ms + d_current_timetag.tow_ms += d_PRN_code_period_ms; + if (d_current_timetag.tow_ms >= 604800000) + { + d_current_timetag.tow_ms -= 604800000; + d_current_timetag.week++; + } + } + } + // *********************************************** + consume_each(1); d_flag_preamble = false; @@ -873,6 +918,18 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_TOW_at_Preamble_ms = static_cast(d_inav_nav.get_TOW5() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); d_inav_nav.set_TOW5_flag(false); + // timetag debug + if (d_valid_timetag == true) + { + int decoder_delay_ms = static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); + int rx_tow_at_preamble = d_current_timetag.tow_ms - decoder_delay_ms; + if (rx_tow_at_preamble < 0) + { + rx_tow_at_preamble += 604800000; + } + uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000); // floor to integer number of seconds + std::cout << "TOW at PREAMBLE: " << d_TOW_at_Preamble_ms << " predicted TOW at preamble: " << predicted_tow_at_preamble_ms << " [ms]\n"; + } } else if (d_inav_nav.is_TOW6_set() == true) // page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) @@ -881,6 +938,18 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_TOW_at_Preamble_ms = static_cast(d_inav_nav.get_TOW6() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); d_inav_nav.set_TOW6_flag(false); + // timetag debug + if (d_valid_timetag == true) + { + int decoder_delay_ms = static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); + int rx_tow_at_preamble = d_current_timetag.tow_ms - decoder_delay_ms; + if (rx_tow_at_preamble < 0) + { + rx_tow_at_preamble += 604800000; + } + uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000); // floor to integer number of seconds + std::cout << "TOW at PREAMBLE: " << d_TOW_at_Preamble_ms << " predicted TOW at preamble: " << predicted_tow_at_preamble_ms << " [ms]\n"; + } } else if (d_inav_nav.is_TOW0_set() == true) // page 0 arrived and decoded { @@ -888,6 +957,18 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_TOW_at_Preamble_ms = static_cast(d_inav_nav.get_TOW0() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); d_inav_nav.set_TOW0_flag(false); + // timetag debug + if (d_valid_timetag == true) + { + int decoder_delay_ms = static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); + int rx_tow_at_preamble = d_current_timetag.tow_ms - decoder_delay_ms; + if (rx_tow_at_preamble < 0) + { + rx_tow_at_preamble += 604800000; + } + uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000); // floor to integer number of seconds + std::cout << "TOW at PREAMBLE: " << d_TOW_at_Preamble_ms << " predicted TOW at preamble: " << predicted_tow_at_preamble_ms << " [ms]\n"; + } } else { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index e6d9d644f..4164be78f 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -26,6 +26,7 @@ #include "galileo_inav_message.h" // for Galileo_Inav_Message #include "gnss_block_interface.h" // for gnss_shared_ptr (adapts smart pointer type to GNU Radio version) #include "gnss_satellite.h" // for Gnss_Satellite +#include "gnss_time.h" // for GnssTime #include "nav_message_packet.h" // for Nav_Message_Packet #include "tlm_conf.h" // for Tlm_Conf #include // for boost::circular_buffer @@ -143,6 +144,8 @@ private: bool d_enable_navdata_monitor; bool d_dump_crc_stats; bool d_enable_reed_solomon_inav; + bool d_valid_timetag; + GnssTime d_current_timetag; }; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index c65edff8d..e4e661b8c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -2021,8 +2021,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) { - //std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. counter " << d_sample_counter << " vs. nread " << this->nitems_read(0) << " containing "; - //std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. nread " << this->nitems_read(0) << " containing "; + // std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. counter " << d_sample_counter << " vs. nread " << this->nitems_read(0) << " containing "; + // std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. nread " << this->nitems_read(0) << " containing "; const std::shared_ptr last_timetag = boost::any_cast>(pmt::any_ref(it->value)); d_last_timetag = *last_timetag; d_last_timetag_samplecounter = it->offset; @@ -2055,6 +2055,27 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) current_synchro_data.Flag_valid_symbol_output = !loss_of_lock; current_synchro_data.Flag_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked; *out[0] = current_synchro_data; + + // generate new tag associated with gnss-synchro object + + if (d_timetag_waiting == true) + { + int64_t diff_samplecount = uint64diff(current_synchro_data.Tracking_sample_counter, d_last_timetag_samplecounter); + + double intpart; + d_last_timetag.tow_ms_fraction = d_last_timetag.tow_ms_fraction + modf(1000.0 * static_cast(diff_samplecount) / d_trk_parameters.fs_in, &intpart); + + const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); + tmp_obj->week = d_last_timetag.week; + tmp_obj->tow_ms = d_last_timetag.tow_ms + static_cast(intpart); + tmp_obj->tow_ms_fraction = d_last_timetag.tow_ms_fraction; + tmp_obj->rx_time = static_cast(current_synchro_data.Tracking_sample_counter) / d_trk_parameters.fs_in; + add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(tmp_obj)); + + // std::cout << "[" << this->nitems_written(0) + 1 << "][diff_time: " << 1000.0 * static_cast(diff_samplecount) / d_trk_parameters.fs_in << "] Sent TimeTag Week: " << d_last_timetag.week << ", TOW: " << d_last_timetag.tow_ms << " [ms], TOW fraction: " << d_last_timetag.tow_ms_fraction << " [ms] \n"; + d_timetag_waiting = false; + } + return 1; } return 0;