前言
ArduPilot 支持的每个板子都有某种形式的永久性存储可用。这是用来保存用户参数、航点、集结点、地形数据和许多其他有用的东西。为了提供对这种存储的访问,ArduPilot 有 4个 基本机制:
- AP_HAL::Storage对象,通过 hal.storage 访问;
- StorageManager库,在 hal.storage 上提供一个更高层次的抽象层;
- 用于存储到板载记录区的 DataFlash;
- 在支持 Posix IO 功能的板子上,将 Posix IO 功能与传统的文件系统(例如 microSD 卡上的VFAT)结合起来。
其他需要永久性存储的库和函数都建立在这些基本系统之上。例如,AP_Param 库(处理用户可设置的参数)是建立在 StorageManager 之上的,而 StorageManager 又是建立在 AP_HAL::Storage 之上的。AP_Terrain 库(处理地形数据)是建立在 Posix IO 函数之上的,用于保存地形数据库。
1 AP_HAL::Storage库
AP_HAL::Storage 对象在所有平台上都可用。在 ArduPilot 支持的主板上,通过这个接口可用的最小存储空间是 4096 字节。有些主板提供了更多的空间 - 例如 PX4v1 有 8k 的 EEPROM,Pixhawk 有 16k 的 FRAM。所有这些都隐藏在 AP_HAL::Storage API 后面。
hal.storage 的 API 非常简单。它只有3个函数:
- init() 用于启动存储子系统;
- read_block() 读取一个字节块;
- write_block() 写一个字节块。
之所以有这个非常简单的 API,是因为我们鼓励开发者使用 StorageManager API 而不是 hal.storage。你应该只在建立新板或调试时才深入研究 hal.storage。
可用的存储空间大小是在 AP_HAL/AP_HAL_Boards.h 的宏 HAL_STORAGE_SIZE 中设置的。这意味着我们还不支持动态地确定这个接口的可用存储空间的大小。如果你想动态确定存储空间的大小,目前你需要使用 Posix IO。
我们没有 AP_HAL::Storage API 的示例概述,所以这是你写一个的机会。如果你已经深入到 ArduPilot 教程,你应该已经看到了足够多的示例概述,知道如何从头开始写一个。所以写一个 libraries/AP_HAL/examples/Storage 的例子,计算 hal.storage 数据的全部内容的 8位XOR,并将其打印到控制台。
然后把这个例子作为一个补丁(submit the example as a patch)提交到 ArduPilot github,注意遵守补丁提交指南。
我很想知道,在我把这个练习添加到教程中后,多久才会有提交...
2 StorageManager库
简单的 hal.storage API 使 ArduPilot 移植到一个新的板子上变得很容易,但对于实际使用的可用存储空间来说并不方便。为此,我们有 StorageManager。StorageManager 库提供了一个 API,它将存储抽象为具有指定用途的伪连续数据块。因此,我们把可用的存储空间分割成若干区域,以提供:
- parameters:参数;
- fence points:围栏点;
- waypoints:航点;
- rally points:集结点;
- signing key:签署密钥;
- RC bind info:RC绑定信息。
去读一下 libraries/StorageManager/StorageManager.cpp。特别是看一下顶部的表格。注意到每种类型的多个区域是如何为拥有较大存储量的系统定义的。这种将多个不相邻的存储区域合并为一个逻辑存储区域的能力是添加 StorageManager 的主要动机。它使我们能够从使用所有板上的 4k 存储空间发展到使用每个板上的全部存储空间,而不要求用户重新加载所有的参数或重新加载他们的航点。
这种试图避免让用户重新配置他们的自动驾驶仪主板的主题是 ArduPilot 中常见的。我们喜欢用户能够更新到最新的固件,而且他们之前设置的一切都能正常工作,同时获得新的功能。有时,这意味着我们作为开发者必须做更多的工作来使之成为可能—例如发生在 StorageManager 上的情况。
StorageManager API 还提供了方便的函数来读写整数等变量。像 AP_Mission 这样的程序库就是用这个 API 来保存和恢复航点的。
现在去查看 libraries/StorageManager/examples/StorageTest.cpp。这是对 StoageManager 层的压力测试,因此也是对 AP_HAL::Storage 对象的压力测试。它在随机长度的随机偏移处做随机 IO。这意味着它做的 IO 是跨越边界的,在 FRAM 或 EEPROM 中单个参数值可能是不连续的。这个测试概述被设计用来对 StorageManager API 进行压力测试,
但是当把 ArduPilot 移植到一个新的电路板上时,它也非常有用,因为它很好地强调了 EEPROM 的访问功能。
在你的板子上试试 StorageTest,但要注意,这是一个破坏性的测试。它不会破坏你的板子,但会清除你所有的参数和航点。所以,如果你在你最喜欢的飞机上测试电路板,请备份你的配置。
作为一个练习,给 StorageTest 添加一些剖析,使其打印出以 kbytes/sec 为单位的总 IO 率,以及读和写的 IO 率。你是否注意到读和写的速度之间有什么不同?你是否注意到在存储中已经设置在该地址的数值的写入速度?看看你是否能在 StorageManager 中找到解释你观察结果的代码。然后向 ArduPilot github 提交一个补丁(submit a patch),添加剖析输出。
接下来在 ArduPilot 库中搜索所有使用 StorageManager 的地方。你要找的是 StorageAccess 句柄,像这样:
StorageAccess AP_Param::_storage(StorageManager::StorageParam);
它声明了一个名为 AP_Param::_storage 的变量,该变量提供了对该板上存储的 StorageParam 区域的访问。哪些库使用 StorageAccess?
3 DataFlash库
自动驾驶仪需要的另一种存储方式是用于机载日志。对于 ArduPilot 来说,这是由 DataFlash 库提供的。在很多方面,这是一个相当奇怪的库。首先,它的名字很奇怪,来自于它最初是围绕 APM1 的 DataFlash 芯片设计的。
它是一个硬件设备驱动库,随着时间的推移演变成了一个通用的日志系统。在内部,这个库的结构从几个方面显示了这个历史(而且不是好的方面!)。
如今,DataFlash 的 API 是围绕着一个日志基础设施模型设计的。它允许你为个别的日志信息定义自我描述的数据结构—比如一个 "GPS"信息来记录 GPS 传感器的数据。它以一种有效的方式处理将数据记录到永久性存储中,
并且还为其他库提供了 API,当用户在飞行后想要下载他们的日志文件时,可以使用这些 API 将数据取回。
如果你看过 ArduPilot 现在下载日志时使用的'*.bin'文件,
那么你已经看到 ArduPilot 用来存储日志信息的格式。它是"自我描述"的,这意味着地面站可以计算出日志文件中信息的格式,而不需要有一些共同的方案。在每个日志文件的前面是一组 FMT 信息,它有一个众所周知的格式,并且描述了后面信息的格式。
去看看 libraries/DataFlash/examples/DataFlash_test/DataFlash_test.cpp。
你会看到顶部有一个小表格,它定义了我们要写的日志信息,在这个例子中,"TEST"信息包含 4 个无符号 16 位整数和两个有符号 32 位整数(这就是 "HHHii"的意思)。它还给出了这 6 个变量的名称(巧妙地标为 V1 到 V4 和 L1 和 L2)。
在 loop() 函数中,你会看到一个相当奇怪的调用,像这样:
DataFlash.get_log_boundaries(log_num, start, end);
这是 DataFlash 库隐藏电路板实际存储日志文件方式的公共 API。在具有 Posix IO 的系统中(如 Pixhawk 或 Linux),日志文件以单独的文件形式存储在 microSD 卡的 "LOGS"目录下。用户可以通过拔出 microSD 卡并将其放入个人电脑来直接复制这些文件。
在像 APM2 这样的板子上,事情就没那么简单了。APM2 的 DataFlash 芯片上有 4兆字节的存储空间,可以通过 SPI 接口访问。该接口本身是面向页面的,所以你需要填充一个 512 字节(或者可能是 528 字节!)的页面,然后告诉芯片将该页面复制到永久性存储,同时填充下一个页面。在这个 DataFlash 上做随机的 IO 不是很好—它是为那些需要连续写入的代码而设计的,这就是日志记录时发生的情况。与自动驾驶仪喜欢记录的数据量相比,4兆字节的大小真的不是很大,所以我们也需要在它填满时处理包装。
所有这些复杂的东西都隐藏在一个 API 的背后,这个 API 提出了 "日志编号"的概念,
这只是在自动驾驶仪的一次飞行中写入的一堆字节。APM1 和 APM2 上的 DataFlash 实现在每个页面的前面使用小标记字节来表示正在写入的日志编号。这些日志编号与用户要求检索其日志时下载的日志编号相对应。
4 Posix IO
ArduPilot 支持的一些自动驾驶仪主板是基于一个具有类似 Posix API 的操作系统。例如,Linux 端口有一个非常好的 Posix 子系统,用于 PX4 的 NuttX 操作系统(如 Pixhawk 上)有一个相当合理的 Posix 层。你可以在 ArduPilot 的库中利用这一点,只要你不依赖它来做任何必须在所有板上工作的事情。
这方面的一个很好的例子是 AP_Terrain 库,它保存着地形数据。地形数据太大,无法装入 EEPROM,而且它是随机访问的,所以不适合用 DataFlash。对于自动驾驶仪的基本功能来说,它也不是必不可少的,所以它是使用 Posix IO 实现的最佳候选
我们使用 Posix IO 的方法是,首先通过检查 AP_HAL_Boards.h 中的 HAVE_OS_POSIX_IO 宏来检查电路板是否有 Posix IO 支持。
然后为了知道你应该在文件系统中的什么地方存储数据,你在 AP_HAL_Boards.h 中添加一个数据特定的宏,给出应该放置这种数据的目录路径。例如,宏 HAL_BOARD_TERRAIN_DIRECTORY 是用来定义地形数据应该存放的目录。
一旦你有了这两样东西,你应该只使用普通的 Posix IO 函数(即打开、关闭、读、写等),尽管有一些注意事项:
- 你必须只从 IO 定时器或你自己的低优先级线程中调用 IO 函数;
- 不要从库中可直接访问的 API 调用任何 IO 函数。即使是像 stat() 这样简单的函数也不行;
- 尽量对慢速存储卡友好,在合理大小的块中进行 IO,并尽可能避免搜索。
这些规则真的很重要。在 Pixhawk 的 microSD 卡上进行简单的 IO 可能需要一秒钟。这足以让你宝贵的四轴飞行器倒过来飞,冲向地面。Pixhawk microSD 卡上的平均 IO 时间很短(几毫秒),但偶尔你会得到一个缓慢的 IO,当 microSD 卡决定它需要花一些时间重新阅读 SD 卡的规范和计算 Pi。不要因为它在大多数时候看起来很快,就想偷偷地进行一点操作。
这方面的一个例外是初始化功能,你知道只有在飞行器启动或加锁时才能调用。在那个时候有一点延迟是可以的。
现在去读一下 libraries/AP_Terrain/TerrainIO.cpp,看看它是如何使用 Posix IO 的。注意它用来处理所有 IO 的小状态机,这些都是由 AP_Terrain::io_timer 函数调用的。看看你是否能发现任何 bug,如果可以的话,请报告它们。