diff --git a/src/algorithms/PVT/libs/has_simple_printer.cc b/src/algorithms/PVT/libs/has_simple_printer.cc index 80218998e..4665e7454 100644 --- a/src/algorithms/PVT/libs/has_simple_printer.cc +++ b/src/algorithms/PVT/libs/has_simple_printer.cc @@ -192,46 +192,19 @@ bool Has_Simple_Printer::print_message(const Galileo_HAS_data* const has_data) d_has_file << indent << indent << "Nsys: " << static_cast(has_data->Nsys) << '\n'; d_has_file << indent << indent << "GNSS ID: " << print_vector(has_data->gnss_id_mask) << " (0: GPS, 2: Galileo)\n"; - // compute Nsat - int Nsat = 0; - std::vector num_sats_in_mask(has_data->Nsys); - for (uint8_t i = 0; i < has_data->Nsys; i++) - { - std::string sat_mask = print_vector_binary(std::vector(1, has_data->satellite_mask[i]), HAS_MSG_SATELLITE_MASK_LENGTH); - Nsat += std::count(sat_mask.begin(), sat_mask.end(), '1'); - } - + int Nsat = has_data->get_nsat(); d_has_file << indent << indent << "Satellite Mask: " << print_vector_binary(has_data->satellite_mask, HAS_MSG_SATELLITE_MASK_LENGTH) << '\n'; d_has_file << indent << indent << " Nsat: " << Nsat << '\n'; + std::vector system_strings = has_data->get_systems_string(); for (uint8_t i = 0; i < has_data->Nsys; i++) { - std::string system("Reserved"); - if (has_data->gnss_id_mask[i] == 0) - { - system = "GPS"; - } - if (has_data->gnss_id_mask[i] == 2) - { - system = "Galileo"; - } - - d_has_file << indent << indent << " PRN for " << system << ": " << print_vector(has_data->get_PRNs_in_mask(i)) << " (" << has_data->get_PRNs_in_mask(i).size() << " satellites)\n"; + d_has_file << indent << indent << " PRN for " << system_strings[i] << ": " << print_vector(has_data->get_PRNs_in_mask(i)) << " (" << has_data->get_PRNs_in_mask(i).size() << " satellites)\n"; } d_has_file << indent << indent << "Signal Mask: " << print_vector_binary(has_data->signal_mask, HAS_MSG_SIGNAL_MASK_LENGTH) << '\n'; for (uint8_t i = 0; i < has_data->Nsys; i++) { - std::string system("Reserved"); - if (has_data->gnss_id_mask[i] == 0) - { - system = "GPS"; - } - if (has_data->gnss_id_mask[i] == 2) - { - system = "Galileo"; - } - - d_has_file << indent << indent << " Bias corrections for " << system << " signals: " << print_vector_string(has_data->get_signals_in_mask(i)) << '\n'; + d_has_file << indent << indent << " Bias corrections for " << system_strings[i] << " signals: " << print_vector_string(has_data->get_signals_in_mask(i)) << '\n'; } d_has_file << indent << indent << "Cell Mask Availability Flag: " << print_vector(has_data->cell_mask_availability_flag) << '\n'; diff --git a/src/core/system_parameters/galileo_has_data.cc b/src/core/system_parameters/galileo_has_data.cc index 4f135212c..5cc8c381b 100644 --- a/src/core/system_parameters/galileo_has_data.cc +++ b/src/core/system_parameters/galileo_has_data.cc @@ -18,6 +18,8 @@ #include "Galileo_CNAV.h" #include #include +#include +#include std::vector Galileo_HAS_data::get_PRNs_in_mask(uint8_t nsys) const @@ -398,3 +400,368 @@ uint8_t Galileo_HAS_data::get_gnss_id(int nsat) const return HAS_MSG_WRONG_SYSTEM; } + + +uint16_t Galileo_HAS_data::get_validity_interval_s(uint8_t validity_interval_index) const +{ + uint16_t validity_interval; + switch (validity_interval_index) + { + case 0: + validity_interval = 5; + break; + case 1: + validity_interval = 10; + break; + case 2: + validity_interval = 15; + break; + case 3: + validity_interval = 20; + break; + case 4: + validity_interval = 30; + break; + case 5: + validity_interval = 60; + break; + case 6: + validity_interval = 90; + break; + case 7: + validity_interval = 120; + break; + case 8: + validity_interval = 180; + break; + case 9: + validity_interval = 240; + break; + case 10: + validity_interval = 300; + break; + case 11: + validity_interval = 600; + break; + case 12: + validity_interval = 900; + break; + case 13: + validity_interval = 1800; + break; + case 14: + validity_interval = 3600; + break; + default: // reserved + validity_interval = 0; + } + return validity_interval; +} + + +std::vector Galileo_HAS_data::get_delta_radial_m() const +{ + std::vector delta_radial_m; + delta_radial_m.reserve(this->delta_radial.size()); + for (const auto& d : this->delta_radial) + { + delta_radial_m.push_back(static_cast(d) * HAS_MSG_DELTA_RADIAL_SCALE_FACTOR); + } + return delta_radial_m; +} + + +std::vector Galileo_HAS_data::get_delta_radial_m(uint8_t nsys) const +{ + std::vector delta_orbit_radial_m = this->get_delta_radial_m(); + if (nsys >= this->Nsys) + { + return delta_orbit_radial_m; + } + std::vector delta_orbit_radial_m_aux; + uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + delta_orbit_radial_m_aux.reserve(num_sats_in_this_system); + + size_t index = 0; + for (uint8_t sys = 0; sys <= nsys; sys++) + { + uint8_t num_sats_in_system = this->get_num_satellites()[sys]; + if (sys != nsys) + { + index += num_sats_in_system; + } + else + { + for (uint8_t sat = 0; sat < num_sats_in_system; sat++) + { + delta_orbit_radial_m_aux.push_back(delta_orbit_radial_m[index]); + index++; + } + } + } + return delta_orbit_radial_m_aux; +} + + +std::vector Galileo_HAS_data::get_delta_along_track_m() const +{ + std::vector delta_along_track_m; + delta_along_track_m.reserve(this->delta_along_track.size()); + for (const auto& d : this->delta_along_track) + { + delta_along_track_m.push_back(static_cast(d) * HAS_MSG_DELTA_ALONG_TRACK_SCALE_FACTOR); + } + return delta_along_track_m; +} + + +std::vector Galileo_HAS_data::get_delta_along_track_m(uint8_t nsys) const +{ + std::vector delta_along_track_m = this->get_delta_along_track_m(); + if (nsys >= this->Nsys) + { + return delta_along_track_m; + } + std::vector delta_along_track_m_aux; + uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + delta_along_track_m_aux.reserve(num_sats_in_this_system); + + size_t index = 0; + for (uint8_t sys = 0; sys <= nsys; sys++) + { + uint8_t num_sats_in_system = this->get_num_satellites()[sys]; + if (sys != nsys) + { + index += num_sats_in_system; + } + else + { + for (uint8_t sat = 0; sat < num_sats_in_system; sat++) + { + delta_along_track_m_aux.push_back(delta_along_track_m[index]); + index++; + } + } + } + return delta_along_track_m_aux; +} + + +std::vector Galileo_HAS_data::get_delta_cross_track_m() const +{ + std::vector delta_cross_track_m; + delta_cross_track_m.reserve(this->delta_cross_track.size()); + for (const auto& d : this->delta_along_track) + { + delta_cross_track_m.push_back(static_cast(d) * HAS_MSG_DELTA_CROSS_TRACK_SCALE_FACTOR); + } + return delta_cross_track_m; +} + + +std::vector Galileo_HAS_data::get_delta_cross_track_m(uint8_t nsys) const +{ + std::vector delta_cross_track_m = this->get_delta_cross_track_m(); + if (nsys >= this->Nsys) + { + return delta_cross_track_m; + } + std::vector delta_cross_track_m_aux; + uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + delta_cross_track_m_aux.reserve(num_sats_in_this_system); + + size_t index = 0; + for (uint8_t sys = 0; sys <= nsys; sys++) + { + uint8_t num_sats_in_system = this->get_num_satellites()[sys]; + if (sys != nsys) + { + index += num_sats_in_system; + } + else + { + for (uint8_t sat = 0; sat < num_sats_in_system; sat++) + { + delta_cross_track_m_aux.push_back(delta_cross_track_m[index]); + index++; + } + } + } + return delta_cross_track_m_aux; +} + + +std::vector Galileo_HAS_data::get_delta_clock_c0_m() const +{ + std::vector delta_clock_c0_m; + delta_clock_c0_m.reserve(this->delta_clock_c0.size()); + for (const auto& d : this->delta_clock_c0) + { + delta_clock_c0_m.push_back(static_cast(d) * HAS_MSG_DELTA_CLOCK_SCALE_FACTOR); + } + return delta_clock_c0_m; +} + + +std::vector Galileo_HAS_data::get_delta_clock_c0_m(uint8_t nsys) const +{ + std::vector delta_clock_c0_m = this->get_delta_clock_c0_m(); + if (nsys >= this->Nsys) + { + return delta_clock_c0_m; + } + std::vector delta_clock_c0_m_aux; + uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + delta_clock_c0_m_aux.reserve(num_sats_in_this_system); + + size_t index = 0; + for (uint8_t sys = 0; sys <= nsys; sys++) + { + uint8_t num_sats_in_system = this->get_num_satellites()[sys]; + if (sys != nsys) + { + index += num_sats_in_system; + } + else + { + for (uint8_t sat = 0; sat < num_sats_in_system; sat++) + { + delta_clock_c0_m_aux.push_back(delta_clock_c0_m[index]); + index++; + } + } + } + return delta_clock_c0_m_aux; +} + + +std::vector> Galileo_HAS_data::get_code_bias_m() const +{ + std::vector> code_bias_m; + const size_t outer_size = this->code_bias.size(); + if (outer_size == 0) + { + return code_bias_m; + } + const size_t inner_size = this->code_bias[0].size(); + code_bias_m = std::vector>(outer_size, std::vector(inner_size)); + for (size_t i = 0; i < outer_size; i++) + { + for (size_t j = 0; j < inner_size; j++) + { + code_bias_m[i][j] = static_cast(this->code_bias[i][j]) * HAS_MSG_CODE_BIAS_SCALE_FACTOR; + } + } + return code_bias_m; +} + + +std::vector> Galileo_HAS_data::get_phase_bias_cycle() const +{ + std::vector> phase_bias_cycle; + const size_t outer_size = this->phase_bias.size(); + if (outer_size == 0) + { + return phase_bias_cycle; + } + const size_t inner_size = this->phase_bias[0].size(); + phase_bias_cycle = std::vector>(outer_size, std::vector(inner_size)); + for (size_t i = 0; i < outer_size; i++) + { + for (size_t j = 0; j < inner_size; j++) + { + phase_bias_cycle[i][j] = static_cast(this->phase_bias[i][j]) * HAS_MSG_PHASE_BIAS_SCALE_FACTOR; + } + } + return phase_bias_cycle; +} + + +std::vector Galileo_HAS_data::get_num_satellites() const +{ + std::vector num_satellites; + if (this->Nsys == 0) + { + return num_satellites; + } + num_satellites.reserve(this->Nsys); + for (uint8_t i = 0; i < this->Nsys; i++) + { + std::stringstream ss; + std::bitset bits(this->satellite_mask[i]); + ss << bits.to_string(); + std::string sat_mask = ss.str(); + num_satellites.push_back(static_cast(std::count(sat_mask.begin(), sat_mask.end(), '1'))); + } + + return num_satellites; +} + + +std::vector Galileo_HAS_data::get_gnss_iod(uint8_t nsys) const +{ + std::vector gnss_iod_v = this->gnss_iod; + + if (nsys >= this->Nsys) + { + return gnss_iod_v; + } + std::vector gnss_iod_aux; + uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + gnss_iod_aux.reserve(num_sats_in_this_system); + + size_t index = 0; + for (uint8_t sys = 0; sys <= nsys; sys++) + { + uint8_t num_sats_in_system = this->get_num_satellites()[sys]; + if (sys != nsys) + { + index += num_sats_in_system; + } + else + { + for (uint8_t sat = 0; sat < num_sats_in_system; sat++) + { + gnss_iod_aux.push_back(gnss_iod_v[index]); + index++; + } + } + } + return gnss_iod_aux; +} + + +uint16_t Galileo_HAS_data::get_nsat() const +{ + std::vector num_satellites = this->get_num_satellites(); + uint16_t total_number_of_sats = std::accumulate(num_satellites.begin(), num_satellites.end(), 0); + return total_number_of_sats; +} + + +std::vector Galileo_HAS_data::get_systems_string() const +{ + const size_t nsys = this->gnss_id_mask.size(); + std::vector systems(nsys, std::string("")); + for (uint8_t i = 0; i < this->Nsys; i++) + { + std::string system("Reserved"); + if (this->gnss_id_mask[i] == 0) + { + system = "GPS"; + } + if (this->gnss_id_mask[i] == 2) + { + system = "Galileo"; + } + systems[i] = system; + } + return systems; +} + + +uint16_t Galileo_HAS_data::get_nsatprime() const +{ + auto Nsatprime = static_cast(this->delta_clock_c0_clock_subset.size()); + return Nsatprime; +} diff --git a/src/core/system_parameters/galileo_has_data.h b/src/core/system_parameters/galileo_has_data.h index e4a638ac0..0785efb08 100644 --- a/src/core/system_parameters/galileo_has_data.h +++ b/src/core/system_parameters/galileo_has_data.h @@ -51,10 +51,26 @@ class Galileo_HAS_data public: Galileo_HAS_data() = default; - std::vector get_PRNs_in_mask(uint8_t nsys) const; - std::vector get_PRNs_in_submask(uint8_t nsys) const; - std::vector get_signals_in_mask(uint8_t nsys) const; - uint8_t get_gnss_id(int nsat) const; + std::vector get_signals_in_mask(uint8_t nsys) const; //!< Get a vector of Nsys std::string with signals in mask for system nsys, with 0 <= nsys < Nsys + std::vector get_systems_string() const; //!< Get Nsys system name strings + std::vector> get_code_bias_m() const; //!< Get Nsat x Ncodes code biases in [m] + std::vector> get_phase_bias_cycle() const; //!< Get Nsat x Nphases phase biases in [cycles] + std::vector get_delta_radial_m() const; //!< Get Nsat delta radial corrections in [m] + std::vector get_delta_radial_m(uint8_t nsys) const; //!< Get delta radial corrections in [m] for system nsys, with 0 <= nsys < Nsys + std::vector get_delta_along_track_m() const; //!< Get Nsat delta along_track corrections in [m] + std::vector get_delta_along_track_m(uint8_t nsys) const; //!< Get alog-track corrections in [m] for system nsys, with 0 <= nsys < Nsys + std::vector get_delta_cross_track_m() const; //!< Get Nsat delta cross_track corrections in [m] + std::vector get_delta_cross_track_m(uint8_t nsys) const; //!< Get delta cross_track corrections in [m] for system nsys, with 0 <= nsys < Nsys + std::vector get_delta_clock_c0_m() const; //!< Get Nsat delta clock C0 corrections in [m] + std::vector get_delta_clock_c0_m(uint8_t nsys) const; //!< Get delta clock C0 corrections in [m] for system nsys, with 0 <= nsys < Nsys + std::vector get_PRNs_in_mask(uint8_t nsys) const; //!< Get PRNs in mask for system nsys, with 0 <= nsys < Nsys + std::vector get_PRNs_in_submask(uint8_t nsys) const; //!< Get PRNs in submask for system nsys, with 0 <= nsys < Nsys + std::vector get_gnss_iod(uint8_t nsys) const; //!< Get GNSS IODs for for system nsys, with 0 <= nsys < Nsys + std::vector get_num_satellites() const; //!< Get Nsys number of satellites + uint16_t get_nsat() const; //!< Get total number of satellites with corrections + uint16_t get_nsatprime() const; //!< Get number of satellites in clock subset corrections + uint16_t get_validity_interval_s(uint8_t validity_interval_index) const; //!< Get validity interbal in [s] from the validity_interval_index + uint8_t get_gnss_id(int nsat) const; //!< Get GNSS ID from the nsat satellite // Mask std::vector gnss_id_mask; @@ -96,7 +112,7 @@ public: uint8_t has_status; uint8_t message_id; - uint8_t Nsys; // in Mask + uint8_t Nsys; //!< Number of GNSS for which corrections are provided uint8_t validity_interval_index_orbit_corrections; // in Orbit corrections uint8_t validity_interval_index_clock_fullset_corrections; // in Clock full-set corrections uint8_t validity_interval_index_clock_subset_corrections; // in Clock subset corrections