MPU6050 DMP 简明使用教程

使用 STM32F103 和 HAL。TIM2 每 10 ms 读取四元数并解算姿态角,TIM3 每 100 ms 通过 USART1 输出。

下面是从 MotionApps 2.0 版本中裁出的代码,对外保留三个函数:

函数 调用位置
mpu6050_dmp_init() 外设初始化之后、启动定时器之前
mpu6050_dmp_read_quaternion() 10 ms 定时回调,读取最新的完整四元数包
mpu6050_dmp_to_euler() 读取成功后,把四元数转换为 yaw、pitch、roll

1. 新建 Inc/mpu6050_dmp.h

完整内容:

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
#ifndef MPU6050_DMP_H
#define MPU6050_DMP_H

#include "i2c.h"

typedef struct
{
float yaw;
float pitch;
float roll;
} MPU6050_Angles;

typedef enum
{
MPU6050_DMP_READ_ERROR = -1,
MPU6050_DMP_NO_DATA = 0,
MPU6050_DMP_DATA_READY = 1,
MPU6050_DMP_FIFO_RESET = 2
} MPU6050_DMP_Status;

HAL_StatusTypeDef mpu6050_dmp_init(void);
MPU6050_DMP_Status mpu6050_dmp_read_quaternion(float q[4]);
void mpu6050_dmp_to_euler(const float q[4], MPU6050_Angles *angles);

#endif

2. 新建 Src/mpu6050_dmp.c

完整内容。I2C 使用 hi2c1,AD0 接地,地址为 0x68。

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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
#include "mpu6050_dmp.h"
#include "mpu6050_dmp_motionapps20.h"
#include <math.h>
#include <string.h>

#define MPU_ADDR 0x68U
#define I2C_TIMEOUT_MS 5U
#define PACKET_SIZE 42U
#define RAD_TO_DEG 57.2957795f

#define CHECK(call) do { \
HAL_StatusTypeDef result = (call); \
if (result != HAL_OK) return result; \
} while (0)

static HAL_StatusTypeDef read_bytes(uint8_t reg, uint8_t *data, uint16_t size)
{
return HAL_I2C_Mem_Read(&hi2c1, MPU_ADDR << 1, reg,
I2C_MEMADD_SIZE_8BIT, data, size, I2C_TIMEOUT_MS);
}

static HAL_StatusTypeDef write_reg(uint8_t reg, uint8_t value)
{
return HAL_I2C_Mem_Write(&hi2c1, MPU_ADDR << 1, reg,
I2C_MEMADD_SIZE_8BIT, &value, 1, I2C_TIMEOUT_MS);
}

static HAL_StatusTypeDef select_memory(uint16_t address)
{
CHECK(write_reg(0x6D, (uint8_t)(address >> 8)));
return write_reg(0x6E, (uint8_t)address);
}

static HAL_StatusTypeDef write_memory(uint16_t address,
const uint8_t *data, uint16_t size)
{
uint8_t buffer[16], verify[16];

while (size > 0U)
{
uint16_t count = size > sizeof(buffer) ? sizeof(buffer) : size;
uint16_t remaining = 256U - (address & 0xFFU);
if (count > remaining) count = remaining;

memcpy(buffer, data, count);
CHECK(select_memory(address));
CHECK(HAL_I2C_Mem_Write(&hi2c1, MPU_ADDR << 1, 0x6F,
I2C_MEMADD_SIZE_8BIT, buffer, count,
I2C_TIMEOUT_MS));
CHECK(select_memory(address));
CHECK(read_bytes(0x6F, verify, count));
if (memcmp(buffer, verify, count) != 0) return HAL_ERROR;

address += count;
data += count;
size -= count;
}
return HAL_OK;
}

static HAL_StatusTypeDef reset_fifo(void)
{
CHECK(write_reg(0x6A, 0x00));
CHECK(write_reg(0x6A, 0x04));
return write_reg(0x6A, 0xC0);
}

HAL_StatusTypeDef mpu6050_dmp_init(void)
{
uint8_t value;
const uint8_t fifo_rate[2] = {0x00, 0x01}; /* 100 Hz */

CHECK(write_reg(0x6B, 0x80));
HAL_Delay(100);
CHECK(read_bytes(0x75, &value, 1));
if (value != 0x68U) return HAL_ERROR;

CHECK(write_reg(0x6B, 0x03));
CHECK(write_reg(0x1B, 0x18));
CHECK(write_reg(0x1C, 0x00));
CHECK(write_memory(0, mpu6050_dmp_firmware, MPU6050_DMP_FIRMWARE_SIZE));

CHECK(write_reg(0x25, 0x7F));
CHECK(write_reg(0x6A, 0x00));
CHECK(write_reg(0x25, MPU_ADDR));
CHECK(write_reg(0x6A, 0x02));
HAL_Delay(20);

CHECK(write_reg(0x19, 0x04));
CHECK(write_reg(0x1A, 0x0B));
CHECK(write_reg(0x1B, 0x18));
CHECK(write_reg(0x1C, 0x00));
CHECK(write_memory(0x0216, fifo_rate, sizeof(fifo_rate)));
CHECK(write_reg(0x70, 0x03));
CHECK(write_reg(0x71, 0x00));

CHECK(read_bytes(0x00, &value, 1));
CHECK(write_reg(0x00, value & 0xFEU));
CHECK(write_reg(0x1F, 2));
CHECK(write_reg(0x20, 80));
CHECK(write_reg(0x21, 156));
CHECK(write_reg(0x22, 0));
CHECK(write_reg(0x38, 0x12));

CHECK(write_reg(0x6A, 0x40));
CHECK(write_reg(0x6A, 0x48));
HAL_Delay(10);
CHECK(write_reg(0x6A, 0x00));
CHECK(write_reg(0x6A, 0x04));
CHECK(read_bytes(0x3A, &value, 1));
return write_reg(0x6A, 0xC0);
}

MPU6050_DMP_Status mpu6050_dmp_read_quaternion(float q[4])
{
uint8_t status, count_bytes[2], packet[PACKET_SIZE];
float decoded[4], norm = 0.0f;

if (q == NULL) return MPU6050_DMP_READ_ERROR;
if (read_bytes(0x3A, &status, 1) != HAL_OK ||
read_bytes(0x72, count_bytes, 2) != HAL_OK)
return MPU6050_DMP_READ_ERROR;

uint16_t count = ((uint16_t)count_bytes[0] << 8) | count_bytes[1];
uint16_t packets = count / PACKET_SIZE;

/* 限制一次中断的读取量,积压超过两包时清空并等待新包。 */
if ((status & 0x10U) || count >= 1024U || packets > 2U)
return reset_fifo() == HAL_OK ? MPU6050_DMP_FIFO_RESET
: MPU6050_DMP_READ_ERROR;

if (packets == 0U) return MPU6050_DMP_NO_DATA;

/* 读取计数时已有的完整包,保留最后一包;不足一包的字节留在 FIFO。 */
for (uint16_t i = 0; i < packets; ++i)
{
if (read_bytes(0x74, packet, sizeof(packet)) != HAL_OK)
{
(void)reset_fifo();
return MPU6050_DMP_READ_ERROR;
}
}

for (uint8_t i = 0; i < 4; ++i)
{
int16_t raw = (int16_t)(((uint16_t)packet[4 * i] << 8) |
packet[4 * i + 1]);
decoded[i] = raw / 16384.0f;
norm += decoded[i] * decoded[i];
}
norm = sqrtf(norm);
if (norm < 0.000001f) return MPU6050_DMP_READ_ERROR;

for (uint8_t i = 0; i < 4; ++i) q[i] = decoded[i] / norm;
return MPU6050_DMP_DATA_READY;
}

void mpu6050_dmp_to_euler(const float q[4], MPU6050_Angles *angles)
{
float w = q[0], x = q[1], y = q[2], z = q[3];
float s = 2.0f * (x * z + w * y);
if (s > 1.0f) s = 1.0f;
if (s < -1.0f) s = -1.0f;

angles->yaw = atan2f(2.0f * (x * y - w * z),
2.0f * (w * w + x * x) - 1.0f) * RAD_TO_DEG;
angles->pitch = -asinf(s) * RAD_TO_DEG;
angles->roll = atan2f(2.0f * (y * z - w * x),
2.0f * (w * w + z * z) - 1.0f) * RAD_TO_DEG;
}

读取成功时,q[0..3] 分别是 w、x、y、z。没有新包时返回 MPU6050_DMP_NO_DATA,不会修改 q。FIFO 溢出或积压超过两包时返回 MPU6050_DMP_FIFO_RESET,等待下一包;读写或解码失败返回 MPU6050_DMP_READ_ERROR。

这里最多读取两个完整包,取读取计数时的最新一包。未完成的包留在 FIFO,下一次读取。初始化中的 HAL_Delay() 只在启动阶段执行。

3. 新建 Inc/mpu6050_dmp_motionapps20.h

下面是完整固件数组,直接复制。内容沿用原有 MotionApps 2.0 镜像。

展开完整文件(1929 字节固件)
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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
#ifndef FLY_MPU6050_DMP_MOTIONAPPS20_H
#define FLY_MPU6050_DMP_MOTIONAPPS20_H

#include <stdint.h>

#define MPU6050_DMP_FIRMWARE_SIZE 1929U

static const uint8_t mpu6050_dmp_firmware[MPU6050_DMP_FIRMWARE_SIZE] = {
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02,
0x00, 0x03, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00,
0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 0x00, 0x02, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01,
0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 0x00, 0x1B, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2,
0x20, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x41, 0xFF, 0x00, 0x00,
0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00,
0x00, 0x05, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9,
0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 0x32, 0x00, 0x13, 0x9D,
0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6,
0x00, 0x00, 0x27, 0x10, 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0,
0xFF, 0xD9, 0x5B, 0xC8, 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9,
0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 0x00, 0x00, 0x00, 0x01,
0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D,
0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 0x00, 0x00, 0x00, 0x00,
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1,
0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00,
0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1B, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1B, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91,
0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0,
0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3,
0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94,
0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2,
0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0,
0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2,
0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF,
0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97,
0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00,
0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0,
0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83,
0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83,
0xB5, 0x93, 0xAF, 0xF0, 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61,
0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 0x96, 0xDB, 0x31, 0xA6,
0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1,
0xB8, 0xA8, 0xB2, 0x86, 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98,
0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 0xAF, 0x8C, 0x96, 0x19,
0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4,
0xCD, 0xC9, 0xF1, 0xB8, 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89,
0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 0x0E, 0x16, 0x1E, 0xA9,
0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64,
0x18, 0xD8, 0xF1, 0xA5, 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A,
0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 0xA3, 0xD9, 0xF8, 0xF0,
0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3,
0xDE, 0xD8, 0x83, 0xA5, 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3,
0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 0xDA, 0x2A, 0xD8, 0x48,
0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8,
0x78, 0x93, 0xA3, 0x4D, 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8,
0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 0x32, 0xD8, 0x70, 0x5D,
0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98,
0xA8, 0xD9, 0x08, 0xD8, 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA,
0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 0x34, 0xB5, 0xB9, 0x94,
0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9,
0x61, 0xD8, 0x6C, 0x68, 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9,
0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 0xA3, 0x83, 0x1A, 0x3E,
0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8,
0x87, 0x9A, 0x35, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8,
0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 0xA5, 0x81, 0x00, 0x0C,
0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8,
0xA3, 0x29, 0x83, 0xDA, 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA,
0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 0x9A, 0xF5, 0x20, 0xAA,
0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06,
0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40,
0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D,
0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79,
0x8B, 0x29, 0x51, 0x79, 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71,
0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 0x8A, 0x64, 0x48, 0x31,
0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,
0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C,
0xF0, 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8,
0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8,
0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31,
0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8,
0x19, 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00,
0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88,
0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E,
0x6E, 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4,
0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A,
0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8,
0x51, 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D,
0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE,
0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E,
0x76, 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C,
0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34,
0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51,
0xD9, 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31,
0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28,
0x34, 0x3C, 0xF3, 0xAB, 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9,
0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0x20, 0x28,
0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38, 0x98, 0xF1, 0xA3, 0xA3,
0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC,
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF,
};

#endif /* FLY_MPU6050_DMP_MOTIONAPPS20_H */

固件来源:jrowberg/i2cdevlib MotionApps 2.0。

4. 配置外设并加入编译

CubeMX 中配置:

外设 配置
I2C1 PB6=SCL,PB7=SDA,100 kHz,7 位寻址
USART1 115200,8N1,开启 USART1 global interrupt
TIM2 Internal Clock,Prescaler=7199,Counter Period=99 (10ms),开启 TIM2 global interrupt
TIM3 Internal Clock,Prescaler=7199,Counter Period=999 (100ms),开启 TIM3 global interrupt

定时器参数按 TIM2、TIM3 输入时钟为 72 MHz 填写,对应 10 ms 和 100 ms。SDA、SCL 上拉到 3.3 V,STM32 与 MPU6050 共地,MPU6050 的 INT 不接。

NVIC 使用抢占优先级分组 4,设置:

中断 抢占优先级
SysTick 0
TIM2 1
TIM3 2
USART1 3

**HAL_I2C_Mem_Read() 的超时依赖 SysTick,因此 SysTick 必须能抢占 TIM2。**这里的 I2C1 由 DMP 使用;定时器运行时,其他代码不能同时访问 I2C1。

生成代码后,把 Src/mpu6050_dmp.c 加入编译,头文件路径包含 Inc。DMP 初始化使用本文的 mpu6050_dmp_init()。

5. main.c 添加头文件和变量

在 /* USER CODE BEGIN Includes */ 中添加:

1
2
#include "mpu6050_dmp.h"
#include <stdio.h>

在 /* USER CODE BEGIN PV */ 中添加:

1
2
3
4
5
static float quaternion[4];
static MPU6050_Angles attitude;
static volatile uint8_t attitude_valid;
static volatile uint32_t attitude_tick;
static char uart_message[96];

uart_message 保持为静态缓冲区,串口中断发送期间仍会读取它。

6. 初始化后启动定时器

在 /* USER CODE BEGIN 2 */ 中添加下面这段初始化代码。位置在 MX_I2C1_Init()、MX_USART1_UART_Init()、MX_TIM2_Init() 和 MX_TIM3_Init() 之后。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);

if (mpu6050_dmp_init() != HAL_OK)
{
Error_Handler();
}

__HAL_TIM_SET_COUNTER(&htim2, 0);
__HAL_TIM_SET_COUNTER(&htim3, 0);
__HAL_TIM_CLEAR_FLAG(&htim2, TIM_FLAG_UPDATE);
__HAL_TIM_CLEAR_FLAG(&htim3, TIM_FLAG_UPDATE);

if (HAL_TIM_Base_Start_IT(&htim2) != HAL_OK ||
HAL_TIM_Base_Start_IT(&htim3) != HAL_OK)
{
Error_Handler();
}

**mpu6050_dmp_init() 已包含复位、固件加载、100 Hz 配置和 DMP 启用。**定时回调中只调用读取和解算函数。

7. 添加 TIM 回调

在 main.c 的 /* USER CODE BEGIN 4 */ 中添加。已经有 HAL_TIM_PeriodElapsedCallback() 时,把两个分支加进已有函数。

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
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM2) /* 10 ms */
{
MPU6050_DMP_Status status =
mpu6050_dmp_read_quaternion(quaternion);

if (status == MPU6050_DMP_DATA_READY)
{
mpu6050_dmp_to_euler(quaternion, &attitude);
attitude_tick = HAL_GetTick();
attitude_valid = 1;
}
else if (status != MPU6050_DMP_NO_DATA)
{
attitude_valid = 0;
}
}
else if (htim->Instance == TIM3) /* 100 ms */
{
if (HAL_UART_GetState(&huart1) != HAL_UART_STATE_READY) return;

/* 复制期间暂时关中断,避免 TIM2 更新到一半。 */
uint32_t mask = __get_PRIMASK();
__disable_irq();
MPU6050_Angles snapshot = attitude;
uint32_t tick = attitude_tick;
uint8_t valid = attitude_valid;
__set_PRIMASK(mask);

if (!valid || (uint32_t)(HAL_GetTick() - tick) > 30U) return;

int length = snprintf(uart_message, sizeof(uart_message),
"yaw=%.2f,pitch=%.2f,roll=%.2f\r\n",
(double)snapshot.yaw,
(double)snapshot.pitch,
(double)snapshot.roll);
if (length > 0 && (size_t)length < sizeof(uart_message))
{
HAL_UART_Transmit_IT(&huart1, (uint8_t *)uart_message,
(uint16_t)length);
}
}
}

TIM2 读到新包后更新 quaternion 和 attitude。三个角度单位都是度:yaw 绕 Z 轴,pitch 绕 Y 轴,roll 绕 X 轴。

TIM3 每 100 ms 复制一次结果并发起串口中断发送。尚未取得姿态、数据超过 30 ms 未更新或串口仍忙时,跳过本次发送。

作者

Noah Shen

发布于

2026-10-07

更新于

2026-10-07

许可协议

评论