硬件准备
| 器件 |
型号 |
数量 |
| 主控 |
STM32F103C8T6 最小系统板 |
1 |
| 传感器 |
MPU6050 模块（GY-521） |
1 |
| 下载器 |
ST-Link V2 |
1 |
| 连接线 |
杜邦线 4 根 |
4 |
硬件连接（I2C1）
1
2
3
4
5
| STM32F103C8T6 MPU6050 (GY-521)
PB6 ──── SCL ──── SCL
PB7 ──── SDA ──── SDA
3.3V ──── VCC ──── VCC
GND ──── GND ──── GND
|
I2C1 复用引脚：PB6=SCL, PB7=SDA。MPU6050 默认 7 位地址 0x68 << 1 = 0xD0（AD0 接地）。
CubeMX 配置
| 外设 |
配置 |
说明 |
| I2C1 |
Fast Mode 400KHz, PB6/PB7 |
MPU6050 通信 |
| USART1 |
115200-8-N-1, PA9(TX) |
串口输出角度 |
| TIM2 |
8ms 定时中断, 72MHz→PSC=71, ARR=7999 |
125Hz 采样节拍 |
| SYS |
Debug: Serial Wire |
ST-Link 调试 |
Clock: HSE 8MHz → PLL ×9 = 72MHz, APB1=36MHz, APB2=72MHz。
软件架构
1
2
3
4
5
| main.c
├─ MPU6050_Init() HAL_I2C 初始化传感器寄存器
├─ MPU6050_Read_All() HAL_I2C_Mem_Read 批量读 14 字节
├─ Complementary_Filter() 互补滤波融合
└─ HAL_TIM_PeriodElapsedCallback() 定时中断 → 读数据 → 算角度 → 串口输出
|
代码实现
1. MPU6050 寄存器宏定义
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
|
#ifndef __MPU6050_H
#define __MPU6050_H
#include "stm32f1xx_hal.h"
#define MPU6050_ADDR 0x68
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_XOUT_H 0x3B
#define PWR_MGMT_1 0x6B
void MPU6050_Init(I2C_HandleTypeDef *hi2c);
void MPU6050_Read_All(I2C_HandleTypeDef *hi2c, int16_t *accel, int16_t *gyro);
#endif
|
2. MPU6050 驱动（HAL 实现）
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
|
#include "mpu6050.h"
static void MPU6050_WriteReg(I2C_HandleTypeDef *hi2c, uint8_t reg, uint8_t data) {
HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR << 1, reg,
I2C_MEMADD_SIZE_8BIT, &data, 1, 100);
}
void MPU6050_Init(I2C_HandleTypeDef *hi2c) {
MPU6050_WriteReg(hi2c, PWR_MGMT_1, 0x00);
HAL_Delay(100);
MPU6050_WriteReg(hi2c, SMPLRT_DIV, 0x07);
MPU6050_WriteReg(hi2c, CONFIG, 0x06);
MPU6050_WriteReg(hi2c, GYRO_CONFIG, 0x18);
MPU6050_WriteReg(hi2c, ACCEL_CONFIG, 0x10);
}
void MPU6050_Read_All(I2C_HandleTypeDef *hi2c, int16_t *accel, int16_t *gyro) {
uint8_t buf[14];
HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR << 1, ACCEL_XOUT_H,
I2C_MEMADD_SIZE_8BIT, buf, 14, 200);
accel[0] = (int16_t)(buf[0] << 8 | buf[1]);
accel[1] = (int16_t)(buf[2] << 8 | buf[3]);
accel[2] = (int16_t)(buf[4] << 8 | buf[5]);
gyro[0] = (int16_t)(buf[8] << 8 | buf[9]);
gyro[1] = (int16_t)(buf[10] << 8 | buf[11]);
gyro[2] = (int16_t)(buf[12] << 8 | buf[13]);
}
|
3. 姿态解算
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
|
#ifndef __ATTITUDE_H
#define __ATTITUDE_H
#include <math.h>
#define M_PI_F 3.14159265f
typedef struct {
float roll;
float pitch;
float yaw;
} Attitude_t;
extern Attitude_t atti;
void Attitude_Complementary(int16_t *accel, int16_t *gyro, float dt);
#endif
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
|
#include "attitude.h"
Attitude_t atti = {0};
void Attitude_Complementary(int16_t *accel, int16_t *gyro, float dt) {
float ax = accel[0], ay = accel[1], az = accel[2];
float gx = gyro[0], gy = gyro[1], gz = gyro[2];
float accel_pitch = atan2f(-ax, sqrtf(ay * ay + az * az)) * 180.0f / M_PI_F;
float accel_roll = atan2f( ay, az) * 180.0f / M_PI_F;
float gyro_rate_x = gx / 16.384f;
float gyro_rate_y = gy / 16.384f;
float gyro_rate_z = gz / 16.384f;
#define ALPHA 0.96f
atti.roll = ALPHA * (atti.roll + gyro_rate_x * dt) + (1.0f - ALPHA) * accel_roll;
atti.pitch = ALPHA * (atti.pitch + gyro_rate_y * dt) + (1.0f - ALPHA) * accel_pitch;
atti.yaw += gyro_rate_z * dt;
}
|
3.2 卡尔曼滤波（进阶方案）
互补滤波简单但参数固定。卡尔曼滤波能自适应收敛，精度更高。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
|
#ifndef __KALMAN_H
#define __KALMAN_H
typedef struct {
float Q_angle;
float Q_gyro;
float R_measure;
float angle;
float bias;
float rate;
float P[2][2];
} Kalman_t;
void Kalman_Init(Kalman_t *k, float Q_angle, float Q_gyro, float R_measure);
float Kalman_Update(Kalman_t *k, float measured_angle, float gyro_rate, float dt);
#endif
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
|
#include "kalman.h"
void Kalman_Init(Kalman_t *k, float Q_angle, float Q_gyro, float R_measure) {
k->Q_angle = Q_angle;
k->Q_gyro = Q_gyro;
k->R_measure = R_measure;
k->angle = 0.0f;
k->bias = 0.0f;
k->rate = 0.0f;
k->P[0][0] = 0.0f;
k->P[0][1] = 0.0f;
k->P[1][0] = 0.0f;
k->P[1][1] = 0.0f;
}
float Kalman_Update(Kalman_t *k, float measured_angle, float gyro_rate, float dt) {
k->rate = gyro_rate - k->bias;
k->angle += k->rate * dt;
k->P[0][0] += dt * (dt * k->P[1][1] - k->P[0][1] - k->P[1][0] + k->Q_angle);
k->P[0][1] -= dt * k->P[1][1];
k->P[1][0] -= dt * k->P[1][1];
k->P[1][1] += k->Q_gyro * dt;
float y = measured_angle - k->angle;
float S = k->P[0][0] + k->R_measure;
float K0 = k->P[0][0] / S;
float K1 = k->P[1][0] / S;
k->angle += K0 * y;
k->bias += K1 * y;
float P00_temp = k->P[0][0];
float P01_temp = k->P[0][1];
k->P[0][0] -= K0 * P00_temp;
k->P[0][1] -= K0 * P01_temp;
k->P[1][0] -= K1 * P00_temp;
k->P[1][1] -= K1 * P01_temp;
return k->angle;
}
|
主函数集成：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
|
#include "kalman.h"
Kalman_t kalman_x, kalman_y;
int main(void) {
Kalman_Init(&kalman_x, 0.001f, 0.003f, 0.03f);
Kalman_Init(&kalman_y, 0.001f, 0.003f, 0.03f);
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
if (htim->Instance != TIM2) return;
static uint8_t cnt = 0;
static int16_t accel[3], gyro[3];
MPU6050_Read_All(&hi2c1, accel, gyro);
float accel_roll = atan2f(accel[1], accel[2]) * 180.0f / M_PI_F;
float accel_pitch = atan2f(-accel[0], sqrtf(accel[1]*accel[1] + accel[2]*accel[2])) * 180.0f / M_PI_F;
float gyro_x = gyro[0] / 16.384f;
float gyro_y = gyro[1] / 16.384f;
atti.roll = Kalman_Update(&kalman_x, accel_roll, gyro_x, 0.008f);
atti.pitch = Kalman_Update(&kalman_y, accel_pitch, gyro_y, 0.008f);
atti.yaw += gyro[2] / 16.384f * 0.008f;
cnt++;
if (cnt >= 25) {
cnt = 0;
printf("Roll:%.2f Pitch:%.2f Yaw:%.2f\r\n", atti.roll, atti.pitch, atti.yaw);
}
}
|
滤波算法对比
| 特性 |
互补滤波 |
卡尔曼滤波 |
| 计算量 |
极小（3 行运算） |
中等（矩阵运算） |
| 参数调节 |
1 个参数 ALPHA |
3 个参数 Q_angle/Q_gyro/R |
| 收敛速度 |
固定 |
自适应（增益自动调整） |
| 精度 |
中等 |
高 |
| 零偏估计 |
无 |
自动估计并补偿陀螺仪零偏 |
| RAM |
12 字节 |
~40 字节/轴 |
| 适合场景 |
实时性要求高, STM32F0 |
精度要求高, STM32F1/F4 |
卡尔曼参数调优
| 参数 |
增大效果 |
减小效果 |
Q_angle |
更信任模型预测，响应快但更抖 |
更平滑但响应慢 |
Q_gyro |
更快追踪陀螺仪零偏变化 |
零偏估计更稳定 |
R_measure |
更不信任加速度计，抗振动好 |
更快收敛到加速度计角度 |
推荐起步值：Q_angle=0.001, Q_gyro=0.003, R=0.03
4. 主函数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
|
#include "stm32f1xx_hal.h"
#include "mpu6050.h"
#include "attitude.h"
#include <stdio.h>
I2C_HandleTypeDef hi2c1;
UART_HandleTypeDef huart1;
TIM_HandleTypeDef htim2;
static void MX_I2C1_Init(void) {
hi2c1.Instance = I2C1;
hi2c1.Init.ClockSpeed = 400000;
hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
HAL_I2C_Init(&hi2c1);
}
static void MX_USART1_UART_Init(void) {
huart1.Instance = USART1;
huart1.Init.BaudRate = 115200;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
huart1.Init.Mode = UART_MODE_TX;
HAL_UART_Init(&huart1);
}
static void MX_TIM2_Init(void) {
htim2.Instance = TIM2;
htim2.Init.Prescaler = 71;
htim2.Init.Period = 7999;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&htim2);
HAL_TIM_Base_Start_IT(&htim2);
}
int fputc(int ch, FILE *f) {
HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 10);
return ch;
}
void HAL_I2C_MspInit(I2C_HandleTypeDef *hi2c) {
GPIO_InitTypeDef GPIO_Init = {0};
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_I2C1_CLK_ENABLE();
GPIO_Init.Pin = GPIO_PIN_6 | GPIO_PIN_7;
GPIO_Init.Mode = GPIO_MODE_AF_OD;
GPIO_Init.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_Init);
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
if (htim->Instance != TIM2) return;
static uint8_t cnt = 0;
static int16_t accel[3], gyro[3];
MPU6050_Read_All(&hi2c1, accel, gyro);
Attitude_Complementary(accel, gyro, 0.008f);
cnt++;
if (cnt >= 25) {
cnt = 0;
printf("Roll:%.2f Pitch:%.2f Yaw:%.2f\r\n",
atti.roll, atti.pitch, atti.yaw);
}
}
int main(void) {
HAL_Init();
SystemClock_Config();
MX_I2C1_Init();
MX_USART1_UART_Init();
MX_TIM2_Init();
MPU6050_Init(&hi2c1);
printf("STM32F103 + MPU6050 (HAL) 姿态解算\r\n");
while (1) {
}
}
|
关键 HAL API 对比
| 功能 |
标准库 (SPL) |
HAL 库 |
| 写寄存器 |
I2C_GenerateSTART→I2C_SendData…（6 步） |
HAL_I2C_Mem_Write（1 步） |
| 读寄存器 |
同上，手动序列 |
HAL_I2C_Mem_Read（1 步） |
| 串口发送 |
USART_SendData |
HAL_UART_Transmit |
| 定时中断 |
TIM_ITConfig |
HAL_TIM_Base_Start_IT |
| 引脚初始化 |
GPIO_Init |
HAL_GPIO_Init |
运行结果
串口助手输出（115200bps）：
1
2
3
4
| STM32F103 + MPU6050 (HAL) 姿态解算
Roll:1.23 Pitch:-0.56 Yaw:45.12
Roll:1.18 Pitch:-0.52 Yaw:45.20
Roll:1.15 Pitch:-0.49 Yaw:45.28
|
转动模块，Roll/Pitch 实时变化，Yaw 持续积分（无磁力计会漂移）。
调参指南
| 参数 |
位置 |
作用 |
建议 |
ALPHA |
attitude.c |
陀螺仪权重 |
静止 0.90, 动态 0.98 |
低通滤波 0x06 |
mpu6050.c |
去抖 |
5Hz(0x06) 适合静态, 20Hz(0x03) 适合动态 |
采样率分频 0x07 |
mpu6050.c |
输出频率 |
125Hz 够用, 快速运动可改 200Hz |
量程 GYRO 0x18 |
mpu6050.c |
陀螺仪满量程 |
±2000°/s 通用, 云台可改 ±250°/s |
| 定时器周期 |
main.c |
采样间隔 |
8ms(125Hz) 配合采样率必须一致 |
扩展方向
| 方向 |
方案 |
难度 |
| DMP 解算 |
用 MPU6050 内置 DMP 输出四元数，省去互补滤波 |
中 |
| OLED 显示 |
0.96寸 SSD1306 I2C OLED 实时显示 Roll/Pitch |
易 |
| 上位机波形 |
串口发原始数据 → Python matplotlib 画波形 |
易 |
| 卡尔曼滤波 |
替代互补滤波，精度更高但计算量大 |
难 |
| 云台控制 |
角度作反馈 → PID → PWM 控制舵机 |
中 |
| 蓝牙无线 |
HC-05 透传 → 手机 App 显示姿态 |
易 |
| FreeRTOS |
采集中断 → 滤波任务 → 通信任务 分离 |
中 |
常见问题
| 现象 |
原因 |
解决 |
| HAL_I2C_Mem_Read 返回 HAL_ERROR |
I2C 无应答 |
检查接线 / 地址写对 0x68<<1 |
| 角度一直 0 |
MPU6050 未初始化 |
先写 PWR_MGMT_1=0x00 唤醒 |
| Roll/Pitch 抖 |
低通滤波太弱 |
降低 CONFIG 值（如 0x06→0x05） |
| Yaw 几秒就漂几十度 |
无磁力计，正常 |
加 HMC5883L 磁力计融合 |
| 数据乱跳 |
电源噪声 |
VCC/GND 加 100nF + 10μF 电容 |
