简介本资源面向STM32嵌入式开发与四旋翼无人机控制的学习者聚焦MPU6050六轴传感器的姿态解算及匿名上位机串口通讯实现适合已具备一定C语言与单片机基础、希望深入理解飞行姿态估计与实时通信的开发者。压缩包共1033个文件以566个c源文件、251个h头文件、51个s汇编文件为主另含工程配置、库文件与少量文档整体约28.44MB目录结构完整便于按模块查阅与移植。代码涵盖传感器初始化、原始数据采集与温度补偿、互补滤波或卡尔曼滤波姿态解算、UART串口通信以及PWM电机控制等关键环节并配套匿名上位机V5.0版本可用于实时显示姿态数据、发送控制指令与调试飞行参数。目前已有12971人学习下载适合作为无人机飞控入门与STM32综合实践的参考工程。1. 从一串飘忽的欧拉角说起这块 MPU6050 到底怎么把姿态算稳很多人第一次把 MPU6050 焊上 STM32 四旋翼串口打印出来的 Roll、Pitch、Yaw 三个角要么疯狂抖动要么静止时缓慢漂移飞控还没起飞就已经在“自嗨”。这个项目要解决的就是这件事用 STM32 读取 MPU6050 的原始加速度和角速度在单片机里完成姿态解算把六轴数据融合成稳定的姿态角并通过串口按匿名上位机协议实时上传波形。它适合正在做四旋翼飞控、想搞懂姿态解算链路、或者需要一个能直接对接匿名上位机看波形的开发者。整套代码围绕“采集—解算—上传”这条主线展开跑通之后你就能在匿名上位机上看到三条平滑的姿态曲线而不是一团噪声。2. 姿态解算链路拆解从 I2C 原始数据到欧拉角输出2.1 为什么不能直接用加速度算角度加速度计测的是重力分量静止时确实能反推出倾角但一旦机体有运动加速度这个角度就被污染了。角速度积分短期平滑长期又会漂移。所以姿态解算的本质是互补用加速度计的低频分量去修正陀螺仪积分的高频漂移。常见做法是互补滤波或 Mahony 滤波这个项目里两条路都值得走一遍先跑通互补滤波理解原理再换 Mahony 看效果差异。MPU6050 内部其实自带 DMP可以直接输出四元数但很多开发者选择自己解算原因有两个一是 DMP 固件加载麻烦、可调性差二是自己写滤波能真正理解姿态从哪来。这个项目走的是自己解算的路线代码可控参数可调适合二次开发。2.2 数据采集I2C 读原始寄存器的关键配置MPU6050 通过 I2C 与 STM32 通信初始化阶段要配置采样率、量程和数字低通滤波。下面这段是典型的初始化代码量程和滤波参数直接决定后面解算的噪声水平。// MPU6050 初始化配置陀螺仪和加速度计量程、采样率、DLPF void MPU6050_Init(void) { MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01); // 解除休眠选择陀螺仪X轴时钟 MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09); // 采样率 1kHz/(19) 100Hz MPU6050_WriteReg(MPU6050_CONFIG, 0x03); // DLPF 44Hz陀螺仪延迟 4.9ms MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18); // 陀螺仪量程 ±2000°/s MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x10); // 加速度计量程 ±8g }这里几个参数值得说清楚。SMPLRT_DIV设成 9对应 100Hz 采样和后面滤波的积分周期要匹配否则时间常数会算错。CONFIG的 DLPF 设成 44Hz是为了在采集阶段先滤掉一部分高频振动四旋翼电机振动很大这一步不做后面滤波压力会很大。陀螺仪量程选 ±2000°/s 是因为四旋翼翻滚时角速度可能很大量程太小会削顶。加速度计 ±8g 是留了余量正常悬停时只有 1g 左右。读取原始数据的函数要保证一次连续读出 14 个字节避免加速度和角速度来自不同时刻// 连续读取 14 字节加速度 6 温度 2 陀螺仪 6 void MPU6050_ReadRaw(int16_t *acc, int16_t *gyro) { uint8_t buf[14]; MPU6050_ReadRegs(MPU6050_ACCEL_XOUT_H, buf, 14); acc[0] (int16_t)((buf[0] 8) | buf[1]); // AccX acc[1] (int16_t)((buf[2] 8) | buf[3]); // AccY acc[2] (int16_t)((buf[4] 8) | buf[5]); // AccZ gyro[0] (int16_t)((buf[8] 8) | buf[9]); // GyroX gyro[1] (int16_t)((buf[10] 8) | buf[11]);// GyroY gyro[2] (int16_t)((buf[12] 8) | buf[13]);// GyroZ }注意 buf[6]、buf[7] 是温度这里跳过。连续读的意义在于保证六轴数据的时间一致性如果分两次读中间隔了几百微秒高速旋转时解算会引入额外误差。2.3 互补滤波用一行公式把漂移压下去互补滤波的核心就一个公式角度 陀螺仪积分 × 高频权重 加速度角度 × 低频权重。代码里通常写成先积分再修正的形式// 互补滤波姿态解算dt 为采样周期秒 void ComplementaryFilter(float *angle, float gyro_rate, float acc_angle, float dt) { // 陀螺仪积分得到预测角度 float predicted *angle gyro_rate * dt; // 用加速度角度做低频修正系数 0.98 表示信任陀螺仪 98% *angle 0.98f * predicted 0.02f * acc_angle; }dt必须和实际采样周期一致100Hz 采样就是 0.01。系数 0.98 是经验值越大越平滑但修正越慢越小跟随越快但噪声越大。实际调试时可以先从 0.95 开始看波形再往上加。这个公式对 Roll 和 Pitch 有效Yaw 因为没有绝对参考没有磁力计只能靠陀螺仪积分会缓慢漂移这是六轴方案的固有短板项目里如果只做姿态显示可以接受做航向控制就需要加磁力计。2.4 Mahony 滤波四元数更新的完整实现互补滤波够用但不够优雅Mahony 滤波用四元数表示姿态通过加速度计修正陀螺仪偏差效果更稳。核心是三步用当前四元数估计重力方向、与实测重力做叉积得到误差、用误差修正角速度并更新四元数。// Mahony 滤波核心误差修正 四元数更新 void Mahony_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; // 归一化加速度加速度为 0 时跳过修正 recipNorm invSqrt(ax*ax ay*ay az*az); ax * recipNorm; ay * recipNorm; az * recipNorm; // 用四元数估计重力方向旋转矩阵第三列 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); // 积分误差修正陀螺仪零偏 if (twoKi 0.0f) { exInt twoKi * halfex * dt; eyInt twoKi * halfey * dt; ezInt twoKi * halfez * dt; gx exInt; gy eyInt; gz ezInt; } // 比例项直接修正角速度 gx twoKp * halfex; gy twoKp * halfey; gz twoKp * halfez; // 一阶龙格库塔更新四元数 gx * (0.5f * dt); gy * (0.5f * dt); gz * (0.5f * dt); qa q0; qb q1; qc q2; q0 (-qb*gx - qc*gy - q3*gz); q1 (qa*gx qc*gz - q3*gy); q2 (qa*gy - qb*gz q3*gx); q3 (qa*gz qb*gy - qc*gx); // 归一化四元数防止数值漂移 recipNorm invSqrt(q0*q0 q1*q1 q2*q2 q3*q3); q0 * recipNorm; q1 * recipNorm; q2 * recipNorm; q3 * recipNorm; }twoKp是比例增益决定修正强度一般取 1.0 到 2.0twoKi是积分增益用来消除陀螺仪零偏取 0.001 到 0.01 量级。这两个参数是 Mahony 调参的核心twoKp太大会引入加速度噪声太小收敛慢twoKi太大会低频振荡。invSqrt是快速平方根倒数STM32 没有硬件除法时能省不少时间也可以用标准库的1.0f/sqrtf()替代。四元数更新完最后转成欧拉角输出// 四元数转欧拉角输出单位为度 void Quat_ToEuler(float *roll, float *pitch, float *yaw) { *roll atan2f(2.0f*(q0*q1 q2*q3), 1.0f - 2.0f*(q1*q1 q2*q2)) * 57.29578f; *pitch asinf(2.0f*(q0*q2 - q3*q1)) * 57.29578f; *yaw atan2f(2.0f*(q0*q3 q1*q2), 1.0f - 2.0f*(q2*q2 q3*q3)) * 57.29578f; }57.29578是 180/π把弧度转成度。asinf的参数要保证在 -1 到 1 之间浮点误差可能导致越界稳妥做法是加一个限幅。3. 匿名上位机串口通讯把波形实时画出来3.1 匿名上位机协议帧格式匿名上位机Anonymous Ground Station是飞控调试常用的波形工具它有一套固定的帧格式。典型的数据帧是帧头 0xAA 0xAA 功能字 数据长度 数据 校验和。功能字 0x01 到 0x0A 对应不同的数据组合比如 0x03 表示发送三个 int16 数据。下面是一个发送 Roll、Pitch、Yaw 三路姿态的帧封装// 按匿名上位机协议封装三路姿态数据并发送 void Send_Attitude_Frame(float roll, float pitch, float yaw) { uint8_t buf[16]; int16_t r (int16_t)(roll * 100); // 放大 100 倍保留两位小数 int16_t p (int16_t)(pitch * 100); int16_t y (int16_t)(yaw * 100); uint8_t sum 0; buf[0] 0xAA; buf[1] 0xAA; // 帧头 buf[2] 0x03; // 功能字三路 int16 buf[3] 6; // 数据长度 6 字节 buf[4] r 0xFF; buf[5] r 8; // Roll 低字节在前 buf[6] p 0xFF; buf[7] p 8; // Pitch buf[8] y 0xFF; buf[9] y 8; // Yaw for (int i 0; i 10; i) sum buf[i]; buf[10] sum; // 校验和 UART_SendBytes(buf, 11); }数据放大 100 倍是为了在匿名上位机上显示小数接收端会除以 100。字节序是小端低字节在前这是匿名上位机的约定。校验和是从帧头累加到最后一个数据字节不包括校验和本身。发送频率建议和采样率一致或减半100Hz 采样可以 50Hz 发送避免串口带宽不够。3.2 串口发送的实时性处理姿态解算在主循环里跑串口发送如果阻塞太久会影响解算周期。常见做法是用 DMA 发送或者环形缓冲区把数据丢进缓冲区就返回由中断或 DMA 慢慢发。简单项目里直接阻塞发送也能跑但要注意波特率。115200 波特率下11 字节的帧大约 1ms 发完50Hz 发送占用 5% 的 CPU 时间可以接受。如果波特率只有 9600一帧要 11ms50Hz 就发不过来了波形会卡顿。// 环形缓冲区发送避免阻塞主循环 #define TX_BUF_SIZE 256 static uint8_t tx_buf[TX_BUF_SIZE]; static volatile uint16_t tx_head 0, tx_tail 0; void UART_SendBytes_DMA(uint8_t *data, uint16_t len) { for (uint16_t i 0; i len; i) { uint16_t next (tx_head 1) % TX_BUF_SIZE; if (next tx_tail) break; // 缓冲区满丢弃 tx_buf[tx_head] data[i]; tx_head next; } // 触发 DMA 发送具体实现依赖 HAL 库或寄存器配置 UART_Start_DMA_If_Idle(); }缓冲区大小 256 字节按 11 字节一帧算能存 23 帧足够应对短时突发。tx_head和tx_tail用volatile修饰因为会在中断里被修改。缓冲区满时直接丢弃新数据比阻塞等待更安全姿态数据丢一帧不影响观察。3.3 上位机端的配置与波形观察匿名上位机打开后要选对串口号和波特率然后在“数据格式”里选对应的功能字。比如代码里用 0x03上位机就选“三轴数据”或自定义格式。波形窗口里可以设置每路数据的颜色和缩放Roll 和 Pitch 正常应该在 ±180 和 ±90 范围内Yaw 会缓慢漂移。观察时重点看三点静止时波形是否平稳、快速转动时是否跟随、回正后是否收敛。如果静止时抖动大回去调 DLPF 或滤波系数如果转动时滞后明显增大twoKp或互补滤波的陀螺仪权重。4. 避坑与排查姿态解算最容易翻车的几个点4.1 现象静止时角度缓慢漂移几分钟偏几十度原因陀螺仪零偏没有校准积分后误差累积。MPU6050 出厂零偏可能达到 ±20°/s不校准根本不能用。解决上电后保持静止采集 500 到 1000 个陀螺仪样本求平均作为零偏在后续读数里减掉。Mahony 的积分项也能消除零偏但收敛需要时间手动校准更快。// 上电静止校准陀螺仪零偏 void Gyro_Calibrate(int16_t *gyro_offset) { int32_t sum[3] {0}; int16_t raw[3]; for (int i 0; i 1000; i) { MPU6050_ReadGyro(raw); sum[0] raw[0]; sum[1] raw[1]; sum[2] raw[2]; Delay_ms(2); } gyro_offset[0] sum[0] / 1000; gyro_offset[1] sum[1] / 1000; gyro_offset[2] sum[2] / 1000; }校准期间不能有振动放在桌面上别碰。1000 次采样 2ms 间隔总共 2 秒可以接受。4.2 现象波形周期性抖动和电机转速相关原因电机振动通过机架传到 MPU6050加速度计被高频噪声污染滤波修正时把噪声引入角度。解决硬件上加减震棉或减震板软件上降低 DLPF 截止频率或加低通滤波。DLPF 设到 20Hz 以下能滤掉大部分电机振动但会引入延迟需要权衡。另一个办法是在加速度计数据进滤波前先做滑动平均。4.3 现象匿名上位机波形不动或乱码原因串口波特率不匹配、帧格式不对、校验和算错、字节序反了。解决先用示波器或逻辑分析仪看串口波形确认波特率。然后检查帧头是不是 0xAA 0xAA功能字和上位机设置是否一致。校验和最容易错注意是从帧头累加到数据末尾不包括校验和自己。字节序如果反了波形会变成乱跳的大数把高低字节对调试试。4.4 现象快速转动后角度回不到零有残余误差原因互补滤波系数太小修正太慢或者 Mahony 的twoKp太小收敛慢。解决增大修正系数。互补滤波从 0.98 降到 0.95 试试Mahony 的twoKp从 1.0 加到 2.0。但别加太大否则静止时噪声会被放大。调参时一边看波形一边改找到抖动和收敛的平衡点。4.5 现象Yaw 一直往一个方向转停不下来原因六轴方案没有绝对航向参考Yaw 只能靠陀螺仪积分零偏和噪声都会导致漂移。解决这是原理性缺陷软件上只能减缓不能消除。如果应用需要稳定航向加磁力计做九轴融合用磁力计修正 Yaw。项目里如果只是观察姿态Yaw 漂移可以忽略或者定期手动置零。5. 二次开发改造从能跑到好用我踩过的那些坑代码跑通只是第一步真正用到飞控里还要做不少改造。第一个改造点是解算周期。主循环里如果还有其他任务姿态解算的dt就不准了滤波会出问题。我的做法是用定时器中断触发解算固定 5ms 或 10ms 一次dt用定时器实际周期这样不管主循环多忙解算节奏都是稳的。定时器中断里只做数据读取和滤波串口发送放到主循环避免中断里耗时太长。第二个改造点是参数在线可调。调滤波系数时反复烧录很麻烦我一般会加一个简单的串口命令解析收到特定字符串就修改twoKp、twoKi或互补滤波系数。这样在匿名上位机旁边就能边看波形边调效率高很多。命令格式可以很简单比如KP1.5表示把twoKp设成 1.5解析时用sscanf或自己写状态机。第三个改造点是数据记录。匿名上位机只能看实时波形想回放分析就得存数据。可以在串口发送的同时把姿态数据写到 SD 卡或 Flash事后导出成 CSV 用 Python 画图。SD 卡写入慢建议用 DMA 或者降低写入频率比如每 10 帧存一次。第四个改造点是传感器融合扩展。六轴方案 Yaw 漂移是硬伤加一个 HMC5883L 或 QMC5883 磁力计做九轴融合Yaw 就能稳定。磁力计校准比陀螺仪麻烦要转八字校准而且电机磁场干扰大安装位置要远离电源线。融合算法可以用 Mahony 的九轴版本把磁力计数据也做叉积修正。最后一个改造点是故障保护。MPU6050 的 I2C 通信偶尔会失败读出来全是 0 或 0xFFFF。如果不判断直接进滤波姿态会瞬间跳变。我的习惯是在读取后加一个合理性检查加速度模长应该在 1g 附近角速度不应该超过量程超出范围就丢弃这一帧用上一帧数据继续积分。这样偶尔的通信失败不会导致姿态崩溃。从那以后我每次调姿态解算都强制先跑一遍静止校准再看波形确认零偏和噪声水平最后才上电机测试。这个顺序能省掉很多返工。希望帮到你。本文还有配套的精品资源点击获取
