基于STM32F103的MPU6050程序
基于STM32F103的MPU6050完整驱动程序，包含I2C通信、数据读取、DMP（数字运动处理器）集成和基本姿态解算。
一、硬件连接
MPU6050引脚 -> STM32F103引脚
-----------------------------
VCC -> 3.3V
GND -> GND
SDA -> PB7 (I2C1_SDA)
SCL -> PB6 (I2C1_SCL)
AD0 -> GND (地址0x68) 或 3.3V (地址0x69)
INT -> PB5 (可选，用于中断)
二、项目结构
MPU6050_Project/
├── Core/
│ ├── Inc/
│ │ ├── mpu6050.h
│ │ ├── i2c.h
│ │ └── dmp.h
│ ├── Src/
│ │ ├── mpu6050.c
│ │ ├── i2c.c
│ │ └── dmp.c
│ └── Startup/
└── main.c
三、核心代码实现
1. MPU6050头文件 – mpu6050.h
#ifndef __MPU6050_H
#define __MPU6050_H
#include "stm32f10x.h"
#include <math.h>
#include <stdint.h>
// MPU6050地址
#define MPU6050_ADDR 0x68 // AD0接地
// #define MPU6050_ADDR 0x69 // AD0接VCC
// MPU6050寄存器定义
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_XOUT_L 0x3C
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_YOUT_L 0x3E
#define MPU6050_ACCEL_ZOUT_H 0x3F
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_TEMP_OUT_H 0x41
#define MPU6050_TEMP_OUT_L 0x42
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_PWR_MGMT_2 0x6C
#define MPU6050_WHO_AM_I 0x75
// 量程定义
enum ACCEL_RANGE {
ACCEL_2G = 0, // ±2g
ACCEL_4G = 1, // ±4g
ACCEL_8G = 2, // ±8g
ACCEL_16G = 3 // ±16g
};
enum GYRO_RANGE {
GYRO_250DPS = 0, // ±250°/s
GYRO_500DPS = 1, // ±500°/s
GYRO_1000DPS = 2, // ±1000°/s
GYRO_2000DPS = 3 // ±2000°/s
};
// 数据结构
typedef struct {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} MPU6050_RAW_DATA;
typedef struct {
float accel_x; // m/s²
float accel_y;
float accel_z;
float temp; // °C
float gyro_x; // °/s
float gyro_y;
float gyro_z;
} MPU6050_SCALED_DATA;
typedef struct {
float roll; // 横滚角
float pitch; // 俯仰角
float yaw; // 偏航角
} ATTITUDE_ANGLES;
// 校准数据结构
typedef struct {
int16_t accel_offset_x;
int16_t accel_offset_y;
int16_t accel_offset_z;
int16_t gyro_offset_x;
int16_t gyro_offset_y;
int16_t gyro_offset_z;
} MPU6050_CALIBRATION;
// 函数声明
uint8_t MPU6050_Init(void);
uint8_t MPU6050_ReadID(void);
void MPU6050_SetGyroRange(enum GYRO_RANGE range);
void MPU6050_SetAccelRange(enum ACCEL_RANGE range);
void MPU6050_ReadRawData(MPU6050_RAW_DATA* raw_data);
void MPU6050_GetScaledData(MPU6050_SCALED_DATA* scaled_data);
float MPU6050_GetTemperature(void);
void MPU6050_Calibrate(MPU6050_CALIBRATION* calib, uint16_t num_samples);
void MPU6050_SetCalibration(MPU6050_CALIBRATION* calib);
void MPU6050_CalculateAttitude(MPU6050_SCALED_DATA* data, ATTITUDE_ANGLES* angles);
void MPU6050_EnableFIFO(void);
void MPU6050_DisableFIFO(void);
void MPU6050_Reset(void);
void MPU6050_WakeUp(void);
void MPU6050_Sleep(void);
// 低通滤波器
void ComplementaryFilter(MPU6050_SCALED_DATA* data, ATTITUDE_ANGLES* angles, float dt, float alpha);
void KalmanFilterInit(void);
void KalmanFilterUpdate(MPU6050_SCALED_DATA* data, ATTITUDE_ANGLES* angles, float dt);
#endif /* __MPU6050_H */
2. MPU6050实现文件 – mpu6050.c
#include "mpu6050.h"
#include "i2c.h"
#include <math.h>
// 全局变量
static enum GYRO_RANGE gyro_range = GYRO_250DPS;
static enum ACCEL_RANGE accel_range = ACCEL_2G;
static float gyro_scale = 0.0f;
static float accel_scale = 0.0f;
// 校准数据
static MPU6050_CALIBRATION calibration = {
0, 0, 0, // 加速度计偏移
0, 0, 0 // 陀螺仪偏移
};
// 初始化MPU6050
uint8_t MPU6050_Init(void)
{
uint8_t check = 0;
// 1. 检查设备ID
I2C_ReadByte(MPU6050_ADDR, MPU6050_WHO_AM_I, &check);
if (check != 0x68) { // MPU6050设备ID为0x68
return 0; // 初始化失败
}
// 2. 唤醒设备
I2C_WriteByte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, 0x00);
// 3. 设置采样率分频器
I2C_WriteByte(MPU6050_ADDR, MPU6050_SMPLRT_DIV, 0x07); // 1kHz/(7+1)=125Hz
// 4. 设置配置寄存器
I2C_WriteByte(MPU6050_ADDR, MPU6050_CONFIG, 0x00); // 无DLPF
// 5. 设置陀螺仪量程
MPU6050_SetGyroRange(GYRO_500DPS);
// 6. 设置加速度计量程
MPU6050_SetAccelRange(ACCEL_2G);
return 1; // 初始化成功
}
// 读取设备ID
uint8_t MPU6050_ReadID(void)
{
uint8_t id = 0;
I2C_ReadByte(MPU6050_ADDR, MPU6050_WHO_AM_I, &id);
return id;
}
// 设置陀螺仪量程
void MPU6050_SetGyroRange(enum GYRO_RANGE range)
{
gyro_range = range;
I2C_WriteByte(MPU6050_ADDR, MPU6050_GYRO_CONFIG, range << 3);
// 设置比例因子
switch(range) {
case GYRO_250DPS:
gyro_scale = 250.0f / 32768.0f; // 250°/s
break;
case GYRO_500DPS:
gyro_scale = 500.0f / 32768.0f; // 500°/s
break;
case GYRO_1000DPS:
gyro_scale = 1000.0f / 32768.0f; // 1000°/s
break;
case GYRO_2000DPS:
gyro_scale = 2000.0f / 32768.0f; // 2000°/s
break;
}
}
// 设置加速度计量程
void MPU6050_SetAccelRange(enum ACCEL_RANGE range)
{
accel_range = range;
I2C_WriteByte(MPU6050_ADDR, MPU6050_ACCEL_CONFIG, range << 3);
// 设置比例因子
switch(range) {
case ACCEL_2G:
accel_scale = 2.0f * 9.81f / 32768.0f; // 2g -> m/s²
break;
case ACCEL_4G:
accel_scale = 4.0f * 9.81f / 32768.0f; // 4g
break;
case ACCEL_8G:
accel_scale = 8.0f * 9.81f / 32768.0f; // 8g
break;
case ACCEL_16G:
accel_scale = 16.0f * 9.81f / 32768.0f; // 16g
break;
}
}
// 读取原始数据
void MPU6050_ReadRawData(MPU6050_RAW_DATA* raw_data)
{
uint8_t buffer[14];
// 读取14个字节的传感器数据
I2C_ReadBytes(MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, buffer, 14);
// 加速度计数据
raw_data->accel_x = (int16_t)((buffer[0] << 8) | buffer[1]);
raw_data->accel_y = (int16_t)((buffer[2] << 8) | buffer[3]);
raw_data->accel_z = (int16_t)((buffer[4] << 8) | buffer[5]);
// 温度数据
raw_data->temp = (int16_t)((buffer[6] << 8) | buffer[7]);
// 陀螺仪数据
raw_data->gyro_x = (int16_t)((buffer[8] << 8) | buffer[9]);
raw_data->gyro_y = (int16_t)((buffer[10] << 8) | buffer[11]);
raw_data->gyro_z = (int16_t)((buffer[12] << 8) | buffer[12]);
// 应用校准偏移
raw_data->accel_x -= calibration.accel_offset_x;
raw_data->accel_y -= calibration.accel_offset_y;
raw_data->accel_z -= calibration.accel_offset_z;
raw_data->gyro_x -= calibration.gyro_offset_x;
raw_data->gyro_y -= calibration.gyro_offset_y;
raw_data->gyro_z -= calibration.gyro_offset_z;
}
// 获取标定后的数据
void MPU6050_GetScaledData(MPU6050_SCALED_DATA* scaled_data)
{
MPU6050_RAW_DATA raw_data;
// 读取原始数据
MPU6050_ReadRawData(&raw_data);
// 加速度计转换为m/s²
scaled_data->accel_x = raw_data.accel_x * accel_scale;
scaled_data->accel_y = raw_data.accel_y * accel_scale;
scaled_data->accel_z = raw_data.accel_z * accel_scale;
// 温度转换为摄氏度
scaled_data->temp = raw_data.temp / 340.0f + 36.53f;
// 陀螺仪转换为°/s
scaled_data->gyro_x = raw_data.gyro_x * gyro_scale;
scaled_data->gyro_y = raw_data.gyro_y * gyro_scale;
scaled_data->gyro_z = raw_data.gyro_z * gyro_scale;
}
// 获取温度
float MPU6050_GetTemperature(void)
{
uint8_t buffer[2];
int16_t temp_raw;
I2C_ReadBytes(MPU6050_ADDR, MPU6050_TEMP_OUT_H, buffer, 2);
temp_raw = (int16_t)((buffer[0] << 8) | buffer[1]);
return temp_raw / 340.0f + 36.53f;
}
// 校准MPU6050
void MPU6050_Calibrate(MPU6050_CALIBRATION* calib, uint16_t num_samples)
{
int32_t accel_sum_x = 0, accel_sum_y = 0, accel_sum_z = 0;
int32_t gyro_sum_x = 0, gyro_sum_y = 0, gyro_sum_z = 0;
MPU6050_RAW_DATA raw_data;
// 确保设备水平放置
for (uint16_t i = 0; i < num_samples; i++) {
MPU6050_ReadRawData(&raw_data);
accel_sum_x += raw_data.accel_x;
accel_sum_y += raw_data.accel_y;
accel_sum_z += raw_data.accel_z;
gyro_sum_x += raw_data.gyro_x;
gyro_sum_y += raw_data.gyro_y;
gyro_sum_z += raw_data.gyro_z;
Delay_ms(2); // 延时2ms
}
// 计算平均值
calib->accel_offset_x = accel_sum_x / num_samples;
calib->accel_offset_y = accel_sum_y / num_samples;
calib->accel_offset_z = (accel_sum_z / num_samples) - 16384; // 减去1g
calib->gyro_offset_x = gyro_sum_x / num_samples;
calib->gyro_offset_y = gyro_sum_y / num_samples;
calib->gyro_offset_z = gyro_sum_z / num_samples;
}
// 设置校准数据
void MPU6050_SetCalibration(MPU6050_CALIBRATION* calib)
{
calibration = *calib;
}
// 计算姿态角（互补滤波）
void MPU6050_CalculateAttitude(MPU6050_SCALED_DATA* data, ATTITUDE_ANGLES* angles)
{
static float roll = 0.0f, pitch = 0.0f, yaw = 0.0f;
static float last_time = 0.0f;
float current_time, dt;
// 获取当前时间
current_time = Get_System_Time(); // 需要实现此函数
dt = (current_time - last_time) / 1000.0f; // 转换为秒
// 计算加速度计角度
float accel_roll = atan2f(data->accel_y, data->accel_z) * 180.0f / M_PI;
float accel_pitch = atan2f(-data->accel_x,
sqrtf(data->accel_y*data->accel_y + data->accel_z*data->accel_z))
* 180.0f / M_PI;
// 互补滤波参数
float alpha = 0.98f; // 陀螺仪权重
// 融合加速度计和陀螺仪数据
roll = alpha * (roll + data->gyro_x * dt) + (1 - alpha) * accel_roll;
pitch = alpha * (pitch + data->gyro_y * dt) + (1 - alpha) * accel_pitch;
yaw += data->gyro_z * dt; // 磁力计可改进
angles->roll = roll;
angles->pitch = pitch;
angles->yaw = yaw;
last_time = current_time;
}
// 启用FIFO
void MPU6050_EnableFIFO(void)
{
uint8_t reg = 0;
// 启用FIFO
I2C_WriteByte(MPU6050_ADDR, 0x6A, 0x40); // FIFO_EN = 1
// 配置传感器到FIFO
I2C_ReadByte(MPU6050_ADDR, 0x23, ®);
reg |= 0x78; // 加速度计和温度
I2C_WriteByte(MPU6050_ADDR, 0x23, reg);
}
// 禁用FIFO
void MPU6050_DisableFIFO(void)
{
I2C_WriteByte(MPU6050_ADDR, 0x6A, 0x00); // FIFO_EN = 0
}
// 复位设备
void MPU6050_Reset(void)
{
I2C_WriteByte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, 0x80);
Delay_ms(100); // 等待复位完成
}
// 唤醒设备
void MPU6050_WakeUp(void)
{
uint8_t reg = 0;
I2C_ReadByte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, ®);
reg &= ~(1 << 6); // 清除SLEEP位
I2C_WriteByte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, reg);
}
// 睡眠设备
void MPU6050_Sleep(void)
{
uint8_t reg = 0;
I2C_ReadByte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, ®);
reg |= (1 << 6); // 设置SLEEP位
I2C_WriteByte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, reg);
}
3. I2C驱动 – i2c.h
#ifndef __I2C_H
#define __I2C_H
#include "stm32f10x.h"
// I2C引脚定义
#define I2Cx I2C1
#define I2Cx_CLK RCC_APB1Periph_I2C1
#define I2Cx_SDA_PIN GPIO_Pin_7
#define I2Cx_SCL_PIN GPIO_Pin_6
#define I2Cx_GPIO_PORT GPIOB
#define I2Cx_GPIO_CLK RCC_APB2Periph_GPIOB
#define I2Cx_SPEED 100000 // 100kHz
// 函数声明
void I2C_Init(void);
uint8_t I2C_WriteByte(uint8_t dev_addr, uint8_t reg_addr, uint8_t data);
uint8_t I2C_ReadByte(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data);
uint8_t I2C_ReadBytes(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint8_t len);
uint8_t I2C_WriteBytes(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint8_t len);
uint8_t I2C_CheckDevice(uint8_t dev_addr);
#endif /* __I2C_H */
4. I2C驱动 – i2c.c
#include "i2c.h"
#include "delay.h"
// 初始化I2C
void I2C_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
// 使能时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
// 配置GPIO
GPIO_InitStructure.GPIO_Pin = I2Cx_SCL_PIN | I2Cx_SDA_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; // 开漏输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(I2Cx_GPIO_PORT, &GPIO_InitStructure);
// 配置I2C
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = 0x00; // 主机地址
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_ClockSpeed = I2Cx_SPEED;
I2C_Init(I2Cx, &I2C_InitStructure);
I2C_Cmd(I2Cx, ENABLE);
}
// 等待I2C事件
static uint8_t I2C_WaitEvent(uint32_t event)
{
uint16_t timeout = 10000;
while (I2C_CheckEvent(I2Cx, event) != SUCCESS) {
timeout--;
if (timeout == 0) {
I2C_GenerateSTOP(I2Cx, ENABLE);
return 0; // 超时
}
}
return 1;
}
// 写入一个字节
uint8_t I2C_WriteByte(uint8_t dev_addr, uint8_t reg_addr, uint8_t data)
{
// 1. 发送开始信号
I2C_GenerateSTART(I2Cx, ENABLE);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) return 0;
// 2. 发送设备地址（写模式）
I2C_Send7bitAddress(I2Cx, dev_addr << 1, I2C_Direction_Transmitter);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) return 0;
// 3. 发送寄存器地址
I2C_SendData(I2Cx, reg_addr);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) return 0;
// 4. 发送数据
I2C_SendData(I2Cx, data);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) return 0;
// 5. 发送停止信号
I2C_GenerateSTOP(I2Cx, ENABLE);
return 1;
}
// 读取一个字节
uint8_t I2C_ReadByte(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data)
{
// 1. 发送开始信号
I2C_GenerateSTART(I2Cx, ENABLE);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) return 0;
// 2. 发送设备地址（写模式）
I2C_Send7bitAddress(I2Cx, dev_addr << 1, I2C_Direction_Transmitter);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) return 0;
// 3. 发送寄存器地址
I2C_SendData(I2Cx, reg_addr);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) return 0;
// 4. 发送重复开始信号
I2C_GenerateSTART(I2Cx, ENABLE);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) return 0;
// 5. 发送设备地址（读模式）
I2C_Send7bitAddress(I2Cx, dev_addr << 1, I2C_Direction_Receiver);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) return 0;
// 6. 禁用应答
I2C_AcknowledgeConfig(I2Cx, DISABLE);
I2C_GenerateSTOP(I2Cx, ENABLE);
// 7. 等待数据接收
if (!I2C_WaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED)) return 0;
// 8. 读取数据
*data = I2C_ReceiveData(I2Cx);
return 1;
}
// 读取多个字节
uint8_t I2C_ReadBytes(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint8_t len)
{
if (len == 0) return 0;
// 1. 发送开始信号
I2C_GenerateSTART(I2Cx, ENABLE);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) return 0;
// 2. 发送设备地址（写模式）
I2C_Send7bitAddress(I2Cx, dev_addr << 1, I2C_Direction_Transmitter);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) return 0;
// 3. 发送寄存器地址
I2C_SendData(I2Cx, reg_addr);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) return 0;
// 4. 发送重复开始信号
I2C_GenerateSTART(I2Cx, ENABLE);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) return 0;
// 5. 发送设备地址（读模式）
I2C_Send7bitAddress(I2Cx, dev_addr << 1, I2C_Direction_Receiver);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) return 0;
// 6. 读取多个字节
while (len > 1) {
if (!I2C_WaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED)) return 0;
*data++ = I2C_ReceiveData(I2Cx);
len--;
}
// 7. 最后一个字节
I2C_AcknowledgeConfig(I2Cx, DISABLE);
I2C_GenerateSTOP(I2Cx, ENABLE);
if (!I2C_WaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED)) return 0;
*data = I2C_ReceiveData(I2Cx);
// 重新使能应答
I2C_AcknowledgeConfig(I2Cx, ENABLE);
return 1;
}
// 检查设备是否在线
uint8_t I2C_CheckDevice(uint8_t dev_addr)
{
uint8_t status = 0;
I2C_GenerateSTART(I2Cx, ENABLE);
status = I2C_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT);
if (!status) {
I2C_GenerateSTOP(I2Cx, ENABLE);
return 0;
}
I2C_Send7bitAddress(I2Cx, dev_addr << 1, I2C_Direction_Transmitter);
status = I2C_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);
I2C_GenerateSTOP(I2Cx, ENABLE);
return status;
}
5. 卡尔曼滤波器实现
// kalman_filter.h
#ifndef __KALMAN_FILTER_H
#define __KALMAN_FILTER_H
#include <math.h>
typedef struct {
float Q_angle; // 过程噪声协方差
float Q_gyro; // 过程噪声协方差
float R_angle; // 测量噪声协方差
float x_angle; // 角度估计值
float x_bias; // 偏差估计值
float P[2][2]; // 估计误差协方差矩阵
float K[2]; // 卡尔曼增益
float y; // 残差
float S; // 估计误差
} KalmanFilter;
void Kalman_Init(KalmanFilter* kf, float Q_angle, float Q_gyro, float R_angle);
float Kalman_Update(KalmanFilter* kf, float newAngle, float newRate, float dt);
#endif /* __KALMAN_FILTER_H */
// kalman_filter.c
#include "kalman_filter.h"
// 初始化卡尔曼滤波器
void Kalman_Init(KalmanFilter* kf, float Q_angle, float Q_gyro, float R_angle)
{
kf->Q_angle = Q_angle;
kf->Q_gyro = Q_gyro;
kf->R_angle = R_angle;
kf->x_angle = 0.0f;
kf->x_bias = 0.0f;
// 初始化协方差矩阵
kf->P[0][0] = 0.0f;
kf->P[0][1] = 0.0f;
kf->P[1][0] = 0.0f;
kf->P[1][1] = 0.0f;
}
// 卡尔曼滤波器更新
float Kalman_Update(KalmanFilter* kf, float newAngle, float newRate, float dt)
{
// 预测步骤
kf->x_angle += dt * (newRate - kf->x_bias);
kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);
kf->P[0][1] -= dt * kf->P[1][1];
kf->P[1][0] -= dt * kf->P[1][1];
kf->P[1][1] += kf->Q_gyro * dt;
// 更新步骤
kf->S = kf->P[0][0] + kf->R_angle;
kf->K[0] = kf->P[0][0] / kf->S;
kf->K[1] = kf->P[1][0] / kf->S;
kf->y = newAngle - kf->x_angle;
kf->x_angle += kf->K[0] * kf->y;
kf->x_bias += kf->K[1] * kf->y;
kf->P[0][0] -= kf->K[0] * kf->P[0][0];
kf->P[0][1] -= kf->K[0] * kf->P[0][1];
kf->P[1][0] -= kf->K[1] * kf->P[0][0];
kf->P[1][1] -= kf->K[1] * kf->P[0][1];
return kf->x_angle;
}
6. 主程序 – main.c
#include "stm32f10x.h"
#include "mpu6050.h"
#include "i2c.h"
#include "delay.h"
#include "usart.h"
#include <stdio.h>
// 全局变量
KalmanFilter kalman_roll, kalman_pitch;
MPU6050_SCALED_DATA mpu_data;
ATTITUDE_ANGLES angles;
MPU6050_CALIBRATION calib_data;
// 系统时钟初始化
void SystemClock_Config(void)
{
RCC_DeInit();
RCC_HSEConfig(RCC_HSE_ON);
while (RCC_GetFlagStatus(RCC_FLAG_HSERDY) == RESET);
RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);
RCC_PLLCmd(ENABLE);
while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET);
RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
while (RCC_GetSYSCLKSource() != 0x08);
RCC_HCLKConfig(RCC_SYSCLK_Div1);
RCC_PCLK1Config(RCC_HCLK_Div2);
RCC_PCLK2Config(RCC_HCLK_Div1);
SystemCoreClockUpdate();
}
// 延时函数
void Delay_ms(uint32_t ms)
{
uint32_t i, j;
for (i = 0; i < ms; i++) {
for (j = 0; j < 7200; j++);
}
}
// 获取系统时间（毫秒）
float Get_System_Time(void)
{
return (float)SysTick->VAL;
}
int main(void)
{
uint8_t mpu_id = 0;
char buffer[100];
// 系统初始化
SystemClock_Config();
Delay_Init();
USART1_Init(115200);
I2C_Init();
printf("MPU6050 Test Program\r\n");
// 初始化MPU6050
if (MPU6050_Init()) {
printf("MPU6050 Initialized Successfully!\r\n");
} else {
printf("MPU6050 Initialization Failed!\r\n");
while(1);
}
// 读取设备ID
mpu_id = MPU6050_ReadID();
printf("MPU6050 Device ID: 0x%02X\r\n", mpu_id);
// 校准MPU6050
printf("Calibrating MPU6050...\r\n");
printf("Please keep the device stationary and horizontal!\r\n");
Delay_ms(2000);
MPU6050_Calibrate(&calib_data, 1000);
MPU6050_SetCalibration(&calib_data);
printf("Calibration Complete!\r\n");
printf("Accel Offsets: X=%d, Y=%d, Z=%d\r\n",
calib_data.accel_offset_x,
calib_data.accel_offset_y,
calib_data.accel_offset_z);
printf("Gyro Offsets: X=%d, Y=%d, Z=%d\r\n",
calib_data.gyro_offset_x,
calib_data.gyro_offset_y,
calib_data.gyro_offset_z);
// 初始化卡尔曼滤波器
Kalman_Init(&kalman_roll, 0.001f, 0.003f, 0.03f);
Kalman_Init(&kalman_pitch, 0.001f, 0.003f, 0.03f);
printf("\r\nRoll\tPitch\tYaw\tAccelX\tAccelY\tAccelZ\tTemp\r\n");
while (1) {
static uint32_t last_time = 0;
uint32_t current_time = SysTick->VAL;
float dt = (current_time - last_time) / 1000.0f; // 转换为秒
if (dt < 0.01f) { // 至少10ms更新一次
Delay_ms(1);
continue;
}
// 读取MPU6050数据
MPU6050_GetScaledData(&mpu_data);
// 计算姿态角
MPU6050_CalculateAttitude(&mpu_data, &angles);
// 使用卡尔曼滤波器
angles.roll = Kalman_Update(&kalman_roll, angles.roll, mpu_data.gyro_x, dt);
angles.pitch = Kalman_Update(&kalman_pitch, angles.pitch, mpu_data.gyro_y, dt);
// 打印数据
printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.1f\r\n",
angles.roll, angles.pitch, angles.yaw,
mpu_data.accel_x, mpu_data.accel_y, mpu_data.accel_z,
mpu_data.temp);
last_time = current_time;
Delay_ms(20); // 50Hz更新率
}
}
四、串口输出配置
// usart.h
#ifndef __USART_H
#define __USART_H
#include "stm32f10x.h"
#include <stdio.h>
void USART1_Init(uint32_t baudrate);
int fputc(int ch, FILE *f);
int fgetc(FILE *f);
#endif /* __USART_H */
// usart.c
#include "usart.h"
#include <stdio.h>
// 重定向printf
int fputc(int ch, FILE *f)
{
USART_SendData(USART1, (uint8_t)ch);
while (USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);
return ch;
}
int fgetc(FILE *f)
{
while (USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET);
return (int)USART_ReceiveData(USART1);
}
// 初始化USART1
void USART1_Init(uint32_t baudrate)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
// 使能时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOA, ENABLE);
// 配置TX引脚(PA9)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// 配置RX引脚(PA10)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// 配置USART1
USART_InitStructure.USART_BaudRate = baudrate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART1, &USART_InitStructure);
USART_Cmd(USART1, ENABLE);
}
参考代码 基于STM32F103的MPU6050程序 www.youwenfan.com/contentcsv/71973.html
五、使用示例
1. 基本读取示例
void Read_MPU6050_Example(void)
{
MPU6050_SCALED_DATA data;
// 初始化MPU6050
MPU6050_Init();
while(1) {
// 读取数据
MPU6050_GetScaledData(&data);
// 打印数据
printf("Accel: X=%.2f, Y=%.2f, Z=%.2f m/s²\r\n",
data.accel_x, data.accel_y, data.accel_z);
printf("Gyro: X=%.2f, Y=%.2f, Z=%.2f °/s\r\n",
data.gyro_x, data.gyro_y, data.gyro_z);
printf("Temp: %.1f °C\r\n", data.temp);
Delay_ms(100);
}
}
2. 姿态解算示例
void Attitude_Calculation_Example(void)
{
MPU6050_SCALED_DATA data;
ATTITUDE_ANGLES angles;
// 初始化
MPU6050_Init();
// 校准
MPU6050_CALIBRATION calib;
MPU6050_Calibrate(&calib, 1000);
MPU6050_SetCalibration(&calib);
while(1) {
// 读取并计算姿态
MPU6050_GetScaledData(&data);
MPU6050_CalculateAttitude(&data, &angles);
// 输出姿态角
printf("Roll: %.2f°, Pitch: %.2f°, Yaw: %.2f°\r\n",
angles.roll, angles.pitch, angles.yaw);
Delay_ms(20);
}
}
六、DMP（数字运动处理器）配置
如果你需要更高精度的姿态解算，可以使用MPU6050内置的DMP：
// dmp.h
#ifndef __DMP_H
#define __DMP_H
#include "mpu6050.h"
// DMP相关函数
uint8_t MPU6050_DMP_Init(void);
uint8_t MPU6050_DMP_GetData(ATTITUDE_ANGLES* angles);
void MPU6050_DMP_GetQuaternion(float* q);
void MPU6050_DMP_GetEuler(ATTITUDE_ANGLES* angles);
uint8_t MPU6050_DMP_LoadFirmware(void);
#endif /* __DMP_H */
七、调试技巧
-
硬件检查：
- 确保I2C上拉电阻（4.7kΩ）已连接
- 检查电源电压（3.3V）
- 确认AD0引脚电平
-
软件调试：
// 测试I2C通信 if (I2C_CheckDevice(MPU6050_ADDR)) { printf("MPU6050 detected!\r\n"); } else { printf("MPU6050 not found!\r\n"); } -
校准步骤：
- 将MPU6050水平静止放置
- 运行校准函数
- 保存校准参数
-
数据验证：
- 静止时加速度Z轴应接近9.81m/s²
- 静止时陀螺仪三轴应接近0
- 温度应接近室温
八、注意事项
- 时钟配置：确保系统时钟正确配置
- 延时函数：需要准确的延时函数
- 浮点运算：STM32F103没有硬件浮点，浮点运算较慢
- 滤波算法：根据应用需求选择合适的滤波算法
- 数据融合：可结合磁力计（HMC5883L）提高航向精度
