diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index f0981f7b8..2d3313a21 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -3,8 +3,8 @@ * \brief GNU Radio block that processes Galileo HAS message pages received from * Galileo E6B telemetry blocks. After successful decoding, sends the content to * the PVT block. - * \author Javier Arribas, 2021. jarribas(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 "display.h" -#include "galileo_has_page.h" // for Galileo_HAS_page -#include "gnss_sdr_make_unique.h" -#include "reed_solomon.h" +#include "display.h" // for colors in terminal +#include "galileo_has_page.h" // for Galileo_HAS_page +#include "gnss_sdr_make_unique.h" // for std::make_unique in C++11 +#include "reed_solomon.h" // for ReedSolomon #include #include #include -#include // std::find -#include // size_t -#include // std::accumulate -#include // std::stringstream -#include // std::out_of_range -#include // typeid +#include // for std::find, std::count +#include // for size_t +#include // for std::back_inserter +#include // for std::stringstream +#include // for std::out_of_range +#include // for typeid #if HAS_GENERIC_LAMBDA #else @@ -68,6 +68,22 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_ // initialize Reed-Solomon decoder d_rs = std::make_unique(); + // Reserve memory for decoding matrices and received PIDs + 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()); + + // Reserve memory to store masks + d_nsat_in_mask_id = std::vector(HAS_MSG_NUMBER_MASK_IDS); + d_gnss_id_in_mask = std::vector>(HAS_MSG_NUMBER_MASK_IDS, std::vector(HAS_MSG_NUMBER_GNSS_IDS)); + d_satellite_mask = std::vector>(HAS_MSG_NUMBER_MASK_IDS, std::vector(HAS_MSG_NUMBER_GNSS_IDS)); + d_signal_mask = std::vector>(HAS_MSG_NUMBER_MASK_IDS, std::vector(HAS_MSG_NUMBER_GNSS_IDS)); + d_cell_mask_availability_flag = std::vector>(HAS_MSG_NUMBER_MASK_IDS, std::vector(HAS_MSG_NUMBER_GNSS_IDS)); + d_cell_mask = std::vector>>>(HAS_MSG_NUMBER_MASK_IDS, {HAS_MSG_NUMBER_GNSS_IDS, {HAS_MSG_NUMBER_SATELLITE_IDS, std::vector(HAS_MSG_NUMBER_SIGNAL_MASKS)}}); + d_nsys_in_mask = std::vector(HAS_MSG_NUMBER_MASK_IDS); + d_nav_message_mask = std::vector>(HAS_MSG_NUMBER_MASK_IDS, std::vector(HAS_MSG_NUMBER_GNSS_IDS)); + + // Initialize values for d_nav_msg_packet d_nav_msg_packet.system = std::string("E"); d_nav_msg_packet.signal = std::string("E6"); 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 + d_new_message = false; if (d_received_pids[has_page.message_id].size() == has_page.message_size) { // 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 std::cout << TEXT_MAGENTA << "New Galileo HAS message type " << static_cast(has_page.message_id) << " received and successfully decoded" << TEXT_RESET << '\n'; - d_new_message = true; - } - else - { - d_new_message = false; + if (d_nsat_in_mask_id[d_HAS_data.header.mask_id] != 0) + { + // if we have the mask for that message, it's ready to be sent to PVT + d_new_message = true; + } } } } @@ -206,13 +223,13 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ msg += ss.str(); LOG(ERROR) << msg; d_received_pids[message_id].clear(); - d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; + d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; return -1; } 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_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 d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(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 for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++) { - std::vector C_column(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, 0); + std::vector C_column(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK); for (auto pid : d_received_pids[message_id]) { 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); 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"; return -1; } - DLOG(INFO) << "Successful HAS page decoding"; std::vector M_column(C_column.begin(), C_column.begin() + GALILEO_CNAV_INFORMATION_VECTOR_LENGTH); for (int i = 0; i < GALILEO_CNAV_INFORMATION_VECTOR_LENGTH; 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); @@ -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(); } } + DLOG(INFO) << "Decoded message ID " << static_cast(message_id) - << " (size: " << static_cast(message_size) << ") with body: " + << " (size: " << static_cast(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()); - // reset data for next decoding - d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(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) { 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)); } + // reset data for next decoding + d_C_matrix[message_id] = std::vector>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(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)); 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) { - 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; } 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 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. auto message = std::string(message_body); int Nsat = 0; + bool have_mask = false; + if (d_HAS_data.header.mask_flag) { - DLOG(INFO) << "TOH: " << static_cast(d_HAS_data.header.toh); // read mask d_HAS_data.Nsys = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_LENGTH)); - DLOG(INFO) << "Nsys " << static_cast(d_HAS_data.Nsys); d_nsys_in_mask[d_HAS_data.header.mask_id] = d_HAS_data.Nsys; if (d_HAS_data.Nsys != 0) { 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.cell_mask = {d_HAS_data.Nsys, std::vector>(40, std::vector(16, false))}; - d_HAS_data.cell_mask_availability_flag.reserve(d_HAS_data.Nsys); - d_HAS_data.nav_message.reserve(d_HAS_data.Nsys); - d_HAS_data.satellite_mask.reserve(d_HAS_data.Nsys); - d_HAS_data.signal_mask.reserve(d_HAS_data.Nsys); + d_HAS_data.gnss_id_mask = std::vector(d_HAS_data.Nsys); + d_HAS_data.cell_mask = {d_HAS_data.Nsys, std::vector>(40, std::vector(16))}; + d_HAS_data.cell_mask_availability_flag = std::vector(d_HAS_data.Nsys); + d_HAS_data.nav_message = std::vector(d_HAS_data.Nsys); + d_HAS_data.satellite_mask = std::vector(d_HAS_data.Nsys); + d_HAS_data.signal_mask = std::vector(d_HAS_data.Nsys); 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)); - DLOG(INFO) << "GNSS ID" << static_cast(i) << ": " << static_cast(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()); std::string msg = message.substr(0, HAS_MSG_SATELLITE_MASK_LENGTH); 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'); Nsat += ones_in_satellite_mask; message = std::string(message.begin() + HAS_MSG_SATELLITE_MASK_LENGTH, message.end()); msg = message.substr(0, HAS_MSG_SIGNAL_MASK_LENGTH); 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'); message = std::string(message.begin() + HAS_MSG_SIGNAL_MASK_LENGTH, message.end()); + d_HAS_data.cell_mask[i] = std::vector>(ones_in_satellite_mask, std::vector(ones_in_signal_mask, false)); + if (message.substr(0, 1) == "1") { 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_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()); 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()); 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()); } 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(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 { // Take data from a previously received mask with the same 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]; - d_HAS_data.Nsys = d_nsys_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]; + 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(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 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()); - d_HAS_data.gnss_iod.reserve(Nsat); - d_HAS_data.delta_radial.reserve(Nsat); - d_HAS_data.delta_along_track.reserve(Nsat); - d_HAS_data.delta_cross_track.reserve(Nsat); + d_HAS_data.gnss_iod = std::vector(Nsat); + d_HAS_data.delta_radial = std::vector(Nsat); + d_HAS_data.delta_along_track = std::vector(Nsat); + d_HAS_data.delta_cross_track = std::vector(Nsat); for (int i = 0; i < Nsat; i++) { 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)); 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 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()); - d_HAS_data.delta_clock_c0_multiplier.reserve(d_HAS_data.Nsys); + d_HAS_data.delta_clock_c0_multiplier = std::vector(d_HAS_data.Nsys); 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(Nsat); + d_HAS_data.delta_clock_c0 = std::vector(Nsat); for (int i = 0; i < Nsat; i++) { 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)); 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) - // { - // // 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)); - // 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)); - // DLOG(INFO) << "Nsysprime: " << static_cast(d_HAS_data.Nsysprime); - // message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end()); - // - // 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); - // d_HAS_data.satellite_submask.reserve(d_HAS_data.Nsysprime); - // d_HAS_data.iod_change_flag_clock_subset.reserve(d_HAS_data.Nsysprime); - // 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[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_CLOCK_SUBSET_LENGTH)); - // message = std::string(message.begin() + HAS_MSG_ID_CLOCK_SUBSET_LENGTH, message.end()); - // if (d_HAS_data.gnss_id_clock_subset[i] != HAS_MSG_GALILEO_SYSTEM) - // { - // 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)); - // message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH, message.end()); - // } - // uint64_t number_sats_this_gnss_id = 0; - // for (uint8_t j = 0; j < d_HAS_data.Nsys; j++) - // { - // if (d_HAS_data.gnss_id_mask[j] == d_HAS_data.gnss_id_clock_subset[i]) - // { - // uint64_t n = d_HAS_data.satellite_mask[j]; - // while (n) - // { - // number_sats_this_gnss_id += n & 1; - // n >>= 1; - // } - // break; - // } - // } - // - // d_HAS_data.satellite_submask[i].reserve(number_sats_this_gnss_id); - // for (uint64_t 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.iod_change_flag_clock_subset[i] = (message[0] == '1' ? true : false); - // 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()); - // } - // } - // - // if (d_HAS_data.header.code_bias_flag) - // { - // // 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()); - // std::vector number_sats(d_HAS_data.Nsys, 0); - // std::vector number_codes(d_HAS_data.Nsys, 0); - // for (int sys = 0; sys < d_HAS_data.Nsys; sys++) - // { - // uint64_t number_sats_this_gnss_id = 0; - // uint64_t number_signals_this_gnss_id = 0; - // if (d_HAS_data.cell_mask_availability_flag[sys] == true) - // { - // uint64_t n = d_HAS_data.satellite_mask[sys]; - // 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; - // } - // } - // else - // { - // number_sats_this_gnss_id = HAS_MSG_MAX_SATS; - // number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; - // } - // number_sats[sys] = number_sats_this_gnss_id; - // number_codes[sys] = number_signals_this_gnss_id; - // } - // uint64_t Nsat_b = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); - // - // d_HAS_data.code_bias.reserve(Nsat_b); - // int sat = 0; - // for (int sys = 0; sys < d_HAS_data.Nsys; sys++) - // { - // d_HAS_data.code_bias[sat].reserve(number_codes[sys]); - // for (uint64_t c = 0; c < number_codes[sys]; c++) - // { - // 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()); - // sat += 1; - // } - // } - // } - // if (d_HAS_data.header.phase_bias_flag) - // { - // // 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()); - // - // std::vector number_sats(d_HAS_data.Nsys, 0); - // std::vector number_phases(d_HAS_data.Nsys, 0); - // for (int sys = 0; sys < d_HAS_data.Nsys; sys++) - // { - // uint64_t number_sats_this_gnss_id = 0; - // uint64_t number_signals_this_gnss_id = 0; - // if (d_HAS_data.cell_mask_availability_flag[sys] == true) - // { - // uint64_t n = d_HAS_data.satellite_mask[sys]; - // 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; - // } - // } - // else - // { - // number_sats_this_gnss_id = HAS_MSG_MAX_SATS; - // number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; - // } - // number_sats[sys] = number_sats_this_gnss_id; - // number_phases[sys] = number_signals_this_gnss_id; - // } - // uint64_t Nsat_p = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); - // - // d_HAS_data.phase_bias.reserve(Nsat_p); - // d_HAS_data.phase_discontinuity_indicator.reserve(Nsat_p); - // int sat = 0; - // for (int sys = 0; sys < d_HAS_data.Nsys; sys++) - // { - // d_HAS_data.phase_bias[sat].reserve(number_phases[sys]); - // d_HAS_data.phase_discontinuity_indicator[sat].reserve(number_phases[sys]); - // for (uint64_t p = 0; p < number_phases[sys]; p++) - // { - // 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()); - // - // 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()); - // sat += 1; - // } - // } - // } - // if (d_HAS_data.header.ura_flag) + + if (d_HAS_data.header.clock_subset_flag && have_mask) + { + // 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)); + 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)); + message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end()); + + if (d_HAS_data.Nsysprime == 0) + { + // Wrong formatted data, aborting + have_mask = false; + d_nsat_in_mask_id[d_HAS_data.header.mask_id] = 0; + } + + d_HAS_data.gnss_id_clock_subset = std::vector(d_HAS_data.Nsysprime); + d_HAS_data.delta_clock_c0_multiplier_clock_subset = std::vector(d_HAS_data.Nsysprime); + d_HAS_data.satellite_submask = std::vector>(d_HAS_data.Nsysprime, std::vector(HAS_MSG_NUMBER_SATELLITE_IDS)); + d_HAS_data.iod_change_flag_clock_subset = std::vector(d_HAS_data.Nsysprime); + d_HAS_data.delta_clock_c0_clock_subset = std::vector(d_HAS_data.Nsysprime); + for (uint8_t i = 0; i < d_HAS_data.Nsysprime; i++) + { + d_HAS_data.gnss_id_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_CLOCK_SUBSET_LENGTH)); + message = std::string(message.begin() + HAS_MSG_ID_CLOCK_SUBSET_LENGTH, message.end()); + + uint8_t clock_multiplier = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH)); + 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 satellite_mask = d_HAS_data.satellite_mask[i]; + std::bitset<40> satellite_mask_bits(satellite_mask); + std::string satellite_mask_string = satellite_mask_bits.to_string(); + 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(number_sats_this_gnss_id); + 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()); + } + + int Nsatprime = std::count(d_HAS_data.satellite_submask[i].begin(), d_HAS_data.satellite_submask[i].end(), 1UL); + for (int j = 0; j < Nsatprime; j++) + { + 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(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)); + 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)); + } + + if (d_HAS_data.header.code_bias_flag && have_mask) + { + // 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()); + + std::vector number_sats(d_HAS_data.Nsys); + std::vector number_codes(d_HAS_data.Nsys); + uint64_t max_signals = 0ULL; + for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + { + uint64_t number_sats_this_gnss_id = 0; + uint64_t number_signals_this_gnss_id = 0; + if (d_HAS_data.cell_mask_availability_flag[sys] == true) + { + // cell mask is provided + 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(); + + if (number_signals_this_gnss_id > max_signals) + { + max_signals = number_signals_this_gnss_id; + } + } + else + { + // corrections for all satellites in satellite_mask + // and all signals in signal mask + uint64_t n = d_HAS_data.satellite_mask[sys]; + 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_codes[sys] = number_signals_this_gnss_id; + } + + d_HAS_data.code_bias = std::vector>(Nsat, std::vector(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 c = 0; c < number_codes[sys]; c++) + { + 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]))) + { + 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()); + } + } + sat += 1; + } + } + + DLOG(INFO) << debug_print_matrix("code bias", d_HAS_data.code_bias); + } + + if (d_HAS_data.header.phase_bias_flag && have_mask) + { + // 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()); + + std::vector number_sats(d_HAS_data.Nsys); + std::vector number_phases(d_HAS_data.Nsys); + uint64_t max_signals = 0ULL; + for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + { + uint64_t number_sats_this_gnss_id = 0; + uint64_t number_signals_this_gnss_id = 0; + if (d_HAS_data.cell_mask_availability_flag[sys] == true) + { + // cell mask is provided + 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(); + + if (number_signals_this_gnss_id > max_signals) + { + max_signals = number_signals_this_gnss_id; + } + } + else + { + // corrections for all satellites in satellite_mask + // and all signals in signal mask + uint64_t n = d_HAS_data.satellite_mask[sys]; + 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>(Nsat, std::vector(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 // 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& bits, const std::pair& parameter) const +uint16_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint16(const std::bitset& bits, const std::pair& parameter) const { - uint8_t value = 0U; + uint16_t value = 0U; for (int j = 0; j < parameter.second; j++) { 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& bits, const std::pair& parameter) const +uint8_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint8(const std::bitset& bits, const std::pair& parameter) const { - uint16_t value = 0U; + uint8_t value = 0U; for (int j = 0; j < parameter.second; j++) { 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(); + for (size_t j = 0; j < len; j++) { 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 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 std::string galileo_e6_has_msg_receiver::debug_print_vector(const std::string& title, const std::vector& 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>& mat) const +template +std::string galileo_e6_has_msg_receiver::debug_print_matrix(const std::string& title, const std::vector>& mat) const { std::string msg(title); msg += ": \n"; 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(mat[row][col]) << " "; + for (size_t col = 0; col < mat[0].size(); col++) + { + ss << static_cast(mat[row][col]) << " "; + } + ss << '\n'; } + } + else + { ss << '\n'; } msg += ss.str(); diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index 86956cc33..2e6187850 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -3,8 +3,8 @@ * \brief GNU Radio block that processes Galileo HAS message pages received from * Galileo E6B telemetry blocks. After successful decoding, sends the content to * the PVT block. - * \author Javier Arribas, 2021. jarribas(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 #include #include -#include +#include // for std::unique_ptr #include #include // std::pair #include @@ -66,30 +66,42 @@ private: void read_MT1_header(const std::string& message_header); 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); - uint8_t read_has_message_header_parameter_uint8(const std::bitset& bits, const std::pair& parameter) const; + uint16_t read_has_message_header_parameter_uint16(const std::bitset& bits, const std::pair& parameter) const; + uint8_t read_has_message_header_parameter_uint8(const std::bitset& bits, const std::pair& parameter) const; bool read_has_message_header_parameter_bool(const std::bitset& bits, const std::pair& 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; + uint16_t read_has_message_body_uint16(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 - std::string debug_print_vector(const std::string& title, const std::vector& vec) const; // only for debug purposes - std::string debug_print_matrix(const std::string& title, const std::vector>& mat) const; // only for debug purposes + std::string debug_print_vector(const std::string& title, const std::vector& vec) const; // only for debug purposes + + template + std::string debug_print_matrix(const std::string& title, const std::vector>& mat) const; // only for debug purposes std::unique_ptr d_rs; Galileo_HAS_data d_HAS_data{}; - std::vector>> d_C_matrix{32, std::vector>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0))}; // 32 x 255 x 53 - std::vector> d_M_matrix{GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; // HAS message matrix 32 x 53 - std::vector> d_received_pids{32, std::vector()}; - std::vector d_nsat_in_mask_id{32, 0}; - std::vector> d_gnss_id_in_mask{32, std::vector(16)}; - std::vector d_nsys_in_mask{32}; + Nav_Message_Packet d_nav_msg_packet; + + // Store decoding matrices and received PIDs + std::vector>> d_C_matrix; + std::vector> d_M_matrix; + std::vector> d_received_pids; + + // Store masks + std::vector d_nsat_in_mask_id; + std::vector> d_gnss_id_in_mask; + std::vector> d_satellite_mask; + std::vector> d_signal_mask; + std::vector> d_cell_mask_availability_flag; + std::vector>>> d_cell_mask; + std::vector d_nsys_in_mask; + std::vector> d_nav_message_mask; + bool d_new_message{}; bool d_enable_navdata_monitor{}; }; diff --git a/src/core/system_parameters/Galileo_CNAV.h b/src/core/system_parameters/Galileo_CNAV.h index 2c5825fe5..484c1f1b6 100644 --- a/src/core/system_parameters/Galileo_CNAV.h +++ b/src/core/system_parameters/Galileo_CNAV.h @@ -29,30 +29,6 @@ /** \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 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; @@ -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_INFORMATION_VECTOR_LENGTH = 32; -constexpr int32_t HAS_MSG_MAX_SATS = 40; -constexpr int32_t HAS_MSG_MAX_SIGNALS = 16; +constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000"; + +// 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_GALILEO_SYSTEM = 2; // Table 8 ICD v1.2 -constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000"; - const std::pair GALILEO_HAS_STATUS({1, 2}); const std::pair GALILEO_HAS_RESERVED({3, 2}); const std::pair GALILEO_HAS_MESSAGE_TYPE({5, 2});