diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index a8e7724762670b3a4c9884b8b0dcf84d0e7dfcbe..0fba9bd5e12a7ce7854a6b2579eadfe720cbdcf9 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -146,6 +146,8 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat assert(_capture == incoming_ptr_); + int common_sats = 0; + for (auto feat_in : _features_in) { auto feat_in_gnss = std::dynamic_pointer_cast<FeatureGnssSatellite>(feat_in); @@ -155,6 +157,9 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat WOLF_DEBUG("tracking " , feat_in->trackId() , ", sat number ", sat_num); + if (untracked_incoming_features_.count(sat_num) != 0) + common_sats++; + if (untracked_incoming_features_.count(sat_num) != 0 and std::abs(untracked_incoming_features_.at(sat_num)->getObservation().L[0]) > 1e-12) // Track only carrier phase valid { @@ -171,7 +176,9 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 1, "\tnot tracked, wrong carrier phase value" ); } } - + WOLF_WARN_COND(_features_out.empty() and not _features_in.empty(), + "ProcessorTrackerGnss::trackFeatures: LOST TRACK OF ALL SATELLITES of ", _features_in.size(), + " - ", common_sats, " wrong Carrier Phase info."); WOLF_DEBUG("ProcessorTrackerGnss::trackFeatures: tracked " , _features_out.size(), " (of ", _features_in.size(), ")"); return _features_out.size(); @@ -680,9 +687,13 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas //assert(false); fac->remove(); } - WOLF_INFO("ProcessorTrackerGnss::removeOutliers:", - "\n\tPseudorange: ", outliers_pseudorange_, "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)"); - if (params_tracker_gnss_->gnss_opt.tdcp.enabled and params_tracker_gnss_->remove_outliers_tdcp) + WOLF_INFO_COND(not remove_fac.empty(), + "ProcessorTrackerGnss::removeOutliers:", + "\n\tPseudorange: ", outliers_pseudorange_, + "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)"); + if (not remove_fac.empty() and + params_tracker_gnss_->gnss_opt.tdcp.enabled and + params_tracker_gnss_->remove_outliers_tdcp) std::cout << "\tTDCP: " << outliers_tdcp_ << "\t( "