From e2224d882884f807df651e7d9a066a60cec38b8e Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Fri, 16 Aug 2019 06:33:10 -0300 Subject: [PATCH] Fix sampling. --- test/ekf_test.cpp | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) 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 +#include #include @@ -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; -- 2.12.0