From da3e29b0ef17e2b0140537e4043844f5646401b4 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 1 Jul 2020 10:14:12 +0200
Subject: [PATCH] Taken into account the time to read the data and process it
in the loop rate.
---
src/bno055_imu_driver.cpp | 10 +++++++++-
1 file changed, 9 insertions(+), 1 deletion(-)
diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp
index 44117d2..0fc0e58 100644
--- a/src/bno055_imu_driver.cpp
+++ b/src/bno055_imu_driver.cpp
@@ -3,6 +3,7 @@
#include "eventexceptions.h"
#include <iostream>
#include <fstream>
+#include <chrono>
#include <math.h>
CBNO055IMUDriver::CBNO055IMUDriver(const std::string &name)
@@ -208,6 +209,7 @@ void *CBNO055IMUDriver::data_thread(void *param)
{
CBNO055IMUDriver *imu=(CBNO055IMUDriver *)param;
unsigned char address=0x08,length=46,data[46];
+ unsigned long int duration;
bool end=false;
while(!end)
@@ -215,10 +217,16 @@ void *CBNO055IMUDriver::data_thread(void *param)
if(imu->op_mode!=config_mode)
{
try{
+ auto t1 = std::chrono::high_resolution_clock::now();
imu->imu_access.enter();
imu->send_read_cmd(address,length);
imu->imu_access.exit();
- usleep((unsigned long int)(1000000.0/imu->data_rate_hz)-1000);
+ auto t2 = std::chrono::high_resolution_clock::now();
+ duration = (unsigned long int)(1000000.0/imu->data_rate_hz) - std::chrono::duration_cast<std::chrono::microseconds>( t2 - t1 ).count();
+ if((signed long int)duration>0)
+ usleep(duration);
+ else
+ usleep((unsigned long int)(1000000.0/imu->data_rate_hz));
imu->imu_access.enter();
imu->get_answer(length,data);
imu->imu_access.exit();
--
GitLab