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 2c2693c44..32f9656fc 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 @@ -430,11 +430,11 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in const std::shared_ptr tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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(d_TOW_at_current_symbol_ms) / 1000.0 - tmp_obj->t_0G + 604800 * (std::fmod(static_cast(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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; } } } diff --git a/src/core/system_parameters/galileo_inav_message.cc b/src/core/system_parameters/galileo_inav_message.cc index 3c946b487..d852d9405 100644 --- a/src/core/system_parameters/galileo_inav_message.cc +++ b/src/core/system_parameters/galileo_inav_message.cc @@ -176,8 +176,6 @@ bool Galileo_Inav_Message::read_navigation_bool(const std::bitset page_type_bits(page_number_bits); // from string to bitset - Page_type = static_cast(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{}; + nma_msg.mack = std::array{}; + nma_msg.hkroot = std::array{}; + } + 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(hkroot_bs.to_ulong()); + mack_sis = static_cast(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& IOD_nav_4 = static_cast(read_navigation_unsigned(data_bits, IOD_NAV_4_BIT)); DLOG(INFO) << "IOD_nav_4= " << IOD_nav_4; SV_ID_PRN_4 = static_cast(read_navigation_unsigned(data_bits, SV_ID_PRN_4_BIT)); + nma_msg.PRN = static_cast(SV_ID_PRN_4); DLOG(INFO) << "SV_ID_PRN_4= " << SV_ID_PRN_4; C_ic_4 = static_cast(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(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{}; + nma_msg.hkroot = std::array{}; + nma_position_filled = std::array{}; 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{}; + nma_msg.mack = std::array{}; + nma_msg.hkroot = std::array{}; + } + 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; + } +} diff --git a/src/core/system_parameters/galileo_inav_message.h b/src/core/system_parameters/galileo_inav_message.h index 1b37e9083..1d986a1b4 100644 --- a/src/core/system_parameters/galileo_inav_message.h +++ b/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 #include #include #include @@ -39,6 +40,14 @@ class ReedSolomon; // Forward declaration of the ReedSolomon class /** \addtogroup System_Parameters * \{ */ +class OSNMA_msg +{ +public: + OSNMA_msg() = default; + std::array mack{}; + std::array 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{}; + nma_msg.hkroot = std::array{}; + page_position_in_inav_subframe = 255; + nma_position_filled = std::array{}; } /* @@ -242,7 +259,7 @@ private: std::unique_ptr rs; // The Reed-Solomon decoder std::vector 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 nma_position_filled{}; + OSNMA_msg nma_msg{}; + uint8_t IODnav_LSB17{}; uint8_t IODnav_LSB18{}; uint8_t IODnav_LSB19{};