Commit da3e29b0 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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

parent 1fdcdf9e
......@@ -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();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment