STM32F407气压计定高四轴实战：从滤波到串级PID完整解析

简介：本资源是一套基于STM32F407微控制器的四轴飞行器飞控源码工程，面向嵌入式开发者、无人机爱好者及高校电赛/毕业设计实践者，聚焦气压计与超声波双模融合定高控制这一核心难点。压缩包共590个文件，含317个C源码（含传感器驱动、PID定高算法、电机控制逻辑）、88个头文件（定义硬件接口与参数结构）、48个编译中间文件（.o/.d）及Keil工程配置（.uvproj/.uvopt）、启动汇编（cstart_thumb2.asm）、固件镜像（.axf/.hex）等，完整覆盖从底层驱动到闭环控制的全链路实现，包体大小为8.53MB。已有269人学习下载，源码中已集成气压计温度补偿、超声波回波滤波、多传感器数据融合策略及可调PID参数表，配合ANO调试工具支持实时高度曲线观测与参数在线调优，是深入理解飞控姿态与高度协同控制的高质量参考工程。

1. 为什么是F407+气压计这套方案：从姿态到高度的第一个台阶

做四轴定高这件事，看起来只是一个“飞控里加一个传感器”的改动，但真正把它从Demo推进到能稳定悬停，中间涉及的坑比想象中多得多。这篇东西我想把整套基于STM32F407飞控板、ANO飞控框架、气压计定高的实现思路和调试过程完整捋一遍，适合已经在跑姿态环、想往更上层加功能的新手，也适合手头正好有F407板子、想从零把一个定高四轴立起来的人。

先交代一下背景。定高这个概念本身不难理解：飞机在遥控器不给油门变化的时候，高度维持在一个给定值附近。但为什么要用F407而不是更便宜的F103？答案很直接：F407主频168MHz，带硬件浮点运算单元FPU，跑ADC采样、I2C总线读取气压计、姿态解算、串级PID控制四个任务叠加起来，CPU占用率仍然能压在40%以内。F103做姿态解算还能凑合，再加上高度融合、气压计滤波、速度估计这些任务，就开始吃紧了，尤其在中断退出延迟上容易让控制环出现抖动，表现出来就是油门响应钝、悬停掉高度。

ANO这套框架我用了挺长时间，匿名科创社区最初开源的ANO_FC就是基于F407做的。它的架构清晰度在同类开源飞控里属于比较友好的那种，任务调度分成：传感器读取任务、姿态解算任务、控制任务、通信任务。我加定高功能时，主要是在传感器任务里加入气压计采样和外置滤波，在控制任务里叠加高度/速度双闭环，思路就是：外环高度环输出期望垂直速度，内环速度环输出期望油门。这一套结构现在很多商业飞控也这么用，只是参数和融合方式各家有差异。

定高四轴的核心价值，是让操作者从“时刻都得盯着油门”里解放出来。飞姿态模式时，哪怕油门打了同一个位置，电池电压从4.2V掉到3.7V，电机转速下降，飞机就会缓慢下沉；有了定高，不用人手微调油门，遥控器油门杆只需要管升降方向或者直接交给高度设定，这对于航拍、室内悬停、编队飞行都是刚需。所以这篇的落点不只是“把代码下载进去能起飞”，而是拆开讲清楚：气压计的原始数据怎么变成可信的高度、高度怎么变成一个稳定的控制量、控制量怎么变成不抖动的油门输出。

2. 硬件选型和飞控板布局：气压计最容易被忽视的“命门”

2.1 飞控板和传感器选型边界

我用的是ANO系列经典的F407飞控板，主控为STM32F407VGT6，板上集成了MPU6500六轴IMU和MS5611气压计。MS5611是个很老练的型号，24位ADC输出，分辨率可以做到10cm级（实际能稳定扑捉到的高度变化约15-20cm），量程从10mbar到1200mbar，对应海拔覆盖从-1000米到20000米左右，家用和航拍机完全够用。相比之下，BMP280虽然便宜，但它主要是为手机气象服务设计的，气压噪声和温漂性能比MS5611差了一个档次，同样是高度输出，BMP280在一米高度上数据能抖出±30cm的跳动，MS5611同样环境下能压到±10cm，这个差距在控制环里会被放大，导致油门来回抽动。因此定高项目里，气压计首选MS5611，预算特别紧才考虑SPL06。

选好气压计之外，飞控板布局的判断更重要。MS5611对气流极其敏感，但也怕热、怕风、怕光。在飞控板上，它离电源模块和主控芯片至少要保持5mm以上间距，否则MOS管和稳压芯片的热量会直接抬高气压计局部温度，造成漂移。我见过一款山寨飞控，把MS5611贴着4A降压芯片放，开机两分钟高度数据就虚漂了3米。所以硬件到手先做静态测试：上电后把板子放桌上静止不动，串口打印高度值，观察两分钟内高度是否漂移超过50cm。如果漂移很大，第一怀疑对象不是算法，而是板子的热源干扰。

2.2 气压计安装的“遮风”与“透气”矛盾

螺旋桨的下洗气流是定高最大的物理干扰源。电机一转动，大量气流直接冲刷飞控板顶部的气压计封装。MS5611的芯片顶部有一个微小的通气孔，外部气压就是通过这个孔感知的。如果不做遮挡处理，下洗气流会形成局部压强波动，高度数据会像喝醉了一样上下乱跳。

常规做法是给气压计盖上一块密度适中的泡棉，泡棉厚度在5mm左右，内部要有一块空腔。注意：泡棉不是把孔堵死，而是缓冲气流的急变。我试过三种方案：

空腔泡棉的原理是：泡棉本身滤掉高频气流扰动，内部空腔给气压变化一个缓冲时间，相当于给气压计加了一个机械低通滤波器。这个方法成本几乎为零，效果却立竿见影。安装时还要注意侧面也要遮挡风向，室内开风扇测试时，侧风也会从泡棉边缘钻进去影响读数。

2.3 供电与I2C总线细节

MS5611是I2C接口，飞控板上通常作为I2C总线上的唯一或之一设备。F407的I2C外设历史上饱受吐槽，很多人直接用GPIO模拟I2C来规避硬件I2C的怪癖。我的经验是：在ANO框架里直接用硬件I2C其实问题不大，但必须把I2C时钟配置在200kHz左右，太高的时钟在长线缆上容易出ACK丢失。另外MS5611的VDD脚要并接100nF和10uF电容，这一点很多自制PCB图省事只放一个百纳法电容，上电瞬间电压跌落会导致传感器初始化失败，串口读到固定值0x0000，排查半天其实是供电不足。

现在我需要强调一个布局顺序：飞控板供电一般由分电板或电源模块通过杜邦线引出，线长超过15cm时，I2C数据线SDA/SCL建议和电源线分开走线，避免电机大电流变化在电源线上产生压降尖峰，耦合到I2C线上干扰通信。成品飞控板一般已经考虑好了，但我见过不少自己改线导致I2C偶发卡死的情况，卡死表现为气压计数据停止刷新，飞控三秒后触发失控保护。

3. 气压计原始数据如何变成可信高度：从I2C读取到滤波融合

3.1 MS5611读取时序与代码骨架

先把气压计的底裤看清。MS5611内部有两个测量通道：压力通道和温度通道。它的工作流程是：发送转换指令启动压力测量，等待转换完成（典型时间由OSR决定），读取24位压力原始值；再启动温度测量，读取24位温度原始值；然后利用芯片出厂时烧录的6组校准系数（C1-C6）做补偿计算，得到真实气压值和温度值。

我的做法是定高循环里用读取优先级轮询的方式，每次调度周期只发起一种测量，然后读上一次的结果，避免阻塞等待。伪代码框架如下：

// 每5ms调用一次 void baro_poll(void) { static uint8_t state = 0; uint32_t raw_press, raw_temp; switch (state) { case 0: ms5611_start_conv(MS5611_PRESS, OSR_2048); state = 1; break; case 1: if (ms5611_read_adc(&raw_press) == 0) { pressure_raw = raw_press; ms5611_start_conv(MS5611_TEMP, OSR_2048); state = 2; } break; case 2: if (ms5611_read_adc(&raw_temp) == 0) { temp_raw = raw_temp; pressure = ms5611_compensate(pressure_raw, temp_raw); state = 0; } break; } }

OSR（过采样率）是个关键参数。MS5611支持256/512/1024/2048/4096五档过采样率，OSR越高内部ADC积分时间越长，输出噪声越低，但同时单次转换时间越长。用OSR_2048对应转换时间约5ms，噪声幅度约0.06mbar，折算高度噪声约0.5米RMS。这个值从原始噪声看并不算小，所以不能直接用瞬时值，必须先经过滤波。

3.2 高度换算与温度补偿细节

从气压值换算高度的公式是国际气压测高公式：

altitude = (1.0 - pow(pressure / 101325.0, 0.190295)) * 44330.0;

注意，公式里的101325Pa是标准海平面气压。很多人都知道这个公式，但忽略了一个重要问题：天气变化和飞行环境温度差会导致基准气压漂移。所以正确的做法是开机时锁定当前气压为H0，之后所有高度输出都相对于H0计算，这样就不需要知道绝对海拔基准，公式变成：

relative_altitude = (1.0 - pow(pressure / pressure_takeoff, 0.190295)) * 44330.0;

其中pressure_takeoff是解锁时记录的基准气压。这个逻辑一定要在起飞瞬间锁定基准，否则你在地面站里校准完、通电后放了一小时，气压变化5米，再起飞时高度就偏了。

温度补偿同样不能省。MS5611的补偿函数会利用C1-C6做二阶补偿，保证从-40°C到+85°C范围内精度在±1.5mbar以内。忽略补偿直接拿原始值用的后果是：飞控板子发热后，高度会缓慢正漂移，看起来像飞机自己在往上爬，定高变成“缓慢爬升模式”。

3.3 滤波方案：从滑动平均到限幅互补

气压计原始高度数据直接用于控制是灾难性的。我在OSR_2048下实测，原始高度数据每25ms跳动大约±0.5米，而这个跳动频率和电机转速、结构振动、机架弹性都有关系。直接把这个值送进PID，P项会让油门疯狂抖动，机身高频率颤抖。

基础处理是滑动平均滤波窗口取20个点，每10ms一个点，窗口200ms，能把高频抖动压缩到±8cm。但滑动平均有个天然的滞后问题，窗口越长滞后越大，飞机从2米快速爬升到3米时，滤波后的高度值要拖后将近半秒才到2.8米，这个滞后同样会降低控制稳定性。

我最终用的是两级组合：先做5点中值滤波剔除离群点，再做20点加权滑动平均，最后和加速度计积分高度做一次互补滤波。互补滤波的思路是：

height_fused = alpha * height_baro + (1 - alpha) * (height_prev + vel_vertical * dt);

alpha取值在0.02到0.05之间。气压计负责长期稳定，加速度计积分负责短期响应。气压计长期漂移但是短期噪声大；加速度计积分短期精准但是长期会漂。两者互补，一个管长期，一个管短期，alpha就是信任权重。这套结构跑下来，高度数据的响应延迟从直接滤波的200ms降到了80ms以内，悬停时高度波动能控制在±15cm左右。

关于加速度计积分这部分多说一句：竖直方向的加速度是机体坐标系下z轴加速度和姿态角的三角函数关系，需要在姿态解算算出四元数后，把机体系加速度投影到世界系，取世界系z轴方向分量，减去1g重力加速度，然后再积分。这一步做错了会导致高度振动，最常见的错误是忘了减重力。我刚开始调试时，高度融合曲线像波浪一样上下窜，排查半天发现是加速度z轴没补偿重力项。

3.4 气压计噪声诊断：先分清是“读数噪声”还是“飞控振动”

很多人在定高不稳的时候一上来就调PID，其实要先把噪声源分清楚。排查方法很简单：飞机上锁放在桌上，螺旋桨不转，读取滤波前气压计原始高度数据序列。如果此时波动都超过±30cm，那说明是传感器自身干扰或硬件布局问题，优先做遮挡、滤波、电源去耦。

接下来螺旋桨油门加到30%，不推杆，飞机还在地上压着，读取同样的数据。如果波动显著加剧，那就是气流冲击和机体振动通过PCB传导给气压计造成的影响。这时候调泡棉位置、加橡胶减震柱，比调PID更管用。

最后才是飞起来看高度曲线的毛刺，确认滤波和融合参数。这个排查顺序能省掉至少两天的瞎调时间。

4. 定高控制闭环：外环高度、内环速度与油门前馈的设计路径

4.1 串级PID结构：为什么外层只输出速度而不是直接输出油门

定高控制最忌讳的就是：高度误差直接映射到油门。这种单环控制在小型四轴上几乎必炸，因为高度到油门之间的传递函数严重非线性：同样高度的偏差，电池电压高时油门修正一点点就够，电压低时油门加2%都不够；四个电机转速变化对升力的贡献又受电池内阻和环境温度影响。

所以这里使用内外环串级控制，这是工程上被反复验证的结构：

外环（高度环）：

期望垂直速度 = Kp_h * (目标高度 - 当前高度) + Ki_h * 高度误差积分

当高度低于目标时，这个值是正的（向上）；高于目标时是负的（向下）。这个速度指令作为内环的输入。

内环（垂直速度/加速度环）：

油门修正 = Kp_v * (期望垂直速度 - 当前垂直速度) + Ki_v * 垂直速度误差积分

当前垂直速度的来源可以用高度差分求导，但我更推荐用融合后的垂直速度估计值（就是互补滤波里那个积分量），因为它减掉了很多气压计高频噪声，让内环控制的微分项不那么暴躁。整个结构看下来，外环保高度稳定，内环保高度变化速率，这本质上是“位置-速度”控制的嵌套，抗扰能力强很多。

4.2 垂直速度估计与高度微分的取舍

通俗解释：高度环相当于开车时看与前车的距离来控制车速，速度环相当于踩油门让实际车速跟上期望车速。如果你只看距离去直接踩油门，路一颠簸，车就会一顿一顿；但你先把距离换成期望车速，再一脚一脚微调油门去匹配这个车速，顺畅得多。四轴定高也是同一个道理。

垂直速度估计用互补滤波里得到的vel_vertical，它融合了加速度积分和高度的差分修正。我实际测试时，速度估计的延迟在60ms以内，这足以支撑内环PID控制在120Hz频率下运行得比较平滑。

4.3 油门前馈：悬停油门的重要作用

定高控制里，PID只是修正量，真正的力量来源是一个恒定悬停油门基线。这个值是飞机悬停时能稳定维持高度所需的油门百分比，取决于机架重量、电机效率、螺旋桨螺距、电池电压，每个机器都不一样。我习惯的做法：

1. 手动模式下把飞机飞到1.5米左右，基本稳住高度，读出遥控器油门通道原始值换算成油门百分比（例如遥控器1000-2000，悬停油门在1450附近，则悬停油门=(1450-1000)/1000=45%）。
2. 把这个值写入参数throttle_hover。
3. 定高模式下，总油门输出 = throttle_hover + PID修正。

没有这个前馈，PID只能从头去“找”悬停油门的位置，每次起飞悬停点都有偏移，就会看到飞机升上去、掉下来、再升上去，越晃越厉害。有了前馈，PID只负责修正扰动，修正量从±20%降到±5%，稳定性一目了然。

4.4 定高PID参数整定的实操顺序

参数整定必须按“从内到外”的规则来。先只跑内环速度环，把外环去掉，给定一个固定的期望垂直速度（比如0.5m/s），观察飞机是否能匀速上升/下降。此时Kp_v从0.5开始加，加到期望速度和实际速度的响应快、超调小，Kp_v大约在0.8-1.2之间；Ki_v加0.1左右用于消除稳态误差。

内环调好后再启用外环，Kp_h从0.5开始，给一个0.5米的高度阶跃（比如从1.5米目标高度改成2.0米），观察高度响应曲线。理想响应是在1-2秒内平滑到达目标高度附近，在一个窄带内稳住，不来回穿越。如果高度在目标值附近以明显的周期震荡，通常K作用不足或I作用过大；如果高度到达目标特别慢但最后也稳了，可以适当加大Kp_h和I。

我最后落地的参数组合是：高度环Kp_h=1.2，Ki_h=0.05；速度环Kp_v=0.9，Ki_v=0.1；控制频率外环50Hz，内环100Hz。这个组合在我这个400mm轴距、1080g起飞重量的机架上表现稳定，悬停高度波动±15cm，推力波动小于±5%。换机架时参数不一定通用，但整定顺序和现象判断方法是通用的。

5. 实测中的坑与调参记录：起飞掉高、洗桨效应与温度漂移

5.1 起飞瞬间掉高：气压计被下洗气流“压住”了吗

第一次试飞定高时，我遇到了一个很典型的坑：接通定高模式后，飞机先迅速向上蹿了30cm，然后又突然掉下来接近50cm，最后才算晃晃悠悠稳住。第一反应是PID参数过激，但串口打印数据显示了一个更有趣的现象：气压计高度在起飞瞬间上升了30cm，而实际飞机高度并没有动。

原因是起飞瞬间，油门加大，螺旋桨下洗气流冲击地面再反弹，在飞控板附近形成了一股瞬时高压气流，气压计感受到的压强比环境高，换算出的高度自然往上跳。飞控以为飞机高了，就减小油门，结果高度又往下掉。这就是“起飞掉高”的一个重要诱因。

解决方法分三层：

• 在解锁后到起飞前，不急着把期望高度设置到当前高度，可以先记录起飞瞬间的高度偏移作为基准。
• 油门切换瞬间速度环饱和限制要加，限制期望垂直速度最大±1.5m/s，避免高度计算错误时输出极端油门。
• 起飞前在地面晚几秒再切定高，让气压计完成“地面气流扰动”到“空中流动场”的过渡。

5.2 洗桨效应：低空悬停时的“地面效应”非线性

在距离地面30cm以内低空悬停时，定高非常容易抽风，表现为飞机上下来回窜。这是地面效应在作祟：近地时，螺旋桨下洗气流撞击地面后向外扩散，相当于在飞机下方垫了一个气垫，同样油门提供的升力会突然变大。这个非线性环节和高度环的PID产生耦合，如果PID响应太快，就会在气垫效应区域形成振荡。

处理经验是：定高高度的生效死区设置为±10cm。当高度误差小于死区时，PID输出保持上一值不变，这样避免微小抖动让执行机构来回动作。另外可以降低低空段的外环P项，这点我通过一个高度相关的增益衰减系数实现：高度小于0.5米时，Kp_h按(实际高度/0.5)的平方衰减。

5.3 温度漂移与“假高度”问题

定高飞行到第三块电池时，我发现了一个隐蔽问题：飞控在暖机状态下，高度自己慢慢从1.5米漂到了2.1米，但是视觉上飞机并没有明显上升。这就是气压计温漂带来的“假高度”。

排查过程很有意思，我先是怀疑滤波算法，把alpha调小，没用；换成双滤波，还是漂。最后用温度数据一对比，恍然大悟：MS5611的温度在开机三十秒内从27°C升到42°C，温差15°C带来的零点偏移足以换算成1米的高程差。虽然传感器做了补偿，但板级局部温度梯度依然存在。

解决方案在硬件层面是加大气压计泡棉和主控之间隔热，软件层面有三个补充手段：

1. 开机预热：飞控上电后至少等待40秒再解锁，观察温度变化速率小于0.5°C/3s时才算稳定。
2. 温漂校正：建立温度和高度漂移的查表，每30秒记录一次温度值和高度值，用线性拟合消除相关性部分。
3. 起飞前锁基准：解锁瞬间重新锁定初始高度，消除长时间静态积累的漂移。

这个方法结合之后，我测试中连续飞行8分钟，定高漂移被压在40cm以内，主要还是由于正常环境气压变化的部分，在可接受范围。

5.4 油门响应不平顺的排查：区分“传感器噪声”还是“控制极限环”

调试后期遇到另一个怪问题：定高时油门总是轻微周期性抽动，每0.5秒左右一次，幅度大约油门值的3%，肉眼可见螺旋桨转速在细微地一变一变的。这个现象持续存在，不是偶发。

通过地面站回看数据，发现高度曲线其实很平滑，说明传感器融合没问题；但速度内环的目标速度和控制输出的差值在周期性波动。进一步分析是：内环速度控制频率100Hz，但高度环输出频率50Hz，两个环数据同步时序上差了半个周期，导致速度目标指令里面的更新带有锯齿状的台阶，内环对台阶比较敏感就产生了0.5秒周期的极限环。

解决办法是在高度环和速度环之间加一级速度指令平滑缓冲，用一个二阶低通滤波器处理期望垂直速度，截止频率大约3Hz，让高度环输出的速度阶跃变成平滑斜坡，速度内环的抖动彻底消失。如果遇到类似油门周期性抽动，不要第一时间怀疑气压计，先检查两个控制环之间的信号同步和指令平滑，这是我调过众多飞控后最深刻的经验之一。

5.5 调试飞行过程的记录方法

整个调试过程中，串口日志和地面站数据回放帮了大忙。我会在飞控代码里把以下几个变量以100Hz频率输出：nudged高度（滤波后融合高度）、期望高度、期望垂直速度、实际垂直速度、油门输出百分比、电机PWM原始值。每次飞完一个架次，把日志拉下来用MATLAB离线画图，图和图对照看，几乎每个异常现象都能定位到具体环节。

记得有一次飞机定高时明显前倾俯仰，看日志才发现油门修正过程中姿态角被干扰了0.5度，导致水平方向分力把飞机推走。这个现象如果只看高度曲线是完全看不出来的，必须把姿态角、油门、高度三张图叠在一起看。这也是为什么我一直建议不要只依赖机上飞行效果判断，而是要建立完整的遥测日志习惯。

6. 最后的调参心得与可复用的参数模板

调试了这么长时间，我最大的体会是：定高四轴的难度不在于某个单一环节，而在于传感器、融合、控制三个层面的耦合。气压计数据脏，后面所有环节都会跟着遭殃；融合滞后严重，PID参数调到天上也压不住振铃；前馈油门不准，一切修正都像是蒙着眼睛开车。所以永远记得自底向上排查，先把数据变干净，再谈控制。

现在给出一份可直接上手的参数模板，供参考，具体机架还需微调：

这份模板在400mm轴距穿越机机架、1080g起飞重量、MS5611 + 空腔泡棉、F407飞控板、ANO飞控框架下，实现了悬停高度波动±15cm、给一个0.5m高度阶跃、1.5秒内稳定到目标高度±5cm的实测效果。电池电压从满电4.2V一路降到3.6V，定高无需重新调整，悬停油门前馈已经自动把电量影响吸收掉。

最后再分享一个小技巧：定高代码加好后，不要急着飞，在地面做一个“假装起飞”测试。用一根细绳把机架吊在支架上，让四个电机离地3-5cm，人不去推油门，只把期望高度改成+1米，这时四个电机转速同步升高，绳子拉住机身不让上升，高度环感受到高度不变、速度为零，应该会把油门推到一个稳定值附近。这个测试能验证整个定高控制链路的完整性和稳定性，又不会炸机，非常值得在每次改参数后做一轮，比直接上实飞安全得多。

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