Nice quatviz-2 result

1 files changed, 5 insertions, 3 deletions

diff --git a/src/main.cpp b/src/main.cpp
index bd31da9..57960dd 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -443,7 +443,7 @@ main(int argc, char **argv)
 #else      /* -----  not ROS_PUBLISH  ----- */
     // Print unicsv header
 #if STATESIZE==13
-    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Quat-X,Quat-Y,Quat-Z,Quat-W,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
+    printf("GPS-stamp,UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Quat-X,Quat-Y,Quat-Z,Quat-W,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
 #else
     printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
 #endif
@@ -463,6 +463,10 @@ main(int argc, char **argv)
                 utmCallback(msg, mu,qbw);
 #endif
                 //printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
+                if (seenutm && seenpva && seencov) {
+                    printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
+                    mu.unicsv();
+                }
                 break;
 
             case IMG:	
@@ -491,8 +495,6 @@ main(int argc, char **argv)
                         geometry_msgs::PoseStamped msg;
                         ros::spinOnce();
 #else
-                    if (seenutm) mu.unicsv();
-                        //mu.unicsv();
 #endif
                     }
                 }