mpu6050 姿态解算示例代码

以下使用 MPU6050 进行姿态解算的示例代码：

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

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

float roll, pitch;

void setup() {
  Wire.begin();
  accelgyro.initialize();
  accelgyro.setDLPFMode(0); // 设置数字低通滤波器的截止频率
}

void loop() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // 读取加速度计和陀螺仪的数据

  // 计算加速度计的倾角
  roll = atan2(ay, az) * 180.0 / M_PI;
  pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / M_PI;

  // 计算陀螺仪的角速度
  float dt = 0.1; // 采样时间，单位为秒
  float gyro_x = gx / 131.0;
  float gyro_y = gy / 131.0;
  float gyro_z = gz / 131.0;

  // 计算欧拉角
  roll += gyro_x * dt;
  pitch += gyro_y * dt;

  // 输出姿态角
  Serial.print("Roll: ");
  Serial.print(roll);
  Serial.print("    Pitch: ");
  Serial.println(pitch);
}

注意，这只是一个简单的示例代码，实际应用中需要进行更复杂的姿态解算，如卡尔曼滤波等。此外，还需要校准传感器，消除误差。