mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Process and apply Galileo HAS corrections to the PVT solution 

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) ## [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: ### Improvements in Availability:
- Fixed bug that made the PVT block to not resolve position anymore after a loss - 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. 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 new configuration parameter `Channels_XX.RF_channel_ID` allows to specify
the signal source per channel group. 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. - Allowed the CMake project to be a sub-project.
See the definitions of concepts and metrics at 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 // 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_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 // make PVT object
pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); 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_enable_rx_clock_correction(conf_.enable_rx_clock_correction),
d_an_printer_enabled(conf_.an_output_enabled), d_an_printer_enabled(conf_.an_output_enabled),
d_log_timetag(conf_.log_source_timetag), 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 // Send feedback message to observables block with the receiver clock offset
this->message_port_register_out(pmt::mp("pvt_to_observables")); 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_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 // set the RTKLIB trace (debug) level
tracelevel(conf_.rtk_trace_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) if (gps_eph->SV_health != 0)
{ {
std::cout << TEXT_RED << "Satellite " << Gnss_Satellite(std::string("GPS"), gps_eph->PRN) 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) 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) if (gps_cnav_ephemeris->signal_health != 0)
{ {
std::cout << "Satellite " << Gnss_Satellite(std::string("GPS"), gps_cnav_ephemeris->PRN) std::cout << "Satellite " << Gnss_Satellite(std::string("GPS"), gps_cnav_ephemeris->PRN)
<< " does not report a healthy status in the CNAV message," << " reports an unhealthy status in the CNAV message,";
<< " use PVT solutions at your own risk.\n"; 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"; 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))) ((galileo_eph->E5b_HS != 0) || (galileo_eph->E5b_DVS == true)))
{ {
std::cout << TEXT_RED << "Satellite " << Gnss_Satellite(std::string("Galileo"), galileo_eph->PRN) 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) 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) if (bds_dnav_eph->SV_health != 0)
{ {
std::cout << TEXT_RED << "Satellite " << Gnss_Satellite(std::string("Beidou"), bds_dnav_eph->PRN) 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) 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 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) 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)); 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) if (d_has_simple_printer)
{ {
d_has_simple_printer->print_message(has_data.get()); 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 // all observables in the map are valid
observables_iter->second.RX_time -= rx_clock_offset_s; observables_iter->second.RX_time -= rx_clock_offset_s;
observables_iter->second.Pseudorange_m -= rx_clock_offset_s * SPEED_OF_LIGHT_M_S; observables_iter->second.Pseudorange_m -= rx_clock_offset_s * SPEED_OF_LIGHT_M_S;
const auto it_freq_map = SIGNAL_FREQ_MAP.find(std::string(observables_iter->second.Signal, 2));
switch (d_mapStringValues[observables_iter->second.Signal]) if (it_freq_map != SIGNAL_FREQ_MAP.cend())
{ {
case evGPS_1C: observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * it_freq_map->second * TWO_PI;
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;
} }
} }
} }
@ -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 // 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) if (d_channel_initialized.at(observables_iter->second.Channel_ID) == false)
{ {
double wavelength_m = 0; double wavelength_m = 1.0;
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: wavelength_m = SPEED_OF_LIGHT_M_S / it_freq_map->second;
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;
} }
const double wrap_carrier_phase_rad = fmod(observables_iter->second.Carrier_phase_rads, TWO_PI); 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; 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, int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items,
gr_vector_void_star& output_items __attribute__((unused))) 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()) if (tmp_eph_iter_gps != d_internal_pvt_solver->gps_ephemeris_map.cend())
{ {
const uint32_t prn_aux = tmp_eph_iter_gps->second.PRN; 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; 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; const uint32_t prn_aux = tmp_eph_iter_gal->second.PRN;
if ((prn_aux == in[i][epoch].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, 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) == 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, 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) == 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("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; 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()) 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; 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; 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()) 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; 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; 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()) 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; 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; 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; 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 ################################ // ############ 2 COMPUTE THE PVT ################################
bool flag_pvt_valid = false; bool flag_pvt_valid = false;
if (d_gnss_observables_map.empty() == 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)); // 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'; // 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()) 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() << " 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]"; << " [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()) /* 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 = " << " 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 Beidou_Dnav_Ephemeris;
class Galileo_Almanac; class Galileo_Almanac;
class Galileo_Ephemeris; class Galileo_Ephemeris;
class Galileo_HAS_data;
class GeoJSON_Printer; class GeoJSON_Printer;
class Gps_Almanac; class Gps_Almanac;
class Gps_Ephemeris; class Gps_Ephemeris;
@ -140,13 +141,15 @@ private:
void msg_handler_telemetry(const pmt::pmt_t& msg); 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 initialize_and_apply_carrier_phase_offset();
void apply_rx_clock_offset(std::map<int, Gnss_Synchro>& observables_map, void apply_rx_clock_offset(std::map<int, Gnss_Synchro>& observables_map,
double rx_clock_offset_s); 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, 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, const std::map<int, Gnss_Synchro>& observables_map_t1,
double rx_time_s); double rx_time_s);
@ -160,7 +163,7 @@ private:
typedef struct typedef struct
{ {
long mtype; // NOLINT(google-runtime-int) required by SysV queue messaging long mtype; // NOLINT(google-runtime-int)
double ttff; double ttff;
} d_ttff_msgbuf; } d_ttff_msgbuf;
bool send_sys_v_ttff_msg(d_ttff_msgbuf ttff) const; bool send_sys_v_ttff_msg(d_ttff_msgbuf ttff) const;
@ -194,23 +197,6 @@ private:
std::vector<bool> d_channel_initialized; std::vector<bool> d_channel_initialized;
std::vector<double> d_initial_carrier_phase_offset_estimation_rads; 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;
std::map<int, Gnss_Synchro> d_gnss_observables_map_t0; std::map<int, Gnss_Synchro> d_gnss_observables_map_t0;
std::map<int, Gnss_Synchro> d_gnss_observables_map_t1; std::map<int, Gnss_Synchro> d_gnss_observables_map_t1;
@ -289,6 +275,8 @@ private:
bool d_an_printer_enabled; bool d_an_printer_enabled;
bool d_log_timetag; bool d_log_timetag;
bool d_use_e6_for_pvt; 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 dump_mat = true;
bool log_source_timetag; bool log_source_timetag;
bool use_e6_for_pvt = true; 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 * data flow and structures
* \authors <ul> * \authors <ul>
* <li> 2017-2019, Javier Arribas * <li> 2017-2019, Javier Arribas
* <li> 2017-2019, Carles Fernandez * <li> 2017-2023, Carles Fernandez
* <li> 2007-2013, T. Takasu * <li> 2007-2013, T. Takasu
* </ul> * </ul>
* *
@ -23,7 +23,7 @@
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017-2019, Javier Arribas * Copyright (C) 2017-2019, Javier Arribas
* Copyright (C) 2017-2019, Carles Fernandez * Copyright (C) 2017-2023, Carles Fernandez
* All rights reserved. * All rights reserved.
* *
* SPDX-License-Identifier: BSD-2-Clause * SPDX-License-Identifier: BSD-2-Clause
@ -33,11 +33,12 @@
#include "rtklib_solver.h" #include "rtklib_solver.h"
#include "Beidou_DNAV.h" #include "Beidou_DNAV.h"
#include "gnss_sdr_filesystem.h" #include "gnss_sdr_filesystem.h"
#include "rtklib_conversions.h"
#include "rtklib_rtkpos.h" #include "rtklib_rtkpos.h"
#include "rtklib_solution.h" #include "rtklib_solution.h"
#include <glog/logging.h> #include <glog/logging.h>
#include <matio.h> #include <matio.h>
#include <algorithm>
#include <cmath>
#include <exception> #include <exception>
#include <utility> #include <utility>
#include <vector> #include <vector>
@ -130,6 +131,8 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
d_rtklib_freq_index[1] = 3; d_rtklib_freq_index[1] = 3;
break; break;
} }
// auto empty_map = std::map < int, HAS_obs_corrections >> ();
// d_has_obs_corr_map["L1 C/A"] = empty_map;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true) 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) 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; 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': case 'G':
{ {
const std::string sig_(gnss_observables_iter->second.Signal); const std::string sig_(gnss_observables_iter->second.Signal, 2);
if (sig_ == "1C") if (sig_ == "1C")
{ {
band1 = true; band1 = true;
@ -522,7 +965,8 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{ {
case 'E': 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 // Galileo E1
if (sig_ == "1B") 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()) if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
{ {
// convert ephemeris from GNSS-SDR class to RTKLIB structure // 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 // convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs{}; obsd_t newobs{};
d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
d_has_obs_corr_map,
galileo_ephemeris_iter->second.WN, galileo_ephemeris_iter->second.WN,
d_rtklib_band_index[sig_]); d_rtklib_band_index[sig_]);
valid_obs++; 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], d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs],
gnss_observables_iter->second, gnss_observables_iter->second,
d_has_obs_corr_map,
galileo_ephemeris_iter->second.WN, galileo_ephemeris_iter->second.WN,
d_rtklib_band_index[sig_]); d_rtklib_band_index[sig_]);
found_E1_obs = true; 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 // insert Galileo E5 obs as new obs and also insert its ephemeris
// convert ephemeris from GNSS-SDR class to RTKLIB structure // 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 // convert observation from GNSS-SDR class to RTKLIB structure
const auto default_code_ = static_cast<unsigned char>(CODE_NONE); const auto default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, 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}, {}}; {}, {0.0, 0.0, 0.0}, {}};
d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
d_has_obs_corr_map,
galileo_ephemeris_iter->second.WN, galileo_ephemeris_iter->second.WN,
d_rtklib_band_index[sig_]); d_rtklib_band_index[sig_]);
valid_obs++; 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], d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs],
gnss_observables_iter->second, gnss_observables_iter->second,
d_has_obs_corr_map,
galileo_ephemeris_iter->second.WN, galileo_ephemeris_iter->second.WN,
d_rtklib_band_index[sig_]); d_rtklib_band_index[sig_]);
found_E1_obs = true; 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 // insert Galileo E6 obs as new obs and also insert its ephemeris
// convert ephemeris from GNSS-SDR class to RTKLIB structure // 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 // convert observation from GNSS-SDR class to RTKLIB structure
const auto default_code_ = static_cast<unsigned char>(CODE_NONE); const auto default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, 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}, {}}; {}, {0.0, 0.0, 0.0}, {}};
d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
d_has_obs_corr_map,
galileo_ephemeris_iter->second.WN, galileo_ephemeris_iter->second.WN,
d_rtklib_band_index[sig_]); d_rtklib_band_index[sig_]);
valid_obs++; valid_obs++;
@ -634,18 +1089,23 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{ {
// GPS L1 // GPS L1
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key // 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") if (sig_ == "1C")
{ {
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_ephemeris_iter != gps_ephemeris_map.cend()) if (gps_ephemeris_iter != gps_ephemeris_map.cend())
{ {
// convert ephemeris from GNSS-SDR class to RTKLIB structure // 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 // convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs{}; obsd_t newobs{};
d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
d_has_obs_corr_map,
gps_ephemeris_iter->second.WN, gps_ephemeris_iter->second.WN,
d_rtklib_band_index[sig_], d_rtklib_band_index[sig_],
this->is_pre_2009()); 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}, {}}; {}, {0.0, 0.0, 0.0}, {}};
d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
d_has_obs_corr_map,
gps_cnav_ephemeris_iter->second.WN, gps_cnav_ephemeris_iter->second.WN,
d_rtklib_band_index[sig_]); d_rtklib_band_index[sig_]);
valid_obs++; valid_obs++;

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

@ -4,7 +4,7 @@
* data flow and structures * data flow and structures
* \authors <ul> * \authors <ul>
* <li> 2017, Javier Arribas * <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez * <li> 2017-2023, Carles Fernandez
* <li> 2007-2013, T. Takasu * <li> 2007-2013, T. Takasu
* </ul> * </ul>
* *
@ -23,7 +23,7 @@
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017-2019, Javier Arribas * Copyright (C) 2017-2019, Javier Arribas
* Copyright (C) 2017-2019, Carles Fernandez * Copyright (C) 2017-2023, Carles Fernandez
* All rights reserved. * All rights reserved.
* *
* SPDX-License-Identifier: BSD-2-Clause * SPDX-License-Identifier: BSD-2-Clause
@ -41,6 +41,7 @@
#include "beidou_dnav_utc_model.h" #include "beidou_dnav_utc_model.h"
#include "galileo_almanac.h" #include "galileo_almanac.h"
#include "galileo_ephemeris.h" #include "galileo_ephemeris.h"
#include "galileo_has_data.h"
#include "galileo_iono.h" #include "galileo_iono.h"
#include "galileo_utc_model.h" #include "galileo_utc_model.h"
#include "glonass_gnav_almanac.h" #include "glonass_gnav_almanac.h"
@ -57,11 +58,13 @@
#include "monitor_pvt.h" #include "monitor_pvt.h"
#include "pvt_solution.h" #include "pvt_solution.h"
#include "rtklib.h" #include "rtklib.h"
#include "rtklib_conversions.h"
#include <array> #include <array>
#include <cstdint> #include <cstdint>
#include <fstream> #include <fstream>
#include <map> #include <map>
#include <string> #include <string>
#include <utility>
/** \addtogroup PVT /** \addtogroup PVT
* \{ */ * \{ */
@ -91,6 +94,8 @@ public:
double get_pdop() const override; double get_pdop() const override;
double get_gdop() const override; double get_gdop() const override;
Monitor_Pvt get_monitor_pvt() const; 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{}; sol_t pvt_sol{};
std::array<ssat_t, MAXSAT> pvt_ssat{}; std::array<ssat_t, MAXSAT> pvt_ssat{};
@ -122,10 +127,23 @@ public:
private: private:
bool save_matfile() const; 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<obsd_t, MAXOBS> d_obs_data{};
std::array<double, 4> d_dop{}; std::array<double, 4> d_dop{};
std::map<int, int> d_rtklib_freq_index; std::map<int, int> d_rtklib_freq_index;
std::map<std::string, int> d_rtklib_band_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::string d_dump_filename;
std::ofstream d_dump_file; std::ofstream d_dump_file;
rtk_t d_rtk{}; rtk_t d_rtk{};

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

@ -4,7 +4,7 @@
* \authors <ul> * \authors <ul>
* <li> 2007-2013, T. Takasu * <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas * <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez * <li> 2017-2023, Carles Fernandez
* </ul> * </ul>
* *
* This is a derived work from RTKLIB http://www.rtklib.com/ * This is a derived work from RTKLIB http://www.rtklib.com/
@ -22,7 +22,7 @@
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017, Javier Arribas * Copyright (C) 2017, Javier Arribas
* Copyright (C) 2017, Carles Fernandez * Copyright (C) 2017-2023, Carles Fernandez
* All rights reserved. * All rights reserved.
* *
* SPDX-License-Identifier: BSD-2-Clause * SPDX-License-Identifier: BSD-2-Clause
@ -442,15 +442,20 @@ typedef struct
/* SV orbit parameters */ /* SV orbit parameters */
double A, e, i0, OMG0, omg, M0, deln, OMGd, idot; double A, e, i0, OMG0, omg, M0, deln, OMGd, idot;
double crc, crs, cuc, cus, cic, cis; double crc, crs, cuc, cus, cic, cis;
double toes; /* Toe (s) in week */ double toes; /* Toe (s) in week */
double fit; /* fit interval (h) */ double fit; /* fit interval (h) */
double f0, f1, f2; /* SV clock parameters (af0,af1,af2) */ double f0, f1, f2; /* SV clock parameters (af0,af1,af2) */
double tgd[4]; /* group delay parameters */ double tgd[4]; /* group delay parameters */
/* GPS/QZS:tgd[0]=TGD */ /* GPS/QZS:tgd[0]=TGD */
/* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */
/* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */
double isc[4]; /* GPS :isc[0]=ISCL1, isc[1]=ISCL2, isc[2]=ISCL5I, isc[3]=ISCL5Q */ double isc[4]; /* GPS :isc[0]=ISCL1, isc[1]=ISCL2, isc[2]=ISCL5I, isc[3]=ISCL5Q */
double Adot, ndot; /* Adot,ndot for CNAV */ 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; } eph_t;

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

@ -28,13 +28,17 @@
#include "gps_ephemeris.h" // for Gps_Ephemeris #include "gps_ephemeris.h" // for Gps_Ephemeris
#include "rtklib_rtkcmn.h" #include "rtklib_rtkcmn.h"
#include <cmath> #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 // 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.D[band] = gnss_synchro.Carrier_Doppler_hz;
rtklib_obs.P[band] = gnss_synchro.Pseudorange_m; 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; 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; 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) geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glonass_Gnav_Utc_Model& gnav_clock_model)
{ {
int week; 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) 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, 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 // 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.sat = gal_eph.PRN + NSATGPS + NSATGLO;
rtklib_sat.A = gal_eph.sqrtA * gal_eph.sqrtA; 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.toc = gpst2time(rtklib_sat.week, toc);
rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); 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; return rtklib_sat;
} }
eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph, bool pre_2009_file) 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, 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.sat = gps_eph.PRN;
rtklib_sat.A = gps_eph.sqrtA * gps_eph.sqrtA; rtklib_sat.A = gps_eph.sqrtA * gps_eph.sqrtA;
rtklib_sat.M0 = gps_eph.M_0; 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.toc = gpst2time(rtklib_sat.week, toc);
rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); 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; 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 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, 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.sat = bei_eph.PRN + NSATGPS + NSATGLO + NSATGAL + NSATQZS;
rtklib_sat.A = bei_eph.sqrtA * bei_eph.sqrtA; rtklib_sat.A = bei_eph.sqrtA * bei_eph.sqrtA;
rtklib_sat.M0 = bei_eph.M_0; 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.toc = gpst2time(rtklib_sat.week, toc);
rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); 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; 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 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, 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.sat = gps_cnav_eph.PRN;
rtklib_sat.A = gps_cnav_eph.sqrtA * gps_cnav_eph.sqrtA; rtklib_sat.A = gps_cnav_eph.sqrtA * gps_cnav_eph.sqrtA;
rtklib_sat.M0 = gps_cnav_eph.M_0; 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.toc = gpst2time(rtklib_sat.week, toc);
rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); 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; return rtklib_sat;
} }

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

@ -18,6 +18,9 @@
#define GNSS_SDR_RTKLIB_CONVERSIONS_H #define GNSS_SDR_RTKLIB_CONVERSIONS_H
#include "rtklib.h" #include "rtklib.h"
#include <cstdint>
#include <map>
#include <string>
/** \addtogroup PVT /** \addtogroup PVT
* \{ */ * \{ */
@ -35,8 +38,48 @@ class Gps_Almanac;
class Gps_CNAV_Ephemeris; class Gps_CNAV_Ephemeris;
class Gps_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 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 Gps_CNAV_Ephemeris& gps_cnav_eph);
eph_t eph_to_rtklib(const Beidou_Dnav_Ephemeris& bei_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); 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); 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> * \authors <ul>
* <li> 2007-2013, T. Takasu * <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas * <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez * <li> 2017-2023, Carles Fernandez
* </ul> * </ul>
* *
* This is a derived work from RTKLIB http://www.rtklib.com/ * This is a derived work from RTKLIB http://www.rtklib.com/
@ -22,7 +22,7 @@
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017, Javier Arribas * Copyright (C) 2017, Javier Arribas
* Copyright (C) 2017, Carles Fernandez * Copyright (C) 2017-2023, Carles Fernandez
* All rights reserved. * All rights reserved.
* *
* SPDX-License-Identifier: BSD-2-Clause * 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 sys;
int prn; int prn;
double has_relativistic_correction = 0.0;
trace(4, "eph2pos : time=%s sat=%2d\n", time_str(time, 3), eph->sat); trace(4, "eph2pos : time=%s sat=%2d\n", time_str(time, 3), eph->sat);
if (eph->A <= 0.0) 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[0] = x * cosO - y * cosi * sinO;
rs[1] = x * sinO + y * cosi * cosO; rs[1] = x * sinO + y * cosi * cosO;
rs[2] = y * sin(i); 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); tk = timediffweekcrossover(time, eph->toc);
*dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk; *dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk;
/* relativity correction */ /* 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 */ /* position and clock error variance */
*var = var_uraeph(eph->sva); *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}}; 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}, 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, 0.0, 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}, 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}; 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'}; 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}, 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, 0.0, 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 toc;
double sqrtA; double sqrtA;
int i = 48; 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}, 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, 0.0, 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 toc;
double sqrtA; double sqrtA;
char *msg; 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}, 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, 0.0, 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 toc;
double sqrtA; double sqrtA;
char *msg; 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}, 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, 0.0, 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 toc;
double sqrtA; double sqrtA;
char *msg; 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}, 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, 0.0, 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 toc;
double sqrtA; double sqrtA;
char *msg; 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}, 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, 0.0, 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 toc;
double sqrtA; double sqrtA;
char *msg; 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}, 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, 0.0, 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 toc;
double sqrtA; double sqrtA;
char *msg; 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; 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, 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}; geph_t geph0 = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {}, {}, {}, 0.0, 0.0, 0.0};
char buff[4096]; char buff[4096];
char *p; 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}; '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}, 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, 0.0, 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}, 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}; 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}; 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_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_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); 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() 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) if (it->Flag_valid_pseudorange)
{ {
// 0. get wavelength for the current signal // 0. get wavelength for the current signal
double wavelength_m = 0; double wavelength_m = 0.0;
switch (d_mapStringValues[it->Signal]) 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: wavelength_m = SPEED_OF_LIGHT_M_S / it_freq_map->second;
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;
} }
// todo: propagate the PLL lock status in Gnss_Synchro // 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 <cstddef> // for size_t
#include <cstdint> // for int32_t #include <cstdint> // for int32_t
#include <fstream> // for std::ofstream #include <fstream> // for std::ofstream
#include <map> // for std::map
#include <memory> // for std::shared, std:unique_ptr #include <memory> // for std::shared, std:unique_ptr
#include <queue> #include <queue> // for std::queue
#include <string> // for std::string #include <string> // for std::string
#include <typeinfo> // for typeid #include <typeinfo> // for typeid
#include <vector> // for std::vector #include <vector> // for std::vector
/** \addtogroup Observables /** \addtogroup Observables
* \{ */ * \{ */
@ -84,24 +83,6 @@ private:
Obs_Conf d_conf; 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 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 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.has_status = d_current_has_status;
d_HAS_data.message_id = d_current_message_id; 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); auto has_data_ptr = std::make_shared<Galileo_HAS_data>(d_HAS_data);
d_new_message = false; d_new_message = false;
d_printed_mids[d_current_message_id] = true; 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.has_status = d_current_has_status;
d_HAS_data.message_id = d_current_message_id; 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_mids[d_current_message_id] = true;
d_printed_timestamps[d_current_message_id] = timestamp; d_printed_timestamps[d_current_message_id] = timestamp;
auto has_data_ptr = std::make_shared<Galileo_HAS_data>(d_HAS_data); 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 // count satellites in the mask
std::bitset<HAS_MSG_SATELLITE_MASK_LENGTH> satellite_mask_bits(satellite_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 = satellite_mask_bits.count();
int number_sats_this_gnss_id = std::count(satellite_mask_string.begin(), satellite_mask_string.end(), '1');
d_HAS_data.satellite_submask[i] = read_has_message_body_uint64(message.substr(0, number_sats_this_gnss_id)); 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()); 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: public:
Galileo_HAS_data() = default; 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_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::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::vector<float>> get_code_bias_m() const; //!< Get Nsat x Ncodes code biases in [m] std::vector<std::string> get_systems_string() const; //!< Get Nsys system name strings
std::vector<std::vector<float>> get_phase_bias_cycle() const; //!< Get Nsat x Nphases phase biases in [cycles] std::vector<std::string> get_systems_subset_string() const; //!< Get Nsat system name strings present in clock corrections subset
std::vector<float> get_delta_radial_m() const; //!< Get Nsat delta radial corrections in [m] std::vector<std::vector<float>> get_code_bias_m() const; //!< Get Nsat x Ncodes code biases 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<std::vector<float>> get_phase_bias_cycle() const; //!< Get Nsat x Nphases phase biases in [cycles]
std::vector<float> get_delta_in_track_m() const; //!< Get Nsat delta in-track corrections in [m] 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_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_radial_m() const; //!< Get Nsat delta radial corrections in [m]
std::vector<float> get_delta_cross_track_m() const; //!< Get Nsat delta cross-track 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_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_in_track_m() const; //!< Get Nsat delta in-track corrections in [m]
std::vector<float> get_delta_clock_correction_m() const; //!< Get Nsat delta clock C0 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_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_cross_track_m() const; //!< Get Nsat delta cross-track corrections in [m]
std::vector<int> get_PRNs_in_mask(uint8_t nsys) const; //!< Get PRNs in mask for system nsys, with 0 <= nsys < Nsys 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<int> get_PRNs_in_submask(uint8_t nsys) const; //!< Get PRNs in submask 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<uint16_t> get_gnss_iod(uint8_t nsys) const; //!< Get GNSS IODs for for system nsys, with 0 <= nsys < Nsys 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<uint8_t> get_num_satellites() const; //!< Get Nsys number of satellites 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
uint16_t get_nsat() const; //!< Get total number of satellites with corrections std::vector<int> get_PRNs_in_mask(uint8_t nsys) const; //!< Get PRNs in mask for system nsys, with 0 <= nsys < Nsys
uint16_t get_nsat_sub() const; //!< Get number of satellites in clock subset corrections std::vector<int> get_PRNs_in_mask(const std::string& system) const; //!< Get PRNs in mask for system ("GPS"/"Galileo")
uint16_t get_validity_interval_s(uint8_t validity_interval_index) const; //!< Get validity interval in [s] from the validity_interval_index std::vector<int> get_PRNs_in_submask(uint8_t nsys) const; //!< Get PRNs in submask for system nsys, with 0 <= nsys < Nsys
uint8_t get_gnss_id(int nsat) const; //!< Get GNSS ID from the nsat satellite 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 // Mask
std::vector<uint8_t> gnss_id_mask; //!< GNSS ID. See HAS SIS ICD 1.0 Section 5.2.1.1 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 #ifndef GNSS_SDR_GNSS_FREQUENCIES_H
#define GNSS_SDR_GNSS_FREQUENCIES_H #define GNSS_SDR_GNSS_FREQUENCIES_H
#include <string>
#include <unordered_map>
/** \addtogroup Core /** \addtogroup Core
* \{ */ * \{ */
/** \addtogroup System_Parameters /** \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 FREQ2_BDS = 1.20714e9; //!< BeiDou B2 frequency (Hz)
constexpr double FREQ3_BDS = 1.26852e9; //!< BeiDou B3 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][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_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 else
{ {