Taken into account the time to read the data and process it in the loop rate.

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();