From a7e72cd522015dd98d15b635a2ca27f175f4bacd Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 19 Jul 2026 10:10:56 +0200 Subject: [PATCH] Fix Galileo reduced CED handling --- docs/CHANGELOG.md | 4 +- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 95 +++++---- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 2 + src/algorithms/PVT/libs/rtklib_solver.cc | 198 ++++++++++++++---- src/algorithms/PVT/libs/rtklib_solver.h | 4 + .../galileo_telemetry_decoder_gs.cc | 87 ++++++-- .../galileo_telemetry_decoder_gs.h | 6 + .../system_parameters/galileo_inav_message.cc | 36 +--- .../system_parameters/galileo_inav_message.h | 18 +- .../system_parameters/galileo_reduced_ced.cc | 39 +++- .../system_parameters/galileo_reduced_ced.h | 42 +++- 11 files changed, 387 insertions(+), 144 deletions(-) diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index d8fe01510..5734c58a8 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -483,8 +483,8 @@ https://gnss-sdr.org/design-forces/ ### Improvements in Usability: - The Galileo E1B Reduced CED parameters usage has been set to `false` by - default. You can activate its usage with `Galileo_E1B_Telemetry_Decoder=true` - in your configuration file. + default. You can activate its usage with + `TelemetryDecoder_1B.use_reduced_ced=true` in your configuration file. - The generation of Galileo E6B observables has been disabled if the user sets `PVT.use_e6_for_pvt=false`, fixing the PVT computation in some multi-band configurations. diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index ad43f764b..ff13dc17f 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -28,6 +28,7 @@ #include "galileo_ephemeris.h" #include "galileo_has_data.h" #include "galileo_iono.h" +#include "galileo_reduced_ced.h" #include "galileo_utc_model.h" #include "geohash.h" #include "geojson_printer.h" @@ -145,6 +146,7 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_gps_cnav_utc_model_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), d_gps_almanac_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), d_galileo_ephemeris_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), + d_galileo_reduced_ced_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), d_galileo_iono_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), d_galileo_utc_model_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), d_galileo_almanac_helper_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), @@ -1386,6 +1388,22 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) } } } + else if (msg_type_hash_code == d_galileo_reduced_ced_sptr_type_hash_code) + { + // Reduced CED remains separate from full Galileo ephemerides so it cannot + // enter RINEX, RTCM, HAS, or IOD-based OSNMA processing. + const auto reduced_ced = wht::any_cast>(pmt::any_ref(msg)); + if (reduced_ced->PRN >= 1U && reduced_ced->PRN <= 36U) + { + d_internal_pvt_solver->galileo_reduced_ced_map[reduced_ced->PRN] = *reduced_ced; + if (d_enable_rx_clock_correction == true) + { + d_user_pvt_solver->galileo_reduced_ced_map[reduced_ced->PRN] = *reduced_ced; + } + DLOG(INFO) << "Galileo Reduced CED stored for PRN " << reduced_ced->PRN + << " at WN=" << reduced_ced->WN << ", TOW=" << reduced_ced->TOTRedCED; + } + } else if (msg_type_hash_code == d_galileo_iono_sptr_type_hash_code) { // ### Galileo IONO ### @@ -1706,6 +1724,7 @@ void rtklib_pvt_gs::clear_ephemeris() d_internal_pvt_solver->gps_ephemeris_map.clear(); d_internal_pvt_solver->gps_almanac_map.clear(); d_internal_pvt_solver->galileo_ephemeris_map.clear(); + d_internal_pvt_solver->galileo_reduced_ced_map.clear(); d_internal_pvt_solver->galileo_almanac_map.clear(); d_internal_pvt_solver->beidou_dnav_ephemeris_map.clear(); d_internal_pvt_solver->beidou_dnav_almanac_map.clear(); @@ -1714,6 +1733,7 @@ void rtklib_pvt_gs::clear_ephemeris() d_user_pvt_solver->gps_ephemeris_map.clear(); d_user_pvt_solver->gps_almanac_map.clear(); d_user_pvt_solver->galileo_ephemeris_map.clear(); + d_user_pvt_solver->galileo_reduced_ced_map.clear(); d_user_pvt_solver->galileo_almanac_map.clear(); d_user_pvt_solver->beidou_dnav_ephemeris_map.clear(); d_user_pvt_solver->beidou_dnav_almanac_map.clear(); @@ -2044,52 +2064,55 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item store_valid_observable = true; } } - if (tmp_eph_iter_gal != d_internal_pvt_solver->galileo_ephemeris_map.cend()) + Galileo_Ephemeris selected_galileo_ephemeris; + bool selected_from_reduced_ced = false; + const std::string galileo_signal(gnss_synchro.Signal, 2); + const uint32_t galileo_observation_tow = static_cast(gnss_synchro.interp_TOW_ms / 1000.0); + if (d_internal_pvt_solver->select_galileo_ephemeris(gnss_synchro.PRN, + galileo_signal, galileo_observation_tow, + selected_galileo_ephemeris, selected_from_reduced_ced)) { - const uint32_t prn_aux = tmp_eph_iter_gal->second.PRN; - if ((prn_aux == gnss_synchro.PRN) && - (((std::string(gnss_synchro.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(gnss_synchro.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(gnss_synchro.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)))))) + const bool signal_is_healthy = + (galileo_signal == "1B" && (d_use_unhealthy_sats || (!selected_galileo_ephemeris.E1B_DVS && selected_galileo_ephemeris.E1B_HS == 0))) || + (galileo_signal == "5X" && (d_use_unhealthy_sats || (!selected_galileo_ephemeris.E5a_DVS && selected_galileo_ephemeris.E5a_HS == 0))) || + (galileo_signal == "7X" && (d_use_unhealthy_sats || (!selected_galileo_ephemeris.E5b_DVS && selected_galileo_ephemeris.E5b_HS == 0))); + if (signal_is_healthy && !selected_from_reduced_ced && d_osnma_strict) { - if (d_osnma_strict) + // Pick up only recently authenticated full-precision navigation data. + const auto eph_gst = osnma::galileo_gst_seconds(osnma::galileo_week_to_uint(selected_galileo_ephemeris.WN), + osnma::galileo_tow_to_uint(selected_galileo_ephemeris.tow)); + auto IOD_nav_list = d_auth_nav_data_map.find(selected_galileo_ephemeris.PRN); + if (IOD_nav_list != d_auth_nav_data_map.cend()) { - // Pick up only recently authenticated nav data. IOD_nav is 10 bits and can be reused. - const auto eph_gst = osnma::galileo_gst_seconds(osnma::galileo_week_to_uint(tmp_eph_iter_gal->second.WN), - osnma::galileo_tow_to_uint(tmp_eph_iter_gal->second.tow)); - auto IOD_nav_list = d_auth_nav_data_map.find(tmp_eph_iter_gal->second.PRN); - if (IOD_nav_list != d_auth_nav_data_map.cend()) + for (auto auth_it = IOD_nav_list->second.begin(); auth_it != IOD_nav_list->second.end();) { - for (auto auth_it = IOD_nav_list->second.begin(); auth_it != IOD_nav_list->second.end();) + if (osnma::auth_gst_is_stale(auth_it->second, eph_gst)) { - if (osnma::auth_gst_is_stale(auth_it->second, eph_gst)) - { - auth_it = IOD_nav_list->second.erase(auth_it); - } - else - { - ++auth_it; - } + auth_it = IOD_nav_list->second.erase(auth_it); } - - const auto IOD_nav = static_cast(tmp_eph_iter_gal->second.IOD_nav); - const auto auth_it = IOD_nav_list->second.find(IOD_nav); - if (auth_it != IOD_nav_list->second.cend() && - osnma::auth_gst_matches_nav_data(auth_it->second, eph_gst)) + else { - store_valid_observable = true; - } - - if (IOD_nav_list->second.empty()) - { - d_auth_nav_data_map.erase(IOD_nav_list); + ++auth_it; } } + + const auto IOD_nav = static_cast(selected_galileo_ephemeris.IOD_nav); + const auto auth_it = IOD_nav_list->second.find(IOD_nav); + if (auth_it != IOD_nav_list->second.cend() && + osnma::auth_gst_matches_nav_data(auth_it->second, eph_gst)) + { + store_valid_observable = true; + } + + if (IOD_nav_list->second.empty()) + { + d_auth_nav_data_map.erase(IOD_nav_list); + } } - else - { - store_valid_observable = true; - } + } + else if (signal_is_healthy && !d_osnma_strict) + { + store_valid_observable = true; } } if (!d_osnma_strict && tmp_eph_iter_cnav != d_internal_pvt_solver->gps_cnav_ephemeris_map.cend()) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 2d5198cf8..4e4f1c17e 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -54,6 +54,7 @@ class Beidou_Dnav_Ephemeris; class Galileo_Almanac; class Galileo_Ephemeris; class Galileo_HAS_data; +class Galileo_Reduced_CED; class Geohash; class GeoJSON_Printer; class Gps_Almanac; @@ -224,6 +225,7 @@ private: const size_t d_gps_cnav_utc_model_sptr_type_hash_code; const size_t d_gps_almanac_sptr_type_hash_code; const size_t d_galileo_ephemeris_sptr_type_hash_code; + const size_t d_galileo_reduced_ced_sptr_type_hash_code; const size_t d_galileo_iono_sptr_type_hash_code; const size_t d_galileo_utc_model_sptr_type_hash_code; const size_t d_galileo_almanac_helper_sptr_type_hash_code; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 3a78745d7..983dafdc0 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -38,8 +38,10 @@ #include "rtklib_rtkpos.h" #include "signal_enabled_flags.h" #include +#include #include #include +#include #include #include @@ -1147,10 +1149,88 @@ void Rtklib_Solver::clear_applied_has_phase_bias_discontinuity(const HAS_obs_cor } +bool Rtklib_Solver::select_galileo_ephemeris(uint32_t prn, const std::string &signal, uint32_t observation_tow, + Galileo_Ephemeris &ephemeris, bool &from_reduced_ced) const +{ + from_reduced_ced = false; + if (observation_tow >= 604800U) + { + return false; + } + + const auto full_ephemeris = galileo_ephemeris_map.find(static_cast(prn)); + if (full_ephemeris != galileo_ephemeris_map.cend()) + { + double toe_distance = std::fabs(static_cast(observation_tow) - full_ephemeris->second.toe); + if (toe_distance > 302400.0) + { + toe_distance = 604800.0 - toe_distance; + } + if (toe_distance <= MAXDTOE_GAL) + { + ephemeris = full_ephemeris->second; + return true; + } + } + + // The ICD only defines Reduced CED use for the E1/E5b service. + if (signal != "1B" && signal != "7X") + { + return false; + } + + const auto reduced_ced = galileo_reduced_ced_map.find(static_cast(prn)); + if (reduced_ced == galileo_reduced_ced_map.cend()) + { + return false; + } + + uint32_t observation_week = reduced_ced->second.WN; + uint32_t week_reference_tow = reduced_ced->second.TOTRedCED; + // A later GST model prevents a stale Reduced CED from becoming valid + // again after a week. Older assistance data must not override the directly + // decoded Reduced CED epoch. + if (galileo_iono.WN > 0 && + static_cast(galileo_iono.WN) > reduced_ced->second.WN && + galileo_iono.tow >= 0 && galileo_iono.tow < 604800) + { + observation_week = static_cast(galileo_iono.WN); + week_reference_tow = static_cast(galileo_iono.tow); + } + + if (observation_tow < week_reference_tow && + week_reference_tow - observation_tow > 302400U) + { + if (observation_week == std::numeric_limits::max()) + { + return false; + } + ++observation_week; + } + else if (observation_tow > week_reference_tow && + observation_tow - week_reference_tow > 302400U) + { + if (observation_week == 0U) + { + return false; + } + --observation_week; + } + + if (!reduced_ced->second.is_valid_at(observation_week, observation_tow)) + { + return false; + } + + ephemeris = reduced_ced->second.compute_eph(); + from_reduced_ced = true; + return true; +} + + bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, double kf_update_interval_s, const SensorDataAggregator &sensor_data_aggregator, bool dump_this_epoch) { std::map::const_iterator gnss_observables_iter; - std::map::const_iterator galileo_ephemeris_iter; std::map::const_iterator gps_ephemeris_iter; std::map::const_iterator gps_cnav_ephemeris_iter; std::map::const_iterator glonass_gnav_ephemeris_iter; @@ -1180,31 +1260,43 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ const std::string sig_(gnss_observables_iter->second.Signal, 2); const uint32_t obs_tow = gnss_observables_iter->second.interp_TOW_ms / 1000.0; const int prn = static_cast(gnss_observables_iter->second.PRN); - if (has_active_has_do_not_use(gal_str, prn, obs_tow)) + Galileo_Ephemeris selected_galileo_ephemeris; + bool selected_from_reduced_ced = false; + const bool has_selected_galileo_ephemeris = select_galileo_ephemeris( + gnss_observables_iter->second.PRN, sig_, obs_tow, + selected_galileo_ephemeris, selected_from_reduced_ced); + if (!has_selected_galileo_ephemeris || + (!selected_from_reduced_ced && has_active_has_do_not_use(gal_str, prn, obs_tow))) { break; } // Galileo E1 if (sig_ == "1B") { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + if (has_selected_galileo_ephemeris) { // convert ephemeris from GNSS-SDR class to RTKLIB structure - 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]); + eph_data[valid_obs] = selected_from_reduced_ced ? eph_to_rtklib(selected_galileo_ephemeris) : eph_to_rtklib(selected_galileo_ephemeris, this->d_has_orbit_corrections_store_map[gal_str], this->d_has_clock_corrections_store_map[gal_str]); // convert observation from GNSS-SDR class to RTKLIB structure obsd_t newobs{}; const HAS_obs_corrections *applied_has_correction = nullptr; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_], - &applied_has_correction, - false); + if (selected_from_reduced_ced) + { + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + selected_galileo_ephemeris.WN, + d_rtklib_band_index[sig_]); + } + else + { + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + d_has_obs_corr_map, + selected_galileo_ephemeris.WN, + d_rtklib_band_index[sig_], + &applied_has_correction, + false); + } clear_applied_has_phase_bias_discontinuity(applied_has_correction, prn); valid_obs++; } @@ -1217,9 +1309,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // Galileo E5 if ((sig_ == "5X") || (sig_ == "7X")) { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + if (has_selected_galileo_ephemeris) { bool found_E1_obs = false; for (int i = 0; i < valid_obs; i++) @@ -1227,13 +1317,23 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) { const HAS_obs_corrections *applied_has_correction = nullptr; - d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], - gnss_observables_iter->second, - d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_], - &applied_has_correction, - false); + if (selected_from_reduced_ced) + { + d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], + gnss_observables_iter->second, + selected_galileo_ephemeris.WN, + d_rtklib_band_index[sig_]); + } + else + { + d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], + gnss_observables_iter->second, + d_has_obs_corr_map, + selected_galileo_ephemeris.WN, + d_rtklib_band_index[sig_], + &applied_has_correction, + false); + } clear_applied_has_phase_bias_discontinuity(applied_has_correction, prn); found_E1_obs = true; break; @@ -1243,22 +1343,30 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { // insert Galileo E5 obs as new obs and also insert its ephemeris // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, - this->d_has_orbit_corrections_store_map[gal_str], - this->d_has_clock_corrections_store_map[gal_str]); + eph_data[valid_obs] = selected_from_reduced_ced ? eph_to_rtklib(selected_galileo_ephemeris) : eph_to_rtklib(selected_galileo_ephemeris, this->d_has_orbit_corrections_store_map[gal_str], this->d_has_clock_corrections_store_map[gal_str]); // convert observation from GNSS-SDR class to RTKLIB structure const auto default_code_ = static_cast(CODE_NONE); obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {default_code_, default_code_, default_code_}, {}, {0.0, 0.0, 0.0}, {}}; const HAS_obs_corrections *applied_has_correction = nullptr; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_], - &applied_has_correction, - false); + if (selected_from_reduced_ced) + { + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + selected_galileo_ephemeris.WN, + d_rtklib_band_index[sig_]); + } + else + { + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + d_has_obs_corr_map, + selected_galileo_ephemeris.WN, + d_rtklib_band_index[sig_], + &applied_has_correction, + false); + } clear_applied_has_phase_bias_discontinuity(applied_has_correction, prn); valid_obs++; } @@ -1270,8 +1378,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } if (sig_ == "E6" && d_conf.use_e6_for_pvt) { - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + if (has_selected_galileo_ephemeris) { bool found_E1_obs = false; for (int i = 0; i < valid_obs; i++) @@ -1282,7 +1389,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], gnss_observables_iter->second, d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, + selected_galileo_ephemeris.WN, d_rtklib_band_index[sig_], &applied_has_correction, false); @@ -1295,7 +1402,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { // insert Galileo E6 obs as new obs and also insert its ephemeris // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, + eph_data[valid_obs] = eph_to_rtklib(selected_galileo_ephemeris, 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 @@ -1307,7 +1414,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, + selected_galileo_ephemeris.WN, d_rtklib_band_index[sig_], &applied_has_correction, false); @@ -1322,8 +1429,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } else if (sig_ == "E6") { - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + if (has_selected_galileo_ephemeris) { bool found_E1_obs = false; for (int i = 0; i < valid_obs; i++) @@ -1332,7 +1438,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], gnss_observables_iter->second, - galileo_ephemeris_iter->second.WN, + selected_galileo_ephemeris.WN, 2); // Band E6 found_E1_obs = true; break; @@ -1342,7 +1448,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { // insert Galileo E6 obs as new obs and also insert its ephemeris // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); + eph_data[valid_obs] = eph_to_rtklib(selected_galileo_ephemeris); // convert observation from GNSS-SDR class to RTKLIB structure const auto default_code_ = static_cast(CODE_NONE); obsd_t newobs = {{0, 0}, '0', '0', {}, {}, @@ -1350,9 +1456,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ {}, {0.0, 0.0, 0.0}, {}}; d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, - galileo_ephemeris_iter->second.WN, + selected_galileo_ephemeris.WN, 2); // Band E6 - // std::cout << "Week " << galileo_ephemeris_iter->second.WN << '\n'; + // std::cout << "Week " << selected_galileo_ephemeris.WN << '\n'; valid_obs++; } } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 439745459..cd80f7190 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -43,6 +43,7 @@ #include "galileo_ephemeris.h" #include "galileo_has_data.h" #include "galileo_iono.h" +#include "galileo_reduced_ced.h" #include "galileo_utc_model.h" #include "glonass_gnav_almanac.h" #include "glonass_gnav_ephemeris.h" @@ -101,11 +102,14 @@ public: void store_has_data(const Galileo_HAS_data& new_has_data); void clear_has_corrections(); void update_has_corrections(const std::map& obs_map); + bool select_galileo_ephemeris(uint32_t prn, const std::string& signal, uint32_t observation_tow, + Galileo_Ephemeris& ephemeris, bool& from_reduced_ced) const; sol_t pvt_sol{}; std::array pvt_ssat{}; std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris + std::map galileo_reduced_ced_map; //!< Map storing provisional Galileo Reduced CED std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris std::map gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris std::map glonass_gnav_ephemeris_map; //!< Map storing new GLONASS GNAV Ephemeris diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 0feacc0a7..3e08564f4 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -88,6 +88,8 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(const Tlm_Conf &conf, d_preamble_index(0ULL), d_last_valid_preamble(0ULL), d_received_sample_counter(0), + d_pending_reduced_ced_start_symbol(0ULL), + d_pending_reduced_ced_cn0(0.0), d_frame_type(frame_type), d_CRC_error_counter(0), d_channel(0), @@ -108,6 +110,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(const Tlm_Conf &conf, d_dump_mat(conf.dump_mat), d_remove_dat(conf.remove_dat), d_first_eph_sent(false), + d_pending_reduced_ced(false), d_cnav_dummy_page(false), d_print_cnav_page(true), d_enable_navdata_monitor(conf.enable_navdata_monitor), @@ -420,6 +423,65 @@ void galileo_telemetry_decoder_gs::advance_current_tow(int64_t delta_ms) } +void galileo_telemetry_decoder_gs::capture_pending_reduced_ced(double cn0) +{ + if (d_band != '1' || !d_use_ced || d_first_eph_sent) + { + return; + } + + const int64_t decoder_delay_ms = galileo_tow::inav_current_symbol_delay_ms(d_required_symbols, d_PRN_code_period_ms); + const uint64_t decoder_delay_symbols = static_cast(decoder_delay_ms / static_cast(d_PRN_code_period_ms)); + if (d_symbol_counter < decoder_delay_symbols) + { + return; + } + + d_pending_reduced_ced_start_symbol = d_symbol_counter - decoder_delay_symbols; + d_pending_reduced_ced_cn0 = cn0; + d_pending_reduced_ced = true; +} + + +void galileo_telemetry_decoder_gs::publish_pending_reduced_ced() +{ + if (!d_pending_reduced_ced || d_first_eph_sent || !d_inav_nav.has_valid_word_5() || + d_TOW_week == GALILEO_TOW_MAP_INVALID_WEEK) + { + return; + } + + const int64_t symbol_delta = galileo_tow::sample_counter_delta(d_pending_reduced_ced_start_symbol, d_symbol_counter); + const int64_t code_period_ms = static_cast(d_PRN_code_period_ms); + if (symbol_delta > std::numeric_limits::max() / code_period_ms || + symbol_delta < std::numeric_limits::min() / code_period_ms) + { + d_pending_reduced_ced = false; + return; + } + const int64_t tow_delta_ms = symbol_delta * code_period_ms; + uint32_t reduced_ced_week = GALILEO_TOW_MAP_INVALID_WEEK; + if (!galileo_tow::week_after_delta(d_TOW_week, d_TOW_at_current_symbol_ms, tow_delta_ms, reduced_ced_week)) + { + d_pending_reduced_ced = false; + return; + } + + Galileo_Reduced_CED reduced_ced = d_inav_nav.get_reduced_ced(); + reduced_ced.WN = reduced_ced_week; + reduced_ced.TOTRedCED = galileo_tow::add_ms(d_TOW_at_current_symbol_ms, tow_delta_ms) / 1000U; + const auto tmp_obj = std::make_shared(reduced_ced); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + + const auto default_precision = std::cout.precision(); + std::cout << "New Galileo E1 I/NAV reduced CED message received in channel " + << d_channel << " from satellite " << d_satellite << " with CN0=" + << std::setprecision(2) << d_pending_reduced_ced_cn0 << std::setprecision(default_precision) + << " dB-Hz" << std::endl; + d_pending_reduced_ced = false; +} + + galileo_telemetry_decoder_gs::CnavPageReceptionTime galileo_telemetry_decoder_gs::get_cnav_page_reception_time(const Gnss_Synchro ¤t_symbol) const { CnavPageReceptionTime page_reception_time{GALILEO_TOW_MAP_INVALID_WEEK, GALILEO_TOW_MAP_INVALID_TOW_MS}; @@ -563,6 +625,11 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in d_inav_nav.reset_osnma_nav_bits_adkd4(); } + if (d_inav_nav.have_new_reduced_ced()) + { + capture_pending_reduced_ced(cn0); + } + if (d_inav_nav.have_new_ephemeris() == true) // C: tells if W1-->W4 available from same block (and W5!) { // get object for this SV (mandatory) @@ -586,20 +653,7 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in } this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); d_first_eph_sent = true; // do not send reduced CED anymore, since we have the full ephemeris set - } - else - { - // If we still do not have ephemeris, check if we have a reduced CED - if ((d_band == '1') && d_use_ced && !d_first_eph_sent && (d_inav_nav.have_new_reduced_ced() == true)) - { - const std::shared_ptr tmp_obj = std::make_shared(d_inav_nav.get_reduced_ced()); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - const auto default_precision = std::cout.precision(); - std::cout << "New Galileo E1 I/NAV reduced CED message received in channel " - << d_channel << " from satellite " << d_satellite << " with CN0=" - << std::setprecision(2) << cn0 << std::setprecision(default_precision) - << " dB-Hz" << std::endl; - } + d_pending_reduced_ced = false; } if (d_inav_nav.have_new_iono_and_GST() == true) // C: W5 @@ -876,6 +930,8 @@ void galileo_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satellite d_galileo_week_valid = false; d_E6_TOW_set = false; d_valid_timetag = false; + d_first_eph_sent = false; + d_pending_reduced_ced = false; d_inav_nav.init_PRN(d_satellite.get_PRN()); d_symbol_history.clear(); clear_galileo_tow_map_entry(); @@ -906,6 +962,8 @@ void galileo_telemetry_decoder_gs::reset() d_received_sample_counter = GALILEO_TOW_MAP_INVALID_SAMPLE_COUNTER; d_viterbi->reset(); d_valid_timetag = false; + d_first_eph_sent = false; + d_pending_reduced_ced = false; d_symbol_history.clear(); clear_galileo_tow_map_entry(); if (d_enable_reed_solomon_inav == true) @@ -1242,6 +1300,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( advance_current_tow(d_PRN_code_period_ms); } } + publish_pending_reduced_ced(); if (d_enable_navdata_monitor && !d_nav_msg_packet.nav_message.empty()) { d_nav_msg_packet.system = std::string(1, current_symbol.System); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index e37bc67c2..6d9f2907f 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -79,6 +79,8 @@ private: bool update_known_galileo_week(int32_t week); bool set_current_tow_from_preamble(uint32_t preamble_week, uint32_t preamble_tow_ms, int64_t delay_ms); void advance_current_tow(int64_t delta_ms); + void capture_pending_reduced_ced(double cn0); + void publish_pending_reduced_ced(); CnavPageReceptionTime get_cnav_page_reception_time(const Gnss_Synchro ¤t_symbol) const; void deinterleaver(int32_t rows, int32_t cols, const float *in, float *out); void decode_INAV_word(float *page_part_symbols, int32_t frame_length, double cn0); @@ -112,6 +114,9 @@ private: uint64_t d_preamble_index; uint64_t d_last_valid_preamble; uint64_t d_received_sample_counter; + uint64_t d_pending_reduced_ced_start_symbol; + + double d_pending_reduced_ced_cn0; int32_t d_mm; int32_t d_codelength; @@ -146,6 +151,7 @@ private: const bool d_dump_mat; const bool d_remove_dat; bool d_first_eph_sent; + bool d_pending_reduced_ced; bool d_cnav_dummy_page; bool d_print_cnav_page; const bool d_enable_navdata_monitor; diff --git a/src/core/system_parameters/galileo_inav_message.cc b/src/core/system_parameters/galileo_inav_message.cc index 3153df7ed..069a8b488 100644 --- a/src/core/system_parameters/galileo_inav_message.cc +++ b/src/core/system_parameters/galileo_inav_message.cc @@ -417,7 +417,7 @@ bool Galileo_Inav_Message::have_new_almanac() // Check if we have a new almanac bool Galileo_Inav_Message::have_new_reduced_ced() { // Check if we have a new CED data set stored in the galileo navigation class - if ((flag_CED == true) && (WN_5 > 0)) // We need the week number to compute GST + if (flag_CED == true) { flag_CED = false; return true; @@ -592,30 +592,10 @@ Galileo_Almanac_Helper Galileo_Inav_Message::get_almanac() const } -Galileo_Ephemeris Galileo_Inav_Message::get_reduced_ced() const +Galileo_Reduced_CED Galileo_Inav_Message::get_reduced_ced() const { Galileo_Reduced_CED ced{}; ced.PRN = SV_ID_PRN_4; - // From ICD: TOTRedCED is the start time of transmission of the - // Reduced CED word 16 in GST - if (TOW_5 > TOW_6) - { - ced.TOTRedCED = WN_5 * 604800 + TOW_5 + 4; // According to ICD 2.0, Table 38 - } - else - { - ced.TOTRedCED = WN_5 * 604800 + TOW_6 + 10; // According to ICD 2.0, Table 38 - } - std::array iod_navs = {IOD_nav_1, IOD_nav_2, IOD_nav_3, IOD_nav_4}; - int32_t max_IOD_nav = IOD_nav_1; - for (int i = 1; i < 4; i++) - { - if (iod_navs[i] > max_IOD_nav) - { - max_IOD_nav = iod_navs[i]; - } - } - ced.IODnav = max_IOD_nav; ced.DeltaAred = ced_DeltaAred; ced.exred = ced_exred; ced.eyred = ced_eyred; @@ -624,10 +604,13 @@ Galileo_Ephemeris Galileo_Inav_Message::get_reduced_ced() const ced.lambda0red = ced_lambda0red; ced.af0red = ced_af0red; ced.af1red = ced_af1red; + ced.BGD_E1E5b = BGD_E1E5b_5; + ced.E5b_HS = E5b_HS_5; + ced.E1B_HS = E1B_HS_5; + ced.E5b_DVS = E5b_DVS_5; + ced.E1B_DVS = E1B_DVS_5; - Galileo_Ephemeris eph = ced.compute_eph(); - - return eph; + return ced; } @@ -1061,7 +1044,8 @@ int32_t Galileo_Inav_Message::page_jk_decoder(const char* data_jk) spare_5 = static_cast(read_navigation_unsigned(data_jk_bits, SPARE_5_BIT)); DLOG(INFO) << "spare_5= " << spare_5; flag_iono_and_GST = true; // set to false externally - flag_TOW_set = true; // set to false externally + flag_word_5_received = true; + flag_TOW_set = true; // set to false externally DLOG(INFO) << "flag_tow_set" << flag_TOW_set; nav_bits_word_5 = data_jk_bits.to_string().substr(6, 67); break; diff --git a/src/core/system_parameters/galileo_inav_message.h b/src/core/system_parameters/galileo_inav_message.h index ab4c8bfd5..e20c5f0e3 100644 --- a/src/core/system_parameters/galileo_inav_message.h +++ b/src/core/system_parameters/galileo_inav_message.h @@ -24,6 +24,7 @@ #include "galileo_ephemeris.h" #include "galileo_iono.h" #include "galileo_ism.h" +#include "galileo_reduced_ced.h" #include "galileo_utc_model.h" #include "gnss_sdr_make_unique.h" // for std::unique_ptr in C++11 #include @@ -129,9 +130,17 @@ public: Galileo_Almanac_Helper get_almanac() const; /* - * \brief Returns a Galileo_Ephemeris object filled with the latest reduced CED received + * \brief Returns a Galileo_Reduced_CED object filled with the latest Word 16 and Word 5 data received */ - Galileo_Ephemeris get_reduced_ced() const; + Galileo_Reduced_CED get_reduced_ced() const; + + /* + * \brief Returns true after a CRC-valid Word 5 has been received for the current satellite. + */ + inline bool has_valid_word_5() const + { + return flag_word_5_received; + } /* * \brief Returns a Galileo_ISMs object filled with the latest ISM data received @@ -259,6 +268,8 @@ public: inline void init_PRN(uint32_t prn) { SV_ID_PRN_4 = prn; + flag_CED = false; + flag_word_5_received = false; nma_msg.PRN = prn; nma_msg.mack = std::array{}; nma_msg.hkroot = std::array{}; @@ -482,7 +493,8 @@ private: bool flag_ephemeris_3{}; // Flag indicating that ephemeris 3/4 (word 3) have been received bool flag_ephemeris_4{}; // Flag indicating that ephemeris 4/4 (word 4) have been received - bool flag_iono_and_GST{}; // Flag indicating that ionospheric and GST parameters (word 5) have been received + bool flag_iono_and_GST{}; // Flag indicating that ionospheric and GST parameters (word 5) have been received + bool flag_word_5_received{}; // Persistent Word 5 validity for Reduced CED auxiliary data bool flag_TOW_5{}; bool flag_TOW_6{}; bool flag_TOW_0{}; diff --git a/src/core/system_parameters/galileo_reduced_ced.cc b/src/core/system_parameters/galileo_reduced_ced.cc index 69ee40bfb..e5a1d6b2b 100644 --- a/src/core/system_parameters/galileo_reduced_ced.cc +++ b/src/core/system_parameters/galileo_reduced_ced.cc @@ -34,12 +34,13 @@ Galileo_Ephemeris Galileo_Reduced_CED::compute_eph() const eph.M_0 = lambda0red * GNSS_PI - eph.omega; // Mean anomaly at reference time [rad] eph.OMEGA_0 = Omega0red * GNSS_PI; // Longitude of ascending node of orbital plane at weekly epoch [rad] - eph.flag_all_ephemeris = true; - eph.IOD_ephemeris = IODnav; - eph.IOD_nav = IODnav; + // Word 16 has no IODnav and is not a full-precision ephemeris set. + eph.flag_all_ephemeris = false; + eph.IOD_ephemeris = -1; + eph.IOD_nav = -1; eph.PRN = PRN; - int32_t t0r = (30 * (TOTRedCED / 30) + 1) % 604800; + const uint32_t t0r = reference_time(); eph.toe = t0r; // Ephemeris reference time [s] // Clock correction parameters @@ -48,8 +49,34 @@ Galileo_Ephemeris Galileo_Reduced_CED::compute_eph() const eph.af1 = af1red; // SV clock drift correction coefficient [s/s] // GST - eph.WN = TOTRedCED / 604800; // Week number - eph.tow = TOTRedCED % 604800; // Time of Week + eph.WN = WN; + eph.tow = TOTRedCED; + + eph.BGD_E1E5b = BGD_E1E5b; + eph.E5b_HS = E5b_HS; + eph.E1B_HS = E1B_HS; + eph.E5b_DVS = E5b_DVS; + eph.E1B_DVS = E1B_DVS; return eph; } + + +uint32_t Galileo_Reduced_CED::reference_time() const +{ + return (30U * (TOTRedCED / 30U) + 1U) % 604800U; +} + + +bool Galileo_Reduced_CED::is_valid_at(uint32_t week, uint32_t tow) const +{ + if (TOTRedCED >= 604800U || tow >= 604800U) + { + return false; + } + + const uint64_t reference_epoch = static_cast(WN) * 604800ULL + reference_time(); + const uint64_t observation_epoch = static_cast(week) * 604800ULL + tow; + return observation_epoch >= reference_epoch && + observation_epoch - reference_epoch < static_cast(CED_VALIDITY_SECONDS); +} diff --git a/src/core/system_parameters/galileo_reduced_ced.h b/src/core/system_parameters/galileo_reduced_ced.h index 0478f7200..5f68b3df8 100644 --- a/src/core/system_parameters/galileo_reduced_ced.h +++ b/src/core/system_parameters/galileo_reduced_ced.h @@ -43,17 +43,37 @@ public: */ Galileo_Ephemeris compute_eph() const; - uint32_t PRN{}; //!< Satellite ID - int32_t TOTRedCED{}; //!< Start time of transmission of the Reduced CED word in GST - int32_t IODnav{}; //!< Issue of Data - double DeltaAred{}; //!< Difference between the Reduced CED semi-major axis and the nominal semi-major axis [meters] - double exred{}; //!< Reduced CED eccentricity vector component x - double eyred{}; //!< Reduced CED eccentricity vector component y - double Deltai0red{}; //!< Difference between the Reduced CED inclination angle at reference time and the nominal inclination [rad] - double Omega0red{}; //!< Reduced CED longitude of ascending node at weekly epoch [rad] - double lambda0red{}; //!< Reduced CED mean argument of latitude [rad] - double af0red{}; //!< Reduced CED satellite clock bias correction coefficient [seconds] - double af1red{}; //!< Reduced CED satellite clock drift correction coefficient [seconds/seconds] + /*! + * \brief Return true when this Reduced CED set is applicable at the given GST epoch. + */ + bool is_valid_at(uint32_t week, uint32_t tow) const; + + /*! + * \brief Return the Reduced CED orbit and clock reference time of week. + */ + uint32_t reference_time() const; + + static constexpr uint32_t CED_VALIDITY_SECONDS = 600U; + + uint32_t PRN{}; //!< Satellite ID + uint32_t WN{}; //!< GST week containing the start of transmission + uint32_t TOTRedCED{}; //!< Start time of transmission of the Reduced CED word as GST TOW [s] + double DeltaAred{}; //!< Difference between the Reduced CED semi-major axis and the nominal semi-major axis [meters] + double exred{}; //!< Reduced CED eccentricity vector component x + double eyred{}; //!< Reduced CED eccentricity vector component y + double Deltai0red{}; //!< Difference between the Reduced CED inclination angle at reference time and the nominal inclination [rad] + double Omega0red{}; //!< Reduced CED longitude of ascending node at weekly epoch [rad] + double lambda0red{}; //!< Reduced CED mean argument of latitude [rad] + double af0red{}; //!< Reduced CED satellite clock bias correction coefficient [seconds] + double af1red{}; //!< Reduced CED satellite clock drift correction coefficient [seconds/seconds] + + // Word 5 data used by the E1/E5b user algorithms. These are not combined + // with full-precision CED orbit or clock parameters. + double BGD_E1E5b{}; //!< E1-E5b Broadcast Group Delay [s] + int32_t E5b_HS{}; //!< E5b Signal Health Status + int32_t E1B_HS{}; //!< E1B Signal Health Status + bool E5b_DVS{}; //!< E5b Data Validity Status + bool E1B_DVS{}; //!< E1B Data Validity Status };