基于STM32F103的MPU6050姿态测量程序

以下是基于STM32F103的MPU6050姿态测量程序的示例代码：

#include "stm32f10x.h"
#include "math.h"
#include "uart.h"
#include "mpu6050.h"

#define PI 3.1415926535f

float Acc_X, Acc_Y, Acc_Z;
float Gyro_X, Gyro_Y, Gyro_Z;
float Roll, Pitch, Yaw;

void Delay(u32 count)
{
    u32 i,j;
    for(i=0;i<count;i++)
    {
        for(j=0;j<1000;j++);
    }
}

void MPU6050_Init(void)
{
    I2C_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x00);
    I2C_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG, 0x06);
    I2C_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x00);
    I2C_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0x18);
    I2C_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0x01);
}

void MPU6050_Read(void)
{
    Acc_X = (float)I2C_ReadWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H) / 16384.0f;
    Acc_Y = (float)I2C_ReadWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_YOUT_H) / 16384.0f;
    Acc_Z = (float)I2C_ReadWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_ZOUT_H) / 16384.0f;

    Gyro_X = (float)I2C_ReadWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_XOUT_H) / 16.4f;
    Gyro_Y = (float)I2C_ReadWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_YOUT_H) / 16.4f;
    Gyro_Z = (float)I2C_ReadWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_ZOUT_H) / 16.4f;
}

void Get_Attitude(void)
{
    Roll = atan2(Acc_Y, Acc_Z) * 180.0f / PI;
    Pitch = atan2(-Acc_X, sqrt(Acc_Y * Acc_Y + Acc_Z * Acc_Z)) * 180.0f / PI;

    Yaw = Gyro_Z * 0.001f;
}

int main(void)
{
    UART_Config();
    I2C_Config();
    MPU6050_Init();

    while(1)
    {
        MPU6050_Read();
        Get_Attitude();

        printf("Roll: %.2f, Pitch: %.2f, Yaw: %.2f\r\n", Roll, Pitch, Yaw);

        Delay(100);
    }
}

代码解析：

1. 在 `MPU6050_Init()` 函数中，写入 MPU6050 的初始化配置，包括关闭睡眠模式、配置陀螺仪和加速度计的满量程范围等。
2. 在 `MPU6050_Read()` 函数中，读取 MPU6050 的加速度计和陀螺仪数据，并将数据转换为浮点数类型。
3. 在 `Get_Attitude()` 函数中，根据加速度计数据计算出 Roll 和 Pitch 角度，根据陀螺仪数据计算出 Yaw 角速度。
4. 在 `main()` 函数中，循环读取 MPU6050 数据并计算姿态角，并通过串口输出。

需要注意的是，以上代码中的 `UART_Config()`、`I2C_Config()`、`I2C_WriteByte()`、`I2C_ReadWord()` 函数并未给出实现，需要根据实际情况自行实现或引用已有的库函数。