Fix sampling.

diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp
index 0571921..0be6e05 100644 (file)
--- a/test/ekf_test.cpp
+++ b/test/ekf_test.cpp
@@ -20,6 +20,7 @@
 *******************************************************************************/
 
 #include <unistd.h>
+#include <sys/time.h>
 
 #include <iostream>
 
@@ -84,8 +85,24 @@ int main(int argc,char *argv[])
        const double T=0.01;
        imufusion::Ekf ekf(q0,P0,Pw,Pv,T);
        
+       struct timeval tv0;
+       gettimeofday(&tv0,NULL);
+       double t1=0.0;
+       
        for(;;)
        {
+               struct timeval tv;
+               double t;
+               int s=0;
+               if(t1 > 0.0) do
+               {
+                       if(s > 0) usleep(s);
+                       gettimeofday(&tv,NULL);
+                       t=(tv.tv_sec-tv0.tv_sec)*1e6+tv.tv_usec-tv0.tv_usec;
+                       s=(int)(T*1e6-(t-t1)-120);
+               }
+               while(t < t1+T*1e6);
+
                try
                {
                
@@ -101,10 +118,12 @@ int main(int argc,char *argv[])
                                << "Magnetometer=" << mag.transpose() << " uT" << std::endl
                                << "Gyroscope=" << gyro.transpose() << " rad/s" << std::endl;
 
+                       Eigen::Quaterniond qEkf=ekf.quaternion();
+
                        Eigen::Quaterniond q=imu->quaternion();
-                       Eigen::Quaterniond qEkf;//=ekf.quaternion();
-                       std::cout << "Quaternion=" << q.w() << " " << q.vec().transpose() << std::endl
-                               << "EKF=" << qEkf.w() << " " << qEkf.vec().transpose() << std::endl
+
+                       std::cout << "EKF=" << qEkf.w() << " " << qEkf.vec().transpose() << std::endl
+                               << "Quaternion=" << q.w() << " " << q.vec().transpose() << std::endl
                                << "Euler angles=" << imu->euler() << " rad" << std::endl
                                << "Linear acceleration=" << imu->linearAccel().transpose() << " m/s^2" << std::endl
                                << "Gravity=" << imu->gravity().transpose() << " m/s^2" << std::endl;
@@ -113,7 +132,6 @@ int main(int argc,char *argv[])
                {
                        std::cerr << e.what() << std::endl;
                }
-               usleep(10000); 
        }
        
        return 0;