diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc index 0688af6e9..b1d83b3d9 100644 --- a/src/core/libs/osnma_msg_receiver.cc +++ b/src/core/libs/osnma_msg_receiver.cc @@ -24,6 +24,7 @@ #include "gnss_satellite.h" #include "osnma_dsm_reader.h" // for OSNMA_DSM_Reader #include "osnma_helper.h" +#include "osnma_nav_data_manager.h" // TODO - all these repeated includes, is it good practice to include them in the source file? #include // for gr::io_signature::make #include #include @@ -72,6 +73,7 @@ osnma_msg_receiver::osnma_msg_receiver( d_dsm_reader = std::make_unique(); d_crypto = std::make_unique(crtFilePath, merkleFilePath); d_helper = std::make_unique(); + d_nav_data_manager = 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 @@ -114,8 +116,8 @@ void osnma_msg_receiver::msg_handler_osnma(const pmt::pmt_t& msg) std::cout << output_message.str() << std::endl; process_osnma_message(nma_msg); - } - else if (msg_type_hash_code == typeid(std::shared_ptr>).hash_code()) + } // OSNMA frame received + else if (msg_type_hash_code == typeid(std::shared_ptr>).hash_code()) // Navigation data bits for OSNMA received { // TODO - PRNa is a typo here, I think for d_satellite_nav_data, is PRN_d the name to use const auto inav_data = wht::any_cast>>(pmt::any_ref(msg)); @@ -123,23 +125,7 @@ void osnma_msg_receiver::msg_handler_osnma(const pmt::pmt_t& msg) std::string nav_data = std::get<1>(*inav_data); uint32_t TOW = std::get<2>(*inav_data); - // iono data => 549 bits, utc data, 141 bits. - if (nav_data.size() == 549) - { - // LOG(INFO) << "Galileo OSNMA: received ADKD=0/12 navData, PRN_d (" << PRNa << ") " - // << "TOW_sf=" << TOW; - d_satellite_nav_data[PRNa][TOW].ephemeris_iono_vector_2 = nav_data; - } - else if (nav_data.size() == 141) - { - // LOG(INFO) << "Galileo OSNMA: received ADKD=4 navData, PRN_d (" << PRNa << ") " - // << "TOW_sf=" << TOW; - d_satellite_nav_data[PRNa][TOW].utc_vector_2 = nav_data; - } - else - { - LOG(WARNING) << "Galileo OSNMA: osnma_msg_receiver incorrect navData parsing!"; - } + d_nav_data_manager->add_navigation_data(nav_data,PRNa,TOW); } else { @@ -614,8 +600,7 @@ void osnma_msg_receiver::read_and_process_mack_block(const std::shared_ptrTOW_sf0; if (d_kroot_verified || d_tesla_key_verified || d_osnma_data.d_dsm_kroot_message.ts != 0 /*mack parser needs to know the tag size, otherwise cannot parse mack messages*/) // C: 4 ts < ts < 10 {// TODO - correct? with this, MACK would not be processed unless a Kroot is available -- no, if TK available MACK sould go on, this has to change in future @@ -963,7 +948,7 @@ void osnma_msg_receiver::process_mack_message() for (auto& it : d_tags_awaiting_verify) { bool ret; - if (tag_has_key_available(it.second) && tag_has_nav_data_available(it.second)) + if (tag_has_key_available(it.second) && d_nav_data_manager->have_nav_data(it.second))//tag_has_nav_data_available(it.second)) { ret = verify_tag(it.second); /* TODO - take into account: @@ -1043,6 +1028,17 @@ void osnma_msg_receiver::process_mack_message() } } + uint8_t tag_size = 0; + const auto it = OSNMA_TABLE_11.find(d_osnma_data.d_dsm_kroot_message.ts); + if (it != OSNMA_TABLE_11.cend()) + { + tag_size = it->second; + } + d_nav_data_manager->update_nav_data(d_tags_awaiting_verify, tag_size); + auto data_to_send = d_nav_data_manager->get_verified_data(); + d_nav_data_manager->print_status(); + send_data_to_pvt(data_to_send); + remove_verified_tags(); control_tags_awaiting_verify_size(); // remove the oldest tags if size is too big. @@ -1123,7 +1119,7 @@ std::vector osnma_msg_receiver::get_merkle_tree_leaves(const DSM_PKR_me } -bool osnma_msg_receiver::verify_tag(const Tag& tag) const +bool osnma_msg_receiver::verify_tag(Tag& tag) const { // Debug // LOG(INFO) << "Galileo OSNMA: Tag verification :: Start for tag Id= " @@ -1209,36 +1205,25 @@ bool osnma_msg_receiver::verify_tag(const Tag& tag) const computed_mac += static_cast(mac[4]); } + tag.computed_tag = computed_mac; // update with computed value // Compare computed tag with received one truncated if (tag.received_tag == computed_mac) { - LOG(INFO) << "Galileo OSNMA: Tag verification :: SUCCESS for tag Id=" - << tag.tag_id - << ", value=0x" << std::setfill('0') << std::setw(10) << std::hex << std::uppercase - << tag.received_tag << std::dec - << ", TOW=" - << tag.TOW - << ", ADKD=" - << static_cast(tag.ADKD) - << ", PRNa=" - << static_cast(tag.PRNa) - << ", PRNd=" - << static_cast(tag.PRN_d); - std::cout << "Galileo OSNMA: Tag verification :: SUCCESS for tag Id=" - << tag.tag_id - << ", ADKD=" - << static_cast(tag.ADKD) - << ", PRNa=" - << static_cast(tag.PRNa) - << ", PRNd=" - << static_cast(tag.PRN_d) << std::endl; return true; } return false; } -std::vector osnma_msg_receiver::build_message(const Tag& tag) const +/** + * \brief generates the message for computing the tag + * \remarks It also sets some parameters to the Tag object, based on the verification process. + * + * \param tag The tag containing the information to be included in the message. + * + * \return The built OSNMA message as a vector of uint8_t. + */ +std::vector osnma_msg_receiver::build_message(Tag& tag) const { std::vector m; if (tag.CTR != 1) @@ -1257,40 +1242,9 @@ std::vector osnma_msg_receiver::build_message(const Tag& tag) const m.push_back(two_bits_nmas); // Add applicable NavData bits to message - std::string applicable_nav_data; - std::vector applicable_nav_data_bytes; - if (tag.ADKD == 0 || tag.ADKD == 12) // note: for ADKD=12 still the same logic applies. Only the Key selection is shifted 10 Subframes into the future. - { - const auto it = d_satellite_nav_data.find(tag.PRN_d); - if (it != d_satellite_nav_data.cend()) - { - const auto it2 = it->second.find(tag.TOW - 30); - if (it2 != it->second.cend()) - { - applicable_nav_data = it2->second.ephemeris_iono_vector_2; - } - } - // LOG(INFO) << "|---> Galileo OSNMA :: applicable NavData (PRN_d="<< static_cast(tag.PRN_d) << ", TOW=" << tag.TOW - 30 <<"): 0b" << applicable_nav_data; - } - else if (tag.ADKD == 4) - { - const auto it = d_satellite_nav_data.find(tag.PRN_d); - if (it != d_satellite_nav_data.cend()) - { - const auto it2 = it->second.find(tag.TOW - 30); - if (it2 != it->second.cend()) - { - applicable_nav_data = it2->second.utc_vector_2; - } - } - // LOG(INFO) << "|---> Galileo OSNMA :: applicable NavData (PRN_d="<< static_cast(tag.PRN_d) << ", TOW=" << tag.TOW - 30 <<"): 0b" << applicable_nav_data; - } - else - { - LOG(WARNING) << "Galileo OSNMA: Tag verification :: unknown ADKD"; - } - // convert std::string to vector - applicable_nav_data_bytes = d_helper->bytes(applicable_nav_data); + std::string applicable_nav_data = d_nav_data_manager->get_navigation_data(tag); + std::vector applicable_nav_data_bytes = d_helper->bytes(applicable_nav_data); + tag.nav_data = applicable_nav_data; // update tag with applicable data // Convert and add NavData bytes into the message, taking care of that NMAS has only 2 bits for (uint8_t byte : applicable_nav_data_bytes) @@ -1322,17 +1276,17 @@ std::vector osnma_msg_receiver::build_message(const Tag& tag) const } -void osnma_msg_receiver::add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData& data) -{ - // control size of container - while (d_satellite_nav_data[SV_ID].size() >= 25) - { - d_satellite_nav_data[SV_ID].erase(d_satellite_nav_data[SV_ID].begin()); - } - // d_osnma_data[TOW] = crypto; // crypto - d_satellite_nav_data[SV_ID][TOW] = data; // nav - // std::cout << "Galileo OSNMA: added element, size is " << d_satellite_nav_data[SV_ID].size() << std::endl; -} +//void osnma_msg_receiver::add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData& data) +//{ +// // control size of container +// while (d_satellite_nav_data[SV_ID].size() >= 25) +// { +// d_satellite_nav_data[SV_ID].erase(d_satellite_nav_data[SV_ID].begin()); +// } +// // d_osnma_data[TOW] = crypto; // crypto +// d_satellite_nav_data[SV_ID][TOW] = data; // nav +// // std::cout << "Galileo OSNMA: added element, size is " << d_satellite_nav_data[SV_ID].size() << std::endl; +//} void osnma_msg_receiver::display_data() @@ -1411,6 +1365,7 @@ bool osnma_msg_receiver::verify_tesla_key(std::vector& key, uint32_t TO * @brief Removes the tags that have been verified from the multimap d_tags_awaiting_verify. * * This function iterates through the multimap d_tags_awaiting_verify, and removes the tags that have a status of SUCCESS or FAIL. + * \remarks it also prints the current unverified tags */ void osnma_msg_receiver::remove_verified_tags() { @@ -1458,9 +1413,25 @@ void osnma_msg_receiver::remove_verified_tags() } } LOG(INFO) << "Galileo OSNMA: d_tags_awaiting_verify :: size: " << d_tags_awaiting_verify.size(); + for (const auto& it : d_tags_awaiting_verify) + { + LOG(INFO) << "Galileo OSNMA: Tag verification :: status tag Id=" + << it.second.tag_id + << ", value=0x" << std::setfill('0') << std::setw(10) << std::hex << std::uppercase + << it.second.received_tag << std::dec + << ", TOW=" + << it.second.TOW + << ", ADKD=" + << static_cast(it.second.ADKD) + << ", PRNa=" + << static_cast(it.second.PRNa) + << ", PRNd=" + << static_cast(it.second.PRN_d) + << ", status= " + << d_helper->verification_status_str(it.second.status); + } } - /** * @brief Control the size of the tags awaiting verification multimap. * @@ -1847,3 +1818,16 @@ std::vector osnma_msg_receiver::verify_macseq_new(const MACK_ return verified_tags; } } +void osnma_msg_receiver::send_data_to_pvt(std::vector data) +{ + if (!data.empty()) + { + for (size_t i = 0; i < data.size(); i++) + { + const auto tmp_obj = std::make_shared(data[i]); + this->message_port_pub(pmt::mp("OSNMA_to_PVT"), pmt::make_any(tmp_obj)); + } + + } + +} diff --git a/src/core/libs/osnma_msg_receiver.h b/src/core/libs/osnma_msg_receiver.h index 7ecbe7428..dd8d6ebf3 100644 --- a/src/core/libs/osnma_msg_receiver.h +++ b/src/core/libs/osnma_msg_receiver.h @@ -27,6 +27,7 @@ #include "gnss_block_interface.h" // for gnss_shared_ptr #include "gnss_sdr_make_unique.h" // for std::make:unique in C++11 #include "osnma_data.h" // for OSNMA_data structures +#include "osnma_nav_data_manager.h" #include // for gr::block #include // for pmt::pmt_t #include // for std::array @@ -77,13 +78,14 @@ private: void read_mack_header(); void read_mack_body(); void process_mack_message(); - void add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData& data); +// void add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData& data); void remove_verified_tags(); void control_tags_awaiting_verify_size(); void display_data(); + void send_data_to_pvt(std::vector); bool verify_tesla_key(std::vector& key, uint32_t TOW); - bool verify_tag(const Tag& tag) const; + bool verify_tag(Tag& tag) const; bool tag_has_nav_data_available(const Tag& t) const; bool tag_has_key_available(const Tag& t) const; bool verify_macseq(const MACK_message& mack); @@ -91,7 +93,7 @@ private: std::vector get_merkle_tree_leaves(const DSM_PKR_message& dsm_pkr_message) const; std::vector compute_merkle_root(const DSM_PKR_message& dsm_pkr_message, const std::vector& m_i) const; - std::vector build_message(const Tag& tag) const; + std::vector build_message(Tag& tag) const; std::vector hash_chain(uint32_t num_of_hashes_needed, const std::vector& key, uint32_t GST_SFi, const uint8_t lk_bytes) const; std::vector verify_macseq_new(const MACK_message& mack); @@ -110,6 +112,7 @@ private: std::unique_ptr d_dsm_reader; // osnma parameters parser std::unique_ptr d_crypto; // access to cryptographic functions std::unique_ptr d_helper; + std::unique_ptr d_nav_data_manager; // refactor for holding and processing navigation data OSNMA_data d_osnma_data{}; diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index 055ff256e..5e42732c7 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -32,6 +32,7 @@ set(SYSTEM_PARAMETERS_SOURCES osnma_data.cc osnma_dsm_reader.cc osnma_helper.cc + osnma_nav_data_manager.cc ) set(SYSTEM_PARAMETERS_HEADERS @@ -98,6 +99,7 @@ set(SYSTEM_PARAMETERS_HEADERS osnma_data.h osnma_dsm_reader.h osnma_helper.h + osnma_nav_data_manager.h ) list(SORT SYSTEM_PARAMETERS_HEADERS) diff --git a/src/core/system_parameters/osnma_data.cc b/src/core/system_parameters/osnma_data.cc index 49051b7be..14382e21a 100644 --- a/src/core/system_parameters/osnma_data.cc +++ b/src/core/system_parameters/osnma_data.cc @@ -25,148 +25,37 @@ */ uint32_t Tag::id_counter = 0; -void NavData::init(const std::shared_ptr &osnma_msg) +uint32_t NavData::id_counter = 0; + +bool NavData::add_nav_data(std::string nav_data) { - EphemerisData = osnma_msg->EphemerisData; - IonoData = osnma_msg->IonoData; - UtcData = osnma_msg->UtcModelData; - generate_eph_iono_vector(); - generate_utc_vector(); - 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; -// utc_vector_2 = osnma_msg->TimingData_2; -}; -void NavData::generate_eph_iono_vector() -{ - ephemeris_iono_vector.clear(); - uint64_t bit_buffer = 0; // variable to store the bits to be extracted, it can contain bits from different variables - int bit_count = 0; // Number of bits in the buffer, i.e. to be extracted - - // create structure to hold the variables to store into the vector along with their bit size - std::vector> variables = { - // data from word type 1 - {static_cast(&EphemerisData.IOD_nav), sizeof(EphemerisData.IOD_nav) * 8}, - {static_cast(&EphemerisData.toe), sizeof(EphemerisData.toe) * 8}, - {static_cast(&EphemerisData.M_0), sizeof(EphemerisData.M_0) * 8}, - {static_cast(&EphemerisData.ecc), sizeof(EphemerisData.ecc) * 8}, - {static_cast(&EphemerisData.sqrtA), sizeof(EphemerisData.sqrtA) * 8}, - // data from word type 2 - {static_cast(&EphemerisData.IOD_nav), sizeof(EphemerisData.IOD_nav) * 8}, - {static_cast(&EphemerisData.OMEGA_0), sizeof(EphemerisData.OMEGA_0) * 8}, - {static_cast(&EphemerisData.i_0), sizeof(EphemerisData.i_0) * 8}, - {static_cast(&EphemerisData.omega), sizeof(EphemerisData.omega) * 8}, - {static_cast(&EphemerisData.idot), sizeof(EphemerisData.idot) * 8}, - {static_cast(&EphemerisData.IOD_nav), sizeof(EphemerisData.IOD_nav) * 8}, - // data from word type 3 - {static_cast(&EphemerisData.OMEGAdot), sizeof(EphemerisData.OMEGAdot) * 8}, - {static_cast(&EphemerisData.delta_n), sizeof(EphemerisData.delta_n) * 8}, - {static_cast(&EphemerisData.Cuc), sizeof(EphemerisData.Cuc) * 8}, - {static_cast(&EphemerisData.Cus), sizeof(EphemerisData.Cus) * 8}, - {static_cast(&EphemerisData.Crc), sizeof(EphemerisData.Crc) * 8}, - {static_cast(&EphemerisData.Crs), sizeof(EphemerisData.Crs) * 8}, - {static_cast(&EphemerisData.SISA), sizeof(EphemerisData.SISA) * 8}, - // data from word type 4 - {static_cast(&EphemerisData.IOD_nav), sizeof(EphemerisData.IOD_nav) * 8}, - {static_cast(&EphemerisData.PRN), sizeof(EphemerisData.PRN) * 8}, - {static_cast(&EphemerisData.Cic), sizeof(EphemerisData.Cic) * 8}, - {static_cast(&EphemerisData.Cis), sizeof(EphemerisData.Cis) * 8}, - {static_cast(&EphemerisData.toe), sizeof(EphemerisData.toe) * 8}, - {static_cast(&EphemerisData.af0), sizeof(EphemerisData.af0) * 8}, - {static_cast(&EphemerisData.af1), sizeof(EphemerisData.af1) * 8}, - {static_cast(&EphemerisData.af2), sizeof(EphemerisData.af2) * 8}, - // data from word type 5 - {static_cast(&IonoData.ai0), sizeof(IonoData.ai0) * 8}, - {static_cast(&IonoData.ai1), sizeof(IonoData.ai1) * 8}, - {static_cast(&IonoData.ai2), sizeof(IonoData.ai2) * 8}, - {static_cast(&IonoData.Region1_flag), sizeof(IonoData.Region1_flag) * 8}, - {static_cast(&IonoData.Region2_flag), sizeof(IonoData.Region2_flag) * 8}, - {static_cast(&IonoData.Region3_flag), sizeof(IonoData.Region3_flag) * 8}, - {static_cast(&IonoData.Region4_flag), sizeof(IonoData.Region4_flag) * 8}, - {static_cast(&IonoData.Region5_flag), sizeof(IonoData.Region5_flag) * 8}, - {static_cast(&EphemerisData.BGD_E1E5a), sizeof(EphemerisData.BGD_E1E5a) * 8}, - {static_cast(&EphemerisData.BGD_E1E5b), sizeof(EphemerisData.BGD_E1E5b) * 8}, - {static_cast(&EphemerisData.E5b_HS), sizeof(EphemerisData.E5b_HS) * 8}, - {static_cast(&EphemerisData.E1B_HS), sizeof(EphemerisData.E1B_HS) * 8}, - {static_cast(&EphemerisData.E5b_DVS), sizeof(EphemerisData.E5b_DVS) * 8}, - {static_cast(&EphemerisData.E1B_DVS), sizeof(EphemerisData.E1B_DVS) * 8}, - }; - - for (auto& var : variables) - { - // extract the bits from the variable - uint64_t binary_representation; - memcpy(&binary_representation, var.first, var.second / 8); - - // Append the bits to the buffer and update the bit count - bit_buffer = (bit_buffer << var.second) | binary_representation; - bit_count += var.second; - // While there are 8 or more bits in the buffer - while (bit_count >= 8) + if (nav_data.size() == 549) { - // Extract the 8 bits starting from last bit position and add them to the vector - uint8_t extracted_bits = (bit_buffer >> (bit_count - 8)) & 0xFF; - ephemeris_iono_vector.push_back(extracted_bits); - - // Remove the extracted bits from the buffer - bit_count -= 8; - bit_buffer = bit_buffer & ~(0xFF << bit_count); + ephemeris_iono_vector_2 = nav_data; + std::bitset<10> bits(nav_data.substr(0,10)); + IOD_nav = static_cast(bits.to_ulong()); + return true; } - - } - - // If there are any bits left in the buffer, add them to the vector - if (bit_count > 0) - { - ephemeris_iono_vector.push_back(static_cast(bit_buffer)); - } + else if (nav_data.size() == 141) + { + utc_vector_2 = nav_data; + return true; + } + return false; } - -void NavData::generate_utc_vector() +std::string NavData::get_utc_data() const { - utc_vector.clear(); - uint64_t bit_buffer = 0; - int bit_count = 0; - - std::vector> variables = { - {static_cast(&UtcData.A0), sizeof(UtcData.A0) * 8}, - {static_cast(&UtcData.A1), sizeof(UtcData.A1) * 8}, - {static_cast(&UtcData.Delta_tLS), sizeof(UtcData.Delta_tLS) * 8}, - {static_cast(&UtcData.tot), sizeof(UtcData.tot) * 8}, - {static_cast(&UtcData.WNot), sizeof(UtcData.WNot) * 8}, - {static_cast(&UtcData.WN_LSF), sizeof(UtcData.WN_LSF) * 8}, - {static_cast(&UtcData.DN), sizeof(UtcData.DN) * 8}, - {static_cast(&UtcData.Delta_tLSF), sizeof(UtcData.Delta_tLSF) * 8}, - {static_cast(&UtcData.A_0G), sizeof(UtcData.A_0G) * 8}, - {static_cast(&UtcData.A_1G), sizeof(UtcData.A_1G) * 8}, - {static_cast(&UtcData.t_0G), sizeof(UtcData.t_0G) * 8}, - {static_cast(&UtcData.WN_0G), sizeof(UtcData.WN_0G) * 8}, - }; - - for (auto& var : variables) - { - uint64_t binary_representation; - memcpy(&binary_representation, var.first, var.second / 8); - - bit_buffer = (bit_buffer << var.second) | binary_representation; - bit_count += var.second; - - while (bit_count >= 8) - { - uint8_t extracted_bits = (bit_buffer >> (bit_count - 8)) & 0xFF; - utc_vector.push_back(extracted_bits); - - bit_count -= 8; - bit_buffer = bit_buffer & ~(0xFF << bit_count); - } - } - - if (bit_count > 0) - { - utc_vector.push_back(static_cast(bit_buffer)); - } + return utc_vector_2; +} +std::string NavData::get_ephemeris_data() const +{ + return ephemeris_iono_vector_2; +} +/** + * Updates the last TOW the NavData bits were received. + * @param TOW + */ +void NavData::update_last_received_timestamp(uint32_t TOW) +{ + last_received_TOW = TOW; } - diff --git a/src/core/system_parameters/osnma_data.h b/src/core/system_parameters/osnma_data.h index 484fa0162..57e4b7012 100644 --- a/src/core/system_parameters/osnma_data.h +++ b/src/core/system_parameters/osnma_data.h @@ -129,23 +129,28 @@ public: class NavData { 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{}; - std::string utc_vector_2{}; - uint32_t PRNa{}; - uint32_t WN_sf0{}; - uint32_t TOW_sf0{}; + NavData(): nav_data_id(id_counter++){ + + } + bool have_this_bits(std::string nav_data); + bool add_nav_data(std::string nav_data); + void update_last_received_timestamp(uint32_t TOW); + const uint32_t nav_data_id; + uint32_t verified_bits{0}; + uint32_t TOW_sf0{0}; + uint32_t last_received_TOW{0}; + uint32_t IOD_nav{0}; + std::string get_utc_data() const; + std::string get_ephemeris_data() const; + bool verified{false}; + uint32_t PRNd{0}; + uint32_t ADKD{}; private: - Galileo_Ephemeris EphemerisData; - Galileo_Iono IonoData; - Galileo_Utc_Model UtcData; - void generate_eph_iono_vector(); // TODO pass data directly fro Telemetry Decoder (if bits are in the needed order) - void generate_utc_vector(); // TODO + std::string ephemeris_iono_vector_2{}; + std::string utc_vector_2{}; + uint32_t static id_counter; }; /*! @@ -202,20 +207,19 @@ public: { } const uint32_t tag_id; + uint32_t static id_counter; uint32_t TOW; uint32_t WN; uint32_t PRNa; uint8_t CTR; e_verification_status status; uint64_t received_tag; - - uint32_t static id_counter; uint64_t computed_tag; - uint8_t PRN_d; uint8_t ADKD; uint8_t cop; uint32_t skipped; + std::string nav_data; }; /** \} */ /** \} */ diff --git a/src/core/system_parameters/osnma_nav_data_manager.cc b/src/core/system_parameters/osnma_nav_data_manager.cc new file mode 100644 index 000000000..c8c979306 --- /dev/null +++ b/src/core/system_parameters/osnma_nav_data_manager.cc @@ -0,0 +1,226 @@ +// +// Created by cgm on 23/07/24. +// + +#include "osnma_nav_data_manager.h" +#if USE_GLOG_AND_GFLAGS +#include // for DLOG +#else +#include +#endif + +/** + * @brief Adds the navigation data bits to the container holding NavData objects. + * + * @param nav_bits The navigation bits. + * @param PRNd The satellite ID. + * @param TOW The TOW of the received data. + */ +void OSNMA_nav_data_Manager::add_navigation_data(std::string nav_bits, uint32_t PRNd, uint32_t TOW) +{ + if(not have_nav_data(nav_bits, PRNd, TOW)) + { + _satellite_nav_data[PRNd][TOW].add_nav_data(nav_bits); + _satellite_nav_data[PRNd][TOW].PRNd = PRNd; + _satellite_nav_data[PRNd][TOW].TOW_sf0 = TOW; + } +} +/** + * @brief loops over the verified tags and updates the navigation data tag length + */ +void OSNMA_nav_data_Manager::update_nav_data(const std::multimap& tags_verified, const uint8_t tag_size) +{ + // loop through all tags + for (const auto& tag : tags_verified) + { + // if tag status is verified, look for corresponding navData and add increase verified tag bits. + if (tag.second.status == Tag::e_verification_status::SUCCESS) + { + if(have_PRNd_nav_data(tag.second.PRN_d)) + { + std::map tow_map = _satellite_nav_data.find(tag.second.PRN_d)->second; + for (auto tow_it = tow_map.begin(); tow_it != tow_map.end(); ++tow_it) // note: starts with smallest (i.e. oldest) navigation dataset + { + std::string nav_data; + if(tag.second.ADKD == 0 || tag.second.ADKD == 12){ + nav_data = tow_it->second.get_ephemeris_data(); + } + else if(tag.second.ADKD == 4){ + nav_data = tow_it->second.get_utc_data(); + } + // find associated navData + if (tag.second.nav_data == nav_data){ + _satellite_nav_data[tag.second.PRN_d][tow_it->first].verified_bits += tag_size; + } + } + } + } + } +} +bool OSNMA_nav_data_Manager::have_PRNd_nav_data(uint32_t PRNd) +{ + // check if have data from PRNd in _satellite_nav_data + return _satellite_nav_data.find(PRNd) != _satellite_nav_data.end(); +} +std::vector OSNMA_nav_data_Manager::get_verified_data() +{ + std::vector result; + for (const auto& prna : _satellite_nav_data) + { + for (const auto& tow_navdata : prna.second) + { + if (tow_navdata.second.verified_bits >= L_t_min) + { + result.push_back(tow_navdata.second); + _satellite_nav_data[prna.first][tow_navdata.first].verified = true; + } + } + } + return result; +} +bool OSNMA_nav_data_Manager::have_nav_data(uint32_t PRNd, uint32_t TOW, uint8_t ADKD) +{ + if (ADKD == 0 || ADKD == 12) + { + const auto it = _satellite_nav_data.find(PRNd); + if (it != _satellite_nav_data.cend()) + { + const auto it2 = it->second.find(TOW); + if (it2 != it->second.cend() && it->second[TOW].get_ephemeris_data() != "") + { + return true; + } + } + } + else if (ADKD == 4) + { + const auto it = _satellite_nav_data.find(PRNd); + if (it != _satellite_nav_data.cend()) + { + const auto it2 = it->second.find(TOW); + if (it2 != it->second.cend() && it->second[TOW].get_utc_data() != "") + { + return true; + } + } + } + return false; +} +/** + * @brief returns NavData object. + * @remarks assumes it exists (called have_nav_data before), otherwise undefined behavior + * TODO - maybe add const promise and use find() instead? this is kinda sensitive topic. + */ +std::string OSNMA_nav_data_Manager::get_navigation_data(const Tag& tag) +{ + auto prn_it = _satellite_nav_data.find(tag.PRN_d); + if (prn_it == _satellite_nav_data.end()){ + return ""; + } + + // satellite was found, check if TOW exists in inner map + std::map tow_map = prn_it->second; + for (auto tow_it = tow_map.begin(); tow_it != tow_map.end(); ++tow_it) // note: starts with smallest (i.e. oldest) navigation dataset + { + // Check if current key (TOW) fulfills condition + if ((tag.TOW - 30 * tag.cop) <= tow_it->first && tow_it->first <= tag.TOW - 30) + { + if(tag.ADKD == 0 || tag.ADKD == 12) + { + if(tow_it->second.get_ephemeris_data() != ""){ + return tow_it->second.get_ephemeris_data(); + } + } + else if(tag.ADKD == 4) + { + if(tow_it->second.get_utc_data() != ""){ + return tow_it->second.get_utc_data(); + } + } + } + } + return ""; +} +/** + * @brief Checks if the navData bits are already present. In case affirmative, it updates the NavData 'last received' timestamp + * @remarks e.g.: a SV may repeat the bits over several subframes. In that case, need to save them only once. + * @param nav_bits + * @param PRNd + * @return + */ +bool OSNMA_nav_data_Manager::have_nav_data(std::string nav_bits, uint32_t PRNd, uint32_t TOW) +{ + if(_satellite_nav_data.find(PRNd) != _satellite_nav_data.end()){ + for (auto& data_timestamp : _satellite_nav_data[PRNd]) + { + if(nav_bits.size() == EPH_SIZE){ + if(data_timestamp.second.get_ephemeris_data() == nav_bits){ + data_timestamp.second.update_last_received_timestamp(TOW); + return true; + } + } + else if(nav_bits.size() == UTC_SIZE){ + if(data_timestamp.second.get_utc_data() == nav_bits){ + data_timestamp.second.update_last_received_timestamp(TOW); + return true; + } + } + } + } + return false; +} +/** + * @brief Checks if there is a NavData element within the COP time interval for a Tag t + * @param t Tag object + * @return True if the needed navigation data for the tag is available (oldest possible NavData available) + */ +bool OSNMA_nav_data_Manager::have_nav_data(const Tag& t) const +{ + auto prn_it = _satellite_nav_data.find(t.PRN_d); + if (prn_it == _satellite_nav_data.end()){ + return false; + } + // satellite was found, check if TOW exists in inner map + std::map tow_map = prn_it->second; + for (auto tow_it = tow_map.begin(); tow_it != tow_map.end(); ++tow_it) // note: starts with smallest (i.e. oldest) navigation dataset + { + // Check if current key (TOW) fulfills condition + if (t.TOW - 30 * t.cop <= tow_it->first && tow_it->first <= t.TOW - 30) + { + if(t.ADKD == 0 || t.ADKD == 12) + { + if(tow_it->second.get_ephemeris_data() != ""){ + return true; + } + } + else if(t.ADKD == 4) + { + if(tow_it->second.get_utc_data() != ""){ + return true; + } + } + + } + } + return false; +} +void OSNMA_nav_data_Manager::print_status() +{ + for (const auto& satellite : _satellite_nav_data){ + LOG(INFO) << "Galileo OSNMA: NavData status :: SVID=" << satellite.first; + auto& tow_data = satellite.second; + for (const auto& nav_data : tow_data) + LOG(INFO) << "Galileo OSNMA: IOD_nav=0b" << std::uppercase + << std::bitset<10>(nav_data.second.IOD_nav) + << ", TOW_start=" + << nav_data.second.TOW_sf0 + << ", TOW_last=" + << nav_data.second.last_received_TOW + << ", l_t=" + << nav_data.second.verified_bits + << ", PRNd=" + << nav_data.second.PRNd + << ", verified=" + << nav_data.second.verified; + } +} diff --git a/src/core/system_parameters/osnma_nav_data_manager.h b/src/core/system_parameters/osnma_nav_data_manager.h new file mode 100644 index 000000000..9471a14a8 --- /dev/null +++ b/src/core/system_parameters/osnma_nav_data_manager.h @@ -0,0 +1,41 @@ +// +// Created by cgm on 23/07/24. +// + +#ifndef GNSS_SDR_OSNMA_NAV_DATA_MANAGER_H +#define GNSS_SDR_OSNMA_NAV_DATA_MANAGER_H + +#include "osnma_data.h" // NavData +#include // uint32_t +#include +#include + +/** + * @class OSNMA_nav_data_Manager + * @brief Class for managing OSNMA navigation data + * @details It does good stuff + * @remarks throw it whatever, it will improve it. Does good stuff + */ +class OSNMA_nav_data_Manager{ +public: + OSNMA_nav_data_Manager() = default; + bool have_nav_data(std::string nav_bits, uint32_t PRNd, uint32_t TOW); + bool have_nav_data(uint32_t PRNd, uint32_t TOW, uint8_t ADKD); + bool have_nav_data(const Tag& t) const; + void add_navigation_data(std::string nav_bits, uint32_t PRNd, uint32_t TOW); // gets the bits and adds them to the list + std::string get_navigation_data(const Tag& t); + + void update_nav_data(const std::multimap& tags_verified, const uint8_t tag_size); + std::vector get_verified_data(); + void print_status(); +private: + bool have_PRNd_nav_data(uint32_t PRNd); + + std::map> _satellite_nav_data{}; // NavData sorted by [PRNd][TOW_start] + const uint32_t L_t_min{40}; + const uint16_t EPH_SIZE{549}; + const uint16_t UTC_SIZE{141}; + const uint16_t MAX_ALLOWED_SIZE{150}; // arbitrary maximum for the navigation data container + +}; +#endif // GNSS_SDR_OSNMA_NAV_DATA_MANAGER_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 d0df27a3b..d741dfbf6 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 @@ -64,6 +64,7 @@ public: TEST_F(OsnmaMsgReceiverTest, ComputeMerkleRoot) { + // input data taken from Receiver Guidelines v1.3, A.7 // Arrange // ---------- std::vector computed_merkle_root; @@ -74,6 +75,7 @@ TEST_F(OsnmaMsgReceiverTest, ComputeMerkleRoot) dsm_pkr_message.mid = 0x01; std::vector base_leaf = helper.convert_from_hex_string("120303B2CE64BC207BDD8BC4DF859187FCB686320D63FFA091410FC158FBB77980EA"); + // ITN std::vector vec = helper.convert_from_hex_string("7CBE05D9970CFC9E22D0A43A340EF557624453A2E821AADEAC989C405D78BA06" "956380BAB0D2C939EC6208151040CCFFCF1FB7156178FD1255BA0AECAAA253F7" "407B6C5DD4DF059FF8789474061301E1C34881DB7A367A913A3674300E21EAB1" @@ -92,6 +94,7 @@ TEST_F(OsnmaMsgReceiverTest, ComputeMerkleRoot) TEST_F(OsnmaMsgReceiverTest, ComputeBaseLeaf) { + // input data taken from Receiver Guidelines v1.3, A.7 // Arrange // ---------- std::vector expected_base_leaf = helper.convert_from_hex_string("120303B2CE64BC207BDD8BC4DF859187FCB686320D63FFA091410FC158FBB77980EA"); @@ -109,7 +112,9 @@ TEST_F(OsnmaMsgReceiverTest, ComputeBaseLeaf) ASSERT_EQ(computed_base_leaf,expected_base_leaf); } -TEST_F(OsnmaMsgReceiverTest, VerifyPublicKey){ // values taken from RG A.7 +TEST_F(OsnmaMsgReceiverTest, VerifyPublicKey){ + + // input data taken from Receiver Guidelines v1.3, A.7 // Arrange // ---------- osnma->d_crypto->set_merkle_root(helper.convert_from_hex_string("A10C440F3AA62453526DB4AF76DF8D9410D35D8277397D7053C700D192702B0D")); @@ -126,7 +131,7 @@ TEST_F(OsnmaMsgReceiverTest, VerifyPublicKey){ // values taken from RG A.7 // Act // ---------- - bool result = osnma->verify_dsm_pkr(dsm_pkr_message); + bool result = osnma->verify_dsm_pkr(dsm_pkr_message); // TODO - refactor method so that output is more than a boolean. // Assert // ---------- @@ -136,6 +141,7 @@ TEST_F(OsnmaMsgReceiverTest, VerifyPublicKey){ // values taken from RG A.7 TEST_F(OsnmaMsgReceiverTest, BuildTagMessageM0) { + // input data taken from Receiver Guidelines v1.3, A.6.5.1 // Arrange // ---------- // m0 @@ -181,6 +187,7 @@ TEST_F(OsnmaMsgReceiverTest, BuildTagMessageM0) } TEST_F(OsnmaMsgReceiverTest, TagVerification) { + // input data taken from Receiver Guidelines v1.3, A.6.5.1 // Arrange // ---------- // Tag0 @@ -204,20 +211,10 @@ TEST_F(OsnmaMsgReceiverTest, TagVerification) { MTI.tag_info.cop = 0x0F; Tag t0(MTI, TOW_Tag0, WN, PRNa, CTR); - - // Act // ---------- bool result_tag0 = osnma->verify_tag(t0); - - - - - // Assert - // ---------- - //ASSERT_TRUE(result_tag0); - // Tag3 uint32_t TOW_Tag3 = 345660; uint32_t TOW_NavData_Tag3 = TOW_Tag3 - 30; @@ -247,6 +244,7 @@ TEST_F(OsnmaMsgReceiverTest, TagVerification) { } TEST_F(OsnmaMsgReceiverTest, TeslaKeyVerification) { + // input data taken from Receiver Guidelines v1.3, A.5.2 // Arrange // ---------- osnma->d_tesla_key_verified = false; @@ -262,16 +260,9 @@ TEST_F(OsnmaMsgReceiverTest, TeslaKeyVerification) { std::vector key = {0x2D, 0xC3, 0xA3, 0xCD, 0xB1, 0x17, 0xFA, 0xAD, 0xB8, 0x3B, 0x5F, 0x0B, 0x6F, 0xEA, 0x88, 0xEB}; // K2 uint32_t TOW = 345630; - - - // Act // ---------- - bool result = osnma->verify_tesla_key(key, TOW); - - - - + bool result = osnma->verify_tesla_key(key, TOW); // TODO - refactor so that output is not a boolean. Or use last_verified_tesla_key? // Assert // ----------