mpu6050角度积分代码

以下是使用MPU6050进行角度积分的示例代码：

首先，需要引入I2C库和MPU6050库（需要下载并安装）：

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

接着，需要定义MPU6050对象和一些变量：

MPU6050 mpu;

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

float gyroXangle = 0.0;
float gyroYangle = 0.0;
float gyroZangle = 0.0;

float AccXangle = 0.0;
float AccYangle = 0.0;
float AccZangle = 0.0;

float CFangleX = 0.0;
float CFangleY = 0.0;

在setup()函数中，需要初始化MPU6050：

mpu.initialize();

在loop()函数中，需要获取MPU6050的角速度和加速度数据，并计算出当前的角度：

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

// 计算角速度
gyroXangle += gx * dt;
gyroYangle += gy * dt;
gyroZangle += gz * dt;

// 计算加速度
AccXangle = atan2(ay, az) * RAD_TO_DEG;
AccYangle = atan2(ax, az) * RAD_TO_DEG;
AccZangle = atan2(ax, ay) * RAD_TO_DEG;

// 综合角度
CFangleX = 0.93 * (CFangleX + gx * dt) + 0.07 * AccXangle;
CFangleY = 0.93 * (CFangleY + gy * dt) + 0.07 * AccYangle;

其中，dt为两次采样之间的时间间隔（单位为秒）。

最终的roll、pitch和yaw角度可以通过计算得出：

roll = CFangleX;
pitch = CFangleY;
yaw = gyroZangle;

完整代码示例：

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

MPU6050 mpu;

const float RAD_TO_DEG = 180.0 / M_PI;
float roll = 0.0;
float pitch = 0.0;
float yaw = 0.0;

float gyroXangle = 0.0;
float gyroYangle = 0.0;
float gyroZangle = 0.0;

float AccXangle = 0.0;
float AccYangle = 0.0;
float AccZangle = 0.0;

float CFangleX = 0.0;
float CFangleY = 0.0;

unsigned long timer;
float dt;

void setup() {
  Wire.begin();
  Serial.begin(115200);

  mpu.initialize();

  timer = micros();
}

void loop() {
  // 获取当前时间
  unsigned long now = micros();
  dt = (now - timer) / 1000000.0;
  timer = now;

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

  // 计算角速度
  gyroXangle += gx * dt;
  gyroYangle += gy * dt;
  gyroZangle += gz * dt;

  // 计算加速度
  AccXangle = atan2(ay, az) * RAD_TO_DEG;
  AccYangle = atan2(ax, az) * RAD_TO_DEG;
  AccZangle = atan2(ax, ay) * RAD_TO_DEG;

  // 综合角度
  CFangleX = 0.93 * (CFangleX + gx * dt) + 0.07 * AccXangle;
  CFangleY = 0.93 * (CFangleY + gy * dt) + 0.07 * AccYangle;

  // 输出角度
  roll = CFangleX;
  pitch = CFangleY;
  yaw = gyroZangle;

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

  delay(10);
}