From aa4c95ee889b4b1393b31338daf06752cb4a3e14 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 23 Sep 2026 20:15:01 +0200 Subject: [PATCH] Add GLONASS Doppler prediction --- src/core/receiver/gnss_flowgraph.cc | 5 +- src/core/receiver/satellite_visibility.cc | 167 +++++++++++++++++++++- src/core/receiver/satellite_visibility.h | 19 ++- 3 files changed, 180 insertions(+), 11 deletions(-) diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index b8830119a..08c3bf70b 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1835,7 +1835,8 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who) // No dual-frequency assist: fall back to a geometric Doppler // prediction from ephemeris/almanac (SatelliteVisibility), if // this is the primary signal of a satellite already classified - // visible and a live PVT fix currently exists. No-op (stays + // visible (for GLONASS, any slot on its FDMA frequency) and a + // live PVT fix currently exists. No-op (stays // false) unless visibility-aware search is enabled -- // PredictedDopplerHz() itself requires a live fix for now; the // AGNSS-reference (no-fix) case is a follow-up. @@ -1845,7 +1846,7 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who) { const auto pvt_ptr = get_pvt(); const Gnss_Satellite& sat = channels_[current_channel]->get_signal().get_satellite(); - if (pvt_ptr && satellite_visibility_->IsVisible(sat)) + if (pvt_ptr && satellite_visibility_->IsSearchVisible(sat)) { double receiver_time_s = 0.0; const Monitor_Pvt fix_status = channels_status_->get_current_status_pvt(&receiver_time_s); diff --git a/src/core/receiver/satellite_visibility.cc b/src/core/receiver/satellite_visibility.cc index 3a8bac8c5..8e9c5a4d9 100644 --- a/src/core/receiver/satellite_visibility.cc +++ b/src/core/receiver/satellite_visibility.cc @@ -17,6 +17,7 @@ #include "satellite_visibility.h" #include "Beidou_CNAV2.h" #include "GLONASS_L1_L2_CA.h" // for GLONASS_PRN +#include "MATH_CONSTANTS.h" // for D2R, SPEED_OF_LIGHT_M_S #include "agnss_ref_location.h" // for parse_agnss_ref_location #include "agnss_ref_time.h" // for parse_agnss_ref_utc_time #include "configuration_interface.h" @@ -26,7 +27,7 @@ #include "pvt_interface.h" #include "qzss.h" #include "rtklib_conversions.h" // for alm_to_rtklib, eph_to_rtklib -#include "rtklib_ephemeris.h" // for alm2pos, eph2pos +#include "rtklib_ephemeris.h" // for alm2pos, eph2pos, glorbit #include "rtklib_rtkcmn.h" // for utc2gpst, gpst2time #include #include @@ -52,6 +53,44 @@ constexpr int kDataCheckEveryNTicks = 20; // Permit the normal 500 ms PVT cadence, but do not retain a one-bin // velocity/clock estimate through a solution outage. constexpr double kMaxDopplerFixAgeS = 1.0; + +// Integration step of the GLONASS ephemeris orbit model, as in RTKLIB's +// geph2pos(). +constexpr double kGlonassIntegrationStepS = 60.0; + +// Half-width of the central difference used to derive a GLONASS almanac +// satellite velocity, which the ICD position-only algorithm does not provide. +constexpr double kGlonassAlmanacVelocityHalfStepS = 0.5; + + +// Same state propagation as RTKLIB's geph2pos(), keeping the velocity that +// geph2pos() discards. Both are PZ-90 Earth-fixed. +void glonass_ephemeris_pos_vel(const gtime_t& time, const geph_t& geph, + std::array& position_m, std::array& velocity_mps) +{ + double t = timediff(time, geph.toe); + std::array x{}; + for (int i = 0; i < 3; i++) + { + x[i] = geph.pos[i]; + x[i + 3] = geph.vel[i]; + } + double tt = t < 0.0 ? -kGlonassIntegrationStepS : kGlonassIntegrationStepS; + while (std::fabs(t) > 1e-9) + { + if (std::fabs(t) < kGlonassIntegrationStepS) + { + tt = t; + } + glorbit(tt, x.data(), geph.acc); + t -= tt; + } + for (int i = 0; i < 3; i++) + { + position_m[i] = x[i]; + velocity_mps[i] = x[i + 3]; + } +} } // namespace @@ -1085,9 +1124,10 @@ bool SatelliteVisibility::PredictedDopplerHz(const std::shared_ptr // Gnss_Ephemeris::predicted_doppler()/Gnss_Almanac::predicted_doppler()'s // own band codes, keyed by signal the same way SIGNAL_FREQ_MAP is. + // GLONASS uses them only to tell L1 from L2. static const std::map kBandForSignal = { - {"1C", 1}, {"1B", 1}, {"1D", 1}, {"J1", 1}, {"B1", 1}, - {"2S", 2}, {"B2", 2}, + {"1C", 1}, {"1B", 1}, {"1D", 1}, {"J1", 1}, {"B1", 1}, {"1G", 1}, + {"2S", 2}, {"B2", 2}, {"2G", 2}, {"L5", 5}, {"5X", 5}, {"J5", 5}, {"E6", 6}, {"7X", 7}, @@ -1099,7 +1139,7 @@ bool SatelliteVisibility::PredictedDopplerHz(const std::shared_ptr return false; } const int band = band_it->second; - const double carrier_freq_hz = freq_it->second; + double carrier_freq_hz = freq_it->second; const double lat_deg = fix_status.latitude; const double lon_deg = fix_status.longitude; @@ -1177,6 +1217,25 @@ bool SatelliteVisibility::PredictedDopplerHz(const std::shared_ptr geometric_available = true; } } + if (geometric_available && signal == "1D") + { + // The native BeiDou band-1 prediction is for B1I. B1C + // shares the orbit, but its carrier scales the range rate + // differently. Apply this before the receiver clock term, + // which already uses the requested signal's frequency. + geometric_doppler_hz *= carrier_freq_hz / FREQ1_BDS; + } + } + else if (system == "Glonass") + { + const std::array rx_llh{{lat_deg * D2R, lon_deg * D2R, h_m}}; + const std::array rx_enu_vel{{ve_mps, vn_mps, vu_mps}}; + std::array rx_pos{}; + std::array rx_vel{}; + pos2ecef(rx_llh.data(), rx_pos.data()); + enu2ecef(rx_llh.data(), rx_enu_vel.data(), rx_vel.data()); + geometric_available = GlonassGeometricDopplerHz(pvt_ptr, gps_gtime, static_cast(prn), band, + rx_pos, rx_vel, geometric_doppler_hz, carrier_freq_hz); } if (!geometric_available) @@ -1194,3 +1253,103 @@ bool SatelliteVisibility::PredictedDopplerHz(const std::shared_ptr doppler_hz = geometric_doppler_hz - clock_offset_hz; return true; } + + +bool SatelliteVisibility::GlonassGeometricDopplerHz(const std::shared_ptr& pvt_ptr, + const gtime_t& gps_gtime, uint32_t prn, int band, const std::array& rx_pos_m, + const std::array& rx_vel_mps, double& geometric_doppler_hz, double& carrier_freq_hz) const +{ + const auto channel_it = GLONASS_PRN.find(prn); + if (prn == 0 || channel_it == GLONASS_PRN.cend() || (band != 1 && band != 2)) + { + return false; + } + + // Acquisition searches an FDMA frequency, which antipodal slots share + // (see GetSearchVisibility()), and the channel's PRN is only one of + // them. Predict for the slot that is actually visible. If none or + // several are, the Doppler cannot be pinned to a single bin. + uint32_t visible_slot = 0; + int visible_slots = 0; + for (const auto& slot : GLONASS_PRN) + { + if (slot.first != 0 && slot.second == channel_it->second && + visible_.count(std::make_pair(std::string("Glonass"), slot.first)) != 0) + { + visible_slot = slot.first; + ++visible_slots; + } + } + if (visible_slots != 1) + { + return false; + } + + std::array sat_pos{}; + std::array sat_vel{}; + bool state_available = false; + const auto eph_map = pvt_ptr->get_glonass_ephemeris(); + const auto eph_it = eph_map.find(static_cast(visible_slot)); + if (eph_it != eph_map.cend() && eph_it->second.d_N_T >= 1.0 && eph_it->second.d_N_T <= 1461.0 && eph_it->second.d_yr >= 1996.0) + { + const auto geph = eph_to_rtklib(eph_it->second, pvt_ptr->get_glonass_utc_model(), glonass_strict_health_); + if (std::abs(timediff(gps_gtime, geph.toe)) <= MAXDTOE_GLO) + { + glonass_ephemeris_pos_vel(gps_gtime, geph, sat_pos, sat_vel); + state_available = true; + } + } + if (!state_available) + { + const auto alm_map = pvt_ptr->get_glonass_almanac(); + const auto alm_it = alm_map.find(static_cast(visible_slot)); + if (alm_it != alm_map.cend()) + { + const auto epoch = glonass_almanac_epoch(alm_it->second); + if (epoch.time != 0 && std::abs(timediff(gps_gtime, epoch)) <= almanac_max_age_s_) + { + // The almanac model runs on UTC(SU) elapsed time. + const double elapsed_s = timediff(gpst2utc(gps_gtime), gpst2utc(epoch)); + std::array before{}; + std::array after{}; + if (alm_it->second.satellite_position(elapsed_s, sat_pos) && + alm_it->second.satellite_position(elapsed_s - kGlonassAlmanacVelocityHalfStepS, before) && + alm_it->second.satellite_position(elapsed_s + kGlonassAlmanacVelocityHalfStepS, after)) + { + for (int i = 0; i < 3; i++) + { + sat_vel[i] = (after[i] - before[i]) / (2.0 * kGlonassAlmanacVelocityHalfStepS); + } + state_available = true; + } + } + } + } + if (!state_available) + { + return false; + } + + double range_m = 0.0; + double range_rate_mps = 0.0; + for (int i = 0; i < 3; i++) + { + const double los = sat_pos[i] - rx_pos_m[i]; + range_m += los * los; + range_rate_mps += (sat_vel[i] - rx_vel_mps[i]) * los; + } + range_m = std::sqrt(range_m); + if (!std::isfinite(range_m) || range_m <= 0.0 || !std::isfinite(range_rate_mps)) + { + return false; + } + range_rate_mps /= range_m; + + // Each frequency channel k transmits on its own carrier. The acquisition + // block adds the same k offset (see pcps_acquisition::is_fdma()), so the + // Doppler it searches is relative to this carrier. + const int32_t k = channel_it->second; + carrier_freq_hz = (band == 1) ? (FREQ1_GLO + k * DFRQ1_GLO) : (FREQ2_GLO + k * DFRQ2_GLO); + geometric_doppler_hz = -range_rate_mps / SPEED_OF_LIGHT_M_S * carrier_freq_hz; + return true; +} diff --git a/src/core/receiver/satellite_visibility.h b/src/core/receiver/satellite_visibility.h index 480449dbe..06edbe98a 100644 --- a/src/core/receiver/satellite_visibility.h +++ b/src/core/receiver/satellite_visibility.h @@ -170,11 +170,10 @@ public: // returns true. Only meaningful with a live PVT fix -- returns false // (leaving doppler_hz untouched) when disabled, no fix is currently // valid, the signal isn't a recognized carrier, or neither ephemeris nor - // almanac is currently usable for sat. No AGNSS-reference fallback yet: that's deferred to a - // follow-up (needs the pre-fix clock/velocity uncertainty this signal's - // Doppler tolerance can't absorb from a single search bin -- see - // GNSS-SDR.clock_frequency_max_error_ppm / receiver_max_velocity_m_s in - // the follow-up PR). + // almanac is currently usable for sat. For GLONASS, sat stands for its + // FDMA frequency: the prediction uses the single visible slot on that + // frequency and its own carrier, and fails if no slot or more than one + // is visible. // Tick() must have observed this fix, at most one second ago on the // sample clock. Older or unanchored fixes require a full-grid search. bool PredictedDopplerHz(const std::shared_ptr& pvt_ptr, @@ -191,6 +190,16 @@ private: SearchVisibility GetSearchVisibility(const Gnss_Satellite& sat) const; + // GLONASS part of PredictedDopplerHz(). Resolves which orbital slot + // sharing prn's FDMA frequency is visible and computes its geometric + // Doppler on band (1: L1, 2: L2) from the ephemeris, or else from the + // almanac, at the receiver's ECEF position and velocity. Also returns + // that slot's carrier frequency. False unless exactly one slot on the + // frequency is visible and it has usable navigation data. + bool GlonassGeometricDopplerHz(const std::shared_ptr& pvt_ptr, + const gtime_t& gps_gtime, uint32_t prn, int band, const std::array& rx_pos_m, + const std::array& rx_vel_mps, double& geometric_doppler_hz, double& carrier_freq_hz) const; + // changed_prns_out, when non-null, receives every (system, PRN) added, // updated, or removed since the last check, so Tick() can recompute only // those. Left untouched on the first call, which forces a full recompute