diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index a89dda829..9914c0f28 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -24,6 +24,7 @@ #include "galileo_almanac.h" #include "galileo_almanac_helper.h" #include "galileo_ephemeris.h" +#include "galileo_has_data.h" #include "galileo_iono.h" #include "galileo_utc_model.h" #include "geojson_printer.h" @@ -500,6 +501,7 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_galileo_utc_model_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_galileo_almanac_helper_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_galileo_almanac_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); + d_galileo_has_message_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_glonass_gnav_ephemeris_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_glonass_gnav_utc_model_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_glonass_gnav_almanac_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); @@ -1315,6 +1317,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) d_user_pvt_solver->galileo_almanac_map[galileo_alm->PRN] = *galileo_alm; } } + else if (msg_type_hash_code == d_galileo_has_message_sptr_type_hash_code) + { + // Store HAS message and print its content + } // **************** GLONASS GNAV Telemetry ************************* else if (msg_type_hash_code == d_glonass_gnav_ephemeris_sptr_type_hash_code) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index ae8a73b3f..4e6fd3144 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -216,6 +216,7 @@ private: size_t d_galileo_utc_model_sptr_type_hash_code; size_t d_galileo_almanac_helper_sptr_type_hash_code; size_t d_galileo_almanac_sptr_type_hash_code; + size_t d_galileo_has_message_sptr_type_hash_code; size_t d_glonass_gnav_ephemeris_sptr_type_hash_code; size_t d_glonass_gnav_utc_model_sptr_type_hash_code; size_t d_glonass_gnav_almanac_sptr_type_hash_code; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index ad6161b21..667cc0f8e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -26,6 +26,7 @@ #include "display.h" #include "galileo_almanac_helper.h" // for Galileo_Almanac_Helper #include "galileo_ephemeris.h" // for Galileo_Ephemeris +#include "galileo_has_data.h" // For Galileo HAS messages #include "galileo_iono.h" // for Galileo_Iono #include "galileo_utc_model.h" // for Galileo_Utc_Model #include "gnss_synchro.h" @@ -512,15 +513,15 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t // 4. If we have a new full message, read it if (d_cnav_nav.have_new_HAS_message() == true) { - // TODO: Retrieve data from message and send it somewhere - // Galileo_HAS_data has_data = d_cnav_nav.get_HAS_data(); - if (d_cnav_nav.is_HAS_message_dummy()) + if (d_cnav_nav.is_HAS_message_dummy() == true) { - std::cout << TEXT_MAGENTA << "New Galileo E6 HAS message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n'; + std::cout << TEXT_MAGENTA << "New Galileo E6 HAS dummy message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n'; } else { - std::cout << TEXT_MAGENTA << "New Galileo E6 HAS dummy message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n'; + const std::shared_ptr tmp_obj = std::make_shared(d_cnav_nav.get_HAS_data()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + std::cout << TEXT_MAGENTA << "New Galileo E6 HAS message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n'; } } } @@ -649,7 +650,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( if (abs(corr_value) >= d_samples_per_preamble) { d_preamble_index = d_sample_counter; // record the preamble sample stamp - DLOG(INFO) << "Preamble detection for Galileo satellite " << this->d_satellite; + LOG(INFO) << "Preamble detection for Galileo satellite " << this->d_satellite << " in channel " << this->d_channel; d_stat = 1; // enter into frame pre-detection status } } diff --git a/src/core/system_parameters/Galileo_CNAV.h b/src/core/system_parameters/Galileo_CNAV.h index 2a5480977..52110f2a1 100644 --- a/src/core/system_parameters/Galileo_CNAV.h +++ b/src/core/system_parameters/Galileo_CNAV.h @@ -68,14 +68,16 @@ constexpr int32_t GALILEO_CNAV_CRC_LENGTH = 24; constexpr int32_t GALILEO_CNAV_MESSAGE_BITS_PER_PAGE = 424; constexpr int32_t GALILEO_CNAV_PAGE_HEADER_BITS = 24; constexpr int32_t GALILEO_CNAV_PREAMBLE_LENGTH_BITS = 16; -constexpr int32_t GALILEO_CNAV_MAX_NUMBER_ENCODED_BLOCKS = 255; +constexpr int32_t GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK = 255; 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 uint8_t HAS_MSG_GPS_SYSTEM = 0; // Table 8 ICD -constexpr uint8_t HAS_MSG_GALILEO_SYSTEM = 2; // Table 8 ICD +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"; diff --git a/src/core/system_parameters/galileo_cnav_message.cc b/src/core/system_parameters/galileo_cnav_message.cc index f1005a411..8a5abf3c5 100644 --- a/src/core/system_parameters/galileo_cnav_message.cc +++ b/src/core/system_parameters/galileo_cnav_message.cc @@ -3,14 +3,14 @@ * \brief Implementation of a Galileo CNAV Data message as described in * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 * (April 2020) - * \author Carles Fernandez-Prades, 2020 cfernandez(at)cttc.es + * \author Carles Fernandez-Prades, 2020-2021 cfernandez(at)cttc.es * * ----------------------------------------------------------------------------- * * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. * This file is part of GNSS-SDR. * - * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) * SPDX-License-Identifier: GPL-3.0-or-later * * ----------------------------------------------------------------------------- @@ -60,10 +60,11 @@ void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string) if (CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true) { d_flag_CRC_test = true; - // CRC correct: Read HAS page header + // CRC correct: Read 24 bits of HAS page header read_HAS_page_header(page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS, GALILEO_CNAV_PAGE_HEADER_BITS)); bool use_has = false; d_test_mode = false; + // HAS status as defined in ICD v1.2 Table 5 HAS Page Header switch (d_has_page_status) { case 0: // HAS is in Test Mode @@ -73,11 +74,14 @@ void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string) case 1: // HAS is in Operational Mode use_has = true; break; + case 2: // HAS is in "reserved" status + case 3: // Do not use HAS default: break; } if (use_has) { + // Process the 424 bits of encoded data process_HAS_page(page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS + GALILEO_CNAV_PAGE_HEADER_BITS, GALILEO_CNAV_MESSAGE_BITS_PER_PAGE)); } } @@ -101,11 +105,12 @@ void Galileo_Cnav_Message::read_HAS_page_header(const std::string& page_string) } if (!d_page_dummy) { + // ICD v1.2 Table 5: HAS page header const std::bitset has_page_header(page_string); d_has_page_status = read_has_page_header_parameter(has_page_header, GALILEO_HAS_STATUS); d_received_message_type = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_TYPE); d_received_message_id = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_ID); - d_received_message_size = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_SIZE); + d_received_message_size = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_SIZE) + 1; // "0" means 1 d_received_message_page_id = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_PAGE_ID); } } @@ -116,14 +121,27 @@ void Galileo_Cnav_Message::process_HAS_page(const std::string& page_string) if (d_current_message_id == d_received_message_id) { // if receiver pid was not there, store it. - if (std::find(d_list_pid.begin(), d_list_pid.end(), d_received_message_page_id) == d_list_pid.end()) + if (d_received_message_page_id == 0) { - if (d_received_message_type == 1) // contains satellite corrections + // reserved, ignore it + } + else + { + if (std::find(d_list_pid.begin(), d_list_pid.end(), d_received_message_page_id) == d_list_pid.end()) { - d_received_encoded_messages++; - d_list_pid.push_back(d_received_message_page_id); - // Store encoded page - d_encoded_message_type_1 += std::string(page_string); + if (d_received_message_type == 1) // contains satellite corrections + { + d_received_encoded_messages++; + d_list_pid.push_back(d_received_message_page_id); + // Pack encoded string into 53 octets and put it in + // the corresponding row of d_C_matrix. + 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[d_received_message_page_id - 1][k] = static_cast(bs.to_ulong()); + } + } } } } @@ -134,48 +152,112 @@ void Galileo_Cnav_Message::process_HAS_page(const std::string& page_string) d_received_encoded_messages = 0; d_new_message = false; d_current_message_size = d_received_message_size; - // erase stored pages and start storing again - d_encoded_message_type_1.clear(); + // erase stored pages and data, and start storing again d_list_pid.clear(); + d_HAS_data = Galileo_HAS_data(); if (d_received_message_type == 1) { - d_encoded_message_type_1.reserve(GALILEO_CNAV_MAX_NUMBER_ENCODED_BLOCKS * GALILEO_CNAV_MESSAGE_BITS_PER_PAGE); d_received_encoded_messages++; d_list_pid.push_back(d_received_message_page_id); - d_encoded_message_type_1 += std::string(page_string); + // Pack encoded string into 53 octets and put it in + // the corresponding row of d_C_matrix. + 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[d_received_message_page_id - 1][k] = static_cast(bs.to_ulong()); + } } } if (d_received_encoded_messages == d_current_message_size) { - // we have a full encoded message stored in d_encoded_message_type_1 + // we have a full encoded message stored in d_C_matrix d_received_encoded_messages = 0; d_current_message_id = 0; - d_new_message = true; - decode_message_type1(); + + int res = decode_message_type1(); + if (res == 0) + { + d_new_message = true; + } + else + { + d_new_message = false; + } } } -void Galileo_Cnav_Message::decode_message_type1() +int Galileo_Cnav_Message::decode_message_type1() { - // TODO: Reed-Solomon decoding of d_encoded_message_type_1 - // TODO: reordering - // decoded_message_type1 = ... - // read_HAS_message_type1(decoded_message_type1); -} + // All rows in d_C_matrix with no data are erasure positions + std::vector erasure_positions; + erasure_positions.reserve(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK - d_list_pid.size()); + for (uint8_t mpid = 1; mpid <= GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK; mpid++) + { + if (std::find(d_list_pid.begin(), d_list_pid.end(), mpid) == d_list_pid.end()) + { + erasure_positions.push_back(mpid - 1); + } + else + { + d_list_pid.remove(mpid); + } + } -void Galileo_Cnav_Message::read_HAS_message_type1(const std::string& message_string) -{ - d_HAS_data = Galileo_HAS_data(); - read_MT1_header(message_string); - read_MT1_body(message_string); + // 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 (int row = 0; row < GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK; row++) + { + C_column[row] = d_C_matrix[row][col]; + } + int result = rs.decode(C_column, erasure_positions); + if (result < 0) + { + // Decoding failed + d_C_matrix = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; + d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; + return -1; + } + + 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(d_current_message_size * GALILEO_CNAV_OCTETS_IN_SUBPAGE * 8); + for (uint8_t row = 0; row < d_current_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 d_C_matrix and d_M_matrix for next decoding + d_C_matrix = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; + d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; + + // Trigger HAS message content reading + read_MT1_header(decoded_message_type_1); + read_MT1_body(decoded_message_type_1); + + return 0; } void Galileo_Cnav_Message::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); @@ -192,6 +274,7 @@ void Galileo_Cnav_Message::read_MT1_header(const std::string& message_string) void Galileo_Cnav_Message::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) diff --git a/src/core/system_parameters/galileo_cnav_message.h b/src/core/system_parameters/galileo_cnav_message.h index 5913b3b6d..c557185f2 100644 --- a/src/core/system_parameters/galileo_cnav_message.h +++ b/src/core/system_parameters/galileo_cnav_message.h @@ -3,14 +3,14 @@ * \brief Implementation of a Galileo CNAV Data message as described in * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 * (April 2020) - * \author Carles Fernandez-Prades, 2020 cfernandez(at)cttc.es + * \author Carles Fernandez-Prades, 2020-2021 cfernandez(at)cttc.es * * ----------------------------------------------------------------------------- * * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. * This file is part of GNSS-SDR. * - * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) * SPDX-License-Identifier: GPL-3.0-or-later * * ----------------------------------------------------------------------------- @@ -21,10 +21,12 @@ #include "Galileo_CNAV.h" #include "galileo_has_data.h" +#include "reed_solomon.h" #include #include #include #include +#include /** \addtogroup Core * \{ */ @@ -68,23 +70,24 @@ private: bool CRC_test(std::bitset bits, uint32_t checksum) const; void read_HAS_page_header(const std::string& page_string); void process_HAS_page(const std::string& page_string); - void decode_message_type1(); - void read_HAS_message_type1(const std::string& message_string); void read_MT1_header(const std::string& message_string); void read_MT1_body(const std::string& message_string); + int decode_message_type1(); uint8_t read_has_page_header_parameter(std::bitset bits, const std::pair& parameter) const; 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{}; - - std::string d_encoded_message_type_1; + ReedSolomon rs = ReedSolomon(); + std::vector> d_C_matrix{GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; // 255 x 53 + std::vector> d_M_matrix{GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; // 32 x 53 std::list d_list_pid; uint8_t d_has_page_status{}; diff --git a/src/core/system_parameters/reed_solomon.cc b/src/core/system_parameters/reed_solomon.cc index 9324c6d68..c75c5baf0 100644 --- a/src/core/system_parameters/reed_solomon.cc +++ b/src/core/system_parameters/reed_solomon.cc @@ -409,21 +409,21 @@ uint8_t ReedSolomon::galois_mul_table(uint8_t a, uint8_t b) const return 0; } - uint8_t x = log_table[a]; - uint8_t y = log_table[b]; + uint8_t x = d_log_table[a]; + uint8_t y = d_log_table[b]; uint8_t log_mult = (x + y) % d_symbols_per_block; - return antilog[log_mult]; + return d_antilog[log_mult]; } void ReedSolomon::init_log_tables() { - log_table[0] = 0; // dummy value + d_log_table[0] = 0; // dummy value for (int i = 0, x = 1; i < d_symbols_per_block; x = galois_mul(x, d_min_poly), i++) { - log_table[x] = i; - antilog[i] = x; + d_log_table[x] = i; + d_antilog[i] = x; } } diff --git a/src/core/system_parameters/reed_solomon.h b/src/core/system_parameters/reed_solomon.h index 2b1d01dae..24a4f6ad3 100644 --- a/src/core/system_parameters/reed_solomon.h +++ b/src/core/system_parameters/reed_solomon.h @@ -85,7 +85,7 @@ public: * of the input vector. * * The second parameter is optional, and contains a vector of erasure - * positions to be passed to the decoding algorithm. + * positions to be passed to the decoding algorithm. Defaults to empty. * * Returns the number of corrected errors or -1 if decoding failed. */ @@ -105,36 +105,37 @@ public: private: static const int d_symbols_per_block = 255; // the total number of symbols in a RS block. - int decode_rs_8(uint8_t* data, const int* eras_pos, int no_eras, int pad) const; int mod255(int x) const; int rs_min(int a, int b) const; + int decode_rs_8(uint8_t* data, const int* eras_pos, int no_eras, int pad) const; uint8_t galois_mul(uint8_t a, uint8_t b) const; uint8_t galois_add(uint8_t a, uint8_t b) const; uint8_t galois_mul_table(uint8_t a, uint8_t b) const; void encode_rs_8(const uint8_t* data, uint8_t* parity) const; - void init_log_tables(); - void init_alpha_tables(); + void init_log_tables(); // initialize d_log_table and d_antilog + void init_alpha_tables(); // initialize d_alpha_to, d_index_of - std::array log_table{}; - std::array antilog{}; - std::array d_alpha_to{}; - std::array d_index_of{}; - std::vector> d_genmatrix; - std::vector d_genpoly_coeff; - std::vector d_genpoly_index; + std::array d_alpha_to{}; // used for decoding + std::array d_index_of{}; // used for decoding + std::array d_log_table{}; // used for encoding + std::array d_antilog{}; // used for encoding - size_t d_data_in_block; + std::vector> d_genmatrix; // used for encoding + std::vector d_genpoly_coeff; // used for encoding + std::vector d_genpoly_index; // used for encoding + + size_t d_data_in_block; // number of information symbols in a block int d_nroots; // number of parity symbols in a block - int d_prim; // The primitive root of the generator poly. - int d_pad; // the number of pad symbols in a block. + int d_prim; // The primitive root of the generator poly + int d_pad; // the number of pad symbols in a block int d_iprim; // prim-th root of 1, index form int d_fcr; // first consecutive root - uint8_t d_min_poly; - uint8_t d_a0; + uint8_t d_min_poly; // primitive polynomial + uint8_t d_a0; // auxiliar variable }; /** \} */