diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc index 1c1da25a9..29f896f8b 100644 --- a/src/core/libs/osnma_msg_receiver.cc +++ b/src/core/libs/osnma_msg_receiver.cc @@ -22,6 +22,7 @@ #include "gnss_crypto.h" #include "gnss_satellite.h" #include "osnma_dsm_reader.h" // for OSNMA_DSM_Reader +#include "osnma_helper.h" #include // for DLOG #include // for gr::io_signature::make #include @@ -60,6 +61,7 @@ osnma_msg_receiver::osnma_msg_receiver( { d_dsm_reader = std::make_unique(); d_crypto = std::make_unique(pemFilePath, merkleFilePath); + d_helper = std::make_unique(); // register OSNMA input message port from telemetry blocks this->message_port_register_in(pmt::mp("OSNMA_from_TLM")); // register OSNMA output message port to PVT block @@ -89,14 +91,13 @@ void osnma_msg_receiver::msg_handler_osnma(const pmt::pmt_t& msg) { const auto nma_msg = wht::any_cast>(pmt::any_ref(msg)); const auto sat = Gnss_Satellite(std::string("Galileo"), nma_msg->PRN); - LOG(INFO) << "Galileo OSNMA: Subframe received starting at " + LOG(WARNING) << "Galileo OSNMA: Subframe received starting at " << "WN=" << nma_msg->WN_sf0 << ", TOW=" << nma_msg->TOW_sf0 << ", from satellite " - << sat - << std::endl; + << sat; process_osnma_message(nma_msg); @@ -127,7 +128,10 @@ void osnma_msg_receiver::process_osnma_message(const std::shared_ptr& { read_nma_header(osnma_msg->hkroot[0]); if(d_osnma_data.d_nma_header.nmas == 0 || d_osnma_data.d_nma_header.nmas == 3 /*&& d_kroot_verified*/) - return; + { + LOG(ERROR) << "Galileo OSNMA: NMAS invalid, skipping osnma message\n"; + return; + } read_dsm_header(osnma_msg->hkroot[1]); read_dsm_block(osnma_msg); process_dsm_block(osnma_msg); // will process dsm block if received a complete one, then will call mack processing upon re-setting the dsm block to 0 @@ -574,6 +578,9 @@ void osnma_msg_receiver::read_and_process_mack_block(const std::shared_ptrPRN; // FIXME this is ugly. + d_osnma_data.d_mack_message.TOW = osnma_msg->TOW_sf0; + d_osnma_data.d_mack_message.WN = osnma_msg->WN_sf0; read_mack_body(); process_mack_message(); // TODO - shorten the MACK processing for the cases where no TK verified or no Kroot verified (warm and cold start) @@ -649,8 +656,6 @@ void osnma_msg_receiver::read_mack_header() d_osnma_data.d_mack_message.header.tag0 = first_lt_bits; d_osnma_data.d_mack_message.header.macseq = macseq; d_osnma_data.d_mack_message.header.cop = cop; - d_osnma_data.d_mack_message.PRNa = d_osnma_data.d_nav_data.PRNa; // FIXME this is ugly. - d_osnma_data.d_mack_message.TOW = d_osnma_data.d_nav_data.TOW_sf0; } /** @@ -866,9 +871,10 @@ void osnma_msg_receiver::process_mack_message() if(d_tesla_keys.find(mack->TOW + 30) != d_tesla_keys.end()){ bool ret = verify_macseq(*mack); if (ret || d_flag_debug){ - for(auto& tag:mack->tag_and_info) + for(std::size_t i = 0; i < mack->tag_and_info.size(); ++i) { - Tag t(tag, mack->TOW, mack->PRNa); + auto& tag = mack->tag_and_info[i]; + Tag t(tag, mack->TOW, mack->WN, mack->PRNa, i + 2); // tag0 (mack header) has CTR1, so first tag of MTI has CTR = 2. d_tags_awaiting_verify.insert(std::pair(mack->TOW, t)); LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: SUCCESS for Mack at TOW=" << mack->TOW << ", PRN" << mack->PRNa; } @@ -992,7 +998,43 @@ bool osnma_msg_receiver::verify_dsm_pkr(DSM_PKR_message message) } bool osnma_msg_receiver::verify_tag(Tag& tag) { - std::vector m = tag.build_message(); + // build message + std::vector m; + m.push_back(static_cast(tag.PRN_d)); + m.push_back(static_cast(tag.PRNa)); + uint32_t GST = d_helper->compute_gst(tag.TOW, tag.WN); + std::vector GST_uint8 = d_helper->gst_to_uint8(GST); + m.insert(m.end(),GST_uint8.begin(),GST_uint8.end()); + m.push_back(tag.CTR); + // Extracts only two bits from d_osnma_data.d_nma_header.nmas + uint8_t two_bits_nmas = d_osnma_data.d_nma_header.nmas & 0b00000011; + two_bits_nmas = two_bits_nmas << 6; + m.push_back(two_bits_nmas); + + // convert std::string to vector + std::string ephemeris_iono_vector_2 = d_satellite_nav_data[tag.TOW][tag.PRNa].ephemeris_iono_vector_2; + std::vector ephemeris_iono_vector_2_bytes(ephemeris_iono_vector_2.begin(), ephemeris_iono_vector_2.end()); + + // Convert and add ephemeris_iono_vector_2 into the vector + for (uint8_t byte : ephemeris_iono_vector_2_bytes) { + m.back() |= (byte >> 2); // First take the 6 MSB bits of byte and add to m + m.push_back(byte << 6); // Then take the last 2 bits of byte, shift them to MSB position and insert the new element into m + } + if(m.back() == 0) { + m.pop_back(); // Remove the last element if its value is 0 (only padding was added) + } + else { + // Pad with zeros if the last element wasn't full + for (int bits = 2; bits < 8; bits += 2) { + // Check if the last element in the vector has 2 '00' bits in its LSB position + if((m.back() & 0b00000011) == 0) { + m.back() <<= 2; // Shift the existing bits to make room for new 2 bits + } + else { + break; // If it does not have 2 '00' bits in its LSB position, then the padding is complete + } + } + } std::vector mac; if (d_osnma_data.d_dsm_kroot_message.mf == 0) // C: HMAC-SHA-256 @@ -1044,36 +1086,9 @@ bool osnma_msg_receiver::verify_tag(Tag& tag) // Compare computed tag with received one truncated if (tag.received_tag == computed_mac) - { -// if(tag.ADKD == 0 || tag.ADKD == 12) -// { -// std::cout << "Galileo OSNMA: tag verification successful for PRN_a " -// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].PRNa << " with WN=" -// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].WN_sf0 << ", TOW=" -// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].TOW_sf0 << "NavData= " -// << "Ephemeris, Clock and Ionospheric data" << ". " -// << std::endl; -// } -// else if(tag.ADKD == 4) -// { -// std::cout << "Galileo OSNMA: tag verification successful for PRN_a " -// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].PRNa << " with WN=" -// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].WN_sf0 << ", TOW=" -// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].TOW_sf0 << "NavData= " -// << "Timing data" << ". " -// << std::endl; -// } return true; - - } else - { -// std::cout << "Galileo OSNMA: Tag verification failed for PRN_a " -// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].PRNa << " with WN=" -// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].WN_sf0 << ", TOW=" -// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].TOW_sf0 << std::endl; return false; - } } void osnma_msg_receiver::add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData& data) { diff --git a/src/core/libs/osnma_msg_receiver.h b/src/core/libs/osnma_msg_receiver.h index 157c78750..8137c9824 100644 --- a/src/core/libs/osnma_msg_receiver.h +++ b/src/core/libs/osnma_msg_receiver.h @@ -40,6 +40,7 @@ friend class test_case_name##_##test_name##_Test class OSNMA_DSM_Reader; class Gnss_Crypto; +class Osnma_Helper; class osnma_msg_receiver; using osnma_msg_receiver_sptr = gnss_shared_ptr; @@ -88,6 +89,7 @@ private: std::multimap d_tags_awaiting_verify; // container with tags to verify from arbitrary SVIDs, sorted by TOW std::unique_ptr d_dsm_reader; std::unique_ptr d_crypto; + std::unique_ptr d_helper; std::array, 16> d_dsm_message{}; // structure for recording DSM blocks, when filled it sends them to parse and resets itself. std::array, 16> d_dsm_id_received{}; diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index 4c6e9fb20..46c5fb4b7 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -96,6 +96,8 @@ set(SYSTEM_PARAMETERS_HEADERS Galileo_OSNMA.h osnma_data.h osnma_dsm_reader.h + osnma_helper.cc + osnma_helper.h ) list(SORT SYSTEM_PARAMETERS_HEADERS) diff --git a/src/core/system_parameters/galileo_inav_message.h b/src/core/system_parameters/galileo_inav_message.h index 13db80706..0e9c15b25 100644 --- a/src/core/system_parameters/galileo_inav_message.h +++ b/src/core/system_parameters/galileo_inav_message.h @@ -51,7 +51,8 @@ public: uint32_t PRN{}; uint32_t WN_sf0{}; // TODO - this is present in UtcModelData already uint32_t TOW_sf0{}; - std::vector EphemerisClockAndStatusData {}; + std::vector EphemerisClockAndStatusData {}; // TODO _2 rename and substitute this + std::string EphemerisClockAndStatusData_2{}; std::vector TimingData {}; Galileo_Ephemeris EphemerisData {}; Galileo_Iono IonoData {}; diff --git a/src/core/system_parameters/osnma_data.cc b/src/core/system_parameters/osnma_data.cc index 65145ae16..25124bb81 100644 --- a/src/core/system_parameters/osnma_data.cc +++ b/src/core/system_parameters/osnma_data.cc @@ -35,6 +35,9 @@ void NavData::init(const std::shared_ptr &osnma_msg) PRNa = osnma_msg->PRN; WN_sf0 = osnma_msg->WN_sf0; TOW_sf0 = osnma_msg->TOW_sf0; + + // new parsing, directly parsing bits + ephemeris_iono_vector_2 = osnma_msg->EphemerisClockAndStatusData_2; }; void NavData::generate_eph_iono_vector() { @@ -166,8 +169,3 @@ void NavData::generate_utc_vector() } } -std::vector Tag::build_message() -{ - // TODO - return std::vector(); -} diff --git a/src/core/system_parameters/osnma_data.h b/src/core/system_parameters/osnma_data.h index 121853d2a..6e3439a4c 100644 --- a/src/core/system_parameters/osnma_data.h +++ b/src/core/system_parameters/osnma_data.h @@ -121,7 +121,8 @@ public: MACK_header header; std::vector tag_and_info; std::vector key; - uint32_t TOW; // TODO duplicated variable + uint32_t TOW; // TODO duplicated variable, also in NavData + uint32_t WN; uint32_t PRNa; }; @@ -131,6 +132,7 @@ public: NavData()=default; void init(const std::shared_ptr &osnma_msg); std::vector ephemeris_iono_vector{}; + std::string ephemeris_iono_vector_2{}; std::vector utc_vector{}; uint32_t PRNa{}; uint32_t WN_sf0{}; @@ -168,10 +170,12 @@ public: SUCCESS, FAIL, UNVERIFIED}; - Tag(const MACK_tag_and_info& MTI, uint32_t TOW, uint32_t PRNa) + Tag(const MACK_tag_and_info& MTI, uint32_t TOW,uint32_t WN, uint32_t PRNa,uint8_t CTR) : tag_id(id_counter++), - TOW(TOW), + TOW(TOW), // TODO missing for build_message WN for GST computation, CTR, NMAS, NavData missing + WN(WN), PRNa(PRNa), + CTR(CTR), status(UNVERIFIED), received_tag(MTI.tag), computed_tag(0), @@ -184,10 +188,11 @@ public: const uint32_t tag_id; uint32_t TOW; + uint32_t WN; uint32_t PRNa; + uint8_t CTR; e_verification_status status; uint64_t received_tag; - std::vector build_message(); uint32_t static id_counter; uint64_t computed_tag; diff --git a/src/core/system_parameters/osnma_helper.cc b/src/core/system_parameters/osnma_helper.cc new file mode 100644 index 000000000..462fd5d34 --- /dev/null +++ b/src/core/system_parameters/osnma_helper.cc @@ -0,0 +1,34 @@ +/*! +* \file osnma_helper.h +* \brief Class for auxiliary osnma functions +* \author Carles Fernandez-Prades, 2024 cfernandez(at)cttc.es +* +* ----------------------------------------------------------------------------- +* +* GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +* This file is part of GNSS-SDR. +* +* Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) +* SPDX-License-Identifier: GPL-3.0-or-later +* +* ----------------------------------------------------------------------------- + */ + +#include "osnma_helper.h" + +uint32_t Osnma_Helper::compute_gst(uint32_t WN, uint32_t TOW) const +{ + uint32_t GST = (WN & 0x00000FFF) << 20 | (TOW & 0x000FFFFF); + return GST; +} + +std::vector Osnma_Helper::gst_to_uint8(uint32_t GST) const +{ + std::vector res(4); + + res[1] = static_cast((GST & 0xFF000000) >> 24); + res[2] = static_cast((GST & 0x00FF0000) >> 16); + res[3] = static_cast((GST & 0x0000FF00) >> 8); + res[4] = static_cast(GST & 0x000000FF); + return res; +} diff --git a/src/core/system_parameters/osnma_helper.h b/src/core/system_parameters/osnma_helper.h new file mode 100644 index 000000000..0a1b72270 --- /dev/null +++ b/src/core/system_parameters/osnma_helper.h @@ -0,0 +1,33 @@ +/*! +* \file osnma_helper.h +* \brief Class for auxiliary osnma functions +* \author Carles Fernandez-Prades, 2024 cfernandez(at)cttc.es +* +* ----------------------------------------------------------------------------- +* +* GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +* This file is part of GNSS-SDR. +* +* Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) +* SPDX-License-Identifier: GPL-3.0-or-later +* +* ----------------------------------------------------------------------------- +*/ + +#ifndef GNSS_SDR_OSNMA_HELPER_H +#define GNSS_SDR_OSNMA_HELPER_H + + +#include +#include +class Osnma_Helper +{ +public: + Osnma_Helper() = default; + ~Osnma_Helper() = default; + uint32_t compute_gst(uint32_t WN, uint32_t TOW) const; + std::vector gst_to_uint8(uint32_t GST) const; +}; + + +#endif // GNSS_SDR_OSNMA_HELPER_H diff --git a/src/tests/unit-tests/signal-processing-blocks/osnma/osnma_msg_receiver_test.cc b/src/tests/unit-tests/signal-processing-blocks/osnma/osnma_msg_receiver_test.cc index 65dd98006..f251756e8 100644 --- a/src/tests/unit-tests/signal-processing-blocks/osnma/osnma_msg_receiver_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/osnma/osnma_msg_receiver_test.cc @@ -99,6 +99,8 @@ TEST_F(OsnmaMsgReceiverTest, OsnmaTestVectorsSimulation) const int SIZE_SUBFRAME_BYTES{SIZE_PAGE_BYTES*SIZE_SUBFRAME_PAGES}; const int DURATION_SUBFRAME{30}; + const int DUMMY_PAGE{63}; + bool flag_dummy_page{false}; std::cout << "OsnmaTestVectorsSimulation:" << " d_GST_SIS= " << d_GST_SIS << ", TOW=" << TOW << ", WN=" << WN << std::endl; @@ -107,10 +109,12 @@ TEST_F(OsnmaMsgReceiverTest, OsnmaTestVectorsSimulation) // Act while (end_of_hex_stream == false){ // loop over all bytes of data. Note all TestVectors have same amount of data. for(const TestVector& tv : testVectors) { // loop over all SVs, extract a subframe + std::cout << "OsnmaTestVectorsSimulation: SVID "<< tv.svId << std::endl; auto osnmaMsg_sptr = std::make_shared(); std::array hkroot{}; std::array mack{}; byte_index = offset_byte; // reset byte_index to the offset position for the next test vector. Offset is updated at the end of each Subframe (every 30 s or 450 Bytes) + std::map> words; for (int idx = 0; idx < SIZE_SUBFRAME_PAGES; ++idx) // extract all pages of a subframe { // extract bytes of complete page (odd+even) -- extract SIZE_PAGE from tv.navBits, starting from byte_index @@ -133,6 +137,34 @@ TEST_F(OsnmaMsgReceiverTest, OsnmaTestVectorsSimulation) bool tail_bits_OK = even_page.substr(even_page.size() - 6) == "000000" && odd_page.substr(odd_page.size() - 6) == "000000"; if(!even_odd_OK || !page_type_OK || !tail_bits_OK) std::cerr<< "OsnmaTestVectorsSimulation: error parsing pages." << std::endl; + + std::bitset<112> data_k(even_page.substr(2,112)); + std::bitset<16> data_j(odd_page.substr(2,16)); + std::bitset<112> shifted_data_k = data_k; +// uint8_t word_type = 0; +// for(int i = 0; i < 6; ++i) { +// word_type |= (data_k[104 + i] << i); +// } + uint8_t word_type = static_cast((shifted_data_k >>= 106).to_ulong()); // word type is the first 6 bits of the word + std::cout<< "OsnmaTestVectorsSimulation: received Word "<< static_cast(word_type) << std::endl; + if( (word_type >= 1 && word_type <=5) || word_type == 6 || word_type == 10) + { + // store raw word + std::bitset<128> data_combined(data_k.to_string() + data_j.to_string()); + words[word_type] = data_combined; +// std::vector concatenatedData; +// for (std::size_t i = 0; i < data_k.size(); i += 8) { +// std::bitset<8> byte(data_k.to_string().substr(i, 8)); +// concatenatedData.push_back(static_cast(byte.to_ulong())); +// } +// for (std::size_t i = 0; i < data_j.size(); i += 8) { +// std::bitset<8> byte(data_j.to_string().substr(i, 8)); +// concatenatedData.push_back(static_cast(byte.to_ulong())); +// } + } + if(word_type == DUMMY_PAGE) + flag_dummy_page = true; + // place it into osnma object. std::bitset<40> osnmaBits(odd_page.substr(18, 40)); // Extract bits for hkroot and mack std::bitset<8> hkrootBits(osnmaBits.to_string().substr(0, 8)); @@ -142,8 +174,13 @@ TEST_F(OsnmaMsgReceiverTest, OsnmaTestVectorsSimulation) byte_index += SIZE_PAGE_BYTES; } + std::cout<< "----------" << std::endl; if(end_of_hex_stream) break; + if(flag_dummy_page){ + flag_dummy_page = false; + continue; // skip this SV + } osnmaMsg_sptr->hkroot = hkroot; osnmaMsg_sptr->mack = mack; @@ -151,6 +188,63 @@ TEST_F(OsnmaMsgReceiverTest, OsnmaTestVectorsSimulation) osnmaMsg_sptr->WN_sf0 = (d_GST_SIS & 0xFFF00000) >> 20 ; osnmaMsg_sptr->PRN = tv.svId; + bool allWordsReceived = true; + for (int i = 1; i <= 5; ++i) + { + if (words.find(i) == words.end() && flag_dummy_page == false) + { + allWordsReceived = false; + std::cerr<< "OsnmaTestVectorsSimulation: error parsing words 1->5. " + "Word "<< i << " should be received for each subframe but was not." << std::endl; + } + } + if(allWordsReceived) + { + + // Define the starting position and length of bits to extract for each word + std::map> extractionParams = { + {1, {6, 120}}, + {2, {6, 120}}, + {3, {6, 122}}, + {4, {6, 120}}, + {5, {6, 67}}, + // TODO words 6 and 10 for TimingData + }; + + // Iterate over the extraction parameters + for (const auto& param : extractionParams) { + uint8_t wordKey = param.first; + uint8_t start = param.second.first; + uint8_t length = param.second.second; + + // Extract the required bits + std::bitset<128> word = words[wordKey]; + + osnmaMsg_sptr->EphemerisClockAndStatusData_2 += words[wordKey]. + to_string().substr( + start, length); + +// std::bitset<8> byte; +// int byteIndex = 0; +// for (uint8_t i = start; i < start + length; ++i) { +// byte[byteIndex] = word[i]; +// byteIndex++; +// +// // Once we have collected 8 bits, we can add them as an uint8_t to the vector +// if (byteIndex == 8) { +// osnmaMsg_sptr->EphemerisClockAndStatusData.push_back(static_cast(byte.to_ulong())); +// byte.reset(); +// byteIndex = 0; +// } +// } +// +// // Push remaining bits if it didn't reach 8 bits +// if (byteIndex > 0) { +// osnmaMsg_sptr->EphemerisClockAndStatusData.push_back(static_cast(byte.to_ulong())); +// } + } + } + auto temp_obj = pmt::make_any(osnmaMsg_sptr); osnma->msg_handler_osnma(temp_obj); // osnma entry point @@ -169,9 +263,6 @@ TEST_F(OsnmaMsgReceiverTest, OsnmaTestVectorsSimulation) } - - - // Assert // TODO }