mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 04:00:34 +00:00
Add work on osnma receiver
This commit is contained in:
Carles Fernandez
parent
f7c14e3041
commit
78847e33a3
@ -18,6 +18,7 @@
|
||||
|
||||
|
||||
#include "osnma_msg_receiver.h"
|
||||
#include "Galileo_OSNMA.h"
|
||||
#include "gnss_sdr_make_unique.h" // for std::make_unique in C++11
|
||||
#include <glog/logging.h> // for DLOG
|
||||
#include <gnuradio/io_signature.h> // for gr::io_signature::make
|
||||
@ -28,8 +29,7 @@
|
||||
#include <iterator>
|
||||
#include <numeric>
|
||||
#include <typeinfo> // for typeid
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
|
||||
|
||||
#if HAS_GENERIC_LAMBDA
|
||||
#else
|
||||
@ -44,96 +44,6 @@ namespace wht = boost;
|
||||
namespace wht = std;
|
||||
#endif
|
||||
|
||||
|
||||
// OSNMA SIS ICD v1.0 Table 3
|
||||
const std::unordered_map<uint8_t, std::pair<uint16_t, uint16_t>> OSNMA_TABLE_3 = {
|
||||
{0, {0, 0}},
|
||||
{1, {0, 0}},
|
||||
{2, {0, 0}},
|
||||
{3, {0, 0}},
|
||||
{4, {0, 0}},
|
||||
{5, {0, 0}},
|
||||
{6, {0, 0}},
|
||||
{7, {13, 1352}},
|
||||
{8, {14, 1456}},
|
||||
{9, {15, 1560}},
|
||||
{10, {16, 1664}},
|
||||
{11, {0, 0}},
|
||||
{12, {0, 0}},
|
||||
{13, {0, 0}},
|
||||
{14, {0, 0}},
|
||||
{15, {0, 0}}}; // key: nb_dp, value: {num_blocks, l_dp_bits}
|
||||
|
||||
const std::unordered_map<uint8_t, std::string> OSNMA_TABLE_5 = {
|
||||
{0, std::string("Reserved")},
|
||||
{1, std::string("ECDSA P-256")},
|
||||
{2, std::string("Reserved")},
|
||||
{3, std::string("ECDSA P-521")},
|
||||
{4, std::string("OAM")},
|
||||
{5, std::string("Reserved")},
|
||||
{6, std::string("Reserved")},
|
||||
{7, std::string("Reserved")},
|
||||
{8, std::string("Reserved")},
|
||||
{9, std::string("Reserved")},
|
||||
{10, std::string("Reserved")},
|
||||
{11, std::string("Reserved")},
|
||||
{12, std::string("Reserved")},
|
||||
{13, std::string("Reserved")},
|
||||
{14, std::string("Reserved")},
|
||||
{15, std::string("Reserved")}}; // key: nptk, value: message
|
||||
|
||||
const std::unordered_map<std::string, uint16_t> OSNMA_TABLE_6 = {
|
||||
{std::string("ECDSA P-256"), 264},
|
||||
{std::string("ECDSA P-521"), 536}};
|
||||
|
||||
// OSNMA SIS ICD v1.0 Table 7
|
||||
const std::unordered_map<uint8_t, std::pair<uint16_t, uint16_t>> OSNMA_TABLE_7 = {
|
||||
{0, {0, 0}},
|
||||
{1, {7, 728}},
|
||||
{2, {8, 832}},
|
||||
{3, {9, 936}},
|
||||
{4, {10, 1040}},
|
||||
{5, {11, 1144}},
|
||||
{6, {12, 1248}},
|
||||
{7, {13, 1352}},
|
||||
{8, {14, 1456}},
|
||||
{9, {0, 0}},
|
||||
{10, {0, 0}},
|
||||
{11, {0, 0}},
|
||||
{12, {0, 0}},
|
||||
{13, {0, 0}},
|
||||
{14, {0, 0}},
|
||||
{15, {0, 0}}}; // key: nb_dk, value: {num_blocks, l_dk_bits}
|
||||
|
||||
const std::unordered_map<uint8_t, std::string> OSNMA_TABLE_8 = {
|
||||
{0, std::string("SHA-256")},
|
||||
{1, std::string("Reserved")},
|
||||
{2, std::string("SHA3-256")},
|
||||
{3, std::string("Reserved")}}; // key: hs, value: hash_function
|
||||
|
||||
const std::unordered_map<uint8_t, uint16_t> OSNMA_TABLE_10 = {
|
||||
{0, 96},
|
||||
{1, 104},
|
||||
{2, 112},
|
||||
{3, 120},
|
||||
{4, 128},
|
||||
{5, 160},
|
||||
{6, 192},
|
||||
{7, 224},
|
||||
{8, 256},
|
||||
{9, 0},
|
||||
{10, 0},
|
||||
{11, 0},
|
||||
{12, 0},
|
||||
{13, 0},
|
||||
{15, 0},
|
||||
{15, 0}}; // key: ks, value: lk_bits
|
||||
|
||||
const std::unordered_map<std::string, std::pair<uint16_t, uint16_t>> OSNMA_TABLE_15 = {
|
||||
{std::string("ECDSA P-256"), {512, 256}},
|
||||
{std::string("ECDSA P-521"), {1059, 521}}}; // key: ECDSA Curve and hash function, value: {l_ds_bits, key_lenght_bits}
|
||||
|
||||
|
||||
osnma_msg_receiver_sptr osnma_msg_receiver_make()
|
||||
{
|
||||
return osnma_msg_receiver_sptr(new osnma_msg_receiver());
|
||||
@ -190,6 +100,7 @@ void osnma_msg_receiver::msg_handler_osnma(const pmt::pmt_t& msg)
|
||||
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));
|
||||
d_new_data = false;
|
||||
d_osnma_data = OSNMA_data();
|
||||
DLOG(INFO) << "NMA info sent to the PVT block through the OSNMA_to_PVT async message port";
|
||||
}
|
||||
}
|
||||
@ -197,36 +108,42 @@ 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)
|
||||
{
|
||||
const auto hkroot_msg = osnma_msg->hkroot;
|
||||
read_nma_header(hkroot_msg[0]);
|
||||
read_dsm_header(hkroot_msg[1]);
|
||||
read_nma_header(osnma_msg->hkroot[0]);
|
||||
read_dsm_header(osnma_msg->hkroot[1]);
|
||||
read_dsm_block(osnma_msg);
|
||||
}
|
||||
|
||||
|
||||
void osnma_msg_receiver::read_nma_header(uint8_t nma_header)
|
||||
{
|
||||
d_osnma_data.d_nma_header.nmas = (nma_header & 0b11000000) >> 6;
|
||||
d_osnma_data.d_nma_header.cid = (nma_header & 0b00110000) >> 4;
|
||||
d_osnma_data.d_nma_header.cpks = (nma_header & 0b00001110) >> 1;
|
||||
d_osnma_data.d_nma_header.reserved = ((nma_header & 0b00000001) ? true : false);
|
||||
d_osnma_data.d_nma_header.nmas = get_nmas(nma_header);
|
||||
d_osnma_data.d_nma_header.cid = get_cid(nma_header);
|
||||
d_osnma_data.d_nma_header.cpks = get_cpks(nma_header);
|
||||
d_osnma_data.d_nma_header.reserved = get_nma_header_reserved(nma_header);
|
||||
|
||||
// debug
|
||||
const auto it = OSNMA_TABLE_2.find(d_osnma_data.d_nma_header.cpks);
|
||||
if (it != OSNMA_TABLE_2.cend())
|
||||
{
|
||||
LOG(WARNING) << "Chain and Public Key Status: " << it->second;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void osnma_msg_receiver::read_dsm_header(uint8_t dsm_header)
|
||||
{
|
||||
d_osnma_data.d_dsm_header.dsm_id = (dsm_header & 0b11110000) >> 4;
|
||||
d_osnma_data.d_dsm_header.dsm_block_id = dsm_header & 0b00001111; // BID
|
||||
d_osnma_data.d_dsm_header.dsm_id = get_dsm_id(dsm_header);
|
||||
d_osnma_data.d_dsm_header.dsm_block_id = get_dsm_block_id(dsm_header); // BID
|
||||
}
|
||||
|
||||
|
||||
void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_msg)
|
||||
{
|
||||
size_t i = 0;
|
||||
for (auto it = osnma_msg->hkroot.cbegin() + 2; it != osnma_msg->hkroot.cend(); ++it)
|
||||
size_t index = 0;
|
||||
for (const auto* it = osnma_msg->hkroot.cbegin() + 2; it != osnma_msg->hkroot.cend(); ++it)
|
||||
{
|
||||
d_dsm_message[d_osnma_data.d_dsm_header.dsm_id][13 * d_osnma_data.d_dsm_header.dsm_block_id + i] = *it;
|
||||
i++;
|
||||
d_dsm_message[d_osnma_data.d_dsm_header.dsm_id][13 * d_osnma_data.d_dsm_header.dsm_block_id + index] = *it;
|
||||
index++;
|
||||
}
|
||||
if (d_osnma_data.d_dsm_header.dsm_block_id == 0)
|
||||
{
|
||||
@ -256,8 +173,17 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_
|
||||
}
|
||||
// Annotate bid
|
||||
d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id][d_osnma_data.d_dsm_header.dsm_block_id] = 1;
|
||||
|
||||
std::cout << "d_dsm_id_received";
|
||||
for (auto v : d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id])
|
||||
{
|
||||
std::cout << " " << static_cast<uint32_t>(v);
|
||||
}
|
||||
std::cout << std::endl;
|
||||
LOG(WARNING) << "d_number_of blocks: " << static_cast<uint32_t>(d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]) << " BID: " << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_block_id) << " DSM ID: " << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_id);
|
||||
// is message complete? -> process_dsm_message(osnma_msg)
|
||||
if ((d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] != 0) && (d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] == std::accumulate(d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].begin(), d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].end(), 0)))
|
||||
if ((d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] != 0) &&
|
||||
(d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] == std::accumulate(d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].begin(), d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].end(), 0)))
|
||||
{
|
||||
std::vector<uint8_t> dsm_msg(std::size_t(d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]) * 13, 0);
|
||||
for (uint32_t i = 0; i < d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]; i++)
|
||||
@ -267,15 +193,16 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_
|
||||
dsm_msg[i * 13 + j] = d_dsm_message[d_osnma_data.d_dsm_header.dsm_id][i * 13 + j];
|
||||
}
|
||||
}
|
||||
process_dsm_message(osnma_msg, dsm_msg);
|
||||
process_dsm_message(dsm_msg);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void osnma_msg_receiver::process_dsm_message(const std::shared_ptr<OSNMA_msg>& osnma_msg, const std::vector<uint8_t>& dsm_msg)
|
||||
void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
if (d_osnma_data.d_dsm_header.dsm_id < 12)
|
||||
{
|
||||
LOG(WARNING) << "OSNMA: DSM-KROOT message received.";
|
||||
// DSM-KROOT message
|
||||
d_osnma_data.d_dsm_kroot_message.nb_dk = (dsm_msg[0] & 0b11110000) >> 4;
|
||||
d_osnma_data.d_dsm_kroot_message.pkid = (dsm_msg[0] & 0b00001111);
|
||||
@ -318,6 +245,7 @@ void osnma_msg_receiver::process_dsm_message(const std::shared_ptr<OSNMA_msg>& o
|
||||
}
|
||||
else if (d_osnma_data.d_dsm_header.dsm_id >= 12 && d_osnma_data.d_dsm_header.dsm_id < 16)
|
||||
{
|
||||
LOG(WARNING) << "OSNMA: DSM-PKR message received.";
|
||||
// DSM-PKR message
|
||||
d_osnma_data.d_dsm_pkr_message.nb_dp = (dsm_msg[0] & 0b11110000) >> 4;
|
||||
d_osnma_data.d_dsm_pkr_message.mid = (dsm_msg[0] & 0b00001111);
|
||||
@ -360,4 +288,9 @@ void osnma_msg_receiver::process_dsm_message(const std::shared_ptr<OSNMA_msg>& o
|
||||
d_osnma_data.d_dsm_pkr_message.p_dp[k] = dsm_msg[l_dp - l_pd + k];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Reserved message?
|
||||
d_osnma_data = OSNMA_data();
|
||||
}
|
||||
}
|
||||
|
||||
@ -59,7 +59,7 @@ private:
|
||||
void read_nma_header(uint8_t nma_header);
|
||||
void read_dsm_header(uint8_t dsm_header);
|
||||
void read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_msg);
|
||||
void process_dsm_message(const std::shared_ptr<OSNMA_msg>& osnma_msg, const std::vector<uint8_t>& dsm_msg);
|
||||
void process_dsm_message(const std::vector<uint8_t>& dsm_msg);
|
||||
|
||||
std::array<std::array<uint8_t, 256>, 16> d_dsm_message{};
|
||||
std::array<std::array<uint8_t, 16>, 16> d_dsm_id_received{};
|
||||
|
||||
@ -90,6 +90,7 @@ set(SYSTEM_PARAMETERS_HEADERS
|
||||
MATH_CONSTANTS.h
|
||||
reed_solomon.h
|
||||
galileo_has_page.h
|
||||
Galileo_OSNMA.h
|
||||
osnma_data.h
|
||||
)
|
||||
|
||||
|
||||
180
src/core/system_parameters/Galileo_OSNMA.h
Normal file
180
src/core/system_parameters/Galileo_OSNMA.h
Normal file
@ -0,0 +1,180 @@
|
||||
/*!
|
||||
* \file Galileo_OSNMA.h
|
||||
* \brief Galileo OSNMA mesage constants
|
||||
* \author Carles Fernandez, 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
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_GALILEO_OSNMA_H
|
||||
#define GNSS_SDR_GALILEO_OSNMA_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
|
||||
/** \addtogroup Core
|
||||
* \{ */
|
||||
/** \addtogroup System_Parameters
|
||||
* \{ */
|
||||
|
||||
// OSNMA User ICD for the Test Phase, Issue 1.0, 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("New Public Key (NPK)")},
|
||||
{5, std::string("Public Key Revoked (PKREV)")},
|
||||
{6, std::string("Reserved")},
|
||||
{7, std::string("Reserved")}}; // key: cpks, value: Chain and Public Key Status
|
||||
|
||||
// OSNMA User ICD for the Test Phase, Issue 1.0, Table 3
|
||||
const std::unordered_map<uint8_t, std::pair<uint16_t, uint16_t>> OSNMA_TABLE_3 = {
|
||||
{0, {0, 0}},
|
||||
{1, {0, 0}},
|
||||
{2, {0, 0}},
|
||||
{3, {0, 0}},
|
||||
{4, {0, 0}},
|
||||
{5, {0, 0}},
|
||||
{6, {0, 0}},
|
||||
{7, {13, 1352}},
|
||||
{8, {14, 1456}},
|
||||
{9, {15, 1560}},
|
||||
{10, {16, 1664}},
|
||||
{11, {0, 0}},
|
||||
{12, {0, 0}},
|
||||
{13, {0, 0}},
|
||||
{14, {0, 0}},
|
||||
{15, {0, 0}}}; // key: nb_dp, value: {num_blocks, l_dp_bits}
|
||||
|
||||
const std::unordered_map<uint8_t, std::string> OSNMA_TABLE_5 = {
|
||||
{0, std::string("Reserved")},
|
||||
{1, std::string("ECDSA P-256")},
|
||||
{2, std::string("Reserved")},
|
||||
{3, std::string("ECDSA P-521")},
|
||||
{4, std::string("OAM")},
|
||||
{5, std::string("Reserved")},
|
||||
{6, std::string("Reserved")},
|
||||
{7, std::string("Reserved")},
|
||||
{8, std::string("Reserved")},
|
||||
{9, std::string("Reserved")},
|
||||
{10, std::string("Reserved")},
|
||||
{11, std::string("Reserved")},
|
||||
{12, std::string("Reserved")},
|
||||
{13, std::string("Reserved")},
|
||||
{14, std::string("Reserved")},
|
||||
{15, std::string("Reserved")}}; // key: nptk, value: message
|
||||
|
||||
const std::unordered_map<std::string, uint16_t> OSNMA_TABLE_6 = {
|
||||
{std::string("ECDSA P-256"), 264},
|
||||
{std::string("ECDSA P-521"), 536}};
|
||||
|
||||
// OSNMA User ICD for the Test Phase, Issue 1.0, Table 7
|
||||
const std::unordered_map<uint8_t, std::pair<uint16_t, uint16_t>> OSNMA_TABLE_7 = {
|
||||
{0, {0, 0}},
|
||||
{1, {7, 728}},
|
||||
{2, {8, 832}},
|
||||
{3, {9, 936}},
|
||||
{4, {10, 1040}},
|
||||
{5, {11, 1144}},
|
||||
{6, {12, 1248}},
|
||||
{7, {13, 1352}},
|
||||
{8, {14, 1456}},
|
||||
{9, {0, 0}},
|
||||
{10, {0, 0}},
|
||||
{11, {0, 0}},
|
||||
{12, {0, 0}},
|
||||
{13, {0, 0}},
|
||||
{14, {0, 0}},
|
||||
{15, {0, 0}}}; // key: nb_dk, value: {num_blocks, l_dk_bits}
|
||||
|
||||
const std::unordered_map<uint8_t, std::string> OSNMA_TABLE_8 = {
|
||||
{0, std::string("SHA-256")},
|
||||
{1, std::string("Reserved")},
|
||||
{2, std::string("SHA3-256")},
|
||||
{3, std::string("Reserved")}}; // key: hs, value: hash_function
|
||||
|
||||
const std::unordered_map<uint8_t, uint16_t> OSNMA_TABLE_10 = {
|
||||
{0, 96},
|
||||
{1, 104},
|
||||
{2, 112},
|
||||
{3, 120},
|
||||
{4, 128},
|
||||
{5, 160},
|
||||
{6, 192},
|
||||
{7, 224},
|
||||
{8, 256},
|
||||
{9, 0},
|
||||
{10, 0},
|
||||
{11, 0},
|
||||
{12, 0},
|
||||
{13, 0},
|
||||
{15, 0},
|
||||
{15, 0}}; // key: ks, value: lk_bits
|
||||
|
||||
const std::unordered_map<std::string, std::pair<uint16_t, uint16_t>> OSNMA_TABLE_15 = {
|
||||
{std::string("ECDSA P-256"), {512, 256}},
|
||||
{std::string("ECDSA P-521"), {1059, 521}}}; // key: ECDSA Curve and hash function, value: {l_ds_bits, key_lenght_bits}
|
||||
|
||||
#if __cplusplus == 201103L
|
||||
constexpr std::uint8_t mask_nmas{0xC0};
|
||||
constexpr std::uint8_t mask_cid{0x30};
|
||||
constexpr std::uint8_t mask_cpks{0x07};
|
||||
constexpr std::uint8_t mask_nma_header_reserved{0x01};
|
||||
constexpr std::uint8_t mask_dsm_id{0x01};
|
||||
constexpr std::uint8_t mask_dsm_block_id{0x0F};
|
||||
#else
|
||||
constexpr std::uint8_t mask_nmas{0b1100'0000};
|
||||
constexpr std::uint8_t mask_cid{0b0011'0000};
|
||||
constexpr std::uint8_t mask_cpks{0b0000'1110};
|
||||
constexpr std::uint8_t mask_nma_header_reserved{0b0000'0001};
|
||||
constexpr std::uint8_t mask_dsm_id{0b0000'0001};
|
||||
constexpr std::uint8_t mask_dsm_block_id{0b0000'1111};
|
||||
|
||||
#endif
|
||||
|
||||
inline uint8_t get_nmas(uint8_t nma_header)
|
||||
{
|
||||
return (nma_header & mask_nmas) >> 6;
|
||||
}
|
||||
|
||||
inline uint8_t get_cid(uint8_t nma_header)
|
||||
{
|
||||
return (nma_header & mask_cid) >> 4;
|
||||
}
|
||||
|
||||
inline uint8_t get_cpks(uint8_t nma_header)
|
||||
{
|
||||
return (nma_header & mask_cpks) >> 1;
|
||||
}
|
||||
|
||||
inline bool get_nma_header_reserved(uint8_t nma_header)
|
||||
{
|
||||
return ((nma_header & mask_nma_header_reserved) ? true : false);
|
||||
}
|
||||
|
||||
inline uint8_t get_dsm_id(uint8_t dsm_header)
|
||||
{
|
||||
return (dsm_header & mask_dsm_id) >> 4;
|
||||
}
|
||||
|
||||
inline uint8_t get_dsm_block_id(uint8_t dsm_header)
|
||||
{
|
||||
return dsm_header & mask_dsm_block_id;
|
||||
}
|
||||
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_GALILEO_OSNMA_H
|
||||
@ -1392,7 +1392,9 @@ int32_t Galileo_Inav_Message::page_jk_decoder(const char* data_jk)
|
||||
break;
|
||||
}
|
||||
|
||||
if (page_position_in_inav_subframe > 14 && page_position_in_inav_subframe != 255)
|
||||
if (page_position_in_inav_subframe != 0 &&
|
||||
page_position_in_inav_subframe > 14 &&
|
||||
page_position_in_inav_subframe != 255)
|
||||
{
|
||||
// something weird happened, reset
|
||||
page_position_in_inav_subframe = 255;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user