MPU6500 陀螺仪和加速度计 简单融合旋转三轴代码

MPU6500是一款集成了三轴陀螺仪和三轴加速度计的传感器模块，常用于姿态检测和运动追踪。简单融合旋转三轴代码通常涉及读取陀螺仪和加速度计的数据，然后通过某种算法（如互补滤波、卡尔曼滤波等）来估计设备的姿态。

以下是一个使用互补滤波进行简单融合的示例代码：

#include "I2Cdev.h"
#include "MPU6050.h"

// MPU控制台要使用的一些全局变量
volatile bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes here)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

//姿态角变量
float q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0; //四元数分量
float eInt = 2.0f; //互补滤波器误差修正系数
float gyroScale = (2000.0f * M_PI / 180.0f) / 32768.0f; //陀螺仪比例尺
float accelerometerScale = (2000.0f * M_PI / 180.0f) / 32768.0f; //加速度计比例尺

//初始化MPU6050
void setupMPU6050() {
    //加入I2C通信代码
    Wire.begin();
    //初始化MPU6050
    MPU6050 mpu;
    devStatus = mpu.initialize();
}

//读取MPU6050数据
void readMPU6050Data() {
    //读取加速度计数据
    VectorInt16 aa = mpu.readAccel();
    //读取陀螺仪数据
    VectorInt16 gyro = mpu.readGyro();

    //将加速度计数据转换为角度
    float ax = aa.XAxis * accelerometerScale;
    float ay = aa.YAxis * accelerometerScale;
    float az = aa.ZAxis * accelerometerScale;

    //将陀螺仪数据转换为角度变化率
    float gx = gyro.XAxis * gyroScale;
    float gy = gyro.YAxis * gyroScale;
    float gz = gyro.ZAxis * gyroScale;

    //互补滤波融合
    q0 = q0 + (eInt * (-q1 * gx - q2 * gy - q3 * gz));
    q1 = q1 + (eInt * (q0 * gx + q3 * az - q2 * ax));
    q2 = q2 + (eInt * (-q3 * gy - q1 * ay + q0 * ax));
    q3 = q3 + (eInt * (q2 * gx - q0 * gz + q1 * ay));
}

void loop() {
    readMPU6050Data();
    //在这里可以添加更多的处理逻辑，比如控制电机或发送数据到电脑
}