mirror of https://github.com/gnss-sdr/gnss-sdr
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)
This commit is contained in:
parent
617c1747b9
commit
ec56450971
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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,7 +125,11 @@ 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;
|
||||
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_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,7 +174,11 @@ 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;
|
||||
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<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";
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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>();
|
||||
if (static_cast<size_t>(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));
|
||||
|
|
Loading…
Reference in New Issue