APM飞控添加自定义飞行模式

本文为《无人机飞控固件开发教程》系列视频的辅助资料，已经在“网易云课堂”上线 ， 链接：https://study.163.com/course/introduction/1209568864.htm?share=1&shareId=1448054983

无人机调试，飞控硬件定制、固件修改，日志分析，请QQ联系：3500985284

　本节课，通过讲解如何添加一种自定义的飞行模式，来帮助大家深入了解APM固件二次开发的流程。

本章节源程序地址：https://github.com/nufeichuiyun/ardupilot 　对应git分支（branch）：TestStarWP

2021年3月16日更新：我们的课程刚开始录制的时候，APM官网稳定版固件刚发展到Copter-3.6.10，如今最新的稳定版固件已经走到了Copter-4.0.7。由于Copter-3.6.x与Copter-4.0.x在飞行模式方面的架构发生了较大的变化，因此我将本节课和下节课的例程在Copter-4.0.7的基础上重新编写了一下，并添加了中文注释，方便大家学习。建议大家先把本节课视频看一遍，然后再使用“Github Desktop软件”查看新的例程是如何修改的。此例程的源代码地址依然为：https://github.com/nufeichuiyun/ardupilot，但是分支名称为“Copter-4.0.7-StarWP”。同时，我也将切换到此分支并更新好子模块的代码打包上传到百度网盘里了，文件名为：“ardupilot-切换到Copter-4.0.7-StarWP分支-已经更新子模块-解压后可以直接编译.rar”

　　大家解压缩后可以直接编译出本节课的例程固件，不需要再更新子模块（但是如果你在其基础上切换到其他分支，还是需要更新子模块）。网盘链接：

　　https://pan.baidu.com/s/18dtYwIFVABnd5g_yty92iA　　提取码：nfcy

2022年10月12日更新：如果你用的是Copter-4.3.x版本的固件，源代码部分细节可能跟之前的版本又有不同，请结合编译报错信息和搜索引擎，尝试自行解决（都是小的语法问题，搞开源飞控，这些是日常工作）。

一、功能目标

  　待添加的飞行模式的特点：切入此模式后，飞控自动按照“五角星航线”进行自动飞行，因此我们把这个模式命名为“五角星航线模式”。

二、实现步骤

 　 1、在Copter-3.6.10分支基础上新建分支，命名为：TestStarWP；

  　2、研究已有模式的代码，深入了解其运行规律；

　  3、复制一个最接近我们的自定义模式特征的模式的代码，在其基础上修改；

　  4、添加自定义模式特有的代码；

 　 5、编译、仿真、debug。

　仿真用到的命令如下：

// 解锁
// 注意，在AUTO模式下是禁止解锁的，如果无法解锁，请先检查一下飞控是否处于AUTO模式
// 我们可以通过“mode loiter”命令将飞控切到loiter模式，然后解锁起飞，之后再切到其他模式
arm throttle

// 将油门推到一个较高的位置，起飞
rc 3 1800

// 将油门放到中位
rc 3 1500

// 切换到五角星航线模式
mode 24  // 注意，对于新的Copter-4.0.7-StarWP版本例程，此处应为mode 27

　　特别注意，飞控是通过Mavlink协议告知地面站当前的飞行模式的，并且Mavlink该状态帧中只包含了模式的序号，不包含模式的名称，因此在本例程中，由于我们只修改了飞控代码，没有修改地面站代码，地面站在显示新的飞行模式时会错乱，这是正常的，不影响新的飞行模式的功能。同理，在MissionPlanner的全部参数列表中，对应参数（如FLTMODEx）的注释也可能没法与我们新添加的模式编号对应上，这也是正常的。

　　特别注意，本例程需要修改多个源文件，视频中可能讲的不是特别清楚，导致大家编译、运行时有可能会出错，请大家善用github工具，对比第二节课中我提供的源代码压缩包里的程序或者我的github中的程序（二者是同一个东西），看看自己是不是漏掉了哪些修改。

三、效果图

按照下图所示的方法可以调整地图中飞机轨迹线的长度，从而显示出完整的五角星：

四、更进一步

　2020年10月17日更新，有学员提出如下需求：五角星航线运行完成后自动切换到特定的飞行模式，这个需求在很多项目中都是用得到的，我将代码进一步修改如下，经测试运行正常，五角星航线完成后飞控向地面站发送消息“Draw star finished, now go into Loiter Mode.”，然后自动切换到Loiter模式。

　ardupilot/ArduCopter/mode_DrawStar.cpp，62行：

五、实物飞行测试

　在进行实物飞行之前，我们首先要解决如何使用遥控器将飞行模式切换到我们新建的模式的问题，显然，由于我们并没有修改地面站软件MissionPlanner的源代码，因此MissionPlanner是无法正确显示此飞行模式的名称的，并且在下图的模式设置界面中的下拉菜单中也不会有我们新建的模式：

　但是事实上，上图所示的六种模式与全部参数列表中的六个“FLTMODEx”参数一一对应，并且这些参数的值就是我们在程序中添加的飞行模式对应的数字。因此我们只需要将对应的参数设置为自定义模式的数值即可：

　另外，设置完成后，在“初始设置 – 必要硬件 – 飞行模式”中会无法正常显示该模式，这是正常的，不必担心。

　切换到该模式后，在姿态窗口右下角会显示“Unknown”，这同样是正常的。

　上述方法我进行仿真测试时，发现设置“FLTMODE6”的值后，通过“rc 5 1900”这个命令并不能成功触发飞控将模式切换到自定义的模式，但是关闭仿真环境和MissionPlanner后，重新开始仿真时，可以通过该命令切换到自定义飞行模式。大家如果遇到相同的问题不妨也这样试试。当然，对于实物飞行测试，应该不需要重启飞控。

六、疑难解答

1、编译报错“’Copter::<anonymous struct> Copter::failsafe’ is private within this context”

　如果你是在Copter4.0.x版本基础上添加的自定义飞行模式，由于从Copter-4.0.x版本与Copter-3.6.x版本在飞行模式的代码结构上略有差别，因此大概率你可能会少写一行程序，从而导致上述报错，解决方法很简单，在“Copter.h”文件的213行下面，仿照其他模式，将自定义模式的类添加为友元类（friend class）。如果不明白为什么要这么做，请打开百度，搜索“friend class”，打开前几个页面，挨个看一遍，然后你就明白了，并且又学到了新的知识。

2、仿真运行时提示“APM: No such mode”

　在仿真时，当我们通过“mode 27”命令切换到自定义模式时，可能会提示“APM: No such mode”，并且模式并没有切换成功。产生此问题的原因有如下两个：

　1、上次仿真时没有彻底退出后台仿真程序，从而这次仿真时实际上运行的还是修改代码前的仿真程序。如果是这样，可以通过“课时5-建立飞控软件仿真环境”网页资料底部“疑难解答”环节第9条的方法来解决；

　2、由于本节课的程序需要修改多个地方，你遗漏了几行代码，导致程序虽然编译没有报错，但是功能并不全。解决方法： 请大家善用github工具，对比第二节课中我提供的源代码压缩包里的程序或者我的github中的程序（二者是同一个东西），看看自己是不是漏掉了哪些修改。

无人机调试，飞控硬件定制，固件修改，log日志分析，飞控驱动添加
请QQ联系：3500985284