简介：面向STM32四旋翼开发者的MPU6050姿态解算与串口通讯代码包，聚焦无人机姿态估计与控制链路，适合有嵌入式基础、入门飞控算法或需要调试匿名上位机通信的读者。资源共1033个文件，以C源文件和头文件为主，附有ARM数学库、汇编启动文件、编译中间文件及工程配置文档，解压后约28.44MB，项目结构完整，可直接在配套IDE中查阅修改。已有12975人学习浏览，口碑参考价值较高。内容覆盖传感器初始化、原始数据采集与预处理、互补滤波姿态解算、UART串口协议封装及电机PWM控制等关键模块，并集成匿名上位机V5.0通讯版本代码，便于实时观察俯仰、滚转、偏航数据并调试飞行参数。通过研读源码可掌握STM32硬件接口配置、滤波算法工程实现以及串口通信调试方法，为后续飞控二次开发或完整无人机项目搭建提供扎实参考。

1. 四旋翼飞控里的姿态解算卡在哪里：MPU6050、STM32与匿名上位机的配合

做四旋翼飞控时，最折磨人的不是电机转向，而是姿态数据到底信不信得过。手里明明有MPU6050能同时输出三轴加速度和三轴角速度，但直接用陀螺仪积分，姿态绕一两分钟就开始乱飘；用加速度计去补，又全是高频毛刺。你需要一套能从原始传感器数据一路走到上位机波形的完整闭环方案。这份资源拆的是模拟项目X里的姿态解算部分，包括MPU6050的I2C驱动、互补滤波/四元数解算代码，以及匿名上位机串口通讯版本代码，附带可以直接套用的代码包。我把调参过程中踩过的坑也一并列出来了，适合刚把STM32点亮的开发者，拿着就能把姿态数据送进上位机看曲线。

2. MPU6050与姿态解算原理：从原始数据到四元数

2.1 MPU6050内部结构与I2C读取：为什么它就是飞控的“眼睛”

MPU6050是六轴惯性传感器的代表型号，它把三轴加速度计和三轴陀螺仪封装在同一块MEMS芯片里。加速度计测的是物体在三个轴上的线性加速度，包括重力的分量；陀螺仪测的是绕三个轴的旋转角速度。这两个物理量是姿态解算的全部输入。它的典型工作电压是3.3V，但在实际飞控板上，电源纹波会影响输出，所以一般会在VCC和GND之间并联一个100nF和一个10μF的电容，尽量靠近传感器电源引脚。

通信接口方面，I2C是默认方式，SCL和SDA两根线加上拉电阻（通常4.7k或10k）接到3.3V。STM32的引脚开漏输出时需要外部上拉，所以不能只配成普通推挽。我一般把SCL配成复用开漏，SDA同样。如果飞线使用杜邦线，尽量缩短长度，并把两根线绞在一起，否则在电机启动时I2C波形会被EMI干扰，出现设备地址假应答。还有一个较少人注意到的问题：MPU6050模块上的SDA/SCL如果接反，上电后模块不会回应，但示波器上又能看到时钟波形，容易误判为程序死循环。

之所以强调I2C时序，是因为STM32的硬件I2C外设有一个很“出名”的问题：总线忙检测在某些库版本下会卡死，表现出SCL一直拉低。规避方法有两个：一个是用中断方式驱动硬件I2C，另一个是干脆用GPIO模拟。对于刚接触飞控的开发者，我强烈建议用GPIO模拟，代码就是延时控制时钟和数据电平，实现起来完全可控。从工程角度看，这相当于一个纯软件插件，不依赖芯片外设状态，出了问题随时GPIO翻转复位。

MPU6050的寄存器映射里，有几个初始化时必须配置的地址：PWR_MGMT_1（0x6B）控制休眠和时钟源；SMPLRT_DIV（0x19）控制采样率；CONFIG（0x1A）控制数字低通滤波；GYRO_CONFIG（0x1B）和ACCEL_CONFIG（0x1C）控制量程。我把这些寄存器值写成一个结构体，方便不同飞控项目共用。如果你用的是STM32CubeMX生成工程，可以在初始化代码里直接调用自定义的MPU_Set函数，而不是把I2C初始化逻辑散落在主函数里。

读取数据时，建议一次性连续读取14字节，而不要一轴一轴单独读。因为姿态解算是需要同一时刻的六个轴数据，分两次读会导致测量时间差，在快速旋转时解算出的姿态会有滞后。连续读只要记住起始地址0x3B即可。每次读到的原始值是16位有符号补码，两个字节先高后低，组合时要对buf[0]左移8位再或上buf[1]，这一步做错的话，角度会跳得毫无规律。实际调试时，我一般会先打印原始十六进制数据，确认补码方向正确后再往后面做滤波。

一些开发板上的MPU6050模块带有INT中断引脚，可以接到STM32的外部中断上，用于按固定频率唤醒读取，这个可选。飞控一般还是用定时器中断触发读取，以保证解算周期恒定。采样率建议设在200Hz～1kHz，越高延迟越小，但STM32F103级别在400kHz I2C下连续读14字节需要约35微秒，再算上解算和串口发送，200Hz已经足够入门四旋翼。如果你之后要加机身振动补偿，再把采样率提上去，但每一步都需要实测耗时，不能拍脑袋。

2.2 姿态表示方法：欧拉角的坑、四元数的优势与互补滤波的取舍

姿态解算本质上是在解决“如何把三维空间内的旋转用一组数字稳定地表示”。最自然的是欧拉角：绕X轴为roll，绕Y轴为pitch，绕Z轴为yaw。用加速度计可以直接反正切估计roll和pitch，但yaw必须有参考方向，通常由磁力计提供。欧拉角的问题是当pitch接近90度时，roll和yaw会互相耦合，数学上出现奇点，也就是万向锁。四旋翼做大机动飞行时会偶尔经过这种角度，所以不能直接用欧拉角做积分运算。

四元数是一个超复数，一个四元数q定义为q=(w,x,y,z)，其中w为实部，x、y、z为虚部，它对应三维空间里一个旋转轴和旋转角度。姿态更新时，陀螺仪角速度与四元数满足一个差分方程，需要按周期积分。这一步在代码中体现为如下核心：q_dot = 0.5 * q ⊗ p，其中p是角速度四元数。实际实现时用四元数乘法矩阵，保证浮点运算精度。

为什么要绕道四元数而不直接积分欧拉角？除了万向锁，还有一点：欧拉角积分时要经过大量三角函数，F103处理器算得慢，而四元数更新只是乘加运算，运算开销小。等到输出给PID时，再从四元数转欧拉角，使用一个atan2f和asinf即可。我见过有人为了省事，直接对欧拉角做差分积分，结果在pitch超过90度的那几帧，滚转角瞬间翻180度，飞控直接侧翻。

滤波器选择是另一个关键。卡尔曼滤波在理论上更优，但需要建模观测矩阵和调节过程/测量噪声，多个参数调起来很吃力。互补滤波性能足够，参数只有Kp和Ki两个，效果在四旋翼上完全够用。它的基本形式是：角度=(1-α)×(角度+陀螺仪积分)+α×加速度计角度。α越小越相信陀螺仪，越大越相信加速度计。动态环境的飞控上，α常取0.02～0.05，相当于5%的加速度计权重，这样可以抑制高频振动。

互补滤波的经典实现是直接在四元数层面做误差修正。算法步骤如下：先把加速度计测量的向量归一化，再计算四元数估计出的重力向量与测量向量之间的叉积，这个叉积近似等于姿态误差。将误差通过PI控制器后加到陀螺仪角速度上，最后用修正后的角速度去更新四元数。也就是说，加速度计只用于校正陀螺仪的漂移，不直接参与积分。我见过很多开发者把“低通滤波后的加速度计角度”直接去替换陀螺仪积分角度，结果在快速加减速时姿态跟着振动走，这就是没搞懂互补滤波的误差修正本质。

官方MPU6050自带的DMP（数字运动处理器）可以硬件输出四元数，确实是个省事的软件插件，但它是个黑匣子，无法看到中间量。我在入门时用过一段时间，后来发现它的四元数输出频率和STM32的采样时序对齐不容易做，而且DMP初始化代码占Flash较大，最终还是改用裸算互补滤波，反而对理解飞控核心更有效。所以这份资源里的代码并没有用DMP，而是从原始数据开始算。

3. 基于STM32的MPU6050驱动与解算代码：从寄存器到四元数

3.1 硬件连接与I2C初始化：引脚选择、GPIO配置、延时时序

I2C连接建议选PB6、PB7，也可以选任意空闲GPIO，但要注意这两个引脚附近最好不要有电机驱动信号线。软件模拟I2C的好处是引脚自由，不用绑定到硬件的I2C外设。下面是一段典型的GPIO初始化代码：

#define MPU_SCL_PIN GPIO_Pin_6 #define MPU_SDA_PIN GPIO_Pin_7 #define MPU_GPIO GPIOB void MPU_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); GPIO_InitStructure.GPIO_Pin = MPU_SCL_PIN | MPU_SDA_PIN; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; // 开漏输出 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(MPU_GPIO, &GPIO_InitStructure); // 两个引脚都拉高，模拟空闲状态 GPIO_SetBits(MPU_GPIO, MPU_SCL_PIN | MPU_SDA_PIN); } void I2C_Delay(void) { for (volatile uint8_t i = 0; i < 10; i++); // 约1~2微秒 } void I2C_Start(void) { SDA_HIGH(); I2C_Delay(); SCL_HIGH(); I2C_Delay(); SDA_LOW(); I2C_Delay(); SCL_LOW(); }

这段代码的关键在于开漏输出。开漏模式本身只能拉低或释放，释放时依靠外部上拉电阻把电平拉高，所以SDA和SCL不需要推挽驱动，反而避免了总线冲突。I2C_Start先置高SDA、SCL，再从高到低拉低SDA，形成起始条件。I2C_Delay的循环次数决定了总线速度，在72MHz主频下，循环10次大约对应400kbps，接近MPU6050的极限速率。如果发现读取不稳定，把循环次数加到20，时序会更稳，但速度会降到200kbps，对200Hz采样来说完全够用。

3.2 读取原始数据与零偏标定：把LSB换算成物理量

读取函数上文已经给出了14字节连续读版本。实际使用中，原始数据必须先换算成物理量。如果是±2g量程，加速度计灵敏度为16384 LSB/g；如果是±2000°/s量程，陀螺仪灵敏度为16.4 LSB/(°/s)。换算后的值直接送给解算函数。标定这一步最容易被跳过，却直接影响姿态漂移。

typedef struct { int16_t ax_off, ay_off, az_off; int16_t gx_off, gy_off, gz_off; } MPU_Offset; void MPU_Calibrate(MPU_Offset *offset, uint16_t samples) { int32_t ax_sum=0, ay_sum=0, az_sum=0; int32_t gx_sum=0, gy_sum=0, gz_sum=0; int16_t ax,ay,az,gx,gy,gz; for (uint16_t i = 0; i < samples; i++) { MPU_Read_Raw(&ax,&ay,&az,&gx,&gy,&gz); ax_sum += ax; ay_sum += ay; az_sum += az; gx_sum += gx; gy_sum += gy; gz_sum += gz; delay_ms(1); } offset->ax_off = ax_sum / samples; offset->ay_off = ay_sum / samples; offset->az_off = az_sum / samples - 16384; // 静止1g对应16384 offset->gx_off = gx_sum / samples; offset->gy_off = gy_sum / samples; offset->gz_off = gz_sum / samples; }

逻辑说明：在无人机静止水平放置时，加速度计的Z轴应读到1g，也就是16384 LSB，所以把平均值减去16384作为Z轴偏差。X轴、Y轴理论上应该是0g，所以直接把平均值作为零偏。陀螺仪静止时理论值为0，所以平均值就是陀螺仪零偏。samples建议设1000，耗时1秒，标定过程中绝对不要触碰机架，否则均值里混入运动分量。标定完成后，每次读取原始值减去offset，再除以灵敏度，就是物理量。

3.3 姿态解算核心：互补滤波四元数更新与欧拉角输出

下面是常见的Mahony互补滤波实现，代码量不大但需要正确调用。带入的gx、gy、gz单位是°/s，ax、ay、az单位是g。采样周期dt通过定时器中断保证为5ms。

#define Kp 1.5f #define Ki 0.02f #define dt 0.005f // 采样周期200Hz float q0=1, q1=0, q2=0, q3=0; // 四元数 static float exInt=0, eyInt=0, ezInt=0; void IMU_Update(float gx, float gy, float gz, float ax, float ay, float az) { float norm, vx, vy, vz, ex, ey, ez; // 归一化加速度计向量 norm = sqrt(ax*ax + ay*ay + az*az); ax /= norm; ay /= norm; az /= norm; // 由四元数估算重力方向分量 vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; // 叉积求姿态误差 ex = ay*vz - az*vy; ey = az*vx - ax*vz; ez = ax*vy - ay*vx; // 积分误差用Ki消除陀螺仪零偏 exInt += ex*Ki*dt; eyInt += ey*Ki*dt; ezInt += ez*Ki*dt; // 修正角速度 gx += Kp*ex + exInt; gy += Kp*ey + eyInt; gz += Kp*ez + ezInt; // 一阶龙格库塔更新四元数 q0 += (-q1*gx - q2*gy - q3*gz) * dt / 2; q1 += ( q0*gx + q2*gz - q3*gy) * dt / 2; q2 += ( q0*gy - q1*gz + q3*gx) * dt / 2; q3 += ( q0*gz + q1*gy - q2*gx) * dt / 2; // 四元数归一化，防止发散 norm = sqrt(q0*q0+q1*q1+q2*q2+q3*q3); q0/=norm; q1/=norm; q2/=norm; q3/=norm; }

这个函数有几点需要注意。第一，加速度计归一化必须在叉积之前，否则误差大小会被加速度幅值缩放，导致Kp的实际效果随运动强度变化。第二，exInt、eyInt、ezInt积分项在长时间静止时会让陀螺仪修正量自动变化，但Ki不能太大，否则会引入低频振荡。第三，四元数更新前要把角速度从度每秒转成弧度每秒，但上面代码里gx、gy、gz直接使用未转换的度数，是因为Kp和Ki也按度数标定过，这是常见做法，系数已经折算进去。如果你自己调整参数，要注意单位统一。

再从四元数转成欧拉角，供外环PID使用。输出时roll和pitch可以直接用，yaw因为没有磁力计，会随陀螺仪零漂而缓慢漂移，只适合短时间测试。

void Quaternion_To_Euler(float *roll, float *pitch, float *yaw) { *roll = atan2f(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2)) * 180/M_PI; *pitch = asinf(2*(q0*q2-q3*q1)) * 180/M_PI; *yaw = atan2f(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3)) * 180/M_PI; }

这里的扩展参数说明：atan2f的第一个参数是y，第二个是x，顺序不要写反。pitch用asinf，当姿态接近90度时可能出现超过±1的输入，代码里最好加一个钳位。我把这个钳位放在定时器中断里，如果输入超过1就强制置1，简单粗暴，但能避免NaN传播。

4. 匿名上位机串口通讯：收起姿态数据去看曲线

4.1 匿名上位机协议格式：帧头、功能字、长度和校验

匿名上位机是四旋翼调试中常用的串口波形工具，它默认的协议结构很轻量，每个数据帧由帧头、功能字、长度、数据段和校验和组成。常见帧头是0xAA，功能字0xF1代表上传角度数据，长度字段表示数据段的字节数，校验和为前面所有字节的累加和取低8位。数据段里用小端模式存放，也就是低字节在前。把这个格式做成表格，方便对照：

发送时把浮点角度放大100倍转成int16，这样0.01度的分辨率对飞控调试足够了。用整数传输比浮点传输稳定，避免上位机解析时出现字节错位。协议里的长度字段必须精确，少一字节校验会一直失败。

4.2 数据打包与发送：把欧拉角装进数据帧

串口初始化使用USART1，波特率115200。下面是一段完整的发送代码，直接放进定时器中断里调用即可：

void Send_Angle_To_Host(float roll, float pitch, float yaw) { uint8_t tx_buf[13]; uint32_t sum = 0; int16_t r = (int16_t)(roll * 100); int16_t p = (int16_t)(pitch * 100); int16_t y = (int16_t)(yaw * 100); tx_buf[0] = 0xAA; tx_buf[1] = 0xF1; tx_buf[2] = 0x08; // 8字节数据 tx_buf[3] = r & 0xFF; tx_buf[4] = r >> 8; tx_buf[5] = p & 0xFF; tx_buf[6] = p >> 8; tx_buf[7] = y & 0xFF; tx_buf[8] = y >> 8; tx_buf[9] = 0x00; // 第7、8字节可放油门或保留 tx_buf[10] = 0x00; for (uint8_t i = 0; i < 11; i++) { sum += tx_buf[i]; } tx_buf[11] = (uint8_t)(sum & 0xFF); // 串口发送12字节 for (uint8_t i = 0; i < 12; i++) { while (USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET); USART_SendData(USART1, tx_buf[i]); } }

逻辑说明：先把浮点角度乘以100并取出整数位，负数时int16的位运算会自动补码，所以小端发送没有问题。数据段第7、8字节填入0，如果你希望在上位机同步观察油门或高度，可以改成对应变量，但长度保持不变。发送时等待TXE标志，否则会丢字节。这段代码在200Hz下会产生约2400字节每秒的串口数据，115200波特率带宽足够。如果后续要加更高频率的数据，建议把波特率提高到921600。

4.3 上位机配置与波形绑定：串口、ID和曲线显示

连接步骤其实很简单，第一次用匿名上位机的人容易在静态界面上卡住。正确流程是：先打开上位机，选择对应串口号，设置波特率为115200，然后点击打开串口。打开后，在波形显示界面添加三个变量，分别对应角度1、角度2、角度3。这几个变量名和功能字0xF1之间的对应关系已经写在上位机里，所以只要数据帧正确，波形就会自动出现。

如果打开串口后没有任何反应，先检查USB转TTL的TX和RX是否交叉连接，即STM32的TX接USB的RX，STM32的RX接USB的TX。还有一点，匿名上位机的波形显示默认把角度归一化到-180到180度，如果发现曲线从180跳到-180，这是正常的。手动缓慢转动开发板，观察roll和pitch的曲线方向，如果反向，说明传感器安装方向不对，直接在解算代码里取反即可，不用改硬件。

5. 避坑：姿态漂移、I2C锁死、数据乱码的排查记录

5.1 姿态漂移与零偏变化：静止时角度缓慢偏移

现象：无人机完全静止在桌面上，但上位机里的roll和pitch角度会从0慢慢飘到几度，并且持续累加，像时钟一样不受控制。

原因：第一，陀螺仪零偏没有被准确标定，或者标定后温度变化又产生了新的零偏；第二，互补滤波中的Ki太小，在线修正能力弱；第三，采样率与dt不一致，比如定时器配置成250Hz，代码里dt却写的0.005，积分的尺度就错了。

解决：重新执行标定函数，并且保证标定过程持续1秒以上。检查定时器分频值和dt是否严格对应，如果设置的是250Hz，dt要改成0.004。把Ki从0.02调到0.05时，漂移收敛会变快，但注意观察是否有低频振荡。还有一个我踩过的坑：用过长的杜邦线连接传感器，电机转动时的振动导致陀螺仪输出有尖峰，这种尖峰积分后就是漂移，要在读取后加一个简易滑动中值滤波。

5.2 I2C总线无响应或SCL锁死：程序卡在读设备地址

现象：上电后程序运行到MPU_Init里就再也没出来，用调试器看停在I2C_SendByte，SCL引脚被拉低，SDA一直高。

原因：GPIO没有配置成开漏输出，内部上拉不足；或者SDA/SCL接反；更典型的是MPU6050在发起起始信号后，如果SCL上有毛刺，从机就进入错误状态，总线一直忙。

解决：把GPIO模式改成开漏，并在两个引脚到3.3V之间各接一个10k上拉电阻。如果已经接了上拉，那就是软件模拟I2C的时序太快，把I2C_Delay的循环从10增加到20。后来我不再用硬件I2C的原因就是这个问题，硬件I2C一旦进入Busy状态，只有完全复位外设才能恢复，软件模拟至少可以在初始化时自行拉低再拉高做一次总线释放。

5.3 上位机波形乱跳或数值异常大：毛刺和溢出混合

现象：角度曲线整体平缓，但突然出现一个大尖峰，比如从2度瞬间跳到200度，然后又落回2度，频率没有固定规律。

原因：串口数据在连续发送时，中断优先级和数据访问冲突导致发送字节被截断，上位机把错位的字节解析成巨大的角度。也可能是解算本身出现了NaN，四元数某个分量非有限值。

解决：在发送函数进入时关中断，发送完再开，或者用一个环形缓冲区让发送逻辑和中断分离。四元数更新后加有限值检查，如果isnan发现异常，重置四元数单位值。还有一个更隐蔽的问题：如果发送角度时用int16存储，roll*pitch超过32767就溢出了，比如roll超过327.67度，要先钳位到±180度，再做乘法。

5.4 串口波特率偏差导致乱码：帧头一直找不到

现象：上位机能收到字节，但永远找不到帧头，整个数据流全是乱码。用串口助手看十六进制，发现0xAA经常变成0x88或其他值。

原因：STM32时钟配置错误导致串口波特率偏差过大。比如外部晶振用8MHz，但SystemInit配置里按12MHz计算，115200会产生超过5%的误差，超过标准容错范围，所以接收方采样错位。

解决：先检查RCC配置，确认系统时钟等于72MHz。最保险的方法是用逻辑分析仪测一下TX引脚的实际码元宽度，理论上是1/115200约8.68微秒，如果有明显偏差，调整时钟树。我也见过有人把USART的波特率寄存器和CubeMX配置不一致，直接写寄存器时要再核对一遍。

6. 进阶可以用匿名上位机把滤波对比和PID调试变成可见的东西

6.1 同时上传多个变量，看滤波器是否生效

基础代码只发了角度。想确认传感器原始数据是否可信，可以在发送帧里扩展一个功能字0xF2，把滤波前和滤波后的加速度Z轴同时发上来。匿名上位机支持多个通道显示，这样你能直接看到振动幅值在滤波后被压缩了多少。我给一个简单的发送原语的思路：先发0xAA、0xF2、长度6，后面放入三个int16变量，最后的校验和跟上文一样。通过横坐标相同的两个曲线，能直观看出互补滤波到底滤掉了什么。

6.2 用上位机日志辅助PID调参

PID调试时，只靠肉眼看飞机晃动很难判断超调量。我习惯的做法是把目标角度和当前角度同时发到上位机的两个通道，再在机架约束下用手给一个阶跃扰动。观察两条曲线之间的响应时间：如果当前角度追目标角度过程出现往复，说明P过大；如果追得太慢，说明P过小。这个方法比拿着飞机猜测靠谱很多。后来我在某次调试里看到一条曲线有细微高频抖动，但裸眼看无人机完全正常，排查下来发现是加速度计在电机高速转动时受到了共振干扰，解决方法是在读取MPU6050数据前加一段延时，跳过电机PWM切换瞬间的采样窗口。

6.3 一个收尾习惯

那次测试让我养成了一个习惯：每改一次滤波参数，都要把原始数据和解算结果一起录下来，先看原始数据里有没有异常尖峰，再决定动Kp还是Ki。从此以后再没有出现过参数越调越乱的情况。希望这份拆出来的代码和避坑记录能帮你少走几次弯路。

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