From 46aca38adec96cb96dd15bcd4f08596233bddd9f Mon Sep 17 00:00:00 2001
From: Carles Fernandez <carles.fernandez@gmail.com>
Date: Sat, 27 May 2023 11:43:41 +0200
Subject: [PATCH] Add work on osnma receiver

---
 src/core/libs/osnma_msg_receiver.cc        | 5 ++++-
 src/core/system_parameters/Galileo_OSNMA.h | 6 ++++++
 2 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc
index c551ae1b0..ec875d696 100644
--- a/src/core/libs/osnma_msg_receiver.cc
+++ b/src/core/libs/osnma_msg_receiver.cc
@@ -148,7 +148,7 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_
     if (d_osnma_data.d_dsm_header.dsm_block_id == 0)
         {
             // Get number of blocks in message
-            uint8_t nb = (osnma_msg->hkroot[2] & 0b11110000) >> 4;
+            uint8_t nb = get_number_blocks(d_dsm_message[d_osnma_data.d_dsm_header.dsm_id][0]);
             uint16_t number_of_blocks = 0;
             if (d_osnma_data.d_dsm_header.dsm_id < 12)
                 {
@@ -194,6 +194,9 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_
                         }
                 }
             process_dsm_message(dsm_msg);
+            d_dsm_message[d_osnma_data.d_dsm_header.dsm_id] = std::array<uint8_t, 256>{};
+            d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id] = std::array<uint8_t, 16>{};
+            d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] = 0;
         }
 }
 
diff --git a/src/core/system_parameters/Galileo_OSNMA.h b/src/core/system_parameters/Galileo_OSNMA.h
index 7c233a294..89cc06d92 100644
--- a/src/core/system_parameters/Galileo_OSNMA.h
+++ b/src/core/system_parameters/Galileo_OSNMA.h
@@ -134,6 +134,7 @@ constexpr std::uint8_t mask_cpks{0x07};
 constexpr std::uint8_t mask_nma_header_reserved{0x01};
 constexpr std::uint8_t mask_dsm_id{0x01};
 constexpr std::uint8_t mask_dsm_block_id{0x0F};
+constexpr std::uint8_t mask_dsm_number_blocks{0XF0};
 #else
 constexpr std::uint8_t mask_nmas{0b1100'0000};
 constexpr std::uint8_t mask_cid{0b0011'0000};
@@ -141,6 +142,7 @@ constexpr std::uint8_t mask_cpks{0b0000'1110};
 constexpr std::uint8_t mask_nma_header_reserved{0b0000'0001};
 constexpr std::uint8_t mask_dsm_id{0b0000'0001};
 constexpr std::uint8_t mask_dsm_block_id{0b0000'1111};
+constexpr std::uint8_t mask_dsm_number_blocks{0b1111'0000};
 
 #endif
 
@@ -174,6 +176,10 @@ inline uint8_t get_dsm_block_id(uint8_t dsm_header)
     return dsm_header & mask_dsm_block_id;
 }
 
+inline uint8_t get_number_blocks(uint8_t dsm_message_0)
+{
+    return (dsm_message_0 & mask_dsm_number_blocks) >> 4;
+}
 
 /** \} */
 /** \} */