1 files changed, 43 insertions, 9 deletions

diff --git a/src/main.cpp b/src/main.cpp
index 150a141..44e09f3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -32,12 +32,15 @@ imgCallback(const message *msg)
     return;
 }
 void 
-imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q)
+imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt)
 {
-    if (!seenutm || !seenpva) return;
+    if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
     using Eigen::Vector3d;
     Vector3d acc(msg.linear_acceleration.x,msg.linear_acceleration.y,msg.linear_acceleration.z);
     Vector3d ang(msg.angular_velocity.x,msg.angular_velocity.y,msg.angular_velocity.z);
+    Body mu;
+    double dtf = dt.secs+dt.nsecs*1e-9;
+    mu.motionModel(X,acc,ang,q,dtf);
 }
 
 void
@@ -66,10 +69,12 @@ pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<
 void
 utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X)
 {
-    seenutm=true;
-    X[0] = msg.utm.easting;
-    X[1] = msg.utm.northing;
-    X[2] = msg.utm.up;
+    if (!seenutm) {
+        seenutm=true;
+        X[0] = msg.utm.easting;
+        X[1] = msg.utm.northing;
+        X[2] = msg.utm.up;
+    }
     return;
 }
 #endif     /* -----  not USE_ROS  ----- */
@@ -136,10 +141,34 @@ parseLine(char *line, message *data)
     }
 }
 
-int main(int argc, char **argv)
+timestamp 
+update_dt(const timestamp t, timestamp *t_old)
+{
+    double dtf=0;
+    if (t_old->secs!=0 || t_old->nsecs!=0) {
+        double tf = t.secs+1e-9*t.nsecs;
+        double t_oldf = t_old->secs+1e-9*t_old->nsecs;
+        dtf = tf-t_oldf;
+    }
+    t_old->secs=t.secs;
+    t_old->nsecs=t.nsecs;
+
+    timestamp dt;
+    dt.secs = (int) dtf;
+    dt.nsecs = (int)((dtf-(double)dt.secs)*1e9);
+    
+    return dt;
+}
+
+int 
+main(int argc, char **argv)
 {
     Eigen::Quaternion<double> qbw;
     Eigen::Matrix<double,9,1> bodyStateVector;
+    bodyStateVector.segment(6,3) << 0.01465135, -0.03713532, -0.00709229;
+    timestamp dt{0,0};
+    timestamp t_old{0,0};
+
 #ifdef  USE_ROS
     
 #else      /* -----  not USE_ROS  ----- */
@@ -151,6 +180,7 @@ while (scanf("%s", line)!=EOF) {
     
     switch ( msg.msg_type ) {
         case BESTUTM:	
+            dt = update_dt(msg.stamp, &t_old);
             utmCallback(msg,bodyStateVector);
             break;
 
@@ -159,12 +189,16 @@ while (scanf("%s", line)!=EOF) {
             break;
 
         case INSPVAS:	
+            dt = update_dt(msg.stamp, &t_old);
             pvaCallback(msg, bodyStateVector, qbw);
-            std::cout << bodyStateVector << std::endl;
+            //std::cout << bodyStateVector.transpose() << std::endl;
+            if (seenutm)
+                printf("%d,%c,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,bodyStateVector[1],bodyStateVector[0]);
             break;
 
         case RAWIMUS:	
-            imuCallback(msg, bodyStateVector, qbw);
+            dt = update_dt(msg.stamp, &t_old);
+            imuCallback(msg, bodyStateVector, qbw, dt);
             break;
 
         default: