From ec564509715302b23a58c03c77dd11ac64910b4c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 20 Nov 2021 14:36:29 +0100 Subject: [PATCH] Forget data from too old MID messages Do not process data if the MID was already printed (saves computation time and cluttering of repeated messages) --- .../galileo_telemetry_decoder_gs.cc | 6 +- .../galileo_telemetry_decoder_gs.h | 2 +- src/core/libs/galileo_e6_has_msg_receiver.cc | 59 ++++++++++++++++++- src/core/libs/galileo_e6_has_msg_receiver.h | 4 ++ src/core/system_parameters/Galileo_CNAV.h | 2 + .../system_parameters/galileo_cnav_message.h | 5 ++ src/core/system_parameters/galileo_has_page.h | 1 + .../system-parameters/has_decoding_test.cc | 34 ++++++++--- 8 files changed, 97 insertions(+), 16 deletions(-) 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 1656c04a1..307b7077d 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 @@ -515,7 +515,7 @@ void galileo_telemetry_decoder_gs::decode_FNAV_word(float *page_symbols, int32_t } -void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t page_length) +void galileo_telemetry_decoder_gs::decode_CNAV_word(uint64_t time_stamp, float *page_symbols, int32_t page_length) { // 1. De-interleave std::vector page_symbols_soft_value(page_length); @@ -549,7 +549,7 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t } } d_cnav_nav.read_HAS_page(page_String); - + d_cnav_nav.set_time_stamp(time_stamp); // 4. If we have a new HAS page, read it if (d_cnav_nav.have_new_HAS_page() == true) { @@ -810,7 +810,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( decode_FNAV_word(d_page_part_symbols.data(), d_frame_length_symbols); break; case 3: // CNAV - decode_CNAV_word(d_page_part_symbols.data(), d_frame_length_symbols); + decode_CNAV_word(current_symbol.Tracking_sample_counter / static_cast(current_symbol.fs), d_page_part_symbols.data(), d_frame_length_symbols); break; default: return -1; 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..85b95c875 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 @@ -82,7 +82,7 @@ private: void deinterleaver(int32_t rows, int32_t cols, const float *in, float *out); void decode_INAV_word(float *page_part_symbols, int32_t frame_length); void decode_FNAV_word(float *page_symbols, int32_t frame_length); - void decode_CNAV_word(float *page_symbols, int32_t page_length); + void decode_CNAV_word(uint64_t time_stamp, float *page_symbols, int32_t page_length); std::unique_ptr d_viterbi; std::vector d_preamble_samples; diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index 8fae7611e..c0c78be48 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -28,6 +28,7 @@ #include // for std::find, std::count #include // for size_t #include // for std::back_inserter +#include // for std::numeric_limits #include // for std::stringstream #include // for std::out_of_range #include // for typeid @@ -78,6 +79,10 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_ d_C_matrix = std::vector>>(GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE))); // 32 x 255 x 53 d_M_matrix = std::vector>(GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)); // HAS message matrix 32 x 53 d_received_pids = std::vector>(HAS_MSG_NUMBER_MESSAGE_IDS, std::vector()); + d_received_timestamps = std::vector>(HAS_MSG_NUMBER_MESSAGE_IDS, std::vector()); + d_printed_timestamps = std::vector(HAS_MSG_NUMBER_MESSAGE_IDS, std::numeric_limits::max()); + d_printed_mids = std::vector(HAS_MSG_NUMBER_MESSAGE_IDS); + // Reserve memory to store masks d_nsat_in_mask_id = std::vector(HAS_MSG_NUMBER_MASK_IDS); @@ -105,6 +110,7 @@ void galileo_e6_has_msg_receiver::set_enable_navdata_monitor(bool enable) std::shared_ptr galileo_e6_has_msg_receiver::process_test_page(const pmt::pmt_t& msg) { + int64_t timestamp = std::numeric_limits::max(); try { const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); @@ -119,7 +125,11 @@ std::shared_ptr galileo_e6_has_msg_receiver::process_test_page << "PID: " << static_cast(HAS_data_page->message_page_id); d_current_has_status = HAS_data_page->has_status; d_current_message_id = HAS_data_page->message_id; - process_HAS_page(*HAS_data_page.get()); + timestamp = HAS_data_page->time_stamp; + if (d_printed_mids[d_current_message_id] == false) + { + process_HAS_page(*HAS_data_page.get()); + } } else { @@ -138,6 +148,8 @@ std::shared_ptr galileo_e6_has_msg_receiver::process_test_page d_HAS_data.message_id = d_current_message_id; auto has_data_ptr = std::make_shared(d_HAS_data); d_new_message = false; + d_printed_mids[d_current_message_id] = true; + d_printed_timestamps[d_current_message_id] = timestamp; return has_data_ptr; } return nullptr; @@ -147,7 +159,7 @@ std::shared_ptr galileo_e6_has_msg_receiver::process_test_page void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& msg) { gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_galileo_e6_has function called by the scheduler - + int64_t timestamp = std::numeric_limits::max(); try { const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); @@ -162,7 +174,11 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m << "PID: " << static_cast(HAS_data_page->message_page_id); d_current_has_status = HAS_data_page->has_status; d_current_message_id = HAS_data_page->message_id; - process_HAS_page(*HAS_data_page.get()); + timestamp = HAS_data_page->time_stamp; + if (d_printed_mids[d_current_message_id] == false) + { + process_HAS_page(*HAS_data_page.get()); + } } else { @@ -179,6 +195,8 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m { d_HAS_data.has_status = d_current_has_status; d_HAS_data.message_id = d_current_message_id; + d_printed_mids[d_current_message_id] = true; + d_printed_timestamps[d_current_message_id] = timestamp; auto has_data_ptr = std::make_shared(d_HAS_data); this->message_port_pub(pmt::mp("E6_HAS_to_PVT"), pmt::make_any(has_data_ptr)); d_new_message = false; @@ -196,12 +214,14 @@ void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_p { if (has_page.message_type == 1) // contains satellite corrections { + delete_outdated_data(has_page); if (has_page.message_id < HAS_MSG_NUMBER_MESSAGE_IDS) // MID range is from 0 to 31 { if (std::find(d_received_pids[has_page.message_id].begin(), d_received_pids[has_page.message_id].end(), has_page.message_page_id) == d_received_pids[has_page.message_id].end()) { // New pid! Annotate it. d_received_pids[has_page.message_id].push_back(has_page.message_page_id); + d_received_timestamps[has_page.message_id].push_back(has_page.time_stamp); for (int k = 0; k < GALILEO_CNAV_OCTETS_IN_SUBPAGE; k++) { constexpr int bits_in_octet = 8; @@ -237,6 +257,39 @@ void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_p } +void galileo_e6_has_msg_receiver::delete_outdated_data(const Galileo_HAS_page& has_page) +{ + const uint64_t current_time_stamp = has_page.time_stamp; + for (size_t i = 0; i < d_received_pids.size(); i++) + { + uint64_t oldest_time_stamp = std::numeric_limits::max(); + for (size_t j = 0; j < d_received_pids[i].size(); j++) + { + uint64_t timestamp = d_received_timestamps[i][j]; + if (timestamp > 0 && timestamp < oldest_time_stamp) + { + oldest_time_stamp = timestamp; + } + } + if (current_time_stamp > oldest_time_stamp && current_time_stamp - oldest_time_stamp > MAX_SECONDS_REMEMBERING_MID) + { + DLOG(INFO) << "Deleting data for message ID " << i << " because it is too old: " << oldest_time_stamp << " vs " << current_time_stamp; + d_received_pids[i].clear(); + d_received_timestamps[i].clear(); + d_C_matrix[i] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; + } + } + for (size_t mid = 0; mid < HAS_MSG_NUMBER_MESSAGE_IDS; mid++) + { + if (d_printed_mids[mid] == true && current_time_stamp > d_printed_timestamps[mid] && current_time_stamp - d_printed_timestamps[mid] > MAX_SECONDS_REMEMBERING_MID) + { + d_printed_timestamps[mid] = std::numeric_limits::max(); + d_printed_mids[mid] = false; + } + } +} + + int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_t message_size) { DLOG(INFO) << "Start decoding of a HAS message"; diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index da90593c6..e564ecdc7 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -67,6 +67,7 @@ private: void process_HAS_page(const Galileo_HAS_page& has_page); void read_MT1_header(const std::string& message_header); void read_MT1_body(const std::string& message_body); + void delete_outdated_data(const Galileo_HAS_page& has_page); int decode_message_type1(uint8_t message_id, uint8_t message_size); @@ -90,9 +91,12 @@ private: Nav_Message_Packet d_nav_msg_packet; // Store decoding matrices and received PIDs + std::vector> d_received_timestamps; std::vector>> d_C_matrix; std::vector> d_M_matrix; std::vector> d_received_pids; + std::vector d_printed_timestamps; + std::vector d_printed_mids; // Store masks std::vector d_nsat_in_mask_id; diff --git a/src/core/system_parameters/Galileo_CNAV.h b/src/core/system_parameters/Galileo_CNAV.h index da79aa883..a15549af5 100644 --- a/src/core/system_parameters/Galileo_CNAV.h +++ b/src/core/system_parameters/Galileo_CNAV.h @@ -74,6 +74,8 @@ constexpr size_t HAS_MSG_CODE_BIAS_LENGTH = 11; constexpr size_t HAS_MSG_PHASE_BIAS_LENGTH = 11; constexpr size_t HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH = 2; +constexpr uint64_t MAX_SECONDS_REMEMBERING_MID = 100; + constexpr int32_t HAS_MSG_NUMBER_MASK_IDS = 32; constexpr int32_t HAS_MSG_NUMBER_GNSS_IDS = 16; constexpr int32_t HAS_MSG_NUMBER_MESSAGE_IDS = 32; diff --git a/src/core/system_parameters/galileo_cnav_message.h b/src/core/system_parameters/galileo_cnav_message.h index 7df0e4a0a..588adcd8e 100644 --- a/src/core/system_parameters/galileo_cnav_message.h +++ b/src/core/system_parameters/galileo_cnav_message.h @@ -69,6 +69,11 @@ public: return d_flag_CRC_test; } + inline void set_time_stamp(uint64_t time_stamp) + { + has_page.time_stamp = time_stamp; + } + private: uint8_t read_has_page_header_parameter(const std::bitset& bits, const std::pair& parameter) const; bool CRC_test(const std::bitset& bits, uint32_t checksum) const; diff --git a/src/core/system_parameters/galileo_has_page.h b/src/core/system_parameters/galileo_has_page.h index be37c0ecd..7378f68a7 100644 --- a/src/core/system_parameters/galileo_has_page.h +++ b/src/core/system_parameters/galileo_has_page.h @@ -38,6 +38,7 @@ public: Galileo_HAS_page() = default; std::string has_message_string; //!< HAS message content + uint64_t time_stamp{}; //!< HAS page time stamp, in [s] // HAS page header uint8_t has_status{}; //!< HAS status diff --git a/src/tests/unit-tests/system-parameters/has_decoding_test.cc b/src/tests/unit-tests/system-parameters/has_decoding_test.cc index 60bd5995c..def3ca4af 100644 --- a/src/tests/unit-tests/system-parameters/has_decoding_test.cc +++ b/src/tests/unit-tests/system-parameters/has_decoding_test.cc @@ -31,7 +31,12 @@ #include #include + +// Usage: +// ./run_tests --gtest_filter=HAS_Test.Decoder +// ./run_tests --gtest_filter=HAS_Test.Decoder --has_data_test_file=../data/HAS_Messages_sample/encoded/Sample_HAS_Pages_Encoded_20210713_08.txt --start_page_test_file=70 DEFINE_string(has_data_test_file, std::string(""), "File containing encoded HAS pages (format: [time sat_id HAS_page_in_hex] in each line)"); +DEFINE_int32(start_page_test_file, 0, "Starting page in case of reading HAS pages from a file"); #if PMT_USES_BOOST_ANY namespace wht = boost; @@ -53,7 +58,7 @@ private: HasDecoderTester(); public: - std::shared_ptr generate_has_page(const std::string& page); + std::shared_ptr generate_has_page(const std::string& page, int rx_time); ~HasDecoderTester(); //!< Default destructor }; @@ -64,7 +69,7 @@ HasDecoderTester_sptr HasDecoderTester_make() } -std::shared_ptr HasDecoderTester::generate_has_page(const std::string& page) +std::shared_ptr HasDecoderTester::generate_has_page(const std::string& page, int rx_time) { auto gh = std::make_shared(); @@ -84,6 +89,7 @@ std::shared_ptr HasDecoderTester::generate_has_page(const std: { gh->has_message_string = bits.substr(24, 424); } + gh->time_stamp = rx_time; std::bitset<2> b_has_status(bits.substr(0, 2)); gh->has_status = b_has_status.to_ulong(); @@ -165,6 +171,7 @@ public: "07AC8553C81674CA989EE3B762BFE9F7113F9458A5FD2749D0B685A4F49012532088C872254C881194C7641762A7B9495A02BCD6686CE17F", "07AC987E7D3A9854AA56BCCD7170CB6939966DA4F2199A0C6C5F9CAB5B24539786CCB299DA69DE4EEE9698EEDD2D7BD409565C27674B4268", "07ACAB286D5CA9F01FEC5F5105132F0A41EFCFB5E970C06395B3FE72C3D3B476BADF27DC9CA50ED9EC997AB8BED648DF1424EE56FFAD35B1"}; + rx_time = {690883223, 690883223, 690883223, 690883223, 690883223, 690883224, 690883224, 690883224, 690883224, 690883224, 690883224, 690883224}; known_test_data = true; return true; } @@ -206,21 +213,30 @@ TEST(HAS_Test, Decoder) { Read_Encoded_Pages read_pages{}; EXPECT_TRUE(read_pages.read_data(FLAGS_has_data_test_file)); - galileo_e6_has_msg_receiver_sptr gal_e6_has_rx_; - gal_e6_has_rx_ = galileo_e6_has_msg_receiver_make(); - HasDecoderTester_sptr has_tester; - has_tester = HasDecoderTester_make(); - std::unique_ptr has_simple_printer; + auto gal_e6_has_rx_ = galileo_e6_has_msg_receiver_make(); + auto has_tester = HasDecoderTester_make(); + std::unique_ptr has_simple_printer = nullptr; auto pages = read_pages.get_pages(); + auto rx_time = read_pages.get_time(); bool known_data = read_pages.is_known_data(); + int init = 0; if (!known_data) { has_simple_printer = std::make_unique(); + if (static_cast(FLAGS_start_page_test_file) < read_pages.get_number_pages()) + { + init = FLAGS_start_page_test_file; + } + else + { + std::cerr << "The flag --start_page_test_file is set beyond the total number of pages in the file (" << read_pages.get_number_pages() << "), ignoring it.\n"; + } } - for (size_t p = 0; p < read_pages.get_number_pages(); p++) + + for (size_t p = init; p < read_pages.get_number_pages(); p++) { - auto has_page = has_tester->generate_has_page(pages[p]); + auto has_page = has_tester->generate_has_page(pages[p], rx_time[p]); if (!has_page->has_message_string.empty()) // if not dummy { auto has_message = gal_e6_has_rx_->process_test_page(pmt::make_any(has_page));