从 ArduPilot 学习模式管理机制并移植和改进

zxgcz888

背景介绍：
Sugar 的开源麦轮车项目在遥控功能上已经比较完善了，下一步要往“自动化”、“智能化”方向发展。
为了保持开源项目的生命力，发展的每一步都是要踏实认真的。Sugar 一个人求不得速度，但可以用知识、经验保证项目的质量。
本篇就来解决转向的第一个问题：多方号令下车该听谁的？

模式管理

模式管理是一个自动控制上的经典问题。经典的原因是：如果让人说两个“模式”的例子，那么几乎十成十的人都能想到“手动模式”和“自动模式”。下面 Sugar 用表格来说一下开源麦轮车项目中的三个模式计划：

名称	字母代号	描述
手动模式	MAN	车服从遥控器
自动模式	AUTO	车服从车载传感器
智能模式	ROS	车服从车载电脑

因为不同的模式对同一辆车的控制方式有差异，所以有必要引入模式管理机制来让车能明确：应该服从哪一种模式的控制。如下图：



这是一个很明显的“多分支”结构，对应到 C 语言上大概是这个样子：



为了视觉上的感受好一些，上图在 case 中省略了“模式运行”相关函数的调用，只留了必要的架构主干。下面会一点一点把省略的部分加上来。
管理机制设计

Sugar 的麦轮车主要使用和借鉴了两个知名开源项目：RT-Thread 和 ArduPilot。不论是 RT-Thread RTOS 实时操作系统还是 ArduPilot 开源飞控，初学者都不容易在海量的代码中找准入手点。Sugar 认为难找准入手点的重要原因之一是：摸不清需求。

在软件功能设计上，需求是一切的源头。切入点的突破口就在于：清晰简明的表述需求。以本文的模式管理需求为例，Sugar 在深入研究过 ArduPilot 多旋翼飞控的模式管理机制后做出改进设计，如下：



Sugar 把每个模式都划分为“初始化”、“运行”和“退出”三个阶段。模式的切换就是：从上一个模式成功退出到下一个模式初始化完成的过渡过程。

按照 ArduPilot 的模式管理机制，把“模式切换”和“模式运行”分为两段不同的代码，这样做的好处是：能够把模式管理代码完整地分离出来，在主循环中留一个统一的模式调用接口 update_mode() 就行了。下面 Sugar 用图示来表述一下两部分各自的主要流程：



不难看出模式管理的重点是 car_mode 指针，在 ArduPilot 的多旋翼代码中这个指针是 flightmode，如下：



模式管理的代码实现

完整代码在 github 上，公众号回复 car 看 car_407ve_rtt 项目。

1、按上面主循环的流程，把原来遥控的代码换成“模式管理器”的 update_mode() 函数，如下：



2、将遥控代码放到“手动模式”里，如下：

这里 Sugar 依 ArduPilot 的文件组织形式，把各个模式的代码放到各自单独的文件里，这样不但清晰简明，而且方便管理。

3、模式运行、模式切换的主要代码如下：



与“管理机制设计”相关的主要代码就这么多，没有展现全部代码的原因是：要突出切入点。了解 ArduPilot 的读者可以看出：Sugar 在麦轮车的模式管理代码上保持与 ArduPilot 的函数名一致。这样方便想深入了解 ArduPilot 又找不到切入点的初学者参照对比。
模式管理代码设计思想



到 github 上获取 Sugar 麦轮车的完整源码会发现：与 ArduPilot 一样，Sugar 把模式管理的代码也放在了应用层代码目录下，命名为mode.h和mode.cpp。

模式管理采用 C++ 的父、子类形式设计：父类统一接口、子类张显个性。目前 Sugar 只是做了“计划性”的代码，“计划性”的意思是：只有在各个模式功能达到一定的完善程度后，才有必要去考虑细致的切换问题，目前对于 init() 和 exit() 两个虚函数在父类中有个简单实现，子类并没有继承。最初的计划里有，未来需要的时候才能找到地方加。

run() 是纯虚函数，原因是：各个模式运行方式不一样，父类无法也没有必要给出实现。

对于“模式管理”而言，在源码里禁止了复制构造函数和赋值运算符重载，如下：
Mode(const Mode &other)      = delete;Mode &operator=(const Mode&) = delete;
这样做的原因是：从实际出发，模式管理器只能有一个。一个模式管理器已经能够并且足够对不同的模式进行管理，让应用层代码得到各个模式统一的运行接口。试想：如果有多个模式管理器，那么谁来管理各个管理器呢？这个操作看似小，但却体现了“适可而止”的哲学思想。代码本身的意义是：满足需求、解决问题，并且保证稳定。这里禁止了复制构造函数和赋值运算符重载，就把模式管理锁定在了一个确定的范围里，有利于集中全力做好模式管理，加强软件的健壮性。

Number 和 ModeReason 两个枚举使用了 enum class 限定作用域的枚举，这是 C++11 标准里新出现的。限定作用域有利于让编程者更细致、准确地把控代码，从而减少出错的可能。
PS

Sugar 在“遥控车”阶段花了很长的时间（一年多一点），做个遥控车干嘛这么久，不应该是分分钟的事儿么？

花这么长时间磨代码并不是以“做遥控车”为目标的。Sugar 努力做好两件事：
1、在做遥控车的过程中能沉淀足够多的软件能力；
2、用好的架构把这个开源项目的生命力做得更强。

在目前的“遥控车”代码能够解答如下问题：
01、怎样用 CubeMX 开发 STM32（核心是 C/C++ 混编能力）；
02、怎样用 RTOS 开发 STM32（核心是 RT-Thread 操作系统的应用）；
03、如何通过架构统一 CubeMX 和 RT-Thread 软件代码；
04、如何从 ArduPilot 中剥离自己合用的代码；
05、如何通过架构统一 CubeMX、RT-Thread 和 ArduPilot 软件代码；
06、怎样进行控制数据的实时采集记录、回看、分析；
07、面向对象程序设计下的“前后台架构”是什么、怎么用；
08、什么是 FIFO、Ring buffer，并且 Sugar 结合两者长处写了 Ring FIFO；
09、为什么 ArduPilot 的 Log 记录能有 400Hz 那么快；
10、什么是 mavlink 通信协议；
11、怎样在 MATLAB 上用 mavlink；
12、怎样理解 PID 算法及其设计和应用；
13、怎样理解低通滤波算法及其设计和应用；
14、怎样用 IMU 进行姿态解算（属于麦轮车的“番外篇”）；
15、怎样在 IMU 姿态解算上应用互补滤波（属于麦轮车的“番外篇”）；
16、怎样在 IMU 姿态解算上应用线性 Kalman 滤波（属于麦轮车的“番外篇”）；
17、怎样使用 Mecanum 轮的运动分解方法（含正向、逆向）；
18、怎样理解和应用旋转矩阵的；
19、怎样移植和使用 ArduPilot 的 Log 记录程序；
20、怎样用 C++ 实现矩阵运算；
21、怎样理解和应用 C++ 模板类；
… 等等 …

Sugar 推荐自己动手做，边动手边学习。想要节省时间的可以让 Sugar 给做好邮给你。未来 Sugar 会计划一些树莓派、jetson 相关推文，会涉及到 Ubuntu 系统，Sugar 有做好的 Linux2Go 随身系统（小巧方便），可以在“关于我”页面查看。
关注作者

欢迎扫码关注我的公众号MultiMCU EDU。

提示：在公众号“关于我”页面可加作者微信好友。

喜欢本文求点赞，有打赏我会更有动力。