Add logic for HAS message retrieval

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
+    // 4. If we have a new full message, read it
-    if (d_cnav_nav.get_flag_CRC_test() == true)
+    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
                 {
-            DLOG(INFO) << "Galileo E6B CRC error in channel " << d_channel << " from satellite " << d_satellite;
+                    std::cout << TEXT_MAGENTA << "New Galileo E6 HAS dummy message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n';
+                }
         }
-    // TODO
-    // Get full HAS message from different pages
-    // Reed Solomon decoding
-    // Retrieve data from message
 }
 
 

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

src/core/system_parameters/Galileo_CNAV.h

@@ -24,6 +24,7 @@
 #define GNSS_SDR_GALILEO_CNAV_H
 
 #include <cstdint>
+#include <utility>
 
 /** \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<int32_t, int32_t> GALILEO_HAS_STATUS({1, 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_SIZE({12, 5});
+const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_PAGE_ID({17, 8});
+
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_TOH({1, 12});
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_MASK_FLAG({13, 1});
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_ORBIT_CORRECTION_FLAG({14, 1});
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_CLOCK_FULLSET_FLAG({15, 1});
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_CLOCK_SUBSET_FLAG({16, 1});
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_CODE_BIAS_FLAG({17, 1});
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_PHASE_BIAS_FLAG({18, 1});
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_URA_FLAG({19, 1});
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_MASK_ID({23, 5});
+const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_IOD_ID({28, 5});
+
 
 /** \} */
 /** \} */

src/core/system_parameters/galileo_cnav_message.cc

@@ -22,7 +22,7 @@
 #include "galileo_cnav_message.h"
 #include <boost/crc.hpp>             // for boost::crc_basic, boost::crc_optimal
 #include <boost/dynamic_bitset.hpp>  // for boost::dynamic_bitset
-#include <algorithm>                 // for reverse
+#include <algorithm>                 // 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<GALILEO_CNAV_BITS_FOR_CRC> 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<GALILEO_CNAV_BITS_FOR_CRC> Word_for_CRC_bits(has_page_bits);
-    const std::bitset<24> checksum(CRC_data);
+    const std::bitset<GALILEO_CNAV_CRC_LENGTH> 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<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_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<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)
+{
+    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<GALILEO_CNAV_PAGE_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_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<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(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(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;
+}

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 <bitset>
 #include <cstdint>
+#include <list>
 #include <string>
 
 /** \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<GALILEO_CNAV_BITS_FOR_CRC> 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<GALILEO_CNAV_PAGE_HEADER_BITS> bits, const std::pair<int32_t, int32_t>& parameter) const;
+    uint8_t read_has_message_header_parameter_uint8(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(std::bitset<GALILEO_CNAV_MT1_HEADER_BITS> bits, const std::pair<int32_t, int32_t>& parameter) const;
+    bool read_has_message_header_parameter_bool(std::bitset<GALILEO_CNAV_MT1_HEADER_BITS> bits, const std::pair<int32_t, int32_t>& parameter) const;
+
+    Galileo_HAS_data d_HAS_data{};
+
+    std::string d_encoded_message_type_1;
+    std::list<uint8_t> 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{};
 };
 
 

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 <cstdint>
+#include <vector>
+
+/** \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<uint8_t> gnss_id_mask;
+    std::vector<uint64_t> satellite_mask;
+    std::vector<uint16_t> signal_mask;
+    std::vector<bool> cell_mask_availability_flag;
+    std::vector<std::vector<std::vector<uint8_t>>> cell_mask;
+    std::vector<uint8_t> nav_message;
+
+    // Orbit corrections
+    uint8_t validity_interval_index_orbit_corrections;
+    std::vector<uint16_t> gnss_iod;
+    std::vector<int16_t> delta_radial;
+    std::vector<int16_t> delta_along_track;
+    std::vector<int16_t> delta_cross_track;
+
+    // Clock full-set corrections
+    uint8_t validity_interval_index_clock_fullset_corrections;
+    std::vector<uint8_t> delta_clock_c0_multiplier;
+    std::vector<bool> iod_change_flag;
+    std::vector<int16_t> delta_clock_c0;
+
+    // Clock subset corrections
+    uint8_t validity_interval_index_clock_subset_corrections;
+    uint8_t Nsysprime;
+    std::vector<uint8_t> gnss_id_clock_subset;
+    std::vector<uint8_t> delta_clock_c0_multiplier_clock_subset;
+    std::vector<std::vector<uint64_t>> satellite_submask;
+    std::vector<bool> iod_change_flag_clock_subset;
+    std::vector<int16_t> delta_clock_c0_clock_subset;
+
+    // Code bias
+    uint8_t validity_interval_index_code_bias_corrections;
+    std::vector<std::vector<int16_t>> code_bias;
+
+    // Phase bias
+    uint8_t validity_interval_index_phase_bias_corrections;
+    std::vector<std::vector<int16_t>> phase_bias;
+    std::vector<std::vector<uint8_t>> phase_discontinuity_indicator;
+
+    // URA
+    uint8_t validity_interval_index_ura_corrections;
+    std::vector<uint8_t> ura;
+};
+
+
+/** \} */
+/** \} */
+#endif  // GNSS_SDR_GALILEO_HAS_DATA_H