arduino mpu6050获取角度代码

以下是基于Arduino和MPU6050传感器获取角度的代码示例：

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

MPU6050 mpu;

int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;

float accAngleX, accAngleY;
float gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;

void setup() {
  Serial.begin(9600);

  Wire.begin();
  mpu.initialize();

  mpu.dmpInitialize();

  mpu.setDMPEnabled(true);

  // load offsets from calibrationSketch.ino
  mpu.setXAccelOffset(846);
  mpu.setYAccelOffset(1061);
  mpu.setZAccelOffset(1343);
  mpu.setXGyroOffset(39);
  mpu.setYGyroOffset(-9);
  mpu.setZGyroOffset(-68);

  // calculate offsets from calibrationSketch.ino
  AccXYZoffset = (0.5 / 16384) * 9.81 * 1000; // 0.5 -> scale factor to get to SI (9.81 m/s^2 -> 9810 mm/s^2)
  GyroXYZoffset = 0.9863; // scale factor to get to SI (gyroscope output in degrees/s)

  delay(1000);
}

void loop() {
  mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ); // get raw data from MPU6050

  accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * RAD_TO_DEG; // Calculate pitch angle using accelerometer
  accAngleY = atan(-1 * accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * RAD_TO_DEG; // Calculate roll angle using accelerometer

  gyroAngleX += gyroX / 131.0 * GyroXYZoffset * (millis() - prevTime) / 1000.0; // Calculate pitch angle using gyroscope
  gyroAngleY += gyroY / 131.0 * GyroXYZoffset * (millis() - prevTime) / 1000.0; // Calculate roll angle using gyroscope
  gyroAngleZ += gyroZ / 131.0 * GyroXYZoffset * (millis() - prevTime) / 1000.0; // Calculate yaw angle using gyroscope

  pitch = 0.98 * gyroAngleX + 0.02 * accAngleX; // Complementary filter to fuse acceleration and gyroscope data for pitch
  roll = 0.98 * gyroAngleY + 0.02 * accAngleY; // Complementary filter to fuse acceleration and gyroscope data for roll

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

  prevTime = millis();
}

这段代码使用了MPU6050传感器来获取姿态角度。其中，通过加速度计计算出俯仰角（pitch）和横滚角（roll），通过陀螺仪计算出偏航角（yaw），然后使用互补滤波器（complementary filter）将加速度计和陀螺仪的数据进行融合得到最终的姿态角度。