mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-27 07:23:15 +00:00
Code Issues Releases Wiki Activity

Implement HAS message reading

Browse Source
This commit is contained in:
Carles Fernandez 2021-09-20 08:53:23 +02:00
parent 531ca63398
commit 7a0cbd2503
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
3 changed files with 437 additions and 286 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3,8 +3,8 @@
* \brief GNU Radio block that processes Galileo HAS message pages received from * \brief GNU Radio block that processes Galileo HAS message pages received from
* Galileo E6B telemetry blocks. After successful decoding, sends the content to * Galileo E6B telemetry blocks. After successful decoding, sends the content to
* the PVT block. * the PVT block.
* \author Javier Arribas, 2021. jarribas(at)cttc.es
* \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es * \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es
* \author Javier Arribas, 2021. jarribas(at)cttc.es
* *
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* *
@ -19,19 +19,19 @@
#include "galileo_e6_has_msg_receiver.h" #include "galileo_e6_has_msg_receiver.h"
#include "display.h" #include "display.h" // for colors in terminal
#include "galileo_has_page.h" // for Galileo_HAS_page #include "galileo_has_page.h" // for Galileo_HAS_page
#include "gnss_sdr_make_unique.h" #include "gnss_sdr_make_unique.h" // for std::make_unique in C++11
#include "reed_solomon.h" #include "reed_solomon.h" // for ReedSolomon
#include <boost/any.hpp> #include <boost/any.hpp>
#include <glog/logging.h> #include <glog/logging.h>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <algorithm> // std::find #include <algorithm> // for std::find, std::count
#include <cstddef> // size_t #include <cstddef> // for size_t
#include <numeric> // std::accumulate #include <iterator> // for std::back_inserter
#include <sstream> // std::stringstream #include <sstream> // for std::stringstream
#include <stdexcept> // std::out_of_range #include <stdexcept> // for std::out_of_range
#include <typeinfo> // typeid #include <typeinfo> // for typeid
#if HAS_GENERIC_LAMBDA #if HAS_GENERIC_LAMBDA
#else #else
@ -68,6 +68,22 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_
// initialize Reed-Solomon decoder // initialize Reed-Solomon decoder
d_rs = std::make_unique<ReedSolomon>(); d_rs = std::make_unique<ReedSolomon>();
// Reserve memory for decoding matrices and received PIDs
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>());
// Reserve memory to store masks
d_nsat_in_mask_id = std::vector<int>(HAS_MSG_NUMBER_MASK_IDS);
d_gnss_id_in_mask = std::vector<std::vector<uint8_t>>(HAS_MSG_NUMBER_MASK_IDS, std::vector<uint8_t>(HAS_MSG_NUMBER_GNSS_IDS));
d_satellite_mask = std::vector<std::vector<uint64_t>>(HAS_MSG_NUMBER_MASK_IDS, std::vector<uint64_t>(HAS_MSG_NUMBER_GNSS_IDS));
d_signal_mask = std::vector<std::vector<uint16_t>>(HAS_MSG_NUMBER_MASK_IDS, std::vector<uint16_t>(HAS_MSG_NUMBER_GNSS_IDS));
d_cell_mask_availability_flag = std::vector<std::vector<bool>>(HAS_MSG_NUMBER_MASK_IDS, std::vector<bool>(HAS_MSG_NUMBER_GNSS_IDS));
d_cell_mask = std::vector<std::vector<std::vector<std::vector<bool>>>>(HAS_MSG_NUMBER_MASK_IDS, {HAS_MSG_NUMBER_GNSS_IDS, {HAS_MSG_NUMBER_SATELLITE_IDS, std::vector<bool>(HAS_MSG_NUMBER_SIGNAL_MASKS)}});
d_nsys_in_mask = std::vector<uint8_t>(HAS_MSG_NUMBER_MASK_IDS);
d_nav_message_mask = std::vector<std::vector<uint8_t>>(HAS_MSG_NUMBER_MASK_IDS, std::vector<uint8_t>(HAS_MSG_NUMBER_GNSS_IDS));
// Initialize values for d_nav_msg_packet
d_nav_msg_packet.system = std::string("E"); d_nav_msg_packet.system = std::string("E");
d_nav_msg_packet.signal = std::string("E6"); d_nav_msg_packet.signal = std::string("E6");
d_nav_msg_packet.prn = 0; d_nav_msg_packet.prn = 0;
@ -149,6 +165,7 @@ void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_p
} }
// If we have received for this message ID a number of pages equal to the message size // If we have received for this message ID a number of pages equal to the message size
d_new_message = false;
if (d_received_pids[has_page.message_id].size() == has_page.message_size) if (d_received_pids[has_page.message_id].size() == has_page.message_size)
{ {
// Try to decode the message // Try to decode the message
@ -159,11 +176,11 @@ void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_p
// Successful decoding, we have a valid HAS message stored at d_HAS_data // Successful decoding, we have a valid HAS message stored at d_HAS_data
std::cout << TEXT_MAGENTA << "New Galileo HAS message type " << static_cast<float>(has_page.message_id) std::cout << TEXT_MAGENTA << "New Galileo HAS message type " << static_cast<float>(has_page.message_id)
<< " received and successfully decoded" << TEXT_RESET << '\n'; << " received and successfully decoded" << TEXT_RESET << '\n';
d_new_message = true; if (d_nsat_in_mask_id[d_HAS_data.header.mask_id] != 0)
} {
else // if we have the mask for that message, it's ready to be sent to PVT
{ d_new_message = true;
d_new_message = false; }
} }
} }
} }
@ -206,13 +223,13 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_
msg += ss.str(); msg += ss.str();
LOG(ERROR) << msg; LOG(ERROR) << msg;
d_received_pids[message_id].clear(); d_received_pids[message_id].clear();
d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE)};
return -1; return -1;
} }
DLOG(INFO) << debug_print_vector("List of received PIDs", d_received_pids[message_id]); DLOG(INFO) << debug_print_vector("List of received PIDs", d_received_pids[message_id]);
DLOG(INFO) << debug_print_vector("erasure_positions", erasure_positions); DLOG(INFO) << debug_print_vector("erasure_positions", erasure_positions);
DLOG(INFO) << debug_print_matrix("d_C_matrix produced", d_C_matrix[message_id]); DLOG(INFO) << debug_print_matrix("C_matrix", d_C_matrix[message_id]);
// Reset HAS decoded message matrix // Reset HAS decoded message matrix
d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE)};
@ -220,14 +237,12 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_
// Vertical decoding of d_C_matrix // Vertical decoding of d_C_matrix
for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++) for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++)
{ {
std::vector<uint8_t> C_column(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, 0); std::vector<uint8_t> C_column(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK);
for (auto pid : d_received_pids[message_id]) for (auto pid : d_received_pids[message_id])
{ {
C_column[pid - 1] = d_C_matrix[message_id][pid - 1][col]; C_column[pid - 1] = d_C_matrix[message_id][pid - 1][col];
} }
DLOG(INFO) << debug_print_vector("C_column entering the decoder", C_column);
int result = d_rs->decode(C_column, erasure_positions); int result = d_rs->decode(C_column, erasure_positions);
if (result < 0) if (result < 0)
@ -235,13 +250,15 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_
DLOG(ERROR) << "Decoding of HAS page failed"; DLOG(ERROR) << "Decoding of HAS page failed";
return -1; return -1;
} }
DLOG(INFO) << "Successful HAS page decoding";
std::vector<uint8_t> M_column(C_column.begin(), C_column.begin() + GALILEO_CNAV_INFORMATION_VECTOR_LENGTH); std::vector<uint8_t> M_column(C_column.begin(), C_column.begin() + GALILEO_CNAV_INFORMATION_VECTOR_LENGTH);
for (int i = 0; i < GALILEO_CNAV_INFORMATION_VECTOR_LENGTH; i++) for (int i = 0; i < GALILEO_CNAV_INFORMATION_VECTOR_LENGTH; i++)
{ {
d_M_matrix[i][col] = M_column[i]; d_M_matrix[i][col] = M_column[i];
} }
DLOG(INFO) << debug_print_vector("C_column entering the decoder", C_column);
DLOG(INFO) << "Successful HAS page decoding";
} }
DLOG(INFO) << debug_print_matrix("M_matrix", d_M_matrix); DLOG(INFO) << debug_print_matrix("M_matrix", d_M_matrix);
@ -257,17 +274,13 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_
decoded_message_type_1 += bs.to_string(); decoded_message_type_1 += bs.to_string();
} }
} }
DLOG(INFO) << "Decoded message ID " << static_cast<float>(message_id) DLOG(INFO) << "Decoded message ID " << static_cast<float>(message_id)
<< " (size: " << static_cast<float>(message_size) << ") with body: " << " (size: " << static_cast<float>(message_size) << ") with header:\n"
<< std::string(decoded_message_type_1.begin(), decoded_message_type_1.begin() + GALILEO_CNAV_MT1_HEADER_BITS)
<< "\nand body:\n"
<< std::string(decoded_message_type_1.begin() + GALILEO_CNAV_MT1_HEADER_BITS, decoded_message_type_1.end()); << std::string(decoded_message_type_1.begin() + GALILEO_CNAV_MT1_HEADER_BITS, decoded_message_type_1.end());
// reset data for next decoding
d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)};
d_received_pids[message_id].clear();
// Trigger HAS message content reading and fill the d_HAS_data object
d_HAS_data = Galileo_HAS_data();
if (d_enable_navdata_monitor) if (d_enable_navdata_monitor)
{ {
d_nav_msg_packet.nav_message = decoded_message_type_1; d_nav_msg_packet.nav_message = decoded_message_type_1;
@ -275,6 +288,13 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_
this->message_port_pub(pmt::mp("Nav_msg_from_TLM"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("Nav_msg_from_TLM"), pmt::make_any(tmp_obj));
} }
// reset data for next decoding
d_C_matrix[message_id] = std::vector<std::vector<uint8_t>>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE));
d_received_pids[message_id].clear();
// Trigger HAS message content reading and fill the d_HAS_data object
d_HAS_data = Galileo_HAS_data();
read_MT1_header(decoded_message_type_1.substr(0, GALILEO_CNAV_MT1_HEADER_BITS)); read_MT1_header(decoded_message_type_1.substr(0, GALILEO_CNAV_MT1_HEADER_BITS));
try try
@ -283,12 +303,22 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_
} }
catch (const std::out_of_range& oor) catch (const std::out_of_range& oor)
{ {
std::cerr << "Out of Range error when reading HAS messages: " << oor.what() << '\n'; std::cerr << "Error when reading decoded HAS data. Wrong data formatting? The error was: " << oor.what() << '\n';
return -1; return -1;
} }
catch (const std::bad_alloc& e) catch (const std::bad_alloc& e)
{ {
std::cerr << "Allocation failed when reading HAS messages: " << e.what() << '\n'; std::cerr << "Error when reading decoded HAS data. Wrong data formatting? The error was: " << e.what() << '\n';
return -1;
}
catch (const std::exception& e)
{
std::cerr << "Error when reading decoded HAS data. Wrong data formatting? The error was: " << e.what() << '\n';
return -1;
}
catch (...)
{
std::cerr << "Error when reading decoded HAS data. Wrong data formatting?\n";
return -1; return -1;
} }
return 0; return 0;
@ -329,41 +359,44 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
// ICD v1.2 Table 7: MT1 Message Body. // ICD v1.2 Table 7: MT1 Message Body.
auto message = std::string(message_body); auto message = std::string(message_body);
int Nsat = 0; int Nsat = 0;
bool have_mask = false;
if (d_HAS_data.header.mask_flag) if (d_HAS_data.header.mask_flag)
{ {
DLOG(INFO) << "TOH: " << static_cast<float>(d_HAS_data.header.toh);
// read mask // read mask
d_HAS_data.Nsys = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_LENGTH)); d_HAS_data.Nsys = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_LENGTH));
DLOG(INFO) << "Nsys " << static_cast<float>(d_HAS_data.Nsys);
d_nsys_in_mask[d_HAS_data.header.mask_id] = d_HAS_data.Nsys; d_nsys_in_mask[d_HAS_data.header.mask_id] = d_HAS_data.Nsys;
if (d_HAS_data.Nsys != 0) if (d_HAS_data.Nsys != 0)
{ {
message = std::string(message.begin() + HAS_MSG_NSYS_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_NSYS_LENGTH, message.end());
d_HAS_data.gnss_id_mask.reserve(d_HAS_data.Nsys); d_HAS_data.gnss_id_mask = std::vector<uint8_t>(d_HAS_data.Nsys);
d_HAS_data.cell_mask = {d_HAS_data.Nsys, std::vector<std::vector<bool>>(40, std::vector<bool>(16, false))}; d_HAS_data.cell_mask = {d_HAS_data.Nsys, std::vector<std::vector<bool>>(40, std::vector<bool>(16))};
d_HAS_data.cell_mask_availability_flag.reserve(d_HAS_data.Nsys); d_HAS_data.cell_mask_availability_flag = std::vector<bool>(d_HAS_data.Nsys);
d_HAS_data.nav_message.reserve(d_HAS_data.Nsys); d_HAS_data.nav_message = std::vector<uint8_t>(d_HAS_data.Nsys);
d_HAS_data.satellite_mask.reserve(d_HAS_data.Nsys); d_HAS_data.satellite_mask = std::vector<uint64_t>(d_HAS_data.Nsys);
d_HAS_data.signal_mask.reserve(d_HAS_data.Nsys); d_HAS_data.signal_mask = std::vector<uint16_t>(d_HAS_data.Nsys);
for (uint8_t i = 0; i < d_HAS_data.Nsys; i++) for (uint8_t i = 0; i < d_HAS_data.Nsys; i++)
{ {
d_HAS_data.gnss_id_mask[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_MASK_LENGTH)); d_HAS_data.gnss_id_mask[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_MASK_LENGTH));
DLOG(INFO) << "GNSS ID" << static_cast<float>(i) << ": " << static_cast<float>(d_HAS_data.gnss_id_mask[i]);
d_gnss_id_in_mask[d_HAS_data.header.mask_id][i] = d_HAS_data.gnss_id_mask[i]; d_gnss_id_in_mask[d_HAS_data.header.mask_id][i] = d_HAS_data.gnss_id_mask[i];
message = std::string(message.begin() + HAS_MSG_ID_MASK_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_ID_MASK_LENGTH, message.end());
std::string msg = message.substr(0, HAS_MSG_SATELLITE_MASK_LENGTH); std::string msg = message.substr(0, HAS_MSG_SATELLITE_MASK_LENGTH);
d_HAS_data.satellite_mask[i] = read_has_message_body_uint64(msg); d_HAS_data.satellite_mask[i] = read_has_message_body_uint64(msg);
d_satellite_mask[d_HAS_data.header.mask_id][i] = d_HAS_data.satellite_mask[i];
int ones_in_satellite_mask = std::count(msg.begin(), msg.end(), '1'); int ones_in_satellite_mask = std::count(msg.begin(), msg.end(), '1');
Nsat += ones_in_satellite_mask; Nsat += ones_in_satellite_mask;
message = std::string(message.begin() + HAS_MSG_SATELLITE_MASK_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_SATELLITE_MASK_LENGTH, message.end());
msg = message.substr(0, HAS_MSG_SIGNAL_MASK_LENGTH); msg = message.substr(0, HAS_MSG_SIGNAL_MASK_LENGTH);
d_HAS_data.signal_mask[i] = read_has_message_body_uint16(msg); d_HAS_data.signal_mask[i] = read_has_message_body_uint16(msg);
d_signal_mask[d_HAS_data.header.mask_id][i] = d_HAS_data.signal_mask[i];
int ones_in_signal_mask = std::count(msg.begin(), msg.end(), '1'); int ones_in_signal_mask = std::count(msg.begin(), msg.end(), '1');
message = std::string(message.begin() + HAS_MSG_SIGNAL_MASK_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_SIGNAL_MASK_LENGTH, message.end());
d_HAS_data.cell_mask[i] = std::vector<std::vector<bool>>(ones_in_satellite_mask, std::vector<bool>(ones_in_signal_mask, false));
if (message.substr(0, 1) == "1") if (message.substr(0, 1) == "1")
{ {
d_HAS_data.cell_mask_availability_flag[i] = true; d_HAS_data.cell_mask_availability_flag[i] = true;
@ -372,6 +405,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
{ {
d_HAS_data.cell_mask_availability_flag[i] = false; d_HAS_data.cell_mask_availability_flag[i] = false;
} }
d_cell_mask_availability_flag[d_HAS_data.header.mask_id][i] = d_HAS_data.cell_mask_availability_flag[i];
message = std::string(message.begin() + 1, message.end()); message = std::string(message.begin() + 1, message.end());
int size_cell = ones_in_satellite_mask * ones_in_signal_mask; int size_cell = ones_in_satellite_mask * ones_in_signal_mask;
@ -387,28 +421,72 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
message = std::string(message.begin() + size_cell, message.end()); message = std::string(message.begin() + size_cell, message.end());
d_HAS_data.nav_message[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_NAV_MESSAGE_LENGTH)); d_HAS_data.nav_message[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_NAV_MESSAGE_LENGTH));
d_nav_message_mask[d_HAS_data.header.mask_id][i] = d_HAS_data.nav_message[i];
message = std::string(message.begin() + HAS_MSG_NAV_MESSAGE_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_NAV_MESSAGE_LENGTH, message.end());
} }
d_nsat_in_mask_id[d_HAS_data.header.mask_id] = Nsat; d_nsat_in_mask_id[d_HAS_data.header.mask_id] = Nsat;
d_cell_mask[d_HAS_data.header.mask_id] = d_HAS_data.cell_mask;
message = std::string(message.begin() + HAS_MSG_MASK_RESERVED_LENGTH, message.end());
} }
if (Nsat != 0)
{
have_mask = true;
}
DLOG(INFO) << "Nsys: " << static_cast<float>(d_HAS_data.Nsys);
DLOG(INFO) << debug_print_vector("GNSS ID", d_HAS_data.gnss_id_mask);
DLOG(INFO) << debug_print_vector("cell_mask_availability_flag", d_HAS_data.cell_mask_availability_flag);
// for (uint8_t k = 0; k < d_HAS_data.Nsys; k++)
// {
// std::string aux = "cell_mask " + std::to_string(k);
// DLOG(INFO) << debug_print_matrix(aux, d_HAS_data.cell_mask[k]);
// }
DLOG(INFO) << debug_print_vector("nav_message", d_HAS_data.nav_message);
} }
else else
{ {
// Take data from a previously received mask with the same mask_id // Take data from a previously received mask with the same mask_id
Nsat = d_nsat_in_mask_id[d_HAS_data.header.mask_id]; Nsat = d_nsat_in_mask_id[d_HAS_data.header.mask_id];
d_HAS_data.gnss_id_mask = d_gnss_id_in_mask[d_HAS_data.header.mask_id]; if (Nsat != 0)
d_HAS_data.Nsys = d_nsys_in_mask[d_HAS_data.header.mask_id]; {
d_HAS_data.Nsys = d_nsys_in_mask[d_HAS_data.header.mask_id];
std::copy(d_gnss_id_in_mask[d_HAS_data.header.mask_id].begin(), d_gnss_id_in_mask[d_HAS_data.header.mask_id].begin() + d_HAS_data.Nsys, std::back_inserter(d_HAS_data.gnss_id_mask));
std::copy(d_satellite_mask[d_HAS_data.header.mask_id].begin(), d_satellite_mask[d_HAS_data.header.mask_id].begin() + d_HAS_data.Nsys, std::back_inserter(d_HAS_data.satellite_mask));
std::copy(d_signal_mask[d_HAS_data.header.mask_id].begin(), d_signal_mask[d_HAS_data.header.mask_id].begin() + d_HAS_data.Nsys, std::back_inserter(d_HAS_data.signal_mask));
std::copy(d_cell_mask_availability_flag[d_HAS_data.header.mask_id].begin(), d_cell_mask_availability_flag[d_HAS_data.header.mask_id].begin() + d_HAS_data.Nsys, std::back_inserter(d_HAS_data.cell_mask_availability_flag));
d_HAS_data.cell_mask = d_cell_mask[d_HAS_data.header.mask_id];
std::copy(d_nav_message_mask[d_HAS_data.header.mask_id].begin(), d_nav_message_mask[d_HAS_data.header.mask_id].begin() + d_HAS_data.Nsys, std::back_inserter(d_HAS_data.nav_message));
have_mask = true;
DLOG(INFO) << "Nsys: " << static_cast<float>(d_HAS_data.Nsys);
DLOG(INFO) << debug_print_vector("GNSS ID", d_HAS_data.gnss_id_mask);
DLOG(INFO) << debug_print_vector("cell_mask_availability_flag", d_HAS_data.cell_mask_availability_flag);
// for (uint8_t k = 0; k < d_HAS_data.Nsys; k++)
// {
// std::string aux = "cell_mask " + std::to_string(k);
// DLOG(INFO) << debug_print_matrix(aux, d_HAS_data.cell_mask[k]);
// }
DLOG(INFO) << debug_print_vector("nav_message", d_HAS_data.nav_message);
}
} }
if (d_HAS_data.header.orbit_correction_flag && (Nsat != 0) && std::count(d_HAS_data.gnss_id_mask.begin(), d_HAS_data.gnss_id_mask.end(), 1) != 0) // discard data if crazy Values
if (d_HAS_data.header.toh > HAS_MSG_NUMBER_MAX_TOH)
{
have_mask = false;
d_nsat_in_mask_id[d_HAS_data.header.mask_id] = 0;
}
if (d_HAS_data.header.orbit_correction_flag && have_mask)
{ {
// read orbit corrections // read orbit corrections
d_HAS_data.validity_interval_index_orbit_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); d_HAS_data.validity_interval_index_orbit_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
d_HAS_data.gnss_iod.reserve(Nsat); d_HAS_data.gnss_iod = std::vector<uint16_t>(Nsat);
d_HAS_data.delta_radial.reserve(Nsat); d_HAS_data.delta_radial = std::vector<int16_t>(Nsat);
d_HAS_data.delta_along_track.reserve(Nsat); d_HAS_data.delta_along_track = std::vector<int16_t>(Nsat);
d_HAS_data.delta_cross_track.reserve(Nsat); d_HAS_data.delta_cross_track = std::vector<int16_t>(Nsat);
for (int i = 0; i < Nsat; i++) for (int i = 0; i < Nsat; i++)
{ {
if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GPS_SYSTEM) if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GPS_SYSTEM)
@ -430,25 +508,28 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
d_HAS_data.delta_cross_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CROSS_TRACK_LENGTH)); d_HAS_data.delta_cross_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CROSS_TRACK_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_CROSS_TRACK_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_DELTA_CROSS_TRACK_LENGTH, message.end());
} }
DLOG(INFO) << debug_print_vector("gnss_iod", d_HAS_data.gnss_iod);
DLOG(INFO) << debug_print_vector("delta_radial", d_HAS_data.delta_radial);
DLOG(INFO) << debug_print_vector("delta_along_track", d_HAS_data.delta_along_track);
DLOG(INFO) << debug_print_vector("delta_cross_track", d_HAS_data.delta_cross_track);
} }
if (d_HAS_data.header.clock_fullset_flag && (Nsat != 0) && d_HAS_data.Nsys != 0 && std::count(d_HAS_data.gnss_id_mask.begin(), d_HAS_data.gnss_id_mask.end(), 1) != 0) if (d_HAS_data.header.clock_fullset_flag && have_mask)
{ {
// read clock full-set corrections // read clock full-set corrections
d_HAS_data.validity_interval_index_clock_fullset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); d_HAS_data.validity_interval_index_clock_fullset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
d_HAS_data.delta_clock_c0_multiplier.reserve(d_HAS_data.Nsys); d_HAS_data.delta_clock_c0_multiplier = std::vector<uint8_t>(d_HAS_data.Nsys);
for (uint8_t i = 0; i < d_HAS_data.Nsys; i++) for (uint8_t i = 0; i < d_HAS_data.Nsys; i++)
{ {
if (d_HAS_data.gnss_id_mask[i] != HAS_MSG_GALILEO_SYSTEM) d_HAS_data.delta_clock_c0_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH));
{ message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH, message.end());
d_HAS_data.delta_clock_c0_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH, message.end());
}
} }
d_HAS_data.iod_change_flag.reserve(Nsat);
d_HAS_data.delta_clock_c0.reserve(Nsat); d_HAS_data.iod_change_flag = std::vector<bool>(Nsat);
d_HAS_data.delta_clock_c0 = std::vector<int16_t>(Nsat);
for (int i = 0; i < Nsat; i++) for (int i = 0; i < Nsat; i++)
{ {
d_HAS_data.iod_change_flag[i] = (message[0] == '1' ? true : false); d_HAS_data.iod_change_flag[i] = (message[0] == '1' ? true : false);
@ -456,166 +537,211 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
d_HAS_data.delta_clock_c0[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_LENGTH)); d_HAS_data.delta_clock_c0[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_LENGTH, message.end());
} }
DLOG(INFO) << debug_print_vector("delta_clock_c0_multiplier", d_HAS_data.delta_clock_c0_multiplier);
DLOG(INFO) << debug_print_vector("delta_clock_c0", d_HAS_data.delta_clock_c0);
} }
//
// if (d_HAS_data.header.clock_subset_flag) if (d_HAS_data.header.clock_subset_flag && have_mask)
// { {
// // read clock subset corrections // read clock subset corrections
// d_HAS_data.validity_interval_index_clock_subset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); d_HAS_data.validity_interval_index_clock_subset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
// message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
//
// d_HAS_data.Nsysprime = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYSPRIME_LENGTH)); d_HAS_data.Nsysprime = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYSPRIME_LENGTH));
// DLOG(INFO) << "Nsysprime: " << static_cast<float>(d_HAS_data.Nsysprime); message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end());
// message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end());
// if (d_HAS_data.Nsysprime == 0)
// d_HAS_data.gnss_id_clock_subset.reserve(d_HAS_data.Nsysprime); {
// d_HAS_data.delta_clock_c0_multiplier_clock_subset.reserve(d_HAS_data.Nsysprime); // Wrong formatted data, aborting
// d_HAS_data.satellite_submask.reserve(d_HAS_data.Nsysprime); have_mask = false;
// d_HAS_data.iod_change_flag_clock_subset.reserve(d_HAS_data.Nsysprime); d_nsat_in_mask_id[d_HAS_data.header.mask_id] = 0;
// d_HAS_data.delta_clock_c0_clock_subset.reserve(d_HAS_data.Nsysprime); }
// for (uint8_t i = 0; i < d_HAS_data.Nsysprime; i++)
// { d_HAS_data.gnss_id_clock_subset = std::vector<uint8_t>(d_HAS_data.Nsysprime);
// d_HAS_data.gnss_id_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_CLOCK_SUBSET_LENGTH)); d_HAS_data.delta_clock_c0_multiplier_clock_subset = std::vector<uint8_t>(d_HAS_data.Nsysprime);
// message = std::string(message.begin() + HAS_MSG_ID_CLOCK_SUBSET_LENGTH, message.end()); d_HAS_data.satellite_submask = std::vector<std::vector<uint64_t>>(d_HAS_data.Nsysprime, std::vector<uint64_t>(HAS_MSG_NUMBER_SATELLITE_IDS));
// if (d_HAS_data.gnss_id_clock_subset[i] != HAS_MSG_GALILEO_SYSTEM) d_HAS_data.iod_change_flag_clock_subset = std::vector<bool>(d_HAS_data.Nsysprime);
// { d_HAS_data.delta_clock_c0_clock_subset = std::vector<int16_t>(d_HAS_data.Nsysprime);
// d_HAS_data.delta_clock_c0_multiplier_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH)); for (uint8_t i = 0; i < d_HAS_data.Nsysprime; i++)
// message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH, message.end()); {
// } d_HAS_data.gnss_id_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_CLOCK_SUBSET_LENGTH));
// uint64_t number_sats_this_gnss_id = 0; message = std::string(message.begin() + HAS_MSG_ID_CLOCK_SUBSET_LENGTH, message.end());
// for (uint8_t j = 0; j < d_HAS_data.Nsys; j++)
// { uint8_t clock_multiplier = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH));
// if (d_HAS_data.gnss_id_mask[j] == d_HAS_data.gnss_id_clock_subset[i]) d_HAS_data.delta_clock_c0_multiplier_clock_subset[i] = clock_multiplier + 1;
// { message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH, message.end());
// uint64_t n = d_HAS_data.satellite_mask[j];
// while (n) uint64_t satellite_mask = d_HAS_data.satellite_mask[i];
// { std::bitset<40> satellite_mask_bits(satellite_mask);
// number_sats_this_gnss_id += n & 1; std::string satellite_mask_string = satellite_mask_bits.to_string();
// n >>= 1; int number_sats_this_gnss_id = std::count(satellite_mask_string.begin(), satellite_mask_string.end(), '1');
// } d_HAS_data.satellite_submask[i] = std::vector<uint64_t>(number_sats_this_gnss_id);
// break; for (int j = 0; j < number_sats_this_gnss_id; j++)
// } {
// } d_HAS_data.satellite_submask[i][j] = read_has_message_body_uint64(message.substr(0, 1));
// message = std::string(message.begin() + 1, message.end());
// d_HAS_data.satellite_submask[i].reserve(number_sats_this_gnss_id); }
// for (uint64_t j = 0; j < number_sats_this_gnss_id; j++)
// { int Nsatprime = std::count(d_HAS_data.satellite_submask[i].begin(), d_HAS_data.satellite_submask[i].end(), 1UL);
// d_HAS_data.satellite_submask[i][j] = read_has_message_body_uint64(message.substr(0, 1)); for (int j = 0; j < Nsatprime; j++)
// message = std::string(message.begin() + 1, message.end()); {
// } d_HAS_data.delta_clock_c0_clock_subset[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH));
// d_HAS_data.iod_change_flag_clock_subset[i] = (message[0] == '1' ? true : false); message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH, message.end());
// message = std::string(message.begin() + 1, message.end()); }
// }
// d_HAS_data.delta_clock_c0_clock_subset[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH));
// message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH, message.end());
// } DLOG(INFO) << "Nsysprime: " << static_cast<float>(d_HAS_data.Nsysprime);
// } DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("gnss_id_clock_subset", d_HAS_data.gnss_id_clock_subset));
// DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("delta_clock_c0_multiplier_clock_subset", d_HAS_data.delta_clock_c0_multiplier_clock_subset));
// if (d_HAS_data.header.code_bias_flag) DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_matrix("satellite_submask", d_HAS_data.satellite_submask));
// { DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("delta_clock_c0_clock_subset", d_HAS_data.delta_clock_c0_clock_subset));
// // read code bias }
// d_HAS_data.validity_interval_index_code_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
// message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); if (d_HAS_data.header.code_bias_flag && have_mask)
// std::vector<uint64_t> number_sats(d_HAS_data.Nsys, 0); {
// std::vector<uint64_t> number_codes(d_HAS_data.Nsys, 0); // read code bias
// for (int sys = 0; sys < d_HAS_data.Nsys; sys++) d_HAS_data.validity_interval_index_code_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
// { message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
// uint64_t number_sats_this_gnss_id = 0;
// uint64_t number_signals_this_gnss_id = 0; std::vector<uint64_t> number_sats(d_HAS_data.Nsys);
// if (d_HAS_data.cell_mask_availability_flag[sys] == true) std::vector<uint64_t> number_codes(d_HAS_data.Nsys);
// { uint64_t max_signals = 0ULL;
// uint64_t n = d_HAS_data.satellite_mask[sys]; for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
// while (n) {
// { uint64_t number_sats_this_gnss_id = 0;
// number_sats_this_gnss_id += n & 1; uint64_t number_signals_this_gnss_id = 0;
// n >>= 1; if (d_HAS_data.cell_mask_availability_flag[sys] == true)
// } {
// uint64_t m = d_HAS_data.signal_mask[sys]; // cell mask is provided
// while (m) number_sats_this_gnss_id = d_HAS_data.cell_mask[sys].size();
// { number_signals_this_gnss_id = d_HAS_data.cell_mask[sys][0].size();
// number_signals_this_gnss_id += m & 1;
// m >>= 1; if (number_signals_this_gnss_id > max_signals)
// } {
// } max_signals = number_signals_this_gnss_id;
// else }
// { }
// number_sats_this_gnss_id = HAS_MSG_MAX_SATS; else
// number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; {
// } // corrections for all satellites in satellite_mask
// number_sats[sys] = number_sats_this_gnss_id; // and all signals in signal mask
// number_codes[sys] = number_signals_this_gnss_id; uint64_t n = d_HAS_data.satellite_mask[sys];
// } while (n)
// uint64_t Nsat_b = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); {
// number_sats_this_gnss_id += n & 1;
// d_HAS_data.code_bias.reserve(Nsat_b); n >>= 1;
// int sat = 0; }
// for (int sys = 0; sys < d_HAS_data.Nsys; sys++) uint64_t m = d_HAS_data.signal_mask[sys];
// { while (m)
// d_HAS_data.code_bias[sat].reserve(number_codes[sys]); {
// for (uint64_t c = 0; c < number_codes[sys]; c++) number_signals_this_gnss_id += m & 1;
// { m >>= 1;
// d_HAS_data.code_bias[sat][c] = read_has_message_body_int16(message.substr(0, HAS_MSG_CODE_BIAS_LENGTH)); }
// message = std::string(message.begin() + HAS_MSG_CODE_BIAS_LENGTH, message.end()); if (number_signals_this_gnss_id > max_signals)
// sat += 1; {
// } max_signals = number_signals_this_gnss_id;
// } }
// } }
// if (d_HAS_data.header.phase_bias_flag) number_sats[sys] = number_sats_this_gnss_id;
// { number_codes[sys] = number_signals_this_gnss_id;
// // read phase bias }
// d_HAS_data.validity_interval_index_phase_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
// message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); d_HAS_data.code_bias = std::vector<std::vector<int16_t>>(Nsat, std::vector<int16_t>(max_signals));
//
// std::vector<uint64_t> number_sats(d_HAS_data.Nsys, 0); int sat = 0;
// std::vector<uint64_t> number_phases(d_HAS_data.Nsys, 0); for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
// for (int sys = 0; sys < d_HAS_data.Nsys; sys++) {
// { for (uint64_t s = 0; s < number_sats[sys]; s++)
// uint64_t number_sats_this_gnss_id = 0; {
// uint64_t number_signals_this_gnss_id = 0; for (uint64_t c = 0; c < number_codes[sys]; c++)
// if (d_HAS_data.cell_mask_availability_flag[sys] == true) {
// { if ((d_HAS_data.cell_mask_availability_flag[sys] == false) || ((d_HAS_data.cell_mask_availability_flag[sys] == true) && (d_HAS_data.cell_mask[sys][s][c])))
// uint64_t n = d_HAS_data.satellite_mask[sys]; {
// while (n) d_HAS_data.code_bias[sat][c] = read_has_message_body_int16(message.substr(0, HAS_MSG_CODE_BIAS_LENGTH));
// { message = std::string(message.begin() + HAS_MSG_CODE_BIAS_LENGTH, message.end());
// number_sats_this_gnss_id += n & 1; }
// n >>= 1; }
// } sat += 1;
// uint64_t m = d_HAS_data.signal_mask[sys]; }
// while (m) }
// {
// number_signals_this_gnss_id += m & 1; DLOG(INFO) << debug_print_matrix("code bias", d_HAS_data.code_bias);
// m >>= 1; }
// }
// } if (d_HAS_data.header.phase_bias_flag && have_mask)
// else {
// { // read phase bias
// number_sats_this_gnss_id = HAS_MSG_MAX_SATS; d_HAS_data.validity_interval_index_phase_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
// number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
// }
// number_sats[sys] = number_sats_this_gnss_id; std::vector<uint64_t> number_sats(d_HAS_data.Nsys);
// number_phases[sys] = number_signals_this_gnss_id; std::vector<uint64_t> number_phases(d_HAS_data.Nsys);
// } uint64_t max_signals = 0ULL;
// uint64_t Nsat_p = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
// {
// d_HAS_data.phase_bias.reserve(Nsat_p); uint64_t number_sats_this_gnss_id = 0;
// d_HAS_data.phase_discontinuity_indicator.reserve(Nsat_p); uint64_t number_signals_this_gnss_id = 0;
// int sat = 0; if (d_HAS_data.cell_mask_availability_flag[sys] == true)
// for (int sys = 0; sys < d_HAS_data.Nsys; sys++) {
// { // cell mask is provided
// d_HAS_data.phase_bias[sat].reserve(number_phases[sys]); number_sats_this_gnss_id = d_HAS_data.cell_mask[sys].size();
// d_HAS_data.phase_discontinuity_indicator[sat].reserve(number_phases[sys]); number_signals_this_gnss_id = d_HAS_data.cell_mask[sys][0].size();
// for (uint64_t p = 0; p < number_phases[sys]; p++)
// { if (number_signals_this_gnss_id > max_signals)
// d_HAS_data.phase_bias[sat][p] = read_has_message_body_int16(message.substr(0, HAS_MSG_PHASE_BIAS_LENGTH)); {
// message = std::string(message.begin() + HAS_MSG_PHASE_BIAS_LENGTH, message.end()); max_signals = number_signals_this_gnss_id;
// }
// d_HAS_data.phase_discontinuity_indicator[sat][p] = read_has_message_body_uint8(message.substr(0, HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH)); }
// message = std::string(message.begin() + HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH, message.end()); else
// sat += 1; {
// } // corrections for all satellites in satellite_mask
// } // and all signals in signal mask
// } uint64_t n = d_HAS_data.satellite_mask[sys];
// if (d_HAS_data.header.ura_flag) while (n)
{
number_sats_this_gnss_id += n & 1;
n >>= 1;
}
uint64_t m = d_HAS_data.signal_mask[sys];
while (m)
{
number_signals_this_gnss_id += m & 1;
m >>= 1;
}
if (number_signals_this_gnss_id > max_signals)
{
max_signals = number_signals_this_gnss_id;
}
}
number_sats[sys] = number_sats_this_gnss_id;
number_phases[sys] = number_signals_this_gnss_id;
}
d_HAS_data.phase_bias = std::vector<std::vector<int16_t>>(Nsat, std::vector<int16_t>(max_signals));
int sat = 0;
for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
{
for (uint64_t s = 0; s < number_sats[sys]; s++)
{
for (uint64_t p = 0; p < number_phases[sys]; p++)
{
if ((d_HAS_data.cell_mask_availability_flag[sys] == false) || ((d_HAS_data.cell_mask_availability_flag[sys] == true) && (d_HAS_data.cell_mask[sys][s][p])))
{
d_HAS_data.phase_bias[sat][p] = read_has_message_body_int16(message.substr(0, HAS_MSG_CODE_BIAS_LENGTH));
message = std::string(message.begin() + HAS_MSG_CODE_BIAS_LENGTH, message.end());
}
}
sat += 1;
}
}
DLOG(INFO) << debug_print_matrix("phase bias", d_HAS_data.phase_bias);
}
// if (d_HAS_data.header.ura_flag && have_mask)
// { // {
// // read URA // // read URA
// d_HAS_data.validity_interval_index_ura_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); // d_HAS_data.validity_interval_index_ura_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
@ -630,9 +756,9 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
} }
uint8_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint8(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const uint16_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint16(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const
{ {
uint8_t value = 0U; uint16_t value = 0U;
for (int j = 0; j < parameter.second; j++) for (int j = 0; j < parameter.second; j++)
{ {
value <<= 1U; // shift left value <<= 1U; // shift left
@ -645,9 +771,9 @@ uint8_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint8(con
} }
uint16_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint16(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const uint8_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint8(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const
{ {
uint16_t value = 0U; uint8_t value = 0U;
for (int j = 0; j < parameter.second; j++) for (int j = 0; j < parameter.second; j++)
{ {
value <<= 1U; // shift left value <<= 1U; // shift left
@ -671,10 +797,11 @@ bool galileo_e6_has_msg_receiver::read_has_message_header_parameter_bool(const s
} }
uint8_t galileo_e6_has_msg_receiver::read_has_message_body_uint8(const std::string& bits) const uint64_t galileo_e6_has_msg_receiver::read_has_message_body_uint64(const std::string& bits) const
{ {
uint8_t value = 0U; uint64_t value = 0U;
const size_t len = bits.length(); const size_t len = bits.length();
for (size_t j = 0; j < len; j++) for (size_t j = 0; j < len; j++)
{ {
value <<= 1U; // shift left value <<= 1U; // shift left
@ -704,23 +831,6 @@ uint16_t galileo_e6_has_msg_receiver::read_has_message_body_uint16(const std::st
} }
uint64_t galileo_e6_has_msg_receiver::read_has_message_body_uint64(const std::string& bits) const
{
uint64_t value = 0U;
const size_t len = bits.length();
for (size_t j = 0; j < len; j++)
{
value <<= 1U; // shift left
if (bits[j] == '1')
{
value += 1; // insert the bit
}
}
return value;
}
int16_t galileo_e6_has_msg_receiver::read_has_message_body_int16(const std::string& bits) const int16_t galileo_e6_has_msg_receiver::read_has_message_body_int16(const std::string& bits) const
{ {
int16_t value = 0; int16_t value = 0;
@ -750,6 +860,22 @@ int16_t galileo_e6_has_msg_receiver::read_has_message_body_int16(const std::stri
} }
uint8_t galileo_e6_has_msg_receiver::read_has_message_body_uint8(const std::string& bits) const
{
uint8_t value = 0U;
const size_t len = bits.length();
for (size_t j = 0; j < len; j++)
{
value <<= 1U; // shift left
if (bits[j] == '1')
{
value += 1; // insert the bit
}
}
return value;
}
template <class T> template <class T>
std::string galileo_e6_has_msg_receiver::debug_print_vector(const std::string& title, const std::vector<T>& vec) const std::string galileo_e6_has_msg_receiver::debug_print_vector(const std::string& title, const std::vector<T>& vec) const
{ {
@ -765,18 +891,26 @@ std::string galileo_e6_has_msg_receiver::debug_print_vector(const std::string& t
} }
std::string galileo_e6_has_msg_receiver::debug_print_matrix(const std::string& title, const std::vector<std::vector<uint8_t>>& mat) const template <class T>
std::string galileo_e6_has_msg_receiver::debug_print_matrix(const std::string& title, const std::vector<std::vector<T>>& mat) const
{ {
std::string msg(title); std::string msg(title);
msg += ": \n"; msg += ": \n";
std::stringstream ss; std::stringstream ss;
for (size_t row = 0; row < mat.size(); row++) if (!mat.empty())
{ {
for (size_t col = 0; col < mat[0].size(); col++) for (size_t row = 0; row < mat.size(); row++)
{ {
ss << static_cast<float>(mat[row][col]) << " "; for (size_t col = 0; col < mat[0].size(); col++)
{
ss << static_cast<float>(mat[row][col]) << " ";
}
ss << '\n';
} }
}
else
{
ss << '\n'; ss << '\n';
} }
msg += ss.str(); msg += ss.str();

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

@ -3,8 +3,8 @@
* \brief GNU Radio block that processes Galileo HAS message pages received from * \brief GNU Radio block that processes Galileo HAS message pages received from
* Galileo E6B telemetry blocks. After successful decoding, sends the content to * Galileo E6B telemetry blocks. After successful decoding, sends the content to
* the PVT block. * the PVT block.
* \author Javier Arribas, 2021. jarribas(at)cttc.es
* \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es * \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es
* \author Javier Arribas, 2021. jarribas(at)cttc.es
* *
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* *
@ -28,7 +28,7 @@
#include <pmt/pmt.h> #include <pmt/pmt.h>
#include <bitset> #include <bitset>
#include <cstdint> #include <cstdint>
#include <memory> #include <memory> // for std::unique_ptr
#include <string> #include <string>
#include <utility> // std::pair #include <utility> // std::pair
#include <vector> #include <vector>
@ -66,30 +66,42 @@ private:
void read_MT1_header(const std::string& message_header); void read_MT1_header(const std::string& message_header);
void read_MT1_body(const std::string& message_body); void read_MT1_body(const std::string& message_body);
Nav_Message_Packet d_nav_msg_packet;
int decode_message_type1(uint8_t message_id, uint8_t message_size); int decode_message_type1(uint8_t message_id, uint8_t message_size);
uint8_t read_has_message_header_parameter_uint8(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
uint16_t read_has_message_header_parameter_uint16(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const; uint16_t read_has_message_header_parameter_uint16(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
uint8_t read_has_message_header_parameter_uint8(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
bool read_has_message_header_parameter_bool(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const; bool read_has_message_header_parameter_bool(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
uint8_t read_has_message_body_uint8(const std::string& bits) const;
uint16_t read_has_message_body_uint16(const std::string& bits) const;
uint64_t read_has_message_body_uint64(const std::string& bits) const; uint64_t read_has_message_body_uint64(const std::string& bits) const;
uint16_t read_has_message_body_uint16(const std::string& bits) const;
int16_t read_has_message_body_int16(const std::string& bits) const; int16_t read_has_message_body_int16(const std::string& bits) const;
uint8_t read_has_message_body_uint8(const std::string& bits) const;
template <class T> template <class T>
std::string debug_print_vector(const std::string& title, const std::vector<T>& vec) const; // only for debug purposes std::string debug_print_vector(const std::string& title, const std::vector<T>& vec) const; // only for debug purposes
std::string debug_print_matrix(const std::string& title, const std::vector<std::vector<uint8_t>>& mat) const; // only for debug purposes
template <class T>
std::string debug_print_matrix(const std::string& title, const std::vector<std::vector<T>>& mat) const; // only for debug purposes
std::unique_ptr<ReedSolomon> d_rs; std::unique_ptr<ReedSolomon> d_rs;
Galileo_HAS_data d_HAS_data{}; Galileo_HAS_data d_HAS_data{};
std::vector<std::vector<std::vector<uint8_t>>> d_C_matrix{32, std::vector<std::vector<uint8_t>>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0))}; // 32 x 255 x 53 Nav_Message_Packet d_nav_msg_packet;
std::vector<std::vector<uint8_t>> d_M_matrix{GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; // HAS message matrix 32 x 53
std::vector<std::vector<uint8_t>> d_received_pids{32, std::vector<uint8_t>()}; // Store decoding matrices and received PIDs
std::vector<int> d_nsat_in_mask_id{32, 0}; std::vector<std::vector<std::vector<uint8_t>>> d_C_matrix;
std::vector<std::vector<uint8_t>> d_gnss_id_in_mask{32, std::vector<uint8_t>(16)}; std::vector<std::vector<uint8_t>> d_M_matrix;
std::vector<uint8_t> d_nsys_in_mask{32}; std::vector<std::vector<uint8_t>> d_received_pids;
// Store masks
std::vector<int> d_nsat_in_mask_id;
std::vector<std::vector<uint8_t>> d_gnss_id_in_mask;
std::vector<std::vector<uint64_t>> d_satellite_mask;
std::vector<std::vector<uint16_t>> d_signal_mask;
std::vector<std::vector<bool>> d_cell_mask_availability_flag;
std::vector<std::vector<std::vector<std::vector<bool>>>> d_cell_mask;
std::vector<uint8_t> d_nsys_in_mask;
std::vector<std::vector<uint8_t>> d_nav_message_mask;
bool d_new_message{}; bool d_new_message{};
bool d_enable_navdata_monitor{}; bool d_enable_navdata_monitor{};
}; };

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

@ -29,30 +29,6 @@
/** \addtogroup System_Parameters /** \addtogroup System_Parameters
* \{ */ * \{ */
// Galileo HAS message field lengths
constexpr size_t HAS_MSG_NSYS_LENGTH = 4;
constexpr size_t HAS_MSG_ID_MASK_LENGTH = 4;
constexpr size_t HAS_MSG_SATELLITE_MASK_LENGTH = 40;
constexpr size_t HAS_MSG_SIGNAL_MASK_LENGTH = 16;
constexpr size_t HAS_MSG_NAV_MESSAGE_LENGTH = 3;
constexpr size_t HAS_MSG_VALIDITY_INDEX_LENGTH = 4;
constexpr size_t HAS_MSG_IOD_GPS_LENGTH = 8;
constexpr size_t HAS_MSG_IOD_GAL_LENGTH = 10;
constexpr size_t HAS_MSG_DELTA_RADIAL_LENGTH = 14;
constexpr size_t HAS_MSG_DELTA_ALONG_TRACK_LENGTH = 12;
constexpr size_t HAS_MSG_DELTA_CROSS_TRACK_LENGTH = 12;
constexpr size_t HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH = 2;
constexpr size_t HAS_MSG_DELTA_CLOCK_C0_LENGTH = 14;
constexpr size_t HAS_MSG_NSYSPRIME_LENGTH = 4;
constexpr size_t HAS_MSG_ID_CLOCK_SUBSET_LENGTH = 4;
constexpr size_t HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH = 2;
constexpr size_t HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH = 14;
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 size_t HAS_MSG_URA_LENGTH = 2;
// Galileo CNAV message structure // Galileo CNAV message structure
constexpr int32_t GALILEO_CNAV_SYMBOLS_PER_PAGE = 1000; //!< Total numer of symbols per HAS page including the sync pattern constexpr int32_t GALILEO_CNAV_SYMBOLS_PER_PAGE = 1000; //!< Total numer of symbols per HAS page including the sync pattern
constexpr int32_t GALILEO_CNAV_PREAMBLE_PERIOD_SYMBOLS = 1000; constexpr int32_t GALILEO_CNAV_PREAMBLE_PERIOD_SYMBOLS = 1000;
@ -73,14 +49,43 @@ constexpr int32_t GALILEO_CNAV_MT1_HEADER_BITS = 32;
constexpr int32_t GALILEO_CNAV_OCTETS_IN_SUBPAGE = 53; constexpr int32_t GALILEO_CNAV_OCTETS_IN_SUBPAGE = 53;
constexpr int32_t GALILEO_CNAV_INFORMATION_VECTOR_LENGTH = 32; constexpr int32_t GALILEO_CNAV_INFORMATION_VECTOR_LENGTH = 32;
constexpr int32_t HAS_MSG_MAX_SATS = 40; constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000";
constexpr int32_t HAS_MSG_MAX_SIGNALS = 16;
// Galileo HAS message field lengths
constexpr size_t HAS_MSG_NSYS_LENGTH = 4;
constexpr size_t HAS_MSG_ID_MASK_LENGTH = 4;
constexpr size_t HAS_MSG_SATELLITE_MASK_LENGTH = 40;
constexpr size_t HAS_MSG_SIGNAL_MASK_LENGTH = 16;
constexpr size_t HAS_MSG_NAV_MESSAGE_LENGTH = 3;
constexpr size_t HAS_MSG_MASK_RESERVED_LENGTH = 6;
constexpr size_t HAS_MSG_VALIDITY_INDEX_LENGTH = 4;
constexpr size_t HAS_MSG_IOD_GPS_LENGTH = 8;
constexpr size_t HAS_MSG_IOD_GAL_LENGTH = 10;
constexpr size_t HAS_MSG_DELTA_RADIAL_LENGTH = 14;
constexpr size_t HAS_MSG_DELTA_ALONG_TRACK_LENGTH = 12;
constexpr size_t HAS_MSG_DELTA_CROSS_TRACK_LENGTH = 12;
constexpr size_t HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH = 2;
constexpr size_t HAS_MSG_DELTA_CLOCK_C0_LENGTH = 14;
constexpr size_t HAS_MSG_NSYSPRIME_LENGTH = 4;
constexpr size_t HAS_MSG_ID_CLOCK_SUBSET_LENGTH = 4;
constexpr size_t HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH = 2;
constexpr size_t HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH = 13;
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 size_t HAS_MSG_URA_LENGTH = 2;
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;
constexpr int32_t HAS_MSG_NUMBER_SATELLITE_IDS = 40;
constexpr int32_t HAS_MSG_NUMBER_SIGNAL_MASKS = 16;
constexpr uint16_t HAS_MSG_NUMBER_MAX_TOH = 3599;
constexpr uint8_t HAS_MSG_GPS_SYSTEM = 0; // Table 8 ICD v1.2 constexpr uint8_t HAS_MSG_GPS_SYSTEM = 0; // Table 8 ICD v1.2
constexpr uint8_t HAS_MSG_GALILEO_SYSTEM = 2; // Table 8 ICD v1.2 constexpr uint8_t HAS_MSG_GALILEO_SYSTEM = 2; // Table 8 ICD v1.2
constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000";
const std::pair<int32_t, int32_t> GALILEO_HAS_STATUS({1, 2}); const std::pair<int32_t, int32_t> GALILEO_HAS_STATUS({1, 2});
const std::pair<int32_t, int32_t> GALILEO_HAS_RESERVED({3, 2}); const std::pair<int32_t, int32_t> GALILEO_HAS_RESERVED({3, 2});
const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_TYPE({5, 2}); const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_TYPE({5, 2});