diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index 5e43f0bf4..fafe79b12 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -14,6 +14,15 @@ All notable changes to GNSS-SDR will be documented in this file. ## [Unreleased](https://github.com/gnss-sdr/gnss-sdr/tree/next) +### Improvements in Accuracy: + +- Processing and application of the corrections provided by the Galileo High + Accuracy Service (HAS) to the PVT solution. It requires at least a Galileo E1 + (or E5a) + Galileo E6B configuration. A new configuration parameter + `PVT.use_has_corrections`, set to `true` by default, can be used to deactivate + the application of HAS corrections but still retrieve the HAS data if set to + `false`. + ### Improvements in Availability: - Fixed bug that made the PVT block to not resolve position anymore after a loss @@ -99,6 +108,9 @@ All notable changes to GNSS-SDR will be documented in this file. manually set the bandwidth of the bandpass filter on the radio frontend. - The new configuration parameter `Channels_XX.RF_channel_ID` allows to specify the signal source per channel group. +- New configuration parameter `PVT.use_unhealthy_sats`, set by default to + `false`, allows processing observables of satellites that report an unhealthy + status in the navigation message if set to `true`. - Allowed the CMake project to be a sub-project. See the definitions of concepts and metrics at diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index ddadd7242..a607d2655 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -878,6 +878,10 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, // Use E6 for PVT pvt_output_parameters.use_e6_for_pvt = configuration->property(role + ".use_e6_for_pvt", pvt_output_parameters.use_e6_for_pvt); + pvt_output_parameters.use_has_corrections = configuration->property(role + ".use_has_corrections", pvt_output_parameters.use_has_corrections); + + // Use unhealthy satellites + pvt_output_parameters.use_unhealthy_sats = configuration->property(role + ".use_unhealthy_sats", pvt_output_parameters.use_unhealthy_sats); // make PVT object pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index fa487ec1f..500db4d4c 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -176,7 +176,9 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_enable_rx_clock_correction(conf_.enable_rx_clock_correction), d_an_printer_enabled(conf_.an_output_enabled), d_log_timetag(conf_.log_source_timetag), - d_use_e6_for_pvt(conf_.use_e6_for_pvt) + d_use_e6_for_pvt(conf_.use_e6_for_pvt), + d_use_has_corrections(conf_.use_has_corrections), + d_use_unhealthy_sats(conf_.use_unhealthy_sats) { // Send feedback message to observables block with the receiver clock offset this->message_port_register_out(pmt::mp("pvt_to_observables")); @@ -551,19 +553,6 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_user_pvt_solver = d_internal_pvt_solver; } - d_mapStringValues["1C"] = evGPS_1C; - d_mapStringValues["2S"] = evGPS_2S; - d_mapStringValues["L5"] = evGPS_L5; - d_mapStringValues["1B"] = evGAL_1B; - d_mapStringValues["5X"] = evGAL_5X; - d_mapStringValues["E6"] = evGAL_E6; - d_mapStringValues["7X"] = evGAL_7X; - d_mapStringValues["1G"] = evGLO_1G; - d_mapStringValues["2G"] = evGLO_2G; - d_mapStringValues["B1"] = evBDS_B1; - d_mapStringValues["B2"] = evBDS_B2; - d_mapStringValues["B3"] = evBDS_B3; - // set the RTKLIB trace (debug) level tracelevel(conf_.rtk_trace_level); @@ -1207,7 +1196,15 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) if (gps_eph->SV_health != 0) { std::cout << TEXT_RED << "Satellite " << Gnss_Satellite(std::string("GPS"), gps_eph->PRN) - << " is not healthy, not used for navigation" << TEXT_RESET << '\n'; + << " reports an unhealthy status,"; + if (d_use_unhealthy_sats) + { + std::cout << " use PVT solutions at your own risk" << TEXT_RESET << '\n'; + } + else + { + std::cout << " not used for navigation" << TEXT_RESET << '\n'; + } } } else if (msg_type_hash_code == d_gps_iono_sptr_type_hash_code) @@ -1267,8 +1264,15 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) if (gps_cnav_ephemeris->signal_health != 0) { std::cout << "Satellite " << Gnss_Satellite(std::string("GPS"), gps_cnav_ephemeris->PRN) - << " does not report a healthy status in the CNAV message," - << " use PVT solutions at your own risk.\n"; + << " reports an unhealthy status in the CNAV message,"; + if (d_use_unhealthy_sats) + { + std::cout << " use PVT solutions at your own risk.\n"; + } + else + { + std::cout << " not used for navigation.\n"; + } } DLOG(INFO) << "New GPS CNAV ephemeris record has arrived"; } @@ -1354,7 +1358,15 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) ((galileo_eph->E5b_HS != 0) || (galileo_eph->E5b_DVS == true))) { std::cout << TEXT_RED << "Satellite " << Gnss_Satellite(std::string("Galileo"), galileo_eph->PRN) - << " is not healthy, not used for navigation" << TEXT_RESET << '\n'; + << " reports an unhealthy status,"; + if (d_use_unhealthy_sats) + { + std::cout << " use PVT solutions at your own risk" << TEXT_RESET << '\n'; + } + else + { + std::cout << " not used for navigation" << TEXT_RESET << '\n'; + } } } else if (msg_type_hash_code == d_galileo_iono_sptr_type_hash_code) @@ -1530,7 +1542,15 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) if (bds_dnav_eph->SV_health != 0) { std::cout << TEXT_RED << "Satellite " << Gnss_Satellite(std::string("Beidou"), bds_dnav_eph->PRN) - << " is not healthy, not used for navigation" << TEXT_RESET << '\n'; + << " reports an unhealthy status,"; + if (d_use_unhealthy_sats) + { + std::cout << " use PVT solutions at your own risk" << TEXT_RESET << '\n'; + } + else + { + std::cout << " not used for navigation" << TEXT_RESET << '\n'; + } } } else if (msg_type_hash_code == d_beidou_dnav_iono_sptr_type_hash_code) @@ -1578,7 +1598,7 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) } -void rtklib_pvt_gs::msg_handler_has_data(const pmt::pmt_t& msg) const +void rtklib_pvt_gs::msg_handler_has_data(const pmt::pmt_t& msg) { try { @@ -1586,6 +1606,14 @@ void rtklib_pvt_gs::msg_handler_has_data(const pmt::pmt_t& msg) const if (msg_type_hash_code == d_galileo_has_data_sptr_type_hash_code) { const auto has_data = wht::any_cast>(pmt::any_ref(msg)); + if (d_use_has_corrections && (has_data->has_status == 1)) // operational mode + { + d_internal_pvt_solver->store_has_data(*has_data); + if (d_enable_rx_clock_correction == true) + { + d_user_pvt_solver->store_has_data(*has_data); + } + } if (d_has_simple_printer) { d_has_simple_printer->print_message(has_data.get()); @@ -1808,44 +1836,10 @@ void rtklib_pvt_gs::apply_rx_clock_offset(std::map& observabl // all observables in the map are valid observables_iter->second.RX_time -= rx_clock_offset_s; observables_iter->second.Pseudorange_m -= rx_clock_offset_s * SPEED_OF_LIGHT_M_S; - - switch (d_mapStringValues[observables_iter->second.Signal]) + const auto it_freq_map = SIGNAL_FREQ_MAP.find(std::string(observables_iter->second.Signal, 2)); + if (it_freq_map != SIGNAL_FREQ_MAP.cend()) { - case evGPS_1C: - case evSBAS_1C: - case evGAL_1B: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1 * TWO_PI; - break; - case evGPS_L5: - case evGAL_5X: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ5 * TWO_PI; - break; - case evGAL_E6: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ6 * TWO_PI; - break; - case evGAL_7X: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ7 * TWO_PI; - break; - case evGPS_2S: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ2 * TWO_PI; - break; - case evBDS_B3: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ3_BDS * TWO_PI; - break; - case evGLO_1G: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1_GLO * TWO_PI; - break; - case evGLO_2G: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ2_GLO * TWO_PI; - break; - case evBDS_B1: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1_BDS * TWO_PI; - break; - case evBDS_B2: - observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ2_BDS * TWO_PI; - break; - default: - break; + observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * it_freq_map->second * TWO_PI; } } } @@ -1910,44 +1904,11 @@ void rtklib_pvt_gs::initialize_and_apply_carrier_phase_offset() // it is set to false by the work function if the gnss_synchro is not valid if (d_channel_initialized.at(observables_iter->second.Channel_ID) == false) { - double wavelength_m = 0; - switch (d_mapStringValues[observables_iter->second.Signal]) + double wavelength_m = 1.0; + const auto it_freq_map = SIGNAL_FREQ_MAP.find(std::string(observables_iter->second.Signal, 2)); + if (it_freq_map != SIGNAL_FREQ_MAP.cend()) { - case evGPS_1C: - case evSBAS_1C: - case evGAL_1B: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1; - break; - case evGPS_L5: - case evGAL_5X: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ5; - break; - case evGAL_E6: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ6; - break; - case evGAL_7X: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ7; - break; - case evGPS_2S: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2; - break; - case evBDS_B3: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ3_BDS; - break; - case evGLO_1G: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_GLO; - break; - case evGLO_2G: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_GLO; - break; - case evBDS_B1: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_BDS; - break; - case evBDS_B2: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_BDS; - break; - default: - break; + wavelength_m = SPEED_OF_LIGHT_M_S / it_freq_map->second; } const double wrap_carrier_phase_rad = fmod(observables_iter->second.Carrier_phase_rads, TWO_PI); d_initial_carrier_phase_offset_estimation_rads.at(observables_iter->second.Channel_ID) = TWO_PI * round(observables_iter->second.Pseudorange_m / wavelength_m) - observables_iter->second.Carrier_phase_rads + wrap_carrier_phase_rad; @@ -1960,6 +1921,16 @@ void rtklib_pvt_gs::initialize_and_apply_carrier_phase_offset() } +void rtklib_pvt_gs::update_HAS_corrections() +{ + this->d_internal_pvt_solver->update_has_corrections(this->d_gnss_observables_map); + if (d_enable_rx_clock_correction == true) + { + this->d_user_pvt_solver->update_has_corrections(this->d_gnss_observables_map); + } +} + + int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items __attribute__((unused))) { @@ -2021,7 +1992,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (tmp_eph_iter_gps != d_internal_pvt_solver->gps_ephemeris_map.cend()) { const uint32_t prn_aux = tmp_eph_iter_gps->second.PRN; - if ((prn_aux == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal) == std::string("1C")) && (tmp_eph_iter_gps->second.SV_health == 0)) + if ((prn_aux == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal, 2) == std::string("1C")) && (d_use_unhealthy_sats || (tmp_eph_iter_gps->second.SV_health == 0))) { store_valid_observable = true; } @@ -2030,9 +2001,9 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { const uint32_t prn_aux = tmp_eph_iter_gal->second.PRN; if ((prn_aux == in[i][epoch].PRN) && - (((std::string(in[i][epoch].Signal) == std::string("1B")) && (tmp_eph_iter_gal->second.E1B_DVS == false) && (tmp_eph_iter_gal->second.E1B_HS == 0)) || - ((std::string(in[i][epoch].Signal) == std::string("5X")) && (tmp_eph_iter_gal->second.E5a_DVS == false) && (tmp_eph_iter_gal->second.E5a_HS == 0)) || - ((std::string(in[i][epoch].Signal) == std::string("7X")) && (tmp_eph_iter_gal->second.E5b_DVS == false) && (tmp_eph_iter_gal->second.E5b_HS == 0)))) + (((std::string(in[i][epoch].Signal, 2) == std::string("1B")) && (d_use_unhealthy_sats || ((tmp_eph_iter_gal->second.E1B_DVS == false) && (tmp_eph_iter_gal->second.E1B_HS == 0)))) || + ((std::string(in[i][epoch].Signal, 2) == std::string("5X")) && (d_use_unhealthy_sats || ((tmp_eph_iter_gal->second.E5a_DVS == false) && (tmp_eph_iter_gal->second.E5a_HS == 0)))) || + ((std::string(in[i][epoch].Signal, 2) == std::string("7X")) && (d_use_unhealthy_sats || ((tmp_eph_iter_gal->second.E5b_DVS == false) && (tmp_eph_iter_gal->second.E5b_HS == 0)))))) { store_valid_observable = true; } @@ -2040,7 +2011,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (tmp_eph_iter_cnav != d_internal_pvt_solver->gps_cnav_ephemeris_map.cend()) { const uint32_t prn_aux = tmp_eph_iter_cnav->second.PRN; - if ((prn_aux == in[i][epoch].PRN) && (((std::string(in[i][epoch].Signal) == std::string("2S")) || (std::string(in[i][epoch].Signal) == std::string("L5"))))) + if ((prn_aux == in[i][epoch].PRN) && (((std::string(in[i][epoch].Signal, 2) == std::string("2S")) || (std::string(in[i][epoch].Signal, 2) == std::string("L5"))))) { store_valid_observable = true; } @@ -2048,7 +2019,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (tmp_eph_iter_glo_gnav != d_internal_pvt_solver->glonass_gnav_ephemeris_map.cend()) { const uint32_t prn_aux = tmp_eph_iter_glo_gnav->second.PRN; - if ((prn_aux == in[i][epoch].PRN) && ((std::string(in[i][epoch].Signal) == std::string("1G")) || (std::string(in[i][epoch].Signal) == std::string("2G")))) + if ((prn_aux == in[i][epoch].PRN) && ((std::string(in[i][epoch].Signal, 2) == std::string("1G")) || (std::string(in[i][epoch].Signal, 2) == std::string("2G")))) { store_valid_observable = true; } @@ -2056,12 +2027,12 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (tmp_eph_iter_bds_dnav != d_internal_pvt_solver->beidou_dnav_ephemeris_map.cend()) { const uint32_t prn_aux = tmp_eph_iter_bds_dnav->second.PRN; - if ((prn_aux == in[i][epoch].PRN) && (((std::string(in[i][epoch].Signal) == std::string("B1")) || (std::string(in[i][epoch].Signal) == std::string("B3"))) && (tmp_eph_iter_bds_dnav->second.SV_health == 0))) + if ((prn_aux == in[i][epoch].PRN) && (((std::string(in[i][epoch].Signal, 2) == std::string("B1")) || (std::string(in[i][epoch].Signal, 2) == std::string("B3"))) && (d_use_unhealthy_sats || (tmp_eph_iter_bds_dnav->second.SV_health == 0)))) { store_valid_observable = true; } } - if (std::string(in[i][epoch].Signal) == std::string("E6")) + if (std::string(in[i][epoch].Signal, 2) == std::string("E6")) { store_valid_observable = true; } @@ -2123,6 +2094,12 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } } + // ############ 2. APPLY HAS CORRECTIONS IF AVAILABLE #### + if (d_use_has_corrections && !d_gnss_observables_map.empty()) + { + this->update_HAS_corrections(); + } + // ############ 2 COMPUTE THE PVT ################################ bool flag_pvt_valid = false; if (d_gnss_observables_map.empty() == false) @@ -2452,9 +2429,9 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6)); // std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time) << TEXT_RESET << '\n'; - DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_user_pvt_solver->get_position_UTC_time()) - << " UTC using " << d_user_pvt_solver->get_num_valid_observations() << " observations is Lat = " << d_user_pvt_solver->get_latitude() << " [deg], Long = " << d_user_pvt_solver->get_longitude() - << " [deg], Height = " << d_user_pvt_solver->get_height() << " [m]"; + LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_user_pvt_solver->get_position_UTC_time()) + << " UTC using " << d_user_pvt_solver->get_num_valid_observations() << " observations is Lat = " << d_user_pvt_solver->get_latitude() << " [deg], Long = " << d_user_pvt_solver->get_longitude() + << " [deg], Height = " << d_user_pvt_solver->get_height() << " [m]"; /* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_user_pvt_solver->get_position_UTC_time()) << " UTC using "<< d_user_pvt_solver->get_num_valid_observations() <<" observations is HDOP = " << d_user_pvt_solver->get_hdop() << " VDOP = " diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index cb8a2053b..94ffb458a 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -49,6 +49,7 @@ class Beidou_Dnav_Almanac; class Beidou_Dnav_Ephemeris; class Galileo_Almanac; class Galileo_Ephemeris; +class Galileo_HAS_data; class GeoJSON_Printer; class Gps_Almanac; class Gps_Ephemeris; @@ -140,13 +141,15 @@ private: void msg_handler_telemetry(const pmt::pmt_t& msg); - void msg_handler_has_data(const pmt::pmt_t& msg) const; + void msg_handler_has_data(const pmt::pmt_t& msg); void initialize_and_apply_carrier_phase_offset(); void apply_rx_clock_offset(std::map& observables_map, double rx_clock_offset_s); + void update_HAS_corrections(); + std::map interpolate_observables(const std::map& observables_map_t0, const std::map& observables_map_t1, double rx_time_s); @@ -160,7 +163,7 @@ private: typedef struct { - long mtype; // NOLINT(google-runtime-int) required by SysV queue messaging + long mtype; // NOLINT(google-runtime-int) double ttff; } d_ttff_msgbuf; bool send_sys_v_ttff_msg(d_ttff_msgbuf ttff) const; @@ -194,23 +197,6 @@ private: std::vector d_channel_initialized; std::vector d_initial_carrier_phase_offset_estimation_rads; - enum StringValue_ - { - evGPS_1C, - evGPS_2S, - evGPS_L5, - evSBAS_1C, - evGAL_1B, - evGAL_5X, - evGAL_E6, - evGAL_7X, - evGLO_1G, - evGLO_2G, - evBDS_B1, - evBDS_B2, - evBDS_B3 - }; - std::map d_mapStringValues; std::map d_gnss_observables_map; std::map d_gnss_observables_map_t0; std::map d_gnss_observables_map_t1; @@ -289,6 +275,8 @@ private: bool d_an_printer_enabled; bool d_log_timetag; bool d_use_e6_for_pvt; + bool d_use_has_corrections; + bool d_use_unhealthy_sats; }; diff --git a/src/algorithms/PVT/libs/pvt_conf.cc b/src/algorithms/PVT/libs/pvt_conf.cc deleted file mode 100644 index bf16f469e..000000000 --- a/src/algorithms/PVT/libs/pvt_conf.cc +++ /dev/null @@ -1,74 +0,0 @@ -/*! - * \file pvt_conf.cc - * \brief Class that contains all the configuration parameters for a PVT block - * \author Carles Fernandez, 2018. 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-2020 (see AUTHORS file for a list of contributors) - * SPDX-License-Identifier: GPL-3.0-or-later - * - * ----------------------------------------------------------------------------- - */ - -#include "pvt_conf.h" - -Pvt_Conf::Pvt_Conf() -{ - type_of_receiver = 0U; - observable_interval_ms = 20U; - output_rate_ms = 0; - display_rate_ms = 0; - kml_rate_ms = 1000; - gpx_rate_ms = 1000; - geojson_rate_ms = 1000; - nmea_rate_ms = 1000; - - max_obs_block_rx_clock_offset_ms = 40; - rinex_version = 0; - rinexobs_rate_ms = 0; - rinex_name = std::string("-"); - - dump = false; - dump_mat = true; - - flag_nmea_tty_port = false; - - flag_rtcm_server = false; - flag_rtcm_tty_port = false; - rtcm_tcp_port = 0U; - rtcm_station_id = 0U; - - output_enabled = true; - rinex_output_enabled = true; - gpx_output_enabled = true; - geojson_output_enabled = true; - nmea_output_file_enabled = true; - kml_output_enabled = true; - xml_output_enabled = true; - rtcm_output_file_enabled = true; - - output_path = std::string("."); - rinex_output_path = std::string("."); - gpx_output_path = std::string("."); - geojson_output_path = std::string("."); - nmea_output_file_path = std::string("."); - kml_output_path = std::string("."); - xml_output_path = std::string("."); - rtcm_output_file_path = std::string("."); - log_source_timetag_file = "PVT_timetag.dat"; - - enable_rx_clock_correction = true; - monitor_enabled = false; - monitor_ephemeris_enabled = false; - protobuf_enabled = true; - udp_port = 0; - udp_eph_port = 0; - pre_2009_file = false; - show_local_time_zone = false; - - log_source_timetag = false; -} diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index 34852c9c8..d77a0b24c 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -92,6 +92,8 @@ public: bool dump_mat = true; bool log_source_timetag; bool use_e6_for_pvt = true; + bool use_has_corrections = true; + bool use_unhealthy_sats = false; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 95aaa7576..a700e002b 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -4,7 +4,7 @@ * data flow and structures * \authors
    *
  • 2017-2019, Javier Arribas - *
  • 2017-2019, Carles Fernandez + *
  • 2017-2023, Carles Fernandez *
  • 2007-2013, T. Takasu *
* @@ -23,7 +23,7 @@ * ----------------------------------------------------------------------------- * Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2017-2019, Javier Arribas - * Copyright (C) 2017-2019, Carles Fernandez + * Copyright (C) 2017-2023, Carles Fernandez * All rights reserved. * * SPDX-License-Identifier: BSD-2-Clause @@ -33,11 +33,12 @@ #include "rtklib_solver.h" #include "Beidou_DNAV.h" #include "gnss_sdr_filesystem.h" -#include "rtklib_conversions.h" #include "rtklib_rtkpos.h" #include "rtklib_solution.h" #include #include +#include +#include #include #include #include @@ -130,6 +131,8 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, d_rtklib_freq_index[1] = 3; break; } + // auto empty_map = std::map < int, HAS_obs_corrections >> (); + // d_has_obs_corr_map["L1 C/A"] = empty_map; // ############# ENABLE DATA FILE LOG ################# if (d_flag_dump_enabled == true) @@ -457,6 +460,446 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const } +void Rtklib_Solver::store_has_data(const Galileo_HAS_data &new_has_data) +{ + // Compute time of application HAS SIS ICD, Issue 1.0, Section 7.7 + uint16_t toh = new_has_data.header.toh; + uint32_t hr = std::floor(new_has_data.tow / 3600); + uint32_t tmt = 0; + if ((hr * 3600 + toh) <= new_has_data.tow) + { + tmt = hr * 3600 + toh; + } + else + { + tmt = (hr - 1) * 3600 + toh; + } + + const std::string gps_str("GPS"); + const std::string gal_str("Galileo"); + if (new_has_data.header.orbit_correction_flag) + { + LOG(INFO) << "Received HAS orbit corrections"; + // for each satellite in GPS ephemeris + for (const auto &gpseph : gps_ephemeris_map) + { + int prn = gpseph.second.PRN; + int32_t sis_iod = gpseph.second.IODE_SF3; + uint16_t gnss_iod = new_has_data.get_gnss_iod(gps_str, prn); + if (static_cast(gnss_iod) == sis_iod) + { + float radial_m = new_has_data.get_delta_radial_m(gps_str, prn); + if (std::fabs(radial_m + 10.24) < 0.001) // -10.24 means not available + { + radial_m = 0.0; + } + float in_track_m = new_has_data.get_delta_in_track_m(gps_str, prn); + if (std::fabs(in_track_m + 16.384) < 0.001) // -16.384 means not available + { + in_track_m = 0.0; + } + float cross_track_m = new_has_data.get_delta_in_track_m(gps_str, prn); + if (std::fabs(cross_track_m + 16.384) < 0.001) // -16.384 means not available + { + cross_track_m = 0.0; + } + d_has_orbit_corrections_store_map[gps_str][prn].radial_m = radial_m; + d_has_orbit_corrections_store_map[gps_str][prn].in_track_m = in_track_m; + d_has_orbit_corrections_store_map[gps_str][prn].cross_track_m = cross_track_m; + d_has_orbit_corrections_store_map[gps_str][prn].valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_orbit_corrections); + d_has_orbit_corrections_store_map[gps_str][prn].iod = gnss_iod; + // TODO: check for end of week + } + } + + // for each satellite in Galileo ephemeris + for (const auto &galeph : galileo_ephemeris_map) + { + int prn = galeph.second.PRN; + int32_t sis_iod = galeph.second.IOD_ephemeris; + uint16_t gnss_iod = new_has_data.get_gnss_iod(gal_str, prn); + if (static_cast(gnss_iod) == sis_iod) + { + float radial_m = new_has_data.get_delta_radial_m(gal_str, prn); + if (std::fabs(radial_m + 10.24) < 0.001) // -10.24 means not available + { + radial_m = 0.0; + } + float in_track_m = new_has_data.get_delta_in_track_m(gal_str, prn); + if (std::fabs(in_track_m + 16.384) < 0.001) // -16.384 means not available + { + in_track_m = 0.0; + } + float cross_track_m = new_has_data.get_delta_in_track_m(gal_str, prn); + if (std::fabs(cross_track_m + 16.384) < 0.001) // -16.384 means not available + { + cross_track_m = 0.0; + } + d_has_orbit_corrections_store_map[gal_str][prn].radial_m = radial_m; + d_has_orbit_corrections_store_map[gal_str][prn].in_track_m = in_track_m; + d_has_orbit_corrections_store_map[gal_str][prn].cross_track_m = cross_track_m; + d_has_orbit_corrections_store_map[gal_str][prn].valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_orbit_corrections); + d_has_orbit_corrections_store_map[gal_str][prn].iod = gnss_iod; + // TODO: check for end of week + } + } + } + if (new_has_data.header.clock_fullset_flag) + { + LOG(INFO) << "Received HAS clock fullset corrections"; + for (const auto &gpseph : gps_ephemeris_map) + { + int prn = gpseph.second.PRN; + int32_t sis_iod = gpseph.second.IODE_SF3; + auto it = d_has_orbit_corrections_store_map[gps_str].find(prn); + if (it != d_has_orbit_corrections_store_map[gps_str].end()) + { + uint16_t gnss_iod = it->second.iod; + if (static_cast(gnss_iod) == sis_iod) + { + float clock_correction_mult_m = new_has_data.get_clock_correction_mult_m(gps_str, prn); + if ((std::fabs(clock_correction_mult_m + 10.24) < 0.001) || + (std::fabs(clock_correction_mult_m + 20.48) < 0.001) || + (std::fabs(clock_correction_mult_m + 30.72) < 0.001) || + (std::fabs(clock_correction_mult_m + 40.96) < 0.001)) + { + clock_correction_mult_m = 0.0; + } + if ((std::fabs(clock_correction_mult_m - 10.2375) < 0.001) || + (std::fabs(clock_correction_mult_m - 20.475) < 0.001) || + (std::fabs(clock_correction_mult_m - 30.7125) < 0.001) || + (std::fabs(clock_correction_mult_m - 40.95) < 0.001)) + { + // Satellite should not be used! + clock_correction_mult_m = 0.0; + } + d_has_clock_corrections_store_map[gps_str][prn].clock_correction_m = clock_correction_mult_m; + d_has_clock_corrections_store_map[gps_str][prn].valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_clock_fullset_corrections); + // TODO: check for end of week + } + } + } + + // for each satellite in Galileo ephemeris + for (const auto &galeph : galileo_ephemeris_map) + { + int prn = galeph.second.PRN; + int32_t iod_sis = galeph.second.IOD_ephemeris; + auto it = d_has_orbit_corrections_store_map[gal_str].find(prn); + if (it != d_has_orbit_corrections_store_map[gal_str].end()) + { + uint16_t gnss_iod = it->second.iod; + if (static_cast(gnss_iod) == iod_sis) + { + float clock_correction_mult_m = new_has_data.get_clock_correction_mult_m(gal_str, prn); + // std::cout << "Galileo Satellite " << prn + // << " clock correction=" << new_has_data.get_clock_correction_mult_m(gal_str, prn) + // << std::endl; + if ((std::fabs(clock_correction_mult_m + 10.24) < 0.001) || + (std::fabs(clock_correction_mult_m + 20.48) < 0.001) || + (std::fabs(clock_correction_mult_m + 30.72) < 0.001) || + (std::fabs(clock_correction_mult_m + 40.96) < 0.001)) + { + clock_correction_mult_m = 0.0; + } + d_has_clock_corrections_store_map[gal_str][prn].clock_correction_m = clock_correction_mult_m; + d_has_clock_corrections_store_map[gal_str][prn].valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_clock_fullset_corrections); + // TODO: check for end of week + } + } + } + } + if (new_has_data.header.clock_subset_flag) + { + LOG(INFO) << "Received HAS clock subset corrections"; + for (const auto &gpseph : gps_ephemeris_map) + { + int prn = gpseph.second.PRN; + int32_t sis_iod = gpseph.second.IODE_SF3; + int32_t gnss_iod = d_has_orbit_corrections_store_map[gps_str][prn].iod; + if (gnss_iod == sis_iod) + { + // d_has_clock_corrections_store_map[gps_str][prn].clock_correction_m = new_has_data.get_clock_subset_correction_mult_m(gps_str, prn); + // d_has_clock_corrections_store_map[gps_str][prn].valid_until = tmt + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_clock_subset_corrections); + // TODO: check for end of week + } + } + } + if (new_has_data.header.code_bias_flag) + { + LOG(INFO) << "Received HAS code bias corrections"; + uint32_t valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_code_bias_corrections); + auto signals_gal = new_has_data.get_signals_in_mask(gal_str); + for (const auto &it : signals_gal) + { + auto prns = new_has_data.get_PRNs_in_mask(gal_str); + for (auto prn : prns) + { + float code_bias_m = new_has_data.get_code_bias_m(it, prn); + if ((std::fabs(code_bias_m + 20.48) < 0.01)) // -20.48 means not available + { + code_bias_m = 0.0; + } + d_has_code_bias_store_map[it][prn] = {code_bias_m, valid_until}; + } + } + auto signals_gps = new_has_data.get_signals_in_mask(gps_str); + for (const auto &it : signals_gps) + { + auto prns = new_has_data.get_PRNs_in_mask(gps_str); + for (auto prn : prns) + { + float code_bias_m = new_has_data.get_code_bias_m(it, prn); + if ((std::fabs(code_bias_m + 20.48) < 0.01)) // -20.48 means not available + { + code_bias_m = 0.0; + } + d_has_code_bias_store_map[it][prn] = {code_bias_m, valid_until}; + } + } + } + if (new_has_data.header.phase_bias_flag) + { + LOG(INFO) << "Received HAS phase bias corrections"; + uint32_t valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_phase_bias_corrections); + + auto signals_gal = new_has_data.get_signals_in_mask(gal_str); + for (const auto &it : signals_gal) + { + auto prns = new_has_data.get_PRNs_in_mask(gal_str); + for (auto prn : prns) + { + float phase_bias_correction_cycles = new_has_data.get_phase_bias_cycle(it, prn); + if (std::fabs(phase_bias_correction_cycles + 10.24) < 0.001) // -10.24 means not available + { + phase_bias_correction_cycles = 0.0; + } + d_has_phase_bias_store_map[it][prn] = {phase_bias_correction_cycles, valid_until}; + // TODO: process Phase Discontinuity Indicator + } + } + auto signals_gps = new_has_data.get_signals_in_mask(gps_str); + for (const auto &it : signals_gps) + { + auto prns = new_has_data.get_PRNs_in_mask(gps_str); + for (auto prn : prns) + { + float phase_bias_correction_cycles = new_has_data.get_phase_bias_cycle(it, prn); + if (std::fabs(phase_bias_correction_cycles + 10.24) < 0.001) // -10.24 means not available + { + phase_bias_correction_cycles = 0.0; + } + d_has_phase_bias_store_map[it][prn] = {phase_bias_correction_cycles, valid_until}; + // TODO: process Phase Discontinuity Indicator + } + } + } +} + + +void Rtklib_Solver::update_has_corrections(const std::map &obs_map) +{ + this->check_has_orbit_clock_validity(obs_map); + this->get_has_biases(obs_map); +} + + +void Rtklib_Solver::check_has_orbit_clock_validity(const std::map &obs_map) +{ + for (const auto &it : obs_map) + { + uint32_t obs_tow = it.second.interp_TOW_ms / 1000.0; + auto prn = static_cast(it.second.PRN); + + if (it.second.System == 'G') + { + auto it_sys = d_has_orbit_corrections_store_map.find("GPS"); + if (it_sys != d_has_orbit_corrections_store_map.end()) + { + auto it_map_corr = it_sys->second.find(prn); + if (it_map_corr != it_sys->second.end()) + { + auto has_data_valid_until = it_map_corr->second.valid_until; + if (has_data_valid_until < obs_tow) + { + // Delete outdated data + it_sys->second.erase(prn); + } + } + } + auto it_sys_clock = d_has_clock_corrections_store_map.find("GPS"); + if (it_sys_clock != d_has_clock_corrections_store_map.end()) + { + auto it_map_corr = it_sys_clock->second.find(prn); + if (it_map_corr != it_sys_clock->second.end()) + { + auto has_data_valid_until = it_map_corr->second.valid_until; + if (has_data_valid_until < obs_tow) + { + // Delete outdated data + it_sys_clock->second.erase(prn); + } + } + } + } + if (it.second.System == 'E') + { + auto it_sys = d_has_orbit_corrections_store_map.find("Galileo"); + if (it_sys != d_has_orbit_corrections_store_map.end()) + { + auto it_map_corr = it_sys->second.find(prn); + if (it_map_corr != it_sys->second.end()) + { + auto has_data_valid_until = it_map_corr->second.valid_until; + if (has_data_valid_until < obs_tow) + { + // Delete outdated data + it_sys->second.erase(prn); + } + } + } + auto it_sys_clock = d_has_clock_corrections_store_map.find("Galileo"); + if (it_sys_clock != d_has_clock_corrections_store_map.end()) + { + auto it_map_corr = it_sys_clock->second.find(prn); + if (it_map_corr != it_sys_clock->second.end()) + { + auto has_data_valid_until = it_map_corr->second.valid_until; + if (has_data_valid_until < obs_tow) + { + // Delete outdated data + it_sys_clock->second.erase(prn); + } + } + } + } + } +} + + +void Rtklib_Solver::get_has_biases(const std::map &obs_map) +{ + d_has_obs_corr_map.clear(); + if (!d_has_clock_corrections_store_map.empty() && !d_has_orbit_corrections_store_map.empty()) + { + const std::vector e1b_signals = {"E1-B I/NAV OS", "E1-C", "E1-B + E1-C"}; + const std::vector e6_signals = {"E6-B C/NAV HAS", "E6-C", "E6-B + E6-C"}; + const std::vector e5_signals = {"E5a-I F/NAV OS", "E5a-Q", "E5a-I+E5a-Q"}; + const std::vector e7_signals = {"E5bI I/NAV OS", "E5b-Q", "E5b-I+E5b-Q"}; + const std::vector g1c_signals = {"L1 C/A"}; + const std::vector g2s_signals = {"L2 CM", "L2 CL", "L2 CM+CL", "L2 P"}; + const std::vector g5_signals = {"L5 I", "L5 Q", "L5 I + L5 Q"}; + + for (const auto &it : obs_map) + { + uint32_t obs_tow = it.second.interp_TOW_ms / 1000.0; + int prn = static_cast(it.second.PRN); + std::string sig(it.second.Signal, 2); + if (it.second.System == 'E') + { + auto it_sys_clock = d_has_clock_corrections_store_map.find("Galileo"); + if (it_sys_clock != d_has_clock_corrections_store_map.end()) + { + auto it_map_corr = it_sys_clock->second.find(prn); + if (it_map_corr != it_sys_clock->second.end()) + { + if (sig == "1B") + { + for (const auto &has_signal : e1b_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "E6") + { + for (const auto &has_signal : e6_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "5X") + { + for (const auto &has_signal : e5_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "7X") + { + for (const auto &has_signal : e7_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + } + } + } + if (it.second.System == 'G') + { + auto it_sys_clock = d_has_clock_corrections_store_map.find("GPS"); + if (it_sys_clock != d_has_clock_corrections_store_map.end()) + { + auto it_map_corr = it_sys_clock->second.find(prn); + if (it_map_corr != it_sys_clock->second.end()) + { + if (sig == "1C") + { + for (const auto &has_signal : g1c_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "2S") + { + for (const auto &has_signal : g2s_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "L5") + { + for (const auto &has_signal : g5_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + } + } + } + } + } +} + + +void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, uint32_t tow_obs, int prn) +{ + auto code_bias_pair_it = this->d_has_code_bias_store_map[signal].find(prn); + if (code_bias_pair_it != this->d_has_code_bias_store_map[signal].end()) + { + uint32_t valid_until = code_bias_pair_it->second.second; + if (valid_until > tow_obs) + { + this->d_has_obs_corr_map[signal][prn].code_bias_m = code_bias_pair_it->second.first; + } + } + auto phase_bias_pair_it = this->d_has_phase_bias_store_map[signal].find(prn); + if (phase_bias_pair_it != this->d_has_phase_bias_store_map[signal].end()) + { + uint32_t valid_until = phase_bias_pair_it->second.second; + if (valid_until > tow_obs) + { + this->d_has_obs_corr_map[signal][prn].phase_bias_cycle = phase_bias_pair_it->second.first; + } + } +} + + bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging) { std::map::const_iterator gnss_observables_iter; @@ -493,7 +936,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { case 'G': { - const std::string sig_(gnss_observables_iter->second.Signal); + const std::string sig_(gnss_observables_iter->second.Signal, 2); if (sig_ == "1C") { band1 = true; @@ -522,7 +965,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { case 'E': { - const std::string sig_(gnss_observables_iter->second.Signal); + const std::string gal_str("Galileo"); + const std::string sig_(gnss_observables_iter->second.Signal, 2); // Galileo E1 if (sig_ == "1B") { @@ -531,11 +975,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) { // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, + this->d_has_orbit_corrections_store_map[gal_str], + this->d_has_clock_corrections_store_map[gal_str]); // convert observation from GNSS-SDR class to RTKLIB structure obsd_t newobs{}; d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, + d_has_obs_corr_map, galileo_ephemeris_iter->second.WN, d_rtklib_band_index[sig_]); valid_obs++; @@ -560,6 +1007,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], gnss_observables_iter->second, + d_has_obs_corr_map, galileo_ephemeris_iter->second.WN, d_rtklib_band_index[sig_]); found_E1_obs = true; @@ -570,7 +1018,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { // insert Galileo E5 obs as new obs and also insert its ephemeris // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, + this->d_has_orbit_corrections_store_map[gal_str], + this->d_has_clock_corrections_store_map[gal_str]); // convert observation from GNSS-SDR class to RTKLIB structure const auto default_code_ = static_cast(CODE_NONE); obsd_t newobs = {{0, 0}, '0', '0', {}, {}, @@ -578,6 +1028,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ {}, {0.0, 0.0, 0.0}, {}}; d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, + d_has_obs_corr_map, galileo_ephemeris_iter->second.WN, d_rtklib_band_index[sig_]); valid_obs++; @@ -600,6 +1051,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], gnss_observables_iter->second, + d_has_obs_corr_map, galileo_ephemeris_iter->second.WN, d_rtklib_band_index[sig_]); found_E1_obs = true; @@ -610,7 +1062,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { // insert Galileo E6 obs as new obs and also insert its ephemeris // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, + this->d_has_orbit_corrections_store_map[gal_str], + this->d_has_clock_corrections_store_map[gal_str]); // convert observation from GNSS-SDR class to RTKLIB structure const auto default_code_ = static_cast(CODE_NONE); obsd_t newobs = {{0, 0}, '0', '0', {}, {}, @@ -618,6 +1072,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ {}, {0.0, 0.0, 0.0}, {}}; d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, + d_has_obs_corr_map, galileo_ephemeris_iter->second.WN, d_rtklib_band_index[sig_]); valid_obs++; @@ -675,18 +1130,23 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { // GPS L1 // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key - const std::string sig_(gnss_observables_iter->second.Signal); + const std::string gps_str("GPS"); + const std::string sig_(gnss_observables_iter->second.Signal, 2); if (sig_ == "1C") { gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); if (gps_ephemeris_iter != gps_ephemeris_map.cend()) { // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second, this->is_pre_2009()); + eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second, + this->d_has_orbit_corrections_store_map[gps_str], + this->d_has_clock_corrections_store_map[gps_str], + this->is_pre_2009()); // convert observation from GNSS-SDR class to RTKLIB structure obsd_t newobs{}; d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, + d_has_obs_corr_map, gps_ephemeris_iter->second.WN, d_rtklib_band_index[sig_], this->is_pre_2009()); @@ -783,6 +1243,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ {}, {0.0, 0.0, 0.0}, {}}; d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, + d_has_obs_corr_map, gps_cnav_ephemeris_iter->second.WN, d_rtklib_band_index[sig_]); valid_obs++; diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 22d20ba2f..0a7fc056b 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -4,7 +4,7 @@ * data flow and structures * \authors
    *
  • 2017, Javier Arribas - *
  • 2017, Carles Fernandez + *
  • 2017-2023, Carles Fernandez *
  • 2007-2013, T. Takasu *
* @@ -23,7 +23,7 @@ * ----------------------------------------------------------------------------- * Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2017-2019, Javier Arribas - * Copyright (C) 2017-2019, Carles Fernandez + * Copyright (C) 2017-2023, Carles Fernandez * All rights reserved. * * SPDX-License-Identifier: BSD-2-Clause @@ -41,6 +41,7 @@ #include "beidou_dnav_utc_model.h" #include "galileo_almanac.h" #include "galileo_ephemeris.h" +#include "galileo_has_data.h" #include "galileo_iono.h" #include "galileo_utc_model.h" #include "glonass_gnav_almanac.h" @@ -57,11 +58,13 @@ #include "monitor_pvt.h" #include "pvt_solution.h" #include "rtklib.h" +#include "rtklib_conversions.h" #include #include #include #include #include +#include /** \addtogroup PVT * \{ */ @@ -91,6 +94,8 @@ public: double get_pdop() const override; double get_gdop() const override; Monitor_Pvt get_monitor_pvt() const; + void store_has_data(const Galileo_HAS_data& new_has_data); + void update_has_corrections(const std::map& obs_map); sol_t pvt_sol{}; std::array pvt_ssat{}; @@ -122,10 +127,23 @@ public: private: bool save_matfile() const; + void check_has_orbit_clock_validity(const std::map& obs_map); + void get_has_biases(const std::map& obs_map); + void get_current_has_obs_correction(const std::string& signal, uint32_t tow_obs, int prn); + std::array d_obs_data{}; std::array d_dop{}; std::map d_rtklib_freq_index; std::map d_rtklib_band_index; + + std::map> d_has_orbit_corrections_store_map; // first key is system, second key is PRN + std::map> d_has_clock_corrections_store_map; // first key is system, second key is PRN + + std::map>> d_has_code_bias_store_map; // first key is signal, second key is PRN + std::map>> d_has_phase_bias_store_map; // first key is signal, second key is PRN + + std::map> d_has_obs_corr_map; // first key is signal, second key is PRN + std::string d_dump_filename; std::ofstream d_dump_file; rtk_t d_rtk{}; diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 064d7aa0b..b470dca83 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -4,7 +4,7 @@ * \authors
    *
  • 2007-2013, T. Takasu *
  • 2017, Javier Arribas - *
  • 2017, Carles Fernandez + *
  • 2017-2023, Carles Fernandez *
* * This is a derived work from RTKLIB http://www.rtklib.com/ @@ -22,7 +22,7 @@ * ----------------------------------------------------------------------------- * Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2017, Javier Arribas - * Copyright (C) 2017, Carles Fernandez + * Copyright (C) 2017-2023, Carles Fernandez * All rights reserved. * * SPDX-License-Identifier: BSD-2-Clause @@ -442,15 +442,20 @@ typedef struct /* SV orbit parameters */ double A, e, i0, OMG0, omg, M0, deln, OMGd, idot; double crc, crs, cuc, cus, cic, cis; - double toes; /* Toe (s) in week */ - double fit; /* fit interval (h) */ - double f0, f1, f2; /* SV clock parameters (af0,af1,af2) */ - double tgd[4]; /* group delay parameters */ - /* GPS/QZS:tgd[0]=TGD */ - /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ - /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ - double isc[4]; /* GPS :isc[0]=ISCL1, isc[1]=ISCL2, isc[2]=ISCL5I, isc[3]=ISCL5Q */ - double Adot, ndot; /* Adot,ndot for CNAV */ + double toes; /* Toe (s) in week */ + double fit; /* fit interval (h) */ + double f0, f1, f2; /* SV clock parameters (af0,af1,af2) */ + double tgd[4]; /* group delay parameters */ + /* GPS/QZS:tgd[0]=TGD */ + /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ + /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ + double isc[4]; /* GPS :isc[0]=ISCL1, isc[1]=ISCL2, isc[2]=ISCL5I, isc[3]=ISCL5Q */ + double Adot, ndot; /* Adot,ndot for CNAV */ + float has_clock_correction_m; /* Galileo High Accuracy Service clock correction, in [m] */ + float has_orbit_radial_correction_m; /* Galileo High Accuracy Service orbit radial correction, in [m] */ + float has_orbit_in_track_correction_m; /* Galileo High Accuracy Service orbit in-track correction, in [m] */ + float has_orbit_cross_track_correction_m; /* Galileo High Accuracy Service orbit cross-track correction, in [m] */ + bool apply_has_corrections; } eph_t; diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index b7e50519c..e8082b729 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -28,13 +28,17 @@ #include "gps_ephemeris.h" // for Gps_Ephemeris #include "rtklib_rtkcmn.h" #include -#include -#include -obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, const Gnss_Synchro& gnss_synchro, int week, int band, bool pre_2009_file) + +obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, + const Gnss_Synchro& gnss_synchro, + const std::map>& has_obs_corr, + int week, + int band, + bool pre_2009_file) { // Get signal type info to adjust code type based on constellation - std::string sig_ = gnss_synchro.Signal; + const std::string sig_(gnss_synchro.Signal, 2); rtklib_obs.D[band] = gnss_synchro.Carrier_Doppler_hz; rtklib_obs.P[band] = gnss_synchro.Pseudorange_m; @@ -127,10 +131,275 @@ obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, const Gnss_Synchro& gnss_synchro } rtklib_obs.rcv = 1; + + if (!has_obs_corr.empty()) + { + float has_pseudorange_correction_m = 0.0; + float has_bias_correction_cycle = 0.0; + switch (gnss_synchro.System) + { + case 'G': + { + if (sig_ == "1C") + { + const auto it = has_obs_corr.find("L1 C/A"); + if (it != has_obs_corr.cend()) + { + const auto it2 = it->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + } + else if (sig_ == "2S") + { + const auto it = has_obs_corr.find("L2 CM"); + if (it != has_obs_corr.cend()) + { + const auto it2 = it->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + } + else if (sig_ == "L5") + { + // TODO: determine which one + const auto it = has_obs_corr.find("L5 I"); + if (it != has_obs_corr.cend()) + { + const auto it2 = it->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_2nd_attempt = has_obs_corr.find("L5 Q"); + if (it_2nd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_2nd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_2nd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_3rd_attempt = has_obs_corr.find("L5 I + L5 Q"); + if (it_3rd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_3rd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_3rd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + } + } + } + } + break; + case 'E': + { + if (sig_ == "1B") + { + // TODO: determine which one + const auto it = has_obs_corr.find("E1-B I/NAV OS"); + if (it != has_obs_corr.cend()) + { + const auto it2 = it->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_2nd_attempt = has_obs_corr.find("E1-C"); + if (it_2nd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_2nd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_2nd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_3rd_attempt = has_obs_corr.find("E1-B + E1-C"); + if (it_3rd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_3rd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_3rd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + } + } + } + else if (sig_ == "5X") + { + // TODO: determine which one + const auto it = has_obs_corr.find("E5a-I F/NAV OS"); + if (it != has_obs_corr.cend()) + { + const auto it2 = it->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_2nd_attempt = has_obs_corr.find("E5a-Q"); + if (it_2nd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_2nd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_2nd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_3rd_attempt = has_obs_corr.find("E5a-I+E5a-Q"); + if (it_3rd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_3rd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_3rd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + } + } + } + + else if (sig_ == "7X") + { + // TODO: determine which one + const auto it = has_obs_corr.find("E5bI I/NAV OS"); + if (it != has_obs_corr.cend()) + { + const auto it2 = it->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_2nd_attempt = has_obs_corr.find("E5b-Q"); + if (it_2nd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_2nd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_2nd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_3rd_attempt = has_obs_corr.find("E5b-I+E5b-Q"); + if (it_3rd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_3rd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_3rd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + } + } + } + else if (sig_ == "6B") + { + // TODO: determine which one + const auto it = has_obs_corr.find("E6-B C/NAV HAS"); + if (it != has_obs_corr.cend()) + { + const auto it2 = it->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_2nd_attempt = has_obs_corr.find("E6-C"); + if (it_2nd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_2nd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_2nd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + else + { + const auto it_3rd_attempt = has_obs_corr.find("E6-B + E6-C"); + if (it_3rd_attempt != has_obs_corr.cend()) + { + const auto it2 = it_3rd_attempt->second.find(static_cast(gnss_synchro.PRN)); + if (it2 != it_3rd_attempt->second.cend()) + { + has_pseudorange_correction_m = it2->second.code_bias_m; + has_bias_correction_cycle = it2->second.phase_bias_cycle; + } + } + } + } + } + } + break; + default: + break; + } + + rtklib_obs.P[band] += has_pseudorange_correction_m; + rtklib_obs.L[band] += has_bias_correction_cycle; + } return rtklib_obs; } +obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, + const Gnss_Synchro& gnss_synchro, + int week, + int band, + bool pre_2009_file) +{ + std::map> empty_map; + return insert_obs_to_rtklib(rtklib_obs, + gnss_synchro, + empty_map, + week, + band, + pre_2009_file); +} + + geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glonass_Gnav_Utc_Model& gnav_clock_model) { int week; @@ -172,9 +441,19 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glona eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph) +{ + std::map empty_orbit_map; + std::map empty_clock_map; + return eph_to_rtklib(gal_eph, empty_orbit_map, empty_clock_map); +} + + +eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph, + const std::map& orbit_correction_map, + const std::map& clock_correction_map) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; // Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris rtklib_sat.sat = gal_eph.PRN + NSATGPS + NSATGLO; rtklib_sat.A = gal_eph.sqrtA * gal_eph.sqrtA; @@ -226,14 +505,54 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph) rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); + if (!orbit_correction_map.empty() && !clock_correction_map.empty()) + { + int count_has_corrections = 0; + const auto it_orbit = orbit_correction_map.find(static_cast(gal_eph.PRN)); + if (it_orbit != orbit_correction_map.cend()) + { + rtklib_sat.has_orbit_radial_correction_m = it_orbit->second.radial_m; + rtklib_sat.has_orbit_in_track_correction_m = it_orbit->second.in_track_m; + rtklib_sat.has_orbit_cross_track_correction_m = it_orbit->second.cross_track_m; + count_has_corrections++; + } + + const auto it_clock = clock_correction_map.find(static_cast(gal_eph.PRN)); + if (it_clock != clock_correction_map.cend()) + { + rtklib_sat.has_clock_correction_m = it_clock->second.clock_correction_m; + count_has_corrections++; + } + rtklib_sat.apply_has_corrections = (count_has_corrections == 2) ? true : false; + if (rtklib_sat.apply_has_corrections) + { + rtklib_sat.tgd[0] = 0.0; + rtklib_sat.tgd[1] = 0.0; + } + } + else + { + rtklib_sat.apply_has_corrections = false; + } return rtklib_sat; } eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph, bool pre_2009_file) +{ + std::map empty_orbit_map; + std::map empty_clock_map; + return eph_to_rtklib(gps_eph, empty_orbit_map, empty_clock_map, pre_2009_file); +} + + +eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph, + const std::map& orbit_correction_map, + const std::map& clock_correction_map, + bool pre_2009_file) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; rtklib_sat.sat = gps_eph.PRN; rtklib_sat.A = gps_eph.sqrtA * gps_eph.sqrtA; rtklib_sat.M0 = gps_eph.M_0; @@ -284,6 +603,40 @@ eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph, bool pre_2009_file) rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); + if (!orbit_correction_map.empty() && !clock_correction_map.empty()) + { + int count_has_corrections = 0; + const auto it_orbit = orbit_correction_map.find(static_cast(gps_eph.PRN)); + if (it_orbit != orbit_correction_map.cend()) + { + rtklib_sat.has_orbit_radial_correction_m = it_orbit->second.radial_m; + rtklib_sat.has_orbit_in_track_correction_m = it_orbit->second.in_track_m; + rtklib_sat.has_orbit_cross_track_correction_m = it_orbit->second.cross_track_m; + count_has_corrections++; + } + + const auto it_clock = clock_correction_map.find(static_cast(gps_eph.PRN)); + if (it_clock != clock_correction_map.cend()) + { + rtklib_sat.has_clock_correction_m = it_clock->second.clock_correction_m; + count_has_corrections++; + } + rtklib_sat.apply_has_corrections = (count_has_corrections == 2) ? true : false; + if (rtklib_sat.apply_has_corrections) + { + rtklib_sat.tgd[0] = 0.0; + rtklib_sat.tgd[1] = 0.0; + } + } + else + { + rtklib_sat.has_orbit_radial_correction_m = 0.0; + rtklib_sat.has_orbit_in_track_correction_m = 0.0; + rtklib_sat.has_orbit_cross_track_correction_m = 0.0; + rtklib_sat.has_clock_correction_m = 0.0; + rtklib_sat.apply_has_corrections = false; + } + return rtklib_sat; } @@ -291,7 +644,7 @@ eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph, bool pre_2009_file) eph_t eph_to_rtklib(const Beidou_Dnav_Ephemeris& bei_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; rtklib_sat.sat = bei_eph.PRN + NSATGPS + NSATGLO + NSATGAL + NSATQZS; rtklib_sat.A = bei_eph.sqrtA * bei_eph.sqrtA; rtklib_sat.M0 = bei_eph.M_0; @@ -353,6 +706,12 @@ eph_t eph_to_rtklib(const Beidou_Dnav_Ephemeris& bei_eph) rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); + rtklib_sat.has_orbit_radial_correction_m = 0.0; + rtklib_sat.has_orbit_in_track_correction_m = 0.0; + rtklib_sat.has_orbit_cross_track_correction_m = 0.0; + rtklib_sat.has_clock_correction_m = 0.0; + rtklib_sat.apply_has_corrections = false; + return rtklib_sat; } @@ -360,7 +719,7 @@ eph_t eph_to_rtklib(const Beidou_Dnav_Ephemeris& bei_eph) eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; rtklib_sat.sat = gps_cnav_eph.PRN; rtklib_sat.A = gps_cnav_eph.sqrtA * gps_cnav_eph.sqrtA; rtklib_sat.M0 = gps_cnav_eph.M_0; @@ -415,6 +774,12 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph) rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); + rtklib_sat.has_orbit_radial_correction_m = 0.0; + rtklib_sat.has_orbit_in_track_correction_m = 0.0; + rtklib_sat.has_orbit_cross_track_correction_m = 0.0; + rtklib_sat.has_clock_correction_m = 0.0; + rtklib_sat.apply_has_corrections = false; + return rtklib_sat; } diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h index d3109002e..84b077b6f 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -18,6 +18,9 @@ #define GNSS_SDR_RTKLIB_CONVERSIONS_H #include "rtklib.h" +#include +#include +#include /** \addtogroup PVT * \{ */ @@ -35,8 +38,48 @@ class Gps_Almanac; class Gps_CNAV_Ephemeris; class Gps_Ephemeris; +class HAS_clock_corrections +{ +public: + HAS_clock_corrections() = default; + float clock_correction_m{}; + uint32_t valid_until{}; +}; + +class HAS_orbit_corrections +{ +public: + HAS_orbit_corrections() = default; + float radial_m{}; + float in_track_m{}; + float cross_track_m{}; + uint32_t valid_until{}; + uint16_t iod{}; +}; + +class HAS_obs_corrections +{ +public: + HAS_obs_corrections() = default; + float code_bias_m{}; + float phase_bias_cycle{}; +}; + + eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph); -eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph, bool pre_2009_file); + +eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph, + const std::map& orbit_correction_map, + const std::map& clock_correction_map); + +eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph, + bool pre_2009_file = false); + +eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph, + const std::map& orbit_correction_map, + const std::map& clock_correction_map, + bool pre_2009_file = false); + eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph); eph_t eph_to_rtklib(const Beidou_Dnav_Ephemeris& bei_eph); @@ -50,6 +93,13 @@ alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm); */ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glonass_Gnav_Utc_Model& gnav_clock_model); +obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, + const Gnss_Synchro& gnss_synchro, + const std::map>& has_obs_corr, + int week, + int band, + bool pre_2009_file = false); + obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, const Gnss_Synchro& gnss_synchro, int week, int band, bool pre_2009_file = false); diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 6b095bdcd..3bd092a7a 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -4,7 +4,7 @@ * \authors
    *
  • 2007-2013, T. Takasu *
  • 2017, Javier Arribas - *
  • 2017, Carles Fernandez + *
  • 2017-2023, Carles Fernandez *
* * This is a derived work from RTKLIB http://www.rtklib.com/ @@ -22,7 +22,7 @@ * ----------------------------------------------------------------------------- * Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2017, Javier Arribas - * Copyright (C) 2017, Carles Fernandez + * Copyright (C) 2017-2023, Carles Fernandez * All rights reserved. * * SPDX-License-Identifier: BSD-2-Clause @@ -231,6 +231,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, int sys; int prn; + double has_relativistic_correction = 0.0; trace(4, "eph2pos : time=%s sat=%2d\n", time_str(time, 3), eph->sat); if (eph->A <= 0.0) @@ -307,12 +308,102 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, rs[0] = x * cosO - y * cosi * sinO; rs[1] = x * sinO + y * cosi * cosO; rs[2] = y * sin(i); + // Apply HAS orbit correction if available + if (eph->apply_has_corrections) + { + // HAS SIS ICD, Issue 1.0, Section 7.2 + double vel_sat[3]{}; + double cross_pos_vel[3]{}; + double et[3]{}; + double ew[3]{}; + double en[3]{}; + double R[3][3]{}; + double corrections[3]{}; + double rotated_corrections[3]{}; + // Compute satellite velocity + const double OneMinusecosE = 1.0 - (eph->e * cosE); + const double ekdot = (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) / OneMinusecosE; + const double pkdot = sqrt(1.0 - eph->e * eph->e) * ekdot / OneMinusecosE; + const double ukdot = pkdot * (1.0 + 2.0 * (eph->cus * cos2u - eph->cuc * sin2u)); + const double ikdot = eph->idot + 2.0 * pkdot * (eph->cis * cos2u - eph->cic * sin2u); + const double rkdot = eph->A * eph->e * sinE * ekdot + 2.0 * pkdot * (eph->crs * cos2u - eph->crc * sin2u); + const double xpkdot = rkdot * cos(u) - y * ukdot; + const double ypkdot = rkdot * sin(u) + x * ukdot; + const double tmp = ypkdot * cosi - rs[2] * ikdot; + + vel_sat[0] = -(eph->OMGd - omge) * rs[1] + xpkdot * cosO - tmp * sinO; + vel_sat[1] = (eph->OMGd - omge) * rs[0] + xpkdot * sinO + tmp * cosO; + vel_sat[2] = y * cosi * ikdot + ypkdot * sin(i); + + // Compute HAS relativistic clock correction (HAS SIS ICD, Issue 1.0, Section 7.3) + const double pos_by_vel = rs[0] * vel_sat[0] + rs[1] * vel_sat[1] + rs[2] * vel_sat[2]; + has_relativistic_correction = -(2.0 * pos_by_vel) / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); + + // Compute rotation matrix + const double norm_velocity = sqrt(vel_sat[0] * vel_sat[0] + vel_sat[1] * vel_sat[1] + vel_sat[2] * vel_sat[2]); + et[0] = vel_sat[0] / norm_velocity; + et[1] = vel_sat[1] / norm_velocity; + et[2] = vel_sat[2] / norm_velocity; + + cross_pos_vel[0] = rs[1] * vel_sat[2] - rs[2] * vel_sat[1]; + cross_pos_vel[1] = rs[2] * vel_sat[0] - rs[0] * vel_sat[2]; + cross_pos_vel[2] = rs[0] * vel_sat[1] - rs[1] * vel_sat[0]; + const double norm_cross_pos_vel = sqrt(cross_pos_vel[0] * cross_pos_vel[0] + cross_pos_vel[1] * cross_pos_vel[1] + cross_pos_vel[2] * cross_pos_vel[2]); + + ew[0] = cross_pos_vel[0] / norm_cross_pos_vel; + ew[1] = cross_pos_vel[1] / norm_cross_pos_vel; + ew[2] = cross_pos_vel[2] / norm_cross_pos_vel; + + en[0] = et[1] * ew[2] - et[2] * ew[1]; + en[1] = et[2] * ew[0] - et[0] * ew[2]; + en[2] = et[0] * ew[1] - et[1] * ew[0]; + + R[0][0] = en[0]; + R[0][1] = et[0]; + R[0][2] = ew[0]; + + R[1][0] = en[1]; + R[1][1] = et[1]; + R[1][2] = ew[1]; + + R[2][0] = en[2]; + R[2][1] = et[2]; + R[2][2] = ew[2]; + + // Compute rotated corrections + corrections[0] = eph->has_orbit_radial_correction_m; + corrections[1] = eph->has_orbit_in_track_correction_m; + corrections[2] = eph->has_orbit_cross_track_correction_m; + + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 3; col++) + { + rotated_corrections[row] = R[row][col] * corrections[col]; + } + } + + // Apply HAS orbit corrections + rs[0] += rotated_corrections[0]; + rs[1] += rotated_corrections[1]; + rs[2] += rotated_corrections[2]; + } } tk = timediffweekcrossover(time, eph->toc); *dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk; /* relativity correction */ - *dts -= 2.0 * sqrt(mu * eph->A) * eph->e * sinE / std::pow(SPEED_OF_LIGHT_M_S, 2.0); + if (eph->apply_has_corrections) + { + // Apply HAS clock correction (HAS SIS ICD, Issue 1.0, Section 7.3) + *dts += (has_relativistic_correction + (eph->has_clock_correction_m / SPEED_OF_LIGHT_M_S)); + // Note: This is referred to the GST for Galileo satellites. The user must account for + // a possible common offset in the broadcast HAS GPS clock corrections + } + else + { + *dts -= 2.0 * sqrt(mu * eph->A) * eph->e * sinE / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); + } /* position and clock error variance */ *var = var_uraeph(eph->sva); diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm.cc b/src/algorithms/libs/rtklib/rtklib_rtcm.cc index d67199292..db431f513 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtcm.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtcm.cc @@ -49,7 +49,7 @@ int init_rtcm(rtcm_t *rtcm) obsd_t data0 = {{0, 0.0}, 0, 0, {0}, {0}, {0}, {0.0}, {0.0}, {0.0}}; eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; geph_t geph0 = {0, -1, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0.0}, {0.0}, {0.0}, 0.0, 0.0, 0.0}; ssr_t ssr0 = {{{0, 0.0}}, {0.0}, {0}, 0, 0, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, {0.0}, {0.0}, {0.0}, 0.0, 0.0, '0'}; diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm2.cc b/src/algorithms/libs/rtklib/rtklib_rtcm2.cc index 9ae477aff..b07bdd226 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtcm2.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtcm2.cc @@ -228,7 +228,7 @@ int decode_type17(rtcm_t *rtcm, bool pre_2009_file) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; double toc; double sqrtA; int i = 48; diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm3.cc b/src/algorithms/libs/rtklib/rtklib_rtcm3.cc index 6f2f7b204..afed31c1f 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtcm3.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtcm3.cc @@ -1019,7 +1019,7 @@ int decode_type1019(rtcm_t *rtcm, bool pre_2009_file) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; double toc; double sqrtA; char *msg; @@ -1518,7 +1518,7 @@ int decode_type1044(rtcm_t *rtcm, bool pre_2009_file) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; double toc; double sqrtA; char *msg; @@ -1631,7 +1631,7 @@ int decode_type1045(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; double toc; double sqrtA; char *msg; @@ -1745,7 +1745,7 @@ int decode_type1046(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; double toc; double sqrtA; char *msg; @@ -1859,7 +1859,7 @@ int decode_type1047(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; double toc; double sqrtA; char *msg; @@ -1976,7 +1976,7 @@ int decode_type63(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; double toc; double sqrtA; char *msg; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 40a271ece..b4074abb8 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -3610,7 +3610,7 @@ int readnav(const char *file, nav_t *nav) { FILE *fp; eph_t eph0 = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; geph_t geph0 = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {}, {}, {}, 0.0, 0.0, 0.0}; char buff[4096]; char *p; diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index 975a61f86..22e66eadb 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -681,7 +681,7 @@ int rtksvrinit(rtksvr_t *svr) '0', '0', '0', 0, 0, 0}; eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false}; geph_t geph0 = {0, -1, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0.0}, {0.0}, {0.0}, 0.0, 0.0, 0.0}; seph_t seph0 = {0, {0, 0.0}, {0, 0.0}, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, 0.0}; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index b7f2c56be..6fb36d92c 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -100,19 +100,6 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) d_channel_last_pseudorange_smooth = std::vector(d_nchannels_out, 0.0); d_channel_last_carrier_phase_rads = std::vector(d_nchannels_out, 0.0); - d_mapStringValues["1C"] = evGPS_1C; - d_mapStringValues["2S"] = evGPS_2S; - d_mapStringValues["L5"] = evGPS_L5; - d_mapStringValues["1B"] = evGAL_1B; - d_mapStringValues["5X"] = evGAL_5X; - d_mapStringValues["E6"] = evGAL_E6; - d_mapStringValues["7X"] = evGAL_7X; - d_mapStringValues["1G"] = evGLO_1G; - d_mapStringValues["2G"] = evGLO_2G; - d_mapStringValues["B1"] = evBDS_B1; - d_mapStringValues["B2"] = evBDS_B2; - d_mapStringValues["B3"] = evBDS_B3; - d_SourceTagTimestamps = std::vector>(d_nchannels_out); set_tag_propagation_policy(TPP_DONT); // no tag propagation, the time tag will be adjusted and regenerated in work() @@ -581,44 +568,11 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector &data) if (it->Flag_valid_pseudorange) { // 0. get wavelength for the current signal - double wavelength_m = 0; - switch (d_mapStringValues[it->Signal]) + double wavelength_m = 0.0; + const auto it_freq_map = SIGNAL_FREQ_MAP.find(std::string(it->Signal, 2)); + if (it_freq_map != SIGNAL_FREQ_MAP.cend()) { - case evGPS_1C: - case evSBAS_1C: - case evGAL_1B: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1; - break; - case evGPS_L5: - case evGAL_5X: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ5; - break; - case evGAL_E6: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ6; - break; - case evGAL_7X: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ7; - break; - case evGPS_2S: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2; - break; - case evBDS_B3: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ3_BDS; - break; - case evGLO_1G: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_GLO; - break; - case evGLO_2G: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_GLO; - break; - case evBDS_B1: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_BDS; - break; - case evBDS_B2: - wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_BDS; - break; - default: - break; + wavelength_m = SPEED_OF_LIGHT_M_S / it_freq_map->second; } // todo: propagate the PLL lock status in Gnss_Synchro diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index 5c4863c7c..96a83b733 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -29,12 +29,11 @@ #include // for size_t #include // for int32_t #include // for std::ofstream -#include // for std::map #include // for std::shared, std:unique_ptr -#include -#include // for std::string -#include // for typeid -#include // for std::vector +#include // for std::queue +#include // for std::string +#include // for typeid +#include // for std::vector /** \addtogroup Observables * \{ */ @@ -84,24 +83,6 @@ private: Obs_Conf d_conf; - enum StringValue_ - { - evGPS_1C, - evGPS_2S, - evGPS_L5, - evSBAS_1C, - evGAL_1B, - evGAL_5X, - evGAL_E6, - evGAL_7X, - evGLO_1G, - evGLO_2G, - evBDS_B1, - evBDS_B2, - evBDS_B3 - }; - std::map d_mapStringValues; - std::unique_ptr> d_gnss_synchro_history; // Tracking observable history boost::circular_buffer d_Rx_clock_buffer; // time history diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index a18bc23cc..22e1cd524 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -149,7 +149,7 @@ std::shared_ptr galileo_e6_has_msg_receiver::process_test_page { d_HAS_data.has_status = d_current_has_status; d_HAS_data.message_id = d_current_message_id; - d_HAS_data.tow = tow - static_cast(std::remainder(tow, 3600)) + d_HAS_data.header.toh; + d_HAS_data.tow = tow; auto has_data_ptr = std::make_shared(d_HAS_data); d_new_message = false; d_printed_mids[d_current_message_id] = true; @@ -201,7 +201,7 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m { d_HAS_data.has_status = d_current_has_status; d_HAS_data.message_id = d_current_message_id; - d_HAS_data.tow = tow - static_cast(std::remainder(tow, 3600)) + d_HAS_data.header.toh; + d_HAS_data.tow = tow; d_printed_mids[d_current_message_id] = true; d_printed_timestamps[d_current_message_id] = timestamp; auto has_data_ptr = std::make_shared(d_HAS_data); @@ -699,8 +699,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) // count satellites in the mask std::bitset 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'); + int number_sats_this_gnss_id = satellite_mask_bits.count(); 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()); diff --git a/src/core/system_parameters/galileo_has_data.cc b/src/core/system_parameters/galileo_has_data.cc index 447d9faef..5168e33bf 100644 --- a/src/core/system_parameters/galileo_has_data.cc +++ b/src/core/system_parameters/galileo_has_data.cc @@ -18,372 +18,75 @@ #include "Galileo_CNAV.h" #include #include +#include +#include #include #include - - -std::vector Galileo_HAS_data::get_PRNs_in_mask(uint8_t nsys) const -{ - std::vector prn; - if (nsys < satellite_mask.size()) - { - uint64_t sat_mask = satellite_mask[nsys]; - std::bitset bits(sat_mask); - std::string bit_str = bits.to_string(); - for (int32_t i = 0; i < HAS_MSG_NUMBER_SATELLITE_IDS; i++) - { - if (bit_str[i] == '1') - { - prn.push_back(i + 1); - } - } - } - return prn; -} - - -std::vector Galileo_HAS_data::get_PRNs_in_submask(uint8_t nsys) const -{ - std::vector prn; - std::vector prn_in_submask; - if (nsys < satellite_submask.size()) - { - auto it = std::find(gnss_id_mask.begin(), gnss_id_mask.end(), gnss_id_clock_subset[nsys]); - int index = 0; - if (it != gnss_id_mask.end()) - { - index = it - gnss_id_mask.begin(); - } - else - { - return prn_in_submask; - } - - uint8_t nsys_index_in_mask = gnss_id_mask[index]; - uint64_t sat_mask = satellite_mask[nsys_index_in_mask]; - - std::bitset bits(sat_mask); - std::string mask_bit_str = bits.to_string(); - for (int i = 0; i < HAS_MSG_NUMBER_SATELLITE_IDS; i++) - { - if (mask_bit_str[i] == '1') - { - prn.push_back(i + 1); - } - } - - int number_sats_this_gnss_id = std::count(mask_bit_str.begin(), mask_bit_str.end(), '1'); - uint64_t sat_submask = satellite_submask[nsys]; - // convert into string - std::string sat_submask_str(""); - sat_submask_str.reserve(number_sats_this_gnss_id); - uint64_t aux = 1; - const std::string one_str("1"); - const std::string zero_str("0"); - - for (int k = 0; k < number_sats_this_gnss_id - 1; k++) - { - if ((aux & sat_submask) >= 1) - { - sat_submask_str.insert(0, one_str); - } - else - { - sat_submask_str.insert(0, zero_str); - } - aux <<= 1; - } - - for (int i = 0; i < number_sats_this_gnss_id; i++) - { - if (sat_submask_str[i] == '1') - { - prn_in_submask.push_back(prn[i]); - } - } - } - - return prn_in_submask; -} +#include +#include std::vector Galileo_HAS_data::get_signals_in_mask(uint8_t nsys) const { - std::vector signals_in_mask; - if (signal_mask.size() > nsys) + if (signal_mask.size() <= nsys) { - uint16_t sig = signal_mask[nsys]; - std::bitset bits(sig); - std::string bits_str = bits.to_string(); - for (int32_t k = 0; k < HAS_MSG_NUMBER_SIGNAL_MASKS; k++) + return {}; + } + // See HAS SIS ICD v1.0 Table 20 + std::unordered_map> signal_index_table = { + {0, { + {0, "L1 C/A"}, + {1, "Reserved"}, + {2, "Reserved"}, + {3, "L1C(D)"}, + {4, "L1C(P)"}, + {5, "L1C(D+P)"}, + {6, "L2 CM"}, + {7, "L2 CL"}, + {8, "L2 CM+CL"}, + {9, "L2 P"}, + {10, "Reserved"}, + {11, "L5 I"}, + {12, "L5 Q"}, + {13, "L5 I + L5 Q"}, + {14, "Reserved"}, + {15, "Reserved"}, + }}, + {2, { + {0, "E1-B I/NAV OS"}, + {1, "E1-C"}, + {2, "E1-B + E1-C"}, + {3, "E5a-I F/NAV OS"}, + {4, "E5a-Q"}, + {5, "E5a-I+E5a-Q"}, + {6, "E5b-I I/NAV OS"}, + {7, "E5b-Q"}, + {8, "E5b-I+E5b-Q"}, + {9, "E5-I"}, + {10, "E5-Q"}, + {11, "E5-I + E5-Q"}, + {12, "E6-B C/NAV HAS"}, + {13, "E6-C"}, + {14, "E6-B + E6-C"}, + {15, "Reserved"}, + }}}; + + std::vector signals_in_mask; + uint16_t sig = signal_mask[nsys]; + uint8_t system = gnss_id_mask[nsys]; + std::bitset bits(sig); + + for (int32_t k = 0; k < HAS_MSG_NUMBER_SIGNAL_MASKS; k++) + { + if ((bits[HAS_MSG_NUMBER_SIGNAL_MASKS - k - 1])) { - if (bits_str[k] == '1') + if (signal_index_table[system].count(k) == 0) { - uint8_t system = gnss_id_mask[nsys]; - std::string signal; - // See HAS SIS ICD v1.0 Table 20 - switch (k) - { - case 0: - if (system == 0) - { - // GPS - signal = "L1 C/A"; - } - else if (system == 2) - { - // Galileo - signal = "E1-B I/NAV OS"; - } - else - { - signal = "Unknown"; - } - break; - case 1: - if (system == 0) - { - // GPS - signal = "Reserved"; - } - else if (system == 2) - { - // Galileo - signal = "E1-C"; - } - else - { - signal = "Unknown"; - } - break; - case 2: - if (system == 0) - { - // GPS - signal = "Reserved"; - } - else if (system == 2) - { - // Galileo - signal = "E1-B + E1-C"; - } - else - { - signal = "Unknown"; - } - break; - case 3: - if (system == 0) - { - // GPS - signal = "L1C(D)"; - } - else if (system == 2) - { - // Galileo - signal = "E5a-I F/NAV OS"; - } - else - { - signal = "Unknown"; - } - break; - case 4: - if (system == 0) - { - // GPS - signal = "L1C(P)"; - } - else if (system == 2) - { - // Galileo - signal = "E5a-Q"; - } - else - { - signal = "Unknown"; - } - break; - case 5: - if (system == 0) - { - // GPS - signal = "L1C(D+P)"; - } - else if (system == 2) - { - // Galileo - signal = "E5a-I+E5a-Q"; - } - else - { - signal = "Unknown"; - } - break; - case 6: - if (system == 0) - { - // GPS - signal = "L2 CM"; - } - else if (system == 2) - { - // Galileo - signal = "E5b-I I/NAV OS"; - } - else - { - signal = "Unknown"; - } - break; - case 7: - if (system == 0) - { - // GPS - signal = "L2 CL"; - } - else if (system == 2) - { - // Galileo - signal = "E5b-Q"; - } - else - { - signal = "Unknown"; - } - break; - case 8: - if (system == 0) - { - // GPS - signal = "L2 CM+CL"; - } - else if (system == 2) - { - // Galileo - signal = "E5b-I+E5b-Q"; - } - else - { - signal = "Unknown"; - } - break; - case 9: - if (system == 0) - { - // GPS - signal = "L2 P"; - } - else if (system == 2) - { - // Galileo - signal = "E5-I"; - } - else - { - signal = "Unknown"; - } - break; - case 10: - if (system == 0) - { - // GPS - signal = "Reserved"; - } - else if (system == 2) - { - // Galileo - signal = "E5-Q"; - } - else - { - signal = "Unknown"; - } - break; - case 11: - if (system == 0) - { - // GPS - signal = "L5 I"; - } - else if (system == 2) - { - // Galileo - signal = "E5-I + E5-Q"; - } - else - { - signal = "Unknown"; - } - break; - case 12: - if (system == 0) - { - // GPS - signal = "L5 Q"; - } - else if (system == 2) - { - // Galileo - signal = "E6-B C/NAV HAS"; - } - else - { - signal = "Unknown"; - } - break; - case 13: - if (system == 0) - { - // GPS - signal = "L5 I + L5 Q"; - } - else if (system == 2) - { - // Galileo - signal = "E6-C"; - } - else - { - signal = "Unknown"; - } - break; - case 14: - if (system == 0) - { - // GPS - signal = "Reserved"; - } - else if (system == 2) - { - // Galileo - signal = "E6-B + E6-C"; - } - else - { - signal = "Unknown"; - } - break; - case 15: - if (system == 0) - { - // GPS - signal = "Reserved"; - } - else if (system == 2) - { - // Galileo - signal = "Reserved"; - } - else - { - signal = "Unknown"; - } - break; - default: - signal = "Unknown"; - } - signals_in_mask.push_back(signal); + signals_in_mask.emplace_back("Unknown"); + } + else + { + signals_in_mask.push_back(signal_index_table[system][k]); } } } @@ -391,77 +94,139 @@ std::vector Galileo_HAS_data::get_signals_in_mask(uint8_t nsys) con } -uint8_t Galileo_HAS_data::get_gnss_id(int nsat) const +std::vector Galileo_HAS_data::get_signals_in_mask(const std::string& system) const { - int number_sats = 0; - for (uint8_t i = 0; i < Nsys; i++) + auto systems = this->get_systems_string(); + auto sys_it = std::find(systems.begin(), systems.end(), system); + if (sys_it == systems.end()) { - number_sats += static_cast(get_PRNs_in_mask(i).size()); - if (nsat < number_sats) - { - return gnss_id_mask[i]; - } + return {}; } - - return HAS_MSG_WRONG_SYSTEM; + auto system_index = std::distance(systems.begin(), sys_it); + return this->get_signals_in_mask(system_index); } -uint16_t Galileo_HAS_data::get_validity_interval_s(uint8_t validity_interval_index) const +std::vector Galileo_HAS_data::get_systems_string() const { - uint16_t validity_interval; - // See HAS SIS ICD v1.0 Table 23 - switch (validity_interval_index) + if (this->gnss_id_mask.empty()) { - 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 {}; } - return validity_interval; + + std::vector systems; + systems.reserve(this->gnss_id_mask.size()); + + for (uint8_t i = 0; i < this->Nsys; i++) + { + switch (this->gnss_id_mask[i]) + { + case 0: + systems.emplace_back("GPS"); + break; + case 2: + systems.emplace_back("Galileo"); + break; + default: + systems.emplace_back("Reserved"); + } + } + + return systems; +} + + +std::vector Galileo_HAS_data::get_systems_subset_string() const +{ + if (this->gnss_id_clock_subset.empty()) + { + return {}; + } + + std::vector systems; + systems.reserve(this->gnss_id_clock_subset.size()); + + for (uint8_t i = 0; i < this->Nsys_sub; i++) + { + switch (this->gnss_id_clock_subset[i]) + { + case 0: + systems.emplace_back("GPS"); + break; + case 2: + systems.emplace_back("Galileo"); + break; + default: + systems.emplace_back("Reserved"); + } + } + + return systems; +} + + +std::vector> Galileo_HAS_data::get_code_bias_m() const +{ + if (code_bias.empty()) + { + return {}; + } + const size_t outer_size = this->code_bias.size(); + const size_t inner_size = this->code_bias[0].size(); + std::vector> code_bias_m(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 +{ + if (phase_bias.empty()) + { + return {}; + } + const size_t outer_size = this->phase_bias.size(); + const size_t inner_size = this->phase_bias[0].size(); + std::vector> phase_bias_cycle(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_delta_clock_subset_correction_m() const +{ + if (this->delta_clock_correction_clock_subset.empty()) + { + return {}; + } + + std::vector> delta_clock_subset_correction_m; + delta_clock_subset_correction_m.reserve(this->Nsys_sub); + + for (const auto& correction_subset : this->delta_clock_correction_clock_subset) + { + std::vector correction_m; + correction_m.reserve(correction_subset.size()); + for (const auto& d : correction_subset) + { + correction_m.push_back(static_cast(d) * HAS_MSG_DELTA_CLOCK_SCALE_FACTOR); + } + delta_clock_subset_correction_m.emplace_back(std::move(correction_m)); + } + + return delta_clock_subset_correction_m; } @@ -479,19 +244,20 @@ std::vector Galileo_HAS_data::get_delta_radial_m() const 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; + return {}; } + std::vector delta_orbit_radial_m_v = this->get_delta_radial_m(); + std::vector num_satellites = this->get_num_satellites(); std::vector delta_orbit_radial_m_aux; - uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + uint8_t num_sats_in_this_system = 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]; + uint8_t num_sats_in_system = num_satellites[sys]; if (sys != nsys) { index += num_sats_in_system; @@ -500,7 +266,7 @@ std::vector Galileo_HAS_data::get_delta_radial_m(uint8_t nsys) const { for (uint8_t sat = 0; sat < num_sats_in_system; sat++) { - delta_orbit_radial_m_aux.push_back(delta_orbit_radial_m[index]); + delta_orbit_radial_m_aux.push_back(delta_orbit_radial_m_v[index]); index++; } } @@ -523,19 +289,20 @@ std::vector Galileo_HAS_data::get_delta_in_track_m() const std::vector Galileo_HAS_data::get_delta_in_track_m(uint8_t nsys) const { - std::vector delta_in_track_m = this->get_delta_in_track_m(); if (nsys >= this->Nsys) { - return delta_in_track_m; + return {}; } + std::vector delta_in_track_m_v = this->get_delta_in_track_m(); + std::vector num_satellites = this->get_num_satellites(); std::vector delta_in_track_m_aux; - uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + uint8_t num_sats_in_this_system = num_satellites[nsys]; delta_in_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]; + uint8_t num_sats_in_system = num_satellites[sys]; if (sys != nsys) { index += num_sats_in_system; @@ -544,7 +311,7 @@ std::vector Galileo_HAS_data::get_delta_in_track_m(uint8_t nsys) const { for (uint8_t sat = 0; sat < num_sats_in_system; sat++) { - delta_in_track_m_aux.push_back(delta_in_track_m[index]); + delta_in_track_m_aux.push_back(delta_in_track_m_v[index]); index++; } } @@ -567,19 +334,20 @@ std::vector Galileo_HAS_data::get_delta_cross_track_m() const 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; + return {}; } + std::vector delta_cross_track_m_v = this->get_delta_cross_track_m(); std::vector delta_cross_track_m_aux; - uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + std::vector num_satellites = this->get_num_satellites(); + uint8_t num_sats_in_this_system = 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]; + uint8_t num_sats_in_system = num_satellites[sys]; if (sys != nsys) { index += num_sats_in_system; @@ -588,7 +356,7 @@ std::vector Galileo_HAS_data::get_delta_cross_track_m(uint8_t nsys) const { for (uint8_t sat = 0; sat < num_sats_in_system; sat++) { - delta_cross_track_m_aux.push_back(delta_cross_track_m[index]); + delta_cross_track_m_aux.push_back(delta_cross_track_m_v[index]); index++; } } @@ -611,19 +379,20 @@ std::vector Galileo_HAS_data::get_delta_clock_correction_m() const std::vector Galileo_HAS_data::get_delta_clock_correction_m(uint8_t nsys) const { - std::vector delta_clock_correction_m = this->get_delta_clock_correction_m(); if (nsys >= this->Nsys) { - return delta_clock_correction_m; + return {}; } + std::vector delta_clock_correction_m_v = this->get_delta_clock_correction_m(); std::vector delta_clock_correction_m_aux; - uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + std::vector num_satellites = this->get_num_satellites(); + uint8_t num_sats_in_this_system = num_satellites[nsys]; delta_clock_correction_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]; + uint8_t num_sats_in_system = num_satellites[sys]; if (sys != nsys) { index += num_sats_in_system; @@ -632,7 +401,7 @@ std::vector Galileo_HAS_data::get_delta_clock_correction_m(uint8_t nsys) { for (uint8_t sat = 0; sat < num_sats_in_system; sat++) { - delta_clock_correction_m_aux.push_back(delta_clock_correction_m[index]); + delta_clock_correction_m_aux.push_back(delta_clock_correction_m_v[index]); index++; } } @@ -641,85 +410,143 @@ std::vector Galileo_HAS_data::get_delta_clock_correction_m(uint8_t nsys) } -std::vector> Galileo_HAS_data::get_code_bias_m() const +std::vector Galileo_HAS_data::get_delta_clock_subset_correction_m(uint8_t nsys) const { - std::vector> code_bias_m; - const size_t outer_size = this->code_bias.size(); - if (outer_size == 0) + if (nsys >= this->Nsys) { - return code_bias_m; + return {}; } - 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++) + + // get equivalence of nsys in mask with nsys_sub_index in submask + auto gnss_id_in_mask = this->gnss_id_mask[nsys]; + auto sys_it = std::find(gnss_id_clock_subset.begin(), gnss_id_clock_subset.end(), gnss_id_in_mask); + if (sys_it == gnss_id_clock_subset.end()) { - for (size_t j = 0; j < inner_size; j++) + return {}; + } + uint64_t nsys_sub_index = std::distance(gnss_id_clock_subset.begin(), sys_it); + if (nsys_sub_index >= satellite_submask.size()) + { + return {}; + } + + auto subset_corr_matrix = this->get_delta_clock_subset_correction_m(); + std::vector delta_clock_subset_correction_m_v = subset_corr_matrix[nsys_sub_index]; + std::vector delta_clock_subset_correction_m_aux; + std::vector num_satellites_subset = this->get_num_subset_satellites(); + uint8_t num_sats_in_this_system_subset = num_satellites_subset[nsys_sub_index]; + delta_clock_subset_correction_m_aux.reserve(num_sats_in_this_system_subset); + size_t index = 0; + for (uint8_t sys = 0; sys <= nsys; sys++) + { + uint8_t num_sats_in_subset = num_satellites_subset[sys]; + if (sys != nsys) { - code_bias_m[i][j] = static_cast(this->code_bias[i][j]) * HAS_MSG_CODE_BIAS_SCALE_FACTOR; + index += num_sats_in_subset; + } + else + { + for (uint8_t sat = 0; sat < num_sats_in_subset; sat++) + { + delta_clock_subset_correction_m_aux.push_back(delta_clock_subset_correction_m_v[index]); + index++; + } } } - return code_bias_m; + return delta_clock_subset_correction_m_aux; } -std::vector> Galileo_HAS_data::get_phase_bias_cycle() const +std::vector Galileo_HAS_data::get_PRNs_in_mask(uint8_t nsys) const { - std::vector> phase_bias_cycle; - const size_t outer_size = this->phase_bias.size(); - if (outer_size == 0) + if (nsys >= satellite_mask.size()) { - return phase_bias_cycle; + return {}; } - 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++) + std::vector prn; + auto sat_mask = satellite_mask[nsys]; + std::bitset bits(sat_mask); + + for (int32_t i = 0; i < HAS_MSG_NUMBER_SATELLITE_IDS; i++) { - for (size_t j = 0; j < inner_size; j++) + if ((bits[HAS_MSG_NUMBER_SATELLITE_IDS - i - 1])) { - phase_bias_cycle[i][j] = static_cast(this->phase_bias[i][j]) * HAS_MSG_PHASE_BIAS_SCALE_FACTOR; + prn.push_back(i + 1); } } - return phase_bias_cycle; + return prn; } -std::vector Galileo_HAS_data::get_num_satellites() const +std::vector Galileo_HAS_data::get_PRNs_in_mask(const std::string& system) const { - std::vector num_satellites; - if (this->Nsys == 0) + auto systems = this->get_systems_string(); + auto sys_it = std::find(systems.begin(), systems.end(), system); + if (sys_it == systems.end()) { - return num_satellites; + return {}; // system not found } - num_satellites.reserve(this->Nsys); - for (uint8_t i = 0; i < this->Nsys; i++) + auto system_index = std::distance(systems.begin(), sys_it); + return this->get_PRNs_in_mask(system_index); +} + + +std::vector Galileo_HAS_data::get_PRNs_in_submask(uint8_t nsys) const +{ + // nsys refers to the system index in the main mask + if (nsys >= this->Nsys) { - 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 {}; } - return num_satellites; + // get equivalence of nsys in mask with nsys_sub_index in submask + auto gnss_id_in_mask = this->gnss_id_mask[nsys]; + auto sys_it = std::find(gnss_id_clock_subset.begin(), gnss_id_clock_subset.end(), gnss_id_in_mask); + if (sys_it == gnss_id_clock_subset.end()) + { + return {}; + } + uint64_t nsys_sub_index = std::distance(gnss_id_clock_subset.begin(), sys_it); + if (nsys_sub_index >= satellite_submask.size()) + { + return {}; + } + + auto mask_prns = this->get_PRNs_in_mask(nsys); + + std::vector prn_in_submask; + auto sat_submask = this->satellite_submask[nsys_sub_index]; + int count = 0; + while (sat_submask) + { + if (sat_submask & 1) + { + prn_in_submask.push_back(mask_prns[count]); + count++; + } + sat_submask >>= 1; + } + + return prn_in_submask; } 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; + return {}; } + std::vector gnss_iod_aux; - uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; + std::vector num_satellites = this->get_num_satellites(); + uint8_t num_sats_in_this_system = 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]; + uint8_t num_sats_in_system = num_satellites[sys]; if (sys != nsys) { index += num_sats_in_system; @@ -728,7 +555,7 @@ std::vector Galileo_HAS_data::get_gnss_iod(uint8_t nsys) const { for (uint8_t sat = 0; sat < num_sats_in_system; sat++) { - gnss_iod_aux.push_back(gnss_iod_v[index]); + gnss_iod_aux.push_back(this->gnss_iod[index]); index++; } } @@ -737,6 +564,348 @@ std::vector Galileo_HAS_data::get_gnss_iod(uint8_t nsys) const } +std::vector Galileo_HAS_data::get_num_satellites() const +{ + if (this->Nsys == 0) + { + return {}; + } + std::vector num_satellites; + num_satellites.reserve(this->Nsys); + for (uint8_t i = 0; i < this->Nsys; i++) + { + uint64_t sat_mask = this->satellite_mask[i]; + int count = 0; + while (sat_mask) + { + count += sat_mask & 1; + sat_mask >>= 1; + } + num_satellites.push_back(count); + } + + return num_satellites; +} + + +std::vector Galileo_HAS_data::get_num_subset_satellites() const +{ + if (this->Nsys_sub == 0) + { + return {}; + } + std::vector num_satellites_subset; + num_satellites_subset.reserve(this->Nsys_sub); + + for (uint8_t i = 0; i < this->Nsys_sub; i++) + { + uint64_t sat_submask = this->satellite_submask[i]; + int count = 0; + while (sat_submask) + { + count += sat_submask & 1; + sat_submask >>= 1; + } + num_satellites_subset.push_back(count); + } + return num_satellites_subset; +} + + +float Galileo_HAS_data::get_code_bias_m(const std::string& signal, int PRN) const +{ + float code_bias_correction_m = 0.0; + const size_t outer_size = this->code_bias.size(); + if (outer_size == 0) + { + return code_bias_correction_m; + } + const size_t inner_size = this->code_bias[0].size(); + if (inner_size == 0) + { + return code_bias_correction_m; + } + + std::string targeted_system; + if ((signal == "L1 C/A") || (signal == "L1C(D)") || (signal == "L1C(P)" || (signal == "L1C(D+P)") || (signal == "L2 CM") || (signal == "L2 CL") || (signal == "L2 CM+CL") || (signal == "L2 P") || (signal == "L5 I")) || + (signal == "L5 Q") || (signal == "L5 I + L5 Q")) + { + targeted_system = std::string("GPS"); + } + if ((signal == "E1-B I/NAV OS") || (signal == "E1-C") || (signal == "E1-B + E1-C") || (signal == "E5a-I F/NAV OS") || (signal == "E5a-Q") || (signal == "E5a-I+E5a-Q") || (signal == "E5b-I I/NAV OS") || + (signal == "E5b-Q") || (signal == "E5b-I+E5b-Q") || (signal == "E5-I") || (signal == "E5-Q") || (signal == "E5-I + E5-Q") || (signal == "E6-B C/NAV HAS") || (signal == "E6-C") || (signal == "E6-B + E6-C")) + { + targeted_system = std::string("Galileo"); + } + + if (!code_bias.empty() && !targeted_system.empty()) + { + std::vector systems = this->get_systems_string(); + auto Nsys = systems.size(); + auto nsys_index = std::distance(systems.cbegin(), std::find(systems.cbegin(), systems.cend(), targeted_system)); + + if (static_cast(nsys_index) < Nsys) + { + std::vector signals = get_signals_in_mask(static_cast(nsys_index)); + auto sig_index = std::distance(signals.cbegin(), std::find(signals.cbegin(), signals.cend(), signal)); + + if (static_cast(sig_index) < signals.size()) + { + int sat_index = 0; + bool index_found = false; + for (int s = 0; s < static_cast(Nsys); s++) + { + std::vector PRNs_in_mask = get_PRNs_in_mask(s); + if (s == nsys_index) + { + if (index_found == false) + { + auto sati = std::distance(PRNs_in_mask.cbegin(), std::find(PRNs_in_mask.cbegin(), PRNs_in_mask.cend(), PRN)); + if (static_cast(sati) < PRNs_in_mask.size()) + { + sat_index += sati; + index_found = true; + } + } + } + else + { + if (index_found == false) + { + sat_index += PRNs_in_mask.size(); + } + } + } + if (index_found == true) + { + code_bias_correction_m = static_cast(this->code_bias[sat_index][sig_index]) * HAS_MSG_CODE_BIAS_SCALE_FACTOR; + } + } + } + } + + return code_bias_correction_m; +} + + +float Galileo_HAS_data::get_phase_bias_cycle(const std::string& signal, int PRN) const +{ + float phase_bias_correction_cycles = 0.0; + const size_t outer_size = this->phase_bias.size(); + if (outer_size == 0) + { + return phase_bias_correction_cycles; + } + const size_t inner_size = this->phase_bias[0].size(); + if (inner_size == 0) + { + return phase_bias_correction_cycles; + } + + std::string targeted_system; + if ((signal == "L1 C/A") || (signal == "L1C(D)") || (signal == "L1C(P)" || (signal == "L1C(D+P)") || (signal == "L2 CM") || (signal == "L2 CL") || (signal == "L2 CM+CL") || (signal == "L2 P") || (signal == "L5 I")) || + (signal == "L5 Q") || (signal == "L5 I + L5 Q")) + { + targeted_system = std::string("GPS"); + } + if ((signal == "E1-B I/NAV OS") || (signal == "E1-C") || (signal == "E1-B + E1-C") || (signal == "E5a-I F/NAV OS") || (signal == "E5a-Q") || (signal == "E5a-I+E5a-Q") || (signal == "E5b-I I/NAV OS") || + (signal == "E5b-Q") || (signal == "E5b-I+E5b-Q") || (signal == "E5-I") || (signal == "E5-Q") || (signal == "E5-I + E5-Q") || (signal == "E6-B C/NAV HAS") || (signal == "E6-C") || (signal == "E6-B + E6-C")) + { + targeted_system = std::string("Galileo"); + } + + if (!phase_bias.empty() && !targeted_system.empty()) + { + std::vector systems = this->get_systems_string(); + auto Nsys = systems.size(); + auto nsys_index = std::distance(systems.cbegin(), std::find(systems.cbegin(), systems.cend(), targeted_system)); + + if (static_cast(nsys_index) < Nsys) + { + std::vector signals = get_signals_in_mask(static_cast(nsys_index)); + auto sig_index = std::distance(signals.cbegin(), std::find(signals.cbegin(), signals.cend(), signal)); + + if (static_cast(sig_index) < signals.size()) + { + int sat_index = 0; + bool index_found = false; + for (int s = 0; s < static_cast(Nsys); s++) + { + std::vector PRNs_in_mask = get_PRNs_in_mask(s); + if (s == nsys_index) + { + if (index_found == false) + { + auto sati = std::distance(PRNs_in_mask.cbegin(), std::find(PRNs_in_mask.cbegin(), PRNs_in_mask.cend(), PRN)); + if (static_cast(sati) < PRNs_in_mask.size()) + { + sat_index += sati; + index_found = true; + } + } + } + else + { + if (index_found == false) + { + sat_index += PRNs_in_mask.size(); + } + } + } + if (index_found == true) + { + phase_bias_correction_cycles = static_cast(this->phase_bias[sat_index][sig_index]) * HAS_MSG_PHASE_BIAS_SCALE_FACTOR; + } + } + } + } + + return phase_bias_correction_cycles; +} + + +float Galileo_HAS_data::get_delta_radial_m(const std::string& system, int prn) const +{ + if (this->delta_radial.empty()) + { + return 0.0; + } + auto systems = this->get_systems_string(); + auto sys_it = std::find(systems.begin(), systems.end(), system); + if (sys_it == systems.end()) + { + return 0.0; // system not found + } + auto system_index = std::distance(systems.begin(), sys_it); + + auto prns = this->get_PRNs_in_mask(system_index); + auto it = std::find(prns.begin(), prns.end(), prn); + if (it == prns.end()) + { + return 0.0; // PRN not found + } + + auto radial_m_v = this->get_delta_radial_m(system_index); + return radial_m_v[std::distance(prns.begin(), it)]; +} + + +float Galileo_HAS_data::get_delta_in_track_m(const std::string& system, int prn) const +{ + if (this->delta_in_track.empty()) + { + return 0.0; + } + auto systems = this->get_systems_string(); + auto sys_it = std::find(systems.begin(), systems.end(), system); + if (sys_it == systems.end()) + { + return 0.0; // system not found + } + auto system_index = std::distance(systems.begin(), sys_it); + + auto prns = this->get_PRNs_in_mask(system_index); + auto it = std::find(prns.begin(), prns.end(), prn); + if (it == prns.end()) + { + return 0.0; // PRN not found + } + + auto in_track_m_v = this->get_delta_in_track_m(system_index); + return in_track_m_v[std::distance(prns.begin(), it)]; +} + + +float Galileo_HAS_data::get_delta_cross_track_m(const std::string& system, int prn) const +{ + if (this->delta_cross_track.empty()) + { + return 0.0; + } + auto systems = this->get_systems_string(); + auto sys_it = std::find(systems.begin(), systems.end(), system); + if (sys_it == systems.end()) + { + return 0.0; // system not found + } + auto system_index = std::distance(systems.begin(), sys_it); + + auto prns = this->get_PRNs_in_mask(system_index); + auto it = std::find(prns.begin(), prns.end(), prn); + if (it == prns.end()) + { + return 0.0; // PRN not found + } + + auto cross_track_m_v = this->get_delta_cross_track_m(system_index); + return cross_track_m_v[std::distance(prns.begin(), it)]; +} + + +float Galileo_HAS_data::get_clock_correction_mult_m(const std::string& system, int prn) const +{ + if (this->delta_clock_correction.empty()) + { + return 0.0; + } + auto systems = this->get_systems_string(); + auto sys_it = std::find(systems.begin(), systems.end(), system); + if (sys_it == systems.end()) + { + return 0.0; // system not found + } + auto system_index = std::distance(systems.begin(), sys_it); + + auto prns = this->get_PRNs_in_mask(system_index); + auto it = std::find(prns.begin(), prns.end(), prn); + if (it == prns.end()) + { + return 0.0; // PRN not found + } + + auto index = std::distance(prns.begin(), it); + auto clock_correction_m_v = this->get_delta_clock_correction_m(system_index); + auto dcm = static_cast(this->delta_clock_multiplier[system_index]); + return clock_correction_m_v[index] * dcm; +} + + +float Galileo_HAS_data::get_clock_subset_correction_mult_m(const std::string& system, int prn) const +{ + if (this->gnss_id_clock_subset.empty()) + { + return 0.0; + } + + auto systems = this->get_systems_string(); + auto sys_it = std::find(systems.begin(), systems.end(), system); + if (sys_it == systems.end()) + { + return 0.0; // system not found + } + auto system_index = std::distance(systems.begin(), sys_it); + + auto prns_subset = this->get_PRNs_in_submask(system_index); + auto it = std::find(prns_subset.begin(), prns_subset.end(), prn); + if (it == prns_subset.end()) + { + return 0.0; // PRN not found + } + + auto index_subset = std::distance(prns_subset.begin(), it); + auto clock_correction_m_v = this->get_delta_clock_subset_correction_m(system_index); + auto dcm = static_cast(this->delta_clock_multiplier_clock_subset[system_index]); + if (!clock_correction_m_v.empty()) + { + return clock_correction_m_v[index_subset] * dcm; + } + else + { + return 0.0; + } +} + + uint16_t Galileo_HAS_data::get_nsat() const { std::vector num_satellites = this->get_num_satellites(); @@ -745,29 +914,80 @@ uint16_t Galileo_HAS_data::get_nsat() const } -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_nsat_sub() const { auto Nsat_sub = static_cast(this->delta_clock_correction_clock_subset.size()); return Nsat_sub; } + + +uint16_t Galileo_HAS_data::get_validity_interval_s(uint8_t validity_interval_index) const +{ + // See HAS SIS ICD v1.0 Table 23 + const std::map validity_intervals = { + {0, 5}, + {1, 10}, + {2, 15}, + {3, 20}, + {4, 30}, + {5, 60}, + {6, 90}, + {7, 120}, + {8, 180}, + {9, 240}, + {10, 300}, + {11, 600}, + {12, 900}, + {13, 1800}, + {14, 3600}}; + + auto it = validity_intervals.find(validity_interval_index); + if (it == validity_intervals.end()) + { + return 0; + } + return it->second; +} + + +uint16_t Galileo_HAS_data::get_gnss_iod(const std::string& system, int prn) const +{ + if (this->gnss_iod.empty()) + { + return 65535; // not found + } + auto systems = this->get_systems_string(); + auto sys_it = std::find(systems.begin(), systems.end(), system); + if (sys_it == systems.end()) + { + return 65535; // not found + } + auto system_index = std::distance(systems.begin(), sys_it); + + auto prns = this->get_PRNs_in_mask(system_index); + auto it = std::find(prns.begin(), prns.end(), prn); + if (it == prns.end()) + { + return 65535; // PRN not found + } + + auto gnss_iod_v = this->get_gnss_iod(system_index); + return gnss_iod_v[std::distance(prns.begin(), it)]; +} + + +uint8_t Galileo_HAS_data::get_gnss_id(int nsat) const +{ + int number_sats = 0; + for (uint8_t i = 0; i < Nsys; i++) + { + auto prns = get_PRNs_in_mask(i); + number_sats += static_cast(prns.size()); + if (nsat < number_sats) + { + return gnss_id_mask[i]; + } + } + + return HAS_MSG_WRONG_SYSTEM; +} diff --git a/src/core/system_parameters/galileo_has_data.h b/src/core/system_parameters/galileo_has_data.h index 79b50eaeb..b69eeff82 100644 --- a/src/core/system_parameters/galileo_has_data.h +++ b/src/core/system_parameters/galileo_has_data.h @@ -52,26 +52,40 @@ class Galileo_HAS_data public: Galileo_HAS_data() = default; - 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_in_track_m() const; //!< Get Nsat delta in-track corrections in [m] - std::vector get_delta_in_track_m(uint8_t nsys) const; //!< Get delta in-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_correction_m() const; //!< Get Nsat delta clock C0 corrections in [m] - std::vector get_delta_clock_correction_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_nsat_sub() const; //!< Get number of satellites in clock subset corrections - uint16_t get_validity_interval_s(uint8_t validity_interval_index) const; //!< Get validity interval in [s] from the validity_interval_index - uint8_t get_gnss_id(int nsat) const; //!< Get GNSS ID from the nsat satellite + 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_signals_in_mask(const std::string& system) const; //!< Get a vector of Nsys std::string with signals in mask for system ("GPS"/"Galileo") + std::vector get_systems_string() const; //!< Get Nsys system name strings + std::vector get_systems_subset_string() const; //!< Get Nsat system name strings present in clock corrections subset + 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_clock_subset_correction_m() const; //!< Get Nsys_sub vectors with Nsat_sub delta clock C0 corrections in [m] + 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_in_track_m() const; //!< Get Nsat delta in-track corrections in [m] + std::vector get_delta_in_track_m(uint8_t nsys) const; //!< Get delta in-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_correction_m() const; //!< Get Nsat delta clock C0 corrections in [m] + std::vector get_delta_clock_correction_m(uint8_t nsys) const; //!< Get delta clock C0 corrections in [m] for system nsys, with 0 <= nsys < Nsys + std::vector get_delta_clock_subset_correction_m(uint8_t nsys) const; //!< Get delta clock C0 subset 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_mask(const std::string& system) const; //!< Get PRNs in mask for system ("GPS"/"Galileo") + 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 + std::vector get_num_subset_satellites() const; //!< Get Nsys_sub number of satellites + float get_code_bias_m(const std::string& signal, int PRN) const; //!< Get code bias in [m] for a given signal and PRN satellite + float get_phase_bias_cycle(const std::string& signal, int PRN) const; //!< Get phase bias in [cycles] for a given signal and PRN satellite + float get_delta_radial_m(const std::string& system, int prn) const; //!< Get orbital radial correction in [m] for a given system ("GPS"/"Galileo") and PRN + float get_delta_in_track_m(const std::string& system, int prn) const; //!< Get orbital in_track correction in [m] for a given system ("GPS"/"Galileo") and PRN + float get_delta_cross_track_m(const std::string& system, int prn) const; //!< Get orbital cross_track correction in [m] for a given system ("GPS"/"Galileo") and PRN + float get_clock_correction_mult_m(const std::string& system, int prn) const; //!< Get clock correction in [m], already multiplied by its Delta Clock Multiplier, for a given system ("GPS"/"Galileo") and PRN + float get_clock_subset_correction_mult_m(const std::string& system, int prn) const; //!< Get clock correction subset in [m], already multiplied by its Delta Clock Multiplier + uint16_t get_nsat() const; //!< Get total number of satellites with corrections + uint16_t get_nsat_sub() const; //!< Get number of satellites in clock subset corrections + uint16_t get_validity_interval_s(uint8_t validity_interval_index) const; //!< Get validity interval in [s] from the validity_interval_index + uint16_t get_gnss_iod(const std::string& system, int prn) const; //!< Get GNSS IOD from a given system ("GPS"/"Galileo") and PRN + uint8_t get_gnss_id(int nsat) const; //!< Get GNSS ID from the nsat satellite // Mask std::vector gnss_id_mask; //!< GNSS ID. See HAS SIS ICD 1.0 Section 5.2.1.1 diff --git a/src/core/system_parameters/gnss_frequencies.h b/src/core/system_parameters/gnss_frequencies.h index 0d2288ceb..3099bae68 100644 --- a/src/core/system_parameters/gnss_frequencies.h +++ b/src/core/system_parameters/gnss_frequencies.h @@ -19,6 +19,9 @@ #ifndef GNSS_SDR_GNSS_FREQUENCIES_H #define GNSS_SDR_GNSS_FREQUENCIES_H +#include +#include + /** \addtogroup Core * \{ */ /** \addtogroup System_Parameters @@ -41,6 +44,21 @@ constexpr double FREQ1_BDS = 1.561098e9; //!< BeiDou B1 frequency (Hz) constexpr double FREQ2_BDS = 1.20714e9; //!< BeiDou B2 frequency (Hz) constexpr double FREQ3_BDS = 1.26852e9; //!< BeiDou B3 frequency (Hz) +const std::unordered_map SIGNAL_FREQ_MAP = { + {"1C", FREQ1}, + {"2S", FREQ2}, + {"L5", FREQ5}, + {"1B", FREQ1}, + {"5X", FREQ5}, + {"E6", FREQ6}, + {"7X", FREQ7}, + {"1G", FREQ1_GLO}, + {"2G", FREQ2_GLO}, + {"B1", FREQ1_BDS}, + {"B2", FREQ2_BDS}, + {"B3", FREQ3_BDS}, +}; + /** \} */ /** \} */ diff --git a/src/main/main.cc b/src/main/main.cc index 84e349a96..b48e2272d 100644 --- a/src/main/main.cc +++ b/src/main/main.cc @@ -11,7 +11,7 @@ * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. * This file is part of GNSS-SDR. * - * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors) * SPDX-License-Identifier: GPL-3.0-or-later * * ----------------------------------------------------------------------------- @@ -65,17 +65,26 @@ Concurrent_Map global_gps_acq_assist_map; int main(int argc, char** argv) { - const std::string intro_help( - std::string("\nGNSS-SDR is an Open Source GNSS Software Defined Receiver\n") + - "Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)\n" + - "This program comes with ABSOLUTELY NO WARRANTY;\n" + - "See COPYING file to see a copy of the General Public License\n \n"); + try + { + const std::string intro_help( + std::string("\nGNSS-SDR is an Open Source GNSS Software Defined Receiver\n") + + "Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors)\n" + + "This program comes with ABSOLUTELY NO WARRANTY;\n" + + "See COPYING file to see a copy of the General Public License\n \n"); - const std::string gnss_sdr_version(GNSS_SDR_VERSION); - gflags::SetUsageMessage(intro_help); - gflags::SetVersionString(gnss_sdr_version); - gflags::ParseCommandLineFlags(&argc, &argv, true); - std::cout << "Initializing GNSS-SDR v" << gnss_sdr_version << " ... Please wait.\n"; + const std::string gnss_sdr_version(GNSS_SDR_VERSION); + gflags::SetUsageMessage(intro_help); + gflags::SetVersionString(gnss_sdr_version); + gflags::ParseCommandLineFlags(&argc, &argv, true); + std::cout << "Initializing GNSS-SDR v" << gnss_sdr_version << " ... Please wait.\n"; + } + catch (const std::exception& e) + { + std::cerr << e.what() << '\n'; + std::cout << "GNSS-SDR program ended.\n"; + return 1; + } #if CUDA_GPU_ACCEL // Reset the device @@ -100,21 +109,31 @@ int main(int argc, char** argv) } else { - const fs::path p(FLAGS_log_dir); - if (!fs::exists(p)) + try { - std::cout << "The path " - << FLAGS_log_dir - << " does not exist, attempting to create it.\n"; - errorlib::error_code ec; - if (!fs::create_directory(p, ec)) + const fs::path p(FLAGS_log_dir); + if (!fs::exists(p)) { - std::cerr << "Could not create the " << FLAGS_log_dir << " folder. GNSS-SDR program ended.\n"; - gflags::ShutDownCommandLineFlags(); - return 1; + std::cout << "The path " + << FLAGS_log_dir + << " does not exist, attempting to create it.\n"; + errorlib::error_code ec; + if (!fs::create_directory(p, ec)) + { + std::cerr << "Could not create the " << FLAGS_log_dir << " folder. GNSS-SDR program ended.\n"; + gflags::ShutDownCommandLineFlags(); + return 1; + } } + std::cout << "Logging will be written at " << FLAGS_log_dir << '\n'; + } + catch (const std::exception& e) + { + std::cerr << e.what() << '\n'; + std::cerr << "Could not create the " << FLAGS_log_dir << " folder. GNSS-SDR program ended.\n"; + gflags::ShutDownCommandLineFlags(); + return 1; } - std::cout << "Logging will be written at " << FLAGS_log_dir << '\n'; } } @@ -132,9 +151,7 @@ int main(int argc, char** argv) catch (const boost::thread_resource_error& e) { std::cerr << "Failed to create boost thread.\n"; - gflags::ShutDownCommandLineFlags(); - std::cout << "GNSS-SDR program ended.\n"; - return 1; + return_code = 1; } catch (const boost::exception& e) { @@ -147,9 +164,7 @@ int main(int argc, char** argv) { std::cerr << "Boost exception: " << boost::diagnostic_information(e) << '\n'; } - gflags::ShutDownCommandLineFlags(); - std::cout << "GNSS-SDR program ended.\n"; - return 1; + return_code = 1; } catch (const std::exception& ex) { @@ -162,9 +177,7 @@ int main(int argc, char** argv) { std::cerr << "C++ Standard Library exception: " << ex.what() << '\n'; } - gflags::ShutDownCommandLineFlags(); - std::cout << "GNSS-SDR program ended.\n"; - return 1; + return_code = 1; } catch (...) { @@ -177,9 +190,7 @@ int main(int argc, char** argv) { std::cerr << "Unexpected catch. This should not happen.\n"; } - gflags::ShutDownCommandLineFlags(); - std::cout << "GNSS-SDR program ended.\n"; - return 1; + return_code = 1; } // report the elapsed time diff --git a/src/tests/unit-tests/system-parameters/has_decoding_test.cc b/src/tests/unit-tests/system-parameters/has_decoding_test.cc index 5f4c7930f..593ce79de 100644 --- a/src/tests/unit-tests/system-parameters/has_decoding_test.cc +++ b/src/tests/unit-tests/system-parameters/has_decoding_test.cc @@ -353,6 +353,10 @@ TEST(HAS_Test, Decoder) EXPECT_EQ(has_message->phase_discontinuity_indicator[has_message->get_PRNs_in_mask(0).size() + i][2], phase_discontinuity_indicator_expected_gal[i]); EXPECT_EQ(has_message->phase_discontinuity_indicator[has_message->get_PRNs_in_mask(0).size() + i][3], phase_discontinuity_indicator_expected_gal[i]); } + EXPECT_FLOAT_EQ(has_message->get_code_bias_m("L1 C/A", 10), -2.64 * HAS_MSG_CODE_BIAS_SCALE_FACTOR); + EXPECT_FLOAT_EQ(has_message->get_code_bias_m("L1 C/A", 11), 0.0); + EXPECT_FLOAT_EQ(has_message->get_code_bias_m("E1-C", 36), -1.98 * HAS_MSG_CODE_BIAS_SCALE_FACTOR); + EXPECT_FLOAT_EQ(has_message->get_code_bias_m("E1-C", 37), 0.0); } else {