做飞控二次开发这几年我经常被问到一个问题失控之后飞机到底往哪飞很多人以为是直接回起飞点打开源码才发现默认行为里还有一条“中间缓冲”也就是APM里的备降点Rally Point机制。这次我把自己从数据结构、调用链、参数配置到一次真实排错的过程完整梳理了一遍希望能给正在读APM源码或者准备二次开发的你一个足够清楚的参照。1. 让代码开口前备降点要解决的真实飞行困境先说场景。你在野外飞一架四旋翼航线是三块果园的航测飞到第二块地中间遥控器突然报“RC Failsafe”信号丢了。这时候飞控的默认逻辑一般是进RTLReturn To Launch但RTL直接回Home有一个问题如果返航补给点设得比较远飞机要拖着当前剩余电量飞很长一段直线回去中途一旦电量进一步下降连“就地降落”都会变成硬着陆。备降点做的事情就是给这架失控飞机一个“中途撤离地点”在返航路径上或者任务区域附近提前划定一个安全降落点飞机失控后的返航顺序是“最近的备降点优先其次才是Home”。从源码层面理解备降点并不是一个普通航点它是一组独立的“安全网数据”。ArduPilot在多个子系统中都引用了备降点比如Copter的RTL失败保护、固定翼的Return模式、以及部分地面站的虚拟摇杆操作。更重要的是它和地理围栏Geofence是强绑定关系一个备降点如果落在围栏之外飞控会直接当它无效。这一点在改代码时特别容易踩后面我会单独展开。那为什么不用直接设一个普通任务航点当备降点因为普通航点是跟随任务执行的没有“身份优先级”。而备降点有专门的API、专门的存储区、专门的查询逻辑飞行器在失控的时候不会去翻完整任务列表它只需要调用一个类似AP_Rally::find_nearest_rally_point()的方法让它返回“离当前位置最近的那个合法备降点”。这个“专门做一件事”的设计就是备降点源码分析的核心起点。另外强调一下这篇分析以ArduPilot Copter 4.x系列为主线固定翼的逻辑略有差异但数据结构是共用的。读代码之前你的源码目录里应该重点关注libraries/AP_Rally/这个文件夹它同时被Copter、Plane、Rover三个飞控固件引用换句话说备降点不是某一个机型独有的花活而是整个ArduPilot平台的通用安全机制。2. 数据底座RallyLocation结构与AP_Rally库的读写体系很多人第一次打开AP_Rally.h会觉得代码量不大但信息密度极高。整个备降点存储模型的核心是一个叫RallyLocation的结构体我摘出了关键字段并做了注释解释。// libraries/AP_Rally/AP_Rally.h 中的核心数据结构简化整理 struct RallyLocation { int16_t lat; // 纬度单位度乘以10^7实际使用时常按1e7换算 int16_t lng; // 经度单位同上 int16_t alt; // 备降点高度单位厘米注意是相对Home高度不是绝对海拔 int16_t break_alt; // 到达备降点后切换为“着陆模式”的触发高度 uint8_t flags; // 标志位例如是否为双方向可用点等 uint8_t type; // 点类型影响后续任务生成方式 };这里有两处特别容易误读的地方。第一lat和lng在结构体里是int16_t看上去像经纬度的度数实际它背后还有一个编码转换过程地面站先把真实的经纬度乘以1e7之后可能不能直接塞进16位所以APM在处理时会先做一个截断偏移真正传参进RallyLocation的是经过地面站和MAVLink协议协商后的编码值。也就是说你在地面站界面上输入的35.1234567这个纬度要经过MAV_CMD_DO_SET_RALLY_POINT命令和对应的RALLY_POINT消息转换后才会落到这个结构体里。第二alt的单位是“厘米”而且相对Home高度。这一点和我们平时看航点高度不一样航点高度在Mission协议里通常用MAV_FRAME坐标系来区分AMSL还是AGL但备降点本体存的一直是相对Home气压高度。这就带来一个很典型的坑如果你的Home点设在海拔150米的山坡上备降点设置成Home上方的50米那么飞控在计算备降高度时使用的是气压计差值而不是GPS海拔差值。高海拔、冷热变化剧烈的环境下气压计零点偏移会直接影响备降点高度计算的精度。围绕这个结构体AP_Rally类内部维护的是一张备降点列表。最常用的几个操作是load()、append()、save()和find_nearest_rally_point()。其中load()和save()决定了数据是怎么持久化的。// AP_Rally.cpp 中存储流程的大致样子 bool AP_Rally::load() { // 从参数系统读取 RALLY_TOTAL、RALLY_LAT、RALLY_LON、RALLY_ALT 等参数 // 逐条重建 RallyLocation 列表 // 参数表里没有写入的位置对应的点会被标记为无效 } bool AP_Rally::append(const RallyLocation new_loc) { // 先做合法性检查包括与已有点的最小间距、是否在围栏内等 // 通过检查后写入参数表更新 RALLY_TOTAL }这里有一个反直觉的设计备降点数据不是存在独立的文件里而是直接写入飞控的参数系统也就是那些以RALLY_开头参数。这意味着你修改备降点之后它和飞控的PID参数、混控参数一样会被写进EEPROM或参数Flash区随着固件升级、参数重置而一起变化。很多人在排查问题的时候忘了这层关系下意识以为“Rally点是地面站端的配置”实际上飞控重启后的合法性判断完全依赖于参数系统里那串RALLY_LAT和RALLY_LON。这也解释了为什么你在地面站上传备降点后需要主动写一次参数或者等待自动保存否则断电就丢。MAVLink协议侧备降点走的是RALLY_POINT消息和RALLY_FETCH_POINT请求。地面站下发备降点通常不是一条消息搞定而是类似“告诉飞控我要设置第几个备降点了然后发送RALLY_POINT载荷飞控回RALLY_POINT_ACK”这样的握手流程。读源码时如果在GCS_MAVLink相关的handle_rally_point()函数里看到一长串状态机判空逻辑不要觉得啰嗦这些分支都是在保证无线链路丢包时备降点列表不会出现半个写入的脏数据。3. 失控主链路里备降点怎么用上RTL失败保护的关键调用链理解了存储接着看备降点是怎么在失控场景里被拉出来的。这里必须把RTL失败保护的完整调用链理一遍因为备降点并不是“一失控就直接飞过去”而是藏在RTL模式的目标点决策逻辑里。我从Copter源码的RTL模式入口开始追。首先是遥控器信号丢失的判定入口通常是由接收机信号丢失触发failsafe_radio事件随后飞控会判断失联后采取的动作类型是降落、回航还是保持。假设这里配置的是“RTL”那么模式切换会走到ModeRTL::run()。// ArduCopter/mode_rtl.cpp 中的 RTL 模式主控逻辑简化 bool ModeRTL::init() { // 构建返航路径可能先爬升到安全高度也可能直接进入备降点决策 // 真正的目标点选择发生在 get_rtl_target() 内 }真正藏着备降点逻辑的是目标点生成函数。RTL的目标点优先级不是拍脑袋定的源码一般会按下面这个顺序做判断如果用户通过地面站指定了“返航后飞向备降点”并且当前有合法备降点则返回最近合法备降点坐标。如果没有可用备降点则回退到“起飞点/Home点”。如果起飞点数据也异常最终会强制进入“就地降落”兜底。在这个顺序里AP_Rally::find_nearest_rally_point()就是一个典型的“分诊台”。它的输入是当前经纬度输出是一个被判定为可用的RallyLocation。我在源码里看到的实现逻辑大概是这样// AP_Rally::find_nearest_rally_point() 的运作逻辑 bool AP_Rally::find_nearest_rally_point(const Location current_loc, RallyLocation ret) const { float min_dis FLT_MAX; bool found false; for (uint8_t i 0; i _rally_count; i) { // 有效性检查不是所有被占用的序号都代表真实备降点 // 对每个点进行距离计算保留最小距离点 } return found; }这里的关键不是遍历本身而是每一个循环里做的合法性检查。常见检查包括点是否在地理围栏范围以内、点与当前距离是否超出限定距离、点的高度取值是否在合理范围内、点位是否被参数表标记为禁用。正是这些检查导致很多时候你明明设置了备降点最后飞机却“无视”它飞向Home。如果你在改这段逻辑优先检查的不是遍历代码而是那堆竞态条件。接着往下走选定备降点之后下一步不是“直接横飞到点”而是生成一条“进入备降点航线”。Copter通常的做法是先退出当前任务让飞机爬升到RTL的安全高度RTL_ALT然后水平飞向备降点上空到达break_alt附近后切入LAND或自动降落逻辑。这段分化逻辑在源码里表现为get_rtl_target()返回值通常不是简单经纬度而是一个完整的飞行段描述分为“返回段”和“降落段”。如果备降点离得近返回段会被缩短甚至跳过飞机直接进入下降如果备降点离得远飞控会先把当前高度抬起来再巡航过去。这里头有一个很实际的微调点RTL_ALT和备降点自带的alt/break_alt是两套高度体系前者是整条返航航线的统一巡航高度后者是只在备降点上空生效的“开始降低高度”的阈值。设计航拍任务时我通常会把RTL_ALT设得比备降点alt高至少10米避免飞机在横穿途中就触发了下降判定。坐标系方面RTL目标点从RallyLocation到真正的Vector3f需要经过一次“home相对高度到当前位置相对高度”的转换。因为备降点存的是相对Home高度而飞控在失控时很多参考值已经迁移到了当前起飞点的局部坐标系两者之间存在一个home偏移量。读代码时看到一堆加减_home的操作千万不要跳过去这正是很多“备降点高度差了几十米”问题的根源。4. 地面站配置备降点的完整流程与源码侧的“看不见的筛选”很多人对备降点的印象停留在“计划页面上点个红点”但配置流程里每一步都对应着源码里的一条路径。以开源地面站Mission Planner为例完整的操作流程是这样连接飞控读取参数确认飞控版本和机型固件一致。进入飞行计划页面在地图上鼠标右键选择“设置备降点”或类似选项。输入备降点的经纬度与高度。高度输入框填的是相对Home高度单位是米飞控内部会存成厘米。保存并上传。实际上传动作会触发一段“写参数确认回读”的握手。打开参数列表确认RALLY_TOTAL是否大于0并抽查RALLY_LAT/RALLY_LON对应的数值是否为非零。这一步最容易漏也最能提前暴露问题。在源码侧这一段配置流程对应的是GCS_MAVLink的RALLY_POINT处理函数以及AP_Rally的append()链条。我特意在调试时打开了带DEBUG输出的固件版本发现地面站上传备降点后飞控会对每个点做一轮过滤而不是“你发什么我存什么”。这一轮过滤里最容易让人蒙在鼓里的几个判断整理成下面这张表检查项源码里的触发位置失败表现备注备降点数量是否超过上限RALLY_TOTAL参数校验后续点被静默丢弃上限一般在10个以内是否超出与Home的最大距离限制find_nearest_rally_point()或写入前检查点能写进参数表但查询时被忽略距离判断在写入和查询两处都执行是否落在地理围栏外AP_Fence交叉检查点无效RTL时跳过只在设置围栏的版本生效高度是否超出合理范围alt上下限检查点被标记异常高海拔地区易触发与其他备降点间距过近append()里的距离过滤后写入的点被拒绝最小间距视源码版本而定这些过滤逻辑的存在是为了防止“任何一点数据抖动就把飞机导到地图之外”。但它的副作用也很明显地面站界面上看到“备降点已上传”并不代表这个点被飞控接受了。我在实际项目里遇到过一个很典型的例子某次把备降点设在接近地理围栏边界的果园机耕道上围栏设的是一块六边形区域点刚好落在边界外侧十几米。上传时地面站没有报任何异常因为地面站本身对围栏和备降点的关系不做严格校验等到真失控时飞机宁可飞回3公里外的Home也不理会那个近在咫尺的备降点。后来我拿调试版固件的日志一比对才发现问题出在围栏交叉检查上。这种“看不见的筛选”不止发生在写入阶段查询阶段也会再做一遍。也就是说你在源码里改了find_nearest_rally_point()的过滤条件但写入阶段的append()可能还有一套旧标准两套标准不一致的时候就会出现“明明查询代码已经放行了可备降点根本写不进来”的怪象。我自己改代码时一般会把这两个阶段的检查逻辑抽到一个共用函数里否则后续维护真的是自己给自己挖坑。5. 一次“备降点没生效”的真实排查过程与改源码前的思考先交代背景。某次给一台植保机调试失控返航现场配置了3个备降点分别设置在三个地块的田埂边上。地面站显示备降点全部上传成功参数列表里RALLY_TOTAL也显示为3。可测了几次近距离丢控模拟飞机每次都不往最近的备降点飞而是直接掉头往Home冲。我把这次排查链路完整记下来因为它基本覆盖了所有常见的原因面。第一步先确认失控后的模式确实是RTL。有些飞控固件默认失控动作是“降落”而不是“返航”这种情况下备降点本来就不会介入。我看的是FS_THR_ENABLE这类参数的实际取值而不是地面站界面上的“失控行为”显示框。这个差异很隐蔽有的地面站把参数读出来展示成文字但实际生效值还是以参数系统的数字为准两者存在版本错位。确定模式没问题后第二步检查备降点查询函数有没有被调用。这一步必须看日志或调试输出。当时我把日志功能开着用一个简易遥控中断测试触发了RTL日志里显示进入ModeRTL::run()后直接走了Home分支find_nearest_rally_point()返回的是失败。这就把问题从“备降点存储”进一步推到了“备降点查询合法性”上。第三步针对查询合法性逐项比对。距离限制三个备降点离Home都只有一两百米没超。高度范围田埂和Home高度差不到5米没超。类型标志地面站写入的flags是正常的标准值。最后查到了地理围栏交叉检查发现其中两个备降点的经纬度在六边形围栏边界外侧另一个点则因为围栏设的半径太小和Home的距离判定合在一起被过滤掉了。也就是说3个备降点全军覆没和RALLY_TOTAL显示3完全无关。这个教训告诉我读源码做二次开发时永远要把“存储成功”和“可用”分开看。RALLY_TOTAL只是告诉你有几个点写进了参数区它不代表这些点都通过了全部安全检查。备降点这种安全功能为了保守起见默认过滤条件往往偏严宁可多用一次Home点也不冒险把一个不可靠的点交给失控急救逻辑。第四步也是最有参考价值的一步。我尝试在SITL仿真里复现把同样的经纬度和围栏配置加载进去然后切断遥控信号。仿真环境里完整复现了“备降点被围栏过滤掉”的现象这让我确认问题不在硬件传感器异常而就是纯逻辑判断。从这以后我做任何备降点改动都会先把场景在SITL里跑一遍再上真机。关于扩展保底也要聊几句。APM备降点的选点逻辑目前是“最近距离优先”也就是find_nearest_rally_point()里那个距离比较。但在实际植保、电力巡检任务里我认为“距离最近”并不是唯一合理的策略。一个典型的扩展场景是备降点A离飞机1.2公里但方向逆风备降点B离飞机1.6公里但顺风且地面平整只看距离的话飞控会选A可降落成功率反而是B更高。ArduPilot没有内置风向和地面平整度评估但它的架构允许你在find_nearest_rally_point()里加一个自定义打分函数输入当前风向、剩余电量、地形起伏数据输出一个加权分数然后择优选择。我自己的做法是在AP_Rally类外面包了一层任务级“备降策略模块”先拿到备降点候选List再叠加自己的评分逻辑而不是直接改ArduPilot原生选点代码这样后续升级固件时冲突最小。源码级改动的另一个提醒是备降点相关的结构体和函数分布在AP_Rally、AP_Fence、GCS_MAVLink和飞控RTL模式四个区域牵一发动全身。改一个RALLY_LAT的解析逻辑可能同时影响地面站写入、参数导出、机载日志解码。所以我强烈建议在动手扩展前先画一张“调用时序表”标清楚数据从地面站到结构体、从结构体到目标点、再从目标点到任务生成的每一步。这个过程本身比直接改代码更重要它能让你在改之前就知道哪些判断是有意的“保护性保守”哪些只是实现时的临时约束。最后分享一个小习惯。我在源码目录里维护了一个README_rally_mods.md每次改动AP_Rally相关代码都会记录三件事这个点为什么选、过滤条件有没有同步到查询和写入两处、SITL回放验证过了哪个具体场景。几个月后再看这些笔记能省下大把回忆源码逻辑的时间。如果你也准备动备降点这块建议先从git log翻一翻Rally模块最近的提交记录看看社区最近在修什么边界条件再决定自己的改动方向会在很大程度上限制踩坑的范围。