debugging

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" );
         }
     }