diff --git a/src/core/system_parameters/Galileo_CNAV.h b/src/core/system_parameters/Galileo_CNAV.h index ec867a55b..63295dc03 100644 --- a/src/core/system_parameters/Galileo_CNAV.h +++ b/src/core/system_parameters/Galileo_CNAV.h @@ -32,6 +32,29 @@ * \{ */ +// 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; @@ -50,6 +73,12 @@ constexpr int32_t GALILEO_CNAV_PREAMBLE_LENGTH_BITS = 16; constexpr int32_t GALILEO_CNAV_MAX_NUMBER_ENCODED_BLOCKS = 255; constexpr int32_t GALILEO_CNAV_MT1_HEADER_BITS = 32; +constexpr int32_t HAS_MSG_MAX_SATS = 40; +constexpr int32_t HAS_MSG_MAX_SIGNALS = 16; + +constexpr uint8_t HAS_MSG_GPS_SYSTEM = 0; // Table 8 ICD +constexpr uint8_t HAS_MSG_GALILEO_SYSTEM = 2; // Table 8 ICD + constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000"; const std::pair GALILEO_HAS_STATUS({1, 2}); diff --git a/src/core/system_parameters/galileo_cnav_message.cc b/src/core/system_parameters/galileo_cnav_message.cc index cf2b3236d..903f2894f 100644 --- a/src/core/system_parameters/galileo_cnav_message.cc +++ b/src/core/system_parameters/galileo_cnav_message.cc @@ -23,6 +23,7 @@ #include // for boost::crc_basic, boost::crc_optimal #include // for boost::dynamic_bitset #include // for reverse, find +#include // for accumulate using CRC_Galileo_CNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>; @@ -195,35 +196,294 @@ void Galileo_Cnav_Message::read_MT1_header(const std::string& message_string) void Galileo_Cnav_Message::read_MT1_body(const std::string& message_string) { auto message = std::string(message_string.begin() + GALILEO_CNAV_MT1_HEADER_BITS, message_string.end()); // Remove header + int Nsat = 0; if (d_HAS_data.header.mask_flag) { // read mask - // size_t mask_flag_size = X; - // message = std::string(message.begin() + mask_flag_size, message.end()); // Remove mask_flag + d_HAS_data.Nsys = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_LENGTH)); + 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.reserve(d_HAS_data.Nsys); + d_HAS_data.cell_mask_availability_flag.reserve(d_HAS_data.Nsys); + d_HAS_data.nav_message.reserve(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)); + 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); + int ones_in_satellite_mask = 0; + for (size_t i = 0; i < msg.length(); i++) + { + if (msg[i] == '1') + { + ones_in_satellite_mask++; + } + } + 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); + int ones_in_signal_mask = 0; + for (size_t i = 0; i < msg.length(); i++) + { + if (msg[i] == '1') + { + ones_in_signal_mask++; + } + } + message = std::string(message.begin() + HAS_MSG_SIGNAL_MASK_LENGTH, message.end()); + + if (message.substr(0, 1) == "1") + { + d_HAS_data.cell_mask_availability_flag[i] = true; + } + else + { + d_HAS_data.cell_mask_availability_flag[i] = false; + } + message = std::string(message.begin() + 1, message.end()); + int size_cell = ones_in_satellite_mask * ones_in_signal_mask; + + + d_HAS_data.cell_mask[i].reserve(ones_in_satellite_mask); + for (int s = 0; s < ones_in_satellite_mask; s++) + { + d_HAS_data.cell_mask[i][s].reserve(ones_in_signal_mask); + for (int sig = 0; sig < ones_in_signal_mask; sig++) + { + d_HAS_data.cell_mask[i][s][sig] = (message[sig] == '1' ? true : false); + } + } + 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)); + message = std::string(message.begin() + HAS_MSG_NAV_MESSAGE_LENGTH, message.end()); + } } if (d_HAS_data.header.orbit_correction_flag) { // 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); + for (int i = 0; i < Nsat; i++) + { + if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GPS_SYSTEM) + { + d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GPS_LENGTH)); + message = std::string(message.begin() + HAS_MSG_IOD_GPS_LENGTH, message.end()); + } + if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GALILEO_SYSTEM) + { + d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GAL_LENGTH)); + message = std::string(message.begin() + HAS_MSG_IOD_GAL_LENGTH, message.end()); + } + d_HAS_data.delta_radial[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_RADIAL_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_RADIAL_LENGTH, message.end()); + + d_HAS_data.delta_along_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_ALONG_TRACK_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_ALONG_TRACK_LENGTH, message.end()); + + 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()); + } } if (d_HAS_data.header.clock_fullset_flag) { // 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); + 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.iod_change_flag.reserve(Nsat); + d_HAS_data.delta_clock_c0.reserve(Nsat); + for (int i = 0; i < Nsat; i++) + { + d_HAS_data.iod_change_flag[i] = (message[0] == '1' ? true : false); + message = std::string(message.begin() + 1, message.end()); + 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()); + } } 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)); + 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()); + } + int 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 (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.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++) + { + int number_sats_this_gnss_id = 0; + int 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; + } + int Nsat_b = std::accumulate(number_sats.begin(), number_sats.end(), 0); + + 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 (int 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++) + { + int number_sats_this_gnss_id = 0; + int 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; + } + int Nsat_p = std::accumulate(number_sats.begin(), number_sats.end(), 0); + + 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 (int 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) { // read URA + d_HAS_data.validity_interval_index_ura_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.ura.reserve(Nsat); + for (int i = 0; i < Nsat; i++) + { + d_HAS_data.ura[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_URA_LENGTH)); + message = std::string(message.begin() + HAS_MSG_URA_LENGTH, message.end()); + } } } @@ -282,3 +542,83 @@ bool Galileo_Cnav_Message::read_has_message_header_parameter_bool(std::bitset(bits[len - 1 - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +uint16_t Galileo_Cnav_Message::read_has_message_body_uint16(const std::string& bits) const +{ + uint16_t value = 0U; + size_t len = bits.length(); + + for (size_t j = 0; j < len; j++) + { + value <<= 1U; // shift left + if (static_cast(bits[len - 1 - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +uint64_t Galileo_Cnav_Message::read_has_message_body_uint64(const std::string& bits) const +{ + uint64_t value = 0U; + size_t len = bits.length(); + + for (size_t j = 0; j < len; j++) + { + value <<= 1U; // shift left + if (static_cast(bits[len - 1 - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +int16_t Galileo_Cnav_Message::read_has_message_body_int16(const std::string& bits) const +{ + int16_t value = 0; + size_t len = bits.length(); + + // read the MSB and perform the sign extension + if (static_cast(bits[len - 1]) == 1) + { + value ^= 0xFFFF; // 16 bits variable + } + else + { + value &= 0; + } + + for (size_t j = 0; j < len; j++) + { + value *= 2; // shift left the signed integer + value &= 0xFFFE; // reset the corresponding bit (for the 16 bits variable) + if (static_cast(bits[len - 1 - j]) == 1) + { + value += 1; // insert the bit + } + } + + return value; +} diff --git a/src/core/system_parameters/galileo_cnav_message.h b/src/core/system_parameters/galileo_cnav_message.h index 5c04af1f7..1cd03242a 100644 --- a/src/core/system_parameters/galileo_cnav_message.h +++ b/src/core/system_parameters/galileo_cnav_message.h @@ -80,6 +80,10 @@ private: uint8_t read_has_message_header_parameter_uint8(std::bitset bits, const std::pair& parameter) const; uint16_t read_has_message_header_parameter_uint16(std::bitset bits, const std::pair& parameter) const; bool read_has_message_header_parameter_bool(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; + int16_t read_has_message_body_int16(const std::string& bits) const; Galileo_HAS_data d_HAS_data{}; diff --git a/src/core/system_parameters/galileo_has_data.h b/src/core/system_parameters/galileo_has_data.h index 059cd6212..a56ea4a48 100644 --- a/src/core/system_parameters/galileo_has_data.h +++ b/src/core/system_parameters/galileo_has_data.h @@ -61,7 +61,7 @@ public: std::vector satellite_mask; std::vector signal_mask; std::vector cell_mask_availability_flag; - std::vector>> cell_mask; + std::vector>> cell_mask; std::vector nav_message; // Orbit corrections