查看: 1394|回复: 0

Ardupilot移植经验分享(2)

[复制链接]

349

主题

388

帖子

1085

积分

金牌飞友

Rank: 6Rank: 6

积分
1085
飞币
695
注册时间
2017-7-18
发表于 2022-11-23 22:50:28 | 显示全部楼层 |阅读模式
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。

明白了这三块之间的相互关系,上述问题就迎刃而解。一图以蔽之:
Ardupilot移植经验分享(2)w2.jpg


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。

    Ardupilot移植经验分享(2)w3.jpg

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

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,这里就不贴代码了,给张类图:
Ardupilot移植经验分享(2)w4.jpg
HAL类定义在HAL.h中,它用于管理其他各种类的实例,从而为业务层提供便捷的访问。
Ardupilot移植经验分享(2)w5.jpg


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中的类以做示意。若全放出来的话,那这图就没法儿看了。
Ardupilot移植经验分享(2)w6.jpg
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()如其名称一样,将被底层循环调用,以主持飞行工作的大局。
Ardupilot移植经验分享(2)w7.jpg

搞清联系

至目前为止,我们了解了程序框架的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对象。

    Ardupilot移植经验分享(2)w8.jpg

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(),直到断电。

一图以蔽之
Ardupilot移植经验分享(2)w9.jpg
也可以用eclipse的查看调用栈功能来展示:
Ardupilot移植经验分享(2)w10.jpg

Ardupilot移植经验分享(2)w11.jpg

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

首先,记住两点最关键的:
    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。
Ardupilot移植经验分享(2)w12.jpg

为一览全貌,笔者折叠了很多代码块。大家发现什么了没?程序4要素都在其中。example是独立于业务代码之外,可单独运行的代码。其与业务代码处在相同的层次上,它们都依赖于共享库和HAL。
Ardupilot移植经验分享(2)w13.jpg

AHRS_Test.cpp是姿态解算的示例。ardupilot工程中有各种示例代码,有AP_HAL驱动的example,也有各种共享库的example,有两个作用:
    演示功能模块的用法。对功能模块进行测试。当我们实现了新平台的HAL接口时,使用example来验证,远比直接跑业务代码要方便有效。

在工程根目录下查找所有示例:
Ardupilot移植经验分享(2)w14.jpg

怎么用这个示例呢,使用如下命令编译固件并上传:
./waf build --target examples/AHRS_Test --upload
ardupilot是单线程的吗?

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

从pixhawk开始,这一切就不同了。pixhawk平台的底层系统是Nuttx实时操作系统,其支持带优先级的多线程功能,而AP_HAL_PX4充分利用了这一特性。请看笔者呕心沥血绘制的高清大图(请在新标签页中打开图片):
Ardupilot移植经验分享(2)w15.jpg
上图展示了各个线程的大体框架,左边线程的优先级大于右边的。除了main_loop和timer_thread,还有许多其他的线程。最多的是总线通信线程,有SPI, I2C, CAN, Uart。也可通过下表进行了解,优先级的数值越大代表优先级越高。
名称优先级定时间隔函数名说明
bus242DeviceBus::bus_thread用于SPI
timer1811msPX4Scheduler::_timer_thread
main1802.5msmain_loop主线程
uavcan1791msPX4Scheduler::_uavcan_thread
bus178DeviceBus::bus_thread用于I2C
uart601msPX4Scheduler::_uart_thread
storage5910msPX4Scheduler::_storage_thread
io581msPX4Scheduler::_io_thread

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

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



Ardupilot移植经验分享(2)w16.jpg

Ardupilot移植经验分享(2)w17.jpg

嵌入式代码质量与开发效率技术沙龙深圳、上海、北京三城巡回开始啦

RT-Thread联合鉴释科技分享双方在嵌入式软件领域多年积累的保证代码质量的一些经验和方法,助力小伙伴提升代码质量,欢迎小伙伴报名参加

议程

13:30——14:15

嵌入式软件持续集成与测试

演讲人:RT-Thread 技术工程师

14:15——15:00

嵌入式代码的典型漏洞分析和识别

演讲人:肖琳杰 鉴释资深技术工程师

15:00——15:45

提高嵌入式软件“调试”效率

演讲人:RT-Thread 技术工程师

15:45——16:30

WASM 安全性提高

演讲人:梁宇宁  鉴释联合创始人兼CEO

Ardupilot移植经验分享(2)w18.jpg

扫码报名

您需要登录后才可以回帖 登录 | 加入联盟

本版积分规则

快速回复 返回顶部 返回列表