mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-21 12:33:15 +00:00
Code Issues Releases Wiki Activity

Read clock subset corrections in HAS message

Browse Source
This commit is contained in:
Carles Fernandez 2021-09-21 11:52:38 +02:00
parent c2f526be8e
commit e8cc23276f
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
3 changed files with 73 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
src/algorithms/PVT/libs/has_simple_printer.cc
View File

@ -22,6 +22,7 @@
#include "gnss_sdr_filesystem.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <glog/logging.h>
#include <algorithm> // for std::find, std::count
#include <bitset>
#include <cstdint>
#include <ctime> // for tm
@ -187,38 +188,36 @@ bool Has_Simple_Printer::print_message(const Galileo_HAS_data* const has_data)
d_has_file << indent << indent << "GNSS ID: " << print_vector(has_data->gnss_id_clock_subset) << '\n';
d_has_file << indent << indent << "Delta Clock C0 Multiplier: " << print_vector(has_data->delta_clock_c0_multiplier_clock_subset) << '\n';
d_has_file << indent << indent << "Satellite sub-mask: ";
// for (uint8_t k = 0; k < has_data->Nsysprime; k++)
// {
// int j=0;
// auto it = std::find(has_data->gnss_id_mask.begin(), has_data->gnss_id_mask.end(), has_data->gnss_id_clock_subset[k]);
// if (it = !has_data->gnss_id_mask.end())
// {
// int index = it - gnss_id_mask.begin();
// std::string sat_mask = print_vector_binary(has_data->satellite_mask[index], HAS_MSG_SATELLITE_MASK_LENGTH);
// number_sats_satellite_mask = std::count(sat_mask.begin(), sat_mask.end(), '1');
// uint64_t mask_value = has_data->satellite_submask[k][j];
//
// std::string binary("");
// uint64_t mask = 1;
// for (int i = 0; i < number_sats_satellite_mask - 1; i++)
// {
// if ((mask & mask_value) >= 1)
// {
// binary = "1" + binary;
// }
// else
// {
// binary = "0" + binary;
// }
// mask <<= 1;
// }
// d_has_file << binary << " ";
// j++;
// }
// d_has_file << '\n';
// }
d_has_file << '\n';
d_has_file << indent << indent << "Delta Clock C0 [m]: " << print_vector(has_data->delta_clock_c0_clock_subset, HAS_MSG_DELTA_CLOCK_SCALE_FACTOR) << '\n';
for (uint8_t k = 0; k < has_data->Nsysprime; k++)
{
auto it = std::find(has_data->gnss_id_mask.begin(), has_data->gnss_id_mask.end(), has_data->gnss_id_clock_subset[k]);
if (it != has_data->gnss_id_mask.end())
{
int index = it - has_data->gnss_id_mask.begin();
std::string sat_mask = print_vector_binary(std::vector<uint64_t>(1, has_data->satellite_mask[index]), HAS_MSG_SATELLITE_MASK_LENGTH);
int number_sats_satellite_mask = std::count(sat_mask.begin(), sat_mask.end(), '1');
uint64_t mask_value = has_data->satellite_mask[index];
std::string binary("");
uint64_t mask = 1;
for (int i = 0; i < number_sats_satellite_mask - 1; i++)
{
if ((mask & mask_value) >= 1)
{
binary = "1" + binary;
}
else
{
binary = "0" + binary;
}
mask <<= 1;
}
d_has_file << binary << " ";
}
d_has_file << '\n';
d_has_file << indent << indent << "Delta Clock C0 [m]: " << print_vector(has_data->delta_clock_c0_clock_subset[k], HAS_MSG_DELTA_CLOCK_SCALE_FACTOR) << '\n';
}
}
if (has_data->header.code_bias_flag == true)

66
src/core/libs/galileo_e6_has_msg_receiver.cc
View File

@ -436,11 +436,6 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
DLOG(INFO) << "Nsys: " << static_cast<float>(d_HAS_data.Nsys);
DLOG(INFO) << debug_print_vector("GNSS ID", d_HAS_data.gnss_id_mask);
DLOG(INFO) << debug_print_vector("cell_mask_availability_flag", d_HAS_data.cell_mask_availability_flag);
// for (uint8_t k = 0; k < d_HAS_data.Nsys; k++)
// {
// std::string aux = "cell_mask " + std::to_string(k);
// DLOG(INFO) << debug_print_matrix(aux, d_HAS_data.cell_mask[k]);
// }
DLOG(INFO) << debug_print_vector("nav_message", d_HAS_data.nav_message);
}
else
@ -462,11 +457,6 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
DLOG(INFO) << "Nsys: " << static_cast<float>(d_HAS_data.Nsys);
DLOG(INFO) << debug_print_vector("GNSS ID", d_HAS_data.gnss_id_mask);
DLOG(INFO) << debug_print_vector("cell_mask_availability_flag", d_HAS_data.cell_mask_availability_flag);
// for (uint8_t k = 0; k < d_HAS_data.Nsys; k++)
// {
// std::string aux = "cell_mask " + std::to_string(k);
// DLOG(INFO) << debug_print_matrix(aux, d_HAS_data.cell_mask[k]);
// }
DLOG(INFO) << debug_print_vector("nav_message", d_HAS_data.nav_message);
}
}
@ -524,7 +514,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
d_HAS_data.delta_clock_c0_multiplier = std::vector<uint8_t>(d_HAS_data.Nsys);
for (uint8_t i = 0; i < d_HAS_data.Nsys; i++)
{
d_HAS_data.delta_clock_c0_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH));
d_HAS_data.delta_clock_c0_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH)) + 1; // b00 means x1, b01 means x2, etc
message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH, message.end());
}
@ -560,33 +550,55 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
d_HAS_data.gnss_id_clock_subset = std::vector<uint8_t>(d_HAS_data.Nsysprime);
d_HAS_data.delta_clock_c0_multiplier_clock_subset = std::vector<uint8_t>(d_HAS_data.Nsysprime);
d_HAS_data.satellite_submask = std::vector<std::vector<uint64_t>>(d_HAS_data.Nsysprime, std::vector<uint64_t>(HAS_MSG_NUMBER_SATELLITE_IDS));
d_HAS_data.iod_change_flag_clock_subset = std::vector<bool>(d_HAS_data.Nsysprime);
d_HAS_data.delta_clock_c0_clock_subset = std::vector<int16_t>(d_HAS_data.Nsysprime);
d_HAS_data.satellite_submask = std::vector<uint64_t>(d_HAS_data.Nsysprime);
d_HAS_data.iod_change_flag_clock_subset = std::vector<std::vector<bool>>(d_HAS_data.Nsysprime, std::vector<bool>());
d_HAS_data.delta_clock_c0_clock_subset = std::vector<std::vector<int16_t>>(d_HAS_data.Nsysprime, std::vector<int16_t>());
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());
uint8_t clock_multiplier = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH));
d_HAS_data.delta_clock_c0_multiplier_clock_subset[i] = clock_multiplier + 1;
d_HAS_data.delta_clock_c0_multiplier_clock_subset[i] = clock_multiplier + 1; // b00 means x1, b01 means x2, etc
message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH, message.end());
uint64_t satellite_mask = d_HAS_data.satellite_mask[i];
std::bitset<40> satellite_mask_bits(satellite_mask);
// find the satellite mask corresponding to this GNSS ID
auto it = std::find(d_HAS_data.gnss_id_mask.begin(), d_HAS_data.gnss_id_mask.end(), d_HAS_data.gnss_id_clock_subset[i]);
int index = it - d_HAS_data.gnss_id_mask.begin();
uint64_t satellite_mask = d_HAS_data.satellite_mask[index];
// count satellites in the mask
std::bitset<HAS_MSG_SATELLITE_MASK_LENGTH> satellite_mask_bits(satellite_mask);
std::string satellite_mask_string = satellite_mask_bits.to_string();
int number_sats_this_gnss_id = std::count(satellite_mask_string.begin(), satellite_mask_string.end(), '1');
d_HAS_data.satellite_submask[i] = std::vector<uint64_t>(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());
}
int Nsatprime = std::count(d_HAS_data.satellite_submask[i].begin(), d_HAS_data.satellite_submask[i].end(), 1UL);
d_HAS_data.satellite_submask[i] = read_has_message_body_uint64(message.substr(0, number_sats_this_gnss_id));
message = std::string(message.begin() + number_sats_this_gnss_id, message.end());
// Count ones in satellite submask
std::string binary("");
uint64_t aux = 1;
uint64_t mask_value = d_HAS_data.satellite_submask[i];
for (int k = 0; k < number_sats_this_gnss_id - 1; k++)
{
if ((aux & mask_value) >= 1)
{
binary = "1" + binary;
}
else
{
binary = "0" + binary;
}
aux <<= 1;
}
int Nsatprime = std::count(binary.begin(), binary.end(), '1');
d_HAS_data.delta_clock_c0_clock_subset[i].reserve(Nsatprime);
// Read Nsatprime values of delta_clock_c0_clock_subset
for (int j = 0; j < Nsatprime; j++)
{
d_HAS_data.delta_clock_c0_clock_subset[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH));
d_HAS_data.delta_clock_c0_clock_subset[i][j] = 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());
}
}
@ -594,8 +606,8 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body)
DLOG(INFO) << "Nsysprime: " << static_cast<float>(d_HAS_data.Nsysprime);
DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("gnss_id_clock_subset", d_HAS_data.gnss_id_clock_subset));
DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("delta_clock_c0_multiplier_clock_subset", d_HAS_data.delta_clock_c0_multiplier_clock_subset));
DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_matrix("satellite_submask", d_HAS_data.satellite_submask));
DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("delta_clock_c0_clock_subset", d_HAS_data.delta_clock_c0_clock_subset));
DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("satellite_submask", d_HAS_data.satellite_submask));
DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_matrix("delta_clock_c0_clock_subset", d_HAS_data.delta_clock_c0_clock_subset));
}
if (d_HAS_data.header.code_bias_flag && have_mask)

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

@ -79,9 +79,9 @@ public:
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;
std::vector<uint64_t> satellite_submask;
std::vector<std::vector<bool>> iod_change_flag_clock_subset;
std::vector<std::vector<int16_t>> delta_clock_c0_clock_subset;
// Code bias
uint8_t validity_interval_index_code_bias_corrections;