diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index a26e2978a74f63e1d80063545386d1e19c7f76ae..2cb5a773cd2e3c61972376e561f46f7933c72387 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -110,15 +110,19 @@ void ProcessorTrackerGnss::preProcess() untracked_incoming_features_[feat->satNumber()] = feat; } - std::string discarded_str; +#ifdef _WOLF_DEBUG + std::string discarded_str, detected_str; for (auto sat : fix_incoming_.discarded_sats) discarded_str += std::to_string(sat) + " "; + for (auto ftr_pair : untracked_incoming_features_) + detected_str += std::to_string(ftr_pair.first) + " "; +#endif WOLF_DEBUG("ProcessorTrackerGnss::preProcess()", "\n\tinitial observations: ", n_initial, "\n\tRTKLIB discarded: ", fix_incoming_.discarded_sats.size(), " (", discarded_str, ")", "\n\tgnssutils discarded: ", n_initial - untracked_incoming_features_.size() - fix_incoming_.discarded_sats.size(), - "\n\tdetected incoming features: ", untracked_incoming_features_.size()); + "\n\tdetected incoming features: ", untracked_incoming_features_.size(), " (", detected_str, ")"); } unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _features_in, @@ -140,6 +144,8 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat int sat_num = std::static_pointer_cast<FeatureGnssSatellite>(feat_in)->satNumber(); + WOLF_DEBUG("tracking " , feat_in->trackId() , ", sat number ", sat_num); + 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 { @@ -148,7 +154,12 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat _features_out.push_back(ftr); _feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0})); - WOLF_DEBUG("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" ); + WOLF_DEBUG("\ttracked: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" ); + } + else + { + WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 0, "\tnot tracked, missing satellite" ); + WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 1, "\tnot tracked, wrong carrier phase value" ); } }