mpu6050 roll滤波示例

以下是使用滑动平均滤波器对MPU6050的roll角度进行滤波的示例代码：

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// 滤波器参数
const int numReadings = 10;
int readings[numReadings];      // 存储历史读数的数组
int readIndex = 0;              // 当前读数的位置
int total = 0;                  // 存储历史读数的总和
int average = 0;                // 平均读数

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
}

void loop() {
  // 读取原始的roll角度
  int rawRoll = mpu.getRotationX();

  // 计算滤波后的roll角度
  total = total - readings[readIndex];     // 从总和中减去旧的读数
  readings[readIndex] = rawRoll;           // 存储新的读数
  total = total + readings[readIndex];     // 添加新的读数到总和中
  readIndex = readIndex + 1;               // 增加读数的位置

  // 如果已经读取了足够的读数，则计算平均值并重置readIndex
  if (readIndex >= numReadings) {
    readIndex = 0;
  }

  average = total / numReadings;

  Serial.print("Raw Roll: ");
  Serial.print(rawRoll);
  Serial.print("\tFiltered Roll: ");
  Serial.println(average);

  delay(10);
}

此代码将读取MPU6050的原始roll角度，并使用滑动平均滤波器计算平均值。 在此示例中，我们将使用10个历史读数来计算平均值。 您可以根据需要调整numReadings的值以更改滤波器的效果。