mirror of
				https://github.com/gnss-sdr/gnss-sdr
				synced 2025-10-30 23:03:05 +00:00 
			
		
		
		
	Merge next branch
This commit is contained in:
		| @@ -590,7 +590,7 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, | |||||||
|         { |         { | ||||||
|             nsys += SYS_GPS; |             nsys += SYS_GPS; | ||||||
|         } |         } | ||||||
|     if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0)) |     if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0) || (gal_E6_count > 0)) | ||||||
|         { |         { | ||||||
|             nsys += SYS_GAL; |             nsys += SYS_GAL; | ||||||
|         } |         } | ||||||
|   | |||||||
| @@ -26,7 +26,7 @@ | |||||||
| #include "display.h" | #include "display.h" | ||||||
| #include "galileo_almanac_helper.h"  // for Galileo_Almanac_Helper | #include "galileo_almanac_helper.h"  // for Galileo_Almanac_Helper | ||||||
| #include "galileo_ephemeris.h"       // for Galileo_Ephemeris | #include "galileo_ephemeris.h"       // for Galileo_Ephemeris | ||||||
| #include "galileo_has_data.h"        // For Galileo HAS messages | #include "galileo_has_page.h"        // For Galileo_HAS_page | ||||||
| #include "galileo_iono.h"            // for Galileo_Iono | #include "galileo_iono.h"            // for Galileo_Iono | ||||||
| #include "galileo_utc_model.h"       // for Galileo_Utc_Model | #include "galileo_utc_model.h"       // for Galileo_Utc_Model | ||||||
| #include "gnss_synchro.h" | #include "gnss_synchro.h" | ||||||
| @@ -205,6 +205,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( | |||||||
|     d_channel = 0; |     d_channel = 0; | ||||||
|     d_flag_PLL_180_deg_phase_locked = false; |     d_flag_PLL_180_deg_phase_locked = false; | ||||||
|     d_symbol_history.set_capacity(d_required_symbols + 1); |     d_symbol_history.set_capacity(d_required_symbols + 1); | ||||||
|  |     d_cnav_dummy_page = false; | ||||||
|  |  | ||||||
|     // vars for Viterbi decoder |     // vars for Viterbi decoder | ||||||
|     const int32_t max_states = 1U << static_cast<uint32_t>(d_mm);  // 2^d_mm |     const int32_t max_states = 1U << static_cast<uint32_t>(d_mm);  // 2^d_mm | ||||||
| @@ -531,21 +532,29 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t | |||||||
|         } |         } | ||||||
|     d_cnav_nav.read_HAS_page(page_String); |     d_cnav_nav.read_HAS_page(page_String); | ||||||
|  |  | ||||||
|     // 4. If we have a new full message, read it |     // 4. If we have a new HAS page, read it | ||||||
|     if (d_cnav_nav.have_new_HAS_message() == true) |     if (d_cnav_nav.have_new_HAS_page() == true) | ||||||
|         { |         { | ||||||
|             if (d_cnav_nav.is_HAS_message_dummy() == true) |             bool is_page_dummy = d_cnav_nav.is_HAS_page_dummy(); | ||||||
|  |             if (is_page_dummy == true) | ||||||
|                 { |                 { | ||||||
|                     std::cout << TEXT_MAGENTA << "New Galileo E6 HAS dummy message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n'; |                     // Only print the message once | ||||||
|  |                     if (is_page_dummy != d_cnav_dummy_page) | ||||||
|  |                         { | ||||||
|  |                             d_cnav_dummy_page = is_page_dummy; | ||||||
|  |                             std::cout << TEXT_MAGENTA << "Receiving Galileo E6 CNAV dummy pages in channel " | ||||||
|  |                                       << d_channel << " from satellite " << d_satellite | ||||||
|  |                                       << TEXT_RESET << '\n'; | ||||||
|  |                         } | ||||||
|                 } |                 } | ||||||
|             else |             else | ||||||
|                 { |                 { | ||||||
|                     //TODO: do not send HAS data over telemetry msg port |                     const std::shared_ptr<Galileo_HAS_page> tmp_obj = std::make_shared<Galileo_HAS_page>(d_cnav_nav.get_HAS_encoded_page()); | ||||||
|                     const std::shared_ptr<Galileo_HAS_data> tmp_obj = std::make_shared<Galileo_HAS_data>(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'; |  | ||||||
|                     //Send HAS data to HAS processing class |  | ||||||
|                     this->message_port_pub(pmt::mp("E6_HAS_from_TLM"), pmt::make_any(tmp_obj)); |                     this->message_port_pub(pmt::mp("E6_HAS_from_TLM"), pmt::make_any(tmp_obj)); | ||||||
|  |                     std::cout << TEXT_MAGENTA << "New Galileo E6 HAS page received in channel " | ||||||
|  |                               << d_channel << " from satellite " << d_satellite | ||||||
|  |                               << (d_cnav_nav.is_HAS_in_test_mode() == true ? " (test mode)" : "") | ||||||
|  |                               << TEXT_RESET << '\n'; | ||||||
|                 } |                 } | ||||||
|         } |         } | ||||||
| } | } | ||||||
| @@ -767,7 +776,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( | |||||||
|                                 break; |                                 break; | ||||||
|                             } |                             } | ||||||
|                         d_preamble_index = d_sample_counter;  // record the preamble sample stamp (t_P) |                         d_preamble_index = d_sample_counter;  // record the preamble sample stamp (t_P) | ||||||
|                         if (d_inav_nav.get_flag_CRC_test() == true or d_fnav_nav.get_flag_CRC_test() == true) |                         if (d_inav_nav.get_flag_CRC_test() == true or d_fnav_nav.get_flag_CRC_test() == true or d_cnav_nav.get_flag_CRC_test() == true) | ||||||
|                             { |                             { | ||||||
|                                 d_CRC_error_counter = 0; |                                 d_CRC_error_counter = 0; | ||||||
|                                 d_flag_preamble = true;  // valid preamble indicator (initialized to false every work()) |                                 d_flag_preamble = true;  // valid preamble indicator (initialized to false every work()) | ||||||
| @@ -900,6 +909,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( | |||||||
|                 case 3:  // CNAV |                 case 3:  // CNAV | ||||||
|                     { |                     { | ||||||
|                         // TODO |                         // TODO | ||||||
|  |                         d_TOW_at_current_symbol_ms += d_PRN_code_period_ms;  // this is not the TOW! | ||||||
|                         break; |                         break; | ||||||
|                     } |                     } | ||||||
|                 } |                 } | ||||||
| @@ -936,7 +946,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( | |||||||
|             } |             } | ||||||
|         } |         } | ||||||
|  |  | ||||||
|     if (d_inav_nav.get_flag_TOW_set() == true or d_fnav_nav.get_flag_TOW_set() == true) |     if (d_inav_nav.get_flag_TOW_set() == true or d_fnav_nav.get_flag_TOW_set() == true or d_cnav_nav.get_flag_CRC_test() == true) | ||||||
|         { |         { | ||||||
|             current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; |             current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; | ||||||
|             // todo: Galileo to GPS time conversion should be moved to observable block. |             // todo: Galileo to GPS time conversion should be moved to observable block. | ||||||
| @@ -983,7 +993,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( | |||||||
|                         } |                         } | ||||||
|                     catch (const std::ifstream::failure &e) |                     catch (const std::ifstream::failure &e) | ||||||
|                         { |                         { | ||||||
|                             LOG(WARNING) << "Exception writing observables dump file " << e.what(); |                             LOG(WARNING) << "Exception writing navigation data dump file " << e.what(); | ||||||
|                         } |                         } | ||||||
|                 } |                 } | ||||||
|             // 3. Make the output (copy the object contents to the GNURadio reserved memory) |             // 3. Make the output (copy the object contents to the GNURadio reserved memory) | ||||||
|   | |||||||
| @@ -140,6 +140,7 @@ private: | |||||||
|     bool d_dump_mat; |     bool d_dump_mat; | ||||||
|     bool d_remove_dat; |     bool d_remove_dat; | ||||||
|     bool d_first_eph_sent; |     bool d_first_eph_sent; | ||||||
|  |     bool d_cnav_dummy_page; | ||||||
| }; | }; | ||||||
|  |  | ||||||
|  |  | ||||||
|   | |||||||
| @@ -86,6 +86,7 @@ set(SYSTEM_PARAMETERS_HEADERS | |||||||
|     Beidou_DNAV.h |     Beidou_DNAV.h | ||||||
|     MATH_CONSTANTS.h |     MATH_CONSTANTS.h | ||||||
|     reed_solomon.h |     reed_solomon.h | ||||||
|  |     galileo_has_page.h | ||||||
| ) | ) | ||||||
|  |  | ||||||
| list(SORT SYSTEM_PARAMETERS_HEADERS) | list(SORT SYSTEM_PARAMETERS_HEADERS) | ||||||
|   | |||||||
| @@ -63,7 +63,7 @@ constexpr int32_t GALILEO_CNAV_TELEMETRY_RATE_BITS_SECOND = 1000;  // bps | |||||||
| constexpr int32_t GALILEO_CNAV_HAS_PAGE_DATA_BITS = 448; | constexpr int32_t GALILEO_CNAV_HAS_PAGE_DATA_BITS = 448; | ||||||
| constexpr int32_t GALILEO_CNAV_PAGE_RESERVED_BITS = 14; | constexpr int32_t GALILEO_CNAV_PAGE_RESERVED_BITS = 14; | ||||||
| constexpr int32_t GALILEO_CNAV_BITS_FOR_CRC = GALILEO_CNAV_HAS_PAGE_DATA_BITS + GALILEO_CNAV_PAGE_RESERVED_BITS;  // 462 | constexpr int32_t GALILEO_CNAV_BITS_FOR_CRC = GALILEO_CNAV_HAS_PAGE_DATA_BITS + GALILEO_CNAV_PAGE_RESERVED_BITS;  // 462 | ||||||
| constexpr int32_t GALILEO_CNAV_BYTES_FOR_CRC = 60; | constexpr int32_t GALILEO_CNAV_BYTES_FOR_CRC = 58; | ||||||
| constexpr int32_t GALILEO_CNAV_CRC_LENGTH = 24; | constexpr int32_t GALILEO_CNAV_CRC_LENGTH = 24; | ||||||
| constexpr int32_t GALILEO_CNAV_MESSAGE_BITS_PER_PAGE = 424; | constexpr int32_t GALILEO_CNAV_MESSAGE_BITS_PER_PAGE = 424; | ||||||
| constexpr int32_t GALILEO_CNAV_PAGE_HEADER_BITS = 24; | constexpr int32_t GALILEO_CNAV_PAGE_HEADER_BITS = 24; | ||||||
| @@ -82,6 +82,7 @@ constexpr uint8_t HAS_MSG_GALILEO_SYSTEM = 2;  // Table 8 ICD v1.2 | |||||||
| constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000"; | constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000"; | ||||||
|  |  | ||||||
| const std::pair<int32_t, int32_t> GALILEO_HAS_STATUS({1, 2}); | const std::pair<int32_t, int32_t> GALILEO_HAS_STATUS({1, 2}); | ||||||
|  | const std::pair<int32_t, int32_t> GALILEO_HAS_RESERVED({3, 2}); | ||||||
| const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_TYPE({5, 2}); | const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_TYPE({5, 2}); | ||||||
| const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_ID({7, 5}); | const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_ID({7, 5}); | ||||||
| const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_SIZE({12, 5}); | const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_SIZE({12, 5}); | ||||||
|   | |||||||
| @@ -19,30 +19,31 @@ | |||||||
| #include "galileo_cnav_message.h" | #include "galileo_cnav_message.h" | ||||||
| #include <boost/crc.hpp>             // for boost::crc_basic, boost::crc_optimal | #include <boost/crc.hpp>             // for boost::crc_basic, boost::crc_optimal | ||||||
| #include <boost/dynamic_bitset.hpp>  // for boost::dynamic_bitset | #include <boost/dynamic_bitset.hpp>  // for boost::dynamic_bitset | ||||||
| #include <algorithm>                 // for reverse, find | #include <glog/logging.h> | ||||||
| #include <numeric>                   // for accumulate | #include <algorithm>  // for reverse | ||||||
|  | #include <vector> | ||||||
|  |  | ||||||
| using CRC_Galileo_CNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>; | using CRC_Galileo_CNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>; | ||||||
|  |  | ||||||
|  |  | ||||||
| bool Galileo_Cnav_Message::CRC_test(const std::bitset<GALILEO_CNAV_BITS_FOR_CRC>& bits, uint32_t checksum) const | bool Galileo_Cnav_Message::CRC_test(const std::bitset<GALILEO_CNAV_BITS_FOR_CRC>& bits, uint32_t checksum) const | ||||||
| { | { | ||||||
|     CRC_Galileo_CNAV_type CRC_Galileo; |     CRC_Galileo_CNAV_type crc_galileo_e6b; | ||||||
|  |  | ||||||
|     // Galileo CNAV frame for CRC is not an integer multiple of bytes |     // Galileo CNAV frame for CRC is not an integer multiple of bytes | ||||||
|     // it needs to be filled with zeroes at the start of the frame. |     // it needs to be filled with zeroes at the start of the frame. | ||||||
|     // This operation is done in the transformation from bits to bytes |     // This operation is done in the transformation from bits to bytes | ||||||
|     // using boost::dynamic_bitset. |     // using boost::dynamic_bitset. | ||||||
|     boost::dynamic_bitset<unsigned char> frame_bits(std::string(bits.to_string())); |     boost::dynamic_bitset<uint8_t> frame_bits(bits.to_string()); | ||||||
|  |  | ||||||
|     std::vector<unsigned char> bytes; |     std::vector<uint8_t> bytes; | ||||||
|  |     bytes.reserve(GALILEO_CNAV_BYTES_FOR_CRC); | ||||||
|     boost::to_block_range(frame_bits, std::back_inserter(bytes)); |     boost::to_block_range(frame_bits, std::back_inserter(bytes)); | ||||||
|     std::reverse(bytes.begin(), bytes.end()); |     std::reverse(bytes.begin(), bytes.end()); | ||||||
|  |  | ||||||
|     CRC_Galileo.process_bytes(bytes.data(), GALILEO_CNAV_BYTES_FOR_CRC); |     crc_galileo_e6b.process_bytes(bytes.data(), GALILEO_CNAV_BYTES_FOR_CRC); | ||||||
|  |  | ||||||
|     const uint32_t crc_computed = CRC_Galileo.checksum(); |     const uint32_t crc_computed = crc_galileo_e6b.checksum(); | ||||||
|     if (checksum == crc_computed) |     if (checksum == crc_computed) | ||||||
|         { |         { | ||||||
|             return true; |             return true; | ||||||
| @@ -57,7 +58,10 @@ void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string) | |||||||
|     const std::string CRC_data = page_string.substr(GALILEO_CNAV_BITS_FOR_CRC, GALILEO_CNAV_CRC_LENGTH); |     const std::string CRC_data = page_string.substr(GALILEO_CNAV_BITS_FOR_CRC, GALILEO_CNAV_CRC_LENGTH); | ||||||
|     const std::bitset<GALILEO_CNAV_BITS_FOR_CRC> Word_for_CRC_bits(has_page_bits); |     const std::bitset<GALILEO_CNAV_BITS_FOR_CRC> Word_for_CRC_bits(has_page_bits); | ||||||
|     const std::bitset<GALILEO_CNAV_CRC_LENGTH> checksum(CRC_data); |     const std::bitset<GALILEO_CNAV_CRC_LENGTH> checksum(CRC_data); | ||||||
|     if (CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true) |     d_new_HAS_page = false; | ||||||
|  |     has_page = Galileo_HAS_page(); | ||||||
|  |     d_flag_CRC_test = CRC_test(Word_for_CRC_bits, checksum.to_ulong()); | ||||||
|  |     if (d_flag_CRC_test) | ||||||
|         { |         { | ||||||
|             d_flag_CRC_test = true; |             d_flag_CRC_test = true; | ||||||
|             // CRC correct: Read 24 bits of HAS page header |             // CRC correct: Read 24 bits of HAS page header | ||||||
| @@ -65,30 +69,39 @@ void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string) | |||||||
|             bool use_has = false; |             bool use_has = false; | ||||||
|             d_test_mode = false; |             d_test_mode = false; | ||||||
|             // HAS status as defined in ICD v1.2 Table 5 HAS Page Header |             // HAS status as defined in ICD v1.2 Table 5 HAS Page Header | ||||||
|             switch (d_has_page_status) |             if (!d_page_dummy) | ||||||
|                 { |                 { | ||||||
|                 case 0:  // HAS is in Test Mode |                     switch (d_has_page_status) | ||||||
|                     use_has = true; |                         { | ||||||
|                     d_test_mode = true; |                         case 0:  // HAS is in Test Mode | ||||||
|                     break; |                             use_has = true; | ||||||
|                 case 1:  // HAS is in Operational Mode |                             d_test_mode = true; | ||||||
|                     use_has = true; |                             break; | ||||||
|                     break; |                         case 1:  // HAS is in Operational Mode | ||||||
|                 case 2:  // HAS is in "reserved" status |                             use_has = true; | ||||||
|                 case 3:  // Do not use HAS |                             break; | ||||||
|                 default: |                         case 2:  // HAS is in "reserved" status | ||||||
|                     break; |                         case 3:  // Do not use HAS | ||||||
|  |                         default: | ||||||
|  |                             break; | ||||||
|  |                         } | ||||||
|                 } |                 } | ||||||
|             if (use_has) |             if (use_has or d_page_dummy) | ||||||
|                 { |                 { | ||||||
|                     // Process the 424 bits of encoded data |                     // Store the 424 bits of encoded data (CNAV page) and the page header | ||||||
|                     process_HAS_page(page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS + GALILEO_CNAV_PAGE_HEADER_BITS, GALILEO_CNAV_MESSAGE_BITS_PER_PAGE)); |                     has_page.has_message_string = page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS + GALILEO_CNAV_PAGE_HEADER_BITS, GALILEO_CNAV_MESSAGE_BITS_PER_PAGE); | ||||||
|  |                     if (!d_page_dummy) | ||||||
|  |                         { | ||||||
|  |                             has_page.has_status = d_has_page_status; | ||||||
|  |                             has_page.reserved = d_has_reserved; | ||||||
|  |                             has_page.message_type = d_received_message_type; | ||||||
|  |                             has_page.message_id = d_received_message_id; | ||||||
|  |                             has_page.message_size = d_received_message_size; | ||||||
|  |                             has_page.message_page_id = d_received_message_page_id; | ||||||
|  |                         } | ||||||
|  |                     d_new_HAS_page = true; | ||||||
|                 } |                 } | ||||||
|         } |         } | ||||||
|     else |  | ||||||
|         { |  | ||||||
|             d_flag_CRC_test = false; |  | ||||||
|         } |  | ||||||
| } | } | ||||||
|  |  | ||||||
|  |  | ||||||
| @@ -98,6 +111,7 @@ void Galileo_Cnav_Message::read_HAS_page_header(const std::string& page_string) | |||||||
|     if (page_string == "101011110011101111000011")  // Equivalent to AF3BC3 |     if (page_string == "101011110011101111000011")  // Equivalent to AF3BC3 | ||||||
|         { |         { | ||||||
|             d_page_dummy = true; |             d_page_dummy = true; | ||||||
|  |             DLOG(INFO) << "HAS page with dummy header received."; | ||||||
|         } |         } | ||||||
|     else |     else | ||||||
|         { |         { | ||||||
| @@ -108,462 +122,19 @@ void Galileo_Cnav_Message::read_HAS_page_header(const std::string& page_string) | |||||||
|             // ICD v1.2 Table 5: HAS page header |             // ICD v1.2 Table 5: HAS page header | ||||||
|             const std::bitset<GALILEO_CNAV_PAGE_HEADER_BITS> has_page_header(page_string); |             const std::bitset<GALILEO_CNAV_PAGE_HEADER_BITS> has_page_header(page_string); | ||||||
|             d_has_page_status = read_has_page_header_parameter(has_page_header, GALILEO_HAS_STATUS); |             d_has_page_status = read_has_page_header_parameter(has_page_header, GALILEO_HAS_STATUS); | ||||||
|  |             d_has_reserved = read_has_page_header_parameter(has_page_header, GALILEO_HAS_RESERVED); | ||||||
|             d_received_message_type = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_TYPE); |             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_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) + 1;  // "0" means 1 |             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); |             d_received_message_page_id = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_PAGE_ID); | ||||||
|         } |  | ||||||
| } |  | ||||||
|  |  | ||||||
|  |             DLOG(INFO) << "HAS page header received:  " | ||||||
| void Galileo_Cnav_Message::process_HAS_page(const std::string& page_string) |                        << "d_has_page_status: " << static_cast<float>(d_has_page_status) << ", " | ||||||
| { |                        << "d_has_reserved: " << static_cast<float>(d_has_reserved) << ", " | ||||||
|     if (d_current_message_id == d_received_message_id) |                        << "d_received_message_type: " << static_cast<float>(d_received_message_type) << ", " | ||||||
|         { |                        << "d_received_message_id: " << static_cast<float>(d_received_message_id) << ", " | ||||||
|             // if receiver pid was not there, store it. |                        << "d_received_message_size: " << static_cast<float>(d_received_message_size) << ", " | ||||||
|             if (d_received_message_page_id == 0) |                        << "d_received_message_page_id: " << static_cast<float>(d_received_message_page_id); | ||||||
|                 { |  | ||||||
|                     // reserved, ignore it |  | ||||||
|                 } |  | ||||||
|             else |  | ||||||
|                 { |  | ||||||
|                     if (std::find(d_list_pid.begin(), d_list_pid.end(), d_received_message_page_id) == d_list_pid.end()) |  | ||||||
|                         { |  | ||||||
|                             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<uint8_t>(bs.to_ulong()); |  | ||||||
|                                         } |  | ||||||
|                                 } |  | ||||||
|                         } |  | ||||||
|                 } |  | ||||||
|         } |  | ||||||
|     else |  | ||||||
|         { |  | ||||||
|             // Start new message |  | ||||||
|             d_current_message_id = d_received_message_id; |  | ||||||
|             d_received_encoded_messages = 0; |  | ||||||
|             d_new_message = false; |  | ||||||
|             d_current_message_size = d_received_message_size; |  | ||||||
|             // 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_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<uint8_t>(bs.to_ulong()); |  | ||||||
|                         } |  | ||||||
|                 } |  | ||||||
|         } |  | ||||||
|  |  | ||||||
|     if (d_received_encoded_messages == d_current_message_size) |  | ||||||
|         { |  | ||||||
|             // we have a full encoded message stored in d_C_matrix |  | ||||||
|             d_received_encoded_messages = 0; |  | ||||||
|             d_current_message_id = 0; |  | ||||||
|  |  | ||||||
|             int res = decode_message_type1(); |  | ||||||
|             if (res == 0) |  | ||||||
|                 { |  | ||||||
|                     d_new_message = true; |  | ||||||
|                 } |  | ||||||
|             else |  | ||||||
|                 { |  | ||||||
|                     d_new_message = false; |  | ||||||
|                 } |  | ||||||
|         } |  | ||||||
| } |  | ||||||
|  |  | ||||||
|  |  | ||||||
| int Galileo_Cnav_Message::decode_message_type1() |  | ||||||
| { |  | ||||||
|     // All rows in d_C_matrix with no data are erasure positions |  | ||||||
|     std::vector<int> erasure_positions; |  | ||||||
|     erasure_positions.reserve(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK - d_list_pid.size()); |  | ||||||
|  |  | ||||||
|     for (int mpid = 1; mpid <= GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK; mpid++) |  | ||||||
|         { |  | ||||||
|             if (std::find(d_list_pid.begin(), d_list_pid.end(), static_cast<uint8_t>(mpid)) == d_list_pid.end()) |  | ||||||
|                 { |  | ||||||
|                     erasure_positions.push_back(static_cast<uint8_t>(mpid - 1)); |  | ||||||
|                 } |  | ||||||
|             else |  | ||||||
|                 { |  | ||||||
|                     d_list_pid.remove(static_cast<uint8_t>(mpid)); |  | ||||||
|                 } |  | ||||||
|         } |  | ||||||
|  |  | ||||||
|     // Vertical decoding of d_C_matrix |  | ||||||
|     for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++) |  | ||||||
|         { |  | ||||||
|             std::vector<uint8_t> C_column(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, 0); |  | ||||||
|             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<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; |  | ||||||
|                     d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; |  | ||||||
|                     return -1; |  | ||||||
|                 } |  | ||||||
|  |  | ||||||
|             std::vector<uint8_t> M_column(C_column.begin(), C_column.begin() + GALILEO_CNAV_INFORMATION_VECTOR_LENGTH); |  | ||||||
|             for (int i = 0; i < GALILEO_CNAV_INFORMATION_VECTOR_LENGTH; i++) |  | ||||||
|                 { |  | ||||||
|                     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<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; |  | ||||||
|     d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(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<GALILEO_CNAV_MT1_HEADER_BITS> 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); |  | ||||||
| } |  | ||||||
|  |  | ||||||
|  |  | ||||||
| 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) |  | ||||||
|         { |  | ||||||
|             // 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<uint64_t> number_sats(d_HAS_data.Nsys, 0); |  | ||||||
|             std::vector<uint64_t> 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<uint64_t> number_sats(d_HAS_data.Nsys, 0); |  | ||||||
|             std::vector<uint64_t> 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()); |  | ||||||
|                 } |  | ||||||
|         } |         } | ||||||
| } | } | ||||||
|  |  | ||||||
| @@ -581,124 +152,3 @@ uint8_t Galileo_Cnav_Message::read_has_page_header_parameter(const std::bitset<G | |||||||
|         } |         } | ||||||
|     return value; |     return value; | ||||||
| } | } | ||||||
|  |  | ||||||
|  |  | ||||||
| uint8_t Galileo_Cnav_Message::read_has_message_header_parameter_uint8(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const |  | ||||||
| { |  | ||||||
|     uint8_t value = 0U; |  | ||||||
|     for (int j = 0; j < parameter.second; j++) |  | ||||||
|         { |  | ||||||
|             value <<= 1U;  // shift left |  | ||||||
|             if (static_cast<int>(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first - j]) == 1) |  | ||||||
|                 { |  | ||||||
|                     value += 1;  // insert the bit |  | ||||||
|                 } |  | ||||||
|         } |  | ||||||
|     return value; |  | ||||||
| } |  | ||||||
|  |  | ||||||
|  |  | ||||||
| uint16_t Galileo_Cnav_Message::read_has_message_header_parameter_uint16(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const |  | ||||||
| { |  | ||||||
|     uint16_t value = 0U; |  | ||||||
|     for (int j = 0; j < parameter.second; j++) |  | ||||||
|         { |  | ||||||
|             value <<= 1U;  // shift left |  | ||||||
|             if (static_cast<int>(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first - j]) == 1) |  | ||||||
|                 { |  | ||||||
|                     value += 1;  // insert the bit |  | ||||||
|                 } |  | ||||||
|         } |  | ||||||
|     return value; |  | ||||||
| } |  | ||||||
|  |  | ||||||
|  |  | ||||||
| bool Galileo_Cnav_Message::read_has_message_header_parameter_bool(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const |  | ||||||
| { |  | ||||||
|     bool value = false; |  | ||||||
|     if (static_cast<int>(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first]) == 1) |  | ||||||
|         { |  | ||||||
|             value = true; |  | ||||||
|         } |  | ||||||
|     return value; |  | ||||||
| } |  | ||||||
|  |  | ||||||
|  |  | ||||||
| uint8_t Galileo_Cnav_Message::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<int>(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<int>(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<int>(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<int>(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<int>(bits[len - 1 - j]) == 1) |  | ||||||
|                 { |  | ||||||
|                     value += 1;  // insert the bit |  | ||||||
|                 } |  | ||||||
|         } |  | ||||||
|  |  | ||||||
|     return value; |  | ||||||
| } |  | ||||||
|   | |||||||
| @@ -20,13 +20,11 @@ | |||||||
| #define GNSS_SDR_GALILEO_CNAV_MESSAGE_H | #define GNSS_SDR_GALILEO_CNAV_MESSAGE_H | ||||||
|  |  | ||||||
| #include "Galileo_CNAV.h" | #include "Galileo_CNAV.h" | ||||||
| #include "galileo_has_data.h" | #include "galileo_has_page.h" | ||||||
| #include "reed_solomon.h" |  | ||||||
| #include <bitset> | #include <bitset> | ||||||
| #include <cstdint> | #include <cstdint> | ||||||
| #include <list> |  | ||||||
| #include <string> | #include <string> | ||||||
| #include <vector> | #include <utility> | ||||||
|  |  | ||||||
| /** \addtogroup Core | /** \addtogroup Core | ||||||
|  * \{ */ |  * \{ */ | ||||||
| @@ -46,64 +44,49 @@ public: | |||||||
|  |  | ||||||
|     void read_HAS_page(const std::string& page_string); |     void read_HAS_page(const std::string& page_string); | ||||||
|  |  | ||||||
|     inline bool have_new_HAS_message() |  | ||||||
|     { |  | ||||||
|         return d_new_message; |  | ||||||
|     } |  | ||||||
|  |  | ||||||
|     inline bool is_HAS_in_test_mode() const |     inline bool is_HAS_in_test_mode() const | ||||||
|     { |     { | ||||||
|         return d_test_mode; |         return d_test_mode; | ||||||
|     } |     } | ||||||
|  |  | ||||||
|     inline bool is_HAS_message_dummy() const |     inline bool is_HAS_page_dummy() const | ||||||
|     { |     { | ||||||
|         return d_page_dummy; |         return d_page_dummy; | ||||||
|     } |     } | ||||||
|  |  | ||||||
|     inline Galileo_HAS_data get_HAS_data() const |     inline bool have_new_HAS_page() const | ||||||
|     { |     { | ||||||
|         return d_HAS_data; |         return d_new_HAS_page; | ||||||
|  |     } | ||||||
|  |  | ||||||
|  |     inline Galileo_HAS_page get_HAS_encoded_page() const | ||||||
|  |     { | ||||||
|  |         return has_page; | ||||||
|  |     } | ||||||
|  |  | ||||||
|  |     inline bool get_flag_CRC_test() const | ||||||
|  |     { | ||||||
|  |         return d_flag_CRC_test; | ||||||
|     } |     } | ||||||
|  |  | ||||||
| private: | private: | ||||||
|  |     uint8_t read_has_page_header_parameter(const std::bitset<GALILEO_CNAV_PAGE_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const; | ||||||
|     bool CRC_test(const std::bitset<GALILEO_CNAV_BITS_FOR_CRC>& bits, uint32_t checksum) const; |     bool CRC_test(const std::bitset<GALILEO_CNAV_BITS_FOR_CRC>& bits, uint32_t checksum) const; | ||||||
|     void read_HAS_page_header(const std::string& page_string); |     void read_HAS_page_header(const std::string& page_string); | ||||||
|     void process_HAS_page(const std::string& page_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(const std::bitset<GALILEO_CNAV_PAGE_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const; |     Galileo_HAS_page has_page{}; | ||||||
|     uint8_t read_has_message_header_parameter_uint8(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const; |  | ||||||
|     uint16_t read_has_message_header_parameter_uint16(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const; |  | ||||||
|     bool read_has_message_header_parameter_bool(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const; |  | ||||||
|  |  | ||||||
|     uint8_t read_has_message_body_uint8(const std::string& bits) const; |  | ||||||
|     uint16_t read_has_message_body_uint16(const std::string& bits) const; |  | ||||||
|     uint64_t read_has_message_body_uint64(const std::string& bits) const; |  | ||||||
|     int16_t read_has_message_body_int16(const std::string& bits) const; |  | ||||||
|  |  | ||||||
|     Galileo_HAS_data d_HAS_data{}; |  | ||||||
|     ReedSolomon rs = ReedSolomon(); |  | ||||||
|     std::vector<std::vector<uint8_t>> d_C_matrix{GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)};  // 255 x 53 |  | ||||||
|     std::vector<std::vector<uint8_t>> d_M_matrix{GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)};         // 32 x 53 |  | ||||||
|     std::list<uint8_t> d_list_pid; |  | ||||||
|  |  | ||||||
|     uint8_t d_has_page_status{}; |     uint8_t d_has_page_status{}; | ||||||
|     uint8_t d_current_message_id{}; |     uint8_t d_has_reserved{}; | ||||||
|     uint8_t d_current_message_size{}; |  | ||||||
|  |  | ||||||
|     uint8_t d_received_message_page_id{}; |     uint8_t d_received_message_page_id{}; | ||||||
|     uint8_t d_received_message_type{}; |     uint8_t d_received_message_type{}; | ||||||
|     uint8_t d_received_message_id{}; |     uint8_t d_received_message_id{}; | ||||||
|     uint8_t d_received_encoded_messages{}; |  | ||||||
|     uint8_t d_received_message_size{}; |     uint8_t d_received_message_size{}; | ||||||
|  |  | ||||||
|     bool d_test_mode{}; |     bool d_test_mode{}; | ||||||
|     bool d_new_message{}; |  | ||||||
|     bool d_flag_CRC_test{}; |     bool d_flag_CRC_test{}; | ||||||
|     bool d_page_dummy{}; |     bool d_page_dummy{}; | ||||||
|  |     bool d_new_HAS_page{}; | ||||||
| }; | }; | ||||||
|  |  | ||||||
|  |  | ||||||
|   | |||||||
							
								
								
									
										54
									
								
								src/core/system_parameters/galileo_has_page.h
									
									
									
									
									
										Normal file
									
								
							
							
						
						
									
										54
									
								
								src/core/system_parameters/galileo_has_page.h
									
									
									
									
									
										Normal file
									
								
							| @@ -0,0 +1,54 @@ | |||||||
|  | /*! | ||||||
|  |  * \file galileo_has_page.h | ||||||
|  |  * \brief Class for Galileo HAS message page storage | ||||||
|  |  * \author Carles Fernandez-Prades, 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-2021  (see AUTHORS file for a list of contributors) | ||||||
|  |  * SPDX-License-Identifier: GPL-3.0-or-later | ||||||
|  |  * | ||||||
|  |  * ----------------------------------------------------------------------------- | ||||||
|  |  */ | ||||||
|  |  | ||||||
|  |  | ||||||
|  | #ifndef GNSS_SDR_GALILEO_HAS_PAGE_H | ||||||
|  | #define GNSS_SDR_GALILEO_HAS_PAGE_H | ||||||
|  |  | ||||||
|  | #include <cstdint> | ||||||
|  | #include <string> | ||||||
|  |  | ||||||
|  | /** \addtogroup Core | ||||||
|  |  * \{ */ | ||||||
|  | /** \addtogroup System_Parameters | ||||||
|  |  * \{ */ | ||||||
|  |  | ||||||
|  |  | ||||||
|  | /*! | ||||||
|  |  * \brief This class is a storage for Galileo HAS message page, as defined in | ||||||
|  |  * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 | ||||||
|  |  * (April 2020). | ||||||
|  |  */ | ||||||
|  | class Galileo_HAS_page | ||||||
|  | { | ||||||
|  | public: | ||||||
|  |     Galileo_HAS_page() = default; | ||||||
|  |  | ||||||
|  |     std::string has_message_string;  //!< HAS message content | ||||||
|  |  | ||||||
|  |     // HAS page header | ||||||
|  |     uint8_t has_status{};       //!< HAS status | ||||||
|  |     uint8_t reserved{};         //!< HAS reserved field | ||||||
|  |     uint8_t message_type{};     //!< HAS message type (MT) | ||||||
|  |     uint8_t message_id{};       //!< HAS message ID (MID) | ||||||
|  |     uint8_t message_size{};     //!< HAS message size (MS) | ||||||
|  |     uint8_t message_page_id{};  //!< HAS message page ID (PID) | ||||||
|  | }; | ||||||
|  |  | ||||||
|  |  | ||||||
|  | /** \} */ | ||||||
|  | /** \} */ | ||||||
|  | #endif  // GNSS_SDR_GALILEO_HAS_PAGE_H | ||||||
		Reference in New Issue
	
	Block a user
	 Carles Fernandez
					Carles Fernandez