1. 为什么我要自己写一个飞行模式而不是用现成的搞过ArduPilot的人都知道它自带的飞行模式已经覆盖了绝大多数场景AUTO跑航线、LOITER原地悬停、RTL自动返航、GUIDED地面站引导。日常航测、植保、简单的巡检任务用这些模式拼一拼基本都能凑合。但真正落到工业巡检这种活儿上你会发现现成模式总差那么一口气。我接的第一个巡检项目是给一片光伏电站做组件热斑检测。需求听起来不复杂无人机沿着光伏板阵列飞每块板子上面悬停两秒让红外相机拍一张拍完继续飞下一块。问题在于光伏板是按排布的每一排的间距、每一块板的朝向都不完全一致而且现场有检修通道、逆变器房这些障碍物。用AUTO模式跑预先规划好的航线风一吹位置就偏悬停点对不准板子用GUIDED模式手动一块块点飞手累得眼睛发酸效率还低。这时候就冒出一个念头能不能让飞控自己知道“我现在该悬停拍照了”“拍完了该去下一个点了”把巡检的业务逻辑直接写进飞控里答案是能而且ArduPilot的架构对这件事相当友好——它允许你添加自定义飞行模式Custom Flight Mode。你写一个类继承它提供的基类实现几个关键回调函数编译烧录进去地面站里就能像选AUTO一样选你的模式。这篇东西就是把我从零添加一个“自动巡检航线”飞行模式的完整过程拆开讲。包括ArduPilot飞行模式的代码结构长什么样、一个自定义模式最少要实现哪些函数、巡检逻辑怎么塞进去、编译时踩了哪些坑、上机测试怎么一步步验证。适合有一定C基础、玩过ArduPilot源码、想把自己的业务逻辑下沉到飞控里的朋友。如果你只是想让飞机飞个矩形航线那用AUTO就够了不用往下看但如果你面对的是“飞行过程中要根据传感器数据动态决策”这类需求自定义模式几乎是唯一优雅的解法。先给个结论定调ArduPilot添加自定义飞行模式核心工作量不在“写逻辑”而在“理解它的状态机和调度机制”。逻辑本身可能就一两百行但如果你不清楚update()什么时候被调用、init()里该做什么不该做什么、模式切换时哪些状态需要自己清理那写出来的东西轻则行为诡异重则直接炸机。下面我按实际动手的顺序一层层扒开讲。2. ArduPilot飞行模式的代码骨架到底长什么样2.1 从Mode基类说起每个模式都是一个状态机ArduPilot的飞行模式在代码层面就是一个类所有模式都继承自Mode这个基类。你打开ArduCopter/mode.h能看到ModeAuto、ModeLoiter、ModeRTL这些类的声明它们无一例外都是class ModeXxx : public Mode。这个基类定义了一套接口飞控的主循环会在合适的时机调用它们。最关键的几个虚函数是这几个init(bool ignore_checks)模式刚被切换进来时调用一次用来做初始化。update()模式激活期间每个主循环周期都会被调用你的核心逻辑写在这里。run()也是周期调用但和update()有微妙区别后面细说。exit()模式被切走时调用用来清理状态。理解这套机制你可以把它想象成一个公司。Mode是岗位说明书规定了“上岗时要做什么init”“在岗时每天做什么update”“离岗时要交接什么exit”。你写自定义模式就是写一份新的岗位说明书然后把它注册到公司的岗位列表里飞控这个“老板”就会在切换模式时按说明书调用对应的人。这里有个新手最容易懵的点update()和run()到底啥区别我当初也绕了半天。简单说run()是飞控主循环里对所有模式统一调用的入口它内部会先做一些通用检查比如是否触发失控保护然后再调用update()。所以你写业务逻辑绝大多数情况重写update()就够了run()一般不用动。但如果你需要在这个模式里拦截某些通用行为才考虑重写run()。我这次巡检模式只重写了init()、update()和exit()够用了。2.2 模式编号别随便挑避开已占用的坑ArduPilot用数字来标识每个飞行模式比如AUTO是3LOITER是5RTL是6。你添加自定义模式必须给它分配一个没被占用的编号。这个编号定义在ArduCopter/mode.h里的枚举Number中。我踩过的第一个坑就在这儿。当时随手挑了个数字编译过了地面站里也能选但一切换过去飞机行为完全不对像是跑到了别的模式。查了半天才发现我选的编号和某个内部保留模式冲突了。所以动手前第一件事去mode.h里把enum Number从头到尾看一遍找一个明确没被使用的值。通常社区建议自定义模式从比较大的数字开始比如40以上避开官方预留区间。分配好编号后还要在mode.h里声明你的类在mode.cpp里实现并且最关键的一步——在mode.cpp的mode_from_number()函数里注册你的模式让飞控知道“编号X对应我的类”。漏了这一步地面站里根本看不到你的模式。2.3 参数系统让巡检逻辑可配置而不是写死一个成熟的飞行模式不应该把参数写死在代码里。巡检的悬停时间、拍照触发距离、航线点间距这些都应该做成可调参数。ArduPilot有一套成熟的参数系统通过AP_Param宏来定义。我在自定义模式里加了这么几个参数INSP_HOVER_T悬停时间单位秒、INSP_SPEED巡航速度、INSP_TRIG_D触发拍照的距离阈值。定义方式是在类里用AP_Int16、AP_Float这类类型声明成员变量然后在var_info[]表里登记。这样编译烧录后地面站的参数列表里就能看到它们现场调试时不用重新编译直接改参数就行。这一步的价值在实战中体现得淋漓尽致。第一次外场测试我发现悬停两秒拍出来的红外图有点糊现场把INSP_HOVER_T从2改成3重启飞控就生效了。如果写死在代码里我得回酒店改代码、重新编译、再跑一趟现场一天就没了。3. 把巡检业务逻辑翻译成飞控能懂的语言3.1 巡检任务的本质一串带动作的航点回到业务本身。自动巡检航线说白了就是“飞到A点悬停拍照飞到B点悬停拍照……”。这跟AUTO模式跑航点很像但区别在于AUTO模式的航点动作是预先写进任务里的而我的巡检点可能是在飞行过程中根据传感器实时生成的或者需要根据现场情况动态调整顺序。所以我的设计思路是自定义模式内部维护一个航点队列每个航点除了经纬高还带一个“动作类型”字段比如悬停拍照、纯路过、减速通过。update()每次被调用时检查当前是否到达当前目标点到了就执行对应动作动作完成就切到下一个点。这里有个关键决策航点数据从哪来两种方案。一是通过MAVLink从地面站实时发送过来飞控收到后存进队列二是预先存在飞控的存储里模式启动时读取。我选的是第一种因为巡检任务经常需要现场调整地面站上点几下就能重新下发航点比改存储方便得多。实现上就是监听MAVLink的MISSION_ITEM消息收到就入队。这块涉及MAVLink消息处理稍微复杂点但ArduPilot已经有现成的任务协议处理框架我主要是复用。3.2 位置控制别自己算PID用现成的库确定了航点队列接下来是控制飞机飞过去。新手容易犯的错是自己写一套位置到姿态的转换算PID输出。千万别这么干。ArduPilot内部有非常成熟的位置控制器AC_PosControl和姿态控制器你只需要告诉它“我要去这个经纬高”它自己会算油门和舵量。在update()里我调用的是pos_control-input_pos_xyz()或者更高层的wp_nav接口。具体用哪个取决于你的控制精度需求。对于巡检这种点到点飞行用wp_nav的航点导航接口最省事它内部处理了到达判定、减速曲线这些细节。我实测下来飞机飞向巡检点的轨迹相当平滑比自己瞎调参数稳多了。但这里有个细节要注意wp_nav默认的到达判定半径可能不适合巡检。光伏板就那么大到达半径设大了飞机还没对准板子就判定“到了”拍出来的图偏半边。所以我在参数里加了INSP_ACC_R到达半径在update()里覆盖默认值。这个半径设多少取决于你的相机视场角和飞行高度。我一般按“相机视场覆盖范围的一半再打个八折”来估留点余量。3.3 悬停与拍照触发时机比精度更重要到达巡检点后飞机要悬停并触发相机。悬停本身不难把目标位置设成当前点速度设为零飞机自然就定住了。难的是拍照时机的把握。我一开始的做法是到达判定触发后立即发拍照指令。结果发现照片经常是糊的因为飞机虽然位置到了但姿态还在晃云台还没稳。后来改成到达后先等一个“稳定延时”让飞机姿态收敛再触发拍照然后再等一个“曝光延时”确保相机完成曝光才飞向下一个点。这两个延时都做成了参数。触发相机的方式我用的是MAVLink的COMMAND_LONG消息发MAV_CMD_DO_DIGICAM_CONTROL或者相机厂商自定义的命令。如果你的相机支持PWM触发也可以直接控制一个AUX输出口拉高拉低这种方式延迟更低但灵活性差一些。我选MAVLink是因为相机是带云台的集成吊舱走协议控制更省事。提示悬停拍照期间一定要在代码里把“正在执行动作”这个状态标志置位并且在这个状态下忽略新的航点切换请求。我有一次没做这个保护地面站误发了一个新航点飞机在拍照瞬间就开始转向照片全废。4. 编译、烧录与地面站配置的完整链路4.1 源码目录结构与编译环境搭建ArduPilot的源码结构对新手不算友好第一次看容易迷路。跟自定义飞行模式相关的文件主要集中在ArduCopter/目录下mode.h放类的声明和模式编号枚举mode.cpp放实现和注册Parameters.cpp和Parameters.h放参数定义。你基本只需要动这几个文件。编译环境我推荐用官方支持的Linux环境或者Windows下的WSL。我一开始图省事在Windows原生环境编译各种依赖问题折腾了一下午换到WSL后半小时就跑通了。编译命令用./waf configure --board Pixhawk6X板子型号按你的实际硬件填然后./waf copter。第一次编译会比较久十几分钟很正常后面增量编译就快了。编译过程中最常见的报错是“未定义的引用”八成是你声明了函数但没实现或者注册的时候名字写错了。还有一个坑是参数表var_info[]的格式多一个逗号少一个逗号都会报错而且报错信息不一定指向真正的问题行得耐心看。4.2 烧录后地面站里为什么看不到我的模式编译通过、烧录成功兴冲冲打开地面站结果模式列表里没有我的自定义模式——这是几乎每个人都会遇到的一步。原因通常有三个按概率从高到低排第一模式编号没注册。回去检查mode_from_number()里有没有你的编号和类的映射。第二编号和已有模式冲突飞控内部把它当成别的模式了。第三地面站的模式列表是缓存的需要重新读取飞控参数或者重启地面站。我遇到的是第二种。当时选的编号和某个条件编译下的模式撞了在特定固件配置下才暴露。解决办法就是换一个明显安全的编号并且养成习惯注册完后在代码里加一句日志输出模式初始化时打印自己的编号上电看日志确认。4.3 参数配置与首次上电检查清单模式能选到之后别急着起飞。先在地面做一轮静态检查切换到这个模式看飞控日志里init()有没有被调用打印的初始化信息对不对。检查参数列表里你定义的参数是否出现默认值是否合理。用地面站模拟发送几个航点看飞控是否正常接收并存入队列可以通过日志或者自定义的MAVLink遥测消息观察。解锁电机但不装桨推油门看电机响应是否符合预期特别关注模式切换时电机是否会突然加速。这一套走下来能排除掉八成的地面问题。我见过有人跳过这步直接外场结果模式一进去飞机就朝一个方向猛冲差点撞墙。静态检查花二十分钟能省掉一次炸机和一下午的排查。5. 外场实测那些文档里不会写的坑5.1 第一次飞行飞机为什么不听使唤第一次外场我找了个空旷场地规划了三个巡检点信心满满地切模式。结果飞机升空后朝第一个点飞了一半突然开始画圈。我当时心一紧赶紧切回LOITER手动降落。下来查日志发现是到达判定逻辑出了问题。我用的到达判定是“水平距离小于阈值”但没考虑高度。飞机在爬升过程中水平距离可能已经很小了但高度还差很多我的代码就误判“到达”开始执行悬停拍照而位置控制器还在努力爬升两个逻辑打架飞机就画圈了。修复方法很简单到达判定必须同时满足水平距离和垂直距离都小于阈值。这个坑在文档里不会写因为文档默认你会想到。但人在写代码的时候注意力都在水平航线上很容易漏掉高度维度。5.2 风对悬停精度的影响与补偿思路巡检场景经常在户外风是绕不开的。我的悬停拍照逻辑在无风时很稳但三四级风一吹飞机在悬停点附近能飘出半米。照片虽然不至于废但热斑定位的精度就下降了。补偿思路有两个层面。软件层面我在悬停阶段把位置控制器的死区调小让它对位置偏差更敏感同时适当提高位置环的响应速度。但这会带来一个副作用飞机动作变“急”姿态晃动可能更大反而影响拍照。所以硬件层面我给相机加了云台增稳让相机自己抵消飞机的晃动。两者结合实测在四级风下照片可用率从六成提到了九成以上。这里有个经验不要试图用软件把悬停精度做到极致那会牺牲飞行平稳性。工业巡检里云台增稳的性价比远高于死磕飞控参数。5.3 模式切换时的状态清理一个容易被忽略的致命细节exit()函数看起来不起眼但它是保命的。当飞机从你的自定义模式切到别的模式比如失控触发RTL如果你的模式里有一些自定义状态没清理可能会干扰新模式的运行。我遇到过一次巡检模式里我修改了位置控制器的某些限幅参数切到RTL时没恢复导致返航速度异常慢差点没电回来。从那以后我在init()里做的任何全局性修改都在exit()里一一还原。原则就是进来时改了什么出去时还回去不留痕迹。还有一个细节如果你的模式在update()里启动了某些定时器或者挂起了某些回调exit()里一定要取消。否则切走后这些回调还在跑轻则日志刷屏重则和当前模式的控制指令冲突。6. 从能飞到好用巡检模式的进阶优化6.1 断点续巡任务中断后如何接着干实际巡检中电池不够飞完全部点位是常态。飞一半回来换电池换完再飞如果每次都从头开始效率太低。所以我在模式里加了断点续巡功能记录当前执行到第几个航点换电池后重新进入模式从上次中断的点继续。实现上我把当前航点索引存进一个参数或者写进存储模式init()时读取这个值把队列指针跳到对应位置。听起来简单但有个坑换电池后飞机的位置变了如果直接飞向中断点可能会横穿整个场地。所以我在续巡逻辑里加了一个判断如果飞机当前位置离中断点太远先飞到一个安全的中转高度再飞向中断点。6.2 异常处理相机没触发怎么办巡检最怕的是飞完了发现照片没拍上。所以我在模式里加了拍照确认机制触发拍照后等待相机反馈通过MAVLink的相机状态消息或者检测存储卡写入信号。如果超时没收到反馈就重试一次重试还失败就在日志里标记这个点并继续飞下一个点而不是卡死在这里。这个机制救过我一次。有个点的相机连接线接触不良触发没反应模式自动重试后仍然失败就跳过继续了。回来我根据日志里标记的点位单独补拍了那一块没有影响整体进度。如果没有这个机制飞机可能会在那个点一直悬停到电池耗尽。6.3 日志与遥测让每次飞行都有据可查自定义模式的调试日志是命根子。我在update()的关键节点都加了Log_Write记录当前航点索引、到达判定状态、拍照触发时刻这些信息。飞行后把日志导出来用Mission Planner或者PlotJuggler一看整个巡检过程一目了然。遥测方面我通过MAVLink自定义消息把巡检进度已完成点数/总点数实时发给地面站飞手能随时知道飞到哪了。这个功能在长航线巡检时特别有用飞手不用一直盯着飞机看地面站进度条就行。注意日志写入不要太频繁每个主循环都写会拖慢飞控。我的做法是状态变化时才写比如航点切换、拍照触发这些事件点而不是周期性写。7. 我踩过的那些编译与运行时的坑7.1 编译报错排查从“未定义引用”到参数表格式前面提过“未定义引用”这里展开说。ArduPilot的编译系统对符号很敏感你声明了一个成员函数但忘了实现链接阶段就会报这个错。报错信息会告诉你哪个符号没找到顺着去找对应的声明补上实现就行。参数表var_info[]的格式错误更隐蔽。这个表是一个结构体数组每个元素描述一个参数。少一个花括号、多一个逗号编译器可能不报错但运行时参数系统会行为异常比如参数不显示、默认值错乱。我的经验是新增参数时直接复制一个现有参数的完整条目然后改名字和默认值不要手敲格式。7.2 运行时崩溃空指针与数组越界自定义模式里最容易出的运行时问题是空指针和数组越界。比如你从队列里取航点队列空了还去取就崩了。或者航点索引超出了数组长度。我的做法是在所有可能出问题的地方加保护取航点前先判断队列是否为空索引访问前先判断是否越界。这些检查在正常流程下永远不会触发但一旦触发就是炸机级别的后果所以宁可多写几行。还有一个隐蔽的坑init()里访问了某些还没初始化的对象。飞控启动时各个模块的初始化顺序是有讲究的。如果你的模式init()里用到了某个传感器数据但那个传感器还没初始化完读到的就是垃圾值。解决办法是在update()里做这些依赖外部数据的初始化而不是在init()里。7.3 模式切换卡死调度器与阻塞操作ArduPilot的主循环是实时调度器驱动的你的update()函数必须快速返回不能有阻塞操作。我一开始在update()里写了一个while循环等待相机反馈结果整个飞控卡死所有模式都无响应。正确做法是把等待逻辑拆成状态机这次update()检查一下反馈到了没没到就记下状态直接返回下次update()再检查。用状态变量记录“正在等待拍照反馈”而不是用循环阻塞。这个思路的转变是写飞控代码和写普通应用程序最大的区别——飞控里没有“等待”只有“下次再来看”。8. 关于自定义飞行模式我的一些个人体会写自定义飞行模式这件事技术门槛其实没有想象中那么高。ArduPilot的架构设计得相当清晰你只要理解了init/update/exit这套生命周期剩下的就是把自己的业务逻辑翻译成状态机。真正花时间的是那些文档里不写、只有踩过才知道的细节到达判定要算高度、exit()要清理状态、update()不能阻塞、参数表格式要小心。我现在的习惯是每加一个新功能先在地面用日志验证逻辑再装桨低空悬停验证控制最后才外场跑完整任务。这个流程看起来慢但比直接外场试错快得多也安全得多。另外自定义模式不是万能的。如果你的需求用AUTO模式加几个DO命令就能实现那就别写代码。代码意味着维护成本每次ArduPilot升级你都得跟着改。只有当业务逻辑复杂到“飞行过程中需要根据实时数据做决策”时自定义模式才是值得的投入。最后分享一个我常用的调试技巧在自定义模式里加一个“模拟模式”不实际控制飞机只跑逻辑把决策结果通过日志输出。这样你可以在不飞的情况下用地面站模拟发送航点观察你的模式会怎么决策。这个技巧帮我在地面就发现了大部分逻辑错误省下了大量外场时间。