diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h index b0eb4b5f034fa9ac9b84fec4a805005610e6bc59..b4aa048f1d022d581b83a47b6fba0888427c01da 100644 --- a/include/gnss/processor/processor_tracker_gnss.h +++ b/include/gnss/processor/processor_tracker_gnss.h @@ -1,6 +1,7 @@ #ifndef PROCESSOR_TRACKER_GNSS_H_ #define PROCESSOR_TRACKER_GNSS_H_ +#include "gnss/internal/config.h" #include "core/common/wolf.h" #include "core/processor/processor_tracker_feature.h" #include "gnss/sensor/sensor_gnss.h" @@ -23,6 +24,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature double outlier_residual_th; bool init_frames, pseudo_ranges; double enu_map_fix_dist; + int min_sbas_sats; ParamsProcessorTrackerGnss() = default; ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server): @@ -35,6 +37,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span"); enu_map_fix_dist = _server.getParam<double> (prefix + _unique_name + "/enu_map_fix_dist"); pseudo_ranges = _server.getParam<bool> (prefix + _unique_name + "/pseudo_ranges"); + min_sbas_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/min_sbas_sats"); // GNSS OPTIONS (see rtklib.h) gnss_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC) diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index 0fba9bd5e12a7ce7854a6b2579eadfe720cbdcf9..c7828030cf94a9b2c0ac258acaff896225c64d5a 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -88,6 +88,7 @@ void ProcessorTrackerGnss::preProcess() WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose()); // 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 @@ -102,6 +103,57 @@ void ProcessorTrackerGnss::preProcess() fix_incoming_.lat_lon, params_tracker_gnss_->gnss_opt); + /* NOT ENOUGH SATS/RANGES in case of: + * 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)) + { + WOLF_INFO("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 (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; + + // reset observations + inc_snapshot->setObservations(init_obs); + // recompute satellites + inc_snapshot ->computeSatellites(params_tracker_gnss_->gnss_opt.sateph); + // filter + #ifdef _WOLF_DEBUG + discarded_gnssutils = + #endif + inc_snapshot->filterObservations(fix_incoming_.discarded_sats, // discarded sats + fix_incoming_.sat_azel, + false, // check code + false, // check carrier phase + params_tracker_gnss_->gnss_opt); + + + // recompute corrected Ranges + inc_snapshot->computeRanges(fix_incoming_.sat_azel, + fix_incoming_.lat_lon, + params_tracker_gnss_->gnss_opt); + + WOLF_INFO("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 for (auto obs_sat : inc_snapshot->getObservations()->getObservations()) {