开源飞控ArduPilot备降点机制与源码实现解析

项目标题很直白，“开源飞控APM备降点源码分析”。你要是玩过多旋翼或者固定翼，肯定遇到过类似场景：电池电压警报响了，飞控自动切返航，结果返航点Home在你出发的地方，但你现在离它两公里远，一路顶着逆风飞回去，电量已经是红灯。这时候如果提前在顺风方向设了一个备降点，飞控能直接切过去落下来，事情就简单得多。ArduPilot里这个机制叫Rally Points，中文一般叫备降点或者集合点。这功能平时不起眼，但真到救机的时候，它可能比你的遥控器还可靠。

我最初以为备降点就是个“多存几个Home”，直到翻了AP_Rally库的源码，才发现里面的选择逻辑其实挺讲究。它不仅要管“有没有备降点”，还得管“哪个备降点值得去”“高度怎么给”“什么时候允许用”。这篇文章就围绕备降点相关的源码实现，从数据结构一直拆到实际调试，把我翻代码时觉得关键的几个点都拿出来说一遍。适合正在用ArduPilot跑任务、想优化应急流程的飞手，也适合想读飞控源码但不知道从哪里下手的开发者。

1. 备降点机制拆解：为什么RTL关键时刻需要一个“备用地址本”

1.1 一次返航分析：备降点解决的到底是什么问题

先说一个常见的返航案例。固定翼做长距离巡航，起飞点是一片开阔草地，Home设在起飞点。飞到任务末端时低压报警触发自动RTL，飞机开始朝Home飞。问题在于当天的风向是逆风，飞机往Home飞等于顶着风，地速被削掉一大截，能耗比预计高很多。虽然最终落下来了，但落地时电压已经压着危险线，假如当时有点侧风或者绕了个障碍，结果就是另一回事。

如果当时在航线中段靠近侧风区布置了一两个备降点，飞控会在RTL过程中计算“当前位置往Home走是否顺路经过某个备降点”，一旦发现备降点离当前飞行路径更近、方向也更合理，就直接把目标点切换到备降点，飞机不用硬扛到Home，落地风险和电量压力都能小很多。

这就是备降点的核心价值：它不是代替RTL，而是在RTL的基础上提供一个或多个“中途可落”的备选点。源码层面看，这个逻辑本质上是一次目标点重定向，只是一切判断都发生在飞控内部，不依赖地面站处理。这也是它比“让地面站发指令切航点”更可靠的原因——哪怕数传断了，飞控照样能自己选点。

1.2 备降点与Home返航的区别：多一个选项还是多一重保险

很多人把备降点和Home混在一起理解，其实两者的定位差别很大。Home是解锁和起飞时记录的一个固定经纬度，RTL默认往Home飞，这个点只存一个，没得选。备降点是一组可以预先规划好的点，数量从1直到参数表能容纳的上限，每个点除了经纬度，还可以带高度、固定翼的降落方向和进入高度等额外信息。

从源码角度看，Home对应的是AP_Home模块维护的_home变量，而备降点对应的是AP_Rally模块里的一组数组。两者是独立存储的，逻辑上也不互相覆盖。RTL执行时，飞控会先问“有没有备降点可用”，如果有，再问“哪个备降点适合这次返航”，最后才决定目标是Home还是备降点。

所以备降点本质上是Home的补充选项，不是替代品。设计上也是这么考虑的：当find_rally_point找不到合适备降点时，函数返回false，RTL代码老老实实落到Home逻辑上。这种“能找到就用，找不到就降级”的模式在飞控里很常见，安全性靠降级逻辑兜底，而不是靠强制使用。

1.3 备降点模块在ArduPilot整体架构中的位置与影响范围

ArduPilot中备降点相关功能主要集中在libraries/AP_Rally目录，核心类是AP_Rally。这个库本身不直接负责飞行控制，它只负责备降点的存储、校验和“哪一点合适”的决策。真正把决策结果用起来的，是各个飞控固件里的RTL逻辑，比如多旋翼的ArduCopter/mode_rtl.cpp，固定翼的ArduPlane/commands_logic.cpp，还有地面载具里对应的处理函数。

虽然库很小，但它被引用的面其实挺宽。返航模式会调用它，固定翼的降落进近会调用它，部分失控保护流程也会参考它。这就导致一个问题：你在改备降点相关代码时，很难只靠看一个函数就判断影响面，得顺着调用链一路查下去。我当时第一次改AP_Rally里一个过滤条件，改了以后多旋翼返航一切正常，固定翼的RTL却在某些边界条件下不选备降点了，后来才发现固定翼那边对距离限制和角度限制的传参方式跟多旋翼不一样。

这个模块的影响范围大概可以分成三层：

• 存储层：负责备降点的增删改、总量校验、参数读写。
• 决策层：负责在给定当前位置和目的地后，从备降点列表里选出最合适的一个。
• 执行层：位于各固件的控制器中，决定何时调用决策层，以及怎么使用返回的点。

理解了这三层，后面看源码就不会晕。备降点功能并不是一个“单一函数”就能讲完的东西，它是一条完整的数据链。

2. 源码数据层的底层设计：RallyPoint结构体与参数存储链路

2.1 一个备降点的数据结构里到底存了什么

说备降点之前，先看它存的是什么。ArduPilot里定义备降点用的核心结构体大致长这样，不同固件版本字段顺序可能不同，但主体信息是固定的：

struct RallyPoint { int32_t lat; // 纬度，单位 1e7 int32_t lng; // 经度，单位 1e7 int16_t alt; // 高度，单位厘米 uint16_t break_alt; // 进入备降点时开始转弯下落的高度，厘米（固定翼使用） uint16_t land_dir; // 降落方向，单位百分之一度（固定翼使用） uint8_t flags; // 标志位，用于地面站渲染和功能标记 };

第一次看到这个结构体，可能会觉得字段太少。一个备降点居然只有六个字段，连个名字都没有。但飞控存储空间有限，参数表宽度也有限，够用就行。lat和lng是绝对坐标，alt是相对高度，不是海拔，这一点做地面站规划时特别容易踩坑。flags在旧版本里甚至只是用来告诉地面站这个点是不是当前被选中的点，新版才扩展了些用途。

这里要注意一个版本差异：老版本ArduPilot里，备降点高度字段是int16_t，以厘米为单位，意味着最大只有约327米的高度范围。你要是给高原场地的备降点填了个离谱高度，代码里在解析时就会出问题。后来部分版本对这个字段做了调整，用相对Home高度来存，但你要是翻到一个老版本固件，还是要留个心眼。

2.2 从地面站到飞控：备降点的写入与存储链路

备降点不是靠命令行一条条敲进去的，正常流程是地面站写入。Mission Planner里，Flight Plan页面切到Rally面板，在地图上点几个点，点写参数，地面站会通过MAVLink的RALLY_POINT消息把点位传给飞控，飞控再把它们转成参数保存。

与备降点直接相关的参数，核心是两个：

你可能还会在参数列表里看到类似RALLY_POINT1_LAT、RALLY_POINT2_LNG之类的参数，这些是地面站把备降点写入参数块时生成的，不同的板子参数块大小不同，能存的备降点数量也不一样。常见的是几个到十几个不等，具体看你有多少空闲参数位。

源码里处理写入的是AP_Rally的truncate和相关校验逻辑，目的是防止备降点数量超过实际存储容量。这个函数做的事情听起来简单，但实际写的时候要小心：如果地面站一口气上传了超出容量的备降点，是“全部拒绝”还是“截断保留前N个”，不同固件版本策略有差异。我见过有人在老版本里传了8个点，实际只存进去5个，但地面站显示8个，结果飞行中飞控只考虑前5个，差点造成误解。

2.3 为什么经纬度用1e7、高度用厘米：定点数设计的工程考量

很多新手在看备降点代码时，会对1e7和厘米单位感到疑惑。为什么不直接用浮点数的经纬度？为什么高度不直接用米？

这里其实是嵌入式系统里很经典的定点数设计思路。浮点数在PC上随便用，但在MCU里，复杂的浮点运算会消耗更多CPU周期，而且存储上也有额外的格式开销。经纬度如果用纯小数存，存储格式不统一，还容易因为精度问题在边界值上出错。用1e7格式，也就是把小数部分放大一千万倍后存成整数，比如一个纬度23.1234567度，就存成231234567，这样整数运算就能保证精度，传输和存储也稳定。

高度用厘米而不是米，也是同理。飞行控制里很多参数都习惯用厘米做基本单位，比如RTL_ALT默认1500，其实就是15米。如果你在源码里看到高度相关的变量，脑袋里要自动乘一个0.01换算成米。这个单位习惯贯穿整个ArduPilot生态，备降点只是其中一环。做源码分析时，所有长度单位先确认是“厘米”还是“米”，再谈逻辑，否则一对计算就懵。

3. 核心函数实战拆解：find_rally_point的筛选逻辑与边界条件

3.1 find_rally_point函数签名与调用前提

备降点决策层里最核心的函数是find_rally_point。它的作用是：给定飞行器当前位置和计划飞往的目的地，从已存储的备降点列表中找一个“值得去”的返回给调用方。

函数签名大致如下：

bool AP_Rally::find_rally_point(const Location &current_loc, const Location &destination_loc, Location &rally_loc) const;

参数语义很明确。current_loc是当前飞机位置；destination_loc是原本打算飞的终点，在RTL场景下就是Home；rally_loc是出参，如果函数返回true，里面就是被选中的备降点坐标。函数本身是const的，也就是说它只做查询和计算，不会修改任何成员状态。

有一点值得注意：这个函数里的“目的地”不一定非得是Home。有些场景下，飞控在任务中止后会先把目标设置为某个安全点，再去调用备降点查询，这时代码里传进去的destination_loc就不是Home。所以读源码时不能只盯着RTL，要理解函数的通用语义。

在实际ArduPilot版本里，这个函数的具体实现可能有多个重载或辅助函数，比如有些版本会拆出一个get_rally_point_with_alt_max来处理高度限制。阅读时先定位主入口，再往旁边看辅助函数，效率会高很多。

3.2 角度过滤与距离过滤：源码中的选址策略

find_rally_point内部并不是简单地把备降点按距离排个序，而是先做一轮“顺路程度”筛选。它的策略我理解起来像是打车场景里的临时换终点：你现在正从A点往B点开，司机不会为了换一个“直线距离近但方向完全相反”的终点而绕个大圈，只有基本在行进方向附近的点才值得考虑。

源码逻辑用伪代码还原出来大概是这样的：

// 简化示意，具体实现以实际固件源码为准 for (uint8_t i = 0; i < _rally_count; i++) { Location rp_loc; if (!calc_abs_location(i, rp_loc)) { continue; } // 如果设置了距离限制，先做粗筛 float dist_cur = current_loc.get_distance(rp_loc); if (_rally_limit_km > 0 && dist_cur > _rally_limit_km * 1000.0f) { continue; } // 角度条件：备降点相对目的地，与当前点相对目的地方向的夹角 float bearing_dest_to_cur = destination_loc.get_bearing(current_loc); float bearing_dest_to_rp = destination_loc.get_bearing(rp_loc); float angle_diff = fabsf(wrap_180_cd(...)); // 统一到 -180~180 度 if (angle_diff > 45.0f) { continue; } // 备降点不能比当前位置离目的地远太多，否则绕路 float dist_dest_rp = destination_loc.get_distance(rp_loc); float dist_dest_cur = destination_loc.get_distance(current_loc); if (dist_dest_rp > dist_dest_cur * 1.5f) { continue; } // 剩下的候选中选离当前最近的一个 if (dist_cur < nearest_dist) { nearest_dist = dist_cur; nearest_loc = rp_loc; } }

这个筛选策略里，角度条件是最容易被忽略的。如果你只在Mission Planner里把备降点放在了任务航线旁边，但没有考虑它相对Home的方向，飞控很可能在RTL时判断这个点“不顺路”而直接忽略它。我调试时遇到过备降点就在当前飞机旁边几百米、但飞控就是不选的情况，一查角度差，发现备降点相对Home的方位和飞机相对Home的方位夹角接近直角，直接被过滤了。

距离上限的粗筛也容易踩坑。RALLY_LIMIT_KM默认可能是0，也就是不限距离，但如果某些固件版本把这个值设成默认0.3之类的小值，你设置的备降点在1公里外，飞控根本不会纳入考虑。排查备降点不生效时，这个参数一定要先确认。

3.3 返回值语义与主调用端的降级处理

返回值true表示找到了一个可用备降点，false表示没有或者数据异常。调用端拿到false后，必须要有降级路径，也就是继续使用传入的destination_loc。这个降级逻辑在源码里随处可见，比如：

Location target = home; if (g2.rtl_alt_type == RTL_ALT_TYPE_RALLY && rally().find_rally_point(current_loc, home, rally_loc)) { target = rally_loc; }

这段逻辑的含义是：只有同时满足“RTL高度模式允许使用备降点高度”和“确实能找到备降点”两个条件，目标点才会被替换。只要有一个不满足，就继续飞Home。这种双重条件的设计很稳妥，它把“能不能用”和“想不想用”分开了。

所以排查问题时，如果备降点位置设置没问题但飞控还是往Home飞，不要先怪find_rally_point，先看看调用端那个“允许使用备降点”的开关是不是没打开。这个开关在不同固件里并不统一，多旋翼看RTL_ALT_TYPE，固定翼和地面载具的调用条件又有差异，阅读源码时建议全局搜一下find_rally_point的调用处，把每一处都过一遍。

4. 从源码到航线：RTL模式下备降点如何被真正调用

4.1 rtl_alt_type参数解读：返航高度与备降点高度的关系

备降点决策做完后，另一个核心问题是高度。飞机不能直接平着往备降点飞，得先到达某个安全高度，再转弯、下降、降落。ArduPilot里，这个“返航高度策略”由参数RTL_ALT_TYPE控制，常见取值只有0和1。0代表使用RTL_ALT参数设置的高度，1代表优先使用备降点带的高度。

如果设成1，飞控在找到备降点后，会读取备降点的alt字段，把它解析成绝对高度或相对Home高度后，作为这次返航的目标高度。多旋翼的逻辑相对简单，爬升到目标高度后水平飞向备降点，到达后垂直降落。固定翼那边更复杂，会结合break_alt和land_dir做进近降落，这两个字段在设置备降点时必须一起考虑。

这里要特别提一下“相对高度”的概念。备降点存储的alt是相对值，不是海拔。飞控解析时会加上Home或起飞点的高度参考，不同固件对“相对”的基准理解不完全一样。所以在高海拔地区或者地形起伏大的场地，同一个备降点高度设置，在多旋翼和固定翼上表现可能不一样。这是源码里最容易产生“看起来该用但实际没按预期高度飞行”的地方。

4.2 低电压、失联、围栏触发时，备降点的生效条件

备降点不只是手动切RTL时才生效。低电压失控保护触发RTL、遥控信号丢失触发失控保护、地理围栏触发强制返航时，都有机会走到备降点逻辑。但它们的生效细节有所区别。

低电压触发时，飞控通常已经进入一种“省电模式”，不希望飞机绕太远，所以备降点筛选时角度和距离的约束非常关键。如果一个备降点位置很理想但需要绕一大圈才能到，飞控宁可回Home，因为绕行可能更耗电。

遥控失联触发时，飞机完全是机载决策，不存在“地面站干预”的可能。这反而是备降点最能发挥价值的场景，因为备降点不需要任何链路参与，全部决策都在机载端完成。需要注意的是，失控保护动作类型里必须配有RTL，如果FS_ACTION之类的参数被设置成了别的动作，备降点逻辑再完善也没机会走到。

围栏触发的情况稍微复杂。部分固件版本在围栏触发后会先执行一个“回到围栏内”的逻辑，等回到安全区域再执行RTL。这个过程也可能调用备降点，但调用时的目的地可能不再是Home，而是围栏内的某个安全参考点。所以看代码时，要分清楚是哪一层在调用备降点，不能一概而论说“RTL时一定会选备降点”。

4.3 地面站实操：用Mission Planner设置一组可用的备降点

源码分析归分析，最终还是要落地到飞行计划里。Mission Planner里设置备降点的流程不复杂，但有几个细节会影响飞控实际使用。

打开Mission Planner的Flight Plan页面，在菜单里切到Rally Points。地图上右键，就能添加一个新的备降点。添加时要注意对话框里的高度和可选参数，高度务必按你期望的“相对高度”来填，不要填成海拔。多个备降点添加完后，点Upload to AutoPilot把数据写入飞控。

写完后，确认参数表里的RALLY_TOTAL已经等于你设置的点数：

• 如果RALLY_TOTAL还是0，说明上传失败，检查一下地面站版本和飞控通信。
• 如果数量对不上，检查参数块是否溢出。
• 如果只是临时测试，建议先只保留2个备降点，减少变量，方便后期用日志判断走的是哪个点。

固定翼玩家还应该检查每个点的land_dir有没有设置对。有些地面站版本默认不弹这个字段，导致保存下来的降落方向是0度，进近时飞机可能会在错误方向上空盘旋一圈再转回来，飞起来很怪但又不至于出大问题。这是很典型的“参数没填全”的坑。

5. 调试备降点时的常见问题与源码级排查思路

5.1 备降点完全没被选中的排查步骤

最常被问到的问题是：“我在地面站设置了备降点，但RTL时飞控完全不理它，还是直接飞Home了。”遇到这种情况，建议从几个层面一层层查。

先看RALLY_TOTAL有没有写入成功，没有写入成功就谈不上后面的事。然后看RALLY_LIMIT_KM，如果这个参数不是0，确认备降点与当前飞机位置的距离在限制范围内。接着看RTL_ALT_TYPE，多旋翼如果不设成1，即使备降点被找到了，目标点替换也不会发生。

再接下去就要看find_rally_point内部的角度过滤条件了。有时候备降点和目的地之间的角度差会刚好卡在阈值附近，固件版本之间阈值可能差几度，导致有的版本能用、有的版本用不了。这种问题不看地面站数据，得看实际坐标算一遍。

最后可以开一个模拟器或者在地面站里观察飞机的RTL路径。如果飞机起飞后目标方向明显偏向某个备降点，但又很快拉回Home方向，通常就是高度或距离条件没满足导致的二次切换。这个过程能从航迹上看得出来。

5.2 备降点位置合理但飞得特别远的处理

还有一种情况：备降点确实被选中了，但飞行路径看起来不合理，比如飞机先飞过一个备降点附近，却选了更远的那个。这其实不是bug，而是源码里多目标选择的结果。

find_rally_point并不是单纯选“离当前位置最近的备降点”，它会先做一轮角度和距离粗筛，然后在通过筛选的点里选最近的一个。假设A点就在飞机旁边，但A点和目的地方向差40度，B点虽然远一些，但方向差只有5度，那么A可能会因为角度条件被淘汰，B反而入选。这个行为在源码里有明确依据。

如果你希望飞机优先选近处的点，可以考虑：

• 减少备降点数量，避免候选太多导致“看起来绕路”的选择。
• 把“顺路方向”的备降点设置成与航线方向更贴近的位置。
• 适当调大RALLY_LIMIT_KM，但别指望它能改变角度过滤逻辑。

这种“飞得远”不一定代表代码出错，更多时候是地面站规划端对源码策略理解不透。弄清楚筛选顺序，再设置备降点，效果会完全不一样。

5.3 源码级验证技巧：在日志和串口终端里观察RALLY状态

最后说说怎么用源码和日志定位备降点问题。ArduPilot的日志系统里，备降点相关数据不一定每个版本都有专门的RALLY日志块，但参数变化一般会出现在参数记录里。如果地面站支持查看参数历史，可以直接对比飞行前后的RALLY_TOTAL，确认备降点是否还在。

更直接的办法是用串口终端连飞控，输入rally命令查看当前备降点列表。不同固件命令集有差异，有些支持rally dump，有些支持rally list，输错了也会有提示。这个方式适合快速确认飞控内部实际存的内容和地面站显示是否一致。

如果你想深入验证某个筛选条件到底卡在哪一步，最靠谱的办法是本地编译一个调试版固件，在find_rally_point内部临时加日志，输出每个备降点的距离、角度、是否被跳过。这个做法看起来笨，但排查效率非常高。我自己调试时就干过这事，加几条gcs().send_text，重新编译刷固件，飞一趟，串口或者数传回传的消息直接把判定结果列得清清楚楚。

要注意的是，改动源码后务必先做一次静止状态下的模拟测试，确认日志输出正常再上真机。别拿一台正常的飞行器去冒这个险，调代码的过程比想象中更容易引入意外行为。

写在最后的一些实操心得

备降点这个功能我翻过几次源码，每次看都有新发现。说几个实际使用中的体会，供你参考。

第一，备降点不是越多越好。我之前一度喜欢每个机场周围都铺四五个点，后来发现候选点太多只会让筛选逻辑更复杂，得不偿失。正常情况下设3个左右就够，一个放在起降跑道正前方顺风位，一个放在跑道侧方安全区，一个放在航线中段可能出状况的位置。多飞几次之后，你自然会知道哪几个点价值最大。

第二，改参数后一定要重新验证备份。备降点是存在飞控参数区的，升级固件、恢复默认参数、甚至某些重启动作都可能导致这些参数被清空。我吃过一次亏，升级固件后忘了重新上传备降点，飞了个复杂航线，结果返航的时候飞控压根没有备降点可用。现在养成习惯，每次大版本刷完固件，第一件事就是打开地面站确认RALLY_TOTAL还在。

第三，不要只在模拟器里验证备降点。模拟器能证明逻辑跑得通，但真实环境里的风速、侧风和地形遮挡，都会影响备降点是否“真的可用”。找个开阔场地，在低风险条件下故意触发一次RTL，全程看航迹。跑通一次之后，你对这个功能的信任度会完全不一样。

写到最后补一句：ArduPilot的源码读起来可能有些枯燥，但备降点这条链路麻雀虽小、五脏俱全，涉及数据存储、参数系统、航点决策和失控保护，啃明白它，很多相似的航点功能概念也能很快理清。你手里那台飞机，说不定就差一个好好的备降点。