关于Ardupilot移植的经验分享

Ardupilot移植经验分享（1）简要介绍了移植Ardupilot的思路，重点讲述了下载编译源码的要点和搭建源码阅读环境的方法。下载编译源码，一方面是为了搭建源码阅读环境，另一方面是当阅读源码遇到疑问时，可以稍作修改后进行调试验证。搭建便捷高效的源码阅读环境，更是非常重要。若是选择的源码阅读器不能进行函数跳转，不能查看函数调用栈，不能快速导航到目标函数或变量，阅读70万行ardupilot代码的工作将寸步难行。这就是笔者花费很大的篇幅来讲解的原因，没看过第一篇的同学先点击上面的链接噢。

下面开始阅读源码之旅。刚才提到，ardupilot工程有700k行代码，这个统计出版官方开发者wiki。

虽然庞大的体量令人望而生畏，但若明确了目标，把握了脉络，阅读也非难事。下面让笔者按着当初的思路，分享阅读的步骤和要点，带领大家体验一次奇妙的阅读之旅。

下面的内容分为三个部分：

明确目标

把握脉络

深入细节

明确目标

我们的最终目标是通过底层适配的方法，移植ardupilot到自己的硬件平台。不清楚底层适配的同学，请看Ardupilot移植经验分享（1）的“移植Ardupilot的方法有两种”部分。而阅读源码的目标，也就非常简单，即要搞清楚如何进行适配：

要适配哪些接口（或者说函数）？

实现这些接口时，可参考哪些代码？

会不会有难以实现的接口？

明确了这几个问题，可以帮我们缩小阅读的范围。我们并不需要真去阅读那700k行代码，而是围绕移植，这个中心目标，有目的有范围地进行阅读。

前篇文章介绍ardupilot时，提到代码分为5个部分，笔者带领大家回忆下最重要的3个：

载具代码：业务层的代码，比如飞行器初始化，飞行模式控制。

共享库：libraries中除以AP_HAL开头的所有子目录。包含传感器驱动，姿态位置估算与控制。

硬件抽象层（HAL）：libraries中以AP_HAL开头子目录，如AP_HAL， AP_HAL_PX4， AP_HAL_Linux。

明白了这三块之间的相互关系，上述问题就迎刃而解。一图以蔽之：

UML图示说明

本文多处使用UML图来展示ardupilot的框架和流程，有些遵循标准语法，有些则是笔者自己习惯。无论如何，为方便大家理解，笔者会在第一次出现该种图示时做必要说明。

上述是UML组件图，非标准语法。

实线箭头表示使用，比如Vehicle specific flight code使用Shared Libraries。

实线表示关联。AP_HAL_PX4实现了AP_HAL接口，准确说是继承的关系。不过笔者在UML类图中才见过继承的语法，所以这里仅使用实线。

我们要实现的是AP_HAL接口，是上图中的红色圆圈。现有的实现有AP_HAL_PX4（对应Pixhawk平台），AP_HAL_Linux（对应NAVIO）。我们要添加一个平台，为图中蓝色部分，即AP_HAL_TI。

阅读的重点也就确定了：

搞清楚AP_HAL的每一个接口，位于libraries/AP_HAL。

参考AP_HAL_PX4的实现，位于libraires/AP_HAL_PX4。

不过若想完成移植大业，仅阅读这两块内容是不够的。

AP_HAL接口被飞控业务代码以及共享库调用，有些接口确切的功能必须结合具体的使用场景才能搞清楚。并且，想让飞行器飞起来，还是要跑业务代码。在这过程中遇到异常时，若完全不了解业务代码，那将很难定位问题。

AP_HAL_PX4依赖于底层平台（PX4Firmware）所提供的功能。当你想参考AP_HAL_PX4是如何实现某个接口的时候，你可能会发现它只是简单地把任务交给了PX4Firmware。当然，这只是少数情况。参考大部分的实现时，看AP_HAL_PX4本身的代码就足够了。

因此，除了重点阅读AP_HAL和AP_HAL_PX4外，我们需要适当的向上和向下延伸。尤其是向上，掌握飞控业务代码的整体框架，在需要深入某个细节时，至少要知道去哪儿看。

把握脉络

在深入了解HAL的每一个接口之前，我们需要先了解程序的整体框架。

main函数在哪儿？

了解整体框架的第一步，莫过于找到程序入口。

主函数的定义位于ArduCopter/ArduCopter.cpp的结尾处。

AP_HAL_MAIN_CALLBACKS（&copter）;

AP_HAL_MAIN_CALLBACKS用于定义主函数。

#define AP_HAL_MAIN_CALLBACKS（CALLBACKS） extern “C” {

int AP_MAIN（int argc， char* const argv［］）;

int AP_MAIN（int argc， char* const argv［］） {

hal.run（argc， argv， CALLBACKS）;

return 0;

}

}

主函数的名称不一定是大家熟悉的main，对于pixhawk平台来说，其是Ardupilot_main。

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

#define AP_MAIN __EXPORT ArduPilot_main

#endif

经过预编译后，其最终的形态如下，这就是ardupilot的主函数。

extern “C”

{

int __attribute__ （（visibility （“default”））） ArduPilot_main（int argc， char* const argv［］）;

int __attribute__ （（visibility （“default”））） ArduPilot_main（int argc， char* const argv［］）

{

hal.run（argc， argv， &copter）;

return 0;

}

}

也许你会疑惑，为什么要如此大费周章地定义主函数呢？

首先，主函数的名称依平台而定，这肯定是要用宏的。

其次，ardupilot项目不仅仅是ArduCopter，它还有APMrover2，AntennaTracker，ArduPlane和ArduSub，即ardupilot工程有多个入口函数，在编译时根据目标类型来选择。使用AP_HAL_MAIN_CALLBACKS来定义，可避免重复代码，并且后续修改时只需要修改AP_HAL_MAIN_CALLBACKS宏即可。

放一起看下效果：

ArduCopter/ArduCopter.cpp

AP_HAL_MAIN_CALLBACKS（&copter）;

ArduSub/ArduSub.cpp

AP_HAL_MAIN_CALLBACKS（&sub）;

如果你想知道笔者是如何找到这个入口的，请看寻找ardupilot的main函数。

HAL引用

ArduPilot_main中只有一行关键代码：

hal.run（argc， argv， &copter）;

hal是HAL实例的引用，这个引用定义在Copter.cpp中。

const AP_HAL：：HAL& hal = AP_HAL：：get_HAL（）;

get_HAL（）函数为业务层提供获取HAL实例的接口，其在AP_HAL_Namespace.h中声明，由HAL_PX4实现。

libraries/AP_HAL/AP_HAL_Namespace.h

// Must be implemented by the concrete HALs.

const HAL& get_HAL（）;

libraires/AP_HAL_PX4/HAL_PX4_Class.cpp中实现了get_HAL函数，其创建并返回HAL实例。

const AP_HAL：：HAL& AP_HAL：：get_HAL（） {

static const HAL_PX4 hal_px4;

return hal_px4;

}

HAL类是硬件抽象层（HAL）的一个集合类，它包含了对硬件抽象层其他类的实例的引用。HAL类、get_HAL（）以及HAL类引用的其他类，声明在AP_HAL命名空间中。具体可查看AP_HAL_Namespace.h，这里就不贴代码了，给张类图：

HAL类定义在HAL.h中，它用于管理其他各种类的实例，从而为业务层提供便捷的访问。

UML图示说明

上图为UML类图。每一个带C标记的方框表示一个类。方框中可添加成员变量和函数的说明。

成员变量的格式为：name:type

成员函数的格式为：name （parameter-list） ： return-type，每一个形参的格式为：name:type

除了在方框中定义成员变量，还可以使用箭头指向类的方式来表示。所以，上图展示出HAL类中有着各种驱动类的实例。

业务层在获得了hal后，就可以通过它访问具体的驱动接口，比如说：

使用hal.console-》printf（）打印日志

使用AP_HAL：：millis（）和AP_HAL：：micros（） 获取上电启动到现在所经过的时间

使用hal.scheduler-》delay（） 和 hal.scheduler-》delay_microseconds（） 睡眠一定的时间

使用hal.gpio-》pinMode（）， hal.gpio-》read（） 和 hal.gpio-》write（） 访问GPIO引脚

通过hal.i2c_mgr访问I2C设备

通过hal.spi访问SPI设备

AP_HAL_PX4框架

libraries/AP_HAL之中定义的是HAL接口，顺带着介绍下其实现。

PX4的实现位于libraries/AP_HAL_PX4之中，其结构为：

定义了PX4命名空间，基本上所有驱动类位于其中。

每一个AP_HAL空间中的驱动类，都在PX4空间中能找到实现。比如PX4UARTDriver继承自UARTDriver，实现了其定义的全部接口。

HAL_PX4类继承自HAL类。

PX4中的驱动类依赖于PX4Firmware中间件以实现具体的驱动功能，比如PX4_I2C继承自PX4Firmware中的I2C类。

下图描述了AP_HAL空间，PX4空间以及PX4Firmware的关系。只绘制了少部分AP_HAL中的类以做示意。若全放出来的话，那这图就没法儿看了。

HAL_PX4的初始化详见其构造函数，在HAL_PX4_Class.cpp之中

HAL_PX4：：HAL_PX4（） ：

AP_HAL：：HAL（

&uartADriver， /* uartA */

&uartBDriver， /* uartB */

&uartCDriver， /* uartC */

&uartDDriver， /* uartD */

&uartEDriver， /* uartE */

&uartFDriver， /* uartF */

&i2c_mgr_instance，

&spi_mgr_instance，

&analogIn， /* analogin */

&storageDriver， /* storage */

&uartADriver， /* console */

&gpioDriver， /* gpio */

&rcinDriver， /* rcinput */

&rcoutDriver， /* rcoutput */

&schedulerInstance， /* scheduler */

&utilInstance， /* util */

nullptr， /* no onboard optical flow */

nullptr） /* CAN */

{}

setup（）和loop（）

ArduCopter的主要工作，在setup（）和loop（）中执行。setup（）进行初始化，而loop（）如其名称一样，将被底层循环调用，以主持飞行工作的大局。

搞清联系

至目前为止，我们了解了程序框架的4要素，它们是：

程序入口：AP_HAL_MAIN_CALLBACKS（&copter）

随处可用的hal引用：const AP_HAL：：HAL& hal = AP_HAL：：get_HAL（）;

setup（）

loop（）

我们知道setup（）会被底层调用一次，还知道loop（）会被底层循环调用。不过它们是在哪儿被调用的呢？这与AP_HAL_MAIN_CALLBACKS定义出的入口函数有什么关联吗？如果不搞清楚这些问题，笔者觉得并不算是掌握了脉络。好的，现在笔者来深入剖析它们的关系。

setup，loop和底层的关系是这样的：

Callbacks接口定义了两个纯虚函数setup和loop。

ArduCopter.cpp中定义了Copter类，继承Callbacks接口，所以Copter的实例就是Callbacks对象。

ArduCopter.cpp将创建出的Callbacks对象传递给HAL_PX4，以供其调用。就是ArduPilot_main中的那行代码：

hal.run（argc， argv， &copter）;

具体的调用流程是这样的：

AHRS_Test.cpp通过宏AP_HAL_MAIN定义出了Ardupilot_main（）。

上电启动后，硬件平台调用Ardupilot_main（）。

Ardupilot_main（）调用HAL层的入口函数HAL_PX4：：run（），并将自己的Callbacks对象传递给它。

HAL_PX4：：run（）调用了自己的main_loop（）。

main_loop先调用AHRS_Test.cpp的setup（）进行业务初始化。

main_loop循环调用AHRS_Test.cpp的loop（），直到断电。

一图以蔽之

也可以用eclipse的查看调用栈功能来展示：

看完上述内容，是不是感觉有点复杂呢。笔者来总结一下：

首先，记住两点最关键的：

ArduCopter代码定义setup（）和loop（），setup（）进行初始化，而运行时的主要工作是在loop（）之中。

HAL_PX4_Class负责程序调度，即调用setup（）和loop（）。

不过呢，有点绕的是：HAL_PX4_Class并不提供程序入口，程序入口由ArduCopter提供，它们会定义Ardupilot_main（）。由Ardupilot_main（）启动HAL_PX4_Class。

为什么不由HAL_PX4_Class来定义程序入口？再想想？因为程序有很多可选入口嘛。还记得APMrover2，AntennaTracker，ArduPlane和ArduSub吗。

深入浅出

我们了解了程序框架的4要素，并且深入研究了它们之间的调度逻辑，是时候“浅出”一下了。我们来看一个ardupilot里面的example。

为一览全貌，笔者折叠了很多代码块。大家发现什么了没？程序4要素都在其中。example是独立于业务代码之外，可单独运行的代码。其与业务代码处在相同的层次上，它们都依赖于共享库和HAL。

AHRS_Test.cpp是姿态解算的示例。ardupilot工程中有各种示例代码，有AP_HAL驱动的example，也有各种共享库的example，有两个作用：

演示功能模块的用法。

对功能模块进行测试。当我们实现了新平台的HAL接口时，使用example来验证，远比直接跑业务代码要方便有效。

在工程根目录下查找所有示例：

怎么用这个示例呢，使用如下命令编译固件并上传：

。/waf build --target examples/AHRS_Test --upload

ardupilot是单线程的吗？

看了由setup（）和loop（）组成的框架，你可能会认为ardupilot是单线程的。早期的ardupilot跑在ardunio上，setup（）和loop（）就是历史的痕迹。那时的ardupilot确实是loop外加一个定时器回调，算是2个线程。

从pixhawk开始，这一切就不同了。pixhawk平台的底层系统是Nuttx实时操作系统，其支持带优先级的多线程功能，而AP_HAL_PX4充分利用了这一特性。请看笔者呕心沥血绘制的高清大图（请在新标签页中打开图片）：

上图展示了各个线程的大体框架，左边线程的优先级大于右边的。除了main_loop和timer_thread，还有许多其他的线程。最多的是总线通信线程，有SPI， I2C， CAN， Uart。也可通过下表进行了解，优先级的数值越大代表优先级越高。

名称优先级定时间隔函数名说明

bus242

DeviceBus：：bus_thread用于SPI

timer1811msPX4Scheduler：：_timer_thread

main1802.5msmain_loop主线程

uavcan1791msPX4Scheduler：：_uavcan_thread

bus178

DeviceBus：：bus_thread用于I2C

uart601msPX4Scheduler：：_uart_thread

storage5910msPX4Scheduler：：_storage_thread

io581msPX4Scheduler：：_io_thread

main_loop里面的并行实际上是一个伪并行，由ardupilot自己设计的AP_Scheduler系统来进行调度。这里就不展开了，后续有机会的话会单独介绍。

深入细节

不知不觉写了好多，下一篇再讲具体的HAL接口。对了，讲解移植流程，就得等Ardupilot移植经验分享（4）了。

原文标题：Ardupilot移植经验分享（2）

文章出处：【微信公众号：RTThread物联网操作系统】欢迎添加关注！文章转载请注明出处。

责任编辑：haq