MPU6050六轴传感器在STM32F103上的I2C驱动与姿态解算实战

简介这是一份面向STM32F103初学者的MPU6050六轴传感器实验工程覆盖三轴加速度计与三轴陀螺仪的数据读取、I2C通信配置、初始化流程、姿态解算及调试方法可服务于运动追踪、平衡控制、机器人姿态测量等嵌入式场景。压缩包共90个文件以43个h头文件和40个c源文件为主体另含1个uvprojx工程文件、1个hex固件、2个txt说明、1个bat辅助脚本及启动汇编文件工程中集成LCD、按键、LED、USMART调试组件方便对照源码理解各模块作用整体约430KB结构适合边学边练。已有6069人学习下载热度与资料完成度较高。除可直接编译下载的固件外还可从readme与代码中获取I2C时序、寄存器配置、DMP解算与原始数据读取、滤波及姿态解算的完整思路有助于快速完成MPU6050驱动移植并拓展到无人机平衡、两轮自平衡车等实际项目。1. 六轴传感器入门为什么 MPU6050 成了 STM32F103 项目里的常客把三轴加速度计和三轴陀螺仪封装进一个小芯片再通过 I2C 把数据读出来这就是 MPU6050 六轴传感器在 STM32F103 开发里最常见的玩法。你不需要懂 MEMS 内部怎么加工也不需要会卡尔曼滤波就能用它测姿态、测倾角、做手势识别。我做过的那个模拟项目X最初只想要一个能感知“板子有没有被拿起来”的功能结果从 I2C 地址配置到数据抖动处理前后折腾了三个晚上最后发现坑大多不在芯片本身而在寄存器初始化和数据换算上。这篇笔记就是按“接线 → 初始化 → 读原始值 → 换算 → 避坑 → 验证”的顺序把能用得上的代码和参数直接摆出来新手照着走能亮数据熟手能快速拿走寄存器配置清单。2. 硬件接线与 I2C 通信先让芯片“说话”再谈数据2.1 最小系统接线和上拉电阻的选型MPU6050 模块最常见的封装是带稳压和滤波电容的小板引脚一般有 VCC、GND、SCL、SDA、XDA、XCL、AD0、INT。和 STM32F103 连接时只需要 VCC、GND、SCL、SDA外加一根可选的 INT 中断脚。VCC 接 3.3V注意不要接 5V虽然很多模块板载了稳压芯片但直接怼 5V 到某些精简版上会烧。SCL 和 SDA 是开漏输出必须接上拉电阻。模块上通常会预留 4.7kΩ 或 10kΩ 上拉如果你的模块上没有或者你用的是自己焊的芯片而不是模块那就要在 I2C 总线上外接两个 4.7kΩ 电阻到 3.3V。常见翻车场景是代码里 I2C 初始化看起来完全正常但读回来全是 0xFF抓波形发现 SCL 有方波、SDA 一直拉不低多半就是漏掉了上拉电阻。还有一点容易被忽略STM32F103 的 PB6 和 PB7 是硬件 I2C1 的默认引脚但很多人反映硬件 I2C 容易卡死在事件标志上。我一般直接用 GPIO 模拟 I2C两个引脚随意选时序可控、好排查代码移植到其他型号也方便。用模拟 I2C 时把 SCL 和 SDA 配置为开漏输出并在初始化时把 SDA 拉高避免第一次通信时总线状态不确定。#define MPU6050_SCL_PIN GPIO_Pin_6 #define MPU6050_SDA_PIN GPIO_Pin_7 #define MPU6050_GPIO_PORT GPIOB #define MPU6050_GPIO_CLK RCC_APB2Periph_GPIOB void MPU6050_I2C_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(MPU6050_GPIO_CLK, ENABLE); GPIO_InitStructure.GPIO_Pin MPU6050_SCL_PIN | MPU6050_SDA_PIN; GPIO_InitStructure.GPIO_Mode GPIO_Mode_Out_OD; GPIO_InitStructure.GPIO_Speed GPIO_Speed_50MHz; GPIO_Init(MPU6050_GPIO_PORT, GPIO_InitStructure); GPIO_SetBits(MPU6050_GPIO_PORT, MPU6050_SCL_PIN | MPU6050_SDA_PIN); }开漏模式理论上可以不加 GPIO 内部的推挽配置配合外部上拉电阻就能工作。这里把两个引脚都设为开漏输出后立刻置高是为了让总线空闲电平稳定在高位。GPIO_Speed 选 50MHz 是为模拟时序留余量实测 2MHz 以下频率也能跑不过 50MHz 配置不会坏事。2.2 模拟 I2C 时序函数和起始停止条件模拟 I2C 的核心就是“SCL 高电平时 SDA 跳变”这个约定。起始条件是 SCL 高时 SDA 从高拉低停止条件是 SCL 高时 SDA 从低拉高。字节传输则是 SCL 高时 SDA 必须保持稳定数据在 SCL 低电平时变化。很多第一次写的人把时序搞反结果设备一直不应答。下面给出一个标准的模拟 I2C 读写框架代码里我把延时函数统一使用一个 Delay 函数具体实现看你用的时钟主频。I2C 频率一般控制在 100kHz 到 400kHz即每半个时钟周期延时 1.25 到 5 微秒左右。static void MPU6050_I2C_Delay(void) { uint8_t i 8; while (i--) ; } static void MPU6050_I2C_Start(void) { MPU6050_SDA_HIGH(); MPU6050_SCL_HIGH(); MPU6050_I2C_Delay(); MPU6050_SDA_LOW(); MPU6050_I2C_Delay(); MPU6050_SCL_LOW(); } static void MPU6050_I2C_Stop(void) { MPU6050_SDA_LOW(); MPU6050_SCL_HIGH(); MPU6050_I2C_Delay(); MPU6050_SDA_HIGH(); MPU6050_I2C_Delay(); }起始和停止时序里SDA 的变化必须发生在 SCL 高电平期间。如果顺序反了MPU6050 会误判总线状态后续通信全部乱掉。调延时函数时不要图快曾遇到过把延时压到 1 微秒以内单片机本身跑 72MHz 时确实能通信但换一块板子后就不稳定后来统一放宽到 1 微秒以上问题消失。2.3 写字节和读字节应答位处理技巧写字节时从最高位开始逐位发送每个位先拉低 SCL再设置 SDA 电平最后拉高 SCL 让从设备采样。发送完 8 位后释放 SDA拉高 SCL 读取从设备的 ACK 应答位。MPU6050 如果没有正确应答接收方读到的 SDA 是高电平这种情况要能返回错误码方便定位接线和地址问题。static uint8_t MPU6050_I2C_WriteByte(uint8_t data) { uint8_t i; for (i 0; i 8; i) { if (data 0x80) MPU6050_SDA_HIGH(); else MPU6050_SDA_LOW(); data 1; MPU6050_SCL_HIGH(); MPU6050_I2C_Delay(); MPU6050_SCL_LOW(); MPU6050_I2C_Delay(); } MPU6050_SDA_HIGH(); MPU6050_SCL_HIGH(); MPU6050_I2C_Delay(); if (MPU6050_SDA_READ() 0) { MPU6050_SCL_LOW(); return 0; } return 1; }读字节时每读一位要先拉低 SCL再拉高 SCL然后读取 SDA 电平。读完 8 位后主机需要发送一个非应答位通知从设备结束读取如果要连续读取下一字节就发送应答位。这里非应答和应答的选择直接决定多字节读取是否能正确读完最后一个数据。static uint8_t MPU6050_I2C_ReadByte(uint8_t ack) { uint8_t i, data 0; for (i 0; i 8; i) { MPU6050_SCL_HIGH(); MPU6050_I2C_Delay(); data 1; if (MPU6050_SDA_READ()) data | 0x01; MPU6050_SCL_LOW(); MPU6050_I2C_Delay(); } if (ack) MPU6050_I2C_WriteByte_ACK(); else MPU6050_I2C_WriteByte_NACK(); return data; }读字节时注意 SDA 方向切换主机读之前要把引脚拉高并切换到输入模式读完后如果要发应答得重新切回输出模式拉低 SDA。这个“方向切换”是模拟 I2C 里最容易翻车的点很多人把 GPIO 配成输出后直接读寄存器读出来的永远是上一次的输出电平所以必须用 GPIO_ReadInputDataBit 之前切换输入模式。3. 寄存器配置与原始数据读取从初始化到拿到第一个加速度分量3.1 上电时序和 MPU6050 复位释放MPU6050 上电后默认处于睡眠模式除了 I2C 接口和寄存器访问外传感器数据不更新。所以第一步不是读数据而是先确认能正确写入寄存器。一个简单的验证方法是读 WHO_AM_I 寄存器地址 0x75这个寄存器固定返回 0x68。如果读出来不是 0x68要么地址写错、要么接线有问题、要么模块本身是坏的。地址选择上AD0 引脚接地时 I2C 地址是 0x68接高电平时是 0x69。有的模块上 AD0 已经通过电阻接地那么地址就是 0x68。写操作地址是 0xD0读操作地址是 0xD1但在代码里我们一般用 7 位地址 0x68然后通过左移一位补上读写标志位。#define MPU6050_ADDR 0x68 #define MPU6050_WHO_AM_I 0x75 #define MPU6050_PWR_MGMT_1 0x6B uint8_t MPU6050_Check(void) { uint8_t val; if (MPU6050_I2C_ReadReg(MPU6050_ADDR, MPU6050_WHO_AM_I, val) 0) { return val; } return 0xFF; }用一个结构体或者两个字节就够存一次读取结果。这里返回 0xFF 代表通信失败返回 0x68 代表芯片正常。ReadReg 函数的内部逻辑是发送起始条件写入从设备地址加写位写入寄存器地址重新发送起始条件写入从设备地址加读位读取一个字节最后发送停止条件。这个“先写寄存器地址再重发起始读数据”的流程是 I2C 标准写法不要省。3.2 关键寄存器配置清单睡眠模式、时钟源和量程MPU6050 的寄存器不多初始化时真正必须碰的几个电源管理寄存器 10x6B、陀螺仪配置寄存器0x1B、加速度计配置寄存器0x1C、采样率分频寄存器0x19、配置寄存器0x1A。量程决定换算系数这个是新手最容易算错的地方。陀螺仪量程可设为 ±250、±500、±1000、±2000 dps对应满量程输出 16 位有符号数 ±32768所以灵敏度分别是 131、65.5、32.8、16.4 LSB/(°/s)。加速度计量程可设为 ±2g、±4g、±8g、±16g灵敏度分别是 16384、8192、4096、2048 LSB/g。#define MPU6050_PWR_MGMT_1 0x6B #define MPU6050_SMPLRT_DIV 0x19 #define MPU6050_CONFIG 0x1A #define MPU6050_GYRO_CONFIG 0x1B #define MPU6050_ACCEL_CONFIG 0x1C void MPU6050_Init(void) { MPU6050_I2C_WriteReg(MPU6050_ADDR, MPU6050_PWR_MGMT_1, 0x80); Delay_ms(100); MPU6050_I2C_WriteReg(MPU6050_ADDR, MPU6050_PWR_MGMT_1, 0x00); Delay_ms(10); MPU6050_I2C_WriteReg(MPU6050_ADDR, MPU6050_SMPLRT_DIV, 0x07); MPU6050_I2C_WriteReg(MPU6050_ADDR, MPU6050_CONFIG, 0x06); MPU6050_I2C_WriteReg(MPU6050_ADDR, MPU6050_GYRO_CONFIG, 0x08); MPU6050_I2C_WriteReg(MPU6050_ADDR, MPU6050_ACCEL_CONFIG, 0x00); }先把 PWR_MGMT_1 写成 0x80是让芯片复位所有寄存器到默认值。这里不能跳过否则芯片可能处于上一次配置的残留状态。然后写 0x00唤醒芯片并选择内部 8MHz 振荡器作为时钟源。要说明的是0x00 把 CLKSEL 设为 0 即内部振荡器大多数应用够用但想要更精确的角速度积分可以后续配置为 PLL 模式。SMPLRT_DIV 设置为 7也就是采样率 1kHz / (1 7) 125Hz。这个采样率对普通倾角测量够用做手势识别建议提到 200Hz 以上。CONFIG 寄存器设置为 0x06等于开启低通滤波并把带宽设为 5Hz数字低通能明显减少加速度数据的毛刺但响应变慢做快速姿态变化检测时不适合设这么低。陀螺仪配置 0x08 对应 ±1000dps加速度计配置 0x00 对应 ±2g比较适合静止倾角测量。3.3 读取六轴原始数据一个函数收拢所有寄存器读取加速度和陀螺仪数据其实就是连续读 6 个 16 位寄存器ACCEL_XOUT_H0x3B到 GYRO_ZOUT_L0x48。一次 burst 读取比单字节逐次读更效率高而且能保证同一时刻的采样值。写法上注意大小端MPU6050 的数据是高字节在前所以要先读高字节再读低字节合成 16 位有符号数。typedef struct { int16_t accel_x; int16_t accel_y; int16_t accel_z; int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; } MPU6050_Data; uint8_t MPU6050_ReadAll(MPU6050_Data *data) { uint8_t buf[14]; if (MPU6050_I2C_ReadBuffer(MPU6050_ADDR, 0x3B, buf, 14) ! 0) return 1; >#define ACCEL_SENSITIVITY 16384.0f #define GYRO_SENSITIVITY 32.8f typedef struct { float accel_x_g; float accel_y_g; float accel_z_g; float gyro_x_dps; float gyro_y_dps; float gyro_z_dps; } MPU6050_Converted; void MPU6050_Convert(MPU6050_Data *raw, MPU6050_Converted *conv) { conv-accel_x_g (float)raw-accel_x / ACCEL_SENSITIVITY; conv-accel_y_g (float)raw-accel_y / ACCEL_SENSITIVITY; conv-accel_z_g (float)raw-accel_z / ACCEL_SENSITIVITY; conv-gyro_x_dps (float)raw-gyro_x / GYRO_SENSITIVITY; conv-gyro_y_dps (float)raw-gyro_y / GYRO_SENSITIVITY; conv-gyro_z_dps (float)raw-gyro_z / GYRO_SENSITIVITY; }换算出物理单位之后静止放平板子时accel_z 应该接近 1gaccel_x 和 accel_y 接近 0。陀螺仪静止时三个轴都应该接近 0如果读回来有几十 dps 的恒定漂移这属于正常的零偏需要在代码里做一次校准而不是急着怀疑芯片坏。4.2 静态零偏校准取平均值而不是取单次读数零偏校准必须在芯片完全静止时进行一般让系统采集 100 到 200 次数据求平均。单次读数有噪声直接拿那一瞬间的值当偏置后续计算会把这个噪声放大。把校准后的偏置保存到全局变量或者结构体里在每次原始值换算后减去。MPU6050_Converted gyro_bias; void MPU6050_CalibrateGyro(uint16_t samples) { MPU6050_Data raw; MPU6050_Converted conv; float sum_x 0, sum_y 0, sum_z 0; for (uint16_t i 0; i samples; i) { MPU6050_ReadAll(raw); MPU6050_Convert(raw, conv); sum_x conv.gyro_x_dps; sum_y conv.gyro_y_dps; sum_z conv.gyro_z_dps; Delay_ms(2); } gyro_bias.gyro_x_dps sum_x / samples; gyro_bias.gyro_y_dps sum_y / samples; gyro_bias.gyro_z_dps sum_z / samples; }校准过程要求目标板不能有人为晃动最好放在桌面上等几个采样周期再进行。采集样本数 100 到 200 之间样本太少偏置不稳定样本太多浪费时间且设备可能已经发生微位移。校准完后可以在主循环里打印三轴平均结果如果仍然有明显非零值说明采样过程中板子动了要重来。4.3 倾角计算加速度计静态角度与陀螺仪积分角度静态倾角能用加速度计直接算出来核心思路是重力矢量在三个轴上的投影。当板子水平时accel_z 约等于 1g其他两轴约等于 0此时俯仰角 pitch 和横滚角 roll 都接近 0。当板子绕 Y 轴旋转时重力在 X 轴和 Z 轴的分量变化用 atan2 就能算出角度。这里注意不要用 asin 去单独解算某一轴因为 asin 在角度接近 90 度时线性度变差而 atan2 能同时利用两个轴的分量在整个 0 到 360 度范围都有效。陀螺仪积分角度是把角速度对时间积分dx 角度 上一次角度 角速度 × 采样间隔。这个值在短时间内很平滑但会随时间漂移因为积分会把零偏误差不断累积。加速度计角度没有漂移但有噪声互补滤波就是把两者按权重融合静态时信任加速度计动态时信任陀螺仪。#define DT 0.008f float pitch_acc, roll_acc; float pitch_gyro, roll_gyro; float pitch, roll; void MPU6050_UpdateAngle(MPU6050_Converted *conv) { pitch_acc atan2f(conv-accel_y_g, conv-accel_z_g) * 180.0f / 3.14159f; roll_acc atan2f(-conv-accel_x_g, sqrtf(conv-accel_y_g * conv-accel_y_g conv-accel_z_g * conv-accel_z_g)) * 180.0f / 3.14159f; pitch_gyro conv-gyro_x_dps * DT; roll_gyro conv-gyro_y_dps * DT; pitch 0.98f * (pitch_gyro) 0.02f * pitch_acc; roll 0.98f * (roll_gyro) 0.02f * roll_acc; pitch_gyro pitch; roll_gyro roll; }DT 是两次采样之间的时间间隔单位秒。主循环里每次调用这个函数时间隔要尽量均匀。如果主循环里还有其他耗时操作比如串口打印、OLED 刷新DT 会实际变大这时候计算出的积分角度就会偏快。解决办法有两个一是用定时器固定唤醒频率二是在函数里用系统时钟测量真实间隔然后用真实间隔替换 DT。这里的 0.98 和 0.02 是互补滤波系数表示陀螺仪占 98% 权重、加速度计占 2%系数可以根据实际动态调整静止时加速度计噪声大就让系数更偏向陀螺仪但漂移会变大需要权衡。5. 避坑专题读不到数据、数据跳变和姿态漂移三类高频问题5.1 I2C 读回 0xFF 或通信超时地址没对上还是总线没释放现象调用 MPU6050_Check 后返回 0xFF用示波器抓 SCL 和 SDA 发现波形存在但 SDA 在应答位时一直为高。原因分三种一是 AD0 引脚电平不对模块的 AD0 被拉高实际地址变成 0x69代码里还写 0x68二是在第一次通信前 SCL 和 SDA 电平不确定从设备处于异常状态三是 GPIO 配置成了推挽输出而非开漏输出导致 SDA 无法被从设备拉低应答位丢失。解决方法是先确认 AD0 引脚的硬接线再用万用表量 SCL 和 SDA 空闲电压是否为 3.3V接着把 GPIO 模式改成开漏输出最后在初始化函数里先发送 9 个 SCL 脉冲复位总线让从设备释放 SDA。一种常见的补救写法是初始化时把 SCL 和 SDA 都拉高然后手动产生 9 个时钟脉冲让芯片内部控制状态机复位。这个操作对很多 I2C 从设备都有效。我一般在 MPU6050_Init 最前面加上这段能减少很多“第一次上电通信失败复位后就好了”的玄学问题。5.2 静止时加速度数据毛刺大角度在 ±1° 内跳动低通滤波和采样率的配合问题现象芯片放在桌面上不动串口打印出来的 accel_z 在 0.98 到 1.02 之间跳动算出的 pitch 角度在 ±1.5° 范围内来回晃。原因采样率设置过高而数字低通带宽没有相应调整加速度计本身的宽带噪声被完整保留角速度噪声经过积分后也被放大。解决方法是把 SMPLRT_DIV 调大让采样率降到 50Hz 到 125Hz同时把 CONFIG 寄存器的低通带宽设为 10Hz 到 20Hz。如果是做倾角测量低通频率可以设在 5Hz 到 10Hz响应会慢一些但数据干净得多。另外需要说明的是加速度计和陀螺仪的噪声特性不同加速度计白噪声较大陀螺仪是低频漂移更明显。互补滤波系数如果固定为 0.98在剧烈晃动时角度会滞后因为加速度计权重太低了动态响应几乎全靠陀螺仪积分。如果应用里需要快速响应比如做机械臂姿态反馈建议把系数调到 0.9 到 0.95同时牺牲一部分静态稳定性。5.3 角度随时间缓慢漂移静止几分钟后读到十几度零偏校准不彻底现象陀螺仪积分出来的角度在静止时越来越偏最终偏到十几度而加速度计角度仍接近真实角度。原因零偏校准只做了陀螺仪的零点偏移消除但陀螺仪还存在温度漂移上电后芯片温度从室温升高到工作稳定温度零偏在 5 到 10 分钟内会缓慢变化。如果是长时间工作比如超过半小时这个漂移会持续累积。解决方法是每次上电后先等待芯片温度稳定再做一次短时校准然后再进入正常工作如果要求更高可以定期在静止状态下重新校准。对大多数产品一次上电校准就够了但要清楚它不能覆盖温度变化。我和同事曾经遇到一个看起来像“芯片坏了”的问题静止放置一小时后角度漂了 20°最初以为是传感器翻新件后来发现是校准时机太早芯片还没热起来就记录了零偏。从那以后每次上电我都在主循环里先跑 2 秒延时再执行校准顽固问题就少了很多。5.4 DMP 库导致硬件 I2C 卡死代码跑飞中断和时延的冲突现象用官方 DMP 库配合硬件 I2C程序运行几分钟后卡死在 I2C 事件等待循环里复位后又能正常运行一阵。原因DMP 库在后台需要频繁读写 FIFO而如果主循环里的串口打印等阻塞调用占用了太长时间硬件 I2C 的中断事件标志被错过状态机就卡住了。解决方法是把 DMP 读取和主循环任务分离用定时器中断触发读取或者改用模拟 I2C让时序完全受控避免依赖硬件 I2C 的事件标志。我自己在几个项目里都换回模拟 I2C 加 DMP卡死问题没有再出现过代价是多占用两个 GPIO 引脚和一点 CPU 时间。6. 进阶验证技巧用串口绘图和静态测试判断数据是否可信6.1 把六轴数据通过串口输出到 PC 端可视化验证拿到原始数据后第一步不是直接算角度而是先用串口把三轴加速度和三轴角速度以浮点数形式打印出来在 PC 端用串口绘图工具或匿名上位机查看波形。常见做法是每条数据输出一行格式为“accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z,roll,pitch”。这样在绘图工具里能同时看到八条曲线静止时加速度曲线应该是直线角速度曲线应在零附近roll 和 pitch 应保持稳定。printf(ax%.3f ay%.3f az%.3f gx%.2f gy%.2f gz%.2f\r\n, conv.accel_x_g, conv.accel_y_g, conv.accel_z_g, conv.gyro_x_dps, conv.gyro_y_dps, conv.gyro_z_dps); Delay_ms(20);打印频率控制在 50Hz 左右也就是 20ms 一次太高的打印频率会把大量 CPU 时间耗在串口上反而干扰采样节奏。如果用的是 STM32F103 的 USART1波特率设置 115200 或 460800 都行115200 在 50Hz 八通道数据下完全够用但注意别在打印函数里等待发送完成标志改用中断发送或 DMA避免阻塞主循环。6.2 静态测试与动态测试的具体方法静态测试要验证零偏和噪声水平。将板子固定在水平桌面上观察 100 到 200 组数据统计 gyro_x_dps 的平均值和方差。平均值应接近 0方差越小说明噪声越低。加速度计三轴的平方和应接近 1g即 sqrt(ax²ay²az²) 约等于 1误差在 ±0.05g 以内都算正常。如果平方和偏离较大很可能是量程配置和换算系数不匹配。动态测试则是手动绕 X 轴缓慢旋转板子 90°对比加速度计解算的 roll_acc 和互补滤波后的 roll。加速度计角度会瞬间跟随互补滤波角度会有几十毫秒的跟随滞后但静止后应稳定在同一个角度。用手转动时注意不要快速来回甩因为加速度计对线性加速度也很敏感快速移动时算出的倾角会有明显偏差这是物理原理决定的不是代码 bug。6.3 一个能直接复用的主循环框架把读取、换算、滤波、输出全部串起来主循环保持简单。比较稳妥的结构是先校准再进入 while 循环每次循环读取数据、换算物理单位、调用互补滤波更新角度、串口打印。如果后续要接 OLED 或蓝牙把打印部分替换成显示或发送逻辑即可。int main(void) { MPU6050_Data raw; MPU6050_Converted conv; Delay_Init(); USART1_Init(115200); MPU6050_I2C_GPIO_Init(); MPU6050_Init(); if (MPU6050_Check() ! 0x68) { printf(MPU6050 not found!\r\n); while (1); } MPU6050_CalibrateGyro(200); while (1) { MPU6050_ReadAll(raw); MPU6050_Convert(raw, conv); MPU6050_UpdateAngle(conv); printf(pitch%.2f roll%.2f\r\n, pitch, roll); Delay_ms(20); } }这个框架把初始化、校验、校准和主循环分开出现问题容易定位。我在某一次做四轴飞行器模拟姿态时因为直接在 while 里塞了太多传感器融合代码导致采样间隔从 8ms 变成 30ms角度发散严重。后来强制把采样间隔记录下来发现问题在耗时操作而不是滤波算法上。从那以后我每次调 MPU6050 都先串口打印实际采样周期确认时序再谈算法希望这个习惯也能帮你少走弯路。本文还有配套的精品资源点击获取