简介面向STM32四旋翼无人机开发爱好者二阶段项目资源专门针对MPU6050姿态解算与匿名上位机串口通信场景涵盖传感器初始化、原始数据预处理、互补滤波姿态解算、UART协议封装及电机PWM控制等完整链路。压缩包共1033个文件以C源码和头文件为主另含IAR/Keil工程配置、链接脚本、Hex固件、ARM官方数学库及匿名上位机工具整体约28.44MB可直接用于代码研读或工程参考。目前已有12945人学习下载热度较高。资源附带工程结构清晰对理解卡尔曼/互补滤波的工程实现、串口波特率与帧格式设计、STM32中断与定时器配置都有实际帮助能有效降低飞控开发门槛适合需要对照调试和进阶学习的嵌入式开发者。 做四旋翼无人机姿态解算是绕不过去的一关。飞机能不能稳悬停会不会晃转弯顺不顺反馈到代码层面就是姿态这个数算得准不准、更新得快不快。这一篇是系列第二篇接着上一篇的硬件搭建往下走把STM32F103C8T6主控和MPU6050六轴传感器的数据链路打通用I2C读取加速度计和陀螺仪的原始数据做姿态解算得到roll、pitch、yaw最后把姿态值通过串口发给匿名上位机在电脑上实时看到飞机姿态的3D变化。文章里贴的代码是我实际跑通的完整版本包含MPU6050读取、四元数姿态解算、匿名上位机协议打包三个模块去掉了不必要的工程结构可以直接照着移植。1. 整体设计思路传感器选型与通讯链路1.1 为什么选择了MPU6050先说说为什么选MPU6050。四旋翼上要做姿态闭环控制最常见的选择就是六轴IMU也就是三轴加速度计加三轴陀螺仪的组合。MPU6050虽然是2013年前后的老片子但现在依然活跃在各种入门飞控和毕业设计里原因很简单便宜、够用、资料全。一片模块几块钱I2C接口两根线就能接自带数字输出用STM32标准库或者HAL库都能轻松驱动起来。对于四旋翼这种应用场景MPU6050的精度是够用的。加速度计量程默认±2g陀螺仪量程可以配置到±2000dps内部还集成了温度传感器和DMP运动处理器。DMP是芯片里固化的姿态解算单元能直接输出四元数。很多教程会让你用DMP把官方Motion Driver库一移植就能出数据省事是真的但我不推荐在飞控项目里直接用DMP后面会详细说原因。1.2 通讯链路与数据流向整个系统的数据流向是这样的MPU6050通过I2C把六轴原始数据给STM32STM32跑姿态解算得到欧拉角解算结果通过USART1串口发到上位机上位机绘制姿态球实时显示。这里有个容易被忽略的点I2C总线的稳定性。MPU6050的SCL和SDA两根线如果模块上没有上拉电阻必须自己在板子上加4.7k上拉到3.3V。我早期做实验的时候直接用杜邦线把模块和单片机连起来结果I2C读取时好时坏后来用示波器看波形才发现是上拉问题。另外I2C线尽量短超过20cm就开始容易受干扰飞控内部走线足够但如果要做外部调试线长了必须降低I2C速率或者加上拉加强这是很实际的坑。2. MPU6050数据读取I2C寄存器操作细节2.1 初始化配置要点MPU6050的I2C地址是0x68或者0x69由AD0引脚的电平决定绝大多数模块AD0默认接地所以地址固定是0x68。这个地址特别容易在有多个I2C设备时产生迷惑我习惯在初始化之后先读WHO_AM_I寄存器地址0x75验证一下返回值固定是0x68如果读出来不是0x68就说明通讯有问题。初始化需要配置几个关键寄存器0x6BPWR_MGMT_1解除睡眠模式写0x00让芯片进入正常工作态。芯片上电默认是睡眠模式不解除睡眠读出来的数据全是0。0x1ACONFIG配置数字低通滤波器DLPF和采样率。我一般配置成0x03对应的DLPF带宽约44Hz延迟4.8ms这样既滤掉高频噪声又不会让数据太滞后。0x19SMPLRT_DIV采样率分频。配合DLPF采样率1kHz/(1SMPLRT_DIV)。我常用SMPLRT_DIV7采样率125Hz跟解算频率保持一致。0x1BGYRO_CONFIG陀螺仪量程。我配置±2000dps对应16.4 LSB/(dps)量程大一些虽然分辨率低但四旋翼机动时角速度比较大力竭量程才是关键。0x1CACCEL_CONFIG加速度计量程。我配置±2g对应16384 LSB/g这个量程下分辨率最高足够四旋翼使用。配置的代码片段void MPU6050_Init(void) { // 解除睡眠 MPU6050_WriteReg(0x6B, 0x00); // 禁止FIFO配置DLPF带宽约44Hz MPU6050_WriteReg(0x1A, 0x03); // 采样率分频125Hz MPU6050_WriteReg(0x19, 0x07); // 陀螺仪量程±2000dps MPU6050_WriteReg(0x1B, 0x18); // 加速度计量程±2g MPU6050_WriteReg(0x1C, 0x00); // 关闭所有中断 MPU6050_WriteReg(0x38, 0x00); }0x1B配置寄存器里bit4和bit3是FS_SEL0x18对应的二进制是00011000也就是FS_SEL3表示±2000dps。0x1C默认值就是0x00对应的AFS_SEL0即±2g。所以0x1C那一行其实可写可不写但写上更清楚以后改量程也方便。2.2 连续读取14字节MPU6050的加速度和陀螺仪数据分别在寄存器地址0x3B和0x43加上温度传感器数据从0x3B到0x48一共14个字节。I2C支持连续读操作一次性把这14个字节读出来是最推荐的做法。这样做有两个好处一是只发起一次I2C通信耗时更短二是避免逐个寄存器读取时刚好赶上传感器数据更新导致加速度和陀螺仪的数据不是同一时刻采样的也就是数据撕裂。我在项目里用标准库模拟I2C和硬件I2C都试过。硬件I2C效率更高但STM32F1的硬件I2C用起来要小心Busy标志的问题很多人就是在这里卡住。我最终选的是硬件I2C正常模式下的连续读取按下面这个流程走稳定跑几个月都没问题void MPU6050_ReadRaw(int16_t *accel, int16_t *gyro, int16_t *temp) { uint8_t buf[14]; // 从0x3B开始连续读14字节 I2C_ReadBuffer(MPU6050_ADDR, 0x3B, buf, 14); accel[0] (buf[0] 8) | buf[1]; // ACCEL_X accel[1] (buf[2] 8) | buf[3]; // ACCEL_Y accel[2] (buf[4] 8) | buf[5]; // ACCEL_Z *temp (buf[6] 8) | buf[7]; // TEMP gyro[0] (buf[8] 8) | buf[9]; // GYRO_X gyro[1] (buf[10] 8) | buf[11]; // GYRO_Y gyro[2] (buf[12] 8) | buf[13]; // GYRO_Z }读取的原始数据要转换成物理量。加速度数据除以16384得到g为单位的值陀螺仪数据除以16.4得到dps为单位的值。这里有个易错点很多人把陀螺仪量程配置搞混了量程是±250dps时灵敏度是131改成±2000dps后就变成了16.4转换系数必须跟着改。如果换算后角度值小得离谱先查查是不是量程和灵敏度不匹配。3. 姿态解算为什么用Mahony而不是DMP3.1 加速度计和陀螺仪各自的缺陷姿态解算的核心问题是融合加速度计和陀螺仪的数据。先说清楚为什么不能直接用某一类传感器出姿态。陀螺仪测量的是角速度对角速度积分就能得到角度增量。但积分会把零漂误差累积起来时间越长偏差越大几分钟躺着不动姿态角就能飘出去好几度。这就是陀螺仪的积分漂移问题。加速度计在静止时通过测量重力加速度在三轴的分量可以反推出roll和pitch角不会累积误差。但加速度计扛不住震动电机的震动和机体的加速度都会混进测量值里让角度来回跳。只看加速度计悬停时姿态数据抖得像筛子。所以必须把两者融合用陀螺仪做主姿态更新用加速度计修正陀螺仪的漂移。这就是姿态解算的本质。3.2 为什么不用芯片自带的DMPMPU6050内部有DMP可以直接输出四元数。很多教程都是这么教的把Motion Driver库移植进去调用几个API就能拿到数据。我为什么不推荐在飞控项目里用DMP第一DMP的核心算法是固化在芯片里的黑盒没法修改内部的姿态融合策略也没办法精细调节修正速度。第二DMP的输出频率和外部定时采集的同步性可控性差实际飞控用的是控制周期严格固定的姿态数据DMP的异步输出处理起来反而麻烦。第三Motion Driver库代码量不小占用很多片内Flash和内存F103C8的库存本来就不宽裕。第四姿态解算本身是飞控最核心的算法自己写一遍才能深入理解后面要加磁力计做yaw修正或者做更高级的卡尔曼滤波也才有基础。所以我建议自己写解算算法。有两条路线一阶互补滤波和Mahony四元数融合。一阶互补滤波实现极其简单几行代码就能出结果但动态响应和抗扰性能一般。Mahony算法在开源飞控里很常见计算量也不大F103跑200Hz没有问题效果比一阶互补滤波好得多。这个项目我用Mahony。3.3 Mahony核心逻辑与代码Mahony的思路可以这么理解加速度计测量的重力方向是“观测值”陀螺仪积分出来的姿态对应的期望重力方向是“预测值”两者做叉积得到误差。然后把这个误差通过PI控制器修正陀螺仪的角速度再用修正后的角速度去更新四元数。这样既保留了陀螺仪短时高精度的动态优势又通过加速度计持续修正累积漂移。核心代码#define sampleFreq 200.0f #define twoKpDef (2.0f * 0.5f) #define twoKiDef (2.0f * 0.0f) float q0 1.0f, q1 0.0f, q2 0.0f, q3 0.0f; void Mahony_Update(float gx, float gy, float gz, float ax, float ay, float az) { float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; gx * 0.0174533f; // 度转弧度 gy * 0.0174533f; gz * 0.0174533f; // 由当前四元数推算出重力方向 halfvx q1*q3 - q0*q2; halfvy q0*q1 q2*q3; halfvz q0*q0 - 0.5f q3*q3; // 加速度计观测值与推算值叉积 halfex ay*halfvz - az*halfvy; halfey az*halfvx - ax*halfvz; halfez ax*halfvy - ay*halfvx; // PI修正陀螺仪角速度 gx twoKpDef * halfex; gy twoKpDef * halfey; gz twoKpDef * halfez; // 四元数微分方程更新 qa q0; qb q1; qc q2; q0 (-qb*gx - qc*gy - q3*gz) * (1.0f/sampleFreq) * 0.5f; q1 ( qa*gx qc*gz - q3*gy) * (1.0f/sampleFreq) * 0.5f; q2 ( qa*gy - qb*gz q3*gx) * (1.0f/sampleFreq) * 0.5f; q3 ( qa*gz qb*gy - qc*gx) * (1.0f/sampleFreq) * 0.5f; // 四元数归一化 float norm sqrtf(q0*q0 q1*q1 q2*q2 q3*q3); q0 / norm; q1 / norm; q2 / norm; q3 / norm; }注意看代码里的几个关键点。第一陀螺仪数据要转成弧度每秒这个很容易漏掉漏了之后整个解算等于白跑。第二PI控制器里我用的是0.5f的KpKi设成0。对于静止测试和室内慢速飞行这个参数够用如果飞机动作幅度大或者觉得姿态跟随太软、修正太慢可以把Kp调到1.0甚至2.0试试Kp太大则高频噪声会放大姿态会抖。第三四元数归一化是必须的四元数不归一化后面转欧拉角会算出非法的值完成一次更新就驱散一次发散。3.4 四元数转欧拉角四元数虽然计算方便但人眼没法直观理解。最终控制里面用的和上位机显示的都是欧拉角。转换公式float roll atan2f(2*(q0*q1 q2*q3), 1 - 2*(q1*q1 q2*q2)) * 57.29578f; float pitch asinf(2*(q0*q2 - q3*q1)) * 57.29578f; float yaw atan2f(2*(q0*q3 q1*q2), 1 - 2*(q2*q2 q3*q3)) * 57.29578f;乘57.29578是把弧度转成度。需要注意pitch角的asinf定义域如果姿态接近±90度pitch接近±1时可能出现NaN这在四旋翼的基本动作里很少遇到但特技飞行动作要注意。另外欧拉角在pitch±90度附近会出现万向锁问题导致yaw和roll的耦合跳变。对于一般的姿态控制我们控制角度都限制在小范围问题不大但如果做全姿态运动就得考虑用四元数直接做控制绕开欧拉角。4. 串口通讯与匿名上位机协议打包4.1 串口参数与初始化STM32和上位机的通讯我用USART1波特率1152008N1。匿名上位机对波特率的支持很宽松115200够用每包姿态数据的数据量也不大。这里提一个操作习惯串口的发送和接收我都在中断里处理发送用DMA。但简单演示项目用阻塞发送就够了因为一包姿态数据就几十个字节USART1在115200波特率下发送一包数据大约2毫秒而解算和控制周期是5毫秒只要发送安排在解算完成后不影响主循环。不过如果以后加数传、加遥控指令总线冲突会越来越明显那时候再想办法。我的做法是先跑通再去优化。4.2 匿名上位机协议的帧格式匿名上位机的通讯协议不同版本细节有差异我用的是V2.x系列比较通用的格式帧头0xAA帧头校验0xAA功能字0x01表示发送主子式的姿态数据数据长度0x1016字节数据区数据5个shortroll、pitch、yaw、相对高度、预留校验和从帧头到数据区最后一个字节全部累加需要特别提醒匿名上位机的协议出了很多个版本老版本的帧头可能是0xAA 0xFF数据长度字段的定义方式也不一样。直接照抄网上的代码如果上位机版本对不上永远收不到数据。最靠谱的做法是下载上位机的时候顺手把它的协议说明文档一起存下来核对每个字段的定义。我在这个项目里用的就是上面这个格式搭配我下载的匿名上位机V2.6稳定显示没有问题。数据打包代码uint8_t tx_buffer[64]; void Send_Attitude_To_PC(float roll, float pitch, float yaw) { uint8_t i; uint16_t sum 0; int16_t a (int16_t)(roll * 100); int16_t b (int16_t)(pitch * 100); int16_t c (int16_t)(yaw * 100); tx_buffer[0] 0xAA; tx_buffer[1] 0xAA; tx_buffer[2] 0x01; // 功能字 tx_buffer[3] 0x10; // 数据长度16字节 tx_buffer[4] a 8; tx_buffer[5] a 0xFF; tx_buffer[6] b 8; tx_buffer[7] b 0xFF; tx_buffer[8] c 8; tx_buffer[9] c 0xFF; tx_buffer[10] 0x00; tx_buffer[11] 0x00; // 剩余数据区填0 for (i 12; i 19; i) tx_buffer[i] 0x00; sum 0; for (i 0; i 19; i) sum tx_buffer[i]; tx_buffer[19] sum 0xFF; for (i 0; i 20; i) USART1_SendByte(tx_buffer[i]); }注意姿态角扩大100倍再转short上位机那边才能看到带两位小数的角度值。角度是0.01度我发送的是乘以100之后的整数上位机收到再除以100显示。这样既节省带宽又避免了浮点数在串口协议里传输的对端大小端问题。如果直接把float的四个字节发过去看起来省事但上位机侧字节序和浮点表示法只要有一点点不一致显示出来就是个天文数字。4.3 主循环任务调度最后看主循环。整个程序的结构是定时器中断里以固定频率读取MPU6050原始数据跑Mahony解算主循环里把最新算好的欧拉角打包发送给上位机。也可以反过来解算在定时器中断完成发送也放在中断里主循环就做别的事情。我习惯用SysTick定时器做2ms周期调度解算频率稳定在约200Hz和Mahony里的sampleFreq保持一致。实际项目中我观察到如果解算周期波动姿态角会有周期性的微弱跳动。尤其当你用while循环加Delay来控频时一旦主循环里插入耗时操作采样间隔就会抖动解算质量直接受影响。所以解算频率必须由定时器保证不能依赖主循环的速度。这是一个飞控项目里必须养成的好习惯。5. 调试实录常见问题与排查技巧5.1 读取不到MPU6050数据现象I2C读出来的原始数据全是0或者WHO_AM_I读不到0x68。排查顺序先查电源VCC和GND在模块上有没有短接再查SDA和SCL有没有焊反这个比想象中常见然后查上拉电阻很多模块PCB上已经焊了上拉但有的模块是没有的必须外部加4.7kΩ最后查I2C地址AD0引脚悬空或者拉高会把地址变成0x69如果固件里写死0x68就读不到。我的经验是MPU6050读不到数据时首先用逻辑分析仪或者示波器看I2C引脚波形看ACK位有没有正常拉低。直接看波形能省很多猜测的时间。如果没有示波器就在初始化后读WHO_AM_I寄存器对照读返回值用小灯泡逻辑判断哪一步出错了。5.2 姿态角缓慢漂移现象静止放置时roll和pitch还会缓慢变化几分钟内飘了几度。原因大多是陀螺仪零漂。每个传感器出厂时的零点不完全在0存放环境温度变化零漂还会变化。最简单的处理办法是上电校准在上电后先让飞控静止两三秒采100组陀螺仪数据求平均把这个平均值记为GyroOffset在每次读取原始数据之后先减去这个偏移量再做单位换算。这种方法能显著改善姿态漂移。还有一种漂移是因为积分漂移。如果陀螺仪零漂偏大但加速度计对解算的修正力度不够误差就会慢慢积累。这时可以把Mahony的Kp调大一点让加速度计的修正权重更高。Kp太小姿态懒洋洋的Kp太大抗震动会变差我一般从0.5开始步进0.1往上调观察悬停时的表现。5.3 上位机数据跳动现象上位机曲线一直都在但角度在±10度以上来回跳或者飞行时姿态角剧烈抖动。如果静止时数据跳通常先是震动影响加速度计电机转动产生的高频振动会被加速度计捕捉到造成修正方向混乱。处理办法确认DLPF配置是否合理带宽太高会把电机的高频振动放进来检查安装传感器尽量用减振泡棉跟机架隔离看看效果。如果飞行时跳还有一个常见原因是控制周期和解算周期不匹配。比如你以200Hz解算但控制是100Hz两次控制之间姿态数据已经换了好几次了控制输出就会滞后。把解算和控制统一到同一个频率上问题就自然消失了。5.4 上位机完全不显示现象串口助手能看到数据但匿名上位机界面上没反应。优先级最高的可能性是帧格式不匹配功能字或者长度字段对不上。先在上位机的协议说明里确认帧头、功能字、长度字段、校验方式。校验和有一个很隐蔽的坑匿名上位机的部分版本里校验和是从帧头第一个字节开始算到数据区最后一个字节但有些版本是把校验和单独按16位累加再取低8位而有些是8位溢出截断。我测试下来我用的这个版本是8位累加如果你照抄的代码校验方式对应的是另一个版本就会一直校验失败。遇到串口乱码时先看波特率、数据位、停止位、校验位是否和上位机一致再看接地是否共用模块和USB转TTL一定要共地不然数据会乱。5.5 解算出来的角度和实际相反最后补一个很容易踩的逻辑坑有些MPU6050模块安装方向不同X、Y、Z轴和机体的方向对不上直接解算出来的roll和pitch会和实际姿态相反或者X轴加速度数据和Y轴数据反了。这个不是代码问题而是传感器坐标系和机体坐标系的安装关系没有对齐。解决办法在初始化阶段定义一个传感器方向映射表比如把加速度计的X、Y轴做个符号翻转或者交换轴序让传感器的坐标系和机体坐标系对准。我一般会在桌上用软件逐轴转动飞控对比上位机显示的姿态变化方向来确认是否需要翻转。这个步骤在整机装配后一定要做千万不要假设PCB上印的轴方向就一定对。5.6 调试问题速查表把上面这些问题整理成一个速查表现场调试的时候直接对着查现象大概率原因快速检查方法I2C读不到WHO_AM_I接线、地址、上拉问题示波器看ACK检查0x68/0x69静态姿态漂移陀螺仪零漂未校准上电静止求平均偏移量数据跳动震动影响加速度计检查DLPF带宽和减震安装上位机无显示协议版本不匹配、校验错误对照协议文档核对功能字和校验方式串口乱码波特率不一致、未共地检查串口配置和GND连接角度与实际相反传感器坐标轴未对齐逐轴转动飞控对比姿态显示方向这篇就写到这里。个人经验之谈姿态解算这种模块代码本身不难难的是调试时把算法当作黑盒去猜。建议拿到我的代码后自己把四元数更新公式和欧拉角转换公式都画一画搞清楚每一步数据是怎么变换的。后面系列第三篇我会把姿态数据接到PID控制器里再做自稳模式到时候你会发现这篇打下的解算基础直接决定了飞控能不能飞稳。自己写一遍解算算法比抄十遍DMP库都值得。本文还有配套的精品资源点击获取