陀螺仪传感器驱动程序（基于 MPU6050）

一、系统架构设计

┌─────────────────────────────────────────────────────────────┐
│                    陀螺仪传感器驱动架构                    │
├─────────────────────────────────────────────────────────────┤
│  硬件接口层  │  寄存器配置  │  数据处理层  │  应用接口层   │
│              │              │              │              │
│  • I2C/SPI   │  • 量程配置  │  • 原始数据  │  • 初始化    │
│  • GPIO中断  │  • 滤波设置  │  • 校准补偿  │  • 数据读取  │
│  • 时钟源    │  • 采样率    │  • 单位转换  │  • 姿态解算  │
│  • 电源管理  │  • FIFO配置  │  • 温度补偿  │  • 状态监测  │
└─────────────────────────────────────────────────────────────┘

二、硬件连接设计

2.1 MPU6050 硬件连接（STM32）

MPU6050 引脚    STM32F103 引脚    说明
────────────────────────────────────────
VCC           3.3V              电源输入
GND           GND               电源地
SCL           PB6 (I2C1_SCL)    I2C时钟
SDA           PB7 (I2C1_SDA)    I2C数据
INT           PA0              中断输出
AD0           GND              地址选择(0x68)

2.2 关键参数规格

参数	规格
陀螺仪量程	±250/500/1000/2000 °/s
加速度计量程	±2/4/8/16 g
采样率	最高 8kHz
通信接口	I2C (400kHz) / SPI (20MHz)
工作电压	2.375V – 3.46V

三、源码实现

3.1 驱动头文件 (gyro_mpu6050.h)

#ifndef __GYRO_MPU6050_H
#define __GYRO_MPU6050_H

#include "stm32f10x.h"
#include <stdint.h>
#include <stdbool.h>
#include <math.h>

// MPU6050 I2C 地址
#define MPU6050_ADDR_AD0_LOW     0x68    // AD0接地
#define MPU6050_ADDR_AD0_HIGH    0x69    // AD0接VCC
#define MPU6050_ADDR_DEFAULT    MPU6050_ADDR_AD0_LOW

// MPU6050 寄存器地址
#define MPU6050_REG_SMPLRT_DIV   0x19    // 采样率分频
#define MPU6050_REG_CONFIG       0x1A    // 配置寄存器
#define MPU6050_REG_GYRO_CONFIG  0x1B    // 陀螺仪配置
#define MPU6050_REG_ACCEL_CONFIG 0x1C    // 加速度计配置
#define MPU6050_REG_FIFO_EN      0x23    // FIFO使能
#define MPU6050_REG_INT_PIN_CFG  0x37    // 中断引脚配置
#define MPU6050_REG_INT_ENABLE   0x38    // 中断使能
#define MPU6050_REG_INT_STATUS   0x3A    // 中断状态
#define MPU6050_REG_ACCEL_XOUT_H 0x3B    // 加速度X轴高字节
#define MPU6050_REG_ACCEL_XOUT_L 0x3C    // 加速度X轴低字节
#define MPU6050_REG_ACCEL_YOUT_H 0x3D    // 加速度Y轴高字节
#define MPU6050_REG_ACCEL_YOUT_L 0x3E    // 加速度Y轴低字节
#define MPU6050_REG_ACCEL_ZOUT_H 0x3F    // 加速度Z轴高字节
#define MPU6050_REG_ACCEL_ZOUT_L 0x40    // 加速度Z轴低字节
#define MPU6050_REG_TEMP_OUT_H   0x41    // 温度高字节
#define MPU6050_REG_TEMP_OUT_L   0x42    // 温度低字节
#define MPU6050_REG_GYRO_XOUT_H  0x43    // 陀螺仪X轴高字节
#define MPU6050_REG_GYRO_XOUT_L  0x44    // 陀螺仪X轴低字节
#define MPU6050_REG_GYRO_YOUT_H  0x45    // 陀螺仪Y轴高字节
#define MPU6050_REG_GYRO_YOUT_L  0x46    // 陀螺仪Y轴低字节
#define MPU6050_REG_GYRO_ZOUT_H  0x47    // 陀螺仪Z轴高字节
#define MPU6050_REG_GYRO_ZOUT_L  0x48    // 陀螺仪Z轴低字节
#define MPU6050_REG_USER_CTRL    0x6A    // 用户控制
#define MPU6050_REG_PWR_MGMT_1   0x6B    // 电源管理1
#define MPU6050_REG_PWR_MGMT_2   0x6C    // 电源管理2
#define MPU6050_REG_FIFO_COUNTH  0x72    // FIFO计数高字节
#define MPU6050_REG_FIFO_COUNTL  0x73    // FIFO计数低字节
#define MPU6050_REG_FIFO_R_W     0x74    // FIFO读写
#define MPU6050_REG_WHO_AM_I     0x75    // 设备ID

// 陀螺仪量程配置
typedef enum {
    GYRO_FS_250  = 0x00,  // ±250 °/s
    GYRO_FS_500  = 0x08,  // ±500 °/s
    GYRO_FS_1000 = 0x10,  // ±1000 °/s
    GYRO_FS_2000 = 0x18   // ±2000 °/s
} GyroFullScale_t;

// 加速度计量程配置
typedef enum {
    ACCEL_FS_2G  = 0x00,  // ±2 g
    ACCEL_FS_4G  = 0x08,  // ±4 g
    ACCEL_FS_8G  = 0x10,  // ±8 g
    ACCEL_FS_16G = 0x18   // ±16 g
} AccelFullScale_t;

// 数字低通滤波器配置
typedef enum {
    DLPF_BW_260 = 0x00,  // 260Hz带宽，0ms延迟
    DLPF_BW_184 = 0x01,  // 184Hz带宽，2ms延迟
    DLPF_BW_94  = 0x02,  // 94Hz带宽，3ms延迟
    DLPF_BW_44  = 0x03,  // 44Hz带宽，4.9ms延迟
    DLPF_BW_21  = 0x04,  // 21Hz带宽，8.5ms延迟
    DLPF_BW_10  = 0x05,  // 10Hz带宽，13.8ms延迟
    DLPF_BW_5   = 0x06   // 5Hz带宽，19ms延迟
} DLPF_Bandwidth_t;

// 陀螺仪数据结构体
typedef struct {
    float gyro_x;        // X轴角速度 (°/s)
    float gyro_y;        // Y轴角速度 (°/s)
    float gyro_z;        // Z轴角速度 (°/s)
    float accel_x;       // X轴加速度 (g)
    float accel_y;       // Y轴加速度 (g)
    float accel_z;       // Z轴加速度 (g)
    float temperature;   // 温度 (°C)
    uint32_t timestamp;  // 时间戳
    bool valid;          // 数据有效性
} GyroData_t;

// 校准参数结构体
typedef struct {
    float gyro_bias_x;   // 陀螺仪X轴零偏
    float gyro_bias_y;   // 陀螺仪Y轴零偏
    float gyro_bias_z;   // 陀螺仪Z轴零偏
    float accel_bias_x;  // 加速度计X轴零偏
    float accel_scale_x; // 加速度计X轴比例因子
    float accel_scale_y; // 加速度计Y轴比例因子
    float accel_scale_z; // 加速度计Z轴比例因子
    bool calibrated;    // 校准完成标志
} GyroCalibration_t;

// 传感器配置结构体
typedef struct {
    GyroFullScale_t gyro_fs;     // 陀螺仪量程
    AccelFullScale_t accel_fs;   // 加速度计量程
    DLPF_Bandwidth_t dlpf_bw;    // 数字低通滤波器带宽
    uint8_t sample_rate_div;     // 采样率分频
    bool fifo_enabled;           // FIFO使能
    bool int_enabled;            // 中断使能
} GyroConfig_t;

// 函数声明
bool MPU6050_Init(void);
bool MPU6050_Reset(void);
bool MPU6050_Config(GyroConfig_t *config);
bool MPU6050_ReadID(uint8_t *id);
bool MPU6050_ReadSensorData(GyroData_t *data);
bool MPU6050_ReadGyro(float *gx, float *gy, float *gz);
bool MPU6050_ReadAccel(float *ax, float *ay, float *az);
bool MPU6050_ReadTemperature(float *temp);
bool MPU6050_Calibrate(GyroCalibration_t *cal);
void MPU6050_SetCalibration(GyroCalibration_t *cal);
void MPU6050_ApplyCalibration(GyroData_t *data);
bool MPU6050_SetSleepMode(bool sleep);
bool MPU6050_SetCycleMode(bool cycle);
bool MPU6050_InterruptConfig(bool enable);
bool MPU6050_FIFOConfig(uint8_t sensors);
uint16_t MPU6050_GetFIFOCount(void);
bool MPU6050_ReadFIFO(uint8_t *buffer, uint16_t length);
bool MPU6050_ClearFIFO(void);

#endif /* __GYRO_MPU6050_H */

3.2 驱动核心实现 (gyro_mpu6050.c)

#include "gyro_mpu6050.h"
#include "i2c.h"
#include "delay.h"

// 全局变量
static GyroConfig_t gyro_config;
static GyroCalibration_t gyro_calibration = {0};
static bool gyro_initialized = false;

// I2C底层读写函数
static bool I2C_WriteReg(uint8_t reg, uint8_t data) {
    return I2C_WriteByte(MPU6050_ADDR_DEFAULT, reg, data);
}

static bool I2C_ReadReg(uint8_t reg, uint8_t *data) {
    return I2C_ReadByte(MPU6050_ADDR_DEFAULT, reg, data);
}

static bool I2C_ReadRegs(uint8_t reg, uint8_t *data, uint8_t len) {
    return I2C_ReadBytes(MPU6050_ADDR_DEFAULT, reg, data, len);
}

// 初始化MPU6050
bool MPU6050_Init(void) {
    uint8_t id;
    
    // 复位设备
    if (!MPU6050_Reset()) {
        return false;
    }
    Delay_ms(100);
    
    // 读取设备ID
    if (!MPU6050_ReadID(&id)) {
        return false;
    }
    
    if (id != 0x68) {
        return false;  // 设备ID不匹配
    }
    
    // 配置默认参数
    GyroConfig_t default_config = {
        .gyro_fs = GYRO_FS_1000,
        .accel_fs = ACCEL_FS_4G,
        .dlpf_bw = DLPF_BW_44,
        .sample_rate_div = 19,  // 1kHz / (19 + 1) = 50Hz
        .fifo_enabled = false,
        .int_enabled = false
    };
    
    if (!MPU6050_Config(&default_config)) {
        return false;
    }
    
    gyro_initialized = true;
    return true;
}

// 复位MPU6050
bool MPU6050_Reset(void) {
    // 设置复位位
    if (!I2C_WriteReg(MPU6050_REG_PWR_MGMT_1, 0x80)) {
        return false;
    }
    
    Delay_ms(100);
    
    // 清除复位位，选择时钟源为X轴陀螺仪
    if (!I2C_WriteReg(MPU6050_REG_PWR_MGMT_1, 0x01)) {
        return false;
    }
    
    Delay_ms(100);
    return true;
}

// 配置MPU6050参数
bool MPU6050_Config(GyroConfig_t *config) {
    if (config == NULL) {
        return false;
    }
    
    // 设置采样率分频
    if (!I2C_WriteReg(MPU6050_REG_SMPLRT_DIV, config->sample_rate_div)) {
        return false;
    }
    
    // 设置数字低通滤波器
    if (!I2C_WriteReg(MPU6050_REG_CONFIG, config->dlpf_bw)) {
        return false;
    }
    
    // 设置陀螺仪量程
    if (!I2C_WriteReg(MPU6050_REG_GYRO_CONFIG, config->gyro_fs)) {
        return false;
    }
    
    // 设置加速度计量程
    if (!I2C_WriteReg(MPU6050_REG_ACCEL_CONFIG, config->accel_fs)) {
        return false;
    }
    
    // 配置电源管理
    if (!I2C_WriteReg(MPU6050_REG_PWR_MGMT_1, 0x01)) {  // 使用X轴陀螺仪时钟
        return false;
    }
    
    // 配置电源管理2（使能所有轴）
    if (!I2C_WriteReg(MPU6050_REG_PWR_MGMT_2, 0x00)) {
        return false;
    }
    
    gyro_config = *config;
    return true;
}

// 读取设备ID
bool MPU6050_ReadID(uint8_t *id) {
    return I2C_ReadReg(MPU6050_REG_WHO_AM_I, id);
}

// 读取传感器数据
bool MPU6050_ReadSensorData(GyroData_t *data) {
    uint8_t raw_data[14];
    int16_t raw_gyro_x, raw_gyro_y, raw_gyro_z;
    int16_t raw_accel_x, raw_accel_y, raw_accel_z;
    int16_t raw_temp;
    
    if (data == NULL || !gyro_initialized) {
        return false;
    }
    
    // 读取14字节数据（加速度+温度+陀螺仪）
    if (!I2C_ReadRegs(MPU6050_REG_ACCEL_XOUT_H, raw_data, 14)) {
        return false;
    }
    
    // 解析原始数据
    raw_accel_x = (int16_t)((raw_data[0] << 8) | raw_data[1]);
    raw_accel_y = (int16_t)((raw_data[2] << 8) | raw_data[3]);
    raw_accel_z = (int16_t)((raw_data[4] << 8) | raw_data[5]);
    
    raw_temp = (int16_t)((raw_data[6] << 8) | raw_data[7]);
    
    raw_gyro_x = (int16_t)((raw_data[8] << 8) | raw_data[9]);
    raw_gyro_y = (int16_t)((raw_data[10] << 8) | raw_data[11]);
    raw_gyro_z = (int16_t)((raw_data[12] << 8) | raw_data[13]);
    
    // 转换为物理量
    float gyro_scale;
    float accel_scale;
    
    // 陀螺仪比例因子
    switch(gyro_config.gyro_fs) {
        case GYRO_FS_250:  gyro_scale = 250.0f / 32768.0f; break;
        case GYRO_FS_500:  gyro_scale = 500.0f / 32768.0f; break;
        case GYRO_FS_1000: gyro_scale = 1000.0f / 32768.0f; break;
        case GYRO_FS_2000: gyro_scale = 2000.0f / 32768.0f; break;
        default: gyro_scale = 1000.0f / 32768.0f; break;
    }
    
    // 加速度计比例因子
    switch(gyro_config.accel_fs) {
        case ACCEL_FS_2G:  accel_scale = 2.0f / 32768.0f; break;
        case ACCEL_FS_4G:  accel_scale = 4.0f / 32768.0f; break;
        case ACCEL_FS_8G:  accel_scale = 8.0f / 32768.0f; break;
        case ACCEL_FS_16G: accel_scale = 16.0f / 32768.0f; break;
        default: accel_scale = 4.0f / 32768.0f; break;
    }
    
    // 计算物理量
    data->gyro_x = raw_gyro_x * gyro_scale;
    data->gyro_y = raw_gyro_y * gyro_scale;
    data->gyro_z = raw_gyro_z * gyro_scale;
    
    data->accel_x = raw_accel_x * accel_scale;
    data->accel_y = raw_accel_y * accel_scale;
    data->accel_z = raw_accel_z * accel_scale;
    
    // 温度转换：温度(°C) = 36.53 + raw_temp / 340.0
    data->temperature = 36.53f + (float)raw_temp / 340.0f;
    data->timestamp = Get_SystemTime();
    data->valid = true;
    
    // 应用校准参数
    MPU6050_ApplyCalibration(data);
    
    return true;
}

// 读取陀螺仪数据
bool MPU6050_ReadGyro(float *gx, float *gy, float *gz) {
    GyroData_t data;
    
    if (!MPU6050_ReadSensorData(&data)) {
        return false;
    }
    
    if (gx) *gx = data.gyro_x;
    if (gy) *gy = data.gyro_y;
    if (gz) *gz = data.gyro_z;
    
    return true;
}

// 读取加速度计数据
bool MPU6050_ReadAccel(float *ax, float *ay, float *az) {
    GyroData_t data;
    
    if (!MPU6050_ReadSensorData(&data)) {
        return false;
    }
    
    if (ax) *ax = data.accel_x;
    if (ay) *ay = data.accel_y;
    if (az) *az = data.accel_z;
    
    return true;
}

// 读取温度数据
bool MPU6050_ReadTemperature(float *temp) {
    uint8_t raw_data[2];
    int16_t raw_temp;
    
    if (!I2C_ReadRegs(MPU6050_REG_TEMP_OUT_H, raw_data, 2)) {
        return false;
    }
    
    raw_temp = (int16_t)((raw_data[0] << 8) | raw_data[1]);
    *temp = 36.53f + (float)raw_temp / 340.0f;
    
    return true;
}

// 校准陀螺仪
bool MPU6050_Calibrate(GyroCalibration_t *cal) {
    GyroData_t data;
    float gyro_x_sum = 0, gyro_y_sum = 0, gyro_z_sum = 0;
    float accel_x_sum = 0, accel_y_sum = 0, accel_z_sum = 0;
    const uint16_t samples = 1000;
    
    if (cal == NULL) {
        return false;
    }
    
    printf("Starting Gyroscope Calibration...\n");
    printf("Please keep the sensor stationary!\n");
    
    // 采集样本数据
    for (uint16_t i = 0; i < samples; i++) {
        if (MPU6050_ReadSensorData(&data)) {
            gyro_x_sum += data.gyro_x;
            gyro_y_sum += data.gyro_y;
            gyro_z_sum += data.gyro_z;
            accel_x_sum += data.accel_x;
            accel_y_sum += data.accel_y;
            accel_z_sum += data.accel_z;
        }
        Delay_ms(10);
    }
    
    // 计算平均值作为零偏
    cal->gyro_bias_x = gyro_x_sum / samples;
    cal->gyro_bias_y = gyro_y_sum / samples;
    cal->gyro_bias_z = gyro_z_sum / samples;
    
    // 加速度计校准（假设Z轴垂直向上）
    cal->accel_bias_x = accel_x_sum / samples;
    cal->accel_bias_y = accel_y_sum / samples;
    cal->accel_bias_z = (accel_z_sum / samples) - 1.0f;  // 减去重力
    
    // 比例因子（默认1.0）
    cal->accel_scale_x = 1.0f;
    cal->accel_scale_y = 1.0f;
    cal->accel_scale_z = 1.0f;
    cal->calibrated = true;
    
    printf("Calibration Complete!\n");
    printf("Gyro Bias: X=%.4f, Y=%.4f, Z=%.4f °/s\n", 
           cal->gyro_bias_x, cal->gyro_bias_y, cal->gyro_bias_z);
    
    return true;
}

// 设置校准参数
void MPU6050_SetCalibration(GyroCalibration_t *cal) {
    if (cal != NULL) {
        gyro_calibration = *cal;
    }
}

// 应用校准参数
void MPU6050_ApplyCalibration(GyroData_t *data) {
    if (!gyro_calibration.calibrated || data == NULL) {
        return;
    }
    
    // 应用零偏补偿
    data->gyro_x -= gyro_calibration.gyro_bias_x;
    data->gyro_y -= gyro_calibration.gyro_bias_y;
    data->gyro_z -= gyro_calibration.gyro_bias_z;
    
    data->accel_x = (data->accel_x - gyro_calibration.accel_bias_x) * gyro_calibration.accel_scale_x;
    data->accel_y = (data->accel_y - gyro_calibration.accel_bias_y) * gyro_calibration.accel_scale_y;
    data->accel_z = (data->accel_z - gyro_calibration.accel_bias_z) * gyro_calibration.accel_scale_z;
}

// 设置睡眠模式
bool MPU6050_SetSleepMode(bool sleep) {
    uint8_t reg_value;
    
    if (!I2C_ReadReg(MPU6050_REG_PWR_MGMT_1, &reg_value)) {
        return false;
    }
    
    if (sleep) {
        reg_value |= 0x40;  // 设置睡眠位
    } else {
        reg_value &= ~0x40; // 清除睡眠位
    }
    
    return I2C_WriteReg(MPU6050_REG_PWR_MGMT_1, reg_value);
}

// FIFO配置
bool MPU6050_FIFOConfig(uint8_t sensors) {
    return I2C_WriteReg(MPU6050_REG_FIFO_EN, sensors);
}

// 获取FIFO计数
uint16_t MPU6050_GetFIFOCount(void) {
    uint8_t data[2];
    
    if (I2C_ReadRegs(MPU6050_REG_FIFO_COUNTH, data, 2)) {
        return ((uint16_t)data[0] << 8) | data[1];
    }
    
    return 0;
}

3.3 姿态解算模块 (attitude_estimator.c)

#include "gyro_mpu6050.h"
#include "math.h"

// 姿态结构体
typedef struct {
    float roll;        // 横滚角 (°)
    float pitch;       // 俯仰角 (°)
    float yaw;         // 偏航角 (°)
    float roll_rate;   // 横滚角速度 (°/s)
    float pitch_rate;  // 俯仰角速度 (°/s)
    float yaw_rate;    // 偏航角速度 (°/s)
    float quaternion[4]; // 四元数
} Attitude_t;

// 卡尔曼滤波器结构体
typedef struct {
    float q;        // 过程噪声协方差
    float r;        // 测量噪声协方差
    float x;        // 状态估计
    float p;        // 估计误差协方差
    float k;        // 卡尔曼增益
} KalmanFilter_t;

static KalmanFilter_t kalman_roll, kalman_pitch;
static Attitude_t current_attitude = {0};
static uint32_t last_update_time = 0;

// 初始化卡尔曼滤波器
void Kalman_Init(KalmanFilter_t *kalman, float q, float r) {
    kalman->q = q;
    kalman->r = r;
    kalman->x = 0.0f;
    kalman->p = 1.0f;
    kalman->k = 0.0f;
}

// 卡尔曼滤波更新
float Kalman_Update(KalmanFilter_t *kalman, float measurement) {
    // 预测步骤
    kalman->p = kalman->p + kalman->q;
    
    // 更新步骤
    kalman->k = kalman->p / (kalman->p + kalman->r);
    kalman->x = kalman->x + kalman->k * (measurement - kalman->x);
    kalman->p = (1 - kalman->k) * kalman->p;
    
    return kalman->x;
}

// 初始化姿态估计器
void AttitudeEstimator_Init(void) {
    Kalman_Init(&kalman_roll, 0.001f, 0.003f);
    Kalman_Init(&kalman_pitch, 0.001f, 0.003f);
    
    current_attitude.roll = 0.0f;
    current_attitude.pitch = 0.0f;
    current_attitude.yaw = 0.0f;
    
    // 初始化四元数
    current_attitude.quaternion[0] = 1.0f;  // w
    current_attitude.quaternion[1] = 0.0f;  // x
    current_attitude.quaternion[2] = 0.0f;  // y
    current_attitude.quaternion[3] = 0.0f;  // z
    
    last_update_time = Get_SystemTime();
}

// 互补滤波器更新姿态
void AttitudeEstimator_Update(GyroData_t *gyro_data) {
    float dt;
    uint32_t current_time = Get_SystemTime();
    
    if (last_update_time == 0) {
        last_update_time = current_time;
        return;
    }
    
    dt = (float)(current_time - last_update_time) / 1000.0f;  // 转换为秒
    last_update_time = current_time;
    
    if (dt <= 0 || dt > 1.0f) {
        return;  // 时间间隔异常
    }
    
    // 陀螺仪积分（角速度积分得到角度）
    float roll_gyro = current_attitude.roll + gyro_data->gyro_x * dt;
    float pitch_gyro = current_attitude.pitch + gyro_data->gyro_y * dt;
    float yaw_gyro = current_attitude.yaw + gyro_data->gyro_z * dt;
    
    // 加速度计计算姿态（仅用于横滚和俯仰）
    float roll_accel = atan2f(gyro_data->accel_y, gyro_data->accel_z) * 180.0f / M_PI;
    float pitch_accel = atan2f(-gyro_data->accel_x, 
                              sqrtf(gyro_data->accel_y * gyro_data->accel_y + 
                                    gyro_data->accel_z * gyro_data->accel_z)) * 180.0f / M_PI;
    
    // 互补滤波（陀螺仪为主，加速度计为辅）
    float alpha = 0.98f;  // 陀螺仪权重
    current_attitude.roll = alpha * roll_gyro + (1.0f - alpha) * roll_accel;
    current_attitude.pitch = alpha * pitch_gyro + (1.0f - alpha) * pitch_accel;
    current_attitude.yaw = yaw_gyro;  // 偏航角只能用陀螺仪积分
    
    // 卡尔曼滤波平滑
    current_attitude.roll = Kalman_Update(&kalman_roll, current_attitude.roll);
    current_attitude.pitch = Kalman_Update(&kalman_pitch, current_attitude.pitch);
    
    // 更新角速度
    current_attitude.roll_rate = gyro_data->gyro_x;
    current_attitude.pitch_rate = gyro_data->gyro_y;
    current_attitude.yaw_rate = gyro_data->gyro_z;
    
    // 更新四元数（使用陀螺仪数据）
    AttitudeEstimator_UpdateQuaternion(gyro_data, dt);
}

// 使用陀螺仪数据更新四元数
void AttitudeEstimator_UpdateQuaternion(GyroData_t *gyro_data, float dt) {
    float wx = gyro_data->gyro_x * M_PI / 180.0f;  // 转换为rad/s
    float wy = gyro_data->gyro_y * M_PI / 180.0f;
    float wz = gyro_data->gyro_z * M_PI / 180.0f;
    
    float q0 = current_attitude.quaternion[0];
    float q1 = current_attitude.quaternion[1];
    float q2 = current_attitude.quaternion[2];
    float q3 = current_attitude.quaternion[3];
    
    // 四元数微分方程
    float dq0 = 0.5f * (-q1 * wx - q2 * wy - q3 * wz);
    float dq1 = 0.5f * (q0 * wx + q2 * wz - q3 * wy);
    float dq2 = 0.5f * (q0 * wy + q3 * wx - q1 * wz);
    float dq3 = 0.5f * (q0 * wz + q1 * wy - q2 * wx);
    
    // 积分更新
    q0 += dq0 * dt;
    q1 += dq1 * dt;
    q2 += dq2 * dt;
    q3 += dq3 * dt;
    
    // 归一化
    float norm = sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    if (norm > 0) {
        q0 /= norm;
        q1 /= norm;
        q2 /= norm;
        q3 /= norm;
    }
    
    current_attitude.quaternion[0] = q0;
    current_attitude.quaternion[1] = q1;
    current_attitude.quaternion[2] = q2;
    current_attitude.quaternion[3] = q3;
}

// 获取当前姿态
Attitude_t* AttitudeEstimator_GetAttitude(void) {
    return &current_attitude;
}

// 四元数转欧拉角
void QuaternionToEuler(float *quaternion, float *roll, float *pitch, float *yaw) {
    float q0 = quaternion[0];
    float q1 = quaternion[1];
    float q2 = quaternion[2];
    float q3 = quaternion[3];
    
    // 横滚角 (roll)
    *roll = atan2f(2.0f * (q0*q1 + q2*q3), 1.0f - 2.0f * (q1*q1 + q2*q2)) * 180.0f / M_PI;
    
    // 俯仰角 (pitch)
    float sin_pitch = 2.0f * (q0*q2 - q3*q1);
    if (fabsf(sin_pitch) >= 1.0f) {
        *pitch = copysignf(M_PI / 2.0f, sin_pitch) * 180.0f / M_PI;  // 使用90度
    } else {
        *pitch = asinf(sin_pitch) * 180.0f / M_PI;
    }
    
    // 偏航角 (yaw)
    *yaw = atan2f(2.0f * (q0*q3 + q1*q2), 1.0f - 2.0f * (q2*q2 + q3*q3)) * 180.0f / M_PI;
}

3.4 主程序示例 (main.c)

#include "stm32f10x.h"
#include "gyro_mpu6050.h"
#include "attitude_estimator.h"
#include "usart.h"
#include "delay.h"
#include "led.h"

// 系统状态
typedef enum {
    SYS_INIT = 0,
    SYS_CALIBRATING,
    SYS_RUNNING,
    SYS_ERROR
} SystemState_t;

static SystemState_t system_state = SYS_INIT;
static GyroData_t gyro_data;
static uint32_t data_counter = 0;

// 系统初始化
void System_Init(void) {
    SystemClock_Init();
    Delay_Init();
    USART_Init(115200);
    LED_Init();
    I2C_Init();
    
    printf("MPU6050 Gyroscope Driver Test\r\n");
    printf("==============================\r\n");
}

// 显示姿态数据
void Display_AttitudeData(void) {
    Attitude_t *attitude = AttitudeEstimator_GetAttitude();
    
    printf("Roll: %.2f°, Pitch: %.2f°, Yaw: %.2f°\r\n", 
           attitude->roll, attitude->pitch, attitude->yaw);
    printf("Gyro: X=%.2f, Y=%.2f, Z=%.2f °/s\r\n", 
           attitude->roll_rate, attitude->pitch_rate, attitude->yaw_rate);
    printf("Accel: X=%.3f, Y=%.3f, Z=%.3f g\r\n", 
           gyro_data.accel_x, gyro_data.accel_y, gyro_data.accel_z);
    printf("Temp: %.2f°C\r\n", gyro_data.temperature);
    printf("----------------------------------------\r\n");
}

int main(void) {
    GyroCalibration_t calibration;
    
    // 系统初始化
    System_Init();
    
    // 初始化MPU6050
    if (!MPU6050_Init()) {
        printf("MPU6050 Initialization Failed!\r\n");
        system_state = SYS_ERROR;
        while(1);
    }
    
    printf("MPU6050 Initialized Successfully!\r\n");
    
    // 校准传感器
    system_state = SYS_CALIBRATING;
    LED_On(LED0);
    
    if (MPU6050_Calibrate(&calibration)) {
        MPU6050_SetCalibration(&calibration);
        printf("Calibration Successful!\r\n");
    } else {
        printf("Calibration Failed! Using default values.\r\n");
    }
    
    // 初始化姿态估计器
    AttitudeEstimator_Init();
    
    system_state = SYS_RUNNING;
    LED_Off(LED0);
    
    printf("System Running...\r\n");
    printf("Press any key to stop display\r\n");
    
    while(1) {
        // 读取传感器数据
        if (MPU6050_ReadSensorData(&gyro_data)) {
            data_counter++;
            
            // 更新姿态估计
            AttitudeEstimator_Update(&gyro_data);
            
            // 每秒显示一次数据
            if (data_counter % 50 == 0) {  // 50Hz采样，每秒显示一次
                Display_AttitudeData();
                
                // LED指示系统运行状态
                LED_Toggle(LED1);
            }
        } else {
            printf("Sensor Read Error!\r\n");
            LED_Blink(LED0, 100);  // 快速闪烁表示错误
        }
        
        Delay_ms(20);  // 50Hz采样率
    }
}

参考代码 陀螺仪传感器驱动程序 www.youwenfan.com/contentcsu/60586.html

四、工程配置与优化

4.1 编译配置

# 编译选项
CFLAGS = -O2 -Wall -Wextra -std=c99
INCLUDES = -I./Inc -I./Drivers

# 源文件
SOURCES = main.c \
          gyro_mpu6050.c \
          attitude_estimator.c \
          i2c.c \
          delay.c \
          usart.c \
          led.c

# 链接选项
LDFLAGS = -Tlinker.ld -nostdlib

4.2 性能优化建议

优化项目	建议方案
采样率优化	根据应用需求设置合适的采样率（50-200Hz）
滤波优化	使用互补滤波或卡尔曼滤波减少噪声
校准优化	定期重新校准，考虑温度影响
功耗优化	不使用时进入睡眠模式
数据融合	结合磁力计进行更好的姿态估计

4.3 常见问题解决

问题	原因	解决方案
传感器无响应	I2C地址错误或通信失败	检查硬件连接和I2C时序
数据漂移严重	未进行校准或校准不准确	重新进行静态校准
姿态解算不稳定	滤波参数不合适	调整互补滤波系数
温度影响大	未考虑温度补偿	添加温度补偿算法
振动干扰	机械振动影响	增加物理减震，软件滤波