From f3a204fec69c9c93873772dcd449ce28c796acdf Mon Sep 17 00:00:00 2001 From: cesaaargm Date: Sun, 12 May 2024 11:05:20 +0200 Subject: [PATCH] =?UTF-8?q?[TAS-174]=20retrieve=20NavData=20(W1=E2=86=92W5?= =?UTF-8?q?)=20directly=20from=20osnma=20test=20vector=20file.=20Add=20Osn?= =?UTF-8?q?ma=5FHelper=20class.=20remove=20tag.build=5Fmessage.=20Ignore?= =?UTF-8?q?=20W33.=20Reporting=20changes.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/core/libs/osnma_msg_receiver.cc | 87 +++++++++++-------- src/core/libs/osnma_msg_receiver.h | 2 + src/core/system_parameters/CMakeLists.txt | 2 + .../system_parameters/galileo_inav_message.h | 3 +- src/core/system_parameters/osnma_data.cc | 8 +- src/core/system_parameters/osnma_data.h | 13 ++- src/core/system_parameters/osnma_helper.cc | 34 ++++++++ src/core/system_parameters/osnma_helper.h | 33 +++++++ 8 files changed, 136 insertions(+), 46 deletions(-) create mode 100644 src/core/system_parameters/osnma_helper.cc create mode 100644 src/core/system_parameters/osnma_helper.h 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