mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-12 11:10:33 +00:00
[TAS-250] [FEAT] Implement PK renewal and revocation.
This commit is contained in:
cesaaargm
parent
5675b996e8
commit
fc1541ef10
@ -156,7 +156,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?
|
||||
if (d_new_data) // TODO where is it set to true?
|
||||
{
|
||||
auto osnma_data_ptr = std::make_shared<OSNMA_data>(d_osnma_data);
|
||||
this->message_port_pub(pmt::mp("OSNMA_to_PVT"), pmt::make_any(osnma_data_ptr));
|
||||
@ -170,11 +170,22 @@ void osnma_msg_receiver::msg_handler_osnma(const pmt::pmt_t& msg)
|
||||
void osnma_msg_receiver::process_osnma_message(const std::shared_ptr<OSNMA_msg>& osnma_msg)
|
||||
{
|
||||
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*/)
|
||||
if (d_osnma_data.d_nma_header.nmas == 0 || d_osnma_data.d_nma_header.nmas == 3)
|
||||
{
|
||||
LOG(WARNING) << "Galileo OSNMA: NMAS invalid, skipping osnma message";
|
||||
return;
|
||||
}
|
||||
if (d_osnma_data.d_nma_header.nmas == 2 /*OP*/ && d_osnma_data.d_nma_header.cpks == 4 /*NPK*/ && d_GST_PKR_start == 0){
|
||||
d_flag_PK_renewal = true;
|
||||
d_GST_PKR_start = d_helper->compute_gst(osnma_msg->WN_sf0, osnma_msg->TOW_sf0);
|
||||
LOG(INFO) << "Galileo OSNMA: Public Key Renewal :: Start at GST=" << d_GST_PKR_start;
|
||||
std::cout << "Galileo OSNMA: Public Key Renewal :: Start at GST=" << d_GST_PKR_start << std::endl;
|
||||
}
|
||||
if (d_flag_PK_renewal && d_osnma_data.d_nma_header.nmas == 2 /*OP*/ && d_osnma_data.d_nma_header.cpks == 1 /*Nominal*/ ){
|
||||
d_flag_PK_renewal = false;
|
||||
LOG(INFO) << "Galileo OSNMA: Public Key Renewal :: Finished at GST=" << d_helper->compute_gst(osnma_msg->WN_sf0, osnma_msg->TOW_sf0);
|
||||
std::cout << "Galileo OSNMA: Public Key Renewal :: Finished at GST=" << d_helper->compute_gst(osnma_msg->WN_sf0, osnma_msg->TOW_sf0) << std::endl;
|
||||
}
|
||||
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
|
||||
@ -337,7 +348,7 @@ void osnma_msg_receiver::local_time_verification(const std::shared_ptr<OSNMA_msg
|
||||
|
||||
// TODO set flag to false to avoid processing dsm and MACK messages
|
||||
}
|
||||
else if (delta_T > d_T_L && delta_T <= 10 * delta_T)
|
||||
else if (delta_T > d_T_L && delta_T <= 10 * d_T_L)
|
||||
{
|
||||
d_tags_allowed = tags_to_verify::slow_eph;
|
||||
d_tags_to_verify = {12};
|
||||
@ -492,6 +503,12 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
|
||||
<< ", WN=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.wn_k)
|
||||
<< ", TOW=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.towh_k) * 3600;
|
||||
// local_time_verification(osnma_msg); // FIXME TODO: real time verification needed
|
||||
if(d_flag_PK_renewal && d_osnma_data.d_dsm_kroot_message.pkid == d_new_public_key_id && d_flag_NPK_set == false){
|
||||
// set new public key to be used.
|
||||
d_crypto->set_public_key(d_new_public_key);
|
||||
d_crypto->store_public_key(PEMFILE_DEFAULT);
|
||||
d_flag_NPK_set = true;
|
||||
}
|
||||
if (l_ds_bits == 512)
|
||||
{
|
||||
d_kroot_verified = d_crypto->verify_signature_ecdsa_p256(message, d_osnma_data.d_dsm_kroot_message.ds);
|
||||
@ -538,7 +555,11 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
|
||||
d_osnma_data.d_dsm_pkr_message.itn[k] = dsm_msg[k + 1];
|
||||
}
|
||||
d_osnma_data.d_dsm_pkr_message.npkt = d_dsm_reader->get_npkt(dsm_msg);
|
||||
d_osnma_data.d_dsm_pkr_message.npktid = d_dsm_reader->get_npktid(dsm_msg);
|
||||
uint8_t npktid = d_dsm_reader->get_npktid(dsm_msg);
|
||||
if (d_flag_PK_renewal && npktid > d_osnma_data.d_dsm_pkr_message.npktid){
|
||||
d_new_public_key_id = npktid;
|
||||
}
|
||||
d_osnma_data.d_dsm_pkr_message.npktid = npktid;
|
||||
|
||||
uint32_t l_npk_bytes = 0;
|
||||
const auto it = OSNMA_TABLE_5.find(d_osnma_data.d_dsm_pkr_message.npkt);
|
||||
@ -584,12 +605,15 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
|
||||
<< ", TOW=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.towh_k) * 3600*/
|
||||
<< " received";
|
||||
// C: NPK verification against Merkle tree root.
|
||||
if (!d_public_key_verified)
|
||||
{
|
||||
bool verification = verify_dsm_pkr(d_osnma_data.d_dsm_pkr_message);
|
||||
if (verification)
|
||||
{
|
||||
LOG(INFO) << "Galileo OSNMA: DSM-PKR verified successfully";
|
||||
d_public_key_verified = true;
|
||||
if (d_flag_PK_renewal){
|
||||
d_new_public_key = d_osnma_data.d_dsm_pkr_message.npk;
|
||||
}
|
||||
else {
|
||||
d_crypto->set_public_key(d_osnma_data.d_dsm_pkr_message.npk);
|
||||
d_crypto->store_public_key(PEMFILE_DEFAULT);
|
||||
}
|
||||
@ -903,7 +927,7 @@ void osnma_msg_receiver::read_mack_body()
|
||||
*/
|
||||
void osnma_msg_receiver::process_mack_message()
|
||||
{
|
||||
if (d_kroot_verified == false && d_tesla_key_verified == false)
|
||||
if (!d_kroot_verified && !d_tesla_key_verified)
|
||||
{
|
||||
LOG(WARNING) << "Galileo OSNMA: MACK cannot be processed, "
|
||||
<< "no Kroot nor TESLA key available.";
|
||||
|
||||
@ -133,6 +133,7 @@ private:
|
||||
uint32_t d_last_verified_key_GST{0};
|
||||
uint32_t d_GST_0{};
|
||||
uint32_t d_GST_SIS{};
|
||||
uint32_t d_GST_PKR_start{};
|
||||
|
||||
uint8_t d_Lt_min{}; // minimum equivalent tag length
|
||||
uint8_t d_Lt_verified_eph{0}; // verified tag bits - ephemeris
|
||||
@ -146,6 +147,11 @@ private:
|
||||
bool d_tesla_key_verified{false};
|
||||
bool d_flag_debug{false};
|
||||
bool d_flag_hot_start{false};
|
||||
bool d_flag_PK_renewal{false};
|
||||
bool d_flag_PK_revocation{false};
|
||||
uint8_t d_new_public_key_id{};
|
||||
std::vector<uint8_t> d_new_public_key;
|
||||
bool d_flag_NPK_set{false};
|
||||
|
||||
// Provide access to inner functions to Gtest
|
||||
FRIEND_TEST(OsnmaMsgReceiverTest, TeslaKeyVerification);
|
||||
@ -155,6 +161,7 @@ private:
|
||||
FRIEND_TEST(OsnmaMsgReceiverTest, ComputeBaseLeaf);
|
||||
FRIEND_TEST(OsnmaMsgReceiverTest, ComputeMerkleRoot);
|
||||
FRIEND_TEST(OsnmaTestVectors, OsnmaTestVectorsSimulation);
|
||||
FRIEND_TEST(OsnmaTestVectors, PublicKeyRenewal);
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -32,7 +32,7 @@
|
||||
|
||||
constexpr size_t SIZE_DSM_BLOCKS_BYTES = 13;
|
||||
|
||||
// OSNMA User ICD for the Test Phase, Issue 1.0, Table 1
|
||||
// OSNMA User ICD, Issue 1.1, Table 1
|
||||
const std::unordered_map<uint8_t, std::string> OSNMA_TABLE_1 = {
|
||||
{0, std::string("Reserved")},
|
||||
{1, std::string("Test")},
|
||||
@ -40,14 +40,14 @@ const std::unordered_map<uint8_t, std::string> OSNMA_TABLE_1 = {
|
||||
{3, std::string("Don't use")}}; // key: nmas, value: nmas status
|
||||
|
||||
|
||||
// OSNMA User ICD for the Test Phase, Issue 1.0, Table 1
|
||||
// OSNMA User ICD, Issue 1.1, Table 2
|
||||
const std::unordered_map<uint8_t, std::string> OSNMA_TABLE_2 = {
|
||||
{0, std::string("Reserved")},
|
||||
{1, std::string("Nominal")},
|
||||
{2, std::string("End of Chain (EOC)")},
|
||||
{3, std::string("Chain Revoked (CREV)")},
|
||||
{4, std::string("Public Key Revoked (PKREV)")},
|
||||
{5, std::string("Chain Revoked (CREV)")},
|
||||
{4, std::string("New Publick Key (NPK)")},
|
||||
{5, std::string("Public Key Revoked (PKREV)")},
|
||||
{6, std::string("New Merkle Tree (NMT)")},
|
||||
{7, std::string("Alert Message (AM)")}}; // key: cpks, value: cpks status
|
||||
|
||||
|
||||
@ -49,7 +49,7 @@ public:
|
||||
std::array<uint32_t, 15> mack{};
|
||||
std::array<uint8_t, 15> hkroot{};
|
||||
uint32_t PRN{};
|
||||
uint32_t WN_sf0{}; // TODO - this is present in UtcModelData already
|
||||
uint32_t WN_sf0{};
|
||||
uint32_t TOW_sf0{};
|
||||
std::vector<uint8_t> EphemerisClockAndStatusData{}; // TODO _2 rename and substitute this
|
||||
std::string EphemerisClockAndStatusData_2{};
|
||||
|
||||
@ -38,7 +38,7 @@ uint8_t OSNMA_DSM_Reader::get_cpks(uint8_t nma_header) const
|
||||
|
||||
bool OSNMA_DSM_Reader::get_nma_header_reserved(uint8_t nma_header) const
|
||||
{
|
||||
return ((nma_header & mask_nma_header_reserved) ? true : false);
|
||||
return (nma_header & mask_nma_header_reserved) != 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -65,7 +65,7 @@ public:
|
||||
private:
|
||||
static constexpr std::uint8_t mask_nmas{0xC0};
|
||||
static constexpr std::uint8_t mask_cid{0x30};
|
||||
static constexpr std::uint8_t mask_cpks{0x07};
|
||||
static constexpr std::uint8_t mask_cpks{0x0E};
|
||||
static constexpr std::uint8_t mask_nma_header_reserved{0x01};
|
||||
static constexpr std::uint8_t mask_dsm_id{0xF0};
|
||||
static constexpr std::uint8_t mask_dsm_block_id{0x0F};
|
||||
|
||||
@ -16,15 +16,15 @@
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "gnss_crypto.h"
|
||||
#include "osnma_helper.h"
|
||||
#include "osnma_msg_receiver.h"
|
||||
#include <gtest/gtest.h>
|
||||
#include <bitset>
|
||||
#include <chrono>
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include "Galileo_OSNMA.h"
|
||||
#include "gnss_crypto.h"
|
||||
#include "osnma_helper.h"
|
||||
#include "osnma_msg_receiver.h"
|
||||
|
||||
#if USE_GLOG_AND_GFLAGS
|
||||
#include <glog/logging.h> // for LOG
|
||||
@ -43,7 +43,7 @@ protected:
|
||||
uint32_t TOW{};
|
||||
uint32_t WN{};
|
||||
std::tm GST_START_EPOCH = {0, 0, 0, 22, 8 - 1, 1999 - 1900, 0, 0, 0, 0, 0}; // months start with 0 and years since 1900 in std::tm
|
||||
const uint32_t LEAP_SECONDS = 0; // 13 + 5;
|
||||
const uint32_t LEAP_SECONDS = 0; // tried with 13 + 5, which is the official count, but won't parse correctly
|
||||
void set_time(std::tm& input);
|
||||
|
||||
void SetUp() override
|
||||
|
||||
@ -61,19 +61,19 @@ protected:
|
||||
|
||||
void SetUp() override
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
// TODO - split this into configuration 1 and configuration 2
|
||||
TEST_F(OsnmaTestVectors, OsnmaTestVectorsSimulation)
|
||||
{
|
||||
// Arrange
|
||||
// std::tm input_time = {0, 0, 5, 16, 8 - 1, 2023 - 1900, 0, 0, 0, 0, 0}; // conf. 1
|
||||
std::tm input_time = {0, 0, 0, 27, 7 - 1, 2023 - 1900, 0, 0, 0, 0, 0}; // conf. 2
|
||||
set_time(input_time);
|
||||
std::string crtFilePath = std::string(BASE_OSNMA_TEST_VECTORS) + "cryptographic_material/Merkle_tree_2/PublicKey/OSNMA_PublicKey_20230720113300_newPKID_2.crt"; // conf. 2
|
||||
std::string merkleFilePath = std::string(BASE_OSNMA_TEST_VECTORS) + "cryptographic_material/Merkle_tree_2/MerkleTree/OSNMA_MerkleTree_20230720113300_newPKID_2.xml";
|
||||
osnma = osnma_msg_receiver_make(crtFilePath, merkleFilePath);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
TEST_F(OsnmaTestVectors, OsnmaTestVectorsSimulation)
|
||||
{
|
||||
// Arrange
|
||||
std::vector<TestVector> testVectors = readTestVectorsFromFile(std::string(BASE_OSNMA_TEST_VECTORS) + "osnma_test_vectors/configuration_2/27_JUL_2023_GST_00_00_01.csv"); // conf. 2
|
||||
if (testVectors.empty())
|
||||
{
|
||||
@ -96,7 +96,7 @@ TEST_F(OsnmaTestVectors, OsnmaTestVectorsSimulation)
|
||||
|
||||
// Act
|
||||
// loop over all bytes of data. Note: all TestVectors have same amount of data.
|
||||
while (end_of_hex_stream == false)
|
||||
while (!end_of_hex_stream)
|
||||
{
|
||||
// loop over all SVs, extract a subframe
|
||||
for (const TestVector& tv : testVectors)
|
||||
@ -289,7 +289,252 @@ TEST_F(OsnmaTestVectors, OsnmaTestVectorsSimulation)
|
||||
// TODO - create global vars with failed tags and compare to total tags (Tag Id for example)
|
||||
}
|
||||
|
||||
TEST_F(OsnmaTestVectors, PublicKeyRenewal)
|
||||
{
|
||||
// Arrange
|
||||
std::tm input_time_step1 = {0, 45, 2, 7, 10 - 1, 2023 - 1900, 0, 0, 0, 0, 0};
|
||||
std::tm input_time_step2 = {0, 45, 3, 7, 10 - 1, 2023 - 1900, 0, 0, 0, 0, 0};
|
||||
std::tm input_time_step3 = {0, 45, 4, 7, 10 - 1, 2023 - 1900, 0, 0, 0, 0, 0};
|
||||
std::vector<std::tm> input_times = {input_time_step1, input_time_step2, input_time_step3};
|
||||
|
||||
std::string crtFilePath = std::string(BASE_OSNMA_TEST_VECTORS) + "cryptographic_material/Merkle_tree_2/PublicKey/OSNMA_PublicKey_20231007041500_PKID_7.crt";
|
||||
std::string merkleFilePath = std::string(BASE_OSNMA_TEST_VECTORS) + "cryptographic_material/Merkle_tree_2/MerkleTree/OSNMA_MerkleTree_20231007041500_PKID_7.xml";
|
||||
osnma = osnma_msg_receiver_make(crtFilePath, merkleFilePath);
|
||||
|
||||
std::vector<TestVector> testVectors_step1 = readTestVectorsFromFile(std::string(BASE_OSNMA_TEST_VECTORS) + "osnma_test_vectors/npk_step1/07_OCT_2023_GST_02_45_01.csv");
|
||||
std::vector<TestVector> testVectors_step2 = readTestVectorsFromFile(std::string(BASE_OSNMA_TEST_VECTORS) + "osnma_test_vectors/npk_step2/07_OCT_2023_GST_03_45_01.csv");
|
||||
std::vector<TestVector> testVectors_step3 = readTestVectorsFromFile(std::string(BASE_OSNMA_TEST_VECTORS) + "osnma_test_vectors/npk_step3/07_OCT_2023_GST_04_45_01.csv");
|
||||
std::vector<std::vector<TestVector>> testVectors = {testVectors_step1, testVectors_step2, testVectors_step3};
|
||||
|
||||
if (testVectors_step1.empty() || testVectors_step2.empty() || testVectors_step3.empty())
|
||||
{
|
||||
ASSERT_TRUE(false);
|
||||
}
|
||||
|
||||
bool end_of_hex_stream;
|
||||
int offset_byte{0};
|
||||
int byte_index{0}; // index containing the last byte position of the hex stream that was retrieved. Takes advantage that all TVs have same size
|
||||
const int SIZE_PAGE_BYTES{240 / 8}; // total bytes of a page
|
||||
const int SIZE_SUBFRAME_PAGES{15}; // number of pages of a subframe
|
||||
const int DURATION_SUBFRAME{30}; // duration of a subframe, in seconds
|
||||
|
||||
const int DUMMY_PAGE{63};
|
||||
bool flag_dummy_page{false};
|
||||
// Act
|
||||
// loop over all bytes of data. Note: all TestVectors have same amount of data.
|
||||
for (int test_step = 0; test_step < 3 ; test_step++)
|
||||
{
|
||||
// set variables for each file
|
||||
end_of_hex_stream = false;
|
||||
offset_byte = 0;
|
||||
byte_index = 0;
|
||||
set_time(input_times[test_step]);
|
||||
std::cout << "OsnmaTestVectorsSimulation:"
|
||||
<< " d_GST_SIS= " << d_GST_SIS
|
||||
<< ", TOW=" << TOW
|
||||
<< ", WN=" << WN << std::endl;
|
||||
|
||||
if (test_step == 1 ){
|
||||
// step 2: this simulates the osnma connecting to the GSC server and downloading the Merkle tree of the next public key
|
||||
osnma->d_crypto->read_merkle_xml(
|
||||
std::string(BASE_OSNMA_TEST_VECTORS) + "cryptographic_material/Merkle_tree_2/MerkleTree/OSNMA_MerkleTree_20231007081500_PKID_8.xml");
|
||||
}
|
||||
|
||||
while (!end_of_hex_stream)
|
||||
{
|
||||
// loop over all SVs, extract a subframe
|
||||
for (const TestVector& tv : testVectors[test_step])
|
||||
{ // loop over all SVs, extract a subframe
|
||||
std::cout << "OsnmaTestVectorsSimulation: SVID (PRN_a) " << tv.svId << std::endl;
|
||||
auto osnmaMsg_sptr = std::make_shared<OSNMA_msg>();
|
||||
std::array<uint8_t, 15> hkroot{};
|
||||
std::array<uint32_t, 15> 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<uint8_t, std::bitset<128>> words_for_OSNMA; // structure containing <WORD_NUMBER> and <EXTRACTED_BITS>
|
||||
|
||||
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
|
||||
std::vector<uint8_t> page_bytes = extract_page_bytes(tv, byte_index, SIZE_PAGE_BYTES);
|
||||
if (page_bytes.empty())
|
||||
{
|
||||
std::cout << "OsnmaTestVectorsSimulation: end of TestVectors \n"
|
||||
<< "byte_index=" << byte_index << " expected= " << 432000 / 8 << std::endl;
|
||||
end_of_hex_stream = true;
|
||||
break;
|
||||
}
|
||||
// convert them to bitset representation using bytes_to_string
|
||||
std::string page_bits = bytes_to_str(page_bytes);
|
||||
// Extract the 40 OSNMA bits starting from the 18th bit
|
||||
std::string even_page = page_bits.substr(0, page_bits.size() / 2);
|
||||
std::string odd_page = page_bits.substr(page_bits.size() / 2);
|
||||
if (even_page.size() < 120 || odd_page.size() < 120)
|
||||
{
|
||||
std::cout << "OsnmaTestVectorsSimulation: error parsing pages" << std::endl;
|
||||
}
|
||||
bool even_odd_OK = even_page[0] == '0' && odd_page[0] == '1';
|
||||
bool page_type_OK = even_page[1] == '0' && odd_page[1] == '0';
|
||||
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 = static_cast<uint8_t>((shifted_data_k >>= 106).to_ulong()); // word type is the first 6 bits of the word
|
||||
// std::cout << "OsnmaTestVectorsSimulation: received Word " << static_cast<int>(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_for_OSNMA[word_type] = data_combined;
|
||||
}
|
||||
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));
|
||||
std::bitset<32> mackBits(osnmaBits.to_string().substr(8, 32));
|
||||
hkroot[idx] = static_cast<uint8_t>(hkrootBits.to_ulong());
|
||||
mack[idx] = static_cast<uint32_t>(mackBits.to_ulong());
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// Fill osnma object
|
||||
osnmaMsg_sptr->hkroot = hkroot;
|
||||
osnmaMsg_sptr->mack = mack;
|
||||
|
||||
osnmaMsg_sptr->TOW_sf0 = d_GST_SIS & 0x000FFFFF;
|
||||
osnmaMsg_sptr->WN_sf0 = (d_GST_SIS & 0xFFF00000) >> 20;
|
||||
osnmaMsg_sptr->PRN = tv.svId; // PRNa
|
||||
|
||||
// TODO - refactor this logic, currently it is split
|
||||
// check if words_for_OSNMA 1--> 5 words_for_OSNMA are received => fill EphClockStatus data vector
|
||||
bool ephClockStatusWordsReceived = true;
|
||||
for (int i = 1; i <= 5; ++i)
|
||||
{
|
||||
if (words_for_OSNMA.find(i) == words_for_OSNMA.end())
|
||||
{
|
||||
ephClockStatusWordsReceived = false;
|
||||
std::cerr << "OsnmaTestVectorsSimulation: error parsing words_for_OSNMA 1->5. "
|
||||
"Word "
|
||||
<< i << " should be received for each subframe but was not." << std::endl;
|
||||
}
|
||||
}
|
||||
// extract bits as needed by osnma block
|
||||
if (ephClockStatusWordsReceived)
|
||||
{
|
||||
// Define the starting position and length of bits to extract for each word
|
||||
std::map<uint8_t, std::pair<uint8_t, uint8_t>> extractionParams = {
|
||||
{1, {6, 120}},
|
||||
{2, {6, 120}},
|
||||
{3, {6, 122}},
|
||||
{4, {6, 120}},
|
||||
{5, {6, 67}},
|
||||
};
|
||||
|
||||
// Fill NavData bits -- Iterate over the extraction parameters
|
||||
std::string nav_data_ADKD_0_12 = "";
|
||||
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 and fill osnma block
|
||||
nav_data_ADKD_0_12 += words_for_OSNMA[wordKey].to_string().substr(start, length);
|
||||
}
|
||||
// send to osnma block
|
||||
bool check_size_is_ok = nav_data_ADKD_0_12.size() == 549;
|
||||
if (check_size_is_ok)
|
||||
{
|
||||
std::cout << "Galileo OSNMA: sending ADKD=0/12 navData, PRN_d (" << tv.svId << ") "
|
||||
<< "TOW_sf=" << osnmaMsg_sptr->TOW_sf0 << std::endl;
|
||||
const auto tmp_obj_osnma = std::make_shared<std::tuple<uint32_t, std::string, uint32_t>>( // < PRNd , navDataBits, TOW_Sosf>
|
||||
tv.svId,
|
||||
nav_data_ADKD_0_12,
|
||||
osnmaMsg_sptr->TOW_sf0);
|
||||
LOG(INFO) << "|---> Galileo OSNMA :: Telemetry Decoder NavData (PRN_d=" << static_cast<int>(tv.svId) << ", TOW=" << static_cast<int>(osnmaMsg_sptr->TOW_sf0) << "): 0b" << nav_data_ADKD_0_12;
|
||||
osnma->msg_handler_osnma(pmt::make_any(tmp_obj_osnma));
|
||||
}
|
||||
}
|
||||
|
||||
// check w6 && w10 is received => fill TimingData data vector
|
||||
bool timingWordsReceived = words_for_OSNMA.find(6) != words_for_OSNMA.end() &&
|
||||
words_for_OSNMA.find(10) != words_for_OSNMA.end();
|
||||
// extract bits as needed by osnma block
|
||||
if (timingWordsReceived)
|
||||
{
|
||||
// Define the starting position and length of bits to extract for each word
|
||||
std::map<uint8_t, std::pair<uint8_t, uint8_t>> extractionParams = {
|
||||
{6, {6, 99}},
|
||||
{10, {86, 42}}};
|
||||
|
||||
std::string nav_data_ADKD_4 = "";
|
||||
// Fill NavData bits -- 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 and fill osnma block
|
||||
nav_data_ADKD_4 += words_for_OSNMA[wordKey].to_string().substr(start, length);
|
||||
}
|
||||
// send to osnma block
|
||||
bool check_size_is_ok = nav_data_ADKD_4.size() == 141;
|
||||
if (check_size_is_ok)
|
||||
{
|
||||
std::cout << "Galileo OSNMA: sending ADKD=04 navData, PRN_d (" << tv.svId << ") "
|
||||
<< "TOW_sf=" << osnmaMsg_sptr->TOW_sf0 << std::endl;
|
||||
const auto tmp_obj_osnma = std::make_shared<std::tuple<uint32_t, std::string, uint32_t>>( // < PRNd , navDataBits, TOW_Sosf>
|
||||
tv.svId,
|
||||
nav_data_ADKD_4,
|
||||
osnmaMsg_sptr->TOW_sf0);
|
||||
LOG(INFO) << "|---> Galileo OSNMA :: Telemetry Decoder NavData (PRN_d=" << static_cast<int>(tv.svId) << ", TOW=" << static_cast<int>(osnmaMsg_sptr->TOW_sf0) << "): 0b" << nav_data_ADKD_4;
|
||||
osnma->msg_handler_osnma(pmt::make_any(tmp_obj_osnma));
|
||||
}
|
||||
}
|
||||
|
||||
// Call the handler, as if it came from telemetry decoder block
|
||||
auto temp_obj = pmt::make_any(osnmaMsg_sptr);
|
||||
|
||||
osnma->msg_handler_osnma(temp_obj); // osnma entry point
|
||||
}
|
||||
|
||||
if (!end_of_hex_stream)
|
||||
{
|
||||
offset_byte = byte_index; // update offset for the next subframe
|
||||
d_GST_SIS += DURATION_SUBFRAME;
|
||||
TOW = d_GST_SIS & 0x000FFFFF;
|
||||
WN = (d_GST_SIS & 0xFFF00000) >> 20;
|
||||
std::cout << "OsnmaTestVectorsSimulation:"
|
||||
<< " d_GST_SIS= " << d_GST_SIS
|
||||
<< ", TOW=" << TOW
|
||||
<< ", WN=" << WN << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Assert
|
||||
|
||||
// TODO - create global vars with failed tags and compare to total tags (Tag Id for example)
|
||||
|
||||
// Assert
|
||||
|
||||
}
|
||||
// Auxiliary functions for the OsnmaTestVectorsSimulation test fixture.
|
||||
// Essentially, they perform same work as the telemetry decoder block, but adapted to the osnma-test-vector files.
|
||||
std::vector<TestVector> OsnmaTestVectors::readTestVectorsFromFile(const std::string& filename)
|
||||
@ -332,7 +577,6 @@ std::vector<TestVector> OsnmaTestVectors::readTestVectorsFromFile(const std::str
|
||||
return testVectors;
|
||||
}
|
||||
|
||||
|
||||
std::vector<uint8_t> OsnmaTestVectors::parseNavBits(const std::string& hexadecimal)
|
||||
{
|
||||
std::vector<uint8_t> bytes;
|
||||
@ -346,7 +590,6 @@ std::vector<uint8_t> OsnmaTestVectors::parseNavBits(const std::string& hexadecim
|
||||
return bytes;
|
||||
}
|
||||
|
||||
|
||||
std::string OsnmaTestVectors::bytes_to_str(const std::vector<uint8_t>& bytes)
|
||||
{
|
||||
std::string bit_string;
|
||||
@ -359,7 +602,6 @@ std::string OsnmaTestVectors::bytes_to_str(const std::vector<uint8_t>& bytes)
|
||||
return bit_string;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Extracts a range of bytes from a TestVector's navBits vector.
|
||||
*
|
||||
@ -389,7 +631,6 @@ std::vector<uint8_t> OsnmaTestVectors::extract_page_bytes(const TestVector& tv,
|
||||
return extracted_bytes;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Sets the time based on the given input.
|
||||
*
|
||||
@ -398,6 +639,7 @@ std::vector<uint8_t> OsnmaTestVectors::extract_page_bytes(const TestVector& tv,
|
||||
* calculated values in the WN and TOW member variables. Finally, it
|
||||
* combines the WN and TOW into a single 32-bit value and stores it in
|
||||
* the d_GST_SIS member variable.
|
||||
* \post WN, TOW and GST_SIS are set up based on the input time.
|
||||
*
|
||||
* @param input The input time as a tm struct.
|
||||
*/
|
||||
|
||||
Loading…
Reference in New Issue
Block a user