mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-30 06:52:58 +00:00
Code Issues Releases Wiki Activity

Merge next branch

Browse Source
This commit is contained in:
Carles Fernandez
2021-06-02 18:32:32 +02:00
parent 0ad8dc3efe ab6a774f4a
commit bad7c7a529
8 changed files with 151 additions and 651 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@@ -590,7 +590,7 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
{
nsys += SYS_GPS;
}
if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0))
if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0) || (gal_E6_count > 0))
{
nsys += SYS_GAL;
}

36
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc
View File

@@ -26,7 +26,7 @@
#include "display.h"
#include "galileo_almanac_helper.h" // for Galileo_Almanac_Helper
#include "galileo_ephemeris.h" // for Galileo_Ephemeris
#include "galileo_has_data.h" // For Galileo HAS messages
#include "galileo_has_page.h" // For Galileo_HAS_page
#include "galileo_iono.h" // for Galileo_Iono
#include "galileo_utc_model.h" // for Galileo_Utc_Model
#include "gnss_synchro.h"
@@ -205,6 +205,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(
d_channel = 0;
d_flag_PLL_180_deg_phase_locked = false;
d_symbol_history.set_capacity(d_required_symbols + 1);
d_cnav_dummy_page = false;
// vars for Viterbi decoder
const int32_t max_states = 1U << static_cast<uint32_t>(d_mm); // 2^d_mm
@@ -531,21 +532,29 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t
}
d_cnav_nav.read_HAS_page(page_String);
// 4. If we have a new full message, read it
if (d_cnav_nav.have_new_HAS_message() == true)
// 4. If we have a new HAS page, read it
if (d_cnav_nav.have_new_HAS_page() == true)
{
if (d_cnav_nav.is_HAS_message_dummy() == true)
bool is_page_dummy = d_cnav_nav.is_HAS_page_dummy();
if (is_page_dummy == true)
{
std::cout << TEXT_MAGENTA << "New Galileo E6 HAS dummy message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n';
// Only print the message once
if (is_page_dummy != d_cnav_dummy_page)
{
d_cnav_dummy_page = is_page_dummy;
std::cout << TEXT_MAGENTA << "Receiving Galileo E6 CNAV dummy pages in channel "
<< d_channel << " from satellite " << d_satellite
<< TEXT_RESET << '\n';
}
}
else
{
//TODO: do not send HAS data over telemetry msg port
const std::shared_ptr<Galileo_HAS_data> tmp_obj = std::make_shared<Galileo_HAS_data>(d_cnav_nav.get_HAS_data());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
std::cout << TEXT_MAGENTA << "New Galileo E6 HAS message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n';
//Send HAS data to HAS processing class
const std::shared_ptr<Galileo_HAS_page> tmp_obj = std::make_shared<Galileo_HAS_page>(d_cnav_nav.get_HAS_encoded_page());
this->message_port_pub(pmt::mp("E6_HAS_from_TLM"), pmt::make_any(tmp_obj));
std::cout << TEXT_MAGENTA << "New Galileo E6 HAS page received in channel "
<< d_channel << " from satellite " << d_satellite
<< (d_cnav_nav.is_HAS_in_test_mode() == true ? " (test mode)" : "")
<< TEXT_RESET << '\n';
}
}
}
@@ -767,7 +776,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
break;
}
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
if (d_inav_nav.get_flag_CRC_test() == true or d_fnav_nav.get_flag_CRC_test() == true)
if (d_inav_nav.get_flag_CRC_test() == true or d_fnav_nav.get_flag_CRC_test() == true or d_cnav_nav.get_flag_CRC_test() == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
@@ -900,6 +909,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
case 3: // CNAV
{
// TODO
d_TOW_at_current_symbol_ms += d_PRN_code_period_ms; // this is not the TOW!
break;
}
}
@@ -936,7 +946,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
}
}
if (d_inav_nav.get_flag_TOW_set() == true or d_fnav_nav.get_flag_TOW_set() == true)
if (d_inav_nav.get_flag_TOW_set() == true or d_fnav_nav.get_flag_TOW_set() == true or d_cnav_nav.get_flag_CRC_test() == true)
{
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
// todo: Galileo to GPS time conversion should be moved to observable block.
@@ -983,7 +993,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
LOG(WARNING) << "Exception writing navigation data dump file " << e.what();
}
}
// 3. Make the output (copy the object contents to the GNURadio reserved memory)

1
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h
View File

@@ -140,6 +140,7 @@ private:
bool d_dump_mat;
bool d_remove_dat;
bool d_first_eph_sent;
bool d_cnav_dummy_page;
};

1
src/core/system_parameters/CMakeLists.txt
View File

@@ -86,6 +86,7 @@ set(SYSTEM_PARAMETERS_HEADERS
Beidou_DNAV.h
MATH_CONSTANTS.h
reed_solomon.h
galileo_has_page.h
)
list(SORT SYSTEM_PARAMETERS_HEADERS)

3
src/core/system_parameters/Galileo_CNAV.h
View File

@@ -63,7 +63,7 @@ constexpr int32_t GALILEO_CNAV_TELEMETRY_RATE_BITS_SECOND = 1000; // bps
constexpr int32_t GALILEO_CNAV_HAS_PAGE_DATA_BITS = 448;
constexpr int32_t GALILEO_CNAV_PAGE_RESERVED_BITS = 14;
constexpr int32_t GALILEO_CNAV_BITS_FOR_CRC = GALILEO_CNAV_HAS_PAGE_DATA_BITS + GALILEO_CNAV_PAGE_RESERVED_BITS; // 462
constexpr int32_t GALILEO_CNAV_BYTES_FOR_CRC = 60;
constexpr int32_t GALILEO_CNAV_BYTES_FOR_CRC = 58;
constexpr int32_t GALILEO_CNAV_CRC_LENGTH = 24;
constexpr int32_t GALILEO_CNAV_MESSAGE_BITS_PER_PAGE = 424;
constexpr int32_t GALILEO_CNAV_PAGE_HEADER_BITS = 24;
@@ -82,6 +82,7 @@ constexpr uint8_t HAS_MSG_GALILEO_SYSTEM = 2; // Table 8 ICD v1.2
constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000";
const std::pair<int32_t, int32_t> GALILEO_HAS_STATUS({1, 2});
const std::pair<int32_t, int32_t> GALILEO_HAS_RESERVED({3, 2});
const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_TYPE({5, 2});
const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_ID({7, 5});
const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_SIZE({12, 5});

626
src/core/system_parameters/galileo_cnav_message.cc
View File

@@ -19,30 +19,31 @@
#include "galileo_cnav_message.h"
#include <boost/crc.hpp> // for boost::crc_basic, boost::crc_optimal
#include <boost/dynamic_bitset.hpp> // for boost::dynamic_bitset
#include <algorithm> // for reverse, find
#include <numeric> // for accumulate
#include <glog/logging.h>
#include <algorithm> // for reverse
#include <vector>
using CRC_Galileo_CNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>;
bool Galileo_Cnav_Message::CRC_test(const std::bitset<GALILEO_CNAV_BITS_FOR_CRC>& bits, uint32_t checksum) const
{
CRC_Galileo_CNAV_type CRC_Galileo;
CRC_Galileo_CNAV_type crc_galileo_e6b;
// Galileo CNAV frame for CRC is not an integer multiple of bytes
// it needs to be filled with zeroes at the start of the frame.
// This operation is done in the transformation from bits to bytes
// using boost::dynamic_bitset.
boost::dynamic_bitset<unsigned char> frame_bits(std::string(bits.to_string()));
boost::dynamic_bitset<uint8_t> frame_bits(bits.to_string());
std::vector<unsigned char> bytes;
std::vector<uint8_t> bytes;
bytes.reserve(GALILEO_CNAV_BYTES_FOR_CRC);
boost::to_block_range(frame_bits, std::back_inserter(bytes));
std::reverse(bytes.begin(), bytes.end());
CRC_Galileo.process_bytes(bytes.data(), GALILEO_CNAV_BYTES_FOR_CRC);
crc_galileo_e6b.process_bytes(bytes.data(), GALILEO_CNAV_BYTES_FOR_CRC);
const uint32_t crc_computed = CRC_Galileo.checksum();
const uint32_t crc_computed = crc_galileo_e6b.checksum();
if (checksum == crc_computed)
{
return true;
@@ -57,7 +58,10 @@ void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string)
const std::string CRC_data = page_string.substr(GALILEO_CNAV_BITS_FOR_CRC, GALILEO_CNAV_CRC_LENGTH);
const std::bitset<GALILEO_CNAV_BITS_FOR_CRC> Word_for_CRC_bits(has_page_bits);
const std::bitset<GALILEO_CNAV_CRC_LENGTH> checksum(CRC_data);
if (CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true)
d_new_HAS_page = false;
has_page = Galileo_HAS_page();
d_flag_CRC_test = CRC_test(Word_for_CRC_bits, checksum.to_ulong());
if (d_flag_CRC_test)
{
d_flag_CRC_test = true;
// CRC correct: Read 24 bits of HAS page header
@@ -65,6 +69,8 @@ void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string)
bool use_has = false;
d_test_mode = false;
// HAS status as defined in ICD v1.2 Table 5 HAS Page Header
if (!d_page_dummy)
{
switch (d_has_page_status)
{
case 0: // HAS is in Test Mode
@@ -79,15 +85,22 @@ void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string)
default:
break;
}
if (use_has)
{
// Process the 424 bits of encoded data
process_HAS_page(page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS + GALILEO_CNAV_PAGE_HEADER_BITS, GALILEO_CNAV_MESSAGE_BITS_PER_PAGE));
}
}
else
if (use_has or d_page_dummy)
{
d_flag_CRC_test = false;
// Store the 424 bits of encoded data (CNAV page) and the page header
has_page.has_message_string = page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS + GALILEO_CNAV_PAGE_HEADER_BITS, GALILEO_CNAV_MESSAGE_BITS_PER_PAGE);
if (!d_page_dummy)
{
has_page.has_status = d_has_page_status;
has_page.reserved = d_has_reserved;
has_page.message_type = d_received_message_type;
has_page.message_id = d_received_message_id;
has_page.message_size = d_received_message_size;
has_page.message_page_id = d_received_message_page_id;
}
d_new_HAS_page = true;
}
}
}
@@ -98,6 +111,7 @@ void Galileo_Cnav_Message::read_HAS_page_header(const std::string& page_string)
if (page_string == "101011110011101111000011") // Equivalent to AF3BC3
{
d_page_dummy = true;
DLOG(INFO) << "HAS page with dummy header received.";
}
else
{
@@ -108,462 +122,19 @@ void Galileo_Cnav_Message::read_HAS_page_header(const std::string& page_string)
// ICD v1.2 Table 5: HAS page header
const std::bitset<GALILEO_CNAV_PAGE_HEADER_BITS> has_page_header(page_string);
d_has_page_status = read_has_page_header_parameter(has_page_header, GALILEO_HAS_STATUS);
d_has_reserved = read_has_page_header_parameter(has_page_header, GALILEO_HAS_RESERVED);
d_received_message_type = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_TYPE);
d_received_message_id = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_ID);
d_received_message_size = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_SIZE) + 1; // "0" means 1
d_received_message_page_id = read_has_page_header_parameter(has_page_header, GALILEO_HAS_MESSAGE_PAGE_ID);
}
}
void Galileo_Cnav_Message::process_HAS_page(const std::string& page_string)
{
if (d_current_message_id == d_received_message_id)
{
// if receiver pid was not there, store it.
if (d_received_message_page_id == 0)
{
// reserved, ignore it
}
else
{
if (std::find(d_list_pid.begin(), d_list_pid.end(), d_received_message_page_id) == d_list_pid.end())
{
if (d_received_message_type == 1) // contains satellite corrections
{
d_received_encoded_messages++;
d_list_pid.push_back(d_received_message_page_id);
// Pack encoded string into 53 octets and put it in
// the corresponding row of d_C_matrix.
for (int k = 0; k < GALILEO_CNAV_OCTETS_IN_SUBPAGE; k++)
{
std::string bits8 = page_string.substr(k * 8, 8);
std::bitset<8> bs(bits8);
d_C_matrix[d_received_message_page_id - 1][k] = static_cast<uint8_t>(bs.to_ulong());
}
}
}
}
}
else
{
// Start new message
d_current_message_id = d_received_message_id;
d_received_encoded_messages = 0;
d_new_message = false;
d_current_message_size = d_received_message_size;
// erase stored pages and data, and start storing again
d_list_pid.clear();
d_HAS_data = Galileo_HAS_data();
if (d_received_message_type == 1)
{
d_received_encoded_messages++;
d_list_pid.push_back(d_received_message_page_id);
// Pack encoded string into 53 octets and put it in
// the corresponding row of d_C_matrix.
for (int k = 0; k < GALILEO_CNAV_OCTETS_IN_SUBPAGE; k++)
{
std::string bits8 = page_string.substr(k * 8, 8);
std::bitset<8> bs(bits8);
d_C_matrix[d_received_message_page_id - 1][k] = static_cast<uint8_t>(bs.to_ulong());
}
}
}
if (d_received_encoded_messages == d_current_message_size)
{
// we have a full encoded message stored in d_C_matrix
d_received_encoded_messages = 0;
d_current_message_id = 0;
int res = decode_message_type1();
if (res == 0)
{
d_new_message = true;
}
else
{
d_new_message = false;
}
}
}
int Galileo_Cnav_Message::decode_message_type1()
{
// All rows in d_C_matrix with no data are erasure positions
std::vector<int> erasure_positions;
erasure_positions.reserve(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK - d_list_pid.size());
for (int mpid = 1; mpid <= GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK; mpid++)
{
if (std::find(d_list_pid.begin(), d_list_pid.end(), static_cast<uint8_t>(mpid)) == d_list_pid.end())
{
erasure_positions.push_back(static_cast<uint8_t>(mpid - 1));
}
else
{
d_list_pid.remove(static_cast<uint8_t>(mpid));
}
}
// Vertical decoding of d_C_matrix
for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++)
{
std::vector<uint8_t> C_column(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, 0);
for (int row = 0; row < GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK; row++)
{
C_column[row] = d_C_matrix[row][col];
}
int result = rs.decode(C_column, erasure_positions);
if (result < 0)
{
// Decoding failed
d_C_matrix = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)};
d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE)};
return -1;
}
std::vector<uint8_t> M_column(C_column.begin(), C_column.begin() + GALILEO_CNAV_INFORMATION_VECTOR_LENGTH);
for (int i = 0; i < GALILEO_CNAV_INFORMATION_VECTOR_LENGTH; i++)
{
d_M_matrix[i][col] = M_column[i];
}
}
// Form the decoded HAS message by reading rows of d_M_matrix
std::string decoded_message_type_1;
decoded_message_type_1.reserve(d_current_message_size * GALILEO_CNAV_OCTETS_IN_SUBPAGE * 8);
for (uint8_t row = 0; row < d_current_message_size; row++)
{
for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++)
{
std::bitset<8> bs(d_M_matrix[row][col]);
decoded_message_type_1 += bs.to_string();
}
}
// reset d_C_matrix and d_M_matrix for next decoding
d_C_matrix = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)};
d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)};
// Trigger HAS message content reading
read_MT1_header(decoded_message_type_1);
read_MT1_body(decoded_message_type_1);
return 0;
}
void Galileo_Cnav_Message::read_MT1_header(const std::string& message_string)
{
// ICD v1.2 Table 6: MT1 Message Header.
const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS> has_mt1_header(message_string);
d_HAS_data.header.toh = read_has_message_header_parameter_uint16(has_mt1_header, GALILEO_MT1_HEADER_TOH);
d_HAS_data.header.mask_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_MASK_ID);
d_HAS_data.header.iod_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_IOD_ID);
d_HAS_data.header.mask_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_MASK_FLAG);
d_HAS_data.header.orbit_correction_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_ORBIT_CORRECTION_FLAG);
d_HAS_data.header.clock_fullset_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CLOCK_FULLSET_FLAG);
d_HAS_data.header.clock_subset_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CLOCK_SUBSET_FLAG);
d_HAS_data.header.code_bias_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CODE_BIAS_FLAG);
d_HAS_data.header.phase_bias_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_PHASE_BIAS_FLAG);
d_HAS_data.header.ura_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_URA_FLAG);
}
void Galileo_Cnav_Message::read_MT1_body(const std::string& message_string)
{
// ICD v1.2 Table 7: MT1 Message Body.
auto message = std::string(message_string.begin() + GALILEO_CNAV_MT1_HEADER_BITS, message_string.end()); // Remove header
int Nsat = 0;
if (d_HAS_data.header.mask_flag)
{
// read mask
d_HAS_data.Nsys = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_LENGTH));
message = std::string(message.begin() + HAS_MSG_NSYS_LENGTH, message.end());
d_HAS_data.gnss_id_mask.reserve(d_HAS_data.Nsys);
d_HAS_data.cell_mask.reserve(d_HAS_data.Nsys);
d_HAS_data.cell_mask_availability_flag.reserve(d_HAS_data.Nsys);
d_HAS_data.nav_message.reserve(d_HAS_data.Nsys);
for (uint8_t i = 0; i < d_HAS_data.Nsys; i++)
{
d_HAS_data.gnss_id_mask[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_MASK_LENGTH));
message = std::string(message.begin() + HAS_MSG_ID_MASK_LENGTH, message.end());
std::string msg = message.substr(0, HAS_MSG_SATELLITE_MASK_LENGTH);
d_HAS_data.satellite_mask[i] = read_has_message_body_uint64(msg);
int ones_in_satellite_mask = 0;
for (char c : msg)
{
if (c == '1')
{
ones_in_satellite_mask++;
}
}
Nsat += ones_in_satellite_mask;
message = std::string(message.begin() + HAS_MSG_SATELLITE_MASK_LENGTH, message.end());
msg = message.substr(0, HAS_MSG_SIGNAL_MASK_LENGTH);
d_HAS_data.signal_mask[i] = read_has_message_body_uint16(msg);
int ones_in_signal_mask = 0;
for (char c : msg)
{
if (c == '1')
{
ones_in_signal_mask++;
}
}
message = std::string(message.begin() + HAS_MSG_SIGNAL_MASK_LENGTH, message.end());
if (message.substr(0, 1) == "1")
{
d_HAS_data.cell_mask_availability_flag[i] = true;
}
else
{
d_HAS_data.cell_mask_availability_flag[i] = false;
}
message = std::string(message.begin() + 1, message.end());
int size_cell = ones_in_satellite_mask * ones_in_signal_mask;
d_HAS_data.cell_mask[i].reserve(ones_in_satellite_mask);
for (int s = 0; s < ones_in_satellite_mask; s++)
{
d_HAS_data.cell_mask[i][s].reserve(ones_in_signal_mask);
for (int sig = 0; sig < ones_in_signal_mask; sig++)
{
d_HAS_data.cell_mask[i][s][sig] = (message[sig] == '1' ? true : false);
}
}
message = std::string(message.begin() + size_cell, message.end());
d_HAS_data.nav_message[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_NAV_MESSAGE_LENGTH));
message = std::string(message.begin() + HAS_MSG_NAV_MESSAGE_LENGTH, message.end());
}
}
if (d_HAS_data.header.orbit_correction_flag)
{
// read orbit corrections
d_HAS_data.validity_interval_index_orbit_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
d_HAS_data.gnss_iod.reserve(Nsat);
d_HAS_data.delta_radial.reserve(Nsat);
d_HAS_data.delta_along_track.reserve(Nsat);
d_HAS_data.delta_cross_track.reserve(Nsat);
for (int i = 0; i < Nsat; i++)
{
if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GPS_SYSTEM)
{
d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GPS_LENGTH));
message = std::string(message.begin() + HAS_MSG_IOD_GPS_LENGTH, message.end());
}
if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GALILEO_SYSTEM)
{
d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GAL_LENGTH));
message = std::string(message.begin() + HAS_MSG_IOD_GAL_LENGTH, message.end());
}
d_HAS_data.delta_radial[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_RADIAL_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_RADIAL_LENGTH, message.end());
d_HAS_data.delta_along_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_ALONG_TRACK_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_ALONG_TRACK_LENGTH, message.end());
d_HAS_data.delta_cross_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CROSS_TRACK_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_CROSS_TRACK_LENGTH, message.end());
}
}
if (d_HAS_data.header.clock_fullset_flag)
{
// read clock full-set corrections
d_HAS_data.validity_interval_index_clock_fullset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
d_HAS_data.delta_clock_c0_multiplier.reserve(d_HAS_data.Nsys);
for (uint8_t i = 0; i < d_HAS_data.Nsys; i++)
{
if (d_HAS_data.gnss_id_mask[i] != HAS_MSG_GALILEO_SYSTEM)
{
d_HAS_data.delta_clock_c0_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH, message.end());
}
}
d_HAS_data.iod_change_flag.reserve(Nsat);
d_HAS_data.delta_clock_c0.reserve(Nsat);
for (int i = 0; i < Nsat; i++)
{
d_HAS_data.iod_change_flag[i] = (message[0] == '1' ? true : false);
message = std::string(message.begin() + 1, message.end());
d_HAS_data.delta_clock_c0[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_LENGTH, message.end());
}
}
if (d_HAS_data.header.clock_subset_flag)
{
// read clock subset corrections
d_HAS_data.validity_interval_index_clock_subset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
d_HAS_data.Nsysprime = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYSPRIME_LENGTH));
message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end());
d_HAS_data.gnss_id_clock_subset.reserve(d_HAS_data.Nsysprime);
d_HAS_data.delta_clock_c0_multiplier_clock_subset.reserve(d_HAS_data.Nsysprime);
d_HAS_data.satellite_submask.reserve(d_HAS_data.Nsysprime);
d_HAS_data.iod_change_flag_clock_subset.reserve(d_HAS_data.Nsysprime);
d_HAS_data.delta_clock_c0_clock_subset.reserve(d_HAS_data.Nsysprime);
for (uint8_t i = 0; i < d_HAS_data.Nsysprime; i++)
{
d_HAS_data.gnss_id_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_CLOCK_SUBSET_LENGTH));
message = std::string(message.begin() + HAS_MSG_ID_CLOCK_SUBSET_LENGTH, message.end());
if (d_HAS_data.gnss_id_clock_subset[i] != HAS_MSG_GALILEO_SYSTEM)
{
d_HAS_data.delta_clock_c0_multiplier_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH, message.end());
}
uint64_t number_sats_this_gnss_id = 0;
for (uint8_t j = 0; j < d_HAS_data.Nsys; j++)
{
if (d_HAS_data.gnss_id_mask[j] == d_HAS_data.gnss_id_clock_subset[i])
{
uint64_t n = d_HAS_data.satellite_mask[j];
while (n)
{
number_sats_this_gnss_id += n & 1;
n >>= 1;
}
break;
}
}
d_HAS_data.satellite_submask[i].reserve(number_sats_this_gnss_id);
for (uint64_t j = 0; j < number_sats_this_gnss_id; j++)
{
d_HAS_data.satellite_submask[i][j] = read_has_message_body_uint64(message.substr(0, 1));
message = std::string(message.begin() + 1, message.end());
}
d_HAS_data.iod_change_flag_clock_subset[i] = (message[0] == '1' ? true : false);
message = std::string(message.begin() + 1, message.end());
d_HAS_data.delta_clock_c0_clock_subset[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH));
message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH, message.end());
}
}
if (d_HAS_data.header.code_bias_flag)
{
// read code bias
d_HAS_data.validity_interval_index_code_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
std::vector<uint64_t> number_sats(d_HAS_data.Nsys, 0);
std::vector<uint64_t> number_codes(d_HAS_data.Nsys, 0);
for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
{
uint64_t number_sats_this_gnss_id = 0;
uint64_t number_signals_this_gnss_id = 0;
if (d_HAS_data.cell_mask_availability_flag[sys] == true)
{
uint64_t n = d_HAS_data.satellite_mask[sys];
while (n)
{
number_sats_this_gnss_id += n & 1;
n >>= 1;
}
uint64_t m = d_HAS_data.signal_mask[sys];
while (m)
{
number_signals_this_gnss_id += m & 1;
m >>= 1;
}
}
else
{
number_sats_this_gnss_id = HAS_MSG_MAX_SATS;
number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS;
}
number_sats[sys] = number_sats_this_gnss_id;
number_codes[sys] = number_signals_this_gnss_id;
}
uint64_t Nsat_b = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL);
d_HAS_data.code_bias.reserve(Nsat_b);
int sat = 0;
for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
{
d_HAS_data.code_bias[sat].reserve(number_codes[sys]);
for (uint64_t c = 0; c < number_codes[sys]; c++)
{
d_HAS_data.code_bias[sat][c] = read_has_message_body_int16(message.substr(0, HAS_MSG_CODE_BIAS_LENGTH));
message = std::string(message.begin() + HAS_MSG_CODE_BIAS_LENGTH, message.end());
sat += 1;
}
}
}
if (d_HAS_data.header.phase_bias_flag)
{
// read phase bias
d_HAS_data.validity_interval_index_phase_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
std::vector<uint64_t> number_sats(d_HAS_data.Nsys, 0);
std::vector<uint64_t> number_phases(d_HAS_data.Nsys, 0);
for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
{
uint64_t number_sats_this_gnss_id = 0;
uint64_t number_signals_this_gnss_id = 0;
if (d_HAS_data.cell_mask_availability_flag[sys] == true)
{
uint64_t n = d_HAS_data.satellite_mask[sys];
while (n)
{
number_sats_this_gnss_id += n & 1;
n >>= 1;
}
uint64_t m = d_HAS_data.signal_mask[sys];
while (m)
{
number_signals_this_gnss_id += m & 1;
m >>= 1;
}
}
else
{
number_sats_this_gnss_id = HAS_MSG_MAX_SATS;
number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS;
}
number_sats[sys] = number_sats_this_gnss_id;
number_phases[sys] = number_signals_this_gnss_id;
}
uint64_t Nsat_p = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL);
d_HAS_data.phase_bias.reserve(Nsat_p);
d_HAS_data.phase_discontinuity_indicator.reserve(Nsat_p);
int sat = 0;
for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
{
d_HAS_data.phase_bias[sat].reserve(number_phases[sys]);
d_HAS_data.phase_discontinuity_indicator[sat].reserve(number_phases[sys]);
for (uint64_t p = 0; p < number_phases[sys]; p++)
{
d_HAS_data.phase_bias[sat][p] = read_has_message_body_int16(message.substr(0, HAS_MSG_PHASE_BIAS_LENGTH));
message = std::string(message.begin() + HAS_MSG_PHASE_BIAS_LENGTH, message.end());
d_HAS_data.phase_discontinuity_indicator[sat][p] = read_has_message_body_uint8(message.substr(0, HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH));
message = std::string(message.begin() + HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH, message.end());
sat += 1;
}
}
}
if (d_HAS_data.header.ura_flag)
{
// read URA
d_HAS_data.validity_interval_index_ura_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH));
message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end());
d_HAS_data.ura.reserve(Nsat);
for (int i = 0; i < Nsat; i++)
{
d_HAS_data.ura[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_URA_LENGTH));
message = std::string(message.begin() + HAS_MSG_URA_LENGTH, message.end());
}
DLOG(INFO) << "HAS page header received: "
<< "d_has_page_status: " << static_cast<float>(d_has_page_status) << ", "
<< "d_has_reserved: " << static_cast<float>(d_has_reserved) << ", "
<< "d_received_message_type: " << static_cast<float>(d_received_message_type) << ", "
<< "d_received_message_id: " << static_cast<float>(d_received_message_id) << ", "
<< "d_received_message_size: " << static_cast<float>(d_received_message_size) << ", "
<< "d_received_message_page_id: " << static_cast<float>(d_received_message_page_id);
}
}
@@ -581,124 +152,3 @@ uint8_t Galileo_Cnav_Message::read_has_page_header_parameter(const std::bitset<G
}
return value;
}
uint8_t Galileo_Cnav_Message::read_has_message_header_parameter_uint8(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const
{
uint8_t value = 0U;
for (int j = 0; j < parameter.second; j++)
{
value <<= 1U; // shift left
if (static_cast<int>(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first - j]) == 1)
{
value += 1; // insert the bit
}
}
return value;
}
uint16_t Galileo_Cnav_Message::read_has_message_header_parameter_uint16(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const
{
uint16_t value = 0U;
for (int j = 0; j < parameter.second; j++)
{
value <<= 1U; // shift left
if (static_cast<int>(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first - j]) == 1)
{
value += 1; // insert the bit
}
}
return value;
}
bool Galileo_Cnav_Message::read_has_message_header_parameter_bool(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const
{
bool value = false;
if (static_cast<int>(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first]) == 1)
{
value = true;
}
return value;
}
uint8_t Galileo_Cnav_Message::read_has_message_body_uint8(const std::string& bits) const
{
uint8_t value = 0U;
size_t len = bits.length();
for (size_t j = 0; j < len; j++)
{
value <<= 1U; // shift left
if (static_cast<int>(bits[len - 1 - j]) == 1)
{
value += 1; // insert the bit
}
}
return value;
}
uint16_t Galileo_Cnav_Message::read_has_message_body_uint16(const std::string& bits) const
{
uint16_t value = 0U;
size_t len = bits.length();
for (size_t j = 0; j < len; j++)
{
value <<= 1U; // shift left
if (static_cast<int>(bits[len - 1 - j]) == 1)
{
value += 1; // insert the bit
}
}
return value;
}
uint64_t Galileo_Cnav_Message::read_has_message_body_uint64(const std::string& bits) const
{
uint64_t value = 0U;
size_t len = bits.length();
for (size_t j = 0; j < len; j++)
{
value <<= 1U; // shift left
if (static_cast<int>(bits[len - 1 - j]) == 1)
{
value += 1; // insert the bit
}
}
return value;
}
int16_t Galileo_Cnav_Message::read_has_message_body_int16(const std::string& bits) const
{
int16_t value = 0;
size_t len = bits.length();
// read the MSB and perform the sign extension
if (static_cast<int>(bits[len - 1]) == 1)
{
value ^= 0xFFFF; // 16 bits variable
}
else
{
value &= 0;
}
for (size_t j = 0; j < len; j++)
{
value *= 2; // shift left the signed integer
value &= 0xFFFE; // reset the corresponding bit (for the 16 bits variable)
if (static_cast<int>(bits[len - 1 - j]) == 1)
{
value += 1; // insert the bit
}
}
return value;
}

55
src/core/system_parameters/galileo_cnav_message.h
View File

@@ -20,13 +20,11 @@
#define GNSS_SDR_GALILEO_CNAV_MESSAGE_H
#include "Galileo_CNAV.h"
#include "galileo_has_data.h"
#include "reed_solomon.h"
#include "galileo_has_page.h"
#include <bitset>
#include <cstdint>
#include <list>
#include <string>
#include <vector>
#include <utility>
/** \addtogroup Core
* \{ */
@@ -46,64 +44,49 @@ public:
void read_HAS_page(const std::string& page_string);
inline bool have_new_HAS_message()
{
return d_new_message;
}
inline bool is_HAS_in_test_mode() const
{
return d_test_mode;
}
inline bool is_HAS_message_dummy() const
inline bool is_HAS_page_dummy() const
{
return d_page_dummy;
}
inline Galileo_HAS_data get_HAS_data() const
inline bool have_new_HAS_page() const
{
return d_HAS_data;
return d_new_HAS_page;
}
inline Galileo_HAS_page get_HAS_encoded_page() const
{
return has_page;
}
inline bool get_flag_CRC_test() const
{
return d_flag_CRC_test;
}
private:
uint8_t read_has_page_header_parameter(const std::bitset<GALILEO_CNAV_PAGE_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
bool CRC_test(const std::bitset<GALILEO_CNAV_BITS_FOR_CRC>& bits, uint32_t checksum) const;
void read_HAS_page_header(const std::string& page_string);
void process_HAS_page(const std::string& page_string);
void read_MT1_header(const std::string& message_string);
void read_MT1_body(const std::string& message_string);
int decode_message_type1();
uint8_t read_has_page_header_parameter(const std::bitset<GALILEO_CNAV_PAGE_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
uint8_t read_has_message_header_parameter_uint8(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
uint16_t read_has_message_header_parameter_uint16(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
bool read_has_message_header_parameter_bool(const std::bitset<GALILEO_CNAV_MT1_HEADER_BITS>& bits, const std::pair<int32_t, int32_t>& parameter) const;
uint8_t read_has_message_body_uint8(const std::string& bits) const;
uint16_t read_has_message_body_uint16(const std::string& bits) const;
uint64_t read_has_message_body_uint64(const std::string& bits) const;
int16_t read_has_message_body_int16(const std::string& bits) const;
Galileo_HAS_data d_HAS_data{};
ReedSolomon rs = ReedSolomon();
std::vector<std::vector<uint8_t>> d_C_matrix{GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; // 255 x 53
std::vector<std::vector<uint8_t>> d_M_matrix{GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector<uint8_t>(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; // 32 x 53
std::list<uint8_t> d_list_pid;
Galileo_HAS_page has_page{};
uint8_t d_has_page_status{};
uint8_t d_current_message_id{};
uint8_t d_current_message_size{};
uint8_t d_has_reserved{};
uint8_t d_received_message_page_id{};
uint8_t d_received_message_type{};
uint8_t d_received_message_id{};
uint8_t d_received_encoded_messages{};
uint8_t d_received_message_size{};
bool d_test_mode{};
bool d_new_message{};
bool d_flag_CRC_test{};
bool d_page_dummy{};
bool d_new_HAS_page{};
};

54
src/core/system_parameters/galileo_has_page.h Normal file
View File

@@ -0,0 +1,54 @@
/*!
* \file galileo_has_page.h
* \brief Class for Galileo HAS message page storage
* \author Carles Fernandez-Prades, 2021 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-2021 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GALILEO_HAS_PAGE_H
#define GNSS_SDR_GALILEO_HAS_PAGE_H
#include <cstdint>
#include <string>
/** \addtogroup Core
* \{ */
/** \addtogroup System_Parameters
* \{ */
/*!
* \brief This class is a storage for Galileo HAS message page, as defined in
* Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2
* (April 2020).
*/
class Galileo_HAS_page
{
public:
Galileo_HAS_page() = default;
std::string has_message_string; //!< HAS message content
// HAS page header
uint8_t has_status{}; //!< HAS status
uint8_t reserved{}; //!< HAS reserved field
uint8_t message_type{}; //!< HAS message type (MT)
uint8_t message_id{}; //!< HAS message ID (MID)
uint8_t message_size{}; //!< HAS message size (MS)
uint8_t message_page_id{}; //!< HAS message page ID (PID)
};
/** \} */
/** \} */
#endif // GNSS_SDR_GALILEO_HAS_PAGE_H