From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 16 Aug 2019 09:33:10 +0000 (-0300)
Subject: Fix sampling.
X-Git-Tag: f1~9
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=e2224d882884f807df651e7d9a066a60cec38b8e;p=imufusion.git

Fix sampling.
---

diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp
index 0571921..0be6e05 100644
--- 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;