debugging

src/subscriber_gnss_receiver.cpp

@@ -241,6 +241,8 @@ void SubscriberGnssReceiver::publishFixComp()
 
     GnssUtils::Options comp_pos_opt = compute_pos_opt_;
 
+    WOLF_DEBUG("COMPARING RTKLIB pntpos CONFIGURATIONS /////////////////");
+
     for (auto comp : comp_options_)
     {
         if (publishers_pose_.at(comp).getNumSubscribers() == 0 and
@@ -300,11 +302,32 @@ void SubscriberGnssReceiver::publishFixComp()
         else if (comp[2] == '2')
             comp_pos_opt.ionoopt    = IONOOPT_IFLC;
 
+        WOLF_DEBUG("opt ", comp, ": ",
+                   " RAIM: ", comp_pos_opt.raim,
+                   " EPH:  ", comp_pos_opt.sateph,
+                   " IONO: ", comp_pos_opt.ionoopt,
+                   " TROP: ", comp_pos_opt.tropopt)
+
         //std::cout << "//////////// Computing fix comp = " << comp << std::endl;
         auto fix = GnssUtils::computePos(receiver_->getObservations(),
                                          receiver_->getNavigation(),
                                          comp_pos_opt);
-        WOLF_INFO_COND(not fix.success, "computePos failed (opt ", comp, ") with msg: ", fix.msg);
+
+        // print
+        #ifdef _WOLF_DEBUG
+            std::string discarded_str, used_str;
+            for (auto sat : fix.discarded_sats)
+                discarded_str += std::to_string(sat) + " ";
+            for (auto sat : fix.used_sats)
+                used_str += std::to_string(sat) + " ";
+        #endif
+
+        WOLF_INFO_COND(not fix.success, "computePos failed (opt ", comp, ") with msg: ", fix.msg,
+                                        "\n\tused sats: ", fix.used_sats.size(), " (", used_str, ")",
+                                        "\n\tdiscarded sats: ", fix.discarded_sats.size(), " (", discarded_str, ")");
+        WOLF_DEBUG_COND(fix.success, "computePos succeed (opt ", comp, ")",
+                                     "\n\tused sats: ", fix.used_sats.size(), " (", used_str, ")",
+                                     "\n\tdiscarded sats: ", fix.discarded_sats.size(), " (", discarded_str, ")");
 
         //std::cout << "//////////// Computed" << std::endl;
 
@@ -327,6 +350,7 @@ void SubscriberGnssReceiver::publishFixComp()
         last_fixes_ok_.at(comp) = fix.success;
         last_fixes_pose_.at(comp) = pose_msg;
     }
+    WOLF_DEBUG("////////////////////////////////////////////////");
 }
 
 void SubscriberGnssReceiver::fillMsgs(const geometry_msgs::Pose pose, const std::string& comp)