diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index f4a3a4c57..3273e40bf 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1415,7 +1415,7 @@ std::map Rtklib_Solver::get_galileo_ephemeris_map_for_pv 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 + Galileo_Ephemeris &ephemeris, bool &from_reduced_ced) { from_reduced_ced = false; if (!is_galileo_signal_used_in_pvt(signal) || observation_tow >= 604800U) @@ -1423,12 +1423,49 @@ bool Rtklib_Solver::select_galileo_ephemeris(uint32_t prn, const std::string &si return false; } + // Prefer this satellite's ephemeris in the receiver's primary navigation + // service (see the constructor for how that's chosen). Once a satellite + // has successfully used it, it's locked onto that service (below) and + // this function stops trying the other service for it, so a promoted + // satellite's clock/orbit model never bounces back and forth between + // services for no reason -- each such bounce would be a small + // discontinuity even though both services are individually accurate. + // The lock is released the moment the primary-service ephemeris is + // found unusable, whether that's because it just expired for a + // previously-locked satellite or because it was never decoded in the + // first place -- either way, this (re)starts the same bootstrap + // fallback below, so an expired primary ephemeris never permanently + // excludes a satellite that still has a perfectly usable ephemeris from + // the other service. E.g. E1B's I/NAV ephemeris typically decodes well + // before E5a's F/NAV does, so an F/NAV-primary receiver would otherwise + // exclude a satellite from PVT for no reason while a perfectly usable + // I/NAV ephemeris sits unused (this matters most when E1B is Doppler- + // assisting E5a acquisition via GNSS-SDR.assist_dual_frequency_acq, + // since E5a reception can be weak enough that F/NAV never fully decodes + // at all). const Galileo_Ephemeris *full_ephemeris = galileo_ephemeris_store.find( static_cast(prn), d_galileo_nav_message_type_for_pvt); + if (full_ephemeris != nullptr && galileo_ephemeris_is_usable(*full_ephemeris, observation_tow)) + { + ephemeris = *full_ephemeris; + d_galileo_primary_nav_locked_prns_.insert(prn); + return true; + } + d_galileo_primary_nav_locked_prns_.erase(prn); + + // Not locked to the primary service (either never was, or just got + // un-locked above) -- bootstrap with whichever other service IS + // currently available for this satellite instead. + const auto other_nav_message_type = (d_galileo_nav_message_type_for_pvt == Galileo_Nav_Message_Type::INAV) + ? Galileo_Nav_Message_Type::FNAV + : Galileo_Nav_Message_Type::INAV; + full_ephemeris = galileo_ephemeris_store.find(static_cast(prn), other_nav_message_type); const auto compatibility_ephemeris = galileo_ephemeris_map.find(static_cast(prn)); - if (full_ephemeris == nullptr && compatibility_ephemeris != galileo_ephemeris_map.cend() && + if ((full_ephemeris == nullptr || !galileo_ephemeris_is_usable(*full_ephemeris, observation_tow)) && + compatibility_ephemeris != galileo_ephemeris_map.cend() && (compatibility_ephemeris->second.nav_message_type == Galileo_Nav_Message_Type::Unknown || - compatibility_ephemeris->second.nav_message_type == d_galileo_nav_message_type_for_pvt)) + compatibility_ephemeris->second.nav_message_type == d_galileo_nav_message_type_for_pvt || + compatibility_ephemeris->second.nav_message_type == other_nav_message_type)) { full_ephemeris = &compatibility_ephemeris->second; } @@ -1438,9 +1475,11 @@ bool Rtklib_Solver::select_galileo_ephemeris(uint32_t prn, const std::string &si return true; } - // The ICD only defines Reduced CED use for the E1/E5b service. - if (d_galileo_nav_message_type_for_pvt != Galileo_Nav_Message_Type::INAV || - (signal != "1B" && signal != "7X")) + // The ICD only defines Reduced CED use for the E1/E5b service (I/NAV), + // as a startup bootstrap the same as the fallback above -- gate on the + // signal itself; reaching here already means the primary service isn't + // in use for this satellite this epoch (see above). + if (signal != "1B" && signal != "7X") { return false; } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index bca354d50..75a844248 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -81,6 +81,7 @@ #include #include #include +#include #include #include @@ -163,11 +164,25 @@ public: bool get_galileo_signal_health(uint32_t prn, const std::string& signal, uint32_t observation_tow, bool& healthy) const; std::map get_galileo_ephemeris_map_for_pvt() const; bool select_galileo_ephemeris(uint32_t prn, const std::string& signal, uint32_t observation_tow, - Galileo_Ephemeris& ephemeris, bool& from_reduced_ced) const; + Galileo_Ephemeris& ephemeris, bool& from_reduced_ced); sol_t pvt_sol{}; std::array pvt_ssat{}; + // PRNs that have used the receiver's primary Galileo navigation service + // (see the constructor for how that's chosen) at least once. Once a PRN + // is in this set, select_galileo_ephemeris() never falls back to the + // other service for it again, even if the primary type's ephemeris is + // temporarily unusable -- the fallback exists purely to bootstrap + // Galileo availability at startup, before any given satellite's primary- + // service ephemeris has decoded yet, not as a general per-epoch + // failover. Without this, a satellite could bounce between services + // epoch to epoch (e.g. if the fallback type's ephemeris happens to sit + // right at its staleness cutoff), each bounce introducing a small + // clock/orbit discontinuity for that satellite even though both + // services are individually accurate. + std::set d_galileo_primary_nav_locked_prns_; + Galileo_Ephemeris_Store galileo_ephemeris_store; //!< Source-aware Galileo ephemeris storage std::map galileo_ephemeris_map; //!< Compatibility PVT view; source-aware storage is authoritative std::map galileo_reduced_ced_map; //!< Map storing provisional Galileo Reduced CED