mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 07:13:03 +00:00
Code Issues Releases Wiki Activity

Forget data from too old MID messages

Browse Source 
Do not process data if the MID was already printed (saves computation time and cluttering of repeated messages)
This commit is contained in:
Carles Fernandez
2021-11-20 14:36:29 +01:00
parent 617c1747b9
commit ec56450971
8 changed files with 97 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc
View File

@@ -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<float> 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<uint64_t>(current_symbol.fs), d_page_part_symbols.data(), d_frame_length_symbols);
break;
default:
return -1;

2
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h
View File

@@ -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<Viterbi_Decoder> d_viterbi;
std::vector<int32_t> d_preamble_samples;

55
src/core/libs/galileo_e6_has_msg_receiver.cc
View File

@@ -28,6 +28,7 @@
#include <algorithm> // for std::find, std::count
#include <cstddef> // for size_t
#include <iterator> // for std::back_inserter
#include <limits> // for std::numeric_limits
#include <sstream> // for std::stringstream
#include <stdexcept> // for std::out_of_range
#include <typeinfo> // for typeid
@@ -78,6 +79,10 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_
d_C_matrix = std::vector<std::vector<std::vector<uint8_t>>>(GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<std::vector<uint8_t>>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE))); // 32 x 255 x 53
d_M_matrix = std::vector<std::vector<uint8_t>>(GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE)); // HAS message matrix 32 x 53
d_received_pids = std::vector<std::vector<uint8_t>>(HAS_MSG_NUMBER_MESSAGE_IDS, std::vector<uint8_t>());
d_received_timestamps = std::vector<std::vector<uint64_t>>(HAS_MSG_NUMBER_MESSAGE_IDS, std::vector<uint64_t>());
d_printed_timestamps = std::vector<uint64_t>(HAS_MSG_NUMBER_MESSAGE_IDS, std::numeric_limits<uint64_t>::max());
d_printed_mids = std::vector<bool>(HAS_MSG_NUMBER_MESSAGE_IDS);
// Reserve memory to store masks
d_nsat_in_mask_id = std::vector<int>(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_HAS_data> galileo_e6_has_msg_receiver::process_test_page(const pmt::pmt_t& msg)
{
int64_t timestamp = std::numeric_limits<uint64_t>::max();
try
{
const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
@@ -119,8 +125,12 @@ std::shared_ptr<Galileo_HAS_data> galileo_e6_has_msg_receiver::process_test_page
<< "PID: " << static_cast<float>(HAS_data_page->message_page_id);
d_current_has_status = HAS_data_page->has_status;
d_current_message_id = HAS_data_page->message_id;
timestamp = HAS_data_page->time_stamp;
if (d_printed_mids[d_current_message_id] == false)
{
process_HAS_page(*HAS_data_page.get());
}
}
else
{
LOG(WARNING) << "galileo_e6_has_msg_receiver received an unknown object type!";
@@ -138,6 +148,8 @@ std::shared_ptr<Galileo_HAS_data> galileo_e6_has_msg_receiver::process_test_page
d_HAS_data.message_id = d_current_message_id;
auto has_data_ptr = std::make_shared<Galileo_HAS_data>(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_HAS_data> 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<uint64_t>::max();
try
{
const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
@@ -162,8 +174,12 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m
<< "PID: " << static_cast<float>(HAS_data_page->message_page_id);
d_current_has_status = HAS_data_page->has_status;
d_current_message_id = HAS_data_page->message_id;
timestamp = HAS_data_page->time_stamp;
if (d_printed_mids[d_current_message_id] == false)
{
process_HAS_page(*HAS_data_page.get());
}
}
else
{
LOG(WARNING) << "galileo_e6_has_msg_receiver received an unknown object type!";
@@ -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<Galileo_HAS_data>(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<uint64_t>::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<uint8_t>(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<uint64_t>::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";

4
src/core/libs/galileo_e6_has_msg_receiver.h
View File

@@ -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<std::vector<uint64_t>> d_received_timestamps;
std::vector<std::vector<std::vector<uint8_t>>> d_C_matrix;
std::vector<std::vector<uint8_t>> d_M_matrix;
std::vector<std::vector<uint8_t>> d_received_pids;
std::vector<uint64_t> d_printed_timestamps;
std::vector<bool> d_printed_mids;
// Store masks
std::vector<int> d_nsat_in_mask_id;

2
src/core/system_parameters/Galileo_CNAV.h
View File

@@ -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;

5
src/core/system_parameters/galileo_cnav_message.h
View File

@@ -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<GALILEO_CNAV_PAGE_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
bool CRC_test(const std::bitset<GALILEO_CNAV_BITS_FOR_CRC>& bits, uint32_t checksum) const;

1
src/core/system_parameters/galileo_has_page.h
View File

@@ -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

36
src/tests/unit-tests/system-parameters/has_decoding_test.cc
View File

@@ -31,7 +31,12 @@
#include <string>
#include <vector>
// 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<Galileo_HAS_page> generate_has_page(const std::string& page);
std::shared_ptr<Galileo_HAS_page> generate_has_page(const std::string& page, int rx_time);
~HasDecoderTester(); //!< Default destructor
};
@@ -64,7 +69,7 @@ HasDecoderTester_sptr HasDecoderTester_make()
}
std::shared_ptr<Galileo_HAS_page> HasDecoderTester::generate_has_page(const std::string& page)
std::shared_ptr<Galileo_HAS_page> HasDecoderTester::generate_has_page(const std::string& page, int rx_time)
{
auto gh = std::make_shared<Galileo_HAS_page>();
@@ -84,6 +89,7 @@ std::shared_ptr<Galileo_HAS_page> 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> 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> 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<Has_Simple_Printer>();
}
for (size_t p = 0; p < read_pages.get_number_pages(); p++)
if (static_cast<size_t>(FLAGS_start_page_test_file) < read_pages.get_number_pages())
{
auto has_page = has_tester->generate_has_page(pages[p]);
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 = init; p < read_pages.get_number_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));