STM32F407 MPU6050解决零飘问题代码 标准库函数

好的，根据你的问题，我可以提供以下代码参考：

#include "stm32f4xx.h"
#include "stdio.h"
#include "math.h"

#define PI 3.14159265359f
#define Gyro_Gain_X 0.06097560976f // 2000dps / 32767
#define Gyro_Gain_Y 0.06097560976f // 2000dps / 32767
#define Gyro_Gain_Z 0.06097560976f // 2000dps / 32767
#define Acc_Gain_X  0.00006103515f // 2g / 32767
#define Acc_Gain_Y  0.00006103515f // 2g / 32767
#define Acc_Gain_Z  0.00006103515f // 2g / 32767

float acc_x, acc_y, acc_z;
float gyro_x, gyro_y, gyro_z;
float pitch, roll, yaw;

void MPU6050_Init(void);
void MPU6050_I2C_Read(uint8_t REG_Address, uint8_t *Data, uint8_t len);
void MPU6050_Get_RawData(void);
void MPU6050_Calculate(void);
void MPU6050_Calibrate(void);
void MPU6050_Start(void);

void MPU6050_Init(void)
{
    uint8_t temp[2];
    temp[0] = 0x6B;
    temp[1] = 0x00;
    MPU6050_I2C_Write(0x68, temp, 2);
    temp[0] = 0x1B;
    temp[1] = 0x10;
    MPU6050_I2C_Write(0x68, temp, 2);
    temp[0] = 0x1C;
    temp[1] = 0x10;
    MPU6050_I2C_Write(0x68, temp, 2);
}

void MPU6050_I2C_Read(uint8_t REG_Address, uint8_t *Data, uint8_t len)
{
    I2C_StartTransmission(I2C1, I2C_Direction_Transmitter, 0xD0);
    I2C_WriteData(I2C1, REG_Address);
    I2C_StopTransmission(I2C1);

    I2C_StartTransmission(I2C1, I2C_Direction_Receiver, 0xD0);
    for (uint8_t i = 0; i < len; i++) {
        if (i == len - 1) {
            Data[i] = I2C_ReadNack(I2C1);
        } else {
            Data[i] = I2C_ReadAck(I2C1);
        }
    }
    I2C_StopTransmission(I2C1);
}

void MPU6050_Get_RawData(void)
{
    uint8_t buffer[14];
    MPU6050_I2C_Read(0x3B, buffer, 14);

    acc_x = (float)((int16_t)((buffer[0] << 8) | buffer[1])) * Acc_Gain_X;
    acc_y = (float)((int16_t)((buffer[2] << 8) | buffer[3])) * Acc_Gain_Y;
    acc_z = (float)((int16_t)((buffer[4] << 8) | buffer[5])) * Acc_Gain_Z;

    gyro_x = (float)((int16_t)((buffer[8] << 8) | buffer[9])) * Gyro_Gain_X;
    gyro_y = (float)((int16_t)((buffer[10] << 8) | buffer[11])) * Gyro_Gain_Y;
    gyro_z = (float)((int16_t)((buffer[12] << 8) | buffer[13])) * Gyro_Gain_Z;
}

void MPU6050_Calculate(void)
{
    pitch = atan2f(acc_y, acc_z) * 180 / PI;
    roll = atan2f(acc_x, acc_z) * 180 / PI;
    yaw = yaw + gyro_z * 0.01f;
}

void MPU6050_Calibrate(void)
{
    uint16_t count = 1000;
    float sum_gx = 0.0f, sum_gy = 0.0f, sum_gz = 0.0f;

    for (uint16_t i = 0; i < count; i++) {
        MPU6050_Get_RawData();
        sum_gx += gyro_x;
        sum_gy += gyro_y;
        sum_gz += gyro_z;
        HAL_Delay(2);
    }

    gyro_x = -sum_gx / count;
    gyro_y = -sum_gy / count;
    gyro_z = -sum_gz / count;
}

void MPU6050_Start(void)
{
    MPU6050_Init();
    MPU6050_Calibrate();
    while (1) {
        MPU6050_Get_RawData();
        MPU6050_Calculate();
        HAL_Delay(10);
    }
}

这个代码使用了 STM32F407 的标准库函数，实现了从 MPU6050 传感器读取原始数据，进行零飘校准，并计算出俯仰、横滚和偏航角度。需要注意的是，这里的计算是基于加速度计和陀螺仪的数据融合，可以得到更加准确的角度值。