c++ - 使用缓冲区和溢出缓冲区将 IMU 数据写入 csv 文件？

Question

我一直在尝试为通过 I2C 连接到 mbed 板的 LSM9DS1 IMU 实现 C++ 互补滤波器，但时序问题使我无法获得正确的角速率积分。这是因为在我的代码中，我假设我的采样率为 100Hz，而由于我用来实时显示值的 printf() 语句，这并不是数据采样率。当 IMU 放回其原始位置时，这会导致我的滤波器输出角度漂移/不回到原始值。

我被建议遵循以下步骤，以避免我的代码延迟可能会破坏我对时间敏感的应用程序:

• 在程序的每次迭代中，将原始 IMU 数据添加到缓冲区
• 当缓冲区快满时，使用中断写入所有数据.csv 文件的缓冲区
• 当/如果缓冲区溢出，将剩余的数据添加到一个新的“溢出缓冲区”
• 清空第一个缓冲区并用溢出中存储的数据重新填充它缓冲区等
• 通过手动处理数据单独处理过滤计算从 .csv 文件中收集所有内容，以避免计时问题，并查看输出是否符合预期

整个缓冲区/溢出缓冲区来回的事情真的让我很困惑，有人可以帮我澄清如何在技术上实现上述步骤吗？提前致谢!

编辑:

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

在 main.cpp 中:

int main()
{
    init(); // Initialise IMU
    while(1) {
        readValues(); // Read data from the IMUs
        runFilter(); 
    }
 }

Answer 1

正如 Kentaro 在评论中还提到的那样，为 printf 使用单独的线程，并使用 Mbed OS EventQueue 将 printf 语句延迟到它。

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

但是，您可能仍会遇到速度问题。一些一般提示:

1. 使用您的开发板可以处理的最高波特率。
2. 在 printf(使用 Serial)上使用 RawSerial 对象以避免申请互斥。
3. 不要写入 UART，而是写入文件(例如，将 FATFileSystem 挂载到 SD 卡)。这样会快得多。