Write IMU data to csv file using buffer and overflow buffer?

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

You can sample at 100Hz without printf(), correct? How do you ensure 100Hz sample rate? (Timer interrupt?) Is it the filtering calculation or printf() that causes the delay? Your code would be helpful. — Kentaro Okuda, Mar 23 '19 at 10:10
@KentaroOkuda the 100Hz sample rate is ensured thanks to a setting in the LSM9DS1 library. I think both the printf() statements and calculations are causing the delay, so I'd like to follow the steps I mentioned above to see if by avoiding them the data comes out as expected. I will update the post with the relevant parts of my code, thanks! — Andrea Loriedo, Mar 23 '19 at 10:29
Are you waiting for data ready signal in readValues()? If not, you are trying to sample as fast as you can. — Kentaro Okuda, Mar 23 '19 at 11:42
I would use EventQueue (https://os.mbed.com/docs/mbed-os/v5.9/reference/eventqueue.html) and interrupt on data ready and defer printf from the interrupt context. — Kentaro Okuda, Mar 23 '19 at 12:28

score 1 · Accepted Answer · answered Mar 25 '19 at 09:48

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.

Hi again, thanks for your detailed answer! Do you know if using EventQueue and Thread is possible on a LPC1768 board? I get errors when I try to — Andrea Loriedo, Mar 26 '19 at 12:57
Are you sure you're on Mbed OS 5? See https://os.mbed.com/docs/mbed-os/v5.11/tutorials/migrating-to-mbed-os-5.html — Jan Jongboom, Mar 26 '19 at 18:56

Write IMU data to csv file using buffer and overflow buffer?

1 Answers1