mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-05 15:00:33 +00:00
Code Issues Releases Wiki Activity

Process and apply Galileo HAS corrections to the PVT solution

Browse Source 
Add getters to Galileo_HAS_data class, improve implementation of existing ones
Process reception of HAS messages and inject corrections to RTKLIB
Apply HAS corrections to PVT computation within RTKLIB
Add configuration parameter PVT.use_has_corrections=true/false, true by default, to deactivate application of corrections but still retrieve HAS messages
Add configuration parameter PVT.use_unhealthy_sats=true/false, false by default, to use observables from satellites flagged as unhealthy
Use an unordered_map for signals and frequencies
This commit is contained in:
Carles Fernandez 2023-02-28 13:08:53 +01:00
parent 0d60e46390
commit 0a11f1470a
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
24 changed files with 1929 additions and 840 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
docs/CHANGELOG.md
View File

@ -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

4
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -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);

187
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@ -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<std::shared_ptr<Galileo_HAS_data>>(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<int, Gnss_Synchro>& 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 = "

26
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h
View File

@ -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<int, Gnss_Synchro>& observables_map,
double rx_clock_offset_s);
void update_HAS_corrections();
std::map<int, Gnss_Synchro> interpolate_observables(const std::map<int, Gnss_Synchro>& observables_map_t0,
const std::map<int, Gnss_Synchro>& 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<bool> d_channel_initialized;
std::vector<double> 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<std::string, StringValue_> d_mapStringValues;
std::map<int, Gnss_Synchro> d_gnss_observables_map;
std::map<int, Gnss_Synchro> d_gnss_observables_map_t0;
std::map<int, Gnss_Synchro> 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;
};

74
src/algorithms/PVT/libs/pvt_conf.cc
View File

@ -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;
}

2
src/algorithms/PVT/libs/pvt_conf.h
View File

@ -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;
};

481
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -4,7 +4,7 @@
* data flow and structures
* \authors <ul>
* <li> 2017-2019, Javier Arribas
* <li> 2017-2019, Carles Fernandez
* <li> 2017-2023, Carles Fernandez
* <li> 2007-2013, T. Takasu
* </ul>
*
@ -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 <glog/logging.h>
#include <matio.h>
#include <algorithm>
#include <cmath>
#include <exception>
#include <utility>
#include <vector>
@ -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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int, Gnss_Synchro> &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<int, Gnss_Synchro> &obs_map)
{
for (const auto &it : obs_map)
{
uint32_t obs_tow = it.second.interp_TOW_ms / 1000.0;
auto prn = static_cast<int>(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<int, Gnss_Synchro> &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<std::string> e1b_signals = {"E1-B I/NAV OS", "E1-C", "E1-B + E1-C"};
const std::vector<std::string> e6_signals = {"E6-B C/NAV HAS", "E6-C", "E6-B + E6-C"};
const std::vector<std::string> e5_signals = {"E5a-I F/NAV OS", "E5a-Q", "E5a-I+E5a-Q"};
const std::vector<std::string> e7_signals = {"E5bI I/NAV OS", "E5b-Q", "E5b-I+E5b-Q"};
const std::vector<std::string> g1c_signals = {"L1 C/A"};
const std::vector<std::string> g2s_signals = {"L2 CM", "L2 CL", "L2 CM+CL", "L2 P"};
const std::vector<std::string> 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<int>(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<int, Gnss_Synchro> &gnss_observables_map, bool flag_averaging)
{
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
@ -493,7 +936,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {},
@ -578,6 +1028,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {},
@ -618,6 +1072,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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++;
@ -634,18 +1089,23 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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());
@ -742,6 +1202,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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++;

22
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -4,7 +4,7 @@
* data flow and structures
* \authors <ul>
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* <li> 2017-2023, Carles Fernandez
* <li> 2007-2013, T. Takasu
* </ul>
*
@ -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 <array>
#include <cstdint>
#include <fstream>
#include <map>
#include <string>
#include <utility>
/** \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<int, Gnss_Synchro>& obs_map);
sol_t pvt_sol{};
std::array<ssat_t, MAXSAT> pvt_ssat{};
@ -122,10 +127,23 @@ public:
private:
bool save_matfile() const;
void check_has_orbit_clock_validity(const std::map<int, Gnss_Synchro>& obs_map);
void get_has_biases(const std::map<int, Gnss_Synchro>& obs_map);
void get_current_has_obs_correction(const std::string& signal, uint32_t tow_obs, int prn);
std::array<obsd_t, MAXOBS> d_obs_data{};
std::array<double, 4> d_dop{};
std::map<int, int> d_rtklib_freq_index;
std::map<std::string, int> d_rtklib_band_index;
std::map<std::string, std::map<int, HAS_orbit_corrections>> d_has_orbit_corrections_store_map; // first key is system, second key is PRN
std::map<std::string, std::map<int, HAS_clock_corrections>> d_has_clock_corrections_store_map; // first key is system, second key is PRN
std::map<std::string, std::map<int, std::pair<float, uint32_t>>> d_has_code_bias_store_map; // first key is signal, second key is PRN
std::map<std::string, std::map<int, std::pair<float, uint32_t>>> d_has_phase_bias_store_map; // first key is signal, second key is PRN
std::map<std::string, std::map<int, HAS_obs_corrections>> 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{};

27
src/algorithms/libs/rtklib/rtklib.h
View File

@ -4,7 +4,7 @@
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* <li> 2017-2023, Carles Fernandez
* </ul>
*
* 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;

381
src/algorithms/libs/rtklib/rtklib_conversions.cc
View File

@ -28,13 +28,17 @@
#include "gps_ephemeris.h" // for Gps_Ephemeris
#include "rtklib_rtkcmn.h"
#include <cmath>
#include <cstdint>
#include <string>
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<std::string, std::map<int, HAS_obs_corrections>>& 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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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<std::string, std::map<int, HAS_obs_corrections>> 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<int, HAS_orbit_corrections> empty_orbit_map;
std::map<int, HAS_clock_corrections> 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<int, HAS_orbit_corrections>& orbit_correction_map,
const std::map<int, HAS_clock_corrections>& 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<int>(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<int>(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<int, HAS_orbit_corrections> empty_orbit_map;
std::map<int, HAS_clock_corrections> 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<int, HAS_orbit_corrections>& orbit_correction_map,
const std::map<int, HAS_clock_corrections>& 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<int>(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<int>(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;
}

52
src/algorithms/libs/rtklib/rtklib_conversions.h
View File

@ -18,6 +18,9 @@
#define GNSS_SDR_RTKLIB_CONVERSIONS_H
#include "rtklib.h"
#include <cstdint>
#include <map>
#include <string>
/** \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<int, HAS_orbit_corrections>& orbit_correction_map,
const std::map<int, HAS_clock_corrections>& 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<int, HAS_orbit_corrections>& orbit_correction_map,
const std::map<int, HAS_clock_corrections>& 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<std::string, std::map<int, HAS_obs_corrections>>& 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);

97
src/algorithms/libs/rtklib/rtklib_ephemeris.cc
View File

@ -4,7 +4,7 @@
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* <li> 2017-2023, Carles Fernandez
* </ul>
*
* 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);

2
src/algorithms/libs/rtklib/rtklib_rtcm.cc
View File

@ -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'};

2
src/algorithms/libs/rtklib/rtklib_rtcm2.cc
View File

@ -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;

12
src/algorithms/libs/rtklib/rtklib_rtcm3.cc
View File

@ -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;

2
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

@ -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;

2
src/algorithms/libs/rtklib/rtklib_rtksvr.cc
View File

@ -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};

54
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
View File

@ -100,19 +100,6 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_)
d_channel_last_pseudorange_smooth = std::vector<double>(d_nchannels_out, 0.0);
d_channel_last_carrier_phase_rads = std::vector<double>(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<std::queue<GnssTime>>(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<Gnss_Synchro> &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

27
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h
View File

@ -29,12 +29,11 @@
#include <cstddef> // for size_t
#include <cstdint> // for int32_t
#include <fstream> // for std::ofstream
#include <map> // for std::map
#include <memory> // for std::shared, std:unique_ptr
#include <queue>
#include <string> // for std::string
#include <typeinfo> // for typeid
#include <vector> // for std::vector
#include <queue> // for std::queue
#include <string> // for std::string
#include <typeinfo> // for typeid
#include <vector> // 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<std::string, StringValue_> d_mapStringValues;
std::unique_ptr<Gnss_circular_deque<Gnss_Synchro>> d_gnss_synchro_history; // Tracking observable history
boost::circular_buffer<uint64_t> d_Rx_clock_buffer; // time history

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

@ -149,7 +149,7 @@ std::shared_ptr<Galileo_HAS_data> 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<uint32_t>(std::remainder(tow, 3600)) + d_HAS_data.header.toh;
d_HAS_data.tow = tow;
auto has_data_ptr = std::make_shared<Galileo_HAS_data>(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<uint32_t>(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<Galileo_HAS_data>(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<HAS_MSG_SATELLITE_MASK_LENGTH> satellite_mask_bits(satellite_mask);
std::string satellite_mask_string = satellite_mask_bits.to_string();
int number_sats_this_gnss_id = std::count(satellite_mask_string.begin(), satellite_mask_string.end(), '1');
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());

1220
src/core/system_parameters/galileo_has_data.cc
View File

File diff suppressed because it is too large Load Diff

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

@ -52,26 +52,40 @@ class Galileo_HAS_data
public:
Galileo_HAS_data() = default;
std::vector<std::string> 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<std::string> get_systems_string() const; //!< Get Nsys system name strings
std::vector<std::vector<float>> get_code_bias_m() const; //!< Get Nsat x Ncodes code biases in [m]
std::vector<std::vector<float>> get_phase_bias_cycle() const; //!< Get Nsat x Nphases phase biases in [cycles]
std::vector<float> get_delta_radial_m() const; //!< Get Nsat delta radial corrections in [m]
std::vector<float> get_delta_radial_m(uint8_t nsys) const; //!< Get delta radial corrections in [m] for system nsys, with 0 <= nsys < Nsys
std::vector<float> get_delta_in_track_m() const; //!< Get Nsat delta in-track corrections in [m]
std::vector<float> 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<float> get_delta_cross_track_m() const; //!< Get Nsat delta cross-track corrections in [m]
std::vector<float> 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<float> get_delta_clock_correction_m() const; //!< Get Nsat delta clock C0 corrections in [m]
std::vector<float> 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<int> get_PRNs_in_mask(uint8_t nsys) const; //!< Get PRNs in mask for system nsys, with 0 <= nsys < Nsys
std::vector<int> get_PRNs_in_submask(uint8_t nsys) const; //!< Get PRNs in submask for system nsys, with 0 <= nsys < Nsys
std::vector<uint16_t> get_gnss_iod(uint8_t nsys) const; //!< Get GNSS IODs for for system nsys, with 0 <= nsys < Nsys
std::vector<uint8_t> 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<std::string> 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<std::string> 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<std::string> get_systems_string() const; //!< Get Nsys system name strings
std::vector<std::string> get_systems_subset_string() const; //!< Get Nsat system name strings present in clock corrections subset
std::vector<std::vector<float>> get_code_bias_m() const; //!< Get Nsat x Ncodes code biases in [m]
std::vector<std::vector<float>> get_phase_bias_cycle() const; //!< Get Nsat x Nphases phase biases in [cycles]
std::vector<std::vector<float>> get_delta_clock_subset_correction_m() const; //!< Get Nsys_sub vectors with Nsat_sub delta clock C0 corrections in [m]
std::vector<float> get_delta_radial_m() const; //!< Get Nsat delta radial corrections in [m]
std::vector<float> get_delta_radial_m(uint8_t nsys) const; //!< Get delta radial corrections in [m] for system nsys, with 0 <= nsys < Nsys
std::vector<float> get_delta_in_track_m() const; //!< Get Nsat delta in-track corrections in [m]
std::vector<float> 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<float> get_delta_cross_track_m() const; //!< Get Nsat delta cross-track corrections in [m]
std::vector<float> 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<float> get_delta_clock_correction_m() const; //!< Get Nsat delta clock C0 corrections in [m]
std::vector<float> 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<float> 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<int> get_PRNs_in_mask(uint8_t nsys) const; //!< Get PRNs in mask for system nsys, with 0 <= nsys < Nsys
std::vector<int> get_PRNs_in_mask(const std::string& system) const; //!< Get PRNs in mask for system ("GPS"/"Galileo")
std::vector<int> get_PRNs_in_submask(uint8_t nsys) const; //!< Get PRNs in submask for system nsys, with 0 <= nsys < Nsys
std::vector<uint16_t> get_gnss_iod(uint8_t nsys) const; //!< Get GNSS IODs for for system nsys, with 0 <= nsys < Nsys
std::vector<uint8_t> get_num_satellites() const; //!< Get Nsys number of satellites
std::vector<uint8_t> 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<uint8_t> gnss_id_mask; //!< GNSS ID. See HAS SIS ICD 1.0 Section 5.2.1.1

18
src/core/system_parameters/gnss_frequencies.h
View File

@ -19,6 +19,9 @@
#ifndef GNSS_SDR_GNSS_FREQUENCIES_H
#define GNSS_SDR_GNSS_FREQUENCIES_H
#include <string>
#include <unordered_map>
/** \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<std::string, double> 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},
};
/** \} */
/** \} */

4
src/tests/unit-tests/system-parameters/has_decoding_test.cc
View File

@ -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
{