diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index 266d45039..f6dcc52fe 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -1,8 +1,10 @@ /*! * \file galileo_e6_has_msg_receiver.cc - * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message - * sections from Galileo E6 telemetry blocks + * \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 * * ----------------------------------------------------------------------------- * @@ -17,16 +19,16 @@ #include "galileo_e6_has_msg_receiver.h" -#include "galileo_has_data.h" // for Galileo_HAS_data #include "galileo_has_page.h" // for Galileo_HAS_page +#include "gnss_sdr_make_unique.h" +#include "reed_solomon.h" #include #include -#include #include -#include -#include -#include -#include +#include // std::find +#include // size_t +#include // std::accumulate +#include // typeid #if HAS_GENERIC_LAMBDA #else @@ -42,7 +44,7 @@ galileo_e6_has_msg_receiver_sptr galileo_e6_has_msg_receiver_make() galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_e6_has_msg_receiver", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) { - // register Gal E6 HAS input message port + // register Gal E6 HAS input message port from telemetry blocks this->message_port_register_in(pmt::mp("E6_HAS_from_TLM")); this->set_msg_handler(pmt::mp("E6_HAS_from_TLM"), #if HAS_GENERIC_LAMBDA @@ -55,41 +57,615 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_ #endif #endif - // register Gal E6 processed HAS out + // register Gal E6 processed HAS async output message port towards PVT this->message_port_register_out(pmt::mp("E6_HAS_to_PVT")); + + // initialize Reed-Solomon decoder + d_rs = std::make_unique(); } void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& msg) { gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_galileo_e6_has function called by the scheduler - // 1. receive the PMT message and reconstruct the object... + try { const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); - if (msg_type_hash_code == typeid(std::shared_ptr).hash_code()) { - const auto HAS_data_obj_obj = boost::any_cast>(pmt::any_ref(msg)); - // std::cout << HAS_data_obj_obj->has_message_string << '\n'; - // store / do things with the data + const auto HAS_data_page = boost::any_cast>(pmt::any_ref(msg)); + DLOG(INFO) << "New HAS page received: " + << "Status: " << static_cast(HAS_data_page->has_status) << ", " + << "MT: " << static_cast(HAS_data_page->message_type) << ", " + << "MID: " << static_cast(HAS_data_page->message_id) << ", " + << "MS: " << static_cast(HAS_data_page->message_size) << ", " + << "PID: " << static_cast(HAS_data_page->message_page_id); + + process_HAS_page(*HAS_data_page.get()); } else { - LOG(WARNING) << "channel_status_msg_receiver unknown object type!"; + LOG(WARNING) << "galileo_e6_has_msg_receiver received an unknown object type!"; } } catch (const boost::bad_any_cast& e) { - LOG(WARNING) << "channel_status_msg_receiver Bad any_cast: " << e.what(); + LOG(WARNING) << "galileo_e6_has_msg_receiver Bad any_cast: " << e.what(); } - // 2. Trigger the HAS processing function if required - // TODO - - // 3. Send the resulting decoded HAS data (if available) to PVT - - // TODO: fill message object and send to PVT when ready - std::shared_ptr dummy{}; - this->message_port_pub(pmt::mp("E6_HAS_to_PVT"), pmt::make_any(dummy)); + // Send the resulting decoded HAS data (if available) to PVT + if (d_new_message == true) + { + auto has_data_ptr = std::make_shared(d_HAS_data); + this->message_port_pub(pmt::mp("E6_HAS_to_PVT"), pmt::make_any(has_data_ptr)); + d_new_message = false; + DLOG(INFO) << "HAS message sent to the PVT block through the E6_HAS_to_PVT async message port"; + } +} + + +void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_page) +{ + if (has_page.has_status == 0 || has_page.has_status == 1) + { + std::string page_string(has_page.has_message_string); + if (has_page.message_page_id != 0) // PID=0 is reserved, ignore it + { + if (has_page.message_type == 1) // contains satellite corrections + { + if (has_page.message_id < 32) // MID range is from 0 to 31 + { + if (std::find(d_received_pages[has_page.message_id].begin(), d_received_pages[has_page.message_id].end(), has_page.message_page_id) == d_received_pages[has_page.message_id].end()) + { + // New pid! Annotate it. + d_received_pages[has_page.message_id].push_back(has_page.message_page_id); + for (int k = 0; k < GALILEO_CNAV_OCTETS_IN_SUBPAGE; k++) + { + std::string bits8 = page_string.substr(k * 8, 8); + std::bitset<8> bs(bits8); + d_C_matrix[has_page.message_id][has_page.message_page_id - 1][k] = static_cast(bs.to_ulong()); + } + } + } + } + } + } + + // If we have received for this message ID a number of pages equal to the message size + if (d_received_pages[has_page.message_id].size() == has_page.message_size) + { + // Try to decode the message + int res = decode_message_type1(has_page.message_id, has_page.message_size); + + if (res == 0) + { + // Successful decoding, we have a valid HAS message stored at d_HAS_data + d_new_message = true; + } + else + { + d_new_message = false; + } + } +} + + +int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_t message_size) +{ + DLOG(INFO) << "Start decoding of a HAS message"; + + // Compute erasure positions + std::vector erasure_positions; + erasure_positions.reserve(223); // Maximum erasure positions ( = number of parity symbols in a block) + for (int i = 0; i < message_size; i++) // we know that from message_size to 32, the value is 0 + { + if (d_C_matrix[message_id][i][0] == 0) + { + erasure_positions.push_back(i); + } + } + for (int i = 32; i < 255; i++) + { + if (d_C_matrix[message_id][i][0] == 0) + { + erasure_positions.push_back(i); + } + } + + // Reset HAS message matrix + d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; + + // 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); + for (auto pid : d_received_pages[message_id]) + { + C_column[pid - 1] = d_C_matrix[message_id][pid - 1][col]; + } + + int result = d_rs->decode(C_column, erasure_positions); + + if (result < 0) + { + DLOG(INFO) << "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]; + } + } + + // Form the decoded HAS message by reading rows of d_M_matrix + std::string decoded_message_type_1; + decoded_message_type_1.reserve(message_size * GALILEO_CNAV_OCTETS_IN_SUBPAGE * 8); + for (uint8_t row = 0; row < message_size; row++) + { + for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++) + { + std::bitset<8> bs(d_M_matrix[row][col]); + decoded_message_type_1 += bs.to_string(); + } + } + + // 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_pages[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); + read_MT1_body(decoded_message_type_1); + + return 0; +} + + +void galileo_e6_has_msg_receiver::read_MT1_header(const std::string& message_string) +{ + // ICD v1.2 Table 6: MT1 Message Header + const std::bitset has_mt1_header(message_string); + d_HAS_data.header.toh = read_has_message_header_parameter_uint16(has_mt1_header, GALILEO_MT1_HEADER_TOH); + d_HAS_data.header.mask_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_MASK_ID); + d_HAS_data.header.iod_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_IOD_ID); + d_HAS_data.header.mask_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_MASK_FLAG); + d_HAS_data.header.orbit_correction_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_ORBIT_CORRECTION_FLAG); + d_HAS_data.header.clock_fullset_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CLOCK_FULLSET_FLAG); + d_HAS_data.header.clock_subset_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CLOCK_SUBSET_FLAG); + d_HAS_data.header.code_bias_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CODE_BIAS_FLAG); + d_HAS_data.header.phase_bias_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_PHASE_BIAS_FLAG); + d_HAS_data.header.ura_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_URA_FLAG); + + DLOG(INFO) << "MT1 header: " + << "TOH: " << static_cast(d_HAS_data.header.toh) << ", " + << "mask iD: " << static_cast(d_HAS_data.header.mask_id) << ", " + << "iod iD: " << static_cast(d_HAS_data.header.iod_id) << ", " + << "mask_flag: " << static_cast(d_HAS_data.header.mask_flag) << ", " + << "orbit_correction_flag: " << static_cast(d_HAS_data.header.orbit_correction_flag) << ", " + << "clock_fullset_flag: " << static_cast(d_HAS_data.header.clock_fullset_flag) << ", " + << "clock_subset_flag: " << static_cast(d_HAS_data.header.clock_subset_flag) << ", " + << "code_bias_flag: " << static_cast(d_HAS_data.header.code_bias_flag) << ", " + << "phase_bias_flag: " << static_cast(d_HAS_data.header.phase_bias_flag) << ", " + << "ura_flag: " << static_cast(d_HAS_data.header.ura_flag); +} + + +void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_string) +{ + // ICD v1.2 Table 7: MT1 Message Body. + 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 + 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 (char c : msg) + { + if (c == '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 (char c : msg) + { + if (c == '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()); + } + 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) + { + // 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()); + } + } +} + + +uint8_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint8(const std::bitset& bits, const std::pair& parameter) const +{ + uint8_t value = 0U; + for (int j = 0; j < parameter.second; j++) + { + value <<= 1U; // shift left + if (static_cast(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +uint16_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint16(const std::bitset& bits, const std::pair& parameter) const +{ + uint16_t value = 0U; + for (int j = 0; j < parameter.second; j++) + { + value <<= 1U; // shift left + if (static_cast(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +bool galileo_e6_has_msg_receiver::read_has_message_header_parameter_bool(const std::bitset& bits, const std::pair& parameter) const +{ + bool value = false; + if (static_cast(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first]) == 1) + { + value = true; + } + return value; +} + + +uint8_t galileo_e6_has_msg_receiver::read_has_message_body_uint8(const std::string& bits) const +{ + uint8_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; +} + + +uint16_t galileo_e6_has_msg_receiver::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_e6_has_msg_receiver::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_e6_has_msg_receiver::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/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index 6b5e2aa29..3548685d3 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -1,8 +1,10 @@ /*! * \file galileo_e6_has_msg_receiver.h - * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message - * sections from Galileo E6 telemetry blocks + * \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 * * ----------------------------------------------------------------------------- * @@ -18,19 +20,25 @@ #ifndef GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_H #define GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_H +#include "Galileo_CNAV.h" +#include "galileo_has_data.h" #include "gnss_block_interface.h" -#include "reed_solomon.h" #include #include -#include +#include +#include #include +#include +#include // std::pair +#include /** \addtogroup Core * \{ */ /** \addtogroup Core_Receiver_Library * \{ */ - +class Galileo_HAS_page; +class ReedSolomon; class galileo_e6_has_msg_receiver; using galileo_e6_has_msg_receiver_sptr = gnss_shared_ptr; @@ -38,7 +46,10 @@ using galileo_e6_has_msg_receiver_sptr = gnss_shared_ptr& bits, const std::pair& parameter) const; + uint16_t read_has_message_header_parameter_uint16(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; + int16_t read_has_message_body_int16(const std::string& bits) const; + + 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_pages{32, std::vector()}; + bool d_new_message{}; };