Add logic for HAS message retrieval

This commit is contained in:
Carles Fernandez 2020-11-10 21:20:13 +01:00
parent d81ff696b8
commit d1b23ae3fd
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
6 changed files with 409 additions and 19 deletions

View File

@ -486,19 +486,20 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t
}
d_cnav_nav.read_HAS_page(page_String);
// 4. Check CRC
if (d_cnav_nav.get_flag_CRC_test() == true)
// 4. If we have a new full message, read it
if (d_cnav_nav.have_new_HAS_message() == true)
{
DLOG(INFO) << "Galileo E6B CRC correct in channel " << d_channel << " from satellite " << d_satellite;
// TODO: Retrieve data from message and send it somewhere
// Galileo_HAS_data has_data = d_cnav_nav.get_HAS_data();
if (d_cnav_nav.is_HAS_message_dummy())
{
std::cout << TEXT_MAGENTA << "New Galileo E6 HAS message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n';
}
else
{
std::cout << TEXT_MAGENTA << "New Galileo E6 HAS dummy message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n';
}
}
else
{
DLOG(INFO) << "Galileo E6B CRC error in channel " << d_channel << " from satellite " << d_satellite;
}
// TODO
// Get full HAS message from different pages
// Reed Solomon decoding
// Retrieve data from message
}

View File

@ -50,6 +50,7 @@ set(SYSTEM_PARAMETERS_HEADERS
galileo_iono.h
galileo_cnav_message.h
galileo_fnav_message.h
galileo_has_data.h
galileo_inav_message.h
sbas_ephemeris.h
gps_cnav_ephemeris.h

View File

@ -24,6 +24,7 @@
#define GNSS_SDR_GALILEO_CNAV_H
#include <cstdint>
#include <utility>
/** \addtogroup Core
* \{ */
@ -42,11 +43,32 @@ 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_CRC_LENGTH = 24;
constexpr int32_t GALILEO_CNAV_MESSAGE_BITS_PER_PAGE = 424;
constexpr int32_t GALILEO_CNAV_PAGE_HEADER_BITS = 24;
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 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_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});
const std::pair<int32_t, int32_t> GALILEO_HAS_MESSAGE_PAGE_ID({17, 8});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_TOH({1, 12});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_MASK_FLAG({13, 1});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_ORBIT_CORRECTION_FLAG({14, 1});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_CLOCK_FULLSET_FLAG({15, 1});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_CLOCK_SUBSET_FLAG({16, 1});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_CODE_BIAS_FLAG({17, 1});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_PHASE_BIAS_FLAG({18, 1});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_URA_FLAG({19, 1});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_MASK_ID({23, 5});
const std::pair<int32_t, int32_t> GALILEO_MT1_HEADER_IOD_ID({28, 5});
/** \} */
/** \} */

View File

@ -22,7 +22,7 @@
#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
#include <algorithm> // for reverse, find
using CRC_Galileo_CNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>;
@ -56,16 +56,229 @@ bool Galileo_Cnav_Message::CRC_test(std::bitset<GALILEO_CNAV_BITS_FOR_CRC> bits,
void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string)
{
const std::string has_page_bits = page_string.substr(0, GALILEO_CNAV_BITS_FOR_CRC);
const std::string CRC_data = page_string.substr(GALILEO_CNAV_BITS_FOR_CRC, 24);
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<24> checksum(CRC_data);
const std::bitset<GALILEO_CNAV_CRC_LENGTH> checksum(CRC_data);
if (CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true)
{
flag_CRC_test = true;
d_flag_CRC_test = true;
// CRC correct: Read HAS page header
read_HAS_page_header(page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS, GALILEO_CNAV_PAGE_HEADER_BITS));
bool use_has = false;
d_test_mode = false;
switch (d_has_page_status)
{
case 0: // HAS is in Test Mode
use_has = true;
d_test_mode = true;
break;
case 1: // HAS is in Operational Mode
use_has = true;
break;
default:
break;
}
if (use_has)
{
process_HAS_page(page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS + GALILEO_CNAV_PAGE_HEADER_BITS, GALILEO_CNAV_MESSAGE_BITS_PER_PAGE));
}
}
else
{
flag_CRC_test = false;
d_flag_CRC_test = false;
}
}
void Galileo_Cnav_Message::read_HAS_page_header(const std::string& page_string)
{
// check if dummy
if (page_string == "101011110011101111000011") // Equivalent to AF3BC3
{
d_page_dummy = true;
}
else
{
d_page_dummy = false;
}
if (!d_page_dummy)
{
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_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);
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 (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);
// Store encoded page
d_encoded_message_type_1 += std::string(page_string);
}
}
}
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 start storing again
d_encoded_message_type_1.clear();
d_list_pid.clear();
if (d_received_message_type == 1)
{
d_encoded_message_type_1.reserve(GALILEO_CNAV_MAX_NUMBER_ENCODED_BLOCKS * GALILEO_CNAV_MESSAGE_BITS_PER_PAGE);
d_received_encoded_messages++;
d_list_pid.push_back(d_received_message_page_id);
d_encoded_message_type_1 += std::string(page_string);
}
}
if (d_received_encoded_messages == d_current_message_size)
{
// we have a full encoded message stored in d_encoded_message_type_1
d_received_encoded_messages = 0;
d_current_message_id = 0;
d_new_message = true;
decode_message_type1();
}
}
void Galileo_Cnav_Message::decode_message_type1()
{
// TODO: Reed-Solomon decoding of d_encoded_message_type_1
// TODO: reordering
// decoded_message_type1 = ...
// read_HAS_message_type1(decoded_message_type1);
}
void Galileo_Cnav_Message::read_HAS_message_type1(const std::string& message_string)
{
d_HAS_data = Galileo_HAS_data();
read_MT1_header(message_string);
read_MT1_body(message_string);
}
void Galileo_Cnav_Message::read_MT1_header(const std::string& message_string)
{
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)
{
auto message = std::string(message_string.begin() + GALILEO_CNAV_MT1_HEADER_BITS, message_string.end()); // Remove header
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
}
if (d_HAS_data.header.orbit_correction_flag)
{
// read orbit corrections
}
if (d_HAS_data.header.clock_fullset_flag)
{
// read clock full-set corrections
}
if (d_HAS_data.header.clock_subset_flag)
{
// read clock subset corrections
}
if (d_HAS_data.header.code_bias_flag)
{
// read code bias
}
if (d_HAS_data.header.phase_bias_flag)
{
// read phase bias
}
if (d_HAS_data.header.ura_flag)
{
// read URA
}
}
uint8_t Galileo_Cnav_Message::read_has_page_header_parameter(std::bitset<GALILEO_CNAV_PAGE_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_PAGE_HEADER_BITS - parameter.first - j]) == 1)
{
value += 1; // insert the bit
}
}
return value;
}
uint8_t Galileo_Cnav_Message::read_has_message_header_parameter_uint8(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(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(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;
}

View File

@ -23,8 +23,10 @@
#define GNSS_SDR_GALILEO_CNAV_MESSAGE_H
#include "Galileo_CNAV.h"
#include "galileo_has_data.h"
#include <bitset>
#include <cstdint>
#include <list>
#include <string>
/** \addtogroup Core
@ -45,14 +47,59 @@ public:
void read_HAS_page(const std::string& page_string);
inline bool get_flag_CRC_test() const
inline bool have_new_HAS_message()
{
return flag_CRC_test;
return d_new_message;
}
inline bool is_HAS_in_test_mode() const
{
return d_test_mode;
}
inline bool is_HAS_message_dummy() const
{
return d_page_dummy;
}
inline Galileo_HAS_data get_HAS_data() const
{
return d_HAS_data;
}
private:
bool CRC_test(std::bitset<GALILEO_CNAV_BITS_FOR_CRC> bits, uint32_t checksum) const;
bool flag_CRC_test{};
void read_HAS_page_header(const std::string& page_string);
void process_HAS_page(const std::string& page_string);
void decode_message_type1();
void read_HAS_message_type1(const std::string& message_string);
void read_MT1_header(const std::string& message_string);
void read_MT1_body(const std::string& message_string);
uint8_t read_has_page_header_parameter(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(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;
Galileo_HAS_data d_HAS_data{};
std::string d_encoded_message_type_1;
std::list<uint8_t> d_list_pid;
uint8_t d_has_page_status{};
uint8_t d_current_message_id{};
uint8_t d_current_message_size{};
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{};
};

View File

@ -0,0 +1,106 @@
/*!
* \file galileo_has_data.h
* \brief Class for Galileo HAS message type 1 data storage
* \author Carles Fernandez-Prades, 2020 cfernandez(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GALILEO_HAS_DATA_H
#define GNSS_SDR_GALILEO_HAS_DATA_H
#include <cstdint>
#include <vector>
/** \addtogroup Core
* \{ */
/** \addtogroup System_Parameters
* \{ */
struct mt1_header
{
uint16_t toh;
uint8_t mask_id;
uint8_t iod_id;
bool mask_flag;
bool orbit_correction_flag;
bool clock_fullset_flag;
bool clock_subset_flag;
bool code_bias_flag;
bool phase_bias_flag;
bool ura_flag;
};
/*!
* \brief This class is a storage for Galileo HAS message type 1, as defined in
* Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2
* (April 2020).
*/
class Galileo_HAS_data
{
public:
Galileo_HAS_data() = default;
mt1_header header;
// Mask
uint8_t Nsys;
std::vector<uint8_t> gnss_id_mask;
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<uint8_t> nav_message;
// Orbit corrections
uint8_t validity_interval_index_orbit_corrections;
std::vector<uint16_t> gnss_iod;
std::vector<int16_t> delta_radial;
std::vector<int16_t> delta_along_track;
std::vector<int16_t> delta_cross_track;
// Clock full-set corrections
uint8_t validity_interval_index_clock_fullset_corrections;
std::vector<uint8_t> delta_clock_c0_multiplier;
std::vector<bool> iod_change_flag;
std::vector<int16_t> delta_clock_c0;
// Clock subset corrections
uint8_t validity_interval_index_clock_subset_corrections;
uint8_t Nsysprime;
std::vector<uint8_t> gnss_id_clock_subset;
std::vector<uint8_t> delta_clock_c0_multiplier_clock_subset;
std::vector<std::vector<uint64_t>> satellite_submask;
std::vector<bool> iod_change_flag_clock_subset;
std::vector<int16_t> delta_clock_c0_clock_subset;
// Code bias
uint8_t validity_interval_index_code_bias_corrections;
std::vector<std::vector<int16_t>> code_bias;
// Phase bias
uint8_t validity_interval_index_phase_bias_corrections;
std::vector<std::vector<int16_t>> phase_bias;
std::vector<std::vector<uint8_t>> phase_discontinuity_indicator;
// URA
uint8_t validity_interval_index_ura_corrections;
std::vector<uint8_t> ura;
};
/** \} */
/** \} */
#endif // GNSS_SDR_GALILEO_HAS_DATA_H