diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index c3ea38b027a9542a2d713eb5375297783b7ac514..57b37da5eda205dfd3f6ff5f9409942725deb9fe 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -27,6 +27,11 @@ void ProcessorTrackerGnss::preProcess() WOLF_DEBUG("ProcessorTrackerGnss::preProcess"); GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot(); + GnssUtils::Options copy_opt = params_tracker_gnss_->gnss_opt; + bool eph_sbas34 = params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS3 or + params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS4; + bool iono_sbas34 = params_tracker_gnss_->gnss_opt.ionopt == IONOOPT_SBAS3 or + params_tracker_gnss_->gnss_opt.ionopt == IONOOPT_SBAS4; #ifdef _WOLF_DEBUG int n_initial = inc_snapshot->getObservations()->size(); @@ -34,10 +39,14 @@ void ProcessorTrackerGnss::preProcess() for (auto obs : inc_snapshot->getObservations()->getObservations()) initial_str += std::to_string(obs.sat) + " "; #endif + // overload SBAS3 and SBAS4 first chance + if (eph_sbas34) + copy_opt.sateph = EPHOPT_SBAS; + if (iono_sbas34) + copy_opt.ionoopt = IONOPT_SBAS; // compute satellites positions - if (!inc_snapshot ->satellitesComputed()) - inc_snapshot ->computeSatellites(params_tracker_gnss_->gnss_opt.sateph); + inc_snapshot ->computeSatellites(copy_opt); /* iono, tropo, eph and sbas options are the less restrictive to avoid RTKLIB to discard satellites * corrections are applied in computeRanges, this fix is only used for: @@ -87,8 +96,12 @@ void ProcessorTrackerGnss::preProcess() WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose()); + // store initial observation before filtering + GnssUtils::ObservationsPtr init_obs; + if (eph_sbas34 or iono_sbas34) + init_obs = std::make_shared<GnssUtils::Observations>(*inc_snapshot->getObservations()); + // filter observations (available ephemeris, constellations and elevation&SNR) - auto init_obs = std::make_shared<GnssUtils::Observations>(*inc_snapshot->getObservations()); #ifdef _WOLF_DEBUG auto discarded_gnssutils = #endif @@ -96,42 +109,36 @@ void ProcessorTrackerGnss::preProcess() fix_incoming_.sat_azel, false, // check code false, // check carrier phase - params_tracker_gnss_->gnss_opt); + copy_opt); // compute corrected Ranges inc_snapshot->computeRanges(fix_incoming_.sat_azel, fix_incoming_.lat_lon, params_tracker_gnss_->gnss_opt); - /* NOT ENOUGH SATS/RANGES in case of: + /* NOT ENOUGH SATS/RANGES in case of sbas34: * eph: EPHOPT_SBAS3 and EPHOPT_SBAS4 * iono: IONOOPT_SBAS3 and IONOOPT_SBAS4 */ if (inc_snapshot->getObservations()->size() < params_tracker_gnss_->min_sbas_sats and - (params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS3 or - params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS4 or - params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS3 or - params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS4)) + (eph_sbas34 or iono_sbas34)) { WOLF_DEBUG("ProcessorTrackerGnss::preProcess: with SBAS3/SBAS4, not enough ranges: ", inc_snapshot->getObservations()->size(), " Computing with SBAS2/BRDC"); - int eph_opt_old = params_tracker_gnss_->gnss_opt.sateph; - int iono_opt_old = params_tracker_gnss_->gnss_opt.ionoopt; - - if (eph_opt_old == EPHOPT_SBAS3) - params_tracker_gnss_->gnss_opt.sateph = EPHOPT_SBAS2; - if (eph_opt_old == EPHOPT_SBAS4) - params_tracker_gnss_->gnss_opt.sateph = EPHOPT_BRDC; + if (params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS3) + copy_opt.sateph.sateph = EPHOPT_SBAS2; + if (params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS4) + copy_opt.sateph.sateph = EPHOPT_BRDC; - if (iono_opt_old == IONOOPT_SBAS3) - params_tracker_gnss_->gnss_opt.ionoopt = IONOOPT_SBAS2; - if (iono_opt_old == IONOOPT_SBAS4) - params_tracker_gnss_->gnss_opt.ionoopt = IONOOPT_BRDC; + if (params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS3) + copy_opt.ionoopt = IONOOPT_SBAS2; + if (params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS4) + copy_opt.ionoopt = IONOOPT_BRDC; // reset observations inc_snapshot->setObservations(init_obs); // recompute satellites - inc_snapshot ->computeSatellites(params_tracker_gnss_->gnss_opt.sateph); + inc_snapshot ->computeSatellites(copy_opt.sateph); // filter #ifdef _WOLF_DEBUG discarded_gnssutils = @@ -140,18 +147,15 @@ void ProcessorTrackerGnss::preProcess() fix_incoming_.sat_azel, false, // check code false, // check carrier phase - params_tracker_gnss_->gnss_opt); + copy_opt); // recompute corrected Ranges inc_snapshot->computeRanges(fix_incoming_.sat_azel, fix_incoming_.lat_lon, - params_tracker_gnss_->gnss_opt); + copy_opt); WOLF_DEBUG("ProcessorTrackerGnss::preProcess: with SBAS2/BRDC, obtained ranges: ", inc_snapshot->getObservations()->size()); - - params_tracker_gnss_->gnss_opt.sateph = eph_opt_old; - params_tracker_gnss_->gnss_opt.ionoopt = iono_opt_old; } // create features pseudorange