MPU6500 简单XYZ角度代码

MPU6500是一款集成了三轴加速度计和三轴陀螺仪的传感器，常用于姿态检测、运动跟踪等应用。要获取XYZ角度，我们通常需要通过读取加速度计和陀螺仪的数据，然后使用一定的算法（如卡尔曼滤波或互补滤波）来计算出设备的姿态角。

以下是一个简单的示例代码，展示如何使用MPU6500传感器获取XYZ角度：

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

MPU6050 mpu;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }
  Serial.println("MPU6050 connection successful");
}

void loop() {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  
  // Read raw accel/gyro measurements from device
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  // Convert accelerometer values to angles in degrees
  float accXangle = (atan2(ay, az) * 180 / PI);
  float accYangle = (atan2(ax, az) * 180 / PI);
  
  // Print the angles
  Serial.print("Accel X angle: ");
  Serial.println(accXangle);
  Serial.print("Accel Y angle: ");
  Serial.println(accYangle);
  
  delay(100);
}

这段代码首先初始化MPU6050传感器，然后在循环中不断读取加速度计和陀螺仪的数据。通过计算得到X轴和Y轴的角度，并通过串口输出。注意这里只使用了加速度计数据来计算角度，实际应用中可能需要结合陀螺仪数据使用更复杂的算法来提高精度和稳定性。