From 620249f9f2aab0e9741787d0bc64c2daea9723a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cesare=20G=2E=20Mart=C3=ADnez?= Date: Thu, 25 Jan 2024 17:11:28 +0100 Subject: [PATCH] [TAS-126] [TAS-125] NavData storage of last 10 SFs / NavData retrieval: store needed NavData within osnma_data::NavData for the last 10 subframes * In osnma_msg_receiver: d_old_navdata_buffer to store last 10 NavData. * In osnma_msg_receiver::process_mack_message: pass needed data to the newly created structure osnma_data::NavData, generate needed vectors for tag verification. * In osnma_data.h/cc: Create NavData structure, Create osnma_data.cc source file, add to CMakeLists source file. --- src/core/libs/osnma_msg_receiver.cc | 13 +- src/core/libs/osnma_msg_receiver.h | 1 + src/core/system_parameters/CMakeLists.txt | 1 + .../system_parameters/galileo_inav_message.cc | 1 + src/core/system_parameters/osnma_data.cc | 185 ++++++++++++++++++ src/core/system_parameters/osnma_data.h | 22 +++ 6 files changed, 219 insertions(+), 4 deletions(-) create mode 100644 src/core/system_parameters/osnma_data.cc diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc index 68dec02e5..66099e054 100644 --- a/src/core/libs/osnma_msg_receiver.cc +++ b/src/core/libs/osnma_msg_receiver.cc @@ -145,7 +145,7 @@ void osnma_msg_receiver::msg_handler_osnma(const pmt::pmt_t& msg) // Send the resulting decoded NMA data (if available) to PVT if (d_new_data == true) // TODO where is it set to true? { - auto osnma_data_ptr = std::make_shared(d_osnma_data); // C: why create new object and pass it empty to Pvt? + auto osnma_data_ptr = std::make_shared(d_osnma_data); this->message_port_pub(pmt::mp("OSNMA_to_PVT"), pmt::make_any(osnma_data_ptr)); d_new_data = false; // d_osnma_data = OSNMA_data(); @@ -821,9 +821,14 @@ void osnma_msg_receiver::read_mack_body() void osnma_msg_receiver::process_mack_message(const std::shared_ptr& osnma_msg) { - // TODO - is it filled at this point always? - d_old_mack_message.push_back(d_osnma_data.d_mack_message); // C: old mack message is needed for - + d_old_mack_message.push_back(d_osnma_data.d_mack_message); // last 10 MACKs are needed to be stored as per ICD + // populate d_nav_data with three classes of osnma_msg + d_osnma_data.d_nav_data.EphemerisData = osnma_msg->EphemerisData; + d_osnma_data.d_nav_data.IonoData = osnma_msg->IonoData; + d_osnma_data.d_nav_data.UtcData = osnma_msg->UtcModelData; + d_osnma_data.d_nav_data.generate_eph_iono_vector2(); + d_osnma_data.d_nav_data.generate_utc_vector(); + d_old_navdata_buffer.push_back(d_osnma_data.d_nav_data); // last 10 NavData messages are needed to be stored as per ICD // MACSEQ validation - case no FLX Tags // retrieve data to verify MACK tags diff --git a/src/core/libs/osnma_msg_receiver.h b/src/core/libs/osnma_msg_receiver.h index b79719093..6b694f32d 100644 --- a/src/core/libs/osnma_msg_receiver.h +++ b/src/core/libs/osnma_msg_receiver.h @@ -73,6 +73,7 @@ private: void process_mack_message(const std::shared_ptr& osnma_msg); boost::circular_buffer d_old_mack_message; + boost::circular_buffer d_old_navdata_buffer; // buffer that holds last 10 received navdata messages std::unique_ptr d_dsm_reader; std::unique_ptr d_crypto; diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index 9db20a317..dc78ae094 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -30,6 +30,7 @@ set(SYSTEM_PARAMETERS_SOURCES glonass_gnav_navigation_message.cc reed_solomon.cc osnma_dsm_reader.cc + osnma_data.cc ) set(SYSTEM_PARAMETERS_HEADERS diff --git a/src/core/system_parameters/galileo_inav_message.cc b/src/core/system_parameters/galileo_inav_message.cc index 7a500d788..869ba3c35 100644 --- a/src/core/system_parameters/galileo_inav_message.cc +++ b/src/core/system_parameters/galileo_inav_message.cc @@ -1384,6 +1384,7 @@ 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; diff --git a/src/core/system_parameters/osnma_data.cc b/src/core/system_parameters/osnma_data.cc new file mode 100644 index 000000000..db4b5c553 --- /dev/null +++ b/src/core/system_parameters/osnma_data.cc @@ -0,0 +1,185 @@ +/*! +* \file osnma_data.cc +* \brief Class for Galileo OSNMA data storage +* \author Carles Fernandez-Prades, 2020-2023 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_data.h" +#include + +void NavData::generate_eph_iono_vector() +{ + ephemeris_iono_vector.clear(); + ephemeris_iono_vector.push_back(static_cast((EphemerisData.IOD_nav & 0b0000'0000'0000'0000'0000'0011'1111'1100) >> 2)); + ephemeris_iono_vector.push_back(static_cast((EphemerisData.IOD_nav & 0b0000'0000'0000'0000'0000'0000'0000'0011) << 6 + | (EphemerisData.toe & 0b0000'0000'0000'0000'0011'1111'1111'1111) >> 8)); + ephemeris_iono_vector.push_back(static_cast(EphemerisData.toe)); + uint64_t binary_representation; + memcpy(&binary_representation, &EphemerisData.M_0, sizeof(EphemerisData.M_0)); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 8))); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 16))); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 24))); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 32))); + memcpy(&binary_representation, &EphemerisData.ecc, sizeof(EphemerisData.ecc)); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 8))); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 16))); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 24))); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 32))); + memcpy(&binary_representation, &EphemerisData.sqrtA, sizeof(EphemerisData.sqrtA)); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 8))); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 16))); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 24))); + ephemeris_iono_vector.push_back(static_cast(binary_representation >> (64 - 32))); + + // TODO: Implement the function to generate the rest of pages +} + +void NavData::generate_eph_iono_vector2() +{ + std::vector eph_iono_vector; + 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) + { + // 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; + eph_iono_vector.push_back(extracted_bits); + + // Remove the extracted bits from the buffer + bit_count -= 8; + bit_buffer = bit_buffer & ~(0xFF << bit_count); + } + + } + + // If there are any bits left in the buffer, add them to the vector + if (bit_count > 0) + { + eph_iono_vector.push_back(static_cast(bit_buffer)); + } +} + +void NavData::generate_utc_vector() +{ + 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)); + } +} + +std::vector NavData::get_eph_iono_vector() +{ + return ephemeris_iono_vector; +} + +std::vector NavData::get_utc_vector() +{ + return utc_vector; +} diff --git a/src/core/system_parameters/osnma_data.h b/src/core/system_parameters/osnma_data.h index a19666d5b..941e7c8bc 100644 --- a/src/core/system_parameters/osnma_data.h +++ b/src/core/system_parameters/osnma_data.h @@ -18,6 +18,9 @@ #ifndef GNSS_SDR_OSNMA_DATA_H #define GNSS_SDR_OSNMA_DATA_H +#include "galileo_ephemeris.h" +#include "galileo_iono.h" +#include "galileo_utc_model.h" #include #include #include @@ -119,6 +122,24 @@ public: std::vector key; }; +class NavData +{ +public: + NavData() = default; + Galileo_Ephemeris EphemerisData; + Galileo_Iono IonoData; + Galileo_Utc_Model UtcData; + void generate_eph_iono_vector(); // TODO check with Carles procedure and compare with v2 + void generate_eph_iono_vector2(); + void generate_utc_vector(); // TODO + std::vector get_eph_iono_vector(); // TODO + std::vector get_utc_vector(); // TODO +private: + std::vector ephemeris_iono_vector; + std::vector utc_vector; + +}; + /*! * \brief This class handles ONSMA data * See https://www.gsc-europa.eu/sites/default/files/sites/all/files/Galileo_OSNMA_User_ICD_for_Test_Phase_v1.0.pdf @@ -132,6 +153,7 @@ public: DSM_PKR_message d_dsm_pkr_message; DSM_KROOT_message d_dsm_kroot_message; MACK_message d_mack_message; + NavData d_nav_data; };