mpu6050 姿态解算示例代码

以下是使用 Arduino 和 MPU6050 模块进行姿态解算的示例代码。该代码使用了 Mahony 滤波器进行姿态解算。

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

MPU6050 imu;

#define GRAVITY 9.81
#define RAD_TO_DEG 57.29578

float roll = 0.0;
float pitch = 0.0;
float yaw = 0.0;

float gx = 0.0;
float gy = 0.0;
float gz = 0.0;
float ax = 0.0;
float ay = 0.0;
float az = 0.0;
float mx = 0.0;
float my = 0.0;
float mz = 0.0;

float q0 = 1.0;
float q1 = 0.0;
float q2 = 0.0;
float q3 = 0.0;

unsigned long last_time = 0;
float dt = 0.0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  imu.initialize();
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
}

void loop() {
  unsigned long now = millis();
  dt = (now - last_time) / 1000.0;
  last_time = now;

  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float ax_norm = ax / GRAVITY;
  float ay_norm = ay / GRAVITY;
  float az_norm = az / GRAVITY;

  float mx_norm = mx / 480.0;
  float my_norm = my / 480.0;
  float mz_norm = mz / 480.0;

  float q0_last = q0;
  float q1_last = q1;
  float q2_last = q2;
  float q3_last = q3;

  float q0_dot = 0.5 * (-q1_last * gx - q2_last * gy - q3_last * gz);
  float q1_dot = 0.5 * (q0_last * gx + q2_last * gz - q3_last * gy);
  float q2_dot = 0.5 * (q0_last * gy - q1_last * gz + q3_last * gx);
  float q3_dot = 0.5 * (q0_last * gz + q1_last * gy - q2_last * gx);

  q0 += q0_dot * dt;
  q1 += q1_dot * dt;
  q2 += q2_dot * dt;
  q3 += q3_dot * dt;

  float norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 /= norm;
  q1 /= norm;
  q2 /= norm;
  q3 /= norm;

  float yaw_rad = atan2(2.0 * q1 * q2 - 2.0 * q0 * q3, 2.0 * q0 * q0 + 2.0 * q1 * q1 - 1.0);
  float pitch_rad = asin(2.0 * q1 * q3 + 2.0 * q0 * q2);
  float roll_rad = atan2(2.0 * q2 * q3 - 2.0 * q0 * q1, 2.0 * q0 * q0 + 2.0 * q3 * q3 - 1.0);

  yaw = yaw_rad * RAD_TO_DEG;
  pitch = pitch_rad * RAD_TO_DEG;
  roll = roll_rad * RAD_TO_DEG;

  Serial.print("Yaw: ");
  Serial.print(yaw);
  Serial.print(" Pitch: ");
  Serial.print(pitch);
  Serial.print(" Roll: ");
  Serial.println(roll);
}

该示例代码使用了 Mahony 滤波器进行姿态解算，可以在 `MahonyAHRS.h` 和 `MahonyAHRS.cpp` 中找到 Mahony 滤波器的实现代码。如果需要更高精度的姿态解算，可以考虑使用 Kalman 滤波器或者其他更高级的滤波算法。