简介基于STM32的四旋翼无人机姿态解算与匿名上位机通信资源面向嵌入式开发者和无人机爱好者完整呈现MPU6050六轴传感器数据采集、互补滤波/卡尔曼滤波姿态解算、UART串口通信及PWM电机控制等核心环节。代码涵盖传感器初始化、原始数据读取与预处理、姿态解算、匿名上位机协议解析、电机转速输出等模块可帮助理解稳定的姿态估计算法与实时控制链条。资源包含1033个文件以C源程序、头文件、启动文件为主另含STM32CubeMX配置、链接脚本、编译工程及说明文档压缩包共28.44MB。已有12975人浏览学习。通过这份代码可快速掌握STM32 HAL库的串口、定时器、中断配置理解陀螺仪与加速度计的数据融合方法并参照匿名上位机实现实时状态监控与调试为完整飞控系统开发提供可靠参考。1. MPU6050姿态解算四旋翼稳定飞行的第一块拼图调四旋翼的人都懂一个死循环飞机不稳想调PID要调PID得先有可靠的姿态数据而姿态数据恰恰是很多人默认“传感器会直接给我们的”。实际上一颗MPU6050从I2C读出来到上位机显示波形中间隔着寄存器初始化、滤波、数据融合、串口组帧四道坎哪一道翻车飞机都是抖的。这个项目就是专门拆掉这几道坎基于STM32完整实现MPU6050的I2C驱动、DMP与互补滤波两套姿态解算以及匿名上位机的串口通讯协议。对正在从“能起飞”走向“能稳住”的开发者来说这是一份可以直接抄、直接改、直接跑的最小工程对刚接触IMU的新手它也是一条少绕路的完整学习路径。2. 系统架构与硬件接线一条从传感器到上位机的完整链路2.1 系统组成与数据流整个工程可以拆成四个环节MCU主控负责I2C通讯、姿态解算和串口发送MPU6050六轴IMU输出三轴加速度和三轴角速度算法层完成DMP或互补滤波解算把原始数据转换成欧拉角传输层通过USART按匿名协议组帧往地面站实时发送姿态。数据流方向是从传感器到上位机单向的MPU6050通过I2C总线把原始数据交给STM32STM32经过解算得到横滚角roll、俯仰角pitch、航向角yaw再通过串口发送给上位机实时显示。将来如果加PID控制也是在这份姿态数据上做算法所以这版工程本质上是一块“能看见姿态的最小飞控前端”。工程代码采用分层组织核心文件就几个这样的结构非常适合二次开发文件/模块职责关键接口sim_i2c.c模拟I2C通讯I2C_WriteByte / I2C_ReadBytesmpu6050.c传感器寄存器配置与原始数据读取MPU6050_Init / MPU6050_ReadRawattitude.c姿态解算与欧拉角输出Attitude_Update / Get_Attitudeano_serial.c匿名协议组帧与发送ANO_Send_Data / Send_Attitude分层改动起来的优势很明显换一颗传感器只改mpu6050.c换通讯协议只改ano_serial.c算法层和通讯层互不污染。2.2 硬件接线与供电细节MPU6050和STM32之间实际只走两根线。我常用的接法是模拟I2CSCL接PB10SDA接PB11VCC接3.3VGND接地。为什么不直接选STM32的硬件I2C外设因为F1系列的硬件I2C在一些场景下会出现总线忙标志卡死的问题后面避坑章会细说。这里先记一句话模拟I2C是很多飞控项目里更省心的选择够快也足够可靠。有两个引脚容易被忽视。一个是AD0它决定I2C地址接地时地址是0x68接3.3V时是0x69。如果板上还要挂别的I2C设备靠它切换地址。另一个是INT中断引脚在DMP模式下它会在姿态更新完成时输出脉冲如果你想让主循环以事件驱动方式读取姿态可以把INT引到STM32的EXTI引脚工程里预留了这个思路但默认轮询读取。I2C总线是开漏输出所以SCL和SDA必须要有上拉电阻。成品模块板上一般自带如果是自己搭的最小系统板需要加两个4.7kΩ电阻到3.3V。没有上拉时总线读到的全是低电平现象就是设备地址怎么扫描都找不到。2.3 代码分层与可移植性设计这套代码的移植性不错核心在于模拟I2C层把GPIO操作全部封装成宏。换主控时只需要改宏定义里的GPIO端口和引脚号上层驱动不用动。我在自己的项目里把这个文件从F103移植到F407改的仅仅是引脚宏和延时函数一共不到十分钟。MPU6050驱动层同样只暴露两个接口初始化和读取原始数据。初始化做完寄存器配置读取函数把六轴原始数据一次性读回来。上层算法不关心数据是怎么从传感器拿到的只关心读到的int16_t数组长什么样。这种接口粒度对工程调试很友好串口打印、SD卡存储、PID控制都可以直接复用这两步操作。2.4 关键技术选型为什么是这套组合先说姿态解算。工程里同时保留两套方案DMP官方库和互补滤波。DMP相当于把姿态融合放到传感器内部的协处理器里跑主控只需要读取它算好的四元数代码量小、CPU占用低互补滤波则完全由单片机完成适合理解原理、脱离官方库做底层控制的场景。两套方案共用同一个串口发送模块切换时只改主循环里的数据来源。串口协议选择匿名上位机也是刻意为之。相比自己定义一个纯文本协议匿名协议是二进制定长帧带有帧头、功能字、长度和累加和校验解析效率和鲁棒性都更好。在STM32的软件搭配里姿态显示插件几乎是调飞控的刚需匿名上位机恰好支持姿态波形、原始数据、三维模型显示一套协议解决数据回传和可视化问题。3. 核心代码拆解从I2C字节流到姿态四元数3.1 模拟I2C时序与读写封装模拟I2C的核心是四个操作起始、停止、发字节、收字节。起始条件是SCL为高时SDA拉低停止是SCL为高时SDA拉高。代码将引脚操作封装为宏让后续读写函数保持简洁// 模拟I2C引脚宏定义SCLPB10, SDAPB11 #define SCL_H() GPIOB-BSRR GPIO_Pin_10 #define SCL_L() GPIOB-BRR GPIO_Pin_10 #define SDA_H() GPIOB-BSRR GPIO_Pin_11 #define SDA_L() GPIOB-BRR GPIO_Pin_11 #define SDA_READ() (GPIOB-IDR GPIO_Pin_11) static void I2C_Start(void) { SDA_H(); SCL_H(); delay_us(5); SDA_L(); // SCL高电平期间SDA下降沿 起始 delay_us(5); SCL_L(); } static void I2C_Stop(void) { SDA_L(); SCL_H(); delay_us(5); SDA_H(); // SCL高电平期间SDA上升沿 停止 delay_us(5); }起始和停止的时序要求SCL为高时SDA跳变这正好对应I2C协议规范。延时大约5微秒对应100kHz左右的标准模式MPU6050完全能接受。单字节读写是最底层的操作。写字节时从高位开始逐位输出读字节时逐位移入每发8位后读一次ACKstatic void I2C_SendByte(uint8_t dat) { for (uint8_t i 0; i 8; i) { if (dat 0x80) SDA_H(); else SDA_L(); dat 1; SCL_H(); delay_us(2); SCL_L(); delay_us(2); } // 释放SDA接收从机ACK GPIOB-CRL ~GPIO_CNF_CNF11_1; // 把SDA切成输入 SCL_H(); delay_us(2); SCL_L(); GPIOB-CRL | GPIO_CNF_CNF11_1; // 切回输出 } static uint8_t I2C_ReadByte(void) { uint8_t val 0; GPIOB-CRL ~GPIO_CNF_CNF11_1; // SDA切输入 for (uint8_t i 0; i 8; i) { val 1; SCL_H(); delay_us(2); if (SDA_READ()) val | 1; SCL_L(); delay_us(2); } GPIOB-CRL | GPIO_CNF_CNF11_1; // 切回输出 return val; }这里有一个容易忽略的细节发送完8位数据后必须释放SDA才能读到从机的应答位。很多初学者直接在输出模式下读ACK结果永远是1误以为设备没应答。模拟I2C的关键就是GPIO方向切换一条总线上半双工工作。基于这两层封装对外提供两个统一接口写单个寄存器以及连续读多个寄存器。MPU6050所有操作都建立在这两个接口之上void I2C_WriteByte(uint8_t dev_addr, uint8_t reg_addr, uint8_t data) { I2C_Start(); I2C_SendByte(dev_addr 1 | 0); // 设备地址 写位 I2C_SendByte(reg_addr); // 寄存器地址 I2C_SendByte(data); I2C_Stop(); } void I2C_ReadBytes(uint8_t dev_addr, uint8_t reg_addr, uint8_t* buf, uint16_t len) { I2C_Start(); I2C_SendByte(dev_addr 1 | 0); I2C_SendByte(reg_addr); I2C_Start(); // 重启切换读方向 I2C_SendByte(dev_addr 1 | 1); for (uint16_t i 0; i len; i) { buf[i] I2C_ReadByte(); // 最后一个字节不回ACK if (i len - 1) continue; I2C_SendByte(0xFF); // 主机回ACK } I2C_Stop(); }连续读多个寄存器是姿态采集效率的关键。MPU6050的加速度计和陀螺仪数据寄存器在地址上是连续的可以从0x3B一次读出14个字节比单字节读14次省掉大量重复的起始和地址发送这也是项目里主循环读取的默认方式。3.2 MPU6050寄存器初始化配置MPU6050上电默认处于休眠状态不做配置时读到的加速度全为0。初始化按“复位 → 唤醒 → 配采样率 → 配量程 → 配低通滤波”的固定顺序来void MPU6050_Init(void) { // 1. 复位整个芯片 I2C_WriteByte(MPU6050_ADDR, PWR_MGMT_1, 0x80); delay_ms(100); // 复位后必须等待 // 2. 唤醒选择时钟源为X轴陀螺 I2C_WriteByte(MPU6050_ADDR, PWR_MGMT_1, 0x00); // 3. 采样率分频1kHz / (14) 200Hz I2C_WriteByte(MPU6050_ADDR, SMPLRT_DIV, 0x04); // 4. 数字低通滤波带宽约42Hz I2C_WriteByte(MPU6050_ADDR, CONFIG, 0x03); // 5. 陀螺仪量程 ±500°/s I2C_WriteByte(MPU6050_ADDR, GYRO_CONFIG, 0x08); // 6. 加速度计量程 ±2g I2C_WriteByte(MPU6050_ADDR, ACCEL_CONFIG, 0x00); }参数说明PWR_MGMT_1写0x80是触发全芯片复位复位完成后必须延时约100ms再写后续寄存器否则芯片还在复位中写进去的数据会被丢掉。SMPLRT_DIV是采样率分频器实际采样率等于1kHz除以分频值加一写4得到200Hz做四旋翼够用。GYRO_CONFIG写0x08把量程设为±500°/s对应灵敏度131 LSB/(°/s)ACCEL_CONFIG写0x00是±2g量程灵敏度16384 LSB/g。在后续读取原始值时用这两个灵敏度做单位换算。3.3 六轴原始数据读取与预处理初始化完成后读取流程就很固定了从ACCEL_XOUT_H地址起连读14字节前6字节是加速度中间2字节是温度后6字节是陀螺仪。每个轴的数据都是高8位在前、低8位在后拼成一个int16_tvoid MPU6050_ReadRaw(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { uint8_t buf[14] {0}; I2C_ReadBytes(MPU6050_ADDR, ACCEL_XOUT_H, buf, 14); *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[8] 8) | buf[9]); *gy (int16_t)((buf[10] 8) | buf[11]); *gz (int16_t)((buf[12] 8) | buf[13]); }读取出来的原始值要换算成物理量才直观。加速度实际数值等于原始值除以16384单位是g陀螺仪角速度等于原始值除以131单位是°/s。比如静止平放时z轴加速度原始值约等于16384读出来恰好是1g陀螺仪静止时原始值理论为0实际会有几十甚至上百的零偏这需要在解算前处理。磨损和噪声是IMU的老朋友。工程里对加速度做了滑动均值滤波窗口大小取4#define FILTER_N 4 static int32_t acc_buf[3][FILTER_N]; static uint8_t acc_idx; void Acc_Filter(int16_t* ax, int16_t* ay, int16_t* az) { int32_t sum[3] {0}; acc_buf[0][acc_idx] *ax; acc_buf[1][acc_idx] *ay; acc_buf[2][acc_idx] *az; acc_idx (acc_idx 1) % FILTER_N; for (uint8_t i 0; i FILTER_N; i) { sum[0] acc_buf[0][i]; sum[1] acc_buf[1][i]; sum[2] acc_buf[2][i]; } *ax sum[0] / FILTER_N; *ay sum[1] / FILTER_N; *az sum[2] / FILTER_N; }滑动窗口本质上是N点平均滤波器窗口越大噪声压得越低但延迟也越大。在四旋翼场景窗口4到8是比较折中的区间。延迟太大会让控制环感觉“肉”迟滞加剧振荡这属于调参时能直观感受到的变化。3.4 姿态解算DMP官方库与互补滤波两条路DMP方案调用官方motion driver库初始化之后主循环直接读四元数然后做一次欧拉角转换// 主循环前的初始化 mpu_dmp_init(); // 主循环内读取姿态 short quat[4]; unsigned long timestamp; short gyro[3], accel[3]; while (1) { if (dmp_read_fifo(gyro, accel, quat, timestamp, more) 0) { float q0 quat[0] / 16384.0f; float q1 quat[1] / 16384.0f; float q2 quat[2] / 16384.0f; float q3 quat[3] / 16384.0f; float roll atan2f(2.0f * (q0 * q1 q2 * q3), 1 - 2.0f * (q1 * q1 q2 * q2)) * 57.3f; float pitch asinf(2.0f * (q0 * q2 - q3 * q1)) * 57.3f; float yaw atan2f(2.0f * (q0 * q3 q1 * q2), 1 - 2.0f * (q2 * q2 q3 * q3)) * 57.3f; } delay_ms(5); }DMP库输出的四元数是定点格式需要除以16384还原成浮点四元数。四元数转欧拉角用上面三个反三角函数公式即可roll和pitch的取值范围是±90°或±180°atan2f能正确处理象限问题。DMP方案的好处是主控不需要关心融合细节传感器内部完成了陀螺仪积分和加速度修正。互补滤波方案则是完全在STM32里做数据融合。思路是先由陀螺仪积分得到一个角度再用加速度计算出的重力方向反向修正漂移#define dt 0.005f // 采样周期5ms与200Hz采样率对应 #define Kp 0.98f // 陀螺仪权重 void Complementary_Filter(float* roll, float* pitch, float gx, float gy, float acc_roll, float acc_pitch) { // 陀螺仪积分角度 角速度 * 周期 *roll gx * dt; *pitch gy * dt; // 加速度修正把积分结果拉向加速度计参考值 *roll Kp * (*roll) (1.0f - Kp) * acc_roll; *pitch Kp * (*pitch) (1.0f - Kp) * acc_pitch; }其中acc_roll和acc_pitch由加速度分量计算得到acc_roll atan2f(ay, az)acc_pitch atan2f(-ax, sqrtf(ayay azaz))。Kp取0.98表示高度信任陀螺仪积分只拿2%的加速度结果做长期修正。注意yaw没有对应加速度参考所以互补滤波方案里yaw只能靠陀螺仪积分漂移不可避免工程中默认yaw输出原始积分值如果需要长时间稳定就得靠后续加磁力计。4. 匿名上位机协议实现把姿态数据实时送出去4.1 协议帧结构解析本地算出来的角度要发到上位机不能直接用printf打印因为上位机需要稳定、定长、可校验的二进制帧。匿名协议帧结构如下字节位置内容长度0帧头0xAA1字节1帧头0x551字节2功能字0x51表示姿态1字节3数据长度1字节4 ~ 4len-1数据区len字节末尾校验和1字节校验和的计算范围是从功能字到数据区末尾的累加和不包含两个帧头字节。上位机先找帧头再用校验和判断这一帧是否完整有效。0x51是姿态数据帧数据区通常放12字节roll、pitch、yaw各占4字节float。4.2 组帧与串口发送代码发送模块分成两层底层是通用组帧函数上层是姿态专用打包函数。这样以后要发传感器原始数据或遥控通道值只需加一个包装函数void ANO_Send_Data(uint8_t fun, uint8_t* data, uint8_t len) { uint8_t buf[64]; uint8_t crc 0; buf[0] 0xAA; buf[1] 0x55; buf[2] fun; buf[3] len; for (uint8_t i 0; i len; i) { buf[4 i] data[i]; crc fun data[i]; // 累加和范围从功能字开始 } buf[4 len] crc; // 一次性发出整帧 for (uint8_t i 0; i 4 len 1; i) { while (!USART_GetFlagStatus(USART1, USART_FLAG_TXE)); USART_SendData(USART1, buf[i]); } } void Send_Attitude(float roll, float pitch, float yaw) { uint8_t data[12]; // 小端序直接拷贝float字节 memcpy(data[0], roll, 4); memcpy(data[4], pitch, 4); memcpy(data[8], yaw, 4); ANO_Send_Data(0x51, data, 12); }float在STM32里本身就是4字节小端序直接memcpy到字节数组即可上位机按小端解析。如果你从别的平台移植过来顺序不对时角度会显示成乱码这时要考虑字节序转换。发送前等待TXE标志位是USART发送的常规操作保证上一个字节还没挪出移位寄存器时不覆盖数据。注意校验和计算方式匿名协议累加和是逐字节累加有些上位机固件要求把功能字也加进去。如果上位机能显示数据但偶尔丢帧先检查校验和算法是否和上位机端一致。4.3 上位机配置与验证上位机验证是整个项目里最有成就感的一步。打开匿名上位机选择串口波特率设成115200打开串口后把开发板平放能看到roll和pitch接近0度把板子绕z轴转90度yaw跟着变化把板子倾斜横滚角立即响应。如果这些都对说明I2C读取、姿态解算、串口组帧、协议解析四层链路全部打通。上位机里主要看三个东西波形曲线用于观察角度连续性和噪声三维模型用于直觉判断姿态是否匹配物理动作原始数据窗口用于排查传感器数值是否异常。调试时我习惯同时开波形和三维模型一边用手转动板子一边观察数据有没有跳变、延迟有多大一眼就能看出来。ST官方库版本不同导致USART发送API有细微差别但如果用的是标准外设库上面代码可以直接编译。移植到HAL库时把USART发送部分换成HAL_UART_Transmit帧结构和组帧逻辑完全不用改。5. 避坑指南MPU6050常见故障排查记录5.1 I2C总线忙死与设备扫不到现象程序卡在I2C等待标志位无限循环或者上电后设备地址0x68读回来全是0xFF。原因这条我踩过好几次。F1系列硬件I2C外设存在标志位被中断抢占后无法复位的偶发情况表现就是BUSY标志一直为1代码死在等待里另外GPIO如果配置成推挽输出而非开漏再接上拉电阻也会拉低电平失效。解决最省心的方案就是项目里这套模拟I2C绕开硬件外设用GPIO时序直接干活。GPIO配置为开漏输出模式加上4.7kΩ上拉电阻如果设备还是扫不到量一下SCL和SDA引脚对地电压应该是3.3V左右哪一端是0V就说明那一侧被拉死了。5.2 静止时角度漂移和翻面后跳变现象板子放在桌上不动yaw慢慢往一个方向漂几分钟能偏好几度快速翻转板子后roll或pitch数值瞬间跳到离谱的值要过一会儿才恢复正常。原因yaw没有任何外部参考量纯靠陀螺仪积分只要陀螺零偏不为零积分误差就会累积成漂移pitch和roll的跳变则是加速度计在快速运动时叠加了运动加速度直接用atan2算出来的角度包含了运动分量和高频振动混在一起后更明显。解决陀螺仪零偏需要上电时静止采样求平均把这部分补偿掉再积分加速度计尽量保留滤波至少做均值滤波。如果跳变还很大把互补滤波Kp从0.98适当降低让加速度修正更平缓代价是动态响应变钝。5.3 DMP初始化失败与FIFO溢出现象dmp_load_firmware返回非0或者运行一段时间后dmp_read_fifo一直返回FIFO overflow错误。原因DMP固件加载对I2C时序比较敏感模拟I2C延时太小、读写速度太快固件加载就会失败FIFO溢出则通常是主循环读取太慢比如串口阻塞发送时间过长DMP采样率固定FIFO堆积后溢出。解决把模拟I2C的延时从2us加大到5us重新初始化往往能过主循环里如果要做耗时操作先读FIFO再处理数据或者把串口发送改成DMA。DMP库的FIFO缓冲区也可以从默认的512字节加大到1024字节增加容错空间。5.4 同一套代码换个板子角速度全部反向现象换了主控板或换了传感器模块后从地面站看到的角度变化方向与实际翻转方向相反。原因这不是软件bug而是传感器安装方向变了。MPU6050的xyz轴方向是固定的换板子后芯片朝向不同同一方向的旋转在传感器看来符号就不一样。解决打印原始陀螺仪数据确认翻转方向与数据符号对应关系然后在代码里做符号取反。更通用的做法是在上层定义一个轴向配置结构体把正反装、侧装等安装方式做成可配置项二次开发换机型时就不用改算法。6. 二次开发方向从姿态解算到完整飞控6.1 把串口协议改成自己的格式匿名协议虽然好用但如果接的是自己写的地面站帧格式未必匹配。改造思路很直接保留组帧框架只改AN O_Send_Data里的帧头、校验算法和数据区排列。比如我已经把它改成帧头用0xCC 0xCC校验换成CRC16地面上位机按新格式解析。注意改协议后先跑一个回环测试把开发板和上位机连起来确认每帧都能被正确解析再上机。6.2 融合磁力计修正yaw漂移互补滤波版本的yaw漂移困扰了我很久后来加了一颗磁力计修正yaw。常见做法是拿磁力计算出磁航向作为yaw的参考值再套用互补滤波思路yaw Kp * (yaw gz*dt) (1-Kp) * mag_yaw。磁力计容易受电机磁场干扰装机后要做椭球校准否则修正方向本身就偏。这一步做完悬停航向稳定性会明显改善。6.3 上电校准惯用流程从那以后我每次拿到新IMU模块都强制先走一遍完整校准流程先用匿名上位机看原始数据是否波动正常再静止采样200ms求零偏写入Flash最后转动机体确认三轴方向正确确认后再开始写控制算法。这套顺序帮我少走了很多弯路希望帮到你。本文还有配套的精品资源点击获取