diff --git a/src/core/libs/gnss_crypto.h b/src/core/libs/gnss_crypto.h index b308f820b..9fb16f96e 100644 --- a/src/core/libs/gnss_crypto.h +++ b/src/core/libs/gnss_crypto.h @@ -78,7 +78,7 @@ private: void readPublicKeyFromPEM(const std::string& pemFilePath); bool readPublicKeyFromCRT(const std::string& crtFilePath); bool convert_raw_to_der_ecdsa(const std::vector& raw_signature, std::vector& der_signature) const; - std::vector convert_from_hex_str(const std::string& input) const; + std::vector convert_from_hex_str(const std::string& input) const; // TODO - deprecate if OSNMA helper is to do this operation #if USE_GNUTLS_FALLBACK void decompress_public_key_secp256r1(const std::vector& compressed_key, std::vector& x, std::vector& y) const; void decompress_public_key_secp521r1(const std::vector& compressed_key, std::vector& x, std::vector& y) const; diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc index 72e6d7ef0..ace7dc40e 100644 --- a/src/core/libs/osnma_msg_receiver.cc +++ b/src/core/libs/osnma_msg_receiver.cc @@ -183,8 +183,8 @@ void osnma_msg_receiver::msg_handler_osnma(const pmt::pmt_t& msg) if (delta_T <= d_T_L) { d_tags_to_verify = {0, 4, 12}; - LOG(INFO) << "Galileo OSNMA: time constraint OK ( delta_T=" << delta_T << " s)"; - std::cout << "Galileo OSNMA: time constraint OK ( delta_T=" << delta_T << " s)" << std::endl; + LOG(INFO) << "Galileo OSNMA: time constraint OK (delta_T=" << delta_T << " s)"; + std::cout << "Galileo OSNMA: time constraint OK (delta_T=" << delta_T << " s)" << std::endl; } else if (delta_T > d_T_L && delta_T <= 10 * d_T_L) { @@ -1067,7 +1067,18 @@ void osnma_msg_receiver::process_mack_message() // add tag0 first Tag tag0(*mack); d_tags_awaiting_verify.insert(std::pair(mack->TOW, tag0)); - // bool ret = verify_macseq(*mack); + LOG(INFO) << "Galileo OSNMA: Add Tag0 Id= " + << tag0.tag_id + << ", value=0x" << std::setfill('0') << std::setw(10) << std::hex << std::uppercase + << tag0.received_tag << std::dec + << ", TOW=" + << tag0.TOW + << ", ADKD=" + << static_cast(tag0.ADKD) + << ", PRNa=" + << static_cast(tag0.PRNa) + << ", PRNd=" + << static_cast(tag0.PRN_d); std::vector macseq_verified_tags = verify_macseq_new(*mack); for (auto& tag_and_info : macseq_verified_tags) { @@ -1521,11 +1532,11 @@ void osnma_msg_receiver::remove_verified_tags() << static_cast(it->second.PRNa) << ", PRNd=" << static_cast(it->second.PRN_d) - << ", status= " + << ", status=" << d_helper->verification_status_str(it->second.status); it = d_tags_awaiting_verify.erase(it); } - else if (it->second.skipped >= 20) + else if ((it->second.ADKD != 12 && !d_nav_data_manager->have_nav_data(it->second)) || (it->second.ADKD == 12 && (it->second.TOW + 30 * 11 < d_helper->get_TOW(d_last_verified_key_GST)))) { LOG(INFO) << "Galileo OSNMA: Tag verification :: DELETE tag Id=" << it->second.tag_id @@ -1539,8 +1550,9 @@ void osnma_msg_receiver::remove_verified_tags() << static_cast(it->second.PRNa) << ", PRNd=" << static_cast(it->second.PRN_d) - << ", status= " - << d_helper->verification_status_str(it->second.status); + << ", status=" + << d_helper->verification_status_str(it->second.status) + << ". SV out of sight / NavData unavailable."; it = d_tags_awaiting_verify.erase(it); } else @@ -1563,7 +1575,7 @@ void osnma_msg_receiver::remove_verified_tags() << static_cast(it.second.PRNa) << ", PRNd=" << static_cast(it.second.PRN_d) - << ", status= " + << ", status=" << d_helper->verification_status_str(it.second.status); } } diff --git a/src/core/libs/osnma_nav_data_manager.cc b/src/core/libs/osnma_nav_data_manager.cc index 5a9331dae..03d5a4442 100644 --- a/src/core/libs/osnma_nav_data_manager.cc +++ b/src/core/libs/osnma_nav_data_manager.cc @@ -33,8 +33,9 @@ void OSNMA_NavDataManager::add_navigation_data(const std::string& nav_bits, uint if (not have_nav_data(nav_bits, PRNd, TOW)) { d_satellite_nav_data[PRNd][TOW].add_nav_data(nav_bits); - d_satellite_nav_data[PRNd][TOW].PRNd = PRNd; + d_satellite_nav_data[PRNd][TOW].set_prn_d(PRNd); d_satellite_nav_data[PRNd][TOW].set_tow_sf0(TOW); + d_satellite_nav_data[PRNd][TOW].set_last_received_TOW(TOW); } } @@ -74,7 +75,7 @@ void OSNMA_NavDataManager::update_nav_data(const std::multimap& t // find associated OSNMA_NavData if (tag.second.nav_data == nav_data) { - d_satellite_nav_data[tag.second.PRN_d][tow_it.first].verified_bits += tag_size; + d_satellite_nav_data[tag.second.PRN_d][tow_it.first].set_update_verified_bits(tag_size); } } } @@ -89,10 +90,10 @@ std::vector OSNMA_NavDataManager::get_verified_data() { for (const auto& tow_navdata : prna.second) { - if (tow_navdata.second.verified_bits >= L_t_min) + if (tow_navdata.second.get_verified_bits() >= L_t_min) { result.push_back(tow_navdata.second); - d_satellite_nav_data[prna.first][tow_navdata.first].verified = true; + d_satellite_nav_data[prna.first][tow_navdata.first].set_verified_status(true); } } } @@ -129,31 +130,62 @@ bool OSNMA_NavDataManager::have_nav_data(uint32_t PRNd, uint32_t TOW, uint8_t AD std::string OSNMA_NavDataManager::get_navigation_data(const Tag& tag) const { + // Check if Dummy Tag, navData is all zeros + if (tag.cop == 0) + { + if (tag.ADKD == 0 || tag.ADKD == 12) + { + return std::string(549, '0'); + } + else if (tag.ADKD == 4) + { + return std::string(141, '0'); + } + } auto prn_it = d_satellite_nav_data.find(tag.PRN_d); if (prn_it == d_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) // note: starts with smallest (i.e. oldest) navigation dataset + auto nav_data = prn_it->second.find(tag.TOW - 30); + if (nav_data != prn_it->second.end()) { - // 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 (tag.ADKD == 0 || tag.ADKD == 12) + if (!nav_data->second.get_ephemeris_data().empty()) { - if (!tow_it.second.get_ephemeris_data().empty()) - { - return tow_it.second.get_ephemeris_data(); - } + return nav_data->second.get_ephemeris_data(); } - else if (tag.ADKD == 4) + } + else if (tag.ADKD == 4) + { + if (!nav_data->second.get_utc_data().empty()) { - if (!tow_it.second.get_utc_data().empty()) + return nav_data->second.get_utc_data(); + } + } + } + else + { + for (auto rev_it = prn_it->second.rbegin(); rev_it != prn_it->second.rend(); ++rev_it) // note: starts with largest (i.e. newest) navigation dataset + { + // Check if current key (TOW) fulfills condition + if ((tag.TOW - 30 * tag.cop <= rev_it->first || tag.TOW - 30 * tag.cop <= rev_it->second.get_last_received_TOW()) && rev_it->first < tag.TOW) + { + if (tag.ADKD == 0 || tag.ADKD == 12) { - return tow_it.second.get_utc_data(); + if (!rev_it->second.get_ephemeris_data().empty()) + { + return rev_it->second.get_ephemeris_data(); + } + } + else if (tag.ADKD == 4) + { + if (!rev_it->second.get_utc_data().empty()) + { + return rev_it->second.get_utc_data(); + } } } } @@ -179,7 +211,7 @@ bool OSNMA_NavDataManager::have_nav_data(const std::string& nav_bits, uint32_t P { if (data_timestamp.second.get_ephemeris_data() == nav_bits) { - data_timestamp.second.update_last_received_timestamp(TOW); + data_timestamp.second.set_last_received_TOW(TOW); return true; } } @@ -187,7 +219,7 @@ bool OSNMA_NavDataManager::have_nav_data(const std::string& nav_bits, uint32_t P { if (data_timestamp.second.get_utc_data() == nav_bits) { - data_timestamp.second.update_last_received_timestamp(TOW); + data_timestamp.second.set_last_received_TOW(TOW); return true; } } @@ -204,30 +236,57 @@ bool OSNMA_NavDataManager::have_nav_data(const std::string& nav_bits, uint32_t P */ bool OSNMA_NavDataManager::have_nav_data(const Tag& t) const { + if (t.cop == 0) + { + return true; + } auto prn_it = d_satellite_nav_data.find(t.PRN_d); if (prn_it == d_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) // note: starts with smallest (i.e. oldest) navigation dataset + // try find target TOW directly first + auto nav_data = prn_it->second.find(t.TOW - 30); + if (nav_data != prn_it->second.end()) { - // 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 (t.ADKD == 0 || t.ADKD == 12) + if (!nav_data->second.get_ephemeris_data().empty()) { - if (!tow_it.second.get_ephemeris_data().empty()) - { - return true; - } + return true; } - else if (t.ADKD == 4) + } + else if (t.ADKD == 4) + { + if (!nav_data->second.get_utc_data().empty()) { - if (!tow_it.second.get_utc_data().empty()) + return true; + } + } + } + else + { + // iterate in reverse order to find matching TOW with Tag's COP value + std::map tow_map = prn_it->second; + for (auto rev_it = tow_map.rbegin(); rev_it != tow_map.rend(); ++rev_it) // note: starts with largest (i.e. newest) navigation dataset + { + // Check if current key (TOW) fulfills cut-off point and is not received after the tag + if ((t.TOW - 30 * t.cop <= rev_it->first || t.TOW - 30 * t.cop <= rev_it->second.get_last_received_TOW()) && rev_it->first < t.TOW) + { + if (t.ADKD == 0 || t.ADKD == 12) { - return true; + if (!rev_it->second.get_ephemeris_data().empty()) + { + return true; + } + } + else if (t.ADKD == 4) + { + if (!rev_it->second.get_utc_data().empty()) + { + return true; + } } } } @@ -245,17 +304,17 @@ void OSNMA_NavDataManager::log_status() const for (const auto& nav_data : tow_data) { LOG(INFO) << "Galileo OSNMA: IOD_nav=0b" << std::uppercase - << std::bitset<10>(nav_data.second.IOD_nav) + << std::bitset<10>(nav_data.second.get_IOD_nav()) << ", TOW_start=" << nav_data.second.get_tow_sf0() << ", TOW_last=" << nav_data.second.get_last_received_TOW() << ", l_t=" - << nav_data.second.verified_bits + << nav_data.second.get_verified_bits() << ", PRNd=" - << nav_data.second.PRNd + << nav_data.second.get_prn_d() << ", verified=" - << nav_data.second.verified; + << nav_data.second.get_verified_status(); } } } diff --git a/src/core/system_parameters/galileo_inav_message.cc b/src/core/system_parameters/galileo_inav_message.cc index ab62b684c..cb23ddac3 100644 --- a/src/core/system_parameters/galileo_inav_message.cc +++ b/src/core/system_parameters/galileo_inav_message.cc @@ -1413,20 +1413,15 @@ int32_t Galileo_Inav_Message::page_jk_decoder(const char* data_jk) */ OSNMA_msg Galileo_Inav_Message::get_osnma_msg() { - // TODO - why PRN of word 4 is done separately? nma_position_filled = std::array{}; // Fill TOW and WN nma_msg.WN_sf0 = WN_0; - int32_t TOW_sf0 = TOW_5 - 25; //- 24; // according to OS SIS ICD, TOW of word 5 is 25 seconds after Sf start TODO review + int32_t TOW_sf0 = TOW_5 - 25; if (TOW_sf0 < 0) { TOW_sf0 += 604800; } nma_msg.TOW_sf0 = static_cast(TOW_sf0); - // get ephemeris, clock and iono correction datn and GST-UTC and GST-GPS converstion parameters (may be incomplete) - nma_msg.EphemerisData = get_ephemeris(); - nma_msg.IonoData = get_iono(); - nma_msg.UtcModelData = get_utc_model(); return nma_msg; } diff --git a/src/core/system_parameters/galileo_inav_message.h b/src/core/system_parameters/galileo_inav_message.h index 6eeffddaf..92f3963a1 100644 --- a/src/core/system_parameters/galileo_inav_message.h +++ b/src/core/system_parameters/galileo_inav_message.h @@ -48,16 +48,9 @@ public: OSNMA_msg() = default; std::array mack{}; std::array hkroot{}; - uint32_t PRN{}; - uint32_t WN_sf0{}; - uint32_t TOW_sf0{}; - std::vector EphemerisClockAndStatusData{}; // TODO _2 rename and substitute this - std::string EphemerisClockAndStatusData_2{}; - std::vector TimingData{}; - std::string TimingData_2{}; - Galileo_Ephemeris EphemerisData{}; - Galileo_Iono IonoData{}; - Galileo_Utc_Model UtcModelData{}; + uint32_t PRN{}; // PRN_a authentication data PRN + uint32_t WN_sf0{}; // Week number at the start of OSNMA subframe + uint32_t TOW_sf0{}; // TOW at the start of OSNMA subframe }; /*! diff --git a/src/core/system_parameters/osnma_data.cc b/src/core/system_parameters/osnma_data.cc index b5355113f..b56d3b940 100644 --- a/src/core/system_parameters/osnma_data.cc +++ b/src/core/system_parameters/osnma_data.cc @@ -35,25 +35,11 @@ bool OSNMA_NavData::add_nav_data(const std::string& nav_data) } return false; } - - std::string OSNMA_NavData::get_utc_data() const { return d_utc; } - - std::string OSNMA_NavData::get_ephemeris_data() const { return d_ephemeris_iono; } - - -/** - * Updates the last TOW the NavData bits were received. - * @param TOW - */ -void OSNMA_NavData::update_last_received_timestamp(uint32_t TOW) -{ - d_last_received_TOW = TOW; -} diff --git a/src/core/system_parameters/osnma_data.h b/src/core/system_parameters/osnma_data.h index e3c78cdc4..0816abda6 100644 --- a/src/core/system_parameters/osnma_data.h +++ b/src/core/system_parameters/osnma_data.h @@ -137,30 +137,36 @@ class OSNMA_NavData public: OSNMA_NavData() : nav_data_id(id_counter++) {} const uint32_t nav_data_id; - std::string get_utc_data() const; std::string get_ephemeris_data() const; + uint32_t get_verified_bits() const { return verified_bits; } + uint32_t get_prn_d() const { return PRNd; } + uint32_t get_IOD_nav() const { return IOD_nav; } uint32_t get_last_received_TOW() const { return d_last_received_TOW; } uint32_t get_tow_sf0() const { return d_TOW_sf0; } - + bool have_this_bits(std::string nav_data); + bool get_verified_status() const { return verified; } bool add_nav_data(const std::string& nav_data); void set_tow_sf0(int value) { d_TOW_sf0 = value; } void set_ephemeris_data(std::string value) { d_ephemeris_iono = value; } void set_utc_data(std::string value) { d_utc = value; } void update_last_received_timestamp(uint32_t TOW); - - uint32_t verified_bits{0}; - uint32_t IOD_nav{0}; - uint32_t PRNd{0}; - uint32_t ADKD{}; - bool verified{false}; + void set_prn_d(uint32_t value) { PRNd = value; } + void set_last_received_TOW(uint32_t TOW) { d_last_received_TOW = TOW; }; + void set_update_verified_bits(uint32_t morebits) { verified_bits += morebits; } + void set_verified_status(bool value) { verified = value; } + void set_IOD_nav(uint32_t value) { IOD_nav = value; } private: + static uint32_t id_counter; std::string d_ephemeris_iono{""}; std::string d_utc{""}; uint32_t d_TOW_sf0{0}; uint32_t d_last_received_TOW{0}; - static uint32_t id_counter; + uint32_t PRNd{0}; + uint32_t verified_bits{0}; + uint32_t IOD_nav{0}; + bool verified{false}; }; diff --git a/src/tests/unit-tests/signal-processing-blocks/osnma/osnma_test_vectors.cc b/src/tests/unit-tests/signal-processing-blocks/osnma/osnma_test_vectors.cc index 5c097e10b..755725d98 100644 --- a/src/tests/unit-tests/signal-processing-blocks/osnma/osnma_test_vectors.cc +++ b/src/tests/unit-tests/signal-processing-blocks/osnma/osnma_test_vectors.cc @@ -85,6 +85,12 @@ TEST_F(OsnmaTestVectors, NominalTestConf1) ASSERT_TRUE(result); // Assert + LOG(INFO) << "Successful tags count= " << osnma->d_count_successful_tags; + LOG(INFO) << "Failed tags count= " << osnma->d_count_failed_tags; + LOG(INFO) << "Unverified tags count= " << osnma->d_tags_awaiting_verify.size(); + LOG(INFO) << "Failed Kroot count= " << osnma->d_count_failed_Kroot; + LOG(INFO) << "Failed PK count= " << osnma->d_count_failed_pubKey; + LOG(INFO) << "Failed MACSEQ count= " << osnma->d_count_failed_macseq; ASSERT_EQ(osnma->d_count_failed_tags, 0); ASSERT_EQ(osnma->d_count_failed_Kroot, 0); ASSERT_EQ(osnma->d_count_failed_pubKey, 0); @@ -113,6 +119,12 @@ TEST_F(OsnmaTestVectors, NominalTestConf2) ASSERT_TRUE(result); // Assert + LOG(INFO) << "Successful tags count= " << osnma->d_count_successful_tags; + LOG(INFO) << "Failed tags count= " << osnma->d_count_failed_tags; + LOG(INFO) << "Unverified tags count= " << osnma->d_tags_awaiting_verify.size(); + LOG(INFO) << "Failed Kroot count= " << osnma->d_count_failed_Kroot; + LOG(INFO) << "Failed PK count= " << osnma->d_count_failed_pubKey; + LOG(INFO) << "Failed MACSEQ count= " << osnma->d_count_failed_macseq; ASSERT_EQ(osnma->d_count_failed_tags, 0); ASSERT_EQ(osnma->d_count_failed_Kroot, 0); ASSERT_EQ(osnma->d_count_failed_pubKey, 0); @@ -145,6 +157,12 @@ TEST_F(OsnmaTestVectors, PublicKeyRenewal) ASSERT_TRUE(result); // Assert + LOG(INFO) << "Successful tags count= " << osnma->d_count_successful_tags; + LOG(INFO) << "Failed tags count= " << osnma->d_count_failed_tags; + LOG(INFO) << "Unverified tags count= " << osnma->d_tags_awaiting_verify.size(); + LOG(INFO) << "Failed Kroot count= " << osnma->d_count_failed_Kroot; + LOG(INFO) << "Failed PK count= " << osnma->d_count_failed_pubKey; + LOG(INFO) << "Failed MACSEQ count= " << osnma->d_count_failed_macseq; ASSERT_EQ(osnma->d_count_failed_tags, 0); ASSERT_EQ(osnma->d_count_failed_Kroot, 0); ASSERT_EQ(osnma->d_count_failed_pubKey, 0); @@ -176,6 +194,12 @@ TEST_F(OsnmaTestVectors, PublicKeyRevocation) ASSERT_TRUE(result); // Assert + LOG(INFO) << "Successful tags count= " << osnma->d_count_successful_tags; + LOG(INFO) << "Failed tags count= " << osnma->d_count_failed_tags; + LOG(INFO) << "Unverified tags count= " << osnma->d_tags_awaiting_verify.size(); + LOG(INFO) << "Failed Kroot count= " << osnma->d_count_failed_Kroot; + LOG(INFO) << "Failed PK count= " << osnma->d_count_failed_pubKey; + LOG(INFO) << "Failed MACSEQ count= " << osnma->d_count_failed_macseq; ASSERT_EQ(osnma->d_count_failed_tags, 0); ASSERT_EQ(osnma->d_count_failed_Kroot, 0); ASSERT_EQ(osnma->d_count_failed_pubKey, 0);