From d1b23ae3fd48b6b3f913cd8bcb760513d50cb27c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 10 Nov 2020 21:20:13 +0100 Subject: [PATCH] Add logic for HAS message retrieval --- .../galileo_telemetry_decoder_gs.cc | 23 +- src/core/system_parameters/CMakeLists.txt | 1 + src/core/system_parameters/Galileo_CNAV.h | 22 ++ .../system_parameters/galileo_cnav_message.cc | 223 +++++++++++++++++- .../system_parameters/galileo_cnav_message.h | 53 ++++- src/core/system_parameters/galileo_has_data.h | 106 +++++++++ 6 files changed, 409 insertions(+), 19 deletions(-) create mode 100644 src/core/system_parameters/galileo_has_data.h 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 b9afe7e1d..1625844f9 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 @@ -486,19 +486,20 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t } d_cnav_nav.read_HAS_page(page_String); - // 4. Check CRC - if (d_cnav_nav.get_flag_CRC_test() == true) + // 4. If we have a new full message, read it + if (d_cnav_nav.have_new_HAS_message() == true) { - DLOG(INFO) << "Galileo E6B CRC correct in channel " << d_channel << " from satellite " << d_satellite; + // 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()) + { + std::cout << TEXT_MAGENTA << "New Galileo E6 HAS 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'; + } } - else - { - DLOG(INFO) << "Galileo E6B CRC error in channel " << d_channel << " from satellite " << d_satellite; - } - // TODO - // Get full HAS message from different pages - // Reed Solomon decoding - // Retrieve data from message } diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index ee4648438..e0aa0f8a9 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -50,6 +50,7 @@ set(SYSTEM_PARAMETERS_HEADERS galileo_iono.h galileo_cnav_message.h galileo_fnav_message.h + galileo_has_data.h galileo_inav_message.h sbas_ephemeris.h gps_cnav_ephemeris.h diff --git a/src/core/system_parameters/Galileo_CNAV.h b/src/core/system_parameters/Galileo_CNAV.h index a0a3999aa..ec867a55b 100644 --- a/src/core/system_parameters/Galileo_CNAV.h +++ b/src/core/system_parameters/Galileo_CNAV.h @@ -24,6 +24,7 @@ #define GNSS_SDR_GALILEO_CNAV_H #include +#include /** \addtogroup Core * \{ */ @@ -42,11 +43,32 @@ constexpr int32_t GALILEO_CNAV_HAS_PAGE_DATA_BITS = 448; 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_BYTES_FOR_CRC = 60; +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_MT1_HEADER_BITS = 32; + constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000"; +const std::pair GALILEO_HAS_STATUS({1, 2}); +const std::pair GALILEO_HAS_MESSAGE_TYPE({5, 2}); +const std::pair GALILEO_HAS_MESSAGE_ID({7, 5}); +const std::pair GALILEO_HAS_MESSAGE_SIZE({12, 5}); +const std::pair GALILEO_HAS_MESSAGE_PAGE_ID({17, 8}); + +const std::pair GALILEO_MT1_HEADER_TOH({1, 12}); +const std::pair GALILEO_MT1_HEADER_MASK_FLAG({13, 1}); +const std::pair GALILEO_MT1_HEADER_ORBIT_CORRECTION_FLAG({14, 1}); +const std::pair GALILEO_MT1_HEADER_CLOCK_FULLSET_FLAG({15, 1}); +const std::pair GALILEO_MT1_HEADER_CLOCK_SUBSET_FLAG({16, 1}); +const std::pair GALILEO_MT1_HEADER_CODE_BIAS_FLAG({17, 1}); +const std::pair GALILEO_MT1_HEADER_PHASE_BIAS_FLAG({18, 1}); +const std::pair GALILEO_MT1_HEADER_URA_FLAG({19, 1}); +const std::pair GALILEO_MT1_HEADER_MASK_ID({23, 5}); +const std::pair GALILEO_MT1_HEADER_IOD_ID({28, 5}); + /** \} */ /** \} */ diff --git a/src/core/system_parameters/galileo_cnav_message.cc b/src/core/system_parameters/galileo_cnav_message.cc index 32c121856..cf2b3236d 100644 --- a/src/core/system_parameters/galileo_cnav_message.cc +++ b/src/core/system_parameters/galileo_cnav_message.cc @@ -22,7 +22,7 @@ #include "galileo_cnav_message.h" #include // for boost::crc_basic, boost::crc_optimal #include // for boost::dynamic_bitset -#include // for reverse +#include // for reverse, find using CRC_Galileo_CNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>; @@ -56,16 +56,229 @@ bool Galileo_Cnav_Message::CRC_test(std::bitset bits, void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string) { const std::string has_page_bits = page_string.substr(0, GALILEO_CNAV_BITS_FOR_CRC); - const std::string CRC_data = page_string.substr(GALILEO_CNAV_BITS_FOR_CRC, 24); + const std::string CRC_data = page_string.substr(GALILEO_CNAV_BITS_FOR_CRC, GALILEO_CNAV_CRC_LENGTH); const std::bitset Word_for_CRC_bits(has_page_bits); - const std::bitset<24> checksum(CRC_data); + const std::bitset checksum(CRC_data); if (CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true) { - flag_CRC_test = true; + d_flag_CRC_test = true; // CRC correct: Read 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; + switch (d_has_page_status) + { + case 0: // HAS is in Test Mode + use_has = true; + d_test_mode = true; + break; + case 1: // HAS is in Operational Mode + use_has = true; + break; + default: + break; + } + if (use_has) + { + process_HAS_page(page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS + GALILEO_CNAV_PAGE_HEADER_BITS, GALILEO_CNAV_MESSAGE_BITS_PER_PAGE)); + } } else { - flag_CRC_test = false; + d_flag_CRC_test = false; } } + + +void Galileo_Cnav_Message::read_HAS_page_header(const std::string& page_string) +{ + // check if dummy + if (page_string == "101011110011101111000011") // Equivalent to AF3BC3 + { + d_page_dummy = true; + } + else + { + d_page_dummy = false; + } + if (!d_page_dummy) + { + 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_page_id = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_PAGE_ID); + } +} + + +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_type == 1) // contains satellite corrections + { + 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); + } + } + } + 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 start storing again + d_encoded_message_type_1.clear(); + d_list_pid.clear(); + 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); + } + } + + if (d_received_encoded_messages == d_current_message_size) + { + // we have a full encoded message stored in d_encoded_message_type_1 + d_received_encoded_messages = 0; + d_current_message_id = 0; + d_new_message = true; + decode_message_type1(); + } +} + + +void 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); +} + + +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); +} + + +void Galileo_Cnav_Message::read_MT1_header(const std::string& message_string) +{ + 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); +} + + +void Galileo_Cnav_Message::read_MT1_body(const std::string& message_string) +{ + auto message = std::string(message_string.begin() + GALILEO_CNAV_MT1_HEADER_BITS, message_string.end()); // Remove header + if (d_HAS_data.header.mask_flag) + { + // read mask + // size_t mask_flag_size = X; + // message = std::string(message.begin() + mask_flag_size, message.end()); // Remove mask_flag + } + if (d_HAS_data.header.orbit_correction_flag) + { + // read orbit corrections + } + if (d_HAS_data.header.clock_fullset_flag) + { + // read clock full-set corrections + } + if (d_HAS_data.header.clock_subset_flag) + { + // read clock subset corrections + } + if (d_HAS_data.header.code_bias_flag) + { + // read code bias + } + if (d_HAS_data.header.phase_bias_flag) + { + // read phase bias + } + if (d_HAS_data.header.ura_flag) + { + // read URA + } +} + + +uint8_t Galileo_Cnav_Message::read_has_page_header_parameter(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_PAGE_HEADER_BITS - parameter.first - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +uint8_t Galileo_Cnav_Message::read_has_message_header_parameter_uint8(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_Cnav_Message::read_has_message_header_parameter_uint16(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_Cnav_Message::read_has_message_header_parameter_bool(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; +} diff --git a/src/core/system_parameters/galileo_cnav_message.h b/src/core/system_parameters/galileo_cnav_message.h index 5f664b20f..5c04af1f7 100644 --- a/src/core/system_parameters/galileo_cnav_message.h +++ b/src/core/system_parameters/galileo_cnav_message.h @@ -23,8 +23,10 @@ #define GNSS_SDR_GALILEO_CNAV_MESSAGE_H #include "Galileo_CNAV.h" +#include "galileo_has_data.h" #include #include +#include #include /** \addtogroup Core @@ -45,14 +47,59 @@ public: void read_HAS_page(const std::string& page_string); - inline bool get_flag_CRC_test() const + inline bool have_new_HAS_message() { - return flag_CRC_test; + return d_new_message; + } + + inline bool is_HAS_in_test_mode() const + { + return d_test_mode; + } + + inline bool is_HAS_message_dummy() const + { + return d_page_dummy; + } + + inline Galileo_HAS_data get_HAS_data() const + { + return d_HAS_data; } private: bool CRC_test(std::bitset bits, uint32_t checksum) const; - bool flag_CRC_test{}; + 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); + + 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; + + Galileo_HAS_data d_HAS_data{}; + + std::string d_encoded_message_type_1; + std::list d_list_pid; + + uint8_t d_has_page_status{}; + uint8_t d_current_message_id{}; + uint8_t d_current_message_size{}; + + uint8_t d_received_message_page_id{}; + uint8_t d_received_message_type{}; + uint8_t d_received_message_id{}; + uint8_t d_received_encoded_messages{}; + uint8_t d_received_message_size{}; + + bool d_test_mode{}; + bool d_new_message{}; + bool d_flag_CRC_test{}; + bool d_page_dummy{}; }; diff --git a/src/core/system_parameters/galileo_has_data.h b/src/core/system_parameters/galileo_has_data.h new file mode 100644 index 000000000..059cd6212 --- /dev/null +++ b/src/core/system_parameters/galileo_has_data.h @@ -0,0 +1,106 @@ +/*! + * \file galileo_has_data.h + * \brief Class for Galileo HAS message type 1 data storage + * \author Carles Fernandez-Prades, 2020 cfernandez(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GALILEO_HAS_DATA_H +#define GNSS_SDR_GALILEO_HAS_DATA_H + +#include +#include + +/** \addtogroup Core + * \{ */ +/** \addtogroup System_Parameters + * \{ */ + +struct mt1_header +{ + uint16_t toh; + uint8_t mask_id; + uint8_t iod_id; + bool mask_flag; + bool orbit_correction_flag; + bool clock_fullset_flag; + bool clock_subset_flag; + bool code_bias_flag; + bool phase_bias_flag; + bool ura_flag; +}; + +/*! + * \brief This class is a storage for Galileo HAS message type 1, as defined in + * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 + * (April 2020). + */ +class Galileo_HAS_data +{ +public: + Galileo_HAS_data() = default; + + mt1_header header; + + // Mask + uint8_t Nsys; + std::vector gnss_id_mask; + std::vector satellite_mask; + std::vector signal_mask; + std::vector cell_mask_availability_flag; + std::vector>> cell_mask; + std::vector nav_message; + + // Orbit corrections + uint8_t validity_interval_index_orbit_corrections; + std::vector gnss_iod; + std::vector delta_radial; + std::vector delta_along_track; + std::vector delta_cross_track; + + // Clock full-set corrections + uint8_t validity_interval_index_clock_fullset_corrections; + std::vector delta_clock_c0_multiplier; + std::vector iod_change_flag; + std::vector delta_clock_c0; + + // Clock subset corrections + uint8_t validity_interval_index_clock_subset_corrections; + uint8_t Nsysprime; + std::vector gnss_id_clock_subset; + std::vector delta_clock_c0_multiplier_clock_subset; + std::vector> satellite_submask; + std::vector iod_change_flag_clock_subset; + std::vector delta_clock_c0_clock_subset; + + // Code bias + uint8_t validity_interval_index_code_bias_corrections; + std::vector> code_bias; + + // Phase bias + uint8_t validity_interval_index_phase_bias_corrections; + std::vector> phase_bias; + std::vector> phase_discontinuity_indicator; + + // URA + uint8_t validity_interval_index_ura_corrections; + std::vector ura; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_GALILEO_HAS_DATA_H