Записать данные IMU в файл csv, используя буфер и буфер переполнения?

Я пытался реализовать дополнительный фильтр C++ для IMU LSM9DS1, подключенного через I2C к плате mbed, но проблемы с синхронизацией не позволяют мне правильно интегрировать угловую скорость. Это связано с тем, что в моем коде я предполагаю, что моя частота дискретизации составляет 100 Гц, хотя это не совсем та частота, с которой отбираются данные из-за операторов 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(); 
    }
 }

c++ nucleo mbed imu
person Andrea Loriedo    schedule 22.03.2019    source источник
comment
Вы на Mbed OS2 или OS5?   -  person Kentaro Okuda    schedule 22.03.2019
comment
Это Mbed OS5... Любые предложения?   -  person Andrea Loriedo    schedule 23.03.2019
comment
Вы можете сэмплировать на частоте 100 Гц без printf(), верно? Как вы обеспечиваете частоту дискретизации 100 Гц? (Прерывание по таймеру?) Это расчет фильтрации или printf() вызывает задержку? Ваш код был бы полезен.   -  person Kentaro Okuda    schedule 23.03.2019
comment
@KentaroOkuda частота дискретизации 100 Гц обеспечивается благодаря настройке в библиотеке LSM9DS1. Я думаю, что и операторы printf(), и вычисления вызывают задержку, поэтому я хотел бы выполнить шаги, которые я упомянул выше, чтобы увидеть, будут ли данные, избегая их, получаться так, как ожидалось. Я обновлю пост соответствующими частями моего кода, спасибо!   -  person Andrea Loriedo    schedule 23.03.2019
comment
Вы ждете сигнала готовности данных в readValues()? Если нет, вы пытаетесь пробовать как можно быстрее.   -  person Kentaro Okuda    schedule 23.03.2019
comment
Да, он ждет сигнала готовности данных!   -  person Andrea Loriedo    schedule 23.03.2019
comment
Я бы использовал EventQueue (os.mbed.com/docs /mbed-os/v5.9/reference/eventqueue.html) и прерывание при готовности данных и откладывание printf из контекста прерывания.   -  person Kentaro Okuda    schedule 23.03.2019

Ответы (1)


arrow_upward
1
arrow_downward

Как Кентаро также упомянул в комментариях, используйте отдельный поток для 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. Используйте объект RawSerial поверх printf (который использует Serial), чтобы избежать требования мьютекса.
  3. Не записывайте в UART, а записывайте в файл (например, смонтируйте FATFileSystem на SD-карту). Это будет намного быстрее.
person Jan Jongboom    schedule 25.03.2019
comment
Привет еще раз, спасибо за ваш подробный ответ! Знаете ли вы, возможно ли использование EventQueue и Thread на плате LPC1768? Я получаю ошибки, когда пытаюсь - person Andrea Loriedo; 26.03.2019
comment
Вы уверены, что используете Mbed OS 5? См. os.mbed .com/docs/mbed-os/v5.11/tutorials/ - person Jan Jongboom; 26.03.2019