diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 9a4112ff3..4c4bab708 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -914,6 +914,18 @@ std::map Rtklib_Pvt::get_beidou_dnav_ephemeris() con } +std::map Rtklib_Pvt::get_beidou_cnav1_ephemeris() const +{ + return pvt_->get_beidou_cnav1_ephemeris_map(); +} + + +std::map Rtklib_Pvt::get_beidou_cnav2_ephemeris() const +{ + return pvt_->get_beidou_cnav2_ephemeris_map(); +} + + std::map Rtklib_Pvt::get_gps_almanac() const { return pvt_->get_gps_almanac_map(); diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h index 3ce00df9b..835c09a94 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.h +++ b/src/algorithms/PVT/adapters/rtklib_pvt.h @@ -231,6 +231,8 @@ public: std::map get_gps_ephemeris() const override; std::map get_galileo_ephemeris() const override; std::map get_beidou_dnav_ephemeris() const override; + std::map get_beidou_cnav1_ephemeris() const override; + std::map get_beidou_cnav2_ephemeris() const override; std::map get_gps_almanac() const override; std::map get_galileo_almanac() const override; std::map get_beidou_dnav_almanac() 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 435ba2bd5..ac6e56916 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -198,6 +198,7 @@ struct rtklib_pvt_gs::NavigationSnapshot using GalileoEphemerisMap = std::map; using GalileoAlmanacMap = std::map; using BeidouEphemerisMap = std::map; + using BeidouCnavEphemerisMap = std::map; using BeidouAlmanacMap = std::map; std::shared_ptr gps_cnav_ephemeris = std::make_shared(); @@ -209,6 +210,8 @@ struct rtklib_pvt_gs::NavigationSnapshot 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_cnav1_ephemeris = std::make_shared(); + std::shared_ptr beidou_cnav2_ephemeris = std::make_shared(); std::shared_ptr beidou_almanac = std::make_shared(); }; @@ -2104,6 +2107,11 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) { publish_navigation_snapshot(NavigationData::BeidouEphemeris); } + else if (msg_type_hash_code == d_beidou_cnav1_ephemeris_sptr_type_hash_code || + msg_type_hash_code == d_beidou_cnav1_page_data_sptr_type_hash_code) + { + publish_navigation_snapshot(NavigationData::BeidouCnavEphemeris); + } else if (msg_type_hash_code == d_beidou_dnav_almanac_sptr_type_hash_code) { publish_navigation_snapshot(NavigationData::BeidouAlmanac); @@ -2232,6 +2240,23 @@ void rtklib_pvt_gs::publish_navigation_snapshot(NavigationData data) case NavigationData::BeidouEphemeris: updated->beidou_ephemeris = std::make_shared(d_internal_pvt_solver->beidou_dnav_ephemeris_map); break; + case NavigationData::BeidouCnavEphemeris: + { + auto cnav1 = std::make_shared(d_internal_pvt_solver->beidou_cnav1_ephemeris_map); + // B-CNAV1 health may arrive on a later page without a new orbit. + // Use the same latest-page health as the PVT solver. + for (auto& entry : *cnav1) + { + const auto page = d_internal_pvt_solver->beidou_cnav1_page_data_map.find(entry.first); + if (page != d_internal_pvt_solver->beidou_cnav1_page_data_map.cend()) + { + entry.second.hs = page->second.common.hs; + } + } + updated->beidou_cnav1_ephemeris = std::move(cnav1); + updated->beidou_cnav2_ephemeris = std::make_shared(d_internal_pvt_solver->beidou_cnav2_ephemeris_map); + break; + } case NavigationData::BeidouAlmanac: updated->beidou_almanac = std::make_shared(d_internal_pvt_solver->beidou_dnav_almanac_map); break; @@ -2292,6 +2317,18 @@ std::map rtklib_pvt_gs::get_beidou_dnav_ephemeris_ma } +std::map rtklib_pvt_gs::get_beidou_cnav1_ephemeris_map() const +{ + return *navigation_snapshot()->beidou_cnav1_ephemeris; +} + + +std::map rtklib_pvt_gs::get_beidou_cnav2_ephemeris_map() const +{ + return *navigation_snapshot()->beidou_cnav2_ephemeris; +} + + std::map rtklib_pvt_gs::get_beidou_dnav_almanac_map() const { const auto snapshot = navigation_snapshot(); @@ -2310,6 +2347,9 @@ void clear_navigation_maps(Rtklib_Solver& solver, bool keep_almanac) solver.galileo_ephemeris_store.clear(); solver.galileo_reduced_ced_map.clear(); solver.beidou_dnav_ephemeris_map.clear(); + solver.beidou_cnav1_ephemeris_map.clear(); + solver.beidou_cnav2_ephemeris_map.clear(); + solver.beidou_cnav1_page_data_map.clear(); if (keep_almanac) { return; @@ -2353,6 +2393,8 @@ void rtklib_pvt_gs::request_navigation_clear(NavigationClear kind) almanac_only->glonass_ephemeris = d_empty_navigation_snapshot->glonass_ephemeris; almanac_only->galileo_ephemeris = d_empty_navigation_snapshot->galileo_ephemeris; almanac_only->beidou_ephemeris = d_empty_navigation_snapshot->beidou_ephemeris; + almanac_only->beidou_cnav1_ephemeris = d_empty_navigation_snapshot->beidou_cnav1_ephemeris; + almanac_only->beidou_cnav2_ephemeris = d_empty_navigation_snapshot->beidou_cnav2_ephemeris; replacement = std::move(almanac_only); } // A full clear requested before the worker has applied an diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 18b82719c..fc7bbd4a0 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -57,6 +57,7 @@ class Glonass_Gnav_Almanac; class Glonass_Gnav_Utc_Model; class Beidou_Dnav_Almanac; class Beidou_Dnav_Ephemeris; +class Beidou_Cnav1_Ephemeris; class Galileo_Almanac; class Galileo_Ephemeris; class Galileo_HAS_data; @@ -123,6 +124,8 @@ public: * \brief Get latest set of BeiDou DNAV ephemeris from PVT block */ std::map get_beidou_dnav_ephemeris_map() const; + std::map get_beidou_cnav1_ephemeris_map() const; + std::map get_beidou_cnav2_ephemeris_map() const; /*! * \brief Get latest set of BeiDou DNAV almanac from PVT block @@ -240,6 +243,7 @@ private: GalileoEphemeris, GalileoAlmanac, BeidouEphemeris, + BeidouCnavEphemeris, BeidouAlmanac, RetainedAlmanacs }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index fe2e31199..0dce788d0 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -161,14 +161,23 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, d_rtklib_freq_index[0] = 2; } - // B2a-only SPP uses RTKLIB slot 0. Default "5D" mapping is slot 2 - // (L1+L2+L5). RTKLIB BDS satwavelen frq2 is B3 (1268.52 MHz), not L5, - // so lam[0] is overridden to c/FREQ5 in get_PVT. This remap is gated on - // B2a-only: B1I/B1C/B3 present keeps the default dual-frequency map. - if (flags.check_only_enabled(BDS_B2A)) + // Resolve BeiDou slots independently of the other constellations. B2a + // owns slot 2 in a frequency pair; B3I uses the remaining slot when a + // B1 signal is enabled, or the primary slot for B3I+B2a reception. + if (flags.check_any_enabled(BDS_B2A)) { - d_rtklib_band_index["5D"] = 0; - d_rtklib_freq_index[0] = 2; + if (flags.check_any_enabled(BDS_B1, BDS_B1C)) + { + d_rtklib_band_index["B3"] = 1; + } + else if (flags.check_any_enabled(BDS_B3)) + { + d_rtklib_band_index["B3"] = 0; + } + else + { + d_rtklib_band_index["5D"] = 0; + } } // In automatic I/NAV mode E5a observations are not admitted to PVT, so @@ -1542,6 +1551,40 @@ void Rtklib_Solver::reset_relative_filter() } +void Rtklib_Solver::update_beidou_observation_wavelengths(const obsd_t &observation) +{ + if (satsys(observation.sat, nullptr) != SYS_BDS) + { + return; + } + for (int band = 0; band < NFREQ; ++band) + { + const unsigned char code = observation.code[band]; + double frequency = 0.0; + if (is_bds_b1c_code(code)) + { + frequency = FREQ1; + } + else if (is_bds_b2a_code(code)) + { + frequency = FREQ5; + } + else if (code == CODE_L6I || code == CODE_L6Q) + { + frequency = FREQ3_BDS; + } + else if (code == CODE_L2I || code == CODE_L1I) + { + frequency = FREQ1_BDS; + } + if (frequency > 0.0) + { + d_nav_data.lam[observation.sat - 1][band] = SPEED_OF_LIGHT_M_S / frequency; + } + } +} + + 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 @@ -1573,6 +1616,7 @@ int Rtklib_Solver::merge_duplicated_rover_observations(int rover_observation_cou continue; } obsd_t &destination = d_obs_data[match]; + bool overlapping_signal = false; for (int frequency = 0; frequency < NFREQ + NEXOBS; ++frequency) { const bool source_has_data = source.code[frequency] != CODE_NONE && @@ -1583,6 +1627,7 @@ int Rtklib_Solver::merge_duplicated_rover_observations(int rover_observation_cou } const bool destination_has_data = destination.code[frequency] != CODE_NONE && (destination.P[frequency] != 0.0 || destination.L[frequency] != 0.0); + overlapping_signal = overlapping_signal || destination_has_data; if (destination_has_data && destination.SNR[frequency] >= source.SNR[frequency]) { continue; @@ -1594,8 +1639,8 @@ int Rtklib_Solver::merge_duplicated_rover_observations(int rover_observation_cou destination.LLI[frequency] = source.LLI[frequency]; destination.code[frequency] = source.code[frequency]; } - merged_any = true; - if (!d_duplicated_rover_observations_logged) + merged_any = merged_any || overlapping_signal; + if (overlapping_signal && !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"; @@ -1946,8 +1991,10 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_fixed_base_age_s = 0.0; d_fixed_base_common_satellites = 0; - d_obs_data.fill({}); - std::vector eph_data(MAXOBS); + // Before merging, a multi-band satellite contributes several channel + // records and ephemerides. MAXOBS limits satellites, not input channels. + d_obs_data.assign(std::max(static_cast(MAXOBS * 2), gnss_observables_map.size()), obsd_t{}); + std::vector eph_data(std::max(static_cast(MAXOBS), gnss_observables_map.size())); std::vector geph_data(MAXOBS); for (gnss_observables_iter = gnss_observables_map.cbegin(); @@ -2722,25 +2769,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ update_galileo_observation_wavelengths(d_obs_data[i]); } - /* B1C on slot 0: override lam[0] to FREQ1 (satwavelen frq0 is B1I). */ for (int k = 0; k < nobs_total; k++) { - if (satsys(d_obs_data[k].sat, nullptr) != SYS_BDS) - { - continue; - } - const unsigned char c0 = d_obs_data[k].code[0]; - if (is_bds_b1c_code(c0)) - { - d_nav_data.lam[d_obs_data[k].sat - 1][0] = SPEED_OF_LIGHT_M_S / FREQ1; - } - for (int band = 0; band < NFREQ; ++band) - { - if (is_bds_b2a_code(d_obs_data[k].code[band])) - { - d_nav_data.lam[d_obs_data[k].sat - 1][band] = SPEED_OF_LIGHT_M_S / FREQ5; - } - } + update_beidou_observation_wavelengths(d_obs_data[k]); } const int configured_positioning_mode = d_rtk.opt.mode; if (use_single_fallback) diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index fe7482457..51c2e9acd 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -210,8 +210,7 @@ public: std::map beidou_cnav1_page_data_map; private: - friend class GalileoEphemerisSourceTest_E6SlotsFollowRtklibGalileoPolicy_Test; - + void update_beidou_observation_wavelengths(const obsd_t& observation); bool save_matfile() const; bool prepare_fixed_base_observations(const Ntrip_Rtcm_Snapshot& fixed_base, int& rover_observation_count, @@ -246,7 +245,7 @@ private: void reset_relative_filter(); - std::array d_obs_data{}; + std::vector d_obs_data = std::vector(MAXOBS * 2); // per-epoch scratch of prepare_fixed_base_observations(); members so // their capacity is reused across epochs std::vector d_fixed_base_rover_scratch; diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 1e6398f23..8ecda6e64 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -178,6 +178,10 @@ double gettgd_bds_by_obs_code(int sat, const nav_t *nav, unsigned char obs_code) int prn = 0; const int sys = satsys(sat, &prn); const int is_b1c_obs = is_bds_b1c_code(obs_code) ? 1 : 0; + if (sys == SYS_BDS && (obs_code == CODE_L6I || obs_code == CODE_L6Q)) + { + return 0.0; // B3I is the broadcast clock reference. + } for (i = 0; i < nav->n; i++) { @@ -320,46 +324,14 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS; - * BDS: B1I(band0)+B3I(band2). B1C (CODE_L1D/L1P/L1X) is single-frequency on - * slot 0 under GNSS-SDR prefer-B1C XOR (never paired as IF with B1I). */ + * BDS: select from the measured bands, preferring B2a as the IF partner. */ if (sys == SYS_GAL || sys == SYS_SBS) { j = 2; } else if (sys == SYS_BDS) { - const bool b1c0 = is_bds_b1c_code(obs->code[0]); - const bool b1c1 = is_bds_b1c_code(obs->code[1]); - const bool b2a0 = is_bds_b2a_code(obs->code[0]); - const bool b2a1 = is_bds_b2a_code(obs->code[1]); - if (b1c0 || b1c1) - { - i = b1c0 ? 0 : 1; - j = i; /* B1C single-frequency */ - } - else if (b2a0 || b2a1) - { - i = b2a0 ? 0 : 1; - j = i; /* B2a single-frequency */ - } - else if (obs->code[0] != CODE_NONE && obs->code[2] != CODE_NONE) - { - i = 0; - j = 2; /* B1I + B3I */ - } - else if (obs->code[0] != CODE_NONE) - { - i = 0; - j = 2; /* B1I-only: keep legacy pair indices for lam check */ - } - else if (obs->code[2] != CODE_NONE) - { - /* B3I-only: B3I is the DNAV timing reference, so use it - directly without TGD. rescode() scales the broadcast - ionosphere correction using this observation slot. */ - i = 2; - j = 2; - } + bds_observation_slots(obs, &i, &j); } else if (sys == SYS_GPS || sys == SYS_GLO || sys == SYS_QZS) { @@ -484,9 +456,11 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, /* iono-free combination */ if (sys == SYS_BDS) { - /* the DNAV clock is referenced to B3I: remove TGD1 from - the B1I pseudorange before combining (BDS SIS ICD) */ + /* DNAV/B-CNAV clocks use B3I as their delay reference. + Correct each signal with its own navigation-message bias + before combining (BDS B2a SIS ICD, section 7.6). */ P1 -= P1_P2; + P2 -= gettgd_bds_by_obs_code(obs->sat, nav, obs->code[j]); PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); } else if ((sys == SYS_GPS || sys == SYS_QZS) && obs->code[j] == CODE_L5X) diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.cc b/src/algorithms/libs/rtklib/rtklib_ppp.cc index 3d01f52b6..13b2985b0 100644 --- a/src/algorithms/libs/rtklib/rtklib_ppp.cc +++ b/src/algorithms/libs/rtklib/rtklib_ppp.cc @@ -969,7 +969,12 @@ int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel, j = 2; } - if (NFREQ < 2 || lam[i] == 0.0 || lam[j] == 0.0) + if (satsys(obs->sat, nullptr) == SYS_BDS) + { + bds_observation_slots(obs, &i, &j); + } + + if (NFREQ < 2 || i == j || lam[i] == 0.0 || lam[j] == 0.0 || lam[i] == lam[j]) { return 0; } @@ -1011,6 +1016,11 @@ int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel, { P2 += P2_C2; /* C2->P2 */ } + if (satsys(obs->sat, nullptr) == SYS_BDS) + { + P1 -= gettgd_bds_by_obs_code(obs->sat, nav, obs->code[i]); + P2 -= gettgd_bds_by_obs_code(obs->sat, nav, obs->code[j]); + } meas[1] = c1 * P1 + c2 * P2; var[1] = std::pow(ERR_CBIAS, 2.0); @@ -1195,17 +1205,24 @@ int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos, } -/* L1/L2 geometry-free phase measurement -------------------------------------*/ +/* geometry-free phase measurement ------------------------------------------*/ double gfmeas(const obsd_t *obs, const nav_t *nav) { const double *lam = nav->lam[obs->sat - 1]; + int i = 0; + int j = 1; - if (lam[0] == 0.0 || lam[1] == 0.0 || obs->L[0] == 0.0 || obs->L[1] == 0.0) + if (satsys(obs->sat, nullptr) == SYS_BDS) + { + bds_observation_slots(obs, &i, &j); + } + + if (i == j || lam[i] == 0.0 || lam[j] == 0.0 || obs->L[i] == 0.0 || obs->L[j] == 0.0) { return 0.0; } - return lam[0] * obs->L[0] - lam[1] * obs->L[1]; + return lam[i] * obs->L[i] - lam[j] * obs->L[j]; } @@ -1324,7 +1341,8 @@ void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n) for (i = 0; i < n && i < MAXOBS; i++) { - for (j = 0; j < rtk->opt.nf; j++) + // Dual-frequency measurements can occupy slots 0 and 2. + for (j = 0; j < NFREQ; j++) { if (obs[i].L[j] == 0.0 || !(obs[i].LLI[j] & 3)) { @@ -1366,7 +1384,7 @@ void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) trace(3, "detslip_gf: slip detected sat=%2d gf=%8.3f->%8.3f\n", obs[i].sat, g0, g1); - for (j = 0; j < rtk->opt.nf; j++) + for (j = 0; j < NFREQ; j++) { rtk->ssat[obs[i].sat - 1].slip[j] |= 1; } @@ -1381,6 +1399,7 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) double meas[2]; double var[2]; std::vector bias(MAXOBS, 0.0); + std::vector slip(MAXOBS, false); double offset = 0.0; double pos[3] = {0}; int i; @@ -1393,7 +1412,7 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) for (i = 0; i < MAXSAT; i++) { - for (j = 0; j < rtk->opt.nf; j++) + for (j = 0; j < NFREQ; j++) { rtk->ssat[i].slip[j] = 0; } @@ -1430,8 +1449,14 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) trace(2, "%s: sat=%2d correction break\n", time_str(obs[i].time, 0), sat); } bias[i] = meas[0] - meas[1]; - if (rtk->x[j] == 0.0 || - rtk->ssat[sat - 1].slip[0] || rtk->ssat[sat - 1].slip[1]) + int primary = 0; + int secondary = 1; + if (rtk->opt.ionoopt == IONOOPT_IFLC && satsys(sat, nullptr) == SYS_BDS) + { + bds_observation_slots(obs + i, &primary, &secondary); + } + slip[i] = brk || rtk->ssat[sat - 1].slip[primary] || rtk->ssat[sat - 1].slip[secondary]; + if (rtk->x[j] == 0.0 || slip[i]) { continue; } @@ -1459,8 +1484,7 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) rtk->P[j + j * rtk->nx] += std::pow(rtk->opt.prn[0], 2.0) * fabs(rtk->tt); - if (rtk->x[j] != 0.0 && - !rtk->ssat[sat - 1].slip[0] && !rtk->ssat[sat - 1].slip[1]) + if (rtk->x[j] != 0.0 && !slip[i]) { continue; } diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 2a6431194..60eb420b0 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -670,6 +670,46 @@ bool is_bds_b2a_code(unsigned char code) } +/* Select the first measured band and prefer B2a as its IF partner. Without + * B2a, retain the B1C single-band and legacy B1I+B3I policies. */ +void bds_observation_slots(const obsd_t *obs, int *primary, int *secondary) +{ + *primary = 0; + *secondary = 0; + int b2a = -1; + bool found = false; + for (int band = 0; band < NFREQ; ++band) + { + if (obs->code[band] == CODE_NONE || obs->P[band] == 0.0) + { + continue; + } + if (!found) + { + *primary = band; + *secondary = band; + found = true; + } + else + { + *secondary = band; + } + if (is_bds_b2a_code(obs->code[band])) + { + b2a = band; + } + } + if (b2a >= 0 && b2a != *primary) + { + *secondary = b2a; + } + else if (b2a < 0 && is_bds_b1c_code(obs->code[*primary])) + { + *secondary = *primary; + } +} + + /* set code priority ----------------------------------------------------------- * set code priority for multiple codes in a frequency * args : int sys I system (or of SYS_???) diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h index d673c2306..0a570eca9 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h @@ -127,6 +127,7 @@ unsigned char obs2code(const char *obs, int *freq); char *code2obs(unsigned char code, int *freq); bool is_bds_b1c_code(unsigned char code); bool is_bds_b2a_code(unsigned char code); +void bds_observation_slots(const obsd_t *obs, int *primary, int *secondary); void setcodepri(int sys, int freq, const char *pri); int getcodepri(int sys, unsigned char code, const char *opt); unsigned int getbitu(const unsigned char *buff, int pos, int len); diff --git a/src/core/interfaces/pvt_interface.h b/src/core/interfaces/pvt_interface.h index cf8f7387b..0941b0efb 100644 --- a/src/core/interfaces/pvt_interface.h +++ b/src/core/interfaces/pvt_interface.h @@ -23,6 +23,7 @@ #ifndef GNSS_SDR_PVT_INTERFACE_H #define GNSS_SDR_PVT_INTERFACE_H +#include "beidou_cnav1_ephemeris.h" #include "beidou_dnav_almanac.h" #include "beidou_dnav_ephemeris.h" #include "galileo_almanac.h" @@ -68,6 +69,10 @@ public: 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; + // Optional navigation families: implementations without B-CNAV support + // retain the existing DNAV/almanac visibility path. + virtual std::map get_beidou_cnav1_ephemeris() const { return {}; } + virtual std::map get_beidou_cnav2_ephemeris() const { return {}; } virtual std::map get_gps_almanac() const = 0; virtual std::map get_galileo_almanac() const = 0; virtual std::map get_beidou_dnav_almanac() const = 0; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index fbdae288d..f7f1b5080 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -2499,19 +2499,13 @@ bool GNSSFlowgraph::is_multiband() const multiband = true; } } - if (configuration_->property("Channels_B1.count", 0) > 0) + const bool has_bds_b1 = configuration_->property("Channels_B1.count", 0) > 0 || + configuration_->property("Channels_1D.count", 0) > 0; + const bool has_bds_b3 = configuration_->property("Channels_B3.count", 0) > 0; + const bool has_bds_b2a = configuration_->property("Channels_5D.count", 0) > 0; + if ((has_bds_b1 && (has_bds_b3 || has_bds_b2a)) || (has_bds_b3 && has_bds_b2a)) { - if (configuration_->property("Channels_B3.count", 0) > 0) - { - multiband = true; - } - } - if (configuration_->property("Channels_1D.count", 0) > 0) - { - if (configuration_->property("Channels_B3.count", 0) > 0) - { - multiband = true; - } + multiband = true; } if (configuration_->property("Channels_J1.count", 0) > 0) { @@ -2572,12 +2566,15 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal assist_signal_candidates = {"B1", "1D"}; break; + case evBDS_B2A: + assist_signal_candidates = {"B1", "1D", "B3"}; + break; + case evGPS_1C: case evGAL_1B: case evGLO_1G: case evBDS_B1: case evBDS_B1C: - case evBDS_B2A: case evQZS_J1: case evSBAS_1C: is_primary_frequency = true; @@ -2594,6 +2591,15 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal const bool any_assist_configured = std::any_of(assist_signal_candidates.begin(), assist_signal_candidates.end(), [&](const std::string& assist_signal) { return configuration_->property("Channels_" + assist_signal + ".count", 0) > 0; }); + // A BeiDou band without an enabled assisting band must acquire on its + // own, even when another constellation makes the receiver multiband. + // In B3I+B2a, B3I starts first and then assists B2a. + if (!any_assist_configured && (searched_signal == "5D" || searched_signal == "B3")) + { + assist_signal_candidates.clear(); + is_primary_frequency = true; + } + if (any_assist_configured) { // 1. Get the current channel tracking map: locked channels, with or without a diff --git a/src/core/receiver/satellite_visibility.cc b/src/core/receiver/satellite_visibility.cc index a7a83e7ee..b9a548ea0 100644 --- a/src/core/receiver/satellite_visibility.cc +++ b/src/core/receiver/satellite_visibility.cc @@ -15,6 +15,7 @@ */ #include "satellite_visibility.h" +#include "Beidou_CNAV2.h" #include "GLONASS_L1_L2_CA.h" // for GLONASS_PRN #include "agnss_ref_location.h" // for parse_agnss_ref_location #include "agnss_ref_time.h" // for parse_agnss_ref_utc_time @@ -116,7 +117,7 @@ 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", "5D"}}, {"Glonass", {"1G", "2G"}}, {"QZSS", {"J1", "J5"}}}; for (const auto& entry : system_signals) @@ -295,42 +296,82 @@ std::vector> compute_visible_satellites( } } - const std::map bds_eph_map = pvt_ptr->get_beidou_dnav_ephemeris(); + // Classify each BeiDou satellite once, from the usable orbit closest to + // the query epoch. Equal-age records retain DNAV, then CNAV1, then CNAV2 + // priority, independent of telemetry arrival order. Almanac remains the + // fallback when no fresh ephemeris is available. + std::map bds_eph_map; + const auto add_bds_ephemeris = [&](uint32_t prn, const eph_t& ephemeris) { + if (only_prns != nullptr && only_prns->count(std::make_pair(std::string("Beidou"), prn)) == 0) + { + return; + } + const double age = timediff(gps_gtime, ephemeris.toe); + if (!std::isfinite(age) || std::abs(age) > MAXDTOE_BDS || + !std::isfinite(ephemeris.A) || ephemeris.A <= 0.0 || + !std::isfinite(ephemeris.e) || ephemeris.e < 0.0 || ephemeris.e >= 1.0) + { + return; + } + const auto previous = bds_eph_map.find(prn); + if (previous == bds_eph_map.cend() || + std::abs(age) < std::abs(timediff(gps_gtime, previous->second.toe))) + { + bds_eph_map[prn] = ephemeris; + } + }; + for (const auto& entry : pvt_ptr->get_beidou_dnav_ephemeris()) + { + add_bds_ephemeris(entry.second.PRN, eph_to_rtklib(entry.second)); + } + const std::array, 2> bds_cnav_maps{ + {pvt_ptr->get_beidou_cnav1_ephemeris(), pvt_ptr->get_beidou_cnav2_ephemeris()}}; + for (size_t source = 0; source < bds_cnav_maps.size(); ++source) + { + const int expected_source = source == 0 ? BDS_EPH_SOURCE_CNAV1 : BDS_EPH_SOURCE_CNAV2; + for (const auto& entry : bds_cnav_maps[source]) + { + const auto& ephemeris = entry.second; + if (ephemeris.sig_type == expected_source && + (ephemeris.sat_type == 2 || ephemeris.sat_type == 3)) + { + add_bds_ephemeris(ephemeris.PRN, eph_to_rtklib(ephemeris)); + } + } + } for (const auto& it : bds_eph_map) { - if (only_prns != nullptr && only_prns->count(std::make_pair(std::string("Beidou"), 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_BDS) - { - continue; // stale -- treat as if no ephemeris exists; almanac may still classify it - } - note_freshness(age, MAXDTOE_BDS); + const eph_t& rtklib_eph = it.second; 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); + if (!std::all_of(r_sat.cbegin(), r_sat.cend(), [](double value) { return std::isfinite(value); })) + { + continue; + } 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_bds.push_back(it.second.PRN); - if (El > elevation_mask_deg && it.second.SV_health == 0) + if (!std::isfinite(El)) { - 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))); + continue; + } + note_freshness(timediff(gps_gtime, rtklib_eph.toe), MAXDTOE_BDS); + handled_bds.push_back(it.first); + if (El > elevation_mask_deg && rtklib_eph.svh == 0) + { + LOG(INFO) << "Using BeiDou Ephemeris (source " << rtklib_eph.code << "): Sat " << it.first << " Az: " << Az << " El: " << El; + available_satellites.emplace_back(floor(El), Gnss_Satellite(std::string("Beidou"), it.first)); } else if (below_mask_out != nullptr) { - 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)); + LOG(INFO) << "Using BeiDou Ephemeris (source " << rtklib_eph.code << ", excluded): Sat " << it.first << " Az: " << Az << " El: " << El; + below_mask_out->emplace_back(floor(El), Gnss_Satellite(std::string("Beidou"), it.first)); } } @@ -566,9 +607,11 @@ bool SatelliteVisibility::DataChanged(const std::shared_ptr& pvt_p { // 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)); + std::map, NavigationFingerprint> current; + auto add = [¤t](const char* system, const char* type, uint32_t prn, double ref_time, int32_t health, + uint32_t iode = 0, uint32_t iodc = 0, int32_t sat_type = 0, int32_t signal_type = 0) { + current.emplace(std::make_tuple(std::string(system), std::string(type), prn), + std::make_tuple(ref_time, health, iode, iodc, sat_type, signal_type)); }; for (const auto& it : pvt_ptr->get_gps_ephemeris()) { @@ -582,6 +625,18 @@ bool SatelliteVisibility::DataChanged(const std::shared_ptr& pvt_p { add("Beidou", "EPH", it.second.PRN, it.second.WN * 604800.0 + it.second.toe, it.second.SV_health); } + const std::array, 2> bds_cnav_maps{ + {pvt_ptr->get_beidou_cnav1_ephemeris(), pvt_ptr->get_beidou_cnav2_ephemeris()}}; + for (size_t source = 0; source < bds_cnav_maps.size(); ++source) + { + for (const auto& it : bds_cnav_maps[source]) + { + const auto& ephemeris = it.second; + add("Beidou", source == 0 ? "CNAV1" : "CNAV2", ephemeris.PRN, + ephemeris.WN * 604800.0 + ephemeris.toe, ephemeris.hs, + ephemeris.IODE, ephemeris.IODC, ephemeris.sat_type, ephemeris.sig_type); + } + } for (const auto& it : pvt_ptr->get_gps_almanac()) { 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); diff --git a/src/core/receiver/satellite_visibility.h b/src/core/receiver/satellite_visibility.h index c51179a19..cb355cdc5 100644 --- a/src/core/receiver/satellite_visibility.h +++ b/src/core/receiver/satellite_visibility.h @@ -46,7 +46,9 @@ class PvtInterface; * 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 + * exactly one data source. BeiDou uses the fresh DNAV/CNAV1/CNAV2 orbit + * closest to the query epoch (ties: DNAV, CNAV1, CNAV2); B-CNAV is limited + * to IGSO/MEO satellite types. 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. * @@ -230,10 +232,12 @@ private: double last_fix_time_s_{-1.0}; double last_fix_receiver_time_s_{0.0}; - // (system, "EPH"/"ALM"/"CNAV", PRN) -> (toe/toa as absolute seconds, - // health). Catches a PRN's data being replaced, not only new PRNs. + // (system, navigation family, PRN) -> (absolute toe/toa, health, + // IODE, IODC, satellite type, signal type). Extra fields are zero for + // legacy navigation families. Detect replacements as well as new PRNs. // first_data_check_ tells "never checked" from "checked, still empty". - std::map, std::pair> last_data_fingerprints_; + using NavigationFingerprint = std::tuple; + std::map, NavigationFingerprint> last_data_fingerprints_; // Absolute GPST seconds (gtime_t.time + sec) of the last full sweep: // monotonic across week rollover, and receiver time keeps the interval