Write IMU data to csv file using buffer and overflow buffer? - c++

Question

I have been trying to implement a C++ complementary filter for a LSM9DS1 IMU connected via I2C to an mbed board, but timing issues are preventing me from getting the angular rate integration right. This is because in my code I'm assuming that my sample rate is 100Hz, while this isn't exactly the rate at which data is being sampled due to the printf() statements I am using to display values in real time. This results in my filter outputting angles that drift/don't go back to the original value when the IMU is put back in its original position.
I've been recommended to follow the following steps in order to avoid delays in my code that could disrupt my time sensitive application:
On each iteration of the program, add the raw IMU data to a buffer
When the buffer is nearly full, use an interrupt to write all the data from
the buffer to a .csv file
When/if the buffer overflows, add the remaining data to a new "overflow
buffer"
Empty the first buffer and refill it with the data stored in the overflow
buffer, and so on
Handle the filtering calculations separately by manually treating the data
from the .csv file once it's all been collected, so as to avoid timing
issues, and see if the output is as expected
The whole buffer/overflow buffer back and forth thing really confuses me, could someone please help me clarifying how to technically achieve the above steps? Thanks in advance!
Edit:
#include "LSM9DS1.h"
#define DT 1/100
void runFilter()
{
// calculate Euler angles from accelerometer and magnetometer (_roll,
// _pitch,_yaw)
calcAttitude(imu.ax, imu.ay, imu.az, -imu.my, -imu.mx, imu.mz);
_gyroAngleX += (_rateX*DT);
_gyroAngleY += (_rateY*DT);
_gyroAngleZ += (_rateZ*DT);
_xfilt = 0.98f*(_gyroAngleX) + 0.02f*_roll;
_yfilt = 0.98f*(_gyroAngleY) + 0.02f*_pitch;
_zfilt = 0.98f*(_gyroAngleZ) + 0.02f*_yaw;
printf("%.2f, %.2f, %.2f \n", _xfilt, _yfilt, _zfilt);
}
in main.cpp:
int main()
{
init(); // Initialise IMU
while(1) {
readValues(); // Read data from the IMUs
runFilter();
}
}

Answer 1

As Kentaro also mentioned in the comments, use a separate thread for printf and use the Mbed OS EventQueue to defer printf statements to it.
EventQueue queue;
Thread event_thread(osPriorityLow);
int main() {
event_thread.start(callback(&queue, &EventQueue::dispatch_forever));
// after sampling
queue.call(&printf, "%.2f, %.2f, %.2f \n", _xfilt, _yfilt, _zfilt);
However, you might still run into issues with the speed. Some general tips:
Use the highest baud rate that your development board can handle.
Use a RawSerial object over printf (which uses Serial) to avoid claiming a mutex.
Don't write to UART but rather write to a file (e.g. mount a FATFileSystem to an SD card). This will be much faster.