changes

1 files changed, 10 insertions, 4 deletions

diff --git a/src/main.cpp b/src/main.cpp
index 11bf6dd..fa0b5fd 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -57,6 +57,7 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
 #endif
 {
     if (seencov && seenutm && seenpva) return;
+    if (!seenpva) return;
     seencov=true;
     // Rotation from body to world
 #if STATESIZE==13
@@ -153,7 +154,7 @@ imuCallback(const message &msg, State &mu, const timestamp dt)
 imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt)
 #endif
 {
-    if (!seenutm || !seenpva || !seencov || (dt.secs==0 && dt.nsecs==0)) return;
+    if (dt.secs==0 && dt.nsecs==0) return;
     Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z);
     Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z);
     // WARNING: This is the bias for 1112-1
@@ -190,6 +191,7 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q)
       * AngleAxisd(roll, Vector3d::UnitX())
       *  AngleAxisd(pitch, Vector3d::UnitY());
     q = Quaterniond(R);
+    //cout <<q.x() <<" " << q.y() << " " << q.z() << " " << q.w() << endl;
 
     // Set body velocity
     if (!seenpva || !seencov || !seenutm) {
@@ -396,7 +398,11 @@ main(int argc, char **argv)
     ros::Publisher ekfPose = n.advertise<geometry_msgs::PoseStamped>("ekfPose", 100);
 #else      /* -----  not ROS_PUBLISH  ----- */
     // Print unicsv header
-    printf("GPS,UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
+#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");
+#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
 #endif     /* -----  not ROS_PUBLISH  ----- */
     // Read sensors from file
     int i=0;
@@ -412,8 +418,7 @@ main(int argc, char **argv)
 #else
                 utmCallback(msg, mu,qbw);
 #endif
-                printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
-                mu.unicsv();
+                //printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
                 break;
 
             case IMG:	
@@ -426,6 +431,7 @@ main(int argc, char **argv)
 
             case INSPVAS:	
                 pvaCallback(msg, mu, qbw);
+                if (seenutm) mu.unicsv();
                 break;
 
             case RAWIMUS: