Start work on OSNMA

src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc

@@ -430,11 +430,11 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
             const std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(d_inav_nav.get_ephemeris());
             if (d_band == '1')
                 {
-                    std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << '\n';
+                    std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << std::endl;
                 }
             else if (d_band == '7')
                 {
-                    std::cout << TEXT_BLUE << "New Galileo E5b I/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n';
+                    std::cout << TEXT_BLUE << "New Galileo E5b I/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl;
                 }
             this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
             d_first_eph_sent = true;  // do not send reduced CED anymore, since we have the full ephemeris set
@@ -445,7 +445,7 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
             if ((d_band == '1') && !d_first_eph_sent && (d_inav_nav.have_new_reduced_ced() == true))
                 {
                     const std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(d_inav_nav.get_reduced_ced());
-                    std::cout << "New Galileo E1 I/NAV reduced CED message received in channel " << d_channel << " from satellite " << d_satellite << '\n';
+                    std::cout << "New Galileo E1 I/NAV reduced CED message received in channel " << d_channel << " from satellite " << d_satellite << std::endl;
                     this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
                 }
         }
@@ -456,11 +456,11 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
             const std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(d_inav_nav.get_iono());
             if (d_band == '1')
                 {
-                    std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << '\n';
+                    std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << std::endl;
                 }
             else if (d_band == '7')
                 {
-                    std::cout << TEXT_BLUE << "New Galileo E5b I/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
+                    std::cout << TEXT_BLUE << "New Galileo E5b I/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
                 }
             this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
         }
@@ -471,11 +471,11 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
             const std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(d_inav_nav.get_utc_model());
             if (d_band == '1')
                 {
-                    std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << '\n';
+                    std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << std::endl;
                 }
             else if (d_band == '7')
                 {
-                    std::cout << TEXT_BLUE << "New Galileo E5b I/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
+                    std::cout << TEXT_BLUE << "New Galileo E5b I/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
                 }
             this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
             d_delta_t = tmp_obj->A_0G + tmp_obj->A_1G * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - tmp_obj->t_0G + 604800 * (std::fmod(static_cast<float>(d_inav_nav.get_Galileo_week() - tmp_obj->WN_0G), 64.0)));
@@ -489,16 +489,23 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
             // debug
             if (d_band == '1')
                 {
-                    std::cout << "Galileo E1 I/NAV almanac received in channel " << d_channel << " from satellite " << d_satellite << '\n';
+                    std::cout << "Galileo E1 I/NAV almanac received in channel " << d_channel << " from satellite " << d_satellite << std::endl;
                 }
             else if (d_band == '7')
                 {
-                    std::cout << TEXT_BLUE << "Galileo E5b I/NAV almanac received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n';
+                    std::cout << TEXT_BLUE << "Galileo E5b I/NAV almanac received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << std::endl;
                 }
             DLOG(INFO) << "Current parameters:";
             DLOG(INFO) << "d_TOW_at_current_symbol_ms=" << d_TOW_at_current_symbol_ms;
             DLOG(INFO) << "d_nav.WN_0=" << d_inav_nav.get_Galileo_week();
         }
+
+    if (d_band == '1' && d_inav_nav.have_new_nma() == true)
+        {
+            const std::shared_ptr<OSNMA_msg> tmp_obj = std::make_shared<OSNMA_msg>(d_inav_nav.get_osnma_msg());
+            // this->message_port_pub(pmt::mp("whatever"), pmt::make_any(tmp_obj));
+            std::cout << "Galileo OSNMA message received in channel " << d_channel << " from satellite " << d_satellite << std::endl;
+        }
 }
 
 
@@ -557,21 +564,21 @@ void galileo_telemetry_decoder_gs::decode_FNAV_word(float *page_symbols, int32_t
     if (d_fnav_nav.have_new_ephemeris() == true)
         {
             const std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(d_fnav_nav.get_ephemeris());
-            std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n';
+            std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl;
             this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
         }
 
     if (d_fnav_nav.have_new_iono_and_GST() == true)
         {
             const std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(d_fnav_nav.get_iono());
-            std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
+            std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
             this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
         }
 
     if (d_fnav_nav.have_new_utc_model() == true)
         {
             const std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(d_fnav_nav.get_utc_model());
-            std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
+            std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
             this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
         }
 }
@@ -631,7 +638,7 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(uint64_t time_stamp, float *
                             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';
+                                      << TEXT_RESET << std::endl;
                         }
                 }
             else
@@ -648,7 +655,7 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(uint64_t time_stamp, float *
                             std::cout << TEXT_MAGENTA << "Receiving Galileo E6 HAS pages"
                                       << (d_cnav_nav.is_HAS_in_test_mode() == true ? " (test mode) " : " ")
                                       << "in channel " << d_channel << " from satellite " << d_satellite
-                                      << TEXT_RESET << '\n';
+                                      << TEXT_RESET << std::endl;
                         }
                 }
         }

src/core/system_parameters/galileo_inav_message.cc

@@ -176,8 +176,6 @@ bool Galileo_Inav_Message::read_navigation_bool(const std::bitset<GALILEO_DATA_J
 
 void Galileo_Inav_Message::split_page(std::string page_string, int32_t flag_even_word)
 {
-    int32_t Page_type = 0;
-
     if (page_string.at(0) == '1')  // if page is odd
         {
             const std::string& page_Odd = page_string;
@@ -186,21 +184,16 @@ void Galileo_Inav_Message::split_page(std::string page_string, int32_t flag_even
                 {
                     const std::string page_INAV_even = page_Even;
                     const std::string page_INAV = page_INAV_even + page_Odd;  // Join pages: Even + Odd = INAV page
-                    const std::string Even_bit = page_INAV.substr(0, 1);
-                    const std::string Page_type_even = page_INAV.substr(1, 1);
-                    const std::string nominal = "0";
 
                     const std::string Data_k = page_INAV.substr(2, 112);
-                    const std::string Odd_bit = page_INAV.substr(114, 1);
-                    const std::string Page_type_Odd = page_INAV.substr(115, 1);
                     const std::string Data_j = page_INAV.substr(116, 16);
 
-                    const std::string Reserved_1 = page_INAV.substr(132, 40);
+                    const std::string osnma_sis = page_INAV.substr(132, 40);
-                    const std::string SAR = page_INAV.substr(172, 22);
+                    // const std::string SAR = page_INAV.substr(172, 22);
-                    const std::string Spare = page_INAV.substr(194, 2);
+                    // const std::string Spare = page_INAV.substr(194, 2);
                     const std::string CRC_data = page_INAV.substr(196, 24);
-                    const std::string Reserved_2 = page_INAV.substr(220, 8);
+                    // const std::string Reserved_2 = page_INAV.substr(220, 8);
-                    const std::string Tail_odd = page_INAV.substr(228, 6);
+                    // const std::string Tail_odd = page_INAV.substr(228, 6);
 
                     // ************ CRC checksum control *******/
                     std::stringstream TLM_word_for_CRC_stream;
@@ -213,12 +206,29 @@ void Galileo_Inav_Message::split_page(std::string page_string, int32_t flag_even
                         {
                             flag_CRC_test = true;
                             // CRC correct: Decode word
-                            const std::string page_number_bits = Data_k.substr(0, 6);
-                            const std::bitset<GALILEO_PAGE_TYPE_BITS> page_type_bits(page_number_bits);  // from string to bitset
-                            Page_type = static_cast<int32_t>(read_page_type_unsigned(page_type_bits, TYPE));
-                            Page_type_time_stamp = Page_type;
                             const std::string Data_jk_ephemeris = Data_k + Data_j;
                             page_jk_decoder(Data_jk_ephemeris.c_str());
+
+                            // Fill OSNMA data
+                            if (page_position_in_inav_subframe != 255)
+                                {
+                                    if (page_position_in_inav_subframe == 0)
+                                        {
+                                            nma_position_filled = std::array<int8_t, 15>{};
+                                            nma_msg.mack = std::array<uint32_t, 15>{};
+                                            nma_msg.hkroot = std::array<uint8_t, 15>{};
+                                        }
+                                    std::bitset<8> hkroot_bs(osnma_sis.substr(0, 8));
+                                    std::bitset<32> mack_bs(osnma_sis.substr(8, 32));
+                                    if (hkroot_bs.count() != 0 && mack_bs.count() != 0)
+                                        {
+                                            hkroot_sis = static_cast<uint8_t>(hkroot_bs.to_ulong());
+                                            mack_sis = static_cast<uint32_t>(mack_bs.to_ulong());
+                                            nma_msg.mack[page_position_in_inav_subframe] = mack_sis;
+                                            nma_msg.hkroot[page_position_in_inav_subframe] = hkroot_sis;
+                                            nma_position_filled[page_position_in_inav_subframe] = 1;
+                                        }
+                                }
                         }
                     else
                         {
@@ -690,6 +700,7 @@ void Galileo_Inav_Message::read_page_4(const std::bitset<GALILEO_DATA_JK_BITS>&
     IOD_nav_4 = static_cast<int32_t>(read_navigation_unsigned(data_bits, IOD_NAV_4_BIT));
     DLOG(INFO) << "IOD_nav_4= " << IOD_nav_4;
     SV_ID_PRN_4 = static_cast<int32_t>(read_navigation_unsigned(data_bits, SV_ID_PRN_4_BIT));
+    nma_msg.PRN = static_cast<uint32_t>(SV_ID_PRN_4);
     DLOG(INFO) << "SV_ID_PRN_4= " << SV_ID_PRN_4;
     C_ic_4 = static_cast<double>(read_navigation_signed(data_bits, C_IC_4_BIT));
     C_ic_4 = C_ic_4 * C_IC_4_LSB;
@@ -844,6 +855,11 @@ int32_t Galileo_Inav_Message::page_jk_decoder(const char* data_jk)
     const auto page_number = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, PAGE_TYPE_BIT));
     DLOG(INFO) << "Page number = " << page_number;
 
+    if (page_position_in_inav_subframe != 255)
+        {
+            page_position_in_inav_subframe++;
+        }
+
     switch (page_number)
         {
         case 1:  // Word type 1: Ephemeris (1/4)
@@ -884,6 +900,10 @@ int32_t Galileo_Inav_Message::page_jk_decoder(const char* data_jk)
 
         case 2:  // Word type 2: Ephemeris (2/4)
             {
+                page_position_in_inav_subframe = 0;
+                nma_msg.mack = std::array<uint32_t, 15>{};
+                nma_msg.hkroot = std::array<uint8_t, 15>{};
+                nma_position_filled = std::array<int8_t, 15>{};
                 read_page_2(data_jk_bits);
                 if (enable_rs)
                     {
@@ -1371,5 +1391,35 @@ int32_t Galileo_Inav_Message::page_jk_decoder(const char* data_jk)
         default:
             break;
         }
+
+    if (page_position_in_inav_subframe > 14 && page_position_in_inav_subframe != 255)
+        {
+            // something weird happened, reset
+            page_position_in_inav_subframe = 255;
+            nma_position_filled = std::array<int8_t, 15>{};
+            nma_msg.mack = std::array<uint32_t, 15>{};
+            nma_msg.hkroot = std::array<uint8_t, 15>{};
+        }
+
     return page_number;
 }
+
+
+OSNMA_msg Galileo_Inav_Message::get_osnma_msg() const
+{
+    return nma_msg;
+}
+
+
+bool Galileo_Inav_Message::have_new_nma()
+{
+    if (std::all_of(nma_position_filled.begin(), nma_position_filled.end(),
+            [](int32_t element) { return element == 1; }))
+        {
+            return true;
+        }
+    else
+        {
+            return false;
+        }
+}

src/core/system_parameters/galileo_inav_message.h

@@ -25,6 +25,7 @@
 #include "galileo_iono.h"
 #include "galileo_utc_model.h"
 #include "gnss_sdr_make_unique.h"  // for std::unique_ptr in C++11
+#include <array>
 #include <bitset>
 #include <cstdint>
 #include <memory>
@@ -39,6 +40,14 @@ class ReedSolomon;  // Forward declaration of the ReedSolomon class
 /** \addtogroup System_Parameters
  * \{ */
 
+class OSNMA_msg
+{
+public:
+    OSNMA_msg() = default;
+    std::array<uint32_t, 15> mack{};
+    std::array<uint8_t, 15> hkroot{};
+    uint32_t PRN{};
+};
 
 /*!
  * \brief This class handles the Galileo I/NAV Data message, as described in the
@@ -57,13 +66,6 @@ public:
      */
     void split_page(std::string page_string, int32_t flag_even_word);
 
-    /*
-     * \brief Takes in input Data_jk (128 bit) and split it in ephemeris parameters according ICD 4.3.5
-     *
-     * Takes in input Data_jk (128 bit) and split it in ephemeris parameters according ICD 4.3.5
-     */
-    int32_t page_jk_decoder(const char* data_jk);
-
     /*
      * \brief Returns true if new Ephemeris has arrived. The flag is set to false when the function is executed
      */
@@ -89,6 +91,11 @@ public:
      */
     bool have_new_reduced_ced();
 
+    /*
+     * \brief Returns true if new NMA data have arrived. The flag is set to false when the function is executed
+     */
+    bool have_new_nma();
+
     /*
      * \brief Returns a Galileo_Ephemeris object filled with the latest navigation data received
      */
@@ -114,6 +121,11 @@ public:
      */
     Galileo_Ephemeris get_reduced_ced() const;
 
+    /*
+     * \brief Returns an OSNMA_msg object filled with the latest NMA message received. Resets msg buffer.
+     */
+    OSNMA_msg get_osnma_msg() const;
+
     inline bool get_flag_CRC_test() const
     {
         return flag_CRC_test;
@@ -210,6 +222,11 @@ public:
     inline void init_PRN(uint32_t prn)
     {
         SV_ID_PRN_4 = prn;
+        nma_msg.PRN = prn;
+        nma_msg.mack = std::array<uint32_t, 15>{};
+        nma_msg.hkroot = std::array<uint8_t, 15>{};
+        page_position_in_inav_subframe = 255;
+        nma_position_filled = std::array<int8_t, 15>{};
     }
 
     /*
@@ -242,7 +259,7 @@ private:
     std::unique_ptr<ReedSolomon> rs;  // The Reed-Solomon decoder
     std::vector<int> inav_rs_pages;   // Pages 1,2,3,4,17,18,19,20. Holds 1 if the page has arrived, 0 otherwise.
 
-    int32_t Page_type_time_stamp{};
+    int32_t page_jk_decoder(const char* data_jk);
     int32_t IOD_ephemeris{};
 
     // Word type 1: Ephemeris (1/4)
@@ -394,6 +411,13 @@ private:
 
     int32_t current_IODnav{};
 
+    // OSNMA
+    uint32_t mack_sis{};
+    uint8_t hkroot_sis{};
+    uint8_t page_position_in_inav_subframe{255};
+    std::array<int8_t, 15> nma_position_filled{};
+    OSNMA_msg nma_msg{};
+
     uint8_t IODnav_LSB17{};
     uint8_t IODnav_LSB18{};
     uint8_t IODnav_LSB19{};