mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 20:20:35 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into vtl_experimental

Browse Source
This commit is contained in:
Javier Arribas 2020-11-18 10:06:23 +01:00
commit 8ae3d942d3
4 changed files with 377 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,6 +23,7 @@
#ifndef GNSS_SDR_GALILEO_CNAV_H
#define GNSS_SDR_GALILEO_CNAV_H
#include <cstddef>
#include <cstdint>
#include <utility>
@ -32,6 +33,29 @@
* \{ */
// Galileo HAS message field lengths
constexpr size_t HAS_MSG_NSYS_LENGTH = 4;
constexpr size_t HAS_MSG_ID_MASK_LENGTH = 4;
constexpr size_t HAS_MSG_SATELLITE_MASK_LENGTH = 40;
constexpr size_t HAS_MSG_SIGNAL_MASK_LENGTH = 16;
constexpr size_t HAS_MSG_NAV_MESSAGE_LENGTH = 3;
constexpr size_t HAS_MSG_VALIDITY_INDEX_LENGTH = 4;
constexpr size_t HAS_MSG_IOD_GPS_LENGTH = 8;
constexpr size_t HAS_MSG_IOD_GAL_LENGTH = 10;
constexpr size_t HAS_MSG_DELTA_RADIAL_LENGTH = 14;
constexpr size_t HAS_MSG_DELTA_ALONG_TRACK_LENGTH = 12;
constexpr size_t HAS_MSG_DELTA_CROSS_TRACK_LENGTH = 12;
constexpr size_t HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH = 2;
constexpr size_t HAS_MSG_DELTA_CLOCK_C0_LENGTH = 14;
constexpr size_t HAS_MSG_NSYSPRIME_LENGTH = 4;
constexpr size_t HAS_MSG_ID_CLOCK_SUBSET_LENGTH = 4;
constexpr size_t HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH = 2;
constexpr size_t HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH = 14;
constexpr size_t HAS_MSG_CODE_BIAS_LENGTH = 11;
constexpr size_t HAS_MSG_PHASE_BIAS_LENGTH = 11;
constexpr size_t HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH = 2;
constexpr size_t HAS_MSG_URA_LENGTH = 2;
// Galileo CNAV message structure
constexpr int32_t GALILEO_CNAV_SYMBOLS_PER_PAGE = 1000; //!< Total numer of symbols per HAS page including the sync pattern
constexpr int32_t GALILEO_CNAV_PREAMBLE_PERIOD_SYMBOLS = 1000;
@ -50,6 +74,12 @@ constexpr int32_t GALILEO_CNAV_PREAMBLE_LENGTH_BITS = 16;
constexpr int32_t GALILEO_CNAV_MAX_NUMBER_ENCODED_BLOCKS = 255;
constexpr int32_t GALILEO_CNAV_MT1_HEADER_BITS = 32;
constexpr int32_t HAS_MSG_MAX_SATS = 40;
constexpr int32_t HAS_MSG_MAX_SIGNALS = 16;
constexpr uint8_t HAS_MSG_GPS_SYSTEM = 0; // Table 8 ICD
constexpr uint8_t HAS_MSG_GALILEO_SYSTEM = 2; // Table 8 ICD
constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000";
const std::pair<int32_t, int32_t> GALILEO_HAS_STATUS({1, 2});

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

@ -23,6 +23,7 @@
#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
using CRC_Galileo_CNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>;
@ -195,35 +196,294 @@ void Galileo_Cnav_Message::read_MT1_header(const std::string& message_string)
void Galileo_Cnav_Message::read_MT1_body(const std::string& message_string)
{
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
// size_t mask_flag_size = X;
// message = std::string(message.begin() + mask_flag_size, message.end()); // Remove mask_flag
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 (size_t i = 0; i < msg.length(); i++)
{
if (msg[i] == '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 (size_t i = 0; i < msg.length(); i++)
{
if (msg[i] == '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());
}
int 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 (int 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<int> number_sats(d_HAS_data.Nsys, 0);
std::vector<int> number_codes(d_HAS_data.Nsys, 0);
for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
{
int number_sats_this_gnss_id = 0;
int 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;
}
int Nsat_b = std::accumulate(number_sats.begin(), number_sats.end(), 0);
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 (int 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<int> number_sats(d_HAS_data.Nsys, 0);
std::vector<int> number_phases(d_HAS_data.Nsys, 0);
for (int sys = 0; sys < d_HAS_data.Nsys; sys++)
{
int number_sats_this_gnss_id = 0;
int 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;
}
int Nsat_p = std::accumulate(number_sats.begin(), number_sats.end(), 0);
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 (int 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());
}
}
}
@ -282,3 +542,83 @@ bool Galileo_Cnav_Message::read_has_message_header_parameter_bool(std::bitset<GA
}
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;
}

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

@ -80,6 +80,10 @@ private:
uint8_t read_has_message_header_parameter_uint8(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(std::bitset<GALILEO_CNAV_MT1_HEADER_BITS> bits, const std::pair<int32_t, int32_t>& parameter) const;
bool read_has_message_header_parameter_bool(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{};

2
src/core/system_parameters/galileo_has_data.h
View File

@ -61,7 +61,7 @@ public:
std::vector<uint64_t> satellite_mask;
std::vector<uint16_t> signal_mask;
std::vector<bool> cell_mask_availability_flag;
std::vector<std::vector<std::vector<uint8_t>>> cell_mask;
std::vector<std::vector<std::vector<bool>>> cell_mask;
std::vector<uint8_t> nav_message;
// Orbit corrections