STM32驱动MPU6050+HMC5883L+BMP085实现姿态解算与校准
2026/9/20 9:31:24 网站建设 项目流程

简介:一套基于STM32的HMC5883L三轴磁力计、MPU6050六轴姿态传感器与BMP085气压计集成控制工程,可同步采集磁场、运动姿态与大气压数据,适用于无人机、机器人导航及运动监测等场景。资源包共132个文件,以C源码、H头文件、UVPROJ工程文件为主,另含汇编启动文件、链接脚本、备份文件及说明文档,压缩包大小为8.27MB,目录结构清晰,便于对照学习与二次开发。工程代码涵盖I2C/SPI底层驱动、传感器原始数据读取、卡尔曼滤波及姿态解算,同时包含fixbah校准思路,可帮助开发者理解多传感器数据融合与误差修正的完整链路。目前已有1729人学习下载,适合希望基于完整例程快速入门STM32多传感器融合的嵌入式初中级开发者。

1. 为什么是三件套而不是只用一颗九轴融合芯片

很多毕业设计里的“MPU6050+HMC5883L+BMP085”组合,其实是在 2015 年前后大量出现的方案。当时 MPU6050 负责加速度计和陀螺仪,HMC5883L 负责地磁航向,BMP085 负责气压和温度,三颗芯片通过同一条 I2C 总线挂在 STM32 上。相比现在直接买一颗 MPU9250 或者 ICM20948 就能拿到融合好的四元数,这种独立三件套的麻烦在于要自己处理初始化顺序、寄存器偏移、磁力计校准和姿态解算。但也正因为每个数据源的特性都裸露在寄存器里,用这套东西做底层站点感知、水下深度参考、或者飞控原型,比用集成方案更容易定位问题。这篇文章按我自己的重构流程来写:先解决总线与初始化,再谈融合与输出,最后落到校调。适合手头有 STM32F103 板和这三颗独立传感器的开发老手,也适合把网盘那个fixbah工程包当作起点、但不想被里面旧驱动坑住的入门者。

2. 在 STM32 上让三颗传感器共用同一条 I2C 总线

2.1 引脚接线与上拉电阻的取值

我一般把 I2C1 接在 STM32F103C8T6 的 PB6(SCL)和 PB7(SDA)上。HMC5883L 的 Ready 引脚、BMP085 的 EOC 引脚在这种小系统里可以不接,原因是读取流程完全靠软件延时,不需要外部中断。三颗芯片的 VCC 都接 3.3V,注意不要接到板载 5V 排针上,否则 HMC5883L 的磁力计输出会偏得离谱。

设备SCLSDAVCC备注
STM32F103C8T6PB6PB73V3也可以换到 PB8/PB9,但要同步改 I2C1 重映射
MPU6050 模块PB6PB73V3AD0 接地,地址 0x68
HMC5883L 模块PB6PB73V3固定地址 0x1E
BMP085 模块PB6PB73V3固定地址 0x77

上拉电阻的取值是整个项目最容易被忽略的地方。三颗模块单独卖的时候大多数都自带 4.7kΩ 上拉到 VCC,可一旦并联在同一条总线上,总线电容会明显增加。短排线用 4.7kΩ 没问题,线长超过 10cm 时我通常会换成 2.2kΩ,否则时钟沿变缓容易在 200kHz 下出现 ACK 丢失。总线速度上,BMP085 的标称上限不高,我统一设成 100kHz,MPU6050 的 FIFO 读取反而更稳。

2.2 用 HAL 扫描三个器件的地址

拿到一个来路不明的工程包时,我不会先去读它的main.c,而是先烧一个最简单的 I2C 地址扫描程序,确认三颗芯片都活着。下面这段是用 STM32CubeMX 生成的 HAL 工程里加进去的,扫描完把结果从串口打出来。

uint8_t addr = 1; while (addr < 127) { if (HAL_I2C_IsDeviceReady(&hi2c1, (uint16_t)(addr << 1), 1, 10) == HAL_OK) { printf("I2C found: 0x%02X\r\n", addr); } addr++; }

这段代码里有个很容易踩的坑:老标准库的驱动函数传的是 8 位地址,例如 MPU6050 要传0xD0;HAL 库的HAL_I2C_IsDeviceReady内部会用 7 位地址左移一位,所以这里直接传addr << 1。扫描结果里出现了 0x1E、0x68、0x77 并且每隔一秒重复出现,说明硬件链路是好的。

2.3 总线上容易出现的时序问题

三个传感器地址与数据格式对比
设备7 位 I2C 地址数据寄存器起始地址输出格式
MPU60500x68 / 0x690x3B14 字节,大端有符号
HMC5883L0x1E0x036 字节,大端有符号
BMP0850x770xF6校准系数从 0xAA 开始

这三个设备里 BMP085 是最慢的,启动一次温度转换 4.5ms,启动一次压力转换在 oversampling=0 时也要 4.5ms。如果把 BMP085 的查询写在紧循环里,I2C 总线会被它霸占,MPU6050 的 FIFO 很快就溢出。建议在工程里把三个设备的读取分成三个时间片,BMP085 每 100ms 才读一次。另外,HMC5883L 是霍尼韦尔的 Memsic 工艺,内部有一个 3ms 左右的采样周期,连续读得太快时后一个字节会是上一次转换结果,这属于正常现象,不要因此去加无意义的延时。

3. 逐芯片初始化:MPU6050、HMC5883L 与 BMP085 的数据准备

3.1 MPU6050 的寄存器配置与六轴原始数据

“mpu6050姿态解算”的搜索结果很少提到一个前提:解算质量取决于初始化时配置的采样率和满量程。对于室内机器人,加速度计量程 2g、陀螺仪量程 2000 °/s 就够用。量程越大,分辨率越低,姿态角度噪声越明显。

寄存器地址寄存器名写入值含义
0x6BPWR_MGMT_10x00唤醒,使用内部 8MHz 不需要外部晶振
0x19SMPLRT_DIV0x09陀螺仪输出 1kHz,除以 10 得到 100Hz
0x1ACONFIG0x06开启 DLPF,带宽 5Hz,适合低通
0x1BGYRO_CONFIG0x18陀螺仪满量程 ±2000 °/s
0x1CACCEL_CONFIG0x00加速度计满量程 ±2g

初始化的代码里,最重要的是必须先把设备从睡眠模式唤醒,并等待复位完成。下面这段是基于 STM32 HAL 的写法,兼容大多数.rar里的老工程。

void mpu6050_init(void) { uint8_t v = 0x80; // 置位 DEVICE_RESET 位 HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, 0x6B, I2C_MEMADD_SIZE_8BIT, &v, 1, 100); HAL_Delay(50); v = 0x00; // 唤醒 HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, 0x6B, I2C_MEMADD_SIZE_8BIT, &v, 1, 100); v = 0x09; // 采样率 1kHz / (1+9) = 100Hz HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, 0x19, I2C_MEMADD_SIZE_8BIT, &v, 1, 100); v = 0x06; // DLPF 带宽 5Hz HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, 0x1A, I2C_MEMADD_SIZE_8BIT, &v, 1, 100); v = 0x18; // 陀螺仪 ±2000°/s HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, 0x1B, I2C_MEMADD_SIZE_8BIT, &v, 1, 100); v = 0x00; // 加速度计 ±2g HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, 0x1C, I2C_MEMADD_SIZE_8BIT, &v, 1, 100); }

参数0x68 << 1是把 7 位地址左移成 8 位地址,这是 STM32 HAL 库的写法和许多网盘里老标准库工程最大的差异。标准库时代用的是I2C_SendData配合0xD0写成宏定义,所以你从.rar里搬代码时,第一件事就是把所有地址统一换算,否则会出现“MPU6050 能扫描到,但初始化失败”的现象。

读六轴数据时,从 0x3B 一次读 14 字节比分 7 次读寄存器效率高得多:

uint8_t buf[14]; int16_t ax, ay, az, gx, gy, gz; HAL_I2C_Mem_Read(&hi2c1, 0x68 << 1, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100); ax = (int16_t)((buf[0] << 8) | buf[1]); ay = (int16_t)((buf[2] << 8) | buf[3]); az = (int16_t)((buf[4] << 8) | buf[5]); gx = (int16_t)((buf[6] << 8) | buf[7]); gy = (int16_t)((buf[8] << 8) | buf[9]); gz = (int16_t)((buf[10] << 8) | buf[11]);

这里的大端字节序很容易疏忽。很多国产模块把数据输出改成小端,但原厂 MPU6050 在 0x3B 之后都是高字节在前。如果把buf[0] << 8写成buf[0] | (buf[1] << 8),数值会变成 32768 附近的乱跳,排查时看起来像是数据异常饱和。

3.2 HMC5883L 的增益配置与读数

HMC5883L 有三个关键寄存器:0x00 是配置寄存器 A,决定采样平均数和输出速率;0x01 是配置寄存器 B,决定增益;0x02 是模式寄存器,0x00 代表连续测量。默认增益 0x00 对应的灵敏度是 1370 LSB/Gauss,适合地磁场强度在 0.25 到 0.65 Gauss 的环境。如果放在电机附近,磁场强度被叠加到 1Gauss 以上,就要降低增益。

void hmc5883l_init(void) { uint8_t v = 0x70; // 8 次采样平均,15Hz 输出,正常测量配置 HAL_I2C_Mem_Write(&hi2c1, 0x1E << 1, 0x00, I2C_MEMADD_SIZE_8BIT, &v, 1, 100); v = 0x20; // 增益 1090 LSB/Gauss,量程约 ±1.3Ga HAL_I2C_Mem_Write(&hi2c1, 0x1E << 1, 0x01, I2C_MEMADD_SIZE_8BIT, &v, 1, 100); v = 0x00; // 连续测量模式 HAL_I2C_Mem_Write(&hi2c1, 0x1E << 1, 0x02, I2C_MEMADD_SIZE_8BIT, &v, 1, 100); }

读数据时从 0x03 开始,依次是 X 高、X 低、Y 高、Y 低、Z 高、Z 低。合成有符号数时也要注意是大端:

uint8_t buf[6]; int16_t mx, my, mz; HAL_I2C_Mem_Read(&hi2c1, 0x1E << 1, 0x03, I2C_MEMADD_SIZE_8BIT, buf, 6, 100); mx = (int16_t)((buf[0] << 8) | buf[1]); my = (int16_t)((buf[2] << 8) | buf[3]); mz = (int16_t)((buf[4] << 8) | buf[5]);

我见过不少人在初始化里把 0x02 写成 0x01,也就是单次测量模式。这种模式下每转换完一次硬件就自动回到空闲模式,软件只延迟 5ms 再读,后三次读数看起来没问题,但主循环里隔 100ms 才读一次时,数据会变成旧值。连续测量模式才是姿态解算的默认选择。

3.3 BMP085 的校准系数与温度补偿

BMP085 和 BMP180 的区别在于校准系数读取地址不同:BMP085 的 22 字节校准数据从 0xAA 开始,BMP180 是 0xB0 到 0xC9 的 11 个字。很多网盘里的老工程把 BMP085 的驱动改成 BMP180,导致直接从 0xAA 读出的数据是错的。只要驱动原样保留就能正常工作。

#define BMP085_ADDR 0x77 typedef struct { int16_t ac1, ac2, ac3; uint16_t ac4, ac5, ac6; int16_t b1, b2, mb, mc, md; } bmp085_cal_t; bmp085_cal_t cal; uint8_t raw[22]; HAL_I2C_Mem_Read(&hi2c1, BMP085_ADDR << 1, 0xAA, I2C_MEMADD_SIZE_8BIT, raw, 22, 100); cal.ac1 = (int16_t)((raw[0] << 8) | raw[1]); cal.ac2 = (int16_t)((raw[2] << 8) | raw[3]); cal.ac3 = (int16_t)((raw[4] << 8) | raw[5]); cal.ac4 = (uint16_t)((raw[6] << 8) | raw[7]); cal.ac5 = (uint16_t)((raw[8] << 8) | raw[9]); cal.ac6 = (uint16_t)((raw[10] << 8) | raw[11]); cal.b1 = (int16_t)((raw[12] << 8) | raw[13]); cal.b2 = (int16_t)((raw[14] << 8) | raw[15]); cal.mb = (int16_t)((raw[16] << 8) | raw[17]); cal.mc = (int16_t)((raw[18] << 8) | raw[19]); cal.md = (int16_t)((raw[20] << 8) | raw[21]);

温度转换流程是:往 0xF4 写 0x2E,等待 4.5ms,从 0xF6 读两个字节。把这 11 个校准系数代入官方补偿公式时,有个隐藏要求是calac5calac6是无符号数,其余是有符号数。如果读出来的ac1出现负值但代码里用了uint16_t,温度会偏出几十度。气压补偿还会复用一个中间变量B5,所以我在工程里把B5声明成全局int32_t,保证温度和压力两条计算链路都看见它。

4. 姿态解算:加速度计、陀螺仪、磁力计如何融合成欧拉角

4.1 用互补滤波让 roll/pitch 在静止时稳定

MPU6050 的陀螺仪短期积分很准确,但长期会漂移;加速度计长期准确,但受振动干扰。把两者用一阶互补滤波合成是开销最低的方式,也是网盘里fixbah这类老工程最常采用的方法。

先做单位换算和陀螺零偏补偿:

float ax_g = (float)ax / 16384.0f; float ay_g = (float)ay / 16384.0f; float az_g = (float)az / 16384.0f; float gx_dps = (float)gx / 16.4f; // 满量程 ±2000dps,灵敏度 16.4 LSB/dps float gy_dps = (float)gy / 16.4f; float gz_dps = (float)gz / 16.4f;

然后计算加速度计姿态:

float roll_acc = atan2f(ay_g, az_g) * 57.2958f; float pitch_acc = atan2f(-ax_g, sqrtf(ay_g * ay_g + az_g * az_g)) * 57.2958f;

如果模块的安装方向是平放且 x 轴向前,上面公式成立。接下来是核心滤波代码:

#define DT 0.01f // 采样周期 10ms #define ALPHA 0.98f static float roll = 0.0f, pitch = 0.0f; roll = ALPHA * (roll + gx_dps * DT) + (1.0f - ALPHA) * roll_acc; pitch = ALPHA * (pitch + gy_dps * DT) + (1.0f - ALPHA) * pitch_acc;

这里的ALPHA不是越大越好。取 0.98 时高频陀螺仪权重高,适合动态运动;但如果你的机器人底盘振动很强,陀螺积分噪声会被保留,姿态角反而抖。这时候把ALPHA降到 0.95 或者把 MPU6050 的 DLPF 带宽调到 10Hz 附近,效果比单纯调滤波系数更好。

4.2 磁力计倾斜补偿后的航向角

roll/pitch 有了以后,HMC5883L 读出的 x/y/z 值还不能直接算 yaw。因为板子倾斜时,磁力计坐标系和水平坐标系不重合,必须做倾斜补偿。我沿用老工程里常用的公式:

float roll_rad = roll * 3.14159265f / 180.0f; float pitch_rad = pitch * 3.14159265f / 180.0f; float xh = mx * cosf(pitch_rad) + my * sinf(roll_rad) * sinf(pitch_rad) + mz * cosf(roll_rad) * sinf(pitch_rad); float yh = my * cosf(roll_rad) - mz * sinf(roll_rad); float yaw = atan2f(yh, xh) * 57.2958f; if (yaw < 0.0f) yaw += 360.0f;

注意,这段代码假定磁力计平放时测的是地球磁场在机体坐标系的 x/y/z 分量。不同厂家的模块把 x 轴和 y 轴的安装方向定义不同,有的必须用atan2f(-yh, xh),有的要在mx前加负号。不要上来就套用网上的公式,应当把模块水平旋转一整圈,打印mxmy两条波形,确认相位关系后再固定正负号。

4.3 软件解算与 MPU6050 DMP 库的选择

很多MPU6050_fixbah工程包里面带了一个inv_mpu.c,那是 InvenSense 的 MotionApps 库,可以在传感器内部做四元数解算,不占用 STM32 的 FPU。对 F103 这种不带硬件浮点单元的芯片来说,DMP 确实能节约算力,但 DMP 的数读出接口默认适配它自己的从属磁力计,不会自动去读 HMC5883L。要把外部磁力计合并进 DMP 的航向,需要手动做一次磁力计校准,并使用mpu_set_compass_sample_rate注入数据,整个移植量比软件互补滤波大得多。

方案CPU 占用外部 HMC5883L移植难度适合场景
互补滤波极低手动融合简单姿态显示,低速机器人
Mahony 滤波手动融合动态飞行器,姿态要求较高
DMP 四元数极低较难接入老工程包自带,适合仪表显示

如果只是想让云台增稳或者做姿态可视化,我建议放弃 DMP,用 Mahony 的 C 语言实现替代。Mahony 在参数调到默认kp=0.5ki=0.001时,输出角度平滑度已经足够好,而且代码里能直接看到加速度计和陀螺仪的融合过程,排查“重力加速度被当成飞行加速度”这类问题时比黑盒 DMP 方便得多。

4.4 气压高度与温度数据在融合中的位置

BMP085 虽然能直接算高度,但室内关门开门造成的几十帕气压扰动就足够让高度跳动一两米。所以只把它当作缓慢变化的参考高度,不要直接替代滚摆传感器。常用公式是国际标准大气压高度换算:

float pressure = (float)up; // 单位 Pa,已经完成补偿 float altitude = 44330.0f * (1.0f - powf(pressure / 101325.0f, 1.0f / 5.255f));

在 STM32F103 上,powf的耗时是几微秒,可以接受。要注意的是,pressure补偿公式里的up不是直接从寄存器读出的 24 位数据,而是要先除以8 - oversampling,否则高度会偏出几十米。更高端的做法是把这个高度值与别的高度通道做加权平均,但对于这个三件套,能让高度数值在静止时连续十分钟波动小于 0.3 米就已经合格了。

5. 把下载到的 .rar 工程整理成能跑的 Keil 工程,并串口输出

5.1 解压与工程目录检查

fixbah后缀的压缩包通常是发布者声称修复过某段代码,但谁也不能保证里面的 HAL 库版本和你本地一致。我先用命令行检查压缩包完整性:

7z t HMC5883L+MPU6050+BMP085.rar

如果7z t报 CRC 错误,先尝试用 7-Zip 的“保留损坏文件”模式解压。解压后重点看是否包含这三类文件:Libraries下的CMSISProject下的uvprojx,以及Hardware下的MPU6050.cHMC5883L.cBMP085.c。缺少任一项,这个工程都很难直接编译。

常见 .rar 工程目录
目录内容缺失后果
User/main.c主循环和传感器读取无法判断系统流程
HardWare/MPU6050.cI2C 读取与初始化六轴数据缺失
HardWare/HMC5883L.c磁力计读取航向角无法计算
HardWare/BMP085.c气压温度读取高度数据无值

下载工程最常见的问题是里头的stm32f10x_conf.h和当前库版本冲突,编译报错如stm32f10x_tim.h重复定义。我倾向于把三份传感器驱动抽出来,新建一个干净的 STM32 标准库或 HAL 库工程,再把这三个.c文件原样加进去,而不是在原有工程里修来修去。

5.2 用 Keil5 打开前必须处理的芯片包与 ST-Link 问题

Keil5 不会再自动附带老芯片的支持包,所以打开.uvprojx前要先安装 STM32F1 系列芯片包。菜单Pack Installer里搜索STM32F1xx,安装完成后才能在Options for Target的设备列表里找到STM32F103C8

如果烧录时提示error: no stm32 target found!,多半不是芯片包的问题,而是 ST-Link 调试 V2 在 SWD 模式下没有读到核心。最常见的原因有:调试接口使用了JTAG而板上只接了 SWDIO/SWCLK;或者之前在程序里禁用了 JTAG。这种时候用STM32 ST-LINK UtilityConnect under reset方式先擦除整个 Flash,再重新烧录,比在 Keil 里反复改配置更干脆。

5.3 串口数据帧定义与上位机解析

传感器解算结果最终要通过串口送出去,判断数据是否自然。我一直推荐用二进制帧而不是打印字符串,因为字符串在波特率 115200 下会把 CPU 时间浪费在sprintf上。定义一个紧凑结构体:

typedef struct __attribute__((packed)) { uint16_t sync; // 0xAA55 uint8_t dev_id; // 0x01 uint8_t seq; // 帧计数 int16_t acc[3]; int16_t gyro[3]; int16_t mag[3]; uint32_t pressure; int16_t temp; float roll; float pitch; float yaw; } sensor_packet_t;

__attribute__((packed))是为了避免字节对齐在特定编译优化下插入 4 字节空洞。发送时用 DMA 把这一块内存发出去,主循环继续跑传感器读取。

上位机用 Python 的struct解析,同样的格式也要算对:

import struct import serial ser = serial.Serial("COM18", 115200, timeout=0.1) fmt = "<HBBI6h3hI3f" size = struct.calcsize(fmt) + 2 # 加上 acc/gyro 共 6 个 int16

<代表小端。STM32 在 ARM 默认小端下,C 结构体里的int16_tfloat都按小端存储,但你在解析时要把acc[3]当作连续 6 个h,否则会错位。Windows 下如果枚举串口直接弹出stm32 virtual com port 叹号,检查驱动是否安装的是 STMicroelectronics 官方 VCP 驱动,而不是 CDC USB 万能驱动。

6. 动手校调:磁力计校准与传感器验证技巧

6.1 磁力计的平面旋转校准法

HMC5883L 的裸数据在原点附近有一个很大的偏差,通常来自 PCB 上的电流回路和周围铁磁物体。不校准就计算 yaw,会导致转向 90 度后偏航角误差超过 15 度。校准最简单的方法是平放模块,在水平面内旋转 360 度,记录mxmy的最大值和最小值,然后算出偏移量:

mag_offset_x = (mag_x_max + mag_x_min) / 2.0f; mag_scale_x = (mag_x_max - mag_x_min) / 2.0f; mag_offset_y = (mag_y_max + mag_y_min) / 2.0f; mag_scale_y = (mag_y_max - mag_y_min) / 2.0f; mx_cal = (mx - mag_offset_x) * (mag_scale_y / mag_scale_x); my_cal = (my - mag_offset_y);

把校准后的mx_calmy_cal代进倾斜补偿公式。如果旋转一周后mx_calmy_cal合成平面的形状还是一个明显椭圆,说明 PCB 上的软磁影响不可忽略,这时需要做标准的椭圆拟合校准,而不只是减偏移。

6.2 静止与转动的姿态验证

每次初始化后我都先做一组静止测试:模块平放桌面,打印rollpitch,两者应该在 0 度附近波动,幅度不超过 1.5 度。然后把模块竖起来,x 轴直立,pitch应接近 90 度。如果静止时姿态还在缓慢漂移,说明陀螺零偏没补偿或互补滤波系数使用的时间常数不对。偏航验证非常依赖磁力计,平放模块让 yaw 对准正北,然后手慢慢转动 90 度,观察数据是否也偏转 90 度。这里最容易出的问题是在室内铁架桌上做,磁力计读数已经被桌子干扰,验证要在塑料平台或空旷地面上完成。

6.3 把 I2C 时钟和采样周期当作最终调参开关

整个三件套跑通后,如果数据仍有偶发跳变,我优先检查的是 I2C 初始化结构体里的ClockSpeed。有些 STM32CubeMX 生成的代码默认给 400kHz,配上长杜邦线和弱上拉,MPU6050 在连续读 14 字节时容易丢 ACK。降到 100kHz 后最明显的收益不是变慢,而是抖动消失。另一个容易被忽略的点是 BMP085 的压力转换耗时会阻塞住主循环,如果主循环里还依赖同一个HAL_Delay,采样周期就从平稳的 10ms 变成 15ms、20ms 交替,姿态解算的DT却还是固定值。我的做法是把定时器周期设成 10ms,在定时器中断里只置一个标志,主循环检测到标志后按时间片依次读取 MPU6050、HMC5883L 和 BMP085,这样DT可以认为是恒定的,姿态角也不会因为压力转换产生毛刺。最后把.rar里的垃圾注释删掉,把传感器地址、量程配置集中到同一份头文件里,以后更换模块型号时只需要改一行。

本文还有配套的精品资源,点击获取

需要专业的网站建设服务?

联系我们获取免费的网站建设咨询和方案报价,让我们帮助您实现业务目标

立即咨询