我一直在尝试为通过I2C连接到mbed板的LSM9DS1 IMU实现C ++互补滤波器,但是时序问题使我无法正确地进行角速率积分。这是因为在我的代码中我假设我的采样率是100Hz,而这并不是由于我用来实时显示值的printf()语句而采样数据的速率。这导致我的滤波器输出角度在IMU回到其原始位置时漂移/不回到原始值。
我被建议遵循以下步骤,以避免我的代码延迟可能会破坏我的时间敏感应用程序:
整个缓冲区/溢出缓冲区来回真的让我感到困惑,请有人帮我澄清一下技术上如何实现上述步骤?提前致谢!
编辑:
#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();
}
}
正如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);
但是,您可能仍会遇到速度问题。一些一般提示:
RawSerial
(使用printf
)上使用Serial
对象以避免声称互斥锁。FATFileSystem
安装到SD卡)。这会快得多。