From bcb5b7228b393fa0cde2caaba4f09031bdbcceb7 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 12 Sep 2026 14:15:00 +0200 Subject: [PATCH] Extend visibility-aware search and fix dual-band acquisition scheduling Visibility-aware acquisition search (GNSS-SDR.enable_visibility_aware_search): - Classify GLONASS and QZSS satellites too. GLONASS positions come from the GNAV almanac (new Glonass_Gnav_Almanac::satellite_position with the standard perturbation model), so the search order benefits before any GLONASS ephemeris is decoded. - PVT publishes a lock-free navigation snapshot (ephemeris, almanac, UTC models per system) that the classifier reads instead of locking the PVT maps on every recompute. - Recompute on new navigation data, on position changes larger than visibility_recompute_position_threshold_m, every visibility_recompute_interval_s, and when the almanac ages out; fix the periodic and position-triggered recomputes that never fired, and the off-by-one that excluded satellites in [mask, mask+1) degrees. - Enter the start-up standby only when the feature is enabled, so the default search order is untouched. Secondary-frequency acquisition assistance: - Do not start an acquisition on the PRN 0 placeholder left on assisted channels at connect time; it threw inside the control thread and left the flowgraph running headless with the secondary band never acquired. - Match GLONASS assistance by FDMA frequency channel: the search pool keeps one slot per channel while tracked L1 channels report the decoded slot, so L2 was never picked. - Assist from locked channels before their telemetry is decoded: the observables status now reports tracking-only channels and the flowgraph keeps a separate tracking map for the assisted search. --- docs/CHANGELOG.md | 27 + src/algorithms/PVT/adapters/rtklib_pvt.cc | 21 + src/algorithms/PVT/adapters/rtklib_pvt.h | 4 + .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 269 +++++++- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 51 +- src/algorithms/PVT/libs/monitor_pvt.h | 20 +- src/algorithms/PVT/libs/rtklib_solver.cc | 99 ++- src/algorithms/PVT/libs/rtklib_solver.h | 5 +- src/algorithms/libs/geofunctions.cc | 4 +- .../libs/rtklib/rtklib_conversions.cc | 47 +- .../libs/rtklib/rtklib_conversions.h | 29 +- .../libs/rtklib/rtklib_ephemeris.cc | 16 +- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 16 +- .../gnuradio_blocks/hybrid_observables_gs.cc | 26 +- src/core/interfaces/pvt_interface.h | 8 + src/core/libs/channel_status_msg_receiver.cc | 26 + src/core/libs/channel_status_msg_receiver.h | 15 + src/core/receiver/control_thread.cc | 86 +-- src/core/receiver/gnss_flowgraph.cc | 347 +++++----- src/core/receiver/gnss_flowgraph.h | 72 +-- src/core/receiver/satellite_visibility.cc | 602 ++++++++---------- src/core/receiver/satellite_visibility.h | 318 ++++----- src/core/system_parameters/CMakeLists.txt | 1 + .../system_parameters/agnss_ref_location.h | 8 +- src/core/system_parameters/agnss_ref_time.h | 15 +- .../system_parameters/glonass_gnav_almanac.cc | 132 ++++ .../system_parameters/glonass_gnav_almanac.h | 18 + .../glonass_gnav_navigation_message.cc | 12 + src/core/system_parameters/gnss_almanac.h | 7 +- tests/CMakeLists.txt | 2 + tests/test_main.cc | 1 + .../satellite_visibility_test.cc | 547 ++++++++++++++++ 32 files changed, 1859 insertions(+), 992 deletions(-) create mode 100644 src/core/system_parameters/glonass_gnav_almanac.cc create mode 100644 tests/unit-tests/control-plane/satellite_visibility_test.cc diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index e26bd11a0..c6fd2c781 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -132,6 +132,25 @@ All notable changes to GNSS-SDR will be documented in this file. the generic rule `doppler(col) = -doppler_max + doppler_center + doppler_step * col` decodes both full and narrowed grids. Contributed by @joebre. +- Added an optional visibility-aware acquisition search, enabled with + `GNSS-SDR.enable_visibility_aware_search=true` (default `false`, which leaves + the existing search order untouched). Once a receiver position is available, + either from a fix or from `GNSS-SDR.AGNSS_ref_location`, satellites are + continuously classified as visible, excluded (elevation at or below + `GNSS-SDR.search_elevation_mask`, default 0 degrees, or flagged unhealthy), or + not yet known, using the freshest ephemeris or almanac decoded for GPS, + Galileo, BeiDou, GLONASS, and QZSS. Idle channels then favor visible + satellites over unknown ones, at the ratio given by + `GNSS-SDR.visible_vs_mayvisible_search_ratio` (default 3), and skip known + excluded ones, so less CPU is spent acquiring satellites that are below the + horizon. The classification is refreshed whenever new navigation data arrives, + when the receiver moves more than + `GNSS-SDR.visibility_recompute_position_threshold_m` (default 1000 m), every + `GNSS-SDR.visibility_recompute_interval_s` (default 120 s), and when almanac + data becomes older than `GNSS-SDR.visibility_almanac_max_age_s` (default 3 + days). A satellite that is already being tracked is never released because of + this classification, and `PVT.elevation_mask` still decides which observations + enter the navigation solution. Contributed by @joebre. ### Improvements in Interoperability: @@ -333,6 +352,14 @@ All notable changes to GNSS-SDR will be documented in this file. metadata in satellite status, and the NMEA printer emits the strongest available C/N0 with the corresponding NMEA signal identifier. Contributed by @vladisslav2011. +- The custom output stream defined by `monitor_pvt.proto` now includes a + `tracked_satellites` list. Each entry reports one tracked signal (`system`, + `prn`, `signal`), its `azimuth_deg` and `elevation_deg`, whether it was + `combined` with another signal of the same satellite (e.g., the Galileo E1+E5a + ionosphere-free combination), and a `used` flag telling whether it contributed + to the reported fix. Satellites that were tracked but left out of the solution + (below `PVT.elevation_mask`, or excluded by RAIM) are listed with + `used = false`. Contributed by @joebre. ### Improvements in Maintainability: diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index e1d67cd01..df7cc8ec4 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -955,3 +955,24 @@ gr::basic_block_sptr Rtklib_Pvt::get_right_block() { return nullptr; // this is a sink, nothing downstream } + + +std::map Rtklib_Pvt::get_gps_cnav_ephemeris() const +{ + return pvt_->get_gps_cnav_ephemeris_map(); +} + +std::map Rtklib_Pvt::get_glonass_ephemeris() const +{ + return pvt_->get_glonass_ephemeris_map(); +} + +std::map Rtklib_Pvt::get_glonass_almanac() const +{ + return pvt_->get_glonass_almanac_map(); +} + +Glonass_Gnav_Utc_Model Rtklib_Pvt::get_glonass_utc_model() const +{ + return pvt_->get_glonass_utc_model(); +} diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h index 5b2030dd3..fda5783e5 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.h +++ b/src/algorithms/PVT/adapters/rtklib_pvt.h @@ -223,6 +223,10 @@ public: } void clear_ephemeris() override; + std::map get_gps_cnav_ephemeris() const override; + std::map get_glonass_ephemeris() const override; + std::map get_glonass_almanac() const override; + Glonass_Gnav_Utc_Model get_glonass_utc_model() const override; std::map get_gps_ephemeris() const override; std::map get_galileo_ephemeris() const override; std::map get_beidou_dnav_ephemeris() const override; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 170c4162f..387e1ef4d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -184,6 +184,34 @@ rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels, } +struct rtklib_pvt_gs::NavigationSnapshot +{ + // Type aliases keep the closing template brackets out of the member + // initializers: GCC 4.8 parses in-class initializers late and reads a + // ">>" there as a right shift. + using GpsCnavEphemerisMap = std::map; + using GlonassEphemerisMap = std::map; + using GlonassAlmanacMap = std::map; + using GpsEphemerisMap = std::map; + using GpsAlmanacMap = std::map; + using GalileoEphemerisMap = std::map; + using GalileoAlmanacMap = std::map; + using BeidouEphemerisMap = std::map; + using BeidouAlmanacMap = std::map; + + std::shared_ptr gps_cnav_ephemeris = std::make_shared(); + std::shared_ptr glonass_ephemeris = std::make_shared(); + std::shared_ptr glonass_almanac = std::make_shared(); + Glonass_Gnav_Utc_Model glonass_utc_model; + std::shared_ptr gps_ephemeris = std::make_shared(); + std::shared_ptr gps_almanac = std::make_shared(); + std::shared_ptr galileo_ephemeris = std::make_shared(); + std::shared_ptr galileo_almanac = std::make_shared(); + std::shared_ptr beidou_ephemeris = std::make_shared(); + std::shared_ptr beidou_almanac = std::make_shared(); +}; + + rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, const Pvt_Conf& conf_, const rtk_t& rtk, @@ -191,6 +219,8 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, : gr::sync_block("rtklib_pvt_gs", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), gr::io_signature::make(0, 0, 0)), + d_empty_navigation_snapshot(std::make_shared()), + d_navigation_snapshot(d_empty_navigation_snapshot), d_queue_name("gnss_sdr_ttff_message_queue"), d_dump_filename(conf_.dump_filename), d_geohash(std::make_unique()), @@ -721,6 +751,7 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, rtklib_pvt_gs::~rtklib_pvt_gs() { + apply_pending_navigation_clear(); DLOG(INFO) << "PVT block destructor called."; if (d_ntrip_client) { @@ -1441,6 +1472,7 @@ rtklib_pvt_gs::~rtklib_pvt_gs() void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) { + apply_pending_navigation_clear(); try { const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); @@ -1838,9 +1870,11 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) // ### GLONASS GNAV Almanac ### const auto glonass_gnav_almanac = wht::any_cast>(pmt::any_ref(msg)); d_internal_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac; + d_internal_pvt_solver->glonass_gnav_almanac_map[glonass_gnav_almanac->PRN] = *glonass_gnav_almanac; if (d_enable_rx_clock_correction == true) { d_user_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac; + d_user_pvt_solver->glonass_gnav_almanac_map[glonass_gnav_almanac->PRN] = *glonass_gnav_almanac; } DLOG(INFO) << "New GLONASS GNAV Almanac has arrived" << ", GLONASS GNAV Slot Number =" << glonass_gnav_almanac->d_n_A; @@ -1983,6 +2017,47 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) { LOG(WARNING) << "msg_handler_telemetry unknown object type!"; } + + if (msg_type_hash_code == d_gps_ephemeris_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::GpsEphemeris); + } + else if (msg_type_hash_code == d_gps_cnav_ephemeris_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::GpsCnavEphemeris); + } + else if (msg_type_hash_code == d_glonass_gnav_ephemeris_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::GlonassEphemeris); + } + else if (msg_type_hash_code == d_glonass_gnav_almanac_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::GlonassAlmanac); + } + else if (msg_type_hash_code == d_glonass_gnav_utc_model_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::GlonassUtcModel); + } + else if (msg_type_hash_code == d_gps_almanac_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::GpsAlmanac); + } + else if (msg_type_hash_code == d_galileo_ephemeris_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::GalileoEphemeris); + } + else if (msg_type_hash_code == d_galileo_almanac_sptr_type_hash_code || msg_type_hash_code == d_galileo_almanac_helper_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::GalileoAlmanac); + } + else if (msg_type_hash_code == d_beidou_dnav_ephemeris_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::BeidouEphemeris); + } + else if (msg_type_hash_code == d_beidou_dnav_almanac_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::BeidouAlmanac); + } } catch (const wht::bad_any_cast& e) { @@ -2067,46 +2142,131 @@ void rtklib_pvt_gs::msg_handler_osnma(const pmt::pmt_t& msg) } +std::shared_ptr rtklib_pvt_gs::navigation_snapshot() const +{ + std::lock_guard lock(d_snapshot_mutex); + return d_navigation_snapshot; +} + + +void rtklib_pvt_gs::publish_navigation_snapshot(NavigationData data) +{ + const auto previous = navigation_snapshot(); + auto updated = std::make_shared(*previous); + switch (data) + { + case NavigationData::GpsCnavEphemeris: + updated->gps_cnav_ephemeris = std::make_shared(d_internal_pvt_solver->gps_cnav_ephemeris_map); + break; + case NavigationData::GlonassEphemeris: + updated->glonass_ephemeris = std::make_shared(d_internal_pvt_solver->glonass_gnav_ephemeris_map); + break; + case NavigationData::GlonassAlmanac: + updated->glonass_almanac = std::make_shared(d_internal_pvt_solver->glonass_gnav_almanac_map); + break; + case NavigationData::GlonassUtcModel: + updated->glonass_utc_model = d_internal_pvt_solver->glonass_gnav_utc_model; + break; + case NavigationData::GpsEphemeris: + updated->gps_ephemeris = std::make_shared(d_internal_pvt_solver->gps_ephemeris_map); + break; + case NavigationData::GpsAlmanac: + updated->gps_almanac = std::make_shared(d_internal_pvt_solver->gps_almanac_map); + break; + case NavigationData::GalileoEphemeris: + updated->galileo_ephemeris = std::make_shared(d_internal_pvt_solver->get_galileo_ephemeris_map_for_pvt()); + break; + case NavigationData::GalileoAlmanac: + updated->galileo_almanac = std::make_shared(d_internal_pvt_solver->galileo_almanac_map); + break; + case NavigationData::BeidouEphemeris: + updated->beidou_ephemeris = std::make_shared(d_internal_pvt_solver->beidou_dnav_ephemeris_map); + break; + case NavigationData::BeidouAlmanac: + updated->beidou_almanac = std::make_shared(d_internal_pvt_solver->beidou_dnav_almanac_map); + break; + } + std::shared_ptr published = std::move(updated); + { + std::lock_guard lock(d_snapshot_mutex); + // A concurrent clear must not be undone by a snapshot built before + // the worker has applied that clear to the solver maps. + if (d_navigation_generation.load(std::memory_order_relaxed) == d_applied_navigation_generation) + { + d_navigation_snapshot.swap(published); + } + } +} + + std::map rtklib_pvt_gs::get_gps_ephemeris_map() const { - return d_internal_pvt_solver->gps_ephemeris_map; + const auto snapshot = navigation_snapshot(); + return *snapshot->gps_ephemeris; } std::map rtklib_pvt_gs::get_gps_almanac_map() const { - return d_internal_pvt_solver->gps_almanac_map; + const auto snapshot = navigation_snapshot(); + return *snapshot->gps_almanac; } std::map rtklib_pvt_gs::get_galileo_ephemeris_map() const { - return d_internal_pvt_solver->get_galileo_ephemeris_map_for_pvt(); + const auto snapshot = navigation_snapshot(); + return *snapshot->galileo_ephemeris; } std::map rtklib_pvt_gs::get_galileo_almanac_map() const { - return d_internal_pvt_solver->galileo_almanac_map; + const auto snapshot = navigation_snapshot(); + return *snapshot->galileo_almanac; } std::map rtklib_pvt_gs::get_beidou_dnav_ephemeris_map() const { - return d_internal_pvt_solver->beidou_dnav_ephemeris_map; + const auto snapshot = navigation_snapshot(); + return *snapshot->beidou_ephemeris; } std::map rtklib_pvt_gs::get_beidou_dnav_almanac_map() const { - return d_internal_pvt_solver->beidou_dnav_almanac_map; + const auto snapshot = navigation_snapshot(); + return *snapshot->beidou_almanac; } void rtklib_pvt_gs::clear_ephemeris() { + auto previous = d_empty_navigation_snapshot; + { + std::lock_guard lock(d_snapshot_mutex); + d_navigation_generation.fetch_add(1, std::memory_order_release); + d_navigation_snapshot.swap(previous); + } + // The worker owns the solvers. Invalidate readers immediately, but defer + // clearing mutable maps until it is between work/telemetry callbacks. +} + + +void rtklib_pvt_gs::apply_pending_navigation_clear() +{ + const uint32_t generation = d_navigation_generation.load(std::memory_order_acquire); + if (generation == d_applied_navigation_generation) + { + return; + } d_internal_pvt_solver->clear_gps_ephemerides(); d_internal_pvt_solver->gps_almanac_map.clear(); + d_internal_pvt_solver->gps_cnav_ephemeris_map.clear(); + d_internal_pvt_solver->glonass_gnav_ephemeris_map.clear(); + d_internal_pvt_solver->glonass_gnav_almanac_map.clear(); + d_internal_pvt_solver->glonass_gnav_almanac = Glonass_Gnav_Almanac(); d_internal_pvt_solver->galileo_ephemeris_map.clear(); d_internal_pvt_solver->galileo_ephemeris_store.clear(); d_internal_pvt_solver->galileo_reduced_ced_map.clear(); @@ -2117,6 +2277,10 @@ void rtklib_pvt_gs::clear_ephemeris() { d_user_pvt_solver->clear_gps_ephemerides(); d_user_pvt_solver->gps_almanac_map.clear(); + d_user_pvt_solver->gps_cnav_ephemeris_map.clear(); + d_user_pvt_solver->glonass_gnav_ephemeris_map.clear(); + d_user_pvt_solver->glonass_gnav_almanac_map.clear(); + d_user_pvt_solver->glonass_gnav_almanac = Glonass_Gnav_Almanac(); d_user_pvt_solver->galileo_ephemeris_map.clear(); d_user_pvt_solver->galileo_ephemeris_store.clear(); d_user_pvt_solver->galileo_reduced_ced_map.clear(); @@ -2124,6 +2288,7 @@ void rtklib_pvt_gs::clear_ephemeris() d_user_pvt_solver->beidou_dnav_ephemeris_map.clear(); d_user_pvt_solver->beidou_dnav_almanac_map.clear(); } + d_applied_navigation_generation = generation; } @@ -2230,36 +2395,41 @@ bool rtklib_pvt_gs::get_latest_PVT(double* longitude_deg, double* course_over_ground_deg, time_t* UTC_time) const { - if (d_enable_rx_clock_correction == true) + LatestPvt snapshot; + { + std::lock_guard lock(d_snapshot_mutex); + snapshot = d_latest_pvt; + } + if (!snapshot.valid) { - if (d_user_pvt_solver->is_valid_position()) - { - *latitude_deg = d_user_pvt_solver->get_latitude(); - *longitude_deg = d_user_pvt_solver->get_longitude(); - *height_m = d_user_pvt_solver->get_height(); - *ground_speed_kmh = d_user_pvt_solver->get_speed_over_ground() * 3600.0 / 1000.0; - *course_over_ground_deg = d_user_pvt_solver->get_course_over_ground(); - *UTC_time = convert_to_time_t(d_user_pvt_solver->get_position_UTC_time()); - - return true; - } + return false; } - else + *longitude_deg = snapshot.longitude_deg; + *latitude_deg = snapshot.latitude_deg; + *height_m = snapshot.height_m; + *ground_speed_kmh = snapshot.ground_speed_kmh; + *course_over_ground_deg = snapshot.course_over_ground_deg; + *UTC_time = snapshot.utc_time; + return true; +} + + +void rtklib_pvt_gs::publish_latest_pvt() +{ + LatestPvt snapshot; + const auto& solver = d_enable_rx_clock_correction ? d_user_pvt_solver : d_internal_pvt_solver; + snapshot.valid = solver->is_valid_position(); + if (snapshot.valid) { - if (d_internal_pvt_solver->is_valid_position()) - { - *latitude_deg = d_internal_pvt_solver->get_latitude(); - *longitude_deg = d_internal_pvt_solver->get_longitude(); - *height_m = d_internal_pvt_solver->get_height(); - *ground_speed_kmh = d_internal_pvt_solver->get_speed_over_ground() * 3600.0 / 1000.0; - *course_over_ground_deg = d_internal_pvt_solver->get_course_over_ground(); - *UTC_time = convert_to_time_t(d_internal_pvt_solver->get_position_UTC_time()); - - return true; - } + snapshot.latitude_deg = solver->get_latitude(); + snapshot.longitude_deg = solver->get_longitude(); + snapshot.height_m = solver->get_height(); + snapshot.ground_speed_kmh = solver->get_speed_over_ground() * 3.6; + snapshot.course_over_ground_deg = solver->get_course_over_ground(); + snapshot.utc_time = convert_to_time_t(solver->get_position_UTC_time()); } - - return false; + std::lock_guard lock(d_snapshot_mutex); + d_latest_pvt = snapshot; } @@ -2574,6 +2744,8 @@ void rtklib_pvt_gs::report_solution_outage() int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items __attribute__((unused))) { + apply_pending_navigation_clear(); + bool latest_pvt_updated = false; // *************** time tags **************** if (d_enable_rx_clock_correction == false) // todo: currently only works if clock correction is disabled { @@ -2842,6 +3014,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item dump_this_epoch = (rx_time_ms % d_output_rate_ms == 0); } // #### solve PVT and store the corrected observable set + latest_pvt_updated = latest_pvt_updated || !d_enable_rx_clock_correction; if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, *d_sensor_data_aggregator, dump_this_epoch)) { d_pvt_errors_counter = 0; // Reset consecutive PVT error counter @@ -3015,6 +3188,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item d_ntrip_max_correction_age_s); fixed_base = d_fixed_base_snapshot.get(); } + latest_pvt_updated = true; flag_pvt_valid = d_user_pvt_solver->get_PVT( d_gnss_observables_map, d_output_rate_ms / 1000.0, @@ -3302,5 +3476,36 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } } + if (latest_pvt_updated) + { + publish_latest_pvt(); + } return noutput_items; } + + +std::map rtklib_pvt_gs::get_gps_cnav_ephemeris_map() const +{ + const auto snapshot = navigation_snapshot(); + return *snapshot->gps_cnav_ephemeris; +} + + +std::map rtklib_pvt_gs::get_glonass_ephemeris_map() const +{ + const auto snapshot = navigation_snapshot(); + return *snapshot->glonass_ephemeris; +} + + +std::map rtklib_pvt_gs::get_glonass_almanac_map() const +{ + const auto snapshot = navigation_snapshot(); + return *snapshot->glonass_almanac; +} + + +Glonass_Gnav_Utc_Model rtklib_pvt_gs::get_glonass_utc_model() const +{ + return navigation_snapshot()->glonass_utc_model; +} diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index cac1032b4..855f45859 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -31,6 +31,7 @@ #include // for sync_block #include // for gr_vector_const_void_star #include // for pmt_t +#include // for atomic #include // for system_clock #include // for size_t #include // for int32_t @@ -38,6 +39,7 @@ #include // for std::fstream #include // for map #include // for shared_ptr, unique_ptr +#include // for mutex #include // for std::queue #include // for string #include // for vector @@ -49,6 +51,10 @@ * \{ */ +class Gps_CNAV_Ephemeris; +class Glonass_Gnav_Ephemeris; +class Glonass_Gnav_Almanac; +class Glonass_Gnav_Utc_Model; class Beidou_Dnav_Almanac; class Beidou_Dnav_Ephemeris; class Galileo_Almanac; @@ -92,6 +98,10 @@ public: /*! * \brief Get latest set of GPS ephemeris from PVT block */ + std::map get_gps_cnav_ephemeris_map() const; + std::map get_glonass_ephemeris_map() const; + std::map get_glonass_almanac_map() const; + Glonass_Gnav_Utc_Model get_glonass_utc_model() const; std::map get_gps_ephemeris_map() const; /*! @@ -120,7 +130,8 @@ public: std::map get_beidou_dnav_almanac_map() const; /*! - * \brief Clear all ephemeris information and the almanacs for GPS and Galileo + * \brief Clears the published navigation snapshot now and the solver maps + * at the worker's next work() or telemetry callback. */ void clear_ephemeris(); @@ -211,6 +222,44 @@ private: std::unique_ptr d_sensor_data_aggregator; + enum class NavigationData + { + GpsCnavEphemeris, + GlonassEphemeris, + GlonassAlmanac, + GlonassUtcModel, + GpsEphemeris, + GpsAlmanac, + GalileoEphemeris, + GalileoAlmanac, + BeidouEphemeris, + BeidouAlmanac + }; + struct NavigationSnapshot; + struct LatestPvt + { + double longitude_deg = 0.0; + double latitude_deg = 0.0; + double height_m = 0.0; + double ground_speed_kmh = 0.0; + double course_over_ground_deg = 0.0; + time_t utc_time = 0; + bool valid = false; + }; + + std::shared_ptr navigation_snapshot() const; + void publish_navigation_snapshot(NavigationData data); + void apply_pending_navigation_clear(); + void publish_latest_pvt(); + + // Only pointer exchanges and the small PVT value copy hold this mutex. + // Map construction, copying and destruction happen outside the lock. + mutable std::mutex d_snapshot_mutex; + const std::shared_ptr d_empty_navigation_snapshot; + std::shared_ptr d_navigation_snapshot; + LatestPvt d_latest_pvt; + std::atomic d_navigation_generation{0}; + uint32_t d_applied_navigation_generation = 0; // GNU Radio worker only std::shared_ptr d_internal_pvt_solver; std::shared_ptr d_user_pvt_solver; diff --git a/src/algorithms/PVT/libs/monitor_pvt.h b/src/algorithms/PVT/libs/monitor_pvt.h index 3dc529e33..b376cc30c 100644 --- a/src/algorithms/PVT/libs/monitor_pvt.h +++ b/src/algorithms/PVT/libs/monitor_pvt.h @@ -36,14 +36,11 @@ class Monitor_Pvt { public: /*! - * \brief One tracked satellite/signal, with its azimuth/elevation, - * whether it was combined with another signal of the same satellite - * (e.g. Galileo E1+E5a iono-free combination -- see the - * "dual-frequency" branch of prange() in rtklib_pntpos.cc), and whether - * it was actually used in this fix (see the `used` member below). - * Signals are listed individually (one entry per satellite per signal), - * not merged, so a combined satellite appears as two entries both - * flagged combined = true. + * \brief One tracked satellite/signal with its azimuth/elevation, whether + * it was combined with another signal of the same satellite (e.g. Galileo + * E1+E5a iono-free combination) and whether it was used in this fix. + * Signals are listed individually (one entry per satellite per signal): + * a combined satellite appears as two entries, both with combined = true. */ class TrackedSatelliteInfo { @@ -54,11 +51,8 @@ public: double azimuth_deg{}; double elevation_deg{}; bool combined{}; - // false when this satellite/signal was tracked and had azimuth/elevation - // computed but was excluded from the fix itself (e.g. below - // PVT.elevation_mask, or by RAIM FDE) -- azimuth_deg/elevation_deg are - // still valid in that case, only the position solve ignored this - // observation. + // false when tracked (az/el valid) but excluded from the solve, e.g. + // below PVT.elevation_mask or by RAIM FDE. bool used{true}; template diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index d09bd6517..2db7442c0 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1529,6 +1529,74 @@ void Rtklib_Solver::reset_relative_filter() } +int Rtklib_Solver::merge_duplicated_rover_observations(int rover_observation_count) +{ + /* Two channels can deliver the same satellite on the same signal (e.g. a + GLONASS false lock labelled with a slot another channel already tracks). + rescode() only rejects duplicates that are adjacent, and an inconsistent + duplicate makes every solution fail validation. Keep, per frequency + slot, the entry with the highest C/N0; preserve the order of the rest. */ + int out = 0; + bool merged_any = false; + for (int in = 0; in < rover_observation_count; ++in) + { + const obsd_t &source = d_obs_data[in]; + int match = -1; + for (int k = 0; k < out && source.sat > 0; ++k) + { + if (d_obs_data[k].sat == source.sat) + { + match = k; + break; + } + } + if (match < 0) + { + if (out != in) + { + d_obs_data[out] = source; + } + ++out; + continue; + } + obsd_t &destination = d_obs_data[match]; + for (int frequency = 0; frequency < NFREQ + NEXOBS; ++frequency) + { + const bool source_has_data = source.code[frequency] != CODE_NONE && + (source.P[frequency] != 0.0 || source.L[frequency] != 0.0); + if (!source_has_data) + { + continue; + } + const bool destination_has_data = destination.code[frequency] != CODE_NONE && + (destination.P[frequency] != 0.0 || destination.L[frequency] != 0.0); + if (destination_has_data && destination.SNR[frequency] >= source.SNR[frequency]) + { + continue; + } + destination.P[frequency] = source.P[frequency]; + destination.L[frequency] = source.L[frequency]; + destination.D[frequency] = source.D[frequency]; + destination.SNR[frequency] = source.SNR[frequency]; + destination.LLI[frequency] = source.LLI[frequency]; + destination.code[frequency] = source.code[frequency]; + } + merged_any = true; + if (!d_duplicated_rover_observations_logged) + { + LOG(WARNING) << "Duplicated observation of satellite " << satno2id(source.sat) + << " received from two channels: keeping the measurement with the highest C/N0"; + d_duplicated_rover_observations_logged = true; + } + } + if (!merged_any) + { + d_duplicated_rover_observations_logged = false; + } + return out; +} + + bool Rtklib_Solver::prepare_fixed_base_observations(const Ntrip_Rtcm_Snapshot &fixed_base, int &rover_observation_count, int &base_observation_count) @@ -2407,7 +2475,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // ****** SOLVE PVT****************************************************** // ********************************************************************** - int rover_observation_count = valid_obs + glo_valid_obs; + int rover_observation_count = merge_duplicated_rover_observations(valid_obs + glo_valid_obs); int base_observation_count = 0; bool fixed_base_applied = false; if (fixed_base != nullptr) @@ -2861,28 +2929,15 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // prange() in rtklib_pntpos.cc) gets one entry per signal, all // flagged combined = true. // - // rescode() (rtklib_pntpos.cc) computes azel via satazel() - // *before* checking it against PVT.elevation_mask, and - // pntpos() copies that azel into ssat[] unconditionally -- - // only ssat[].vs is gated by the mask. So a satellite that - // is tracked and has a live observation this epoch, but - // falls below PVT.elevation_mask (or was excluded by RAIM - // FDE), still has a valid azel here; only vs is false. - // Report it anyway with used = false instead of dropping - // it, so an excluded satellite shows up in the monitor as - // "not used" rather than as missing/no-az-el (which - // otherwise looks identical to a tracking problem). + // pntpos() fills ssat[].azel for every observation; only + // ssat[].vs is gated by PVT.elevation_mask / RAIM FDE. A + // tracked satellite excluded from the solve is therefore + // reported with used = false rather than dropped, so the + // monitor can tell "excluded" from "not tracked". // - // Deliberately NOT filtered for NaN here: a corrupt ephemeris/ - // almanac (e.g. an out-of-range eccentricity) can propagate NaN - // through eph2pos()/alm2pos()/satazel(), and that NaN is reported - // as-is rather than silently dropping the satellite from the - // monitor -- consumers (gnss-sdr-CtrlApp) are expected to handle - // a NaN azimuth_deg/elevation_deg explicitly (e.g. no sky plot - // entry, "NaN" printed in the signal table) rather than have it - // hidden here. What NaN must NOT do is influence the position fix - // itself -- see rescode()'s prange() isfinite guard in - // rtklib_pntpos.cc for where that's actually enforced. + // NaN az/el (corrupt ephemeris/almanac) is deliberately + // reported as-is for consumers to handle; rescode() + // already keeps NaN out of the position solve. d_monitor_pvt.tracked_satellites.clear(); for (int sat_idx = 0; sat_idx < MAXSAT; sat_idx++) { diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index bca354d50..310798a88 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -198,7 +198,8 @@ public: Qzss_CNAV_Utc_Model qzss_cnav_utc_model; Glonass_Gnav_Utc_Model glonass_gnav_utc_model; //!< Map storing GLONASS GNAV UTC Model - Glonass_Gnav_Almanac glonass_gnav_almanac; //!< Map storing GLONASS GNAV Almanac Model + std::map glonass_gnav_almanac_map; + Glonass_Gnav_Almanac glonass_gnav_almanac; //!< Map storing GLONASS GNAV Almanac Model Beidou_Dnav_Utc_Model beidou_dnav_utc_model; Beidou_Dnav_Iono beidou_dnav_iono; @@ -214,6 +215,7 @@ private: bool prepare_fixed_base_observations(const Ntrip_Rtcm_Snapshot& fixed_base, int& rover_observation_count, int& base_observation_count); + int merge_duplicated_rover_observations(int rover_observation_count); bool galileo_ephemeris_is_usable(const Galileo_Ephemeris& ephemeris, uint32_t observation_tow) const; void update_galileo_observation_wavelengths(const obsd_t& observation); @@ -287,6 +289,7 @@ private: Rtklib_Fixed_Base_Status d_fixed_base_status = Rtklib_Fixed_Base_Status::NOT_REQUESTED; bool d_fixed_base_initialized = false; bool d_fixed_base_was_applied = false; + bool d_duplicated_rover_observations_logged = false; bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; }; diff --git a/src/algorithms/libs/geofunctions.cc b/src/algorithms/libs/geofunctions.cc index 08df9095f..3b60ee90f 100644 --- a/src/algorithms/libs/geofunctions.cc +++ b/src/algorithms/libs/geofunctions.cc @@ -96,8 +96,10 @@ int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma:: if (hor_dis < 1.0E-20) { + // Zenith or nadir: the azimuth is undefined, the elevation is + // +90 deg only if the vector points up. *Az = 0.0; - *El = 90.0; + *El = (U >= 0.0) ? 90.0 : -90.0; } else { diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 7c5549920..011e1148f 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -22,6 +22,7 @@ #include "beidou_dnav_ephemeris.h" // for Beidou_Dnav_Ephemeris #include "galileo_almanac.h" // for Galileo_Almanac #include "galileo_ephemeris.h" // for Galileo_Ephemeris +#include "glonass_gnav_almanac.h" #include "glonass_gnav_ephemeris.h" // for Glonass_Gnav_Ephemeris #include "glonass_gnav_utc_model.h" // for Glonass_Gnav_Utc_Model #include "gnss_obs_codes.h" // for CODE_L1C, CODE_L2S, CODE_L5X @@ -56,13 +57,10 @@ const HAS_obs_corrections* find_has_obs_correction(const std::map(gps_alm.toa)); rtklib_alm.A = gps_alm.sqrtA * gps_alm.sqrtA; @@ -926,18 +922,9 @@ alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm, int ref_week) rtklib_alm.sat = gal_alm.PRN + NSATGPS + NSATGLO; rtklib_alm.svh = gal_alm.E1B_HS; rtklib_alm.svconf = gal_alm.E1B_HS; - // Galileo almanac WNa is only 2 bits (mod 4 -- see WN_A_7_BIT/WN_A_9_BIT - // in Galileo_INAV.h, {bit_position, bit_width} = {.., 2}; NOT 4 bits/ - // mod 16, that width belongs to IOD_a, a different field -- confirmed - // empirically 2026-09-03 by sweeping candidate weeks against real - // ephemeris-derived elevation for satellites with both data sources: - // only the mod-4 resolution produced an exact match, mod-16 candidates - // were off by up to 100+ deg) and GST-relative (same 1024-week GST/GPS - // offset galileo_eph_to_rtklib() applies for ephemeris via - // "gal_eph.WN + 1024") -- resolve mod 4 against the GST-equivalent - // reference week, then convert back to the GPS week scale gpst2time() - // expects. See resolve_truncated_week()'s doc comment for why skipping - // this matters. + // WNa is 2 bits (mod 4; WN_A_*_BIT in Galileo_INAV.h) and GST-relative: + // resolve against the GST reference week (GPS week - 1024), then convert + // back to the GPS week scale gpst2time() expects. const int ref_week_gst = (ref_week > 0) ? (ref_week - 1024) : ref_week; rtklib_alm.week = resolve_truncated_week(gal_alm.WNa, ref_week_gst, 4) + 1024; rtklib_alm.toa = gpst2time(rtklib_alm.week, static_cast(gal_alm.toa)); @@ -979,3 +966,17 @@ alm_t alm_to_rtklib(const Beidou_Dnav_Almanac& bei_alm) return rtklib_alm; } + + +gtime_t glonass_almanac_epoch(const Glonass_Gnav_Almanac& almanac) +{ + if (almanac.d_N_4 < 1 || almanac.d_N_4 > 31 || almanac.d_N_A < 1 || almanac.d_N_A > 1461 || + !std::isfinite(almanac.d_t_lambda_n_A) || almanac.d_t_lambda_n_A < 0.0 || almanac.d_t_lambda_n_A >= 86400.0) + { + return gtime_t{}; + } + const double epoch[6] = {1996.0 + 4.0 * (almanac.d_N_4 - 1), 1.0, 1.0, 0.0, 0.0, 0.0}; + // N_4 counts four-year intervals from 1996, N_A the day within it; + // t_lambda_n_A is seconds of day in MSK (UTC+3), hence the -10800 s. + return utc2gpst(timeadd(epoch2time(epoch), (almanac.d_N_A - 1) * 86400.0 + almanac.d_t_lambda_n_A - 10800.0)); +} diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h index 86fb41e67..449b6752e 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -33,6 +33,7 @@ class Beidou_Dnav_Almanac; class Beidou_Dnav_Ephemeris; class Galileo_Almanac; class Galileo_Ephemeris; +class Glonass_Gnav_Almanac; class Glonass_Gnav_Ephemeris; class Glonass_Gnav_Utc_Model; class Gnss_Synchro; @@ -106,25 +107,21 @@ eph_t eph_to_rtklib(const Beidou_Dnav_Ephemeris& bei_eph); eph_t eph_to_rtklib(const Beidou_Cnav1_Ephemeris& bei_eph); +/*! + * \brief Absolute GPST epoch of a GLONASS almanac (N_4/N_A calendar day plus + * t_lambda_n_A, which is given in MSK = UTC+3). + * \return Zero gtime_t when the broadcast date is unavailable or invalid. + */ +gtime_t glonass_almanac_epoch(const Glonass_Gnav_Almanac& almanac); + /*! * \brief Converts almanac data to RTKLIB's alm_t format. * - * \param ref_week current GPS week, used to resolve the almanac's own - * truncated week number (WNa -- 8 bits/mod-256 for GPS, 2 bits/mod-4 for - * Galileo, both far too short to be used as an absolute week on their own) - * to the closest full week to ref_week, the same way eph_to_rtklib() - * already does for ephemeris. Without this, alm.toa was built directly - * from the raw seconds-of-week value with no week at all (implicitly - * placing it just after the GPS epoch, decades off any real date) -- - * self-consistent per satellite, so not obviously wrong, but propagating - * a Keplerian orbit forward by that many orbital periods is exquisitely - * sensitive to each satellite's own tiny orbital-parameter differences, - * producing an elevation nowhere near the true one for some satellites - * while others happen to alias back close to correct by coincidence. - * Default (0) disables week resolution entirely -- WNa is used as-is, - * matching this function's behavior before ref_week was tracked, for - * callers with no reference week available (e.g. unit tests that only - * check fields unrelated to the resolved week). + * \param ref_week Current GPS week, used to resolve the truncated almanac + * week (WNa: 8 bits for GPS, 2 bits for Galileo) to the closest full week so + * that alm.toa is an absolute epoch, as eph_to_rtklib() does for ephemeris. + * 0 (default) uses WNa as-is; only valid for callers that never propagate + * the orbit (e.g. unit tests checking unrelated fields). */ alm_t alm_to_rtklib(const Gps_Almanac& gps_alm, int ref_week = 0); alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm, int ref_week = 0); diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 93b1b32ea..1747346da 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -271,19 +271,9 @@ void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts) trace(4, "alm2pos : time=%s sat=%2d\n", time_str(time, 3), alm->sat); - // NOT timediffweekcrossover() -- that snaps any |raw diff| over half a - // week (302400s) back by a full week, which is exactly the wrong thing - // here: alm->toa is built by alm_to_rtklib() via resolve_truncated_week() - // (see its own doc comment), already a fully-resolved absolute epoch -- - // not the bare, still-needs-disambiguating seconds-of-week value that - // correction is meant for. A genuinely multi-week-old almanac (real, - // legitimate -- Galileo's WNa cycles every 16 weeks, so up to +-8 weeks - // from any reference is valid) would silently have its tk corrupted by - // exactly one week the moment the true gap crosses that threshold, - // producing a wrong position that looks plausible rather than obviously - // broken. alm2pos() has exactly one caller in this codebase - // (compute_visible_satellites() in satellite_visibility.cc), always via - // alm_to_rtklib(), so nothing depends on the old semantics here. + /* alm->toa is an absolute epoch and almanacs stay valid for weeks, so tk + * may legitimately exceed half a week: the week-crossover wrapper would + * propagate from the wrong epoch. */ tk = timediff(time, alm->toa); if (alm->A <= 0.0) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 31e86b0ba..cf0111b1c 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -723,14 +723,9 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, continue; } double elaux = satazel(pos, e, azel + i * 2); - /* !isfinite(elaux): corrupt ephemeris/almanac (e.g. an out-of-range - * eccentricity) can propagate NaN through eph2pos()/alm2pos() into - * azel -- azel itself is intentionally left as computed (including - * NaN) for callers that report it (e.g. the monitor's per-satellite - * az/el), but a NaN elevation must not be allowed into the position - * solve below: elaux < opt->elmin is a no-op against NaN (every - * relational comparison against NaN is false), so it would - * otherwise fall through as if elmin had been satisfied. */ + /* Reject NaN explicitly: every comparison with NaN is false, so a NaN + * elevation (corrupt ephemeris/almanac) would otherwise pass the + * mask. azel is left as computed for callers that report it. */ if (elaux < opt->elmin || !std::isfinite(elaux)) { trace(4, "satazel error. el = %lf , elmin = %lf\n", elaux, opt->elmin); @@ -739,9 +734,8 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, /* psudorange with code bias correction */ double iono_scale = 1.0; P = prange(obs + i, nav, azel + i * 2, iter, opt, &vmeas, &iono_scale); - /* Same NaN concern as above, for prange()'s own corrections (iono/ - * tropo/code bias) -- P == 0.0 is prange()'s own "no valid pseudorange" - * sentinel and doesn't catch NaN either. */ + /* 0.0 is prange()'s "no valid pseudorange" sentinel; it does not + * catch a NaN from the iono/tropo/code-bias corrections. */ if (P == 0.0 || !std::isfinite(P)) { trace(4, "prange error\n"); diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index ef34e6ea0..33a16de3b 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -1067,12 +1067,10 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) // ************ end time tags ************** for (int32_t m = 0; m < ninput_items[n]; m++) { - if (d_conf.enable_monitor) - { - // Keep the latest tracking data so that channels without a - // valid time reference can be reported through the Monitor - d_last_trk_data[n] = in[n][m]; - } + // Keep the latest tracking data so that channels without a valid + // time reference can be reported through the Monitor and to the + // flowgraph channel status (secondary-frequency acquisition assistance) + d_last_trk_data[n] = in[n][m]; // Latch a restarted carrier phase accumulator reported by the // tracking block: it is flagged on a single sample, which may // not be the one the observation is interpolated from @@ -1186,8 +1184,20 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) { for (uint32_t n = 0; n < d_nchannels_out; n++) { - const std::shared_ptr gnss_synchro_sptr = std::make_shared(epoch_data[n]); - // publish valid gnss_synchro to the gnss_flowgraph channel status monitor + std::shared_ptr gnss_synchro_sptr = std::make_shared(epoch_data[n]); + if (!gnss_synchro_sptr->Flag_valid_pseudorange && has_fresh_trk_data(n, d_Rx_clock_buffer.front())) + { + // Tracking-only status: the channel is locked but has no + // valid time reference yet. The flowgraph uses its Doppler + // to assist the acquisition of the same satellite on a + // secondary frequency. Never a valid observable. + gnss_synchro_sptr = std::make_shared(d_last_trk_data[n]); + gnss_synchro_sptr->Flag_valid_pseudorange = false; + gnss_synchro_sptr->Flag_valid_word = false; + gnss_synchro_sptr->Flag_valid_acquisition = false; + gnss_synchro_sptr->Channel_ID = n; + } + // publish gnss_synchro to the gnss_flowgraph channel status monitor this->message_port_pub(pmt::mp("status"), pmt::make_any(gnss_synchro_sptr)); } d_T_status_report_timer_ms = 0; diff --git a/src/core/interfaces/pvt_interface.h b/src/core/interfaces/pvt_interface.h index f45cec286..a97e0a986 100644 --- a/src/core/interfaces/pvt_interface.h +++ b/src/core/interfaces/pvt_interface.h @@ -27,8 +27,12 @@ #include "beidou_dnav_ephemeris.h" #include "galileo_almanac.h" #include "galileo_ephemeris.h" +#include "glonass_gnav_almanac.h" +#include "glonass_gnav_ephemeris.h" +#include "glonass_gnav_utc_model.h" #include "gnss_block_interface.h" #include "gps_almanac.h" +#include "gps_cnav_ephemeris.h" #include "gps_ephemeris.h" #include @@ -52,6 +56,10 @@ class PvtInterface : public GNSSBlockInterface public: virtual void reset() = 0; virtual void clear_ephemeris() = 0; + virtual std::map get_gps_cnav_ephemeris() const = 0; + virtual std::map get_glonass_ephemeris() const = 0; + virtual std::map get_glonass_almanac() const = 0; + virtual Glonass_Gnav_Utc_Model get_glonass_utc_model() const = 0; virtual std::map get_gps_ephemeris() const = 0; virtual std::map get_galileo_ephemeris() const = 0; virtual std::map get_beidou_dnav_ephemeris() const = 0; diff --git a/src/core/libs/channel_status_msg_receiver.cc b/src/core/libs/channel_status_msg_receiver.cc index c96bb51a8..d5b50fe76 100644 --- a/src/core/libs/channel_status_msg_receiver.cc +++ b/src/core/libs/channel_status_msg_receiver.cc @@ -84,6 +84,17 @@ void channel_status_msg_receiver::msg_handler_channel_status(const pmt::pmt_t& m { d_channel_status_map.erase(gnss_synchro_obj->Channel_ID); } + // Tracking map: every locked channel, with or without a valid time + // reference, for the secondary-frequency acquisition assistance + if ((gnss_synchro_obj->Flag_valid_pseudorange == true) || + ((gnss_synchro_obj->Flag_valid_symbol_output == true) && (gnss_synchro_obj->PRN != 0))) + { + d_channel_tracking_map[gnss_synchro_obj->Channel_ID] = gnss_synchro_obj; + } + else + { + d_channel_tracking_map.erase(gnss_synchro_obj->Channel_ID); + } // std::cout << "-------- \n" << '\n'; // for (std::map>::iterator it = d_channel_status_map.begin(); it != d_channel_status_map.end(); ++it) @@ -121,6 +132,21 @@ std::map> channel_status_msg_receiver::get_cu } +std::map> channel_status_msg_receiver::get_current_tracking_map() +{ + gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_channel_status function called by the scheduler + return d_channel_tracking_map; +} + + +void channel_status_msg_receiver::clear_channel_status(int channel_id) +{ + gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_channel_status function called by the scheduler + d_channel_status_map.erase(channel_id); + d_channel_tracking_map.erase(channel_id); +} + + Monitor_Pvt channel_status_msg_receiver::get_current_status_pvt() { gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_channel_status function called by the scheduler diff --git a/src/core/libs/channel_status_msg_receiver.h b/src/core/libs/channel_status_msg_receiver.h index 5168dc690..3ebc099a0 100644 --- a/src/core/libs/channel_status_msg_receiver.h +++ b/src/core/libs/channel_status_msg_receiver.h @@ -51,17 +51,32 @@ public: */ std::map> get_current_status_map(); + /*! + * \brief return the current status map of all locked channels, including + * those without a valid time reference yet (no valid pseudorange). Meant + * for the secondary-frequency acquisition assistance, which only needs the + * satellite and its Doppler. + */ + std::map> get_current_tracking_map(); + /*! * \brief return the current receiver PVT */ Monitor_Pvt get_current_status_pvt(); + /*! + * \brief forget the status of a channel that has been stopped, so that a + * stale entry is not reported until the channel delivers new observables + */ + void clear_channel_status(int channel_id); + private: friend channel_status_msg_receiver_sptr channel_status_msg_receiver_make(); channel_status_msg_receiver(); void msg_handler_channel_status(const pmt::pmt_t& msg); Monitor_Pvt d_pvt_status{}; std::map> d_channel_status_map; + std::map> d_channel_tracking_map; }; diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index 06825fe76..12d8c2d41 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -323,23 +323,17 @@ void ControlThread::event_dispatcher(bool &valid_event, pmt::pmt_t &msg) } } - // Deliberately outside the else branch above (and so not gated on - // valid_event): the visibility-aware search's own churn through the - // "maybe visible" candidate pool -- picking, acquiring, failing, - // re-picking -- keeps pushing channel events onto control_queue_ fast - // enough that the 100 ms timed_wait_and_pop() above essentially never - // times out once that churn starts. If MaybeUpdateVisibility() only ran - // from the idle branch, it would starve for the rest of the run right - // when it matters most, since reclassifying satellites out of "maybe - // visible" is exactly what would reduce that churn. Calling it - // unconditionally here is safe: Tick()/DataChanged() already throttle - // the expensive recompute internally (kDataCheckEveryNTicks, - // interval_elapsed, etc.), so this just guarantees those checks - // actually get evaluated instead of depending on an idle window that - // may never occur. + // Run on every event, not only in the idle branch above: acquisition + // churn through the "maybe visible" pool (pick, acquire, fail, re-pick) + // keeps the 100 ms timed_wait_and_pop() from timing out, so the idle + // branch would starve exactly when reclassification is needed to reduce + // that churn. Safe unconditionally: Tick() throttles the recompute. if (receiver_on_standby_ == false) { flowgraph_->MaybeUpdateVisibility(); // no-op unless GNSS-SDR.enable_visibility_aware_search=true + // A duplicated satellite breaks every PVT solution until one of + // the two channels is stopped, so do not wait for an idle tick. + flowgraph_->stop_duplicated_satellite_channels(); } } @@ -380,8 +374,14 @@ int ControlThread::run() } // Start the flowgraph flowgraph_->start(); - // Set receiver in stdby mode - flowgraph_->apply_action(0, 10); + if (flowgraph_->visibility_aware_search_enabled()) + { + // Channel assignments made at connect() time predate any + // visibility classification: standby returns them to the search + // pool to be re-picked once the first classification exists. With + // the feature disabled, connect()-time acquisitions run untouched. + flowgraph_->apply_action(0, 10); + } if (flowgraph_->running()) { LOG(INFO) << "Flowgraph started"; @@ -393,18 +393,10 @@ int ControlThread::run() // launch GNSS assistance process AFTER the flowgraph is running because the GNU Radio asynchronous queues must be already running to transport msgs assist_GNSS(); - // No explicit MaybeUpdateVisibility() call here (deliberately -- an - // earlier version had one, which turned out to double up with - // assist_GNSS()'s own call below and log an identical recompute twice). - // When GNSS-SDR.AGNSS_ref_location IS configured, assist_GNSS() already - // hot-started internally (see its own MaybeUpdateVisibility() call right - // before its own apply_action(0, 12)) and channels are already assigned - // by the time we get here. When it's NOT configured, the very next - // idle tick (at most ~100 ms away, see the event_dispatcher() timeout - // branch) calls MaybeUpdateVisibility() itself, right alongside - // acquisition_manager() -- close enough to startup not to need a - // second explicit call here too. - flowgraph_->apply_action(0, 12); + // No MaybeUpdateVisibility() here: assist_GNSS() already ran the first + // classification if GNSS-SDR.AGNSS_ref_location is set, and + // event_dispatcher() runs it within ~100 ms otherwise; its idle-branch + // acquisition_manager() is what restarts acquisition on idle channels. // start the keyboard_listener thread #if USE_GLOG_AND_GFLAGS if (FLAGS_keyboard) @@ -964,16 +956,11 @@ void ControlThread::assist_GNSS() // Set the receiver in Standby mode flowgraph_->apply_action(0, 10); - // MaybeUpdateVisibility() supersedes get_visible_sats()/ - // priorize_satellites() entirely when the feature is enabled -- - // both would otherwise run back to back against the identical - // AGNSS reference time/position, redundantly recomputing (and - // re-printing) the same elevation pass twice for no reason. This - // is the actual first hot-start trigger when - // GNSS-SDR.AGNSS_ref_location is configured (as here); the - // ControlThread::run() call after assist_GNSS() returns is a - // no-op by the time control gets back there, since channels have - // already been assigned by the apply_action(0, 12) below. + // With the visibility-aware search enabled, this is the first + // classification (from the AGNSS reference time/position) and it + // supersedes the legacy get_visible_sats()/priorize_satellites() + // pass, which would recompute the same elevations. Acquisition + // restarts at the next idle tick via acquisition_manager(). if (flowgraph_->visibility_aware_search_enabled()) { flowgraph_->MaybeUpdateVisibility(); @@ -984,8 +971,6 @@ void ControlThread::assist_GNSS() const std::vector> visible_sats = get_visible_sats(ref_rx_utc_time, ref_LLH); flowgraph_->priorize_satellites(visible_sats); } - // Hot Start - flowgraph_->apply_action(0, 12); } } @@ -1022,14 +1007,10 @@ void ControlThread::apply_action(unsigned int what) break; case 12: LOG(INFO) << "Receiver action HOTSTART"; - // Same choice assist_GNSS() makes at initial start: when the - // visibility-aware search is enabled, MaybeUpdateVisibility() - // already supersedes the legacy one-shot reorder below -- take - // that path here too instead of always falling back to it, - // otherwise a TC hotstart would classify satellites differently - // (and reset the continuously-maintained visible/maybe-visible - // buckets) compared to how the receiver started up in the first - // place. + // Mirror assist_GNSS(): with the visibility-aware search enabled, + // MaybeUpdateVisibility() supersedes the legacy one-shot reorder, + // so a TC hotstart classifies satellites as startup did and keeps + // the continuously-maintained visibility buckets. if (flowgraph_->visibility_aware_search_enabled()) { flowgraph_->MaybeUpdateVisibility(); @@ -1088,11 +1069,12 @@ std::vector> ControlThread::get_visible_sats(time std::cout << "Get visible satellites at " << str_time << "UTC, assuming RX position " << LLH[0] << " [deg], " << LLH[1] << " [deg], " << LLH[2] << " [m]\n"; - // 3. Loop through all the available ephemeris/almanac and compute satellite - // elevations -- shared with SatelliteVisibility's runtime recompute, see - // satellite_visibility.h/.cc. + // 3. Compute satellite elevations from all available ephemeris/almanac + // (shared with SatelliteVisibility's runtime recompute). const double elevation_mask_deg = configuration_->property("GNSS-SDR.search_elevation_mask", 0.0); - return compute_visible_satellites(flowgraph_->get_pvt(), gps_gtime, r_eb_e, elevation_mask_deg); + return compute_visible_satellites(flowgraph_->get_pvt(), gps_gtime, r_eb_e, elevation_mask_deg, + nullptr, std::numeric_limits::infinity(), nullptr, nullptr, + configuration_->property("PVT.glonass_strict_health", true)); } diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 6111442c5..91e5e6107 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -57,14 +57,19 @@ #include // for exit #include // for exception #include // for std::ifstream +#include // for std::setprecision #include // for operator<< #include // for insert_iterator, inserter +#include // for std::map #include // for std::shared_ptr #include // for set #include // for std::stringstream #include // for invalid_argument +#include // for std::string #include // for std::thread +#include // for std::tuple #include // for std::move +#include // for std::vector #if USE_GLOG_AND_GFLAGS #include @@ -1647,9 +1652,10 @@ int GNSSFlowgraph::assign_channels() } else { - // Shouldn't happen: the channel_count vs. max_sat_count - // check earlier in this function already guarantees - // enough PRNs exist for every channel being assigned here. + // Pool exhaustion is ruled out by the channel_count vs. max_sat_count + // check above; this happens for assisted secondary-frequency signals + // before any primary is tracked. The channel keeps a PRN 0 placeholder + // until acquisition_manager() assigns it. LOG(WARNING) << "No satellite available to assign to channel " << i << " for signal " << gnss_signal_str; } } @@ -1709,9 +1715,7 @@ void GNSSFlowgraph::push_back_signal(const Gnss_Signal& gs) auto& available_signals = available_signals_map_.at(gs.get_signal_str()); available_signals.remove(gs); available_signals.push_back(gs); - // A new candidate just entered this signal's pool -- worth re-checking - // even if it was previously exhausted. See signals_with_nothing_searchable_'s - // doc comment. + // A new candidate entered this pool: re-check it even if previously exhausted. signals_with_nothing_searchable_.erase(gs.get_signal_str()); } @@ -1764,6 +1768,12 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who) float estimated_doppler; double RX_time; + if ((sat_ == 0) && (channels_state_[current_channel] == 1) && + (channels_[current_channel]->get_signal().get_satellite().get_PRN() == 0)) + { + channels_state_[current_channel] = 0; + } + if ((sat_ == 0) && (channels_state_[current_channel] == 0)) { bool signal_available = true; @@ -1778,12 +1788,9 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who) channels_[current_channel]->set_signal(gnss_signal); start_acquisition = is_primary_freq || assistance_available || !configuration_->property("GNSS-SDR.assist_dual_frequency_acq", multiband_); } - // else: no untracked/unassigned satellite left for this - // signal right now -- leave the channel idle (saves the - // CPU cost of a doomed acquisition attempt) instead of - // crashing the receiver; acquisition_manager() re-checks - // every idle tick and will pick it back up once a - // satellite is requeued. + // else: nothing searchable for this signal now. Leave the channel + // idle instead of starting a doomed acquisition; acquisition_manager() + // re-checks every idle tick and resumes once a satellite is requeued. } else { @@ -1800,9 +1807,8 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who) << ", Signal " << channels_[current_channel]->get_signal().get_signal_str(); if (assistance_available == true && configuration_->property("GNSS-SDR.assist_dual_frequency_acq", multiband_)) { - // Doppler is exactly known from the already-tracked assisting - // frequency (search_next_signal() returns it already projected - // to this band): restrict the search to a single Doppler bin. + // Doppler is known from the tracked assisting band (already + // projected by search_next_signal()): search a single Doppler bin. channels_[current_channel]->assist_acquisition_doppler(estimated_doppler, 0); } else @@ -1827,19 +1833,9 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who) } else if (gnss_signal.get_satellite().get_PRN() != 0) { - // PRN 0 (default-constructed Gnss_Signal) means - // search_next_signal() never actually picked - // anything real this call -- signal_available was - // false (pool exhausted, or an assisted signal - // with no primary-frequency assist available - // right now), so gnss_signal was never populated. - // Pushing that back would call - // push_back_signal()'s available_signals_map_.at("") - // on the default-constructed empty signal string - // and throw std::out_of_range, uncaught, taking - // the whole receiver down. There's nothing real - // here to push back -- the pool was never - // touched in the first place. + // PRN 0 means search_next_signal() picked nothing (signal_available + // was false), so there is nothing to return to the pool; pushing the + // default-constructed signal would throw on its empty signal string. push_back_signal(gnss_signal); DLOG(INFO) << "Channel " << current_channel << " secondary frequency acquisition assistance not available in " @@ -1913,17 +1909,9 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) case 1: gs = channels_[who]->get_signal(); - // Acquisition success is a real RF detection, not just a - // predictive classification -- once it happens, let the channel - // proceed to tracking even if this satellite is (or has since - // become) classified excluded by the search mask. The mask - // exists to avoid wasting CPU searching for satellites unlikely - // to be in view; it has no business tearing down a signal - // that's demonstrably there. PVT.elevation_mask remains the - // real gate on whether this satellite's observations get used - // in the fix. Only the search layer (picking new acquisitions, - // and not retrying after a genuine loss of lock -- see case 2 - // below) is scoped by visibility. + // A successful acquisition is real RF evidence: proceed to tracking even if + // the satellite is now classified excluded. The search mask only saves CPU + // on unlikely searches; PVT.elevation_mask still gates use in the fix. DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << gs.get_satellite(); // If the satellite is in the list of available ones, remove it. @@ -1941,12 +1929,8 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) case 2: gs = channels_[who]->get_signal(); DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << gs.get_satellite(); - // This path (lock loss / cycle slip recovery) otherwise bypasses - // the search pool/visibility entirely: below, it blindly retries - // the very same satellite unconditionally. If that satellite has - // since been classified excluded, don't -- fall through to a - // normal search for a new target instead, same as if this - // channel had nothing assigned. + // The retry below bypasses the search pool: do not retry a satellite that + // has since been classified excluded; search for a new target instead. if (satellite_visibility_ && satellite_visibility_->enabled() && satellite_visibility_->IsExcluded(gs.get_satellite())) { LOG(INFO) << "[visibility] channel " << who << ": not retrying " << gs.get_satellite() @@ -1997,27 +1981,8 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) { if (channels_state_[n] == 1 || channels_state_[n] == 2 || channels_state_[n] == 3) // channel assigned (not yet started), in acquisition, or in tracking { - // recover the satellite assigned. State 1 channels - // (assigned by assign_channels() at connect() time, - // budget-throttled by Channels.in_acquisition from - // ever starting) matter here too: their assignment - // predates any visibility classification -- without - // discarding it, acquisition_manager() later reuses - // that stale pick verbatim the moment a slot frees - // up (its state==1 branch just re-issues whatever - // signal the channel already holds, bypassing - // search_next_signal()/pop_by_visibility() entirely), - // silently holding a pool entry hostage to a pick - // made before any real data existed. Gnss_Signal gs_assigned = channels_[n]->get_signal(); - // PRN 0 is not a real satellite -- it's the - // default-constructed Gnss_Signal a channel is - // still holding if assign_channels() couldn't - // give it a real one at connect() time (e.g. a - // 5X channel whose primary-frequency assist - // wasn't available yet, before any tracking - // could possibly exist). Pushing that back would - // inject a phantom entry into the pool. + if (gs_assigned.get_satellite().get_PRN() != 0) { push_back_signal(gs_assigned); @@ -2095,33 +2060,9 @@ void GNSSFlowgraph::MaybeUpdateVisibility() if (satellite_visibility_->Tick(pvt_ptr, fix_status)) { DLOG(INFO) << "Satellite visibility updated (visibility-aware search enabled)"; - // Classification actually changed -- a signal previously - // exhausted (everything excluded) might not be anymore. See - // signals_with_nothing_searchable_'s doc comment. + // Classification changed: previously exhausted signals may be searchable again. signals_with_nothing_searchable_.clear(); - // A channel already assigned but not yet started (state 1) may - // be holding a satellite that's only now become known to be - // excluded (elevation computed, at/below the mask) -- notably, - // assign_channels()'s very first picks at connect() time - // necessarily predate any classification at all (AGNSS - // assistance data isn't loaded into the PVT block until later, - // in ControlThread::assist_GNSS()), so an early channel can - // easily be sitting on a below-mask satellite with nothing - // having filtered it out yet. Left alone it would go on to - // start acquiring a satellite the mask says to skip. Requeue it - // so it's re-picked properly now that a real classification - // exists -- no work has been invested yet, so this is free. - // - // Deliberately NOT extended to state 2 (acquisition actively - // running): a search that's already started completes on its - // own regardless of a classification change mid-flight -- if it - // fails, it simply won't be re-picked next time since it's now - // excluded; if it succeeds, it becomes state 3 and is handled by - // the loop below. Interrupting a running search on - // reclassification would occasionally discard one that was - // about to succeed, based on a classification that (unlike - // confirmed tracking) can still be wrong. const auto current_channels_status = channels_status_->get_current_status_map(); auto is_currently_tracked = [&](const Gnss_Signal& gs) { const uint32_t prn = gs.get_satellite().get_PRN(); @@ -2156,21 +2097,6 @@ void GNSSFlowgraph::MaybeUpdateVisibility() push_back_signal(gs_assigned); channels_state_[n] = 0; } - - // A channel already in confirmed tracking (state 3) is never - // torn down here, regardless of exclusion. The search mask - // exists to avoid wasting CPU acquiring satellites unlikely to - // be in view -- it is not a correctness gate, and a confirmed - // lock is stronger evidence than any predictive elevation - // classification (which can itself be wrong, e.g. a coarse - // almanac near the mask boundary, or a rising satellite still - // climbing toward it). PVT.elevation_mask remains the real gate - // on whether this satellite's observations get used in the fix. - // Once a genuine loss of lock happens, TRK FAILED handling - // (case 2 in apply_action) already keeps visibility from - // re-acquiring an excluded satellite -- that is where this - // classification takes effect for an already-tracked satellite, - // not by tearing down a good track. } } @@ -2181,6 +2107,67 @@ bool GNSSFlowgraph::visibility_aware_search_enabled() const } +void GNSSFlowgraph::stop_duplicated_satellite_channels() +{ + std::lock_guard lock(signal_list_mutex_); + const auto current_channels_status = channels_status_->get_current_status_map(); + // Group tracking channels by reported (system, PRN, signal), i.e. what PVT receives. + std::map, std::vector> tracked_channels; + for (const auto& cs : current_channels_status) + { + const int channel_id = cs.first; + if (!cs.second || channel_id < 0 || channel_id >= channels_count_ || channels_state_[channel_id] != 3) + { + continue; + } + tracked_channels[std::make_tuple(cs.second->System, cs.second->PRN, std::string(cs.second->Signal, 2))].push_back(channel_id); + } + for (const auto& entry : tracked_channels) + { + const std::vector& channel_ids = entry.second; + if (channel_ids.size() < 2) + { + continue; + } + int strongest = channel_ids.front(); + for (const int channel_id : channel_ids) + { + if (current_channels_status.at(channel_id)->CN0_dB_hz > current_channels_status.at(strongest)->CN0_dB_hz) + { + strongest = channel_id; + } + } + for (const int channel_id : channel_ids) + { + if (channel_id == strongest) + { + continue; + } + const Gnss_Synchro& synchro = *current_channels_status.at(channel_id); + const Gnss_Signal gs_assigned = channels_[channel_id]->get_signal(); + std::ostringstream oss; + oss << "Channel " << channel_id << ", assigned to " << gs_assigned.get_satellite() + << ", is tracking " << synchro.System << synchro.PRN << " (" << std::string(synchro.Signal, 2) + << ") already tracked by channel " << strongest << " with higher C/N0 (" + << std::fixed << std::setprecision(1) << synchro.CN0_dB_hz << " vs " + << current_channels_status.at(strongest)->CN0_dB_hz << " dB-Hz). Stopping channel " << channel_id; + LOG(INFO) << oss.str(); + if (gs_assigned.get_satellite().get_PRN() != 0) + { + push_back_signal(gs_assigned); + } + channels_[channel_id]->stop_channel(); + // Channel status is reported once per second: drop the + // stale entry so the reassigned channel is not stopped + // again before it reports its own new observables. + channels_status_->clear_channel_status(channel_id); + channels_state_[channel_id] = 0; + acquisition_manager(channel_id); + } + } +} + + void GNSSFlowgraph::set_configuration(const std::shared_ptr& configuration) { if (running_) @@ -2301,7 +2288,7 @@ void GNSSFlowgraph::set_signals_list() {"SBAS", {120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138}}, {"Galileo", {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36}}, - {"Glonass", {1, 2, 3, 4, 9, 10, 11, 12, 18, 19, 20, 21, 26, 27, 28}}, + {"Glonass", {1, 2, 3, 4, 9, 10, 11, 12, 18, 19, 20, 21, 26, 27}}, {"Beidou", {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, @@ -2521,18 +2508,6 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal if (available_signals.empty()) { - // Every PRN for this signal is already tracked or assigned to - // another channel. This is expected in steady state once enough - // satellites are being tracked (or, at startup, a genuine - // Channels_*.count vs. PRN-count misconfiguration -- that case is - // already caught earlier, with a clean error return, by - // assign_channels()'s own channel_count/max_sat_count check). - // Either way, crashing the whole receiver here is wrong: tell - // the caller nothing is available so it leaves this channel idle - // instead of starting a doomed acquisition; acquisition_manager() - // re-checks every idle tick, so the channel picks back up on its - // own as soon as a satellite is requeued (ACQ FAILED elsewhere, - // or tracking lost). signal_available = false; const auto& entry = signal_mapping.at(searched_signal); LOG(INFO) << "[visibility] No untracked/unassigned satellites currently available for signal " @@ -2586,8 +2561,11 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal if (any_assist_configured) { - // 1. Get the current channel status map - const auto current_channels_status = channels_status_->get_current_status_map(); + // 1. Get the current channel tracking map: locked channels, with or without a + // valid time reference. Doppler is available as soon as the primary + // frequency is tracked, so the assisted search need not wait for the + // navigation message to be decoded. + const auto current_channels_status = channels_status_->get_current_tracking_map(); // 2. search the currently tracked primary signal satellites and assist the acquisition if the satellite is not tracked on the assisted signal for (const auto& assist_signal : assist_signal_candidates) { @@ -2603,25 +2581,31 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal { if (std::string(current_status.second->Signal) == assist_signal) { + const bool glonass_fdma = (searched_signal == "2G"); + const uint32_t tracked_prn = current_status.second->PRN; std::list::iterator it2; it2 = std::find_if(std::begin(available_signals), std::end(available_signals), - [&](Gnss_Signal const& sig) { return sig.get_satellite().get_PRN() == current_status.second->PRN; }); + [&](Gnss_Signal const& sig) { + if (sig.get_satellite().get_PRN() == tracked_prn) + { + return true; + } + if (!glonass_fdma) + { + return false; + } + const auto pool_channel = GLONASS_PRN.find(sig.get_satellite().get_PRN()); + const auto tracked_channel = GLONASS_PRN.find(tracked_prn); + return (pool_channel != GLONASS_PRN.cend()) && (tracked_channel != GLONASS_PRN.cend()) && + (pool_channel->second == tracked_channel->second); + }); if (it2 != available_signals.end()) { - // "An already-tracked satellite is definitive proof of - // visibility" no longer holds unconditionally: a primary- - // frequency track can itself be below the mask/unhealthy - // during blind acquisition (no classification existed yet - // when it was picked), in which case MaybeUpdateVisibility() - // releases it once real ephemeris/almanac classifies it - // excluded. Without this check, this fast path keeps - // re-offering that same satellite for the secondary - // frequency off the channel-status snapshot (which can - // still show it as tracked for a moment after release), - // right back into the ACQ-SUCCESS exclusion guard above -- - // a tight re-acquire/discard loop instead of the channel - // going idle like it should. + // Skip excluded satellites even if their primary frequency + // is reported as tracked: a blind connect()-time pick can + // sit below the mask, and the status snapshot may lag a + // channel that has already been stopped. if (satellite_visibility_ && satellite_visibility_->enabled() && satellite_visibility_->IsExcluded(it2->get_satellite())) { @@ -2631,16 +2615,14 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal estimated_doppler = static_cast(project_doppler(searched_signal, assist_signal, current_status.second->Carrier_Doppler_hz)); RX_time = current_status.second->RX_time; result = *it2; - // Assist-signal fast path: picks a satellite already - // tracked on its primary frequency directly, bypassing - // pop_by_visibility() entirely (its own bucket/ratio - // logic doesn't apply -- an already-tracked, not-excluded - // satellite is definitive proof of visibility). Logged - // separately from "[visibility] signal ...: picked ..." - // since it's a different pick path -- otherwise a queue's - // remaining visible/mayvisible counts can jump between - // pop_by_visibility() log lines with no logged pick to - // explain it. + if (glonass_fdma && (result.get_satellite().get_PRN() != tracked_prn)) + { + result = Gnss_Signal(Gnss_Satellite(std::string("Glonass"), tracked_prn), searched_signal); + } + // Assist fast path, bypassing pop_by_visibility() (a + // tracked, non-excluded satellite is proof of visibility). + // Logged separately so the queue counts in the + // pop_by_visibility() log lines remain explainable. LOG(INFO) << "[visibility] signal " << searched_signal << ": picked " << result.get_satellite() << " via primary-frequency assist (" << assist_signal << " already tracked) -- bypasses visible/mayvisible bucket selection"; @@ -2656,18 +2638,10 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal if (found_signal == false) { - // Matches upstream gnss-sdr/gnss-sdr's acquisition_manager(): - // with GNSS-SDR.assist_dual_frequency_acq on, a secondary/ - // assisted signal's start_acquisition reduces to just - // assistance_available -- a pick made any way other than the - // assist fast path above is guaranteed not to start (pushed - // straight back by acquisition_manager() the moment it sees - // assistance_available == false). That's not a bug to work - // around here: it's upstream's actual intended behavior - // (verified directly against gnss-sdr/gnss-sdr's next branch). - // Skip the pointless pick-then-immediate-pushback churn: idle - // the channel instead, and let the assist block above pick it - // up for real as soon as its primary frequency is tracked. + // With GNSS-SDR.assist_dual_frequency_acq on, an assisted signal only starts + // acquisition when assistance_available is true, so any pick other than the + // fast path above would be pushed straight back by acquisition_manager(). + // Idle the channel until its primary frequency is tracked. if (!assist_signal_candidates.empty() && configuration_->property("GNSS-SDR.assist_dual_frequency_acq", multiband_)) { signal_available = false; @@ -2716,14 +2690,8 @@ Gnss_Signal GNSSFlowgraph::pop_by_visibility(std::list& available_s { picked = false; - // Already determined nothing is searchable here, and nothing has - // invalidated that since (no push_back_signal() for this signal, no - // visibility reclassification) -- skip the full scan and the - // channels_status_ lock entirely. See signals_with_nothing_searchable_'s - // doc comment for why this matters: without it, an exhausted signal's - // pool gets this same expensive check redone on every idle tick for - // every idle channel, contending with the real-time DSP threads for no - // benefit. + // Cached exhaustion result: skips the scan and the channels_status_ lock until + // a push-back or a reclassification invalidates it. if (signals_with_nothing_searchable_.count(searched_signal) > 0) { return Gnss_Signal{}; @@ -2733,26 +2701,11 @@ Gnss_Signal GNSSFlowgraph::pop_by_visibility(std::list& available_s const uint32_t ratio = satellite_visibility_->search_ratio(); const bool want_visible = counter < ratio; - // A satellite currently tracked on any of its other signals is - // definitive proof it's actually visible right now -- this overrides - // IsExcluded() below. Almanac (coarse, used for the initial optimistic - // classification that can get a satellite tracked in the first place) - // and ephemeris (precise, only decoded once tracking begins) can - // legitimately disagree right at the elevation mask boundary; without - // this override, a satellite could get excluded immediately after - // starting to track, permanently blocking its still-unassigned - // other-frequency entries even while it's demonstrably being received. - // - // channels_status_->get_current_status_map() is a mutex-locked copy of - // the whole channel status map, shared with the real-time tracking/ - // observables threads that need the same lock to publish updates -- - // fetch it lazily, only if the pool actually has an excluded entry that - // could benefit from the override, not unconditionally on every call. - // Once a signal's pool is fully exhausted (everything visible tracked, - // the rest excluded), pop_by_visibility() gets re-invoked on every idle - // tick for every idle channel with nothing new to find; paying for this - // lock/copy every single time was real, measured lock contention against - // the DSP threads, not just wasted CPU. + // A satellite tracked on another signal is proof of visibility and overrides + // IsExcluded(): coarse almanac and precise ephemeris can disagree at the mask, + // and a freshly tracked satellite must not have its other-frequency entries + // blocked. get_current_status_map() copies the map under a lock shared with the + // DSP threads, so fetch it only when an excluded entry could use the override. const bool any_excluded = std::any_of(available_signals.begin(), available_signals.end(), [&](const Gnss_Signal& gs) { return satellite_visibility_->IsExcluded(gs.get_satellite()); }); std::map> current_channels_status; @@ -2774,11 +2727,8 @@ Gnss_Signal GNSSFlowgraph::pop_by_visibility(std::list& available_s return false; }; - // Excluded entries (elevation computable, at/below the mask) are never - // searchable, from either bucket -- unless the satellite is already - // confirmed tracked elsewhere (see above). Otherwise they stay queued - // (so they're picked back up automatically once visibility is - // recomputed and they're no longer excluded) but inert until then. + // Excluded entries are never picked (unless tracked elsewhere) but stay queued + // so they are picked up automatically once reclassified. auto is_searchable = [&](const Gnss_Signal& gs) { return !satellite_visibility_->IsExcluded(gs.get_satellite()) || is_currently_tracked(gs); }; @@ -2791,9 +2741,7 @@ Gnss_Signal GNSSFlowgraph::pop_by_visibility(std::list& available_s bool got_desired_bucket = (it != available_signals.end()); if (!got_desired_bucket) { - // Desired bucket has nothing searchable queued for this signal - // right now -- fall back to the other searchable bucket instead - // of starving acquisition. + // Selected bucket is empty for this signal: fall back to the other one. it = find_bucket(!want_visible); } @@ -2818,12 +2766,9 @@ Gnss_Signal GNSSFlowgraph::pop_by_visibility(std::list& available_s if (it == available_signals.end()) { - // Nothing searchable left for this signal -- every remaining - // queued entry (if any) is excluded (elevation known, not - // visible). Leave them queued and tell the caller to idle this - // channel instead of running a doomed acquisition; this is the - // actual CPU-saving exhaustion case once a complete almanac/ - // ephemeris confirms everything else is below the elevation mask. + // Only excluded entries remain: leave them queued and idle the channel. This + // is the CPU-saving case once a complete almanac/ephemeris places every + // untracked satellite below the mask. LOG(INFO) << "[visibility] signal " << searched_signal << ": nothing searchable (" << remaining_excluded << " excluded entries queued, elevation known and not visible) -- channel stays idle" << " (further checks skipped until this changes)"; @@ -2845,10 +2790,8 @@ Gnss_Signal GNSSFlowgraph::pop_by_visibility(std::list& available_s remaining_mayvisible = remaining_mayvisible > 0 ? remaining_mayvisible - 1 : 0; } - // Always-on diagnostic (LOG, not DLOG -- see satellite_visibility.cc for - // why). Shows exactly which bucket each new acquisition is actually - // drawn from and what's left queued afterwards -- the direct signal for - // "are channels still grinding through maybe-visible satellites". + // Always-on diagnostic: which bucket each acquisition is drawn from and what + // remains queued. LOG(INFO) << "[visibility] signal " << searched_signal << ": picked " << result.get_satellite() << " from " << (picked_visible ? "VISIBLE" : "MAYVISIBLE") << " bucket" << (got_desired_bucket ? "" : " (desired bucket was empty, fell back)") diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index 775aeb166..a5a72c0a0 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -162,24 +162,32 @@ public: void priorize_satellites(const std::vector>& visible_satellites); /*! - * \brief Re-evaluates satellite visibility if warranted (new fix, new - * ephemeris/almanac, or the recompute interval elapsed) and lets - * subsequent search_next_signal() calls favor currently-visible - * satellites. No-op unless GNSS-SDR.enable_visibility_aware_search=true. - * Called from ControlThread's idle-tick loop. + * \brief Re-evaluates satellite visibility when warranted (new fix, new + * ephemeris/almanac, or recompute interval elapsed) so that subsequent + * search_next_signal() calls favor visible satellites. + * No-op unless GNSS-SDR.enable_visibility_aware_search=true. */ void MaybeUpdateVisibility(); /*! - * \brief Whether GNSS-SDR.enable_visibility_aware_search is on. Lets - * callers (ControlThread::assist_GNSS()) skip the legacy - * get_visible_sats()/priorize_satellites() one-shot startup reorder when - * MaybeUpdateVisibility() already supersedes it -- otherwise both would - * run back to back against identical inputs, computing (and printing) - * the same elevation pass twice for no reason. + * \brief Whether GNSS-SDR.enable_visibility_aware_search is on, so callers can + * skip the legacy get_visible_sats()/priorize_satellites() startup reorder that + * MaybeUpdateVisibility() supersedes. */ bool visibility_aware_search_enabled() const; + /*! + * \brief Stops any channel whose decoded satellite is already tracked, on the + * same signal, by another channel with higher C/N0. The stopped channel's + * assignment returns to the search pool and the channel is re-dispatched. + * + * GLONASS channels are assigned a frequency channel and only learn their + * orbital slot from the navigation message, so a false lock can be relabelled + * as a satellite another channel already tracks; duplicate observations of + * one satellite break the PVT solution. + */ + void stop_duplicated_satellite_channels(); + #if ENABLE_FPGA void start_acquisition_helper(); @@ -235,17 +243,12 @@ private: void push_back_signal(const Gnss_Signal& gs); void remove_signal(const Gnss_Signal& gs); - // Visibility-aware replacement for available_signals.front()/pop_front() - // in search_next_signal(): picks the next visible or maybe-visible - // (unknown) entry (per SatelliteVisibility::search_ratio()) from the - // given signal's queue and erases it in place, preserving each bucket's - // relative FIFO order. Falls back to the other bucket if the chosen one - // is empty for this signal. Entries SatelliteVisibility::IsExcluded() - // are never picked, from either bucket -- they stay queued but inert - // until visibility is recomputed. Sets picked=false (and returns a - // default-constructed Gnss_Signal) if nothing searchable remains, i.e. - // every queued entry is excluded. Only called when - // satellite_visibility_->enabled(). + // Visibility-aware replacement for available_signals.front()/pop_front() in + // search_next_signal(): erases and returns the next searchable entry from the + // visible or maybe-visible bucket selected by the search-ratio counter, falling + // back to the other bucket when the selected one is empty; FIFO order is kept + // within each bucket. Excluded entries are never picked but stay queued until + // the next visibility recompute. Sets picked=false when nothing is searchable. Gnss_Signal pop_by_visibility(std::list& available_signals, const std::string& searched_signal, bool& picked); void print_help(); void check_desktop_conf_in_fpga_env(); @@ -290,24 +293,15 @@ private: std::unique_ptr satellite_visibility_; std::unordered_map visibility_pick_counter_; // per signal_str, ratio-based visible/maybe-visible cycling - // Signals for which pop_by_visibility() last determined nothing is - // searchable (every queued entry excluded). acquisition_manager() calls - // search_next_signal() for every idle channel on every idle tick - // (~10 Hz), so once a signal's pool is exhausted this would otherwise - // redo the full exclusion scan -- including a mutex-locked copy of - // channels_status_'s whole map, contended with the real-time tracking/ - // observables threads -- many times a second for no reason, since - // nothing changes between recomputes. Invalidated (entries erased) by - // push_back_signal() for that specific signal, and entirely by - // MaybeUpdateVisibility() whenever a Tick() actually changes - // classification -- see both for details. + // Signals whose pool pop_by_visibility() found fully excluded. Cached because + // acquisition_manager() re-queries every idle channel at ~10 Hz and each scan + // copies channels_status_ under a lock shared with the DSP threads. Invalidated + // per signal by push_back_signal(), and entirely when MaybeUpdateVisibility() + // changes the classification. std::set signals_with_nothing_searchable_; - // Throttles the "no -tracked satellite available" log in - // search_next_signal() to at most once every few seconds per signal -- - // the underlying check itself is cheap (a config lookup, no pool scan), - // but acquisition_manager() re-evaluates every idle channel on every - // idle tick (~10 Hz), so logging it unconditionally floods the log for - // no diagnostic benefit once the condition is already known. + // Rate-limits the "no assist-tracked satellite available" log in + // search_next_signal(): the condition is re-evaluated for every idle channel at + // ~10 Hz and would otherwise flood the log. std::unordered_map no_assist_log_throttle_; enum StringValue diff --git a/src/core/receiver/satellite_visibility.cc b/src/core/receiver/satellite_visibility.cc index 868fa5a36..8abc57b73 100644 --- a/src/core/receiver/satellite_visibility.cc +++ b/src/core/receiver/satellite_visibility.cc @@ -21,6 +21,7 @@ #include "geofunctions.h" #include "monitor_pvt.h" #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_rtkcmn.h" // for utc2gpst, gpst2time @@ -33,35 +34,8 @@ #include #include #include -#include #include -namespace -{ -// Signed age (seconds) of data referenced at TOW ref_tow, relative to the -// current TOW now_tow (both seconds-of-week, [0, 604800)) -- positive means -// the reference is in the past, negative means it's still ahead (normal for -// ephemeris broadcast near the start of its fit interval). Handles the -// week-boundary wrap so data referenced just before a Saturday/Sunday -// midnight rollover isn't misjudged as ~604800 s stale the moment the week -// ticks over. Only unambiguous for |age| well under half a week (302400 s); -// callers must keep their max-age thresholds under that. -double DataAgeS(double now_tow, int32_t ref_tow) -{ - double age = now_tow - static_cast(ref_tow); - if (age > 302400.0) - { - age -= 604800.0; - } - else if (age < -302400.0) - { - age += 604800.0; - } - return age; -} -} // namespace - - std::vector> compute_visible_satellites( const std::shared_ptr& pvt_ptr, const gtime_t& gps_gtime, @@ -70,21 +44,15 @@ std::vector> compute_visible_satellites( std::vector>* below_mask_out, double almanac_max_age_s, double* seconds_until_next_expiry_out, - const std::set>* only_prns) + const std::set>* only_prns, + bool glonass_strict_health) { std::vector> available_satellites; - // Almanac WNa is truncated (2 bits for Galileo, 8 for GPS) and needs a - // nearby full week to resolve against -- gps_gtime (the query time this - // whole computation is FOR) is always a valid, always-current source for - // that, unlike a config-supplied reference week (GNSS-SDR.observation_ - // date), which is easy to leave unset. An unset config value used to - // flow through as ref_gps_week<=0, which resolve_truncated_week() (see - // its doc comment) treated as "no reference -- return WNa unresolved", - // silently reintroducing the ~1024/~16-week rollover error the WN fix - // was supposed to eliminate. Deriving it here instead means a valid - // reference is guaranteed every time this function runs at all. + // Truncated week fields (GPS ephemeris WN mod 1024, GPS/Galileo almanac + // WNa) are resolved against the query epoch's own week: always available + // and near-correct, unlike a config-supplied reference week. int ref_gps_week = 0; - const double now_tow = time2gpst(gps_gtime, &ref_gps_week); + time2gpst(gps_gtime, &ref_gps_week); if (seconds_until_next_expiry_out != nullptr) { *seconds_until_next_expiry_out = std::numeric_limits::infinity(); @@ -95,14 +63,9 @@ std::vector> compute_visible_satellites( *seconds_until_next_expiry_out = std::min(*seconds_until_next_expiry_out, max_age_s - age); } }; - // PRNs already classified (visible or not) via ephemeris -- the almanac - // loops below skip any PRN in here entirely, rather than only skipping - // ones already added as visible. Ephemeris is materially more accurate - // than almanac, so once it has classified a PRN one way or the other, - // almanac must not be allowed to reclassify it the other way; without - // this, the same PRN could end up in both available_satellites (visible) - // and *below_mask_out (elevation known, not visible) depending on which - // data source is inspected first, which callers rely on never happening. + // PRNs already classified by ephemeris (either way). Almanac loops skip + // them, so a satellite never lands in both available_satellites and + // *below_mask_out. std::vector handled_gps; std::vector handled_gal; std::vector handled_bds; @@ -110,17 +73,18 @@ std::vector> compute_visible_satellites( const std::map gps_eph_map = pvt_ptr->get_gps_ephemeris(); for (const auto& it : gps_eph_map) { - if (only_prns != nullptr && only_prns->count(std::make_pair(std::string("GPS"), it.second.PRN)) == 0) + const std::string system = (it.second.PRN >= MINPRNQZS && it.second.PRN <= MAXPRNQZS) ? "QZSS" : "GPS"; + if (only_prns != nullptr && only_prns->count(std::make_pair(system, it.second.PRN)) == 0) { continue; } - const double age = DataAgeS(now_tow, it.second.toe); + const eph_t rtklib_eph = eph_to_rtklib(it.second, ref_gps_week); + const double age = timediff(gps_gtime, rtklib_eph.toe); if (std::abs(age) > MAXDTOE) { continue; // stale -- treat as if no ephemeris exists; almanac may still classify it } note_freshness(age, MAXDTOE); - const eph_t rtklib_eph = eph_to_rtklib(it.second, ref_gps_week); std::array r_sat{}; double clock_bias_s; double sat_pos_variance_m2; @@ -133,20 +97,63 @@ std::vector> compute_visible_satellites( const arma::vec dx = r_sat_eb_e - r_eb_e; topocent(&Az, &El, &dist_m, r_eb_e, dx); handled_gps.push_back(it.second.PRN); - // Broadcast health (0 == OK) is stronger evidence than geometry - // alone -- a satellite explicitly flagged unhealthy is known - // unusable regardless of what its computed elevation says, same - // as an elevation at/below the mask. + // Unhealthy is treated like below-mask, not like "no data". if (El > elevation_mask_deg && it.second.SV_health == 0) { - std::cout << "Using GPS Ephemeris: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using " << system << " Ephemeris: Sat " << it.second.PRN << " Az: " << Az << " El: " << El; available_satellites.emplace_back(floor(El), - (Gnss_Satellite(std::string("GPS"), it.second.PRN))); + (Gnss_Satellite(system, it.second.PRN))); } else if (below_mask_out != nullptr) { - std::cout << "Using GPS Ephemeris (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; - below_mask_out->emplace_back(floor(El), Gnss_Satellite(std::string("GPS"), it.second.PRN)); + LOG(INFO) << "Using " << system << " Ephemeris (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El; + below_mask_out->emplace_back(floor(El), Gnss_Satellite(system, it.second.PRN)); + } + } + + const std::map qzss_cnav_map = pvt_ptr->get_gps_cnav_ephemeris(); + for (const auto& it : qzss_cnav_map) + { + if (it.second.PRN < MINPRNQZS || it.second.PRN > MAXPRNQZS || + std::find(handled_gps.begin(), handled_gps.end(), it.second.PRN) != handled_gps.end()) + { + continue; + } + const std::string system("QZSS"); + if (only_prns != nullptr && only_prns->count(std::make_pair(system, it.second.PRN)) == 0) + { + continue; + } + const eph_t rtklib_eph = eph_to_rtklib(it.second); + const double age = timediff(gps_gtime, rtklib_eph.toe); + if (std::abs(age) > MAXDTOE_QZS) + { + continue; // stale -- treat as if no ephemeris exists; almanac may still classify it + } + note_freshness(age, MAXDTOE_QZS); + std::array r_sat{}; + double clock_bias_s; + double sat_pos_variance_m2; + eph2pos(gps_gtime, &rtklib_eph, r_sat.data(), &clock_bias_s, + &sat_pos_variance_m2); + double Az; + double El; + double dist_m; + const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]}; + const arma::vec dx = r_sat_eb_e - r_eb_e; + topocent(&Az, &El, &dist_m, r_eb_e, dx); + handled_gps.push_back(it.second.PRN); + // Unhealthy is treated like below-mask, not like "no data". + if (El > elevation_mask_deg && it.second.signal_health == 0) + { + LOG(INFO) << "Using " << system << " CNAV ephemeris: Sat " << it.second.PRN << " Az: " << Az << " El: " << El; + available_satellites.emplace_back(floor(El), + (Gnss_Satellite(system, it.second.PRN))); + } + else if (below_mask_out != nullptr) + { + LOG(INFO) << "Using " << system << " CNAV ephemeris (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El; + below_mask_out->emplace_back(floor(El), Gnss_Satellite(system, it.second.PRN)); } } @@ -157,13 +164,13 @@ std::vector> compute_visible_satellites( { continue; } - const double age = DataAgeS(now_tow, it.second.toe); + const eph_t rtklib_eph = eph_to_rtklib(it.second); + const double age = timediff(gps_gtime, rtklib_eph.toe); if (std::abs(age) > MAXDTOE_GAL) { continue; // stale -- treat as if no ephemeris exists; almanac may still classify it } note_freshness(age, MAXDTOE_GAL); - const eph_t rtklib_eph = eph_to_rtklib(it.second); std::array r_sat{}; double clock_bias_s; double sat_pos_variance_m2; @@ -176,20 +183,17 @@ std::vector> compute_visible_satellites( const arma::vec dx = r_sat_eb_e - r_eb_e; topocent(&Az, &El, &dist_m, r_eb_e, dx); handled_gal.push_back(it.second.PRN); - // E1B_HS (0 == "signal OK" per the Galileo ICD) gates this -- - // 1B is the primary/search-initiating signal, and 5X only ever - // gets searched once 1B is confirmed tracked (assist path), so - // an unhealthy 1B makes the whole satellite unusable for search - // regardless of E5a_HS/E5b_HS or computed elevation. + // E1B_HS gates the whole satellite: 1B initiates the search and + // 5X is only searched once 1B is tracked. if (El > elevation_mask_deg && it.second.E1B_HS == 0) { - std::cout << "Using Galileo Ephemeris: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using Galileo Ephemeris: Sat " << it.second.PRN << " Az: " << Az << " El: " << El; available_satellites.emplace_back(floor(El), (Gnss_Satellite(std::string("Galileo"), it.second.PRN))); } else if (below_mask_out != nullptr) { - std::cout << "Using Galileo Ephemeris (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using Galileo Ephemeris (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El; below_mask_out->emplace_back(floor(El), Gnss_Satellite(std::string("Galileo"), it.second.PRN)); } } @@ -201,13 +205,13 @@ std::vector> compute_visible_satellites( { continue; } - const double age = DataAgeS(now_tow, it.second.toe); + const eph_t rtklib_eph = eph_to_rtklib(it.second); + const double age = timediff(gps_gtime, rtklib_eph.toe); if (std::abs(age) > MAXDTOE_BDS) { continue; // stale -- treat as if no ephemeris exists; almanac may still classify it } note_freshness(age, MAXDTOE_BDS); - const eph_t rtklib_eph = eph_to_rtklib(it.second); std::array r_sat{}; double clock_bias_s; double sat_pos_variance_m2; @@ -222,21 +226,102 @@ std::vector> compute_visible_satellites( handled_bds.push_back(it.second.PRN); if (El > elevation_mask_deg && it.second.SV_health == 0) { - std::cout << "Using BeiDou Ephemeris: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using BeiDou Ephemeris: Sat " << it.second.PRN << " Az: " << Az << " El: " << El; available_satellites.emplace_back(floor(El), (Gnss_Satellite(std::string("Beidou"), it.second.PRN))); } else if (below_mask_out != nullptr) { - std::cout << "Using BeiDou Ephemeris (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using BeiDou Ephemeris (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El; below_mask_out->emplace_back(floor(El), Gnss_Satellite(std::string("Beidou"), it.second.PRN)); } } + const auto glonass_utc = pvt_ptr->get_glonass_utc_model(); + std::set handled_glonass; + for (const auto& it : pvt_ptr->get_glonass_ephemeris()) + { + const auto& ephemeris = it.second; + if (only_prns != nullptr && only_prns->count(std::make_pair(std::string("Glonass"), ephemeris.PRN)) == 0) + { + continue; + } + if (ephemeris.d_N_T < 1.0 || ephemeris.d_N_T > 1461.0 || ephemeris.d_yr < 1996.0) + { + continue; + } + const auto geph = eph_to_rtklib(ephemeris, glonass_utc, glonass_strict_health); + const double age = timediff(gps_gtime, geph.toe); + if (std::abs(age) > MAXDTOE_GLO) + { + continue; + } + std::array position{}; + double clock_bias; + double variance; + geph2pos(gps_gtime, &geph, position.data(), &clock_bias, &variance); + if (!std::isfinite(position[0]) || !std::isfinite(position[1]) || !std::isfinite(position[2]) || + std::hypot(std::hypot(position[0], position[1]), position[2]) < 6378136.0) + { + continue; + } + note_freshness(age, MAXDTOE_GLO); + handled_glonass.insert(ephemeris.PRN); + double azimuth; + double elevation; + double distance; + topocent(&azimuth, &elevation, &distance, r_eb_e, arma::vec{position[0], position[1], position[2]} - r_eb_e); + const auto entry = std::make_pair(static_cast(std::floor(elevation)), Gnss_Satellite("Glonass", ephemeris.PRN)); + if (elevation > elevation_mask_deg && geph.svh == 0) + { + available_satellites.push_back(entry); + } + else if (below_mask_out != nullptr) + { + below_mask_out->push_back(entry); + } + } + for (const auto& it : pvt_ptr->get_glonass_almanac()) + { + const auto& almanac = it.second; + if (handled_glonass.count(almanac.PRN) != 0 || + (only_prns != nullptr && only_prns->count(std::make_pair(std::string("Glonass"), almanac.PRN)) == 0)) + { + continue; + } + const auto epoch = glonass_almanac_epoch(almanac); + const double age = timediff(gps_gtime, epoch); + if (epoch.time == 0 || std::abs(age) > almanac_max_age_s) + { + continue; + } + std::array position{}; + if (!almanac.satellite_position(timediff(gpst2utc(gps_gtime), gpst2utc(epoch)), position)) + { + continue; + } + note_freshness(age, almanac_max_age_s); + double azimuth; + double elevation; + double distance; + topocent(&azimuth, &elevation, &distance, r_eb_e, arma::vec{position[0], position[1], position[2]} - r_eb_e); + const auto entry = std::make_pair(static_cast(std::floor(elevation)), Gnss_Satellite("Glonass", almanac.PRN)); + // ICD Table 5.1: Cn=1 is operational; ln=1 indicates malfunction. + if (elevation > elevation_mask_deg && almanac.d_C_n && !almanac.d_l_n) + { + available_satellites.push_back(entry); + } + else if (below_mask_out != nullptr) + { + below_mask_out->push_back(entry); + } + } + const std::map gps_alm_map = pvt_ptr->get_gps_almanac(); for (const auto& it : gps_alm_map) { - if (only_prns != nullptr && only_prns->count(std::make_pair(std::string("GPS"), it.second.PRN)) == 0) + const std::string system = (it.second.PRN >= MINPRNQZS && it.second.PRN <= MAXPRNQZS) ? "QZSS" : "GPS"; + if (only_prns != nullptr && only_prns->count(std::make_pair(system, it.second.PRN)) == 0) { continue; } @@ -244,28 +329,17 @@ std::vector> compute_visible_satellites( { continue; // ephemeris already classified this PRN } - const double age = DataAgeS(now_tow, it.second.toa); + const alm_t rtklib_alm = alm_to_rtklib(it.second, ref_gps_week); + const double age = timediff(gps_gtime, rtklib_alm.toa); if (std::abs(age) > almanac_max_age_s) { continue; // stale -- treat as if no almanac exists for this PRN either } note_freshness(age, almanac_max_age_s); - const alm_t rtklib_alm = alm_to_rtklib(it.second, ref_gps_week); std::array r_sat{}; double clock_bias_s; - // gps_gtime is a proper absolute epoch (see its own construction - // above), matching what alm_to_rtklib() now builds rtklib_alm.toa - // as (gpst2time(week, toa) -- also absolute, since the WN fix). - // This used to instead build a throwaway gtime_t truncated down - // to a bare seconds-of-week value via fmod(..., 604800), which - // was only self-consistent with alm_to_rtklib()'s pre-fix - // toa.time = raw_toa (also un-resolved/small) -- once toa became - // a real absolute epoch, subtracting it from that truncated - // query time in alm2pos() produced a ~56-year-scale tk, the same - // class of error the WN fix was supposed to eliminate, just - // relocated to the other operand. BeiDou's almanac loop below - // never had this truncation and has always passed gps_gtime - // directly -- matching that here. + // gps_gtime and rtklib_alm.toa are both absolute epochs; a + // seconds-of-week query time here would corrupt tk in alm2pos(). alm2pos(gps_gtime, &rtklib_alm, r_sat.data(), &clock_bias_s); double Az; double El; @@ -275,14 +349,14 @@ std::vector> compute_visible_satellites( topocent(&Az, &El, &dist_m, r_eb_e, dx); if (El > elevation_mask_deg && it.second.SV_health == 0) { - std::cout << "Using GPS Almanac: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using " << system << " Almanac: Sat " << it.second.PRN << " Az: " << Az << " El: " << El; available_satellites.emplace_back(floor(El), - (Gnss_Satellite(std::string("GPS"), it.second.PRN))); + (Gnss_Satellite(system, it.second.PRN))); } else if (below_mask_out != nullptr) { - std::cout << "Using GPS Almanac (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; - below_mask_out->emplace_back(floor(El), Gnss_Satellite(std::string("GPS"), it.second.PRN)); + LOG(INFO) << "Using " << system << " Almanac (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El; + below_mask_out->emplace_back(floor(El), Gnss_Satellite(system, it.second.PRN)); } } @@ -297,17 +371,15 @@ std::vector> compute_visible_satellites( { continue; // ephemeris already classified this PRN } - const double age = DataAgeS(now_tow, it.second.toa); + const alm_t rtklib_alm = alm_to_rtklib(it.second, ref_gps_week); + const double age = timediff(gps_gtime, rtklib_alm.toa); if (std::abs(age) > almanac_max_age_s) { continue; // stale -- treat as if no almanac exists for this PRN either } note_freshness(age, almanac_max_age_s); - const alm_t rtklib_alm = alm_to_rtklib(it.second, ref_gps_week); std::array r_sat{}; double clock_bias_s; - // See the matching comment in the GPS almanac loop above -- same - // fix, same reason. alm2pos(gps_gtime, &rtklib_alm, r_sat.data(), &clock_bias_s); double Az; double El; @@ -315,17 +387,9 @@ std::vector> compute_visible_satellites( const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]}; const arma::vec dx = r_sat_eb_e - r_eb_e; topocent(&Az, &El, &dist_m, r_eb_e, dx); - // A handful of single-tick, - // self-correcting extreme-elevation readings have been observed - // (e.g. E11 at -89.8 deg for exactly one Tick(), -16/-15 deg - // immediately before and after) with no almanac decode/overwrite - // for that PRN anywhere near the event, and every other - // satellite in the same Tick() (same gps_gtime, same r_eb_e) - // computing a normal elevation. That rules out a shared - // time/position corruption and points at either the stored - // record for just this PRN or the alm2pos() Kepler solve itself. - // Dump every field alm2pos() actually consumes so the next - // occurrence is caught with enough data to tell those apart. + // Sporadic single-tick extreme elevations have been seen from a + // Galileo almanac while every other satellite in the same tick + // was normal. Dump every alm2pos() input to diagnose the next one. if (std::abs(El) > 85.0) { LOG(WARNING) << "[visibility] extreme-El diag: Galileo Almanac Sat " << it.second.PRN @@ -341,13 +405,13 @@ std::vector> compute_visible_satellites( } if (El > elevation_mask_deg && it.second.E1B_HS == 0) { - std::cout << "Using Galileo Almanac: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using Galileo Almanac: Sat " << it.second.PRN << " Az: " << Az << " El: " << El; available_satellites.emplace_back(floor(El), (Gnss_Satellite(std::string("Galileo"), it.second.PRN))); } else if (below_mask_out != nullptr) { - std::cout << "Using Galileo Almanac (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using Galileo Almanac (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El; below_mask_out->emplace_back(floor(El), Gnss_Satellite(std::string("Galileo"), it.second.PRN)); } } @@ -363,13 +427,13 @@ std::vector> compute_visible_satellites( { continue; // ephemeris already classified this PRN } - const double age = DataAgeS(now_tow, it.second.toa); + const alm_t rtklib_alm = alm_to_rtklib(it.second); + const double age = timediff(gps_gtime, rtklib_alm.toa); if (std::abs(age) > almanac_max_age_s) { continue; // stale -- treat as if no almanac exists for this PRN either } note_freshness(age, almanac_max_age_s); - const alm_t rtklib_alm = alm_to_rtklib(it.second); std::array r_sat{}; double clock_bias_s; alm2pos(gps_gtime, &rtklib_alm, r_sat.data(), &clock_bias_s); @@ -381,13 +445,13 @@ std::vector> compute_visible_satellites( topocent(&Az, &El, &dist_m, r_eb_e, dx); if (El > elevation_mask_deg && it.second.SV_health == 0) { - std::cout << "Using BeiDou Almanac: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using BeiDou Almanac: Sat " << it.second.PRN << " Az: " << Az << " El: " << El; available_satellites.emplace_back(floor(El), (Gnss_Satellite(std::string("Beidou"), it.second.PRN))); } else if (below_mask_out != nullptr) { - std::cout << "Using BeiDou Almanac (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n'; + LOG(INFO) << "Using BeiDou Almanac (excluded): Sat " << it.second.PRN << " Az: " << Az << " El: " << El; below_mask_out->emplace_back(floor(El), Gnss_Satellite(std::string("Beidou"), it.second.PRN)); } } @@ -402,12 +466,11 @@ std::vector> compute_visible_satellites( namespace { -// Keeps (search_ratio_ + 1) comfortably clear of uint32_t overflow in -// GNSSFlowgraph::pop_by_visibility()'s counter modulo, no matter what a -// misconfigured .conf sets. +// Keeps (search_ratio_ + 1) clear of uint32_t overflow in +// GNSSFlowgraph::pop_by_visibility()'s counter modulo. constexpr uint32_t kMaxSearchRatio = 1000000U; -// See ticks_since_data_check_'s doc comment. +// Tick() calls between DataChanged() runs, which copy every ephemeris/almanac map. constexpr int kDataCheckEveryNTicks = 20; } // namespace @@ -415,19 +478,14 @@ constexpr int kDataCheckEveryNTicks = 20; SatelliteVisibility::SatelliteVisibility(const std::shared_ptr& configuration) : enabled_(configuration->property("GNSS-SDR.enable_visibility_aware_search", false)), search_ratio_(std::min(configuration->property("GNSS-SDR.visible_vs_mayvisible_search_ratio", 3U), kMaxSearchRatio)), - // 120 s (not the old 5 s): satellites move negligibly over a couple of - // minutes, and the tighter reactivity that actually matters now comes - // from the event-driven triggers below (data arrival/update, receiver - // movement, data expiry), not from polling this interval hard. + // Satellites move negligibly over minutes; fast reaction comes from the + // event-driven triggers (data arrival, movement, expiry), not polling. recompute_interval_s_(configuration->property("GNSS-SDR.visibility_recompute_interval_s", 120.0)), elevation_mask_deg_(configuration->property("GNSS-SDR.search_elevation_mask", 0.0)), position_threshold_m_(configuration->property("GNSS-SDR.visibility_recompute_position_threshold_m", 1000.0)), - // Half-window around toa -- see compute_visible_satellites()'s - // matching parameter. Must stay well under 302400 s (half a week) for - // the toa-only staleness math to remain unambiguous. Default: 3 days. - // (Ephemeris staleness has no equivalent config parameter -- see - // almanac_max_age_s_'s doc comment.) + // Maximum absolute age of the resolved almanac epoch. Default: 3 days. almanac_max_age_s_(configuration->property("GNSS-SDR.visibility_almanac_max_age_s", 259200.0)), + glonass_strict_health_(configuration->property("PVT.glonass_strict_health", true)), have_agnss_reference_(false), agnss_ref_lat_deg_(0.0), agnss_ref_lon_deg_(0.0), @@ -439,10 +497,8 @@ SatelliteVisibility::SatelliteVisibility(const std::shared_ptr::infinity()), ticks_since_data_check_(kDataCheckEveryNTicks) { - // Shared with ControlThread::init() (see agnss_ref_location.h/ - // agnss_ref_time.h) so this parsing -- including the AGNSS_ref_utc_time - // empty-string-means-wall-clock-time fallback -- lives in exactly one - // place rather than being duplicated per caller. + // Parsing shared with ControlThread::init(); an empty AGNSS_ref_utc_time + // means wall-clock time. const Agnss_Ref_Location parsed_location = parse_agnss_ref_location( configuration->property("GNSS-SDR.AGNSS_ref_location", std::string(""))); have_agnss_reference_ = parsed_location.valid; @@ -451,14 +507,11 @@ SatelliteVisibility::SatelliteVisibility(const std::shared_ptrproperty("GNSS-SDR.AGNSS_ref_utc_time", std::string(""))); - // parsed_time.valid is only false for a non-empty, malformed string -- - // fall back to wall-clock time there too rather than an unusable zero. + // A non-empty malformed string also falls back to wall-clock time. agnss_ref_utc_time_ = parsed_time.valid ? static_cast(parsed_time.seconds) : std::time(nullptr); - // Same GNSS-SDR._banned_prns parsing as - // GNSSFlowgraph::set_signals_list(), for every system - // compute_visible_satellites() covers. - for (const std::string system : {"GPS", "Galileo", "Beidou"}) + // Same GNSS-SDR._banned_prns parsing as GNSSFlowgraph::set_signals_list(). + for (const std::string system : {"GPS", "Galileo", "Beidou", "Glonass", "QZSS"}) { const auto sv_banned = configuration->property("GNSS-SDR." + system + "_banned_prns", std::string("")); if (sv_banned.empty()) @@ -476,23 +529,22 @@ SatelliteVisibility::SatelliteVisibility(const std::shared_ptr> system_signals{ {"GPS", {"1C", "2S", "L5"}}, {"Galileo", {"1B", "5X", "E6", "7X"}}, - {"Beidou", {"B1", "1D", "B3"}}}; + {"Beidou", {"B1", "1D", "B3"}}, + {"Glonass", {"1G", "2G"}}, + {"QZSS", {"J1", "J5"}}}; for (const auto& entry : system_signals) { for (const auto& signal : entry.second) @@ -510,37 +562,53 @@ SatelliteVisibility::SatelliteVisibility(const std::shared_ptr& pvt_ptr, std::set>* changed_prns_out) { - // Per-(system, type, PRN) fingerprint keyed on toe/toa -- finer than a - // map-size count: catches an existing PRN's data being *replaced* (new - // toe/toa broadcast, same PRN, same map size), not just brand new PRNs - // appearing. See last_data_fingerprints_'s doc comment. - std::map, int32_t> current; - auto add = [¤t](const char* system, const char* type, uint32_t prn, int32_t ref_time) { - current.emplace(std::make_tuple(std::string(system), std::string(type), prn), ref_time); + // Fingerprint on (toe/toa, health) so a replaced entry counts as a + // change, not only a new PRN. + std::map, std::pair> current; + auto add = [¤t](const char* system, const char* type, uint32_t prn, double ref_time, int32_t health) { + current.emplace(std::make_tuple(std::string(system), std::string(type), prn), std::make_pair(ref_time, health)); }; for (const auto& it : pvt_ptr->get_gps_ephemeris()) { - add("GPS", "EPH", it.second.PRN, it.second.toe); + add(it.second.PRN >= MINPRNQZS && it.second.PRN <= MAXPRNQZS ? "QZSS" : "GPS", "EPH", it.second.PRN, it.second.WN * 604800.0 + it.second.toe, it.second.SV_health); } for (const auto& it : pvt_ptr->get_galileo_ephemeris()) { - add("Galileo", "EPH", it.second.PRN, it.second.toe); + add("Galileo", "EPH", it.second.PRN, it.second.WN * 604800.0 + it.second.toe, it.second.E1B_HS); } for (const auto& it : pvt_ptr->get_beidou_dnav_ephemeris()) { - add("Beidou", "EPH", it.second.PRN, it.second.toe); + add("Beidou", "EPH", it.second.PRN, it.second.WN * 604800.0 + it.second.toe, it.second.SV_health); } for (const auto& it : pvt_ptr->get_gps_almanac()) { - add("GPS", "ALM", it.second.PRN, it.second.toa); + add(it.second.PRN >= MINPRNQZS && it.second.PRN <= MAXPRNQZS ? "QZSS" : "GPS", "ALM", it.second.PRN, it.second.WNa * 604800.0 + it.second.toa, it.second.SV_health); } for (const auto& it : pvt_ptr->get_galileo_almanac()) { - add("Galileo", "ALM", it.second.PRN, it.second.toa); + add("Galileo", "ALM", it.second.PRN, it.second.WNa * 604800.0 + it.second.toa, it.second.E1B_HS); } for (const auto& it : pvt_ptr->get_beidou_dnav_almanac()) { - add("Beidou", "ALM", it.second.PRN, it.second.toa); + add("Beidou", "ALM", it.second.PRN, it.second.WNa * 604800.0 + it.second.toa, it.second.SV_health); + } + + for (const auto& it : pvt_ptr->get_gps_cnav_ephemeris()) + { + if (it.second.PRN >= MINPRNQZS && it.second.PRN <= MAXPRNQZS) + { + add("QZSS", "CNAV", it.second.PRN, it.second.WN * 604800.0 + it.second.toe1, it.second.signal_health); + } + } + for (const auto& it : pvt_ptr->get_glonass_ephemeris()) + { + add("Glonass", "EPH", it.second.PRN, (it.second.d_yr * 1461.0 + it.second.d_N_T) * 86400.0 + it.second.d_t_b, + (static_cast(it.second.d_B_n) & 4) | static_cast(it.second.d_l3rd_n)); + } + for (const auto& it : pvt_ptr->get_glonass_almanac()) + { + add("Glonass", "ALM", it.second.PRN, (it.second.d_N_4 * 1461.0 + it.second.d_N_A) * 86400.0 + it.second.d_t_lambda_n_A, + (it.second.d_C_n ? 0 : 2) | static_cast(it.second.d_l_n)); } const bool changed = first_data_check_ || (current != last_data_fingerprints_); @@ -574,11 +642,8 @@ bool SatelliteVisibility::DataChanged(const std::shared_ptr& pvt_p if (current.find(entry.first) == current.end()) { ++removed; - // A PRN whose data disappeared entirely has nothing - // left to recompute Az/El from, but Tick()'s merge - // still needs to know about it so it can drop back - // to "maybe visible" if it was previously classified - // -- see the merge logic in Tick(). + // A removed PRN has nothing to recompute from, but + // Tick() must still drop it from visible_/excluded_. if (changed_prns_out != nullptr) { changed_prns_out->emplace(std::get<0>(entry.first), std::get<2>(entry.first)); @@ -620,13 +685,8 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con LLH[1] = static_cast(agnss_ref_lon_deg_); LLH[2] = 0.0F; gtime_t utc_gtime{}; - // agnss_ref_utc_time_ prefers the configured AGNSS_ref_utc_time - // over the host's wall-clock time -- critical for offline/replay - // runs, where the host clock has nothing to do with the GNSS - // time actually embedded in the samples being processed (using - // wall-clock time there silently computes elevation for the - // wrong epoch/orbital configuration entirely). See its - // construction above. + // Configured AGNSS_ref_utc_time (wall clock if unset): in replay + // runs the host clock is unrelated to the samples' GNSS time. utc_gtime.time = agnss_ref_utc_time_; utc_gtime.sec = 0.0; gps_gtime = utc2gpst(utc_gtime); @@ -642,9 +702,8 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con arma::vec v_eb_e = arma::zeros(3, 1); Geo_to_ECEF(LLH_rad, arma::vec{0, 0, 0}, C_tmp, r_eb_e, v_eb_e, C_tmp); - // Throttle DataChanged() itself (see ticks_since_data_check_'s doc - // comment) -- it's only worth paying its full-map-copy cost once every - // few Tick() calls, not on every idle tick. + // DataChanged() copies every ephemeris/almanac map; pay that only every + // few ticks. bool data_changed = false; std::set> changed_prns; if (++ticks_since_data_check_ >= kDataCheckEveryNTicks) @@ -653,48 +712,32 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con data_changed = DataChanged(pvt_ptr, &changed_prns); } - // Recompute cadence is governed by receiver time (fix_status.RX_time), - // not wall-clock time -- see last_recompute_rx_time_s_'s doc comment. - // Not meaningful before the first fix (RX_time isn't ticking yet); - // fix_became_valid/data_changed cover that phase instead. - const bool interval_elapsed = fix_valid && ((fix_status.RX_time - last_recompute_rx_time_s_) >= recompute_interval_s_); + // Include the GPS week so cadence and expiry survive the TOW rollover. + const double rx_time_s = static_cast(gps_gtime.time) + gps_gtime.sec; + const bool interval_elapsed = fix_valid && ((rx_time_s - last_recompute_rx_time_s_) >= recompute_interval_s_); - // See position_threshold_m_'s doc comment. const bool moved_significantly = arma::norm(r_eb_e - last_recompute_r_eb_e_, 2) >= position_threshold_m_; - // See next_expiry_deadline_rx_time_'s doc comment. - const bool expired = fix_valid && (fix_status.RX_time >= next_expiry_deadline_rx_time_); + const bool expired = fix_valid && (rx_time_s >= next_expiry_deadline_rx_time_); if (!fix_became_valid && !data_changed && !interval_elapsed && !moved_significantly && !expired) { return false; } - // fix_became_valid/interval_elapsed/moved_significantly/expired can all - // affect *every* satellite's elevation (receiver position or time moved, - // or cached data anywhere went stale) -- those need a full sweep. Only - // data_changed firing alone means exactly one thing changed (one PRN's - // ephemeris/almanac), so scoping the recompute to just that PRN (via - // only_prns below) is safe and correct, and avoids redoing eph2pos/ - // topocent for the whole constellation on every single ephemeris page - // that happens to arrive during startup. + // Position/time/expiry triggers can change every satellite's elevation, + // so they need a full sweep. data_changed alone only affects the PRNs in + // changed_prns, so the recompute is scoped to them via only_prns. const bool needs_full_recompute = fix_became_valid || interval_elapsed || moved_significantly || expired; - // Both of these gate a *full-sweep* trigger (interval_elapsed, - // moved_significantly) against drift since the last full sweep -- they - // must only advance when a full sweep actually just ran. Updating them - // on every qualifying Tick(), including a data_changed-only (targeted) - // one, would keep resetting both clocks before the interval/displacement - // threshold is ever reached whenever ephemeris/almanac data is arriving - // often enough (routine with a dozen+ tracked satellites) -- silently - // starving the periodic/position-triggered full recompute forever, even - // though a satellite could be rising above the horizon the whole time - // with no ephemeris/almanac change of its own to trigger a targeted one. + // Advance the full-sweep baselines only after a full sweep: resetting + // them on targeted recomputes would starve the interval/displacement + // triggers whenever data arrives faster than either threshold. if (needs_full_recompute) { if (fix_valid) { - last_recompute_rx_time_s_ = fix_status.RX_time; + last_recompute_rx_time_s_ = rx_time_s; } last_recompute_r_eb_e_ = r_eb_e; } @@ -704,84 +747,19 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con const auto elevations = compute_visible_satellites(pvt_ptr, gps_gtime, r_eb_e, elevation_mask_deg_, &below_mask, almanac_max_age_s_, needs_full_recompute ? &seconds_until_next_expiry : nullptr, - needs_full_recompute ? nullptr : &changed_prns); - // Only a full sweep sees every currently-classified satellite's - // freshness, so only a full sweep can correctly narrow this deadline -- - // updating it from a targeted sweep's much smaller view could push it - // later than the true soonest expiry among satellites not touched this - // tick, silently delaying a real expired-data recompute. + needs_full_recompute ? nullptr : &changed_prns, glonass_strict_health_); + // Only a full sweep sees every classified satellite's freshness; a + // targeted sweep could push the deadline past the true soonest expiry. if (needs_full_recompute) { next_expiry_deadline_rx_time_ = (fix_valid && std::isfinite(seconds_until_next_expiry)) - ? fix_status.RX_time + seconds_until_next_expiry + ? rx_time_s + seconds_until_next_expiry : std::numeric_limits::infinity(); } - // Classify every freshly computed elevation (visible or excluded -- - // both mean elevation was computable) using the raw reading itself, - // always -- decoding correctness is enforced at its actual sources - // (week-number resolution in alm_to_rtklib(), almanac freshness/ - // overwrite priority in update_almanac_if_fresher(), the ephemeris/ - // almanac map mutex), not by second-guessing a live reading here. - // reference_elevation_deg_ is updated with every raw reading as it's - // accepted -- see its own doc comment for what it's actually for. - std::vector> accepted_entries; - accepted_entries.reserve(elevations.size() + below_mask.size()); - // Health-check failures from below_mask -- see the loop below. Always - // excluded, unconditionally, regardless of elevation. - std::vector> forced_excluded_entries; - { - auto record_elevation = [&](const std::pair& entry) { - const auto key = std::make_pair(entry.second.get_system(), entry.second.get_PRN()); - const int raw = entry.first; - reference_elevation_deg_[key] = raw; - accepted_entries.emplace_back(raw, entry.second); - }; - for (const auto& entry : elevations) - { - record_elevation(entry); - } - for (const auto& entry : below_mask) - { - reference_elevation_deg_[std::make_pair(entry.second.get_system(), entry.second.get_PRN())] = entry.first; - // A below_mask entry with a raw elevation *above* the mask - // can only mean one thing: it failed the broadcast health - // check in compute_visible_satellites(), not the elevation - // check (that path's condition is `El > mask && healthy`, - // so anything landing here with El > mask must have failed - // on health). This is a real, unconditional classification - // rule (a health flag is definitive, not a heuristic) -- - // route straight to forced-excluded instead of the - // elevation-mask comparison the loop below applies to - // accepted_entries. - // - // entry.first is floor(El) (compute_visible_satellites() - // truncates to int for the diagnostic dump), so this must be - // `>=`, not `>`: a satellite that failed *only* on health at - // a raw elevation in [mask, mask+1) -- e.g. El=15.63 against - // a 15 deg mask -- floors to exactly `mask`, which `>` would - // wrongly treat as an elevation failure instead (verified - // 2026-09-10 against a live G11 El=15.63 misclassified - // excluded). `>=` is exact for every fractional El except El - // landing on the integer mask value to the bit -- a - // measure-zero case for a continuously-varying real angle. - if (static_cast(entry.first) >= elevation_mask_deg_) - { - forced_excluded_entries.push_back(entry); - continue; - } - accepted_entries.emplace_back(entry.first, entry.second); - } - } - - // A full recompute starts classification from scratch, exactly as - // before. A targeted recompute starts from the *current* membership - // instead, with only the recomputed PRNs (changed_prns) first erased - // from both sets -- they may no longer belong in either (e.g. their - // data was removed entirely and compute_visible_satellites() has - // nothing to classify them with this tick) -- so every other - // satellite's classification is left completely untouched, and the loop - // below only re-adds the PRNs that were actually recomputed. + // Full recompute: classify from scratch. Targeted: keep the current + // membership, erase only changed_prns (their data may be gone) and + // re-add whatever was recomputed. std::set> new_visible = needs_full_recompute ? std::set>{} : visible_; std::set> new_excluded = needs_full_recompute ? std::set>{} : excluded_; if (!needs_full_recompute) @@ -792,51 +770,31 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con new_excluded.erase(key); } } - for (const auto& entry : accepted_entries) + // The computation already applied the mask and health check using the + // full-precision elevation. Floored elevations are only for diagnostics. + for (const auto& entry : elevations) { const auto key = std::make_pair(entry.second.get_system(), entry.second.get_PRN()); - // `>=`, not `>` -- see the matching comment on the below_mask - // loop above; entry.first is floor(El), so this must compensate - // for the same truncation-toward-mask bias or a satellite whose - // true elevation is in [mask, mask+1) gets wrongly excluded here - // despite compute_visible_satellites() having already correctly - // accepted it via the untruncated double. - if (static_cast(entry.first) >= elevation_mask_deg_) + reference_elevation_deg_[key] = entry.first; + if (banned_.count(key) == 0) { - if (banned_.count(key) > 0) - { - continue; - } new_visible.emplace(key); } - else - { - new_excluded.emplace(key); - } } - for (const auto& entry : forced_excluded_entries) + for (const auto& entry : below_mask) { - new_excluded.emplace(entry.second.get_system(), entry.second.get_PRN()); + const auto key = std::make_pair(entry.second.get_system(), entry.second.get_PRN()); + reference_elevation_deg_[key] = entry.first; + new_excluded.emplace(key); } const bool changed = (new_visible != visible_) || (new_excluded != excluded_); visible_ = std::move(new_visible); excluded_ = std::move(new_excluded); - // Always-on diagnostic (LOG, not DLOG -- DLOG is compiled out entirely in - // release/NDEBUG builds, so it would be invisible in exactly the builds - // this is most useful for). Reports what SatelliteVisibility currently - // believes is visible and excluded (elevation known, at/below the mask), - // WITH the computed elevation for each, plus the maybe-visible set (no - // ephemeris/almanac yet -- genuinely unknown, still searched at lower - // priority) by PRN only, since it has no elevation to show. Compact, - // sorted-by-PRN groups (E03(+31), not "Galileo 3 (31 deg)") read far - // faster at a glance than the old prose form -- this is what a manually - // reconstructed comparison across recompute snapshots looked like - // before this was built directly into the log line. - // See GNSSFlowgraph::pop_by_visibility() for what actually gets - // *searched* from each bucket, which is the more direct signal for - // diagnosing CPU load. + // Always-on diagnostic (LOG, not DLOG, which is compiled out in release + // builds): VISIBLE/EXCLUDED with floored elevation, MAYBE VISIBLE by PRN + // only. GNSSFlowgraph::pop_by_visibility() logs what is actually searched. { auto short_id = [](const Gnss_Satellite& sat) { std::ostringstream os; @@ -844,7 +802,7 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con return os.str(); }; auto sort_by_prn = [](std::vector> entries) { - std::sort(entries.begin(), entries.end(), [](const auto& a, const auto& b) { + std::sort(entries.begin(), entries.end(), [](const std::pair& a, const std::pair& b) { return a.second.get_PRN() < b.second.get_PRN(); }); return entries; @@ -861,13 +819,9 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con first = false; } }; - // Built from the final, post-merge visible_/excluded_ rather than - // accumulated during the classify loop above, so a targeted - // recompute's report is just as complete as a full one: every - // entry's elevation comes from reference_elevation_deg_, which was - // just refreshed above for whichever PRN(s) were actually - // recomputed this tick and still holds the last known value for - // everyone else, untouched. + // Built from the post-merge sets so a targeted recompute reports every + // satellite; reference_elevation_deg_ keeps the last value of PRNs not + // recomputed this tick. auto elevation_for = [&](const std::pair& key) -> int { const auto it = reference_elevation_deg_.find(key); return it != reference_elevation_deg_.end() ? static_cast(it->second) : 0; @@ -884,26 +838,15 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con { excluded_entries.emplace_back(elevation_for(key), Gnss_Satellite(key.first, key.second)); } - // Maybe-visible has no elevation to report -- listing by PRN only - // needs the full non-banned PRN range of every *configured* system - // compute_visible_satellites() covers (same {"GPS", "Galileo", - // "Beidou"} list as the banned_prns parsing above), not just - // whatever happened to be in visible_/excluded_ this tick. This - // used to be hardcoded to Galileo's own range (1-36) only, from - // when the feature was Galileo-only -- silently dropping every - // other system's not-yet-classified satellites from this specific - // report line once multi-constellation support was added, even - // though visible_/excluded_ themselves were already tracking them - // correctly (that's why GPS satellites show up fine in VISIBLE/ - // EXCLUDED once classified -- only this maybe-visible enumeration - // was ever GPS-blind). Scoped to configured_systems_ (systems with - // at least one channel actually configured) so an unconfigured - // system's whole PRN range doesn't sit here as permanent noise -- - // nothing will ever decode ephemeris/almanac for it in this run. + // Maybe-visible = full PRN range of each configured system minus + // banned/visible/excluded. Scoped to configured_systems_ so an + // unconfigured system's range is not listed as permanent noise. std::vector maybe_visible; for (const auto& system_range : {std::make_tuple(std::string("GPS"), static_cast(MINPRNGPS), static_cast(MAXPRNGPS)), std::make_tuple(std::string("Galileo"), static_cast(MINPRNGAL), static_cast(MAXPRNGAL)), - std::make_tuple(std::string("Beidou"), static_cast(MINPRNBDS), static_cast(MAXPRNBDS))}) + std::make_tuple(std::string("Beidou"), static_cast(MINPRNBDS), static_cast(MAXPRNBDS)), + std::make_tuple(std::string("Glonass"), static_cast(MINPRNGLO), static_cast(MAXPRNGLO)), + std::make_tuple(std::string("QZSS"), static_cast(MINPRNQZS), static_cast(QZSS_L5_MAX_PRN))}) { const auto& system = std::get<0>(system_range); if (configured_systems_.count(system) == 0) @@ -920,7 +863,7 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con maybe_visible.emplace_back(system, prn); } } - std::sort(maybe_visible.begin(), maybe_visible.end(), [](const auto& a, const auto& b) { return a.get_PRN() < b.get_PRN(); }); + std::sort(maybe_visible.begin(), maybe_visible.end(), [](const Gnss_Satellite& a, const Gnss_Satellite& b) { return a.get_PRN() < b.get_PRN(); }); std::ostringstream oss; oss << "[visibility] recompute (triggered by:" @@ -949,7 +892,6 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con first = false; } LOG(INFO) << oss.str(); - std::cout << oss.str() << '\n'; } return changed; @@ -958,11 +900,11 @@ bool SatelliteVisibility::Tick(const std::shared_ptr& pvt_ptr, con bool SatelliteVisibility::IsVisible(const Gnss_Satellite& sat) const { - return visible_.find(std::make_pair(sat.get_system(), sat.get_PRN())) != visible_.end(); + return visible_.find(std::make_pair(sat.get_system(), sat.get_system() == "QZSS" ? qzss_l1cb_prn_to_nominal_prn(sat.get_PRN()) : sat.get_PRN())) != visible_.end(); } bool SatelliteVisibility::IsExcluded(const Gnss_Satellite& sat) const { - return excluded_.find(std::make_pair(sat.get_system(), sat.get_PRN())) != excluded_.end(); + return excluded_.find(std::make_pair(sat.get_system(), sat.get_system() == "QZSS" ? qzss_l1cb_prn_to_nominal_prn(sat.get_PRN()) : sat.get_PRN())) != excluded_.end(); } diff --git a/src/core/receiver/satellite_visibility.h b/src/core/receiver/satellite_visibility.h index 0ccf5e623..83c950415 100644 --- a/src/core/receiver/satellite_visibility.h +++ b/src/core/receiver/satellite_visibility.h @@ -41,72 +41,39 @@ class PvtInterface; * \{ */ /*! - * \brief Computes elevation (deg, floored) for every satellite with usable - * GPS/Galileo/BeiDou ephemeris or almanac data in pvt_ptr, at the given GPS - * time and ECEF receiver position, keeping only those at or above - * elevation_mask_deg AND broadcasting a healthy status (SV_health == 0 for - * GPS/BeiDou; Galileo's E1B_HS == 0, since 1B is the search-initiating - * signal and 5X only ever gets searched once 1B is tracked). A satellite - * marked unhealthy is known unusable regardless of its geometric elevation - * -- that broadcast declaration is stronger evidence than geometry alone, - * so it's treated the same as an elevation at/below the mask (below_mask_out - * below), not folded into "no data" -- otherwise a satellite whose almanac - * genuinely places it above the mask but marks it unhealthy (an old/retired - * satellite still broadcasting plausible-looking orbital elements is the - * common case) would be classified visible and searched forever despite - * never being acquirable. Sorted descending by elevation. + * \brief Computes the elevation of every satellite with usable ephemeris or + * almanac data in pvt_ptr (GPS, Galileo, BeiDou, GLONASS, QZSS) and returns + * those strictly above elevation_mask_deg that broadcast a healthy status. + * Ephemeris is preferred over almanac, so each satellite is classified by + * exactly one data source. An unhealthy satellite (SV_health != 0; Galileo + * E1B_HS != 0, since 1B initiates the search) is reported as below-mask, not + * as "no data": it is known unusable whatever its geometry. * - * This is the elevation-computation core factored out of - * ControlThread::get_visible_sats() so it can be reused by - * SatelliteVisibility's runtime recompute below without duplicating the - * rtklib eph2pos/alm2pos/topocent logic. + * Shared elevation core of ControlThread::get_visible_sats() and + * SatelliteVisibility::Tick(). * - * \param elevation_mask_deg minimum elevation (deg) to be considered - * visible -- see GNSS-SDR.search_elevation_mask. Distinct from (and not - * necessarily equal to) PVT.elevation_mask, which gates the navigation - * solution rather than the acquisition search order. - * \param below_mask_out when non-null, appended with every satellite whose - * elevation *could* be computed (has usable ephemeris/almanac data) but - * came out at or below elevation_mask_deg -- i.e. we know for sure it's not - * visible, as opposed to a satellite with no data at all (we don't know - * either way). Each satellite is classified by exactly one data source - * (ephemeris preferred over almanac when both exist), so the returned list - * and *below_mask_out never overlap. ControlThread::get_visible_sats() - * passes nullptr; SatelliteVisibility's runtime recompute uses it to tell - * "computed, not visible" apart from "can't tell" (see the class doc - * comment below). - * \param almanac_max_age_s maximum |current_time - toa|, in seconds, before - * an almanac entry is treated as stale -- i.e. as if the PRN had no almanac - * at all (reverts to "maybe visible" if ephemeris is also unusable or - * absent). Ephemeris staleness isn't a parameter here: it's checked against - * RTKLIB's own per-system MAXDTOE/MAXDTOE_GAL/MAXDTOE_BDS constants - * (rtklib.h) unconditionally, the same figures RTKLIB's own solver uses to - * decide an ephemeris is usable, rather than a second, independently - * configurable threshold that could disagree with them. toa is seconds-of- - * week, so ages beyond roughly half a week (302400 s) can't be represented - * unambiguously by this check alone -- keep it well under that. Default - * (infinity) disables almanac staleness checking entirely, matching this - * function's behavior before ages were tracked; ControlThread:: - * get_visible_sats() relies on that default (ephemeris staleness checking - * against the RTKLIB constants applies there too, unconditionally). - * \param seconds_until_next_expiry_out when non-null, set to the number of - * receiver-time seconds from now until the *soonest* currently-classified - * (visible or below-mask) satellite's data would cross its staleness - * threshold -- i.e. how long the caller can wait before an expiry-driven - * recompute is needed. Left at +infinity if nothing was classified. - * \param only_prns when non-null, every ephemeris/almanac loop skips any - * (system, PRN) not in this set -- lets a caller recompute just the - * satellite(s) whose data actually changed instead of the whole - * constellation, when nothing else (receiver position, elapsed time) that - * could affect every other satellite's elevation has changed. Left null, - * every satellite with usable data is computed, as before. + * \param gps_gtime query epoch (GPST). Its week also resolves the truncated + * week fields (GPS ephemeris WN mod 1024, GPS/Galileo almanac WNa), so no + * config-supplied reference week is needed. + * \param r_eb_e receiver ECEF position (m). + * \param elevation_mask_deg GNSS-SDR.search_elevation_mask (deg); distinct + * from PVT.elevation_mask, which gates the solution, not the search order. + * \param below_mask_out when non-null, receives every satellite whose + * elevation was computable but at/below the mask (or unhealthy). Never + * overlaps the returned list. + * \param almanac_max_age_s max |gps_gtime - toa| (s) before an almanac entry + * is ignored as stale. Ephemeris staleness is not configurable: RTKLIB's + * per-system MAXDTOE* constants apply. Infinity disables the check. + * \param seconds_until_next_expiry_out when non-null, set to the seconds + * until the soonest classified entry crosses its staleness threshold; + * +infinity if nothing was classified. + * \param only_prns when non-null, restricts the computation to these + * (system, PRN) pairs; valid only when nothing that affects the other + * satellites (position, time) has changed. + * \param glonass_strict_health if true, the Bn MSB also marks a GLONASS + * ephemeris unhealthy (the ln flag always does). + * \return (floor(El) in deg, satellite) pairs, sorted by descending elevation. */ -// The truncated week fields (ephemeris' mod-1024 rollover, almanac's WNa -- -// 2 bits for Galileo, 8 for GPS) are resolved against gps_gtime's own GPS -// week internally, rather than a caller-supplied reference: gps_gtime is the -// query time this whole computation is FOR, so it's always available and -// always close to correct, unlike a config-derived reference week (see -// gps_ref_week_from_config()) that can be left unset. std::vector> compute_visible_satellites( const std::shared_ptr& pvt_ptr, const gtime_t& gps_gtime, @@ -115,59 +82,38 @@ std::vector> compute_visible_satellites( std::vector>* below_mask_out = nullptr, double almanac_max_age_s = std::numeric_limits::infinity(), double* seconds_until_next_expiry_out = nullptr, - const std::set>* only_prns = nullptr); + const std::set>* only_prns = nullptr, + bool glonass_strict_health = true); /*! - * \brief Maintains a runtime-updated classification of the satellites - * configured for search, recomputed on GNSSFlowgraph's control-thread tick, - * into exactly two searchable sets: + * \brief Runtime classification of the searchable satellites, refreshed from + * the control-thread tick, into two sets: * - * - visible: elevation computable and above GNSS-SDR.search_elevation_mask - * right now (IsVisible() true), and not in that system's - * GNSS-SDR._banned_prns list. Searched with priority. - * - maybe visible: elevation NOT computable yet (no ephemeris/almanac) -- - * the system genuinely cannot tell whether it's visible. Not stored - * explicitly; it's whatever is in neither visible_ nor excluded_. Still - * searched, at lower priority (see - * GNSS-SDR.visible_vs_mayvisible_search_ratio), since it might turn out - * to be visible once data arrives. + * - visible_: elevation computable and above GNSS-SDR.search_elevation_mask, + * healthy, and not in GNSS-SDR._banned_prns. Searched first. + * - excluded_: elevation computable but at/below the mask (or unhealthy). + * Not searched while this holds, which is what lets a channel go idle + * once nothing plausible remains. * - * A satellite whose elevation *can* be computed is always removed from - * maybe-visible: above the mask, it's in visible_; at or below it, it's - * excluded from both -- computable-and-not-visible is not the same as - * "don't know", so it isn't searched at all while that holds (this is what - * lets a channel go idle once nothing plausible remains, instead of - * grinding through satellites we're confident aren't there). A satellite - * that's already confirmed tracked on one of its other signals is exempted - * from this exclusion regardless of what the elevation computation says -- - * see GNSSFlowgraph::pop_by_visibility() -- since a coarse almanac and a - * precise ephemeris (only decoded once tracking begins) can legitimately - * disagree right at the mask boundary, and active tracking is stronger - * evidence than either. + * Everything else is "maybe visible" (no usable data yet), searched at lower + * priority per GNSS-SDR.visible_vs_mayvisible_search_ratio. The two sets are + * disjoint by construction (one data source per satellite). A satellite + * already tracked on another signal is exempt from exclusion in + * GNSSFlowgraph::pop_by_visibility(): almanac and ephemeris can disagree at + * the mask boundary, and tracking is stronger evidence than either. * - * visible_ and excluded_ are always disjoint by construction (see - * compute_visible_satellites()'s single-data-source-per-satellite rule). + * Tick() recomputes when: the fix just became valid; any PRN's ephemeris/ + * almanac fingerprint (toe/toa, health) changed; visibility_recompute_interval_s + * of receiver time elapsed; the receiver moved more than + * visibility_recompute_position_threshold_m; or the freshest classified data + * reached its staleness threshold (RTKLIB MAXDTOE* for ephemeris, + * visibility_almanac_max_age_s for almanac). Expiry is enforced inside + * compute_visible_satellites(), so an expired PRN reverts to maybe-visible. * - * Tick() recomputes when any of five conditions holds: the fix just became - * valid; new/updated ephemeris or almanac data arrived for any PRN (a - * per-PRN toe/toa fingerprint, not just a map-size check -- catches a PRN's - * data being replaced, not just a brand new PRN appearing); - * GNSS-SDR.visibility_recompute_interval_s of receiver time has elapsed - * since the last recompute; the receiver has moved more than - * GNSS-SDR.visibility_recompute_position_threshold_m since the last - * recompute; or the freshest data behind the current classification has - * aged past its staleness threshold -- RTKLIB's own per-system - * MAXDTOE/MAXDTOE_GAL/MAXDTOE_BDS constants for ephemeris, - * GNSS-SDR.visibility_almanac_max_age_s for almanac (see - * compute_visible_satellites()'s age parameters -- expiry is enforced - * there, not just detected here, so an expired PRN actually reverts to - * maybe-visible rather than staying classified off stale data). - * - * Disabled unless GNSS-SDR.enable_visibility_aware_search=true (the default); - * Tick() is then a no-op and IsVisible()/IsExcluded() always return false, - * so callers degrade to treating every satellite as maybe-visible (i.e. - * today's undifferentiated single-queue behavior stays intact upstream). + * Inert unless GNSS-SDR.enable_visibility_aware_search=true (default false): + * Tick() is a no-op and IsVisible()/IsExcluded() return false, so every + * satellite is treated as maybe-visible. */ class SatelliteVisibility { @@ -178,32 +124,26 @@ public: uint32_t search_ratio() const { return search_ratio_; } /*! - * \brief Re-evaluates visibility if warranted: the fix just became - * valid, more ephemeris/almanac data arrived since the last check, or - * the recompute interval elapsed. Uses the live PVT fix when valid, - * else the static GNSS-SDR.AGNSS_ref_location if configured, else does - * nothing (insufficient information). + * \brief Re-evaluates visibility when one of the triggers listed in the + * class description fires. Uses the live fix when valid, else + * GNSS-SDR.AGNSS_ref_location/AGNSS_ref_utc_time if configured, else + * does nothing. * - * \returns true iff bucket membership actually changed, i.e. the caller - * should rebuild its per-signal search queues. + * \returns true iff visible_/excluded_ membership changed, i.e. the + * caller must rebuild its per-signal search queues. */ bool Tick(const std::shared_ptr& pvt_ptr, const Monitor_Pvt& fix_status); bool IsVisible(const Gnss_Satellite& sat) const; - // Elevation computable and at/below the mask -- see the class doc - // comment above for why this isn't the same as "not visible", and why - // it's a separate query from IsVisible() rather than its negation. + // Elevation computable and at/below the mask (or unhealthy). Not the + // negation of IsVisible(): a satellite with no data is in neither set. bool IsExcluded(const Gnss_Satellite& sat) const; private: - // changed_prns_out, when non-null, receives the (system, PRN) of every - // entry that was added, updated, or removed relative to the last check - // -- lets Tick() recompute just those satellites instead of the whole - // constellation when nothing else has changed. Left untouched on the - // very first call (first_data_check_): with nothing to diff against yet, - // that call is irrelevant anyway, since moved_significantly is also - // unconditionally true the first time (see last_recompute_r_eb_e_'s doc - // comment), which always forces a full recompute regardless. + // 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 + // anyway. bool DataChanged(const std::shared_ptr& pvt_ptr, std::set>* changed_prns_out = nullptr); @@ -211,109 +151,71 @@ private: uint32_t search_ratio_; double recompute_interval_s_; double elevation_mask_deg_; - // GNSS-SDR.visibility_recompute_position_threshold_m: forces an - // immediate recompute once the receiver has moved this far (meters, - // ECEF) since the last one, independent of recompute_interval_s_ -- - // covers a moving receiver crossing enough ground that elevations could - // have shifted meaningfully before the next periodic tick. + + // GNSS-SDR.visibility_recompute_position_threshold_m: ECEF displacement + // (m) since the last full recompute that forces a new one, independently + // of recompute_interval_s_. double position_threshold_m_; - // GNSS-SDR.visibility_almanac_max_age_s: see compute_visible_satellites()'s - // matching parameter. Ephemeris staleness has no equivalent member here -- - // it's checked against RTKLIB's own per-system MAXDTOE/MAXDTOE_GAL/ - // MAXDTOE_BDS constants unconditionally, not a separately configurable - // value. + + // GNSS-SDR.visibility_almanac_max_age_s (s). Ephemeris staleness is not + // configurable: RTKLIB's per-system MAXDTOE* constants apply. double almanac_max_age_s_; + + bool glonass_strict_health_; bool have_agnss_reference_; double agnss_ref_lat_deg_; double agnss_ref_lon_deg_; - // GNSS-SDR.AGNSS_ref_utc_time (via parse_agnss_ref_utc_time(), which - // already falls back to wall-clock time if unset) -- the target instant - // for the no-fix-yet fallback recompute below. Always usable, no - // separate "have a value" flag needed. This matters for offline/replay - // runs specifically: without a configured reference, wall-clock time - // gets used, which has nothing to do with the GNSS time actually - // embedded in the samples being processed, silently classifying - // satellites against the wrong orbital configuration entirely. + + // GNSS-SDR.AGNSS_ref_utc_time, falling back to wall-clock time if unset. + // Query epoch for the no-fix fallback; in replay runs the wall clock is + // unrelated to the GNSS time in the samples, so configure it explicitly. time_t agnss_ref_utc_time_; - // GNSS-SDR._banned_prns, same parsing as GNSSFlowgraph::set_signals_list(), - // for GPS/Galileo/Beidou (the systems compute_visible_satellites() covers). - // A banned PRN is never classified visible, matching how it's removed - // from the search pool entirely elsewhere -- this just keeps the - // visibility classification/diagnostics consistent with that, even - // though a banned PRN can never actually be picked from the pool anyway. + // GNSS-SDR._banned_prns, parsed as in GNSSFlowgraph::set_signals_list(). + // A banned PRN is never classified visible, keeping the diagnostics + // consistent with its removal from the search pool. std::set> banned_; - // Which of {"GPS", "Galileo", "Beidou"} have at least one channel - // actually configured (any of that system's own signals, e.g. GPS's - // "1C"/"2S"/"L5" -- same Channels_.count check - // search_next_signal() already uses to decide whether a signal is in - // play). Used to scope the maybe-visible report in Tick() to systems - // this receiver is actually searching -- otherwise an unconfigured - // system's full PRN range would sit as permanently unclassified noise, - // since nothing will ever decode ephemeris/almanac for it here. + // Systems with at least one Channels_.count > 0. Scopes the + // maybe-visible report in Tick() so an unconfigured system's PRN range + // is not listed as permanently unclassified. std::set configured_systems_; std::set> visible_; std::set> excluded_; // elevation computable, at/below the mask - // Last elevation (deg) seen for each (system, PRN) -- not part of - // classification (that always uses the raw, freshly computed elevation - // directly: decoding correctness is enforced at its actual sources -- - // week-number resolution in alm_to_rtklib(), almanac freshness/ - // overwrite priority in update_almanac_if_fresher(), the ephemeris/ - // almanac map mutex against the cross-thread data race -- not by - // second-guessing or withholding a live reading here). Kept purely so - // Tick()'s diagnostic report can show every currently classified - // satellite's elevation even on a targeted recompute, which only - // freshly computes the satellite(s) whose data actually changed -- - // this is where every other satellite's last-known value comes from. + // Floored elevations for diagnostics, including unchanged PRNs during + // targeted recomputes. Classification uses the computation's full-precision + // mask and health decisions, never these rounded values. std::map, double> reference_elevation_deg_; bool last_fix_valid_; - // Per-(system, "EPH"/"ALM", PRN) fingerprint (toe or toa) of the last - // ephemeris/almanac data seen for that entry. Finer than a map-size - // count: catches an existing PRN's data being *replaced* (new toe/toa, - // same map size), not just a brand new PRN appearing. Compared - // wholesale in DataChanged(); first_data_check_ handles the "nothing to - // compare against yet" first call explicitly, since an empty map here - // is otherwise indistinguishable from "already checked, still no data". - std::map, int32_t> last_data_fingerprints_; + + // (system, "EPH"/"ALM"/"CNAV", PRN) -> (toe/toa as absolute seconds, + // health). Catches a PRN's data being replaced, not only new PRNs. + // first_data_check_ tells "never checked" from "checked, still empty". + std::map, std::pair> last_data_fingerprints_; + bool first_data_check_; - // Receiver time (fix_status.RX_time, same units/epoch as the gps_gtime - // built in Tick()) at which the last periodic recompute ran -- NOT - // wall-clock time. This receiver frequently runs far slower than - // real-time (heavy acquisition load pegs the CPU) or far faster (fast - // file replay), so a std::chrono::steady_clock-based interval would - // silently drift from its configured meaning in receiver-time terms: - // e.g. a "5 second" wall-clock throttle can correspond to as little as - // ~1 receiver-second under heavy acquisition load, defeating the - // point of throttling exactly when CPU is most contended. Only - // meaningful once a fix is valid (RX_time isn't ticking before that); - // pre-fix recomputes rely on fix_became_valid/data_changed instead. - // Seeded far in the past so the first valid-fix tick always recomputes. + + // Absolute GPST seconds (gtime_t.time + sec) of the last full sweep: + // monotonic across week rollover, and receiver time keeps the interval + // independent of replay speed. double last_recompute_rx_time_s_; - // ECEF position (meters) used at the last recompute -- see - // position_threshold_m_. Seeded at the origin (Earth's core, guaranteed - // far from any real position) so the first recompute's displacement - // check doesn't need special-casing. + + // ECEF position (m) of the last full sweep. Seeded at the origin so the + // first displacement check trivially exceeds position_threshold_m_. arma::vec last_recompute_r_eb_e_; + // Receiver time (same scale as last_recompute_rx_time_s_) at which the - // freshest data behind the current classification will cross its - // staleness threshold -- see compute_visible_satellites()'s - // seconds_until_next_expiry_out. +infinity (never trigger) until a - // recompute with real data has actually run. + // freshest classified data goes stale. +infinity until a full sweep with + // data has run; only full sweeps update it. double next_expiry_deadline_rx_time_; - // Caps how often DataChanged() -- which copies out every - // ephemeris/almanac map just to check .size() -- actually runs, as a - // fixed fraction of Tick() calls (a call-count cap, not a time-based - // one: unlike a wall-clock throttle, this stays correctly proportional - // to Tick()'s own call rate regardless of how fast or slow the - // receiver happens to be progressing relative to real time). Seeded at - // the threshold (not 0) so the very first Tick() call always runs - // DataChanged() -- which itself always reports "changed" on its own - // first call (first_data_check_) -- producing an initial classification - // as early as possible, rather than waiting N calls in. + + // Runs DataChanged() (which copies every ephemeris/almanac map) only + // every kDataCheckEveryNTicks calls; a call count rather than a time + // throttle so it scales with the tick rate. Seeded at the threshold so + // the first Tick() checks immediately. int ticks_since_data_check_; }; diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index b6a5955a9..df3bde27c 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -30,6 +30,7 @@ set(SYSTEM_PARAMETERS_SOURCES Beidou_B1C_codes.cc sbas_ephemeris.cc gps_cnav_navigation_message.cc + glonass_gnav_almanac.cc glonass_gnav_ephemeris.cc glonass_gnav_utc_model.cc glonass_gnav_navigation_message.cc diff --git a/src/core/system_parameters/agnss_ref_location.h b/src/core/system_parameters/agnss_ref_location.h index efc951325..187412880 100644 --- a/src/core/system_parameters/agnss_ref_location.h +++ b/src/core/system_parameters/agnss_ref_location.h @@ -67,11 +67,9 @@ public: /*! - * \brief Parses a "lat,lon" (or "lat lon") GNSS-SDR.AGNSS_ref_location string - * into an Agnss_Ref_Location. valid is false (and lat/lon left default) if - * ref_location_str is empty or doesn't contain a plausible WGS84 position. - * Shared by every caller that needs this parsing (ControlThread::init(), - * SatelliteVisibility) so the rule lives in exactly one place. + * \brief Parses GNSS-SDR.AGNSS_ref_location ("lat,lon" or "lat lon", degrees). + * valid is false if the string is empty or not a plausible WGS84 position. + * Single parsing point for ControlThread and SatelliteVisibility. */ inline Agnss_Ref_Location parse_agnss_ref_location(const std::string& ref_location_str) { diff --git a/src/core/system_parameters/agnss_ref_time.h b/src/core/system_parameters/agnss_ref_time.h index 35837c006..9c73908b8 100644 --- a/src/core/system_parameters/agnss_ref_time.h +++ b/src/core/system_parameters/agnss_ref_time.h @@ -67,16 +67,11 @@ public: /*! - * \brief Parses a GNSS-SDR.AGNSS_ref_utc_time string ("DD/MM/YYYY HH:MM:SS" - * in UTC) into an Agnss_Ref_Time. An empty ref_time_str is an "educated - * guess" case, not an error: it returns the host's current wall-clock time, - * marked valid, matching what a genuinely live run with no fixed reference - * configured should use. malformed_year and malformed_format distinguish - * why a non-empty but unparseable string failed, matching the two distinct - * diagnostics ControlThread::init() has always printed; both are false when - * ref_time_str is empty or parses successfully. Shared by every caller that - * needs this parsing (ControlThread::init(), SatelliteVisibility) so the - * rule -- including the empty-string fallback -- lives in exactly one place. + * \brief Parses GNSS-SDR.AGNSS_ref_utc_time ("DD/MM/YYYY HH:MM:SS", UTC). + * An empty string is not an error: the host's current time is returned, marked + * valid. malformed_year and malformed_format (both optional) tell why a + * non-empty string was rejected. Single parsing point for ControlThread and + * SatelliteVisibility. */ inline Agnss_Ref_Time parse_agnss_ref_utc_time(const std::string& ref_time_str, bool* malformed_year = nullptr, bool* malformed_format = nullptr) { diff --git a/src/core/system_parameters/glonass_gnav_almanac.cc b/src/core/system_parameters/glonass_gnav_almanac.cc new file mode 100644 index 000000000..84e5be5f9 --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_almanac.cc @@ -0,0 +1,132 @@ +/*! + * \file glonass_gnav_almanac.cc + * \brief GLONASS almanac orbit propagation for satellite acquisition + * \author Carles Fernandez, 2026. 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-2026 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "glonass_gnav_almanac.h" +#include + +std::array Glonass_Gnav_Almanac::perturbations(double a, double inclination, + double l, double h, double longitude, double nt) +{ + // GLONASS ICD 5.1, A.3.2.2, equation (1). The first component is delta-a / a. + const double j = 1.5 * 1082.63e-6 * std::pow(6378.136 / a, 2); + const double si = std::sin(inclination); + const double ci = std::cos(inclination); + const double s1 = std::sin(longitude); + const double c1 = std::cos(longitude); + const double s2 = std::sin(2.0 * longitude); + const double c2 = std::cos(2.0 * longitude); + const double s3 = std::sin(3.0 * longitude); + const double c3 = std::cos(3.0 * longitude); + const double s4 = std::sin(4.0 * longitude); + const double c4 = std::cos(4.0 * longitude); + const double b = 1.0 - 1.5 * si * si; + const double node = nt + 3.5 * l * s1 - 2.5 * h * c1 - 0.5 * s2 - 7.0 / 6.0 * l * s3 + 7.0 / 6.0 * h * c3; + return {{2.0 * j * b * (l * c1 + h * s1) + j * si * si * (0.5 * h * s1 - 0.5 * l * c1 + c2 + 3.5 * l * c3 + 3.5 * h * s3), + j * b * (l * nt + s1 + 1.5 * l * s2 - 1.5 * h * c2) - 0.25 * j * si * si * (s1 - 7.0 / 3.0 * s3 + 5.0 * l * s2 - 8.5 * l * s4 + 8.5 * h * c4 + h * c2) + j * ci * ci * (l * nt - 0.5 * l * s2), + j * b * (-h * nt + c1 + 1.5 * l * c2 + 1.5 * h * s2) - 0.25 * j * si * si * (-c1 - 7.0 / 3.0 * c3 - 5.0 * h * s2 - 8.5 * l * c4 - 8.5 * h * s4 + l * c2) + j * ci * ci * (-h * nt + 0.5 * h * s2), + -j * ci * node, + 0.5 * j * si * ci * (-l * c1 + h * s1 + c2 + 7.0 / 3.0 * l * c3 + 7.0 / 3.0 * h * s3), + 2.0 * j * b * (nt + 1.75 * l * s1 - 1.75 * h * c1) + 3.0 * j * si * si * (-7.0 / 24.0 * h * c1 - 7.0 / 24.0 * l * s1 - 49.0 / 72.0 * h * c3 + 49.0 / 72.0 * l * s3 + 0.25 * s2) + j * ci * ci * node}}; +} + + +bool Glonass_Gnav_Almanac::satellite_position(double elapsed_s, std::array& position_m) const +{ + // ICD 5.1, Appendix A.3.2. Units inside the algorithm are kilometres/radians. + const double pi = 3.14159265358979323846; + const double mu = 398600.44; + const double c20 = -1082.63e-6; + const double earth_rate = 7.292115e-5; // Table 3.2 (the A.3.2.2 value has a typographical error). + const double e = d_epsilon_n_A; + const double i = 63.0 * pi / 180.0 + d_Delta_i_n_A; + const double period = 43200.0 + d_Delta_T_n_A; + if (!std::isfinite(elapsed_s) || !std::isfinite(e) || e < 0.0 || e >= 1.0 || + !std::isfinite(i) || !std::isfinite(period) || period <= 0.0 || + !std::isfinite(d_Delta_T_n_A_dot) || !std::isfinite(d_omega_n_A) || !std::isfinite(d_lambda_n_A)) + { + return false; + } + double a = std::cbrt(mu * std::pow(period / (2.0 * pi), 2)); + bool converged = false; + for (int iteration = 0; iteration < 20; ++iteration) + { + const double p = a * (1.0 - e * e); + const double node_factor = 1.0 + e * std::cos(d_omega_n_A); + const double correction = 1.5 * c20 * std::pow(6378.136 / p, 2) * + ((2.0 - 2.5 * std::pow(std::sin(i), 2)) * std::pow(1.0 - e * e, 1.5) / (node_factor * node_factor) + + std::pow(node_factor, 3) / (1.0 - e * e)); + const double next = std::cbrt(mu * std::pow(period / ((1.0 + correction) * 2.0 * pi), 2)); + converged = std::abs(next - a) < 1.0e-3; + a = next; + if (converged) + { + break; + } + } + if (!converged || !std::isfinite(a) || a <= 6378.136) + { + return false; + } + const double revolutions = std::floor(elapsed_s / period); + const double node_elapsed = period * revolutions + d_Delta_T_n_A_dot * revolutions * revolutions; + const double tau = elapsed_s - node_elapsed; + const double n = 2.0 * pi / period; + const double node_rate = 1.5 * c20 * n * std::pow(6378.136 / a, 2) * std::cos(i) / std::pow(1.0 - e * e, 2); + const double eccentric_node = 2.0 * std::atan2(std::sqrt(1.0 - e) * std::sin(-d_omega_n_A / 2.0), + std::sqrt(1.0 + e) * std::cos(-d_omega_n_A / 2.0)); + const double mean_node = eccentric_node - e * std::sin(eccentric_node); + const double l = e * std::cos(d_omega_n_A); + const double h = e * std::sin(d_omega_n_A); + const auto initial = perturbations(a, i, l, h, mean_node + d_omega_n_A, 0.0); + const auto current = perturbations(a, i, l, h, mean_node + d_omega_n_A + n * tau, n * tau); + const double hi = h + current[1] - initial[1]; + const double li = l + current[2] - initial[2]; + const double ei = std::hypot(hi, li); + const double omega = std::atan2(hi, li); + const double ai = a * (1.0 + current[0] - initial[0]); + const double ii = i + current[4] - initial[4]; + // Rotate the inertial result back to ECEF at the query epoch. Sidereal + // time cancels, leaving Earth rotation since the reference ascending node. + const double node = d_lambda_n_A + node_rate * node_elapsed - earth_rate * elapsed_s + current[3] - initial[3]; + const double mean = std::remainder(mean_node + d_omega_n_A + n * tau + current[5] - initial[5] - omega, 2.0 * pi); + if (!std::isfinite(ei) || ei >= 1.0 || !std::isfinite(mean) || ai <= 0.0) + { + return false; + } + double eccentric = mean; + converged = false; + for (int iteration = 0; iteration < 30; ++iteration) + { + const double correction = (eccentric - ei * std::sin(eccentric) - mean) / (1.0 - ei * std::cos(eccentric)); + eccentric -= correction; + if (std::abs(correction) < 1.0e-12) + { + converged = true; + break; + } + } + if (!converged) + { + return false; + } + const double u = std::atan2(std::sqrt(1.0 - ei * ei) * std::sin(eccentric), std::cos(eccentric) - ei) + omega; + const double radius_m = 1000.0 * ai * (1.0 - ei * std::cos(eccentric)); + position_m = {{radius_m * (std::cos(u) * std::cos(node) - std::sin(u) * std::sin(node) * std::cos(ii)), + radius_m * (std::cos(u) * std::sin(node) + std::sin(u) * std::cos(node) * std::cos(ii)), + radius_m * std::sin(u) * std::sin(ii)}}; + return std::isfinite(position_m[0]) && std::isfinite(position_m[1]) && std::isfinite(position_m[2]); +} diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h index 523a1708f..182c4004b 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.h +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -21,6 +21,8 @@ #define GNSS_SDR_GLONASS_ALMANAC_H #include +#include +#include #include /** \addtogroup Core @@ -42,6 +44,12 @@ public: */ Glonass_Gnav_Almanac() = default; + int32_t d_N_A{}; //!< Almanac reference day within its four-year interval (zero if unknown). + int32_t d_N_4{}; //!< Four-year interval containing d_N_A, numbered from 1996. + + //! Compute ECEF position in metres, elapsed_s after the reference ascending node (ICD A.3.2). + bool satellite_position(double elapsed_s, std::array& position_m) const; + double d_n_A{}; //!< Conventional number of satellite within GLONASS space segment [dimensionless] double d_H_n_A{}; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite as table 4.10 (0-31) [dimensionless] double d_lambda_n_A{}; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [radians] @@ -90,9 +98,19 @@ public: archive& BOOST_SERIALIZATION_NVP(d_tau_n_A); archive& BOOST_SERIALIZATION_NVP(d_C_n); archive& BOOST_SERIALIZATION_NVP(d_l_n); + if (version > 0) + { + archive& BOOST_SERIALIZATION_NVP(d_N_A); + archive& BOOST_SERIALIZATION_NVP(d_N_4); + } } + +private: + static std::array perturbations(double a, double inclination, double l, double h, double longitude, double nt); }; +BOOST_CLASS_VERSION(Glonass_Gnav_Almanac, 1) + /** \} */ /** \} */ diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 248ac37de..4f2e259e0 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -299,6 +299,18 @@ void Glonass_Gnav_Navigation_Message::apply_kp_leap_second(int32_t leap_s) void Glonass_Gnav_Navigation_Message::update_almanac_satellite_info() { Glonass_Gnav_Almanac& alm = gnav_almanac[i_alm_satellite_slot_number - 1]; + // Freeze the date with this record; later string-5 updates must not redate + // an older satellite's almanac. N4 describes the current four-year interval. + alm.d_N_A = static_cast(gnav_utc_model.d_N_A); + alm.d_N_4 = static_cast(gnav_utc_model.d_N_4); + if (alm.d_N_A - gnav_ephemeris.d_N_T > 730.0) + { + --alm.d_N_4; + } + else if (gnav_ephemeris.d_N_T - alm.d_N_A > 730.0) + { + ++alm.d_N_4; + } // H_n_A values 25..31 represent carrier frequency channels -7..-1, values // 0..13 map directly to channels 0..13, and values 14..24 are not used // (GLONASS ICD Table 4.10) diff --git a/src/core/system_parameters/gnss_almanac.h b/src/core/system_parameters/gnss_almanac.h index f0e0410bd..5950be211 100644 --- a/src/core/system_parameters/gnss_almanac.h +++ b/src/core/system_parameters/gnss_almanac.h @@ -91,11 +91,8 @@ public: double af0{}; //!< Coefficient 0 of code phase offset model [s] double af1{}; //!< Coefficient 1 of code phase offset model [s/s] - //! True if this entry came from an AGNSS/SUPL bootstrap load at startup - //! rather than a live broadcast decode. Deliberately not part of - //! serialize() below -- it is a runtime-only provenance marker set by - //! the code that performs the startup load, not a field of the almanac - //! itself, and has no business round-tripping through a saved XML file. + //! Loaded by the AGNSS/SUPL startup bootstrap rather than decoded from the + //! broadcast. Runtime-only provenance flag, intentionally not serialized. bool from_startup_load{false}; protected: diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 97fa0e157..c9298686c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1051,6 +1051,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) set(FLOWGRAPH_TEST_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/gnss_flowgraph_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/satellite_visibility_test.cc ) if(USE_CMAKE_TARGET_SOURCES) add_executable(flowgraph_test) @@ -1070,6 +1071,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) input_filter_adapters channel_adapters core_receiver + pvt_gr_blocks algorithms_libs ) if(ENABLE_GLOG_AND_GFLAGS) diff --git a/tests/test_main.cc b/tests/test_main.cc index fbbde387a..3a330b25a 100644 --- a/tests/test_main.cc +++ b/tests/test_main.cc @@ -147,6 +147,7 @@ private: #include "unit-tests/control-plane/gnss_synchro_monitor_test.cc" #include "unit-tests/control-plane/in_memory_configuration_test.cc" #include "unit-tests/control-plane/protobuf_test.cc" +#include "unit-tests/control-plane/satellite_visibility_test.cc" #include "unit-tests/control-plane/string_converter_test.cc" #include "unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc" #include "unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_gsoc2013_test.cc" diff --git a/tests/unit-tests/control-plane/satellite_visibility_test.cc b/tests/unit-tests/control-plane/satellite_visibility_test.cc new file mode 100644 index 000000000..6e7a0824d --- /dev/null +++ b/tests/unit-tests/control-plane/satellite_visibility_test.cc @@ -0,0 +1,547 @@ +/*! + * \file satellite_visibility_test.cc + * \brief Regression tests for satellite visibility classification and timing + * \author Carles Fernandez-Prades, 2026. 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-2026 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "geofunctions.h" +#include "in_memory_configuration.h" +#include "monitor_pvt.h" +#include "pvt_conf.h" +#include "pvt_interface.h" +#include "rtklib_conversions.h" +#include "rtklib_ephemeris.h" +#include "rtklib_pvt_gs.h" +#include "rtklib_rtkcmn.h" +#include "rtklib_rtksvr.h" +#include "satellite_visibility.h" +#include "sensor_data/sensor_data_source_configuration.h" +#include +#include +#include +#include +#include +#include + +class VisibilityTestPvt : public PvtInterface +{ +public: + std::map gps_eph; + std::map gal_eph; + std::map bds_eph; + std::map gps_alm; + std::map gal_alm; + std::map bds_alm; + std::map gps_cnav_ephemeris; + std::map glonass_ephemeris; + std::map glonass_almanac; + Glonass_Gnav_Utc_Model glo_utc; + mutable unsigned int gps_reads = 0; + + std::string role() override { return "PVT"; } + std::string implementation() override { return "VisibilityTestPvt"; } + size_t item_size() override { return 0; } + void connect(gr::top_block_sptr /*top_block*/) override {} + void disconnect(gr::top_block_sptr /*top_block*/) override {} + gr::basic_block_sptr get_left_block() override { return nullptr; } + gr::basic_block_sptr get_right_block() override { return nullptr; } + void reset() override {} + void clear_ephemeris() override {} + std::map get_gps_ephemeris() const override + { + ++gps_reads; + return gps_eph; + } + std::map get_galileo_ephemeris() const override { return gal_eph; } + std::map get_beidou_dnav_ephemeris() const override { return bds_eph; } + std::map get_gps_almanac() const override { return gps_alm; } + std::map get_galileo_almanac() const override { return gal_alm; } + std::map get_beidou_dnav_almanac() const override { return bds_alm; } + std::map get_gps_cnav_ephemeris() const override { return gps_cnav_ephemeris; } + std::map get_glonass_ephemeris() const override { return glonass_ephemeris; } + std::map get_glonass_almanac() const override { return glonass_almanac; } + Glonass_Gnav_Utc_Model get_glonass_utc_model() const override { return glo_utc; } + bool get_latest_PVT(double* /*longitude_deg*/, double* /*latitude_deg*/, double* /*height_m*/, double* /*ground_speed_kmh*/, double* /*course_over_ground_deg*/, time_t* /*UTC_time*/) override { return false; } +}; + + +class SatelliteVisibilityTest : public ::testing::Test +{ +protected: + void SetUp() override + { + configuration = std::make_shared(); + configuration->set_property("GNSS-SDR.enable_visibility_aware_search", "true"); + pvt = std::make_shared(); + fix = Monitor_Pvt{}; + fix.week = 2400; + fix.RX_time = 100000.0; + fix.latitude = 0.0; + fix.longitude = 0.0; + fix.height = 0.0; + } + + void SetGpsAlmanac(int toa) + { + Gps_Almanac almanac; + almanac.PRN = 1; + almanac.WNa = fix.week % 256; + almanac.toa = toa; + almanac.sqrtA = 5153.795; + almanac.ecc = 0.01; + pvt->gps_alm[1] = almanac; + } + + void SetConstellationData(int tow) + { + Gps_Ephemeris gps; + gps.PRN = 1; + gps.WN = fix.week % 1024; + gps.toe = gps.toc = gps.tow = tow; + gps.sqrtA = 5153.795; + pvt->gps_eph[1] = gps; + SetGpsAlmanac(tow); + Gps_Almanac gps_almanac = pvt->gps_alm.at(1); + gps_almanac.PRN = 2; + pvt->gps_alm.clear(); + pvt->gps_alm[2] = gps_almanac; + + Galileo_Ephemeris galileo; + galileo.PRN = 1; + galileo.WN = fix.week - 1024; + galileo.toe = galileo.toc = galileo.tow = tow; + galileo.sqrtA = 5440.0; + pvt->gal_eph[1] = galileo; + Galileo_Almanac galileo_almanac; + galileo_almanac.PRN = 2; + galileo_almanac.WNa = (fix.week - 1024) % 4; + galileo_almanac.toa = tow; + galileo_almanac.sqrtA = 5440.0; + pvt->gal_alm[2] = galileo_almanac; + + Beidou_Dnav_Ephemeris beidou; + beidou.PRN = 10; + beidou.WN = fix.week - 1356; + beidou.toe = beidou.toc = beidou.tow = tow; + beidou.sqrtA = 5282.0; + pvt->bds_eph[10] = beidou; + Beidou_Dnav_Almanac beidou_almanac; + beidou_almanac.PRN = 11; + beidou_almanac.WNa = fix.week - 1356; + beidou_almanac.toa = tow; + beidou_almanac.sqrtA = 5282.0; + pvt->bds_alm[11] = beidou_almanac; + } + + size_t ClassifiedCount(const gtime_t& time) + { + std::vector> excluded; + const auto visible = compute_visible_satellites(pvt, time, arma::vec{6378137.0, 0.0, 0.0}, + -90.0, &excluded, 259200.0); + return visible.size() + excluded.size(); + } + + double GpsElevation() const + { + const auto almanac = alm_to_rtklib(pvt->gps_alm.at(1), fix.week); + double position[3]; + double clock_bias; + alm2pos(gpst2time(fix.week, fix.RX_time), &almanac, position, &clock_bias); + const arma::vec receiver{6378137.0, 0.0, 0.0}; + const arma::vec satellite{position[0], position[1], position[2]}; + double azimuth; + double elevation; + double distance; + topocent(&azimuth, &elevation, &distance, receiver, satellite - receiver); + return elevation; + } + + std::shared_ptr configuration; + std::shared_ptr pvt; + Monitor_Pvt fix{}; +}; + + +TEST_F(SatelliteVisibilityTest, RejectsWeekOldDataAcrossConstellations) +{ + SetConstellationData(100000); + EXPECT_EQ(6U, ClassifiedCount(gpst2time(fix.week, fix.RX_time))); + EXPECT_EQ(0U, ClassifiedCount(gpst2time(fix.week + 1, fix.RX_time))); +} + + +TEST_F(SatelliteVisibilityTest, AcceptsFreshDataAcrossWeekRollover) +{ + SetConstellationData(604700); + EXPECT_EQ(6U, ClassifiedCount(gpst2time(fix.week + 1, 100.0))); +} + + +TEST_F(SatelliteVisibilityTest, PreservesFractionalMaskClassification) +{ + SetGpsAlmanac(100000); + const double elevation = GpsElevation(); + const double mask = std::floor(elevation) + 0.1; + ASSERT_GT(elevation, mask); + configuration->set_property("GNSS-SDR.search_elevation_mask", std::to_string(mask)); + SatelliteVisibility visibility(configuration); + EXPECT_TRUE(visibility.Tick(pvt, fix)); + EXPECT_TRUE(visibility.IsVisible(Gnss_Satellite("GPS", 1))); + EXPECT_FALSE(visibility.IsExcluded(Gnss_Satellite("GPS", 1))); +} + + +TEST_F(SatelliteVisibilityTest, ExcludesUnhealthySatelliteAboveFractionalMask) +{ + SetGpsAlmanac(100000); + const double mask = std::floor(GpsElevation()) + 0.1; + configuration->set_property("GNSS-SDR.search_elevation_mask", std::to_string(mask)); + pvt->gps_alm.at(1).SV_health = 1; + SatelliteVisibility visibility(configuration); + EXPECT_TRUE(visibility.Tick(pvt, fix)); + EXPECT_FALSE(visibility.IsVisible(Gnss_Satellite("GPS", 1))); + EXPECT_TRUE(visibility.IsExcluded(Gnss_Satellite("GPS", 1))); +} + + +TEST_F(SatelliteVisibilityTest, ExcludesHealthySatelliteBelowMask) +{ + SetGpsAlmanac(100000); + configuration->set_property("GNSS-SDR.search_elevation_mask", std::to_string(GpsElevation() + 0.1)); + SatelliteVisibility visibility(configuration); + EXPECT_TRUE(visibility.Tick(pvt, fix)); + EXPECT_FALSE(visibility.IsVisible(Gnss_Satellite("GPS", 1))); + EXPECT_TRUE(visibility.IsExcluded(Gnss_Satellite("GPS", 1))); +} + + +TEST_F(SatelliteVisibilityTest, PeriodicSweepCrossesWeekRollover) +{ + SetGpsAlmanac(604700); + fix.RX_time = 604700.0; + SatelliteVisibility visibility(configuration); + visibility.Tick(pvt, fix); + const unsigned int reads = pvt->gps_reads; + fix.week++; + fix.RX_time = 19.0; + visibility.Tick(pvt, fix); + EXPECT_EQ(reads, pvt->gps_reads); + fix.RX_time = 20.0; + visibility.Tick(pvt, fix); + EXPECT_GT(pvt->gps_reads, reads); +} + + +TEST_F(SatelliteVisibilityTest, ExpiryCrossesWeekRolloverWithoutPeriodicSweep) +{ + configuration->set_property("GNSS-SDR.visibility_recompute_interval_s", "1000000"); + configuration->set_property("GNSS-SDR.visibility_almanac_max_age_s", "200"); + SetGpsAlmanac(604700); + fix.RX_time = 604790.0; + SatelliteVisibility visibility(configuration); + visibility.Tick(pvt, fix); + const Gnss_Satellite satellite("GPS", 1); + ASSERT_TRUE(visibility.IsVisible(satellite) || visibility.IsExcluded(satellite)); + fix.week++; + fix.RX_time = 101.0; + EXPECT_TRUE(visibility.Tick(pvt, fix)); + EXPECT_FALSE(visibility.IsVisible(satellite)); + EXPECT_FALSE(visibility.IsExcluded(satellite)); +} + + +TEST_F(SatelliteVisibilityTest, DisabledSearchDoesNotReadNavigationData) +{ + auto disabled_configuration = std::make_shared(); + SatelliteVisibility visibility(disabled_configuration); + SetGpsAlmanac(100000); + EXPECT_FALSE(visibility.Tick(pvt, fix)); + EXPECT_EQ(0U, pvt->gps_reads); +} + + +TEST_F(SatelliteVisibilityTest, ConcurrentNavigationSnapshotsAndClears) +{ + Pvt_Conf conf; + conf.output_enabled = false; + conf.rinex_output_enabled = false; + conf.gpx_output_enabled = false; + conf.geojson_output_enabled = false; + conf.nmea_output_file_enabled = false; + conf.kml_output_enabled = false; + conf.xml_output_enabled = false; + conf.rtcm_output_file_enabled = false; + conf.enable_rx_clock_correction = false; + conf.output_rate_ms = 1000; + conf.display_rate_ms = 1000; + conf.signal_enabled_flags = 1; + rtk_t rtk{}; + rtk.opt = PRCOPT_DEFAULT; + const SensorDataSourceConfiguration sensor_configuration(configuration.get()); + const auto block = rtklib_make_pvt_gs(1, conf, rtk, sensor_configuration); + const auto source = gr::blocks::null_source::make(sizeof(Gnss_Synchro)); + const auto flowgraph = gr::make_top_block("visibility_navigation_snapshot_test"); + flowgraph->connect(source, 0, block, 0); + flowgraph->start(); + + // Telemetry runs on the GNU Radio worker while snapshots and clears run + // on this thread, just as they do during visibility-aware acquisition. + for (int update = 1; update <= 256; ++update) + { + auto almanac = std::make_shared(); + almanac->PRN = 1 + update % 32; + almanac->toa = update; + almanac->af0 = update; + almanac->af1 = -update; + block->_post(pmt::mp("telemetry"), pmt::make_any(almanac)); + for (const auto& entry : block->get_gps_almanac_map()) + { + EXPECT_EQ(entry.first, static_cast(entry.second.PRN)); + EXPECT_DOUBLE_EQ(entry.second.toa, entry.second.af0); + EXPECT_DOUBLE_EQ(-entry.second.toa, entry.second.af1); + } + if (update % 32 == 0) + { + block->clear_ephemeris(); + } + } + + // Wait for a final record after the last clear, proving that the worker + // consumed telemetry and did not deadlock against the snapshot getter. + auto sentinel = std::make_shared(); + sentinel->PRN = 1; + sentinel->toa = 10000; + block->_post(pmt::mp("telemetry"), pmt::make_any(sentinel)); + bool received = false; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (std::chrono::steady_clock::now() < deadline) + { + const auto snapshot = block->get_gps_almanac_map(); + const auto it = snapshot.find(1); + if (it != snapshot.end() && it->second.toa == sentinel->toa) + { + received = true; + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(received); + const auto retained_almanac = block->get_gps_almanac_map(); + auto gps = std::make_shared(); + gps->PRN = 1; + auto galileo = std::make_shared(); + galileo->PRN = 1; + galileo->nav_message_type = Galileo_Nav_Message_Type::INAV; + auto galileo_almanac = std::make_shared(); + galileo_almanac->PRN = 1; + auto beidou = std::make_shared(); + beidou->PRN = 10; + auto beidou_almanac = std::make_shared(); + beidou_almanac->PRN = 10; + block->_post(pmt::mp("telemetry"), pmt::make_any(gps)); + block->_post(pmt::mp("telemetry"), pmt::make_any(galileo)); + block->_post(pmt::mp("telemetry"), pmt::make_any(galileo_almanac)); + block->_post(pmt::mp("telemetry"), pmt::make_any(beidou)); + block->_post(pmt::mp("telemetry"), pmt::make_any(beidou_almanac)); + const auto navigation_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (block->get_beidou_dnav_almanac_map().empty() && std::chrono::steady_clock::now() < navigation_deadline) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_EQ(1U, block->get_gps_ephemeris_map().size()); + EXPECT_EQ(1U, block->get_galileo_ephemeris_map().size()); + EXPECT_EQ(1U, block->get_galileo_almanac_map().size()); + EXPECT_EQ(1U, block->get_beidou_dnav_ephemeris_map().size()); + EXPECT_EQ(1U, block->get_beidou_dnav_almanac_map().size()); + + // Clear must be immediately visible to readers. The first subsequent + // message must also clear the worker's maps before publishing new data. + block->clear_ephemeris(); + EXPECT_TRUE(block->get_gps_almanac_map().empty()); + EXPECT_TRUE(block->get_gps_ephemeris_map().empty()); + EXPECT_TRUE(block->get_galileo_ephemeris_map().empty()); + EXPECT_TRUE(block->get_galileo_almanac_map().empty()); + EXPECT_TRUE(block->get_beidou_dnav_ephemeris_map().empty()); + EXPECT_TRUE(block->get_beidou_dnav_almanac_map().empty()); + auto replacement = std::make_shared(*sentinel); + replacement->PRN = 2; + block->_post(pmt::mp("telemetry"), pmt::make_any(replacement)); + const auto replacement_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (block->get_gps_almanac_map().count(2) == 0 && std::chrono::steady_clock::now() < replacement_deadline) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + const auto replaced = block->get_gps_almanac_map(); + EXPECT_EQ(1U, replaced.size()); + EXPECT_EQ(1U, replaced.count(2)); + EXPECT_EQ(0U, replaced.count(1)); + EXPECT_EQ(sentinel->toa, retained_almanac.at(1).toa); + EXPECT_TRUE(block->get_gps_ephemeris_map().empty()); + EXPECT_TRUE(block->get_galileo_ephemeris_map().empty()); + EXPECT_TRUE(block->get_galileo_almanac_map().empty()); + EXPECT_TRUE(block->get_beidou_dnav_ephemeris_map().empty()); + EXPECT_TRUE(block->get_beidou_dnav_almanac_map().empty()); + + double longitude; + double latitude; + double height; + double speed; + double course; + time_t utc; + EXPECT_FALSE(block->get_latest_PVT(&longitude, &latitude, &height, &speed, &course, &utc)); + flowgraph->stop(); + flowgraph->wait(); + block->clear_ephemeris(); + EXPECT_TRUE(block->get_gps_almanac_map().empty()); +} + + +TEST_F(SatelliteVisibilityTest, GlonassAlmanacMatchesIcdExample) +{ + // GLONASS ICD 5.1, Appendix A.3.2.3. Published coordinates are inertial + // kilometres; rotate them to ECEF using the example's sidereal angle. + const double pi = std::acos(-1.0); + Glonass_Gnav_Almanac almanac; + almanac.d_N_4 = 2; + almanac.d_N_A = 615; + almanac.d_lambda_n_A = -0.189986229 * pi; + almanac.d_t_lambda_n_A = 27122.09375; + almanac.d_Delta_i_n_A = 0.011929512 * pi; + almanac.d_Delta_T_n_A = -2655.76171875; + almanac.d_Delta_T_n_A_dot = 0.000549316; + almanac.d_epsilon_n_A = 0.001482010; + almanac.d_omega_n_A = 0.440277100 * pi; + std::array position{}; + ASSERT_TRUE(almanac.satellite_position(33300.0 - almanac.d_t_lambda_n_A, position)); + const double sidereal = 6.02401539573 + 7.292115e-5 * (33300.0 - 10800.0); + EXPECT_NEAR(10947021.572 * std::cos(sidereal) + 13078978.287 * std::sin(sidereal), position[0], 0.2); + EXPECT_NEAR(-10947021.572 * std::sin(sidereal) + 13078978.287 * std::cos(sidereal), position[1], 0.2); + EXPECT_NEAR(18922063.362, position[2], 0.2); + const double utc_epoch[6] = {2001, 9, 6, 0, 0, 0}; + EXPECT_NEAR(almanac.d_t_lambda_n_A - 10800.0, + timediff(gpst2utc(glonass_almanac_epoch(almanac)), epoch2time(utc_epoch)), 1.0e-9); +} + + +TEST_F(SatelliteVisibilityTest, GlonassAlmanacHealthAgeAndMissingDate) +{ + Glonass_Gnav_Almanac almanac; + almanac.PRN = 1; + almanac.d_N_4 = 8; + almanac.d_N_A = 1; + almanac.d_t_lambda_n_A = 10000.0; + almanac.d_C_n = true; + pvt->glonass_almanac[1] = almanac; + const auto epoch = glonass_almanac_epoch(almanac); + std::vector> excluded; + const arma::vec receiver{6378137.0, 0.0, 0.0}; + auto visible = compute_visible_satellites(pvt, epoch, receiver, 0.0, &excluded, 259200.0); + ASSERT_EQ(1U, visible.size()); + EXPECT_EQ("Glonass", visible.front().second.get_system()); + EXPECT_TRUE(excluded.empty()); + pvt->glonass_almanac[1].d_C_n = false; + EXPECT_TRUE(compute_visible_satellites(pvt, epoch, receiver, 0.0, &excluded, 259200.0).empty()); + EXPECT_EQ(1U, excluded.size()); + pvt->glonass_almanac[1].d_C_n = true; + pvt->glonass_almanac[1].d_l_n = true; + excluded.clear(); + EXPECT_TRUE(compute_visible_satellites(pvt, epoch, receiver, 0.0, &excluded, 259200.0).empty()); + EXPECT_EQ(1U, excluded.size()); + EXPECT_EQ(0U, ClassifiedCount(timeadd(epoch, 604800.0))); + EXPECT_EQ(1U, ClassifiedCount(timeadd(epoch, 86400.0))); // across the four-year boundary + pvt->glonass_almanac[1].d_N_A = 0; + EXPECT_EQ(0U, ClassifiedCount(epoch)); + pvt->glonass_almanac[1].d_N_A = 1; + pvt->glonass_almanac[1].d_epsilon_n_A = 1.0; + EXPECT_EQ(0U, ClassifiedCount(epoch)); +} + + +TEST_F(SatelliteVisibilityTest, GlonassEphemerisPrecedesAlmanacAndExpires) +{ + Glonass_Gnav_Ephemeris ephemeris; + ephemeris.PRN = ephemeris.i_satellite_slot_number = 1; + ephemeris.d_yr = 2024; + ephemeris.d_N_T = 1; + ephemeris.d_t_b = ephemeris.d_t_k = 10800.0; + ephemeris.d_Xn = -25500.0; // below the horizon + ephemeris.d_VZn = 3.9; + pvt->glonass_ephemeris[1] = ephemeris; + Glonass_Gnav_Almanac almanac; + almanac.PRN = 1; + almanac.d_N_4 = 8; + almanac.d_N_A = 1; + almanac.d_t_lambda_n_A = 10800.0; + almanac.d_C_n = true; // directly above the receiver at this ascending node + pvt->glonass_almanac[1] = almanac; + const auto epoch = glonass_almanac_epoch(almanac); + const arma::vec receiver{6378137.0, 0.0, 0.0}; + std::vector> excluded; + double expiry = 0.0; + EXPECT_TRUE(compute_visible_satellites(pvt, epoch, receiver, 0.0, &excluded, 259200.0, &expiry).empty()); + ASSERT_EQ(1U, excluded.size()); + EXPECT_NEAR(MAXDTOE_GLO, expiry, 1.0e-6); + excluded.clear(); + EXPECT_EQ(1U, compute_visible_satellites(pvt, timeadd(epoch, MAXDTOE_GLO + 1.0), receiver, 0.0, &excluded, 259200.0).size()); + EXPECT_TRUE(excluded.empty()); + pvt->glonass_almanac.clear(); + pvt->glonass_ephemeris[1].d_Xn = 25500.0; + pvt->glonass_ephemeris[1].d_B_n = 4; + EXPECT_TRUE(compute_visible_satellites(pvt, epoch, receiver, 0.0).empty()); + EXPECT_EQ(1U, compute_visible_satellites(pvt, epoch, receiver, 0.0, nullptr, 259200.0, nullptr, nullptr, false).size()); + pvt->glonass_ephemeris[1].d_l3rd_n = true; + EXPECT_TRUE(compute_visible_satellites(pvt, epoch, receiver, 0.0, nullptr, 259200.0, nullptr, nullptr, false).empty()); + EXPECT_EQ(0U, ClassifiedCount(timeadd(epoch, 86400.0))); +} + + +TEST_F(SatelliteVisibilityTest, QzssLnavCnavAlmanacAndAlias) +{ + SetConstellationData(static_cast(fix.RX_time)); + auto lnav = pvt->gps_eph.at(1); + auto almanac = pvt->gps_alm.at(2); + pvt->gps_eph.clear(); + pvt->gps_alm.clear(); + pvt->gal_eph.clear(); + pvt->gal_alm.clear(); + pvt->bds_eph.clear(); + pvt->bds_alm.clear(); + lnav.PRN = 196; + lnav.sqrtA = std::sqrt(42164000.0); + pvt->gps_eph[196] = lnav; + almanac.PRN = 194; + almanac.sqrtA = lnav.sqrtA; + almanac.set_system('J'); + pvt->gps_alm[194] = almanac; + Gps_CNAV_Ephemeris cnav; + cnav.PRN = 195; + cnav.WN = fix.week; + cnav.toe1 = cnav.toe2 = cnav.toc = cnav.tow = static_cast(fix.RX_time); + cnav.sqrtA = lnav.sqrtA; + pvt->gps_cnav_ephemeris[195] = cnav; + configuration->set_property("GNSS-SDR.search_elevation_mask", "-90"); + SatelliteVisibility visibility(configuration); + EXPECT_TRUE(visibility.Tick(pvt, fix)); + EXPECT_TRUE(visibility.IsVisible(Gnss_Satellite("QZSS", 194))); + EXPECT_TRUE(visibility.IsVisible(Gnss_Satellite("QZSS", 195))); + EXPECT_TRUE(visibility.IsVisible(Gnss_Satellite("QZSS", 196))); + EXPECT_TRUE(visibility.IsVisible(Gnss_Satellite("QZSS", 203))); // alias of 196 + EXPECT_EQ(3U, ClassifiedCount(gpst2time(fix.week, fix.RX_time))); + EXPECT_EQ(0U, ClassifiedCount(gpst2time(fix.week + 1, fix.RX_time))); + pvt->gps_eph[196].SV_health = 1; + for (int tick = 0; tick < 20; ++tick) + { + visibility.Tick(pvt, fix); + } + EXPECT_TRUE(visibility.IsExcluded(Gnss_Satellite("QZSS", 203))); +}