源代码分析

Pixhawk源码笔记一：APM代码基本结构

Pixhawk源码笔记一：APM代码基本结构

基础知识

详细参考：http://dev.ardupilot.com/wiki/learning-the-ardupilot-codebase/

第一部分：介绍

详细参考：http://dev.ardupilot.com/wiki/learning-ardupilot-introduction/ ArduPilot 代码分为5个主要部分，基本结构分类如下： vehicle directories AP_HAL libraries

tools directories

external support code

1、vehicle directories模型类型 当前共有4种模型：ArduPlane, ArduCopter, APMrover2 and AntennaTracker。都是.pde文件，就是为了兼容arduino平台，以后可能会放弃。

2、AP_HAL硬件抽象层

硬件抽象层，使得在不同硬件平台上的移植变得简单。

其中AP_HAL目录定义了一个通用的接口。其他的目录AP_HAL_XXX针对不同硬件平台进行详细的定义。例如AP_HAL_AVR目录对于AVR平台，AP_HAL_PX4对应PX4平台，AP_HAL_Linux对应Linux平台。

3、tools directories工具目录

主要提供支持。For examples, tools/autotest provides the autotest infrastructure behind the autotest.diydrones.com site and tools/Replay provides our log replay utility. 4、external support code外部支持代码

对于其他平台，需要外部支持代码。例如Pixhawk、PX4的支持代码如下： PX4NuttX – 板载实时系统。the core NuttX RTOS used on PX4 boards

PX4Firmware – PX4固件。the base PX4 middleware and drivers used on PX4 boards uavcan – 飞行器CAN通信协议。the uavcan CANBUS implementation used in ArduPilot mavlink – Mavlink通信协议。the mavlink protocol and code generator

5、系统编译

针对不同的硬件板，编译可以采用“make TARGET”的形式。 make apm1 – the APM1 board make apm2 – the APM2 board make px4-v1 – the PX4v1

make px4-v2 – the Pixhawk

如果要移植到新的硬件，可以在mk/targets.mk文件中添加。 比如： make apm2-octa -j8 或者： make px4-v2 -j8

采用8通道并行编译方式，针对APM、Pixhawk硬件板（AVR、STM32），编译八旋翼代码。

第二部分： 学习sketch例程代码

http://dev.ardupilot.com/wiki/learning-ardupilot-the-example-sketches/ sketch，是指使用 .pde 文件编写的主程序。

开始之前，你可以试着阅读、编译并运行下面的sketches libraries/AP_GPS/examples/GPS_AUTO_test libraries/AP_InertialSensor/examples/INS_generic libraries/AP_Compass/examples/AP_Compass_test libraries/AP_Baro/examples/BARO_generic libraries/AP_AHRS/examples/AHRS_Test

例如，下面的编译方法，将在Pixhawk上安装AP_GPS例程sketch。 cd libraries/AP_GPS/examples/GPS_AUTO_test make px4-clean make px4-v2

make px4-v2-upload

正确理解sketch例程代码，我们以GPS_AUTO_test.pde代码为例（目录ardupilot\\libraries\\AP_GPS\\examples\\GPS_AUTO_test），主要几个特点： 1、 pde文件包含很多 includes； 2、 定义了 hal 引用声明； 3、 代码非常粗糙；

4、 setup() 和 loop()函数 1、include文件

pde文件转变为C++文件后，提供必要的库引用支持。 2、hal引用声明

定义如下：

const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;// pixhawk等价于AP_HAL_PX4

该定义，方便访问硬件接口，比如console终端、定时器、I2C、SPI接口等。 实际的定义是在HAL_PX4_Class.cpp中定义，如下： const HAL_PX4 AP_HAL_PX4; hal是针对 AP_HAL_PX4 的引用。 经常使用的方法如下：

终端字符输出。hal.console->printf() and hal.console->printf_P() to print strings (use the _P to use less memory on AVR)

获取当前运行时间。hal.scheduler->millis() and hal.scheduler->micros() to get the time since boot

延时。hal.scheduler->delay() and hal.scheduler->delay_microseconds() to sleep for a short time

IO输入输出。hal.gpio->pinMode(), hal.gpio->read() and hal.gpio->write() for accessing GPIO pins

I2C操作，hal.i2c SPI操作，hal.spi 3、setup()和loop()

每个sketch都有一个setup()和loop()函数。板子启动时，setup()被调用。这些调用都来自HAL代码中的main()函数调用（HAL_PX4_Class.cpp文件main_loop()）。setup()函数只调用一次，用于初始化所有libraries。 Loop()循环被调用，执行主任务。 4、AP_HAL_MAIN()宏指令

每一个sketch(.pde文件)最底部，都有一个“AP_HAL_MAIN();”指令，它是一个HAL宏，用于定义一个C++ main函数，整个程序的入口。它真正的定义在AP_HAL_PX4_Main.h中。

#define AP_HAL_MAIN() \\

extern \ int SKETCH_MAIN(int argc, char * const argv[]) { \\ hal.init(argc, argv); \\ return OK; \\ }

作为程序的起点，在AP_HAL_MAIN()里，就正式调用了hal.init()初始化代码。 程序的执行过程就是：程序起点AP_HAL_MAIN() à hal.init() à hal.main_loop() à sketch中的setup()和loop()。

Pixhawk源码笔记二：APM线程

Pixhawk源码笔记一：APM代码基本结构，参见： http://blog.sina.com.cn/s/blog_402c071e0102v59r.html

这里，我们对 APM 线程进行讲解。如有问题，可以交流30175224@qq.com。新浪@WalkAnt，转载本博客文章，请注明出处，以便更大范围的交流，谢谢。 第三部分 APM线程

详细参考：http://dev.ardupilot.com/wiki/learning-ardupilot-threading/

对于APM1、APM2硬件板，不支持多线程，所以只能通过简单的定时器加回调函数来实现。类似PX4和Linux硬件板支持Posix标准的多线程。线程一般是指基于多任务操作系统的并行任务，我们首先要明白的几个概念如下： 1、 定时回调

2、 HAL专属线程 3、 驱动专属线程

4、 APM驱动与板级驱动 5、 板级专属线程、任务

6、 AP_Scheduler任务调度系统 7、 信号灯（任务队列互锁用） 8、 lockless data structures

如果你对操作系统运行机制比较了解，那就很好理解了。 1、定时回调The timer callbacks

每个飞控平台都提供一个1kHz的定时器（见AP_HAL），通过“注册”一个定时器函数来获取1kHz定时功能。所有注册的定时器将被顺序调用。调用形式如下：

hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_update));

定时器优先级为181，高于主进程的180。上面代码是以MS5611气压计驱动为例，其中 AP_HAL_MEMBERPROC() 宏，主要作用是将一个C++成员函数包装起来，作为一个回调参数。其定义在AP_HAL_Namespace.h文件中，如下：

// macro to hide the details of AP_HAL::MemberProc

#define AP_HAL_MEMBERPROC(func) fastdelegate::MakeDelegate(this, func)

使用hal.scheduler->millis() and hal.scheduler->micros() 可以记录时间。

好了，你可以试着自己边一个简单的sketch，在setup()和loop()函数中练习一下1秒钟向USB终端输出一个时间或字符。 2、HAL专属线程

以PX4为例，HAL专属线程有：

1、 UART线程，用于读、写串行接口数据（包括USB）； 2、 定时器线程，支持1kHz定时功能；

3、 IO线程，支持写microSD、EEPROM、FRAM等。 对于Pixhawk，请准备一条调试电缆，连接到nsh console（serial 5 端口），波特率57600。如果已经连接，试下”ps”命令，你会得到如下信息： PID PRI SCHD TYPE NP STATE NAME 0 0 FIFO TASK READY Idle Task()

1 192 FIFO KTHREAD WAITSIG hpwork() 2 50 FIFO KTHREAD WAITSIG lpwork() 3 100 FIFO TASK RUNNING init()

37 180 FIFO TASK WAITSEM AHRS_Test() AHRS线程 38 181 FIFO PTHREAD WAITSEM (20005400) 定时器线程

39 60 FIFO PTHREAD READY (20005400) UART线程 40 59 FIFO PTHREAD WAITSEM (20005400) IO线程 10 240 FIFO TASK WAITSEM px4io() 13 100 FIFO TASK WAITSEM fmuservo() 30 240 FIFO TASK WAITSEM uavcan()

上面的线程为定时器线程（优先级181），UART线程(60)， IO线程(59)，以及其他线程诸如：px4io, fmuservo, uavcan, lpwork, hpwork and idle tasks

线程的主要目的是在不干扰主进程的情况下，在后台处理一些低优先级任务。例如 AP_Terrain library，需要向microSD卡写地形文件，它的实现方式如下：

hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&AP_Terrain::io_timer));

注意：IO线程优先级59，相比定时器181优先级慢了很多。 3、Driver专属线程

没什么好说的，请参考英文原版，需要提的一点是，我们可以利用register_io_process() 和register_timer_process()来处理驱动的访问。 4、APM驱动与板级（原生）驱动

我们可以看到MPU6000驱动有两个版本：一个是APM版本，在libraries/AP_InertalSensor/AP_InertialSensor_MPU6000.cpp,另一个为原生代码版本，在PX4Firmware/src/drivers/mpu6000。

注意，对于Pixhawk，APM代码使用的是Pixhawk原生驱动，因为原生驱动已经做得很好了。libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp中可以查看详情。

在非PX4平台上，我们使用AP_InertialSensor_MPU6000.cpp驱动，在PX4平台上，我们就用PX4原生驱动AP_InertialSensor_PX4.cpp 5、板级专属线程、任务 在上面第2节“HAL专属线程”讲到”ps”命令显示的线程。很多都不是 AP_HAL_PX4 Schedule启动的线程，这些线程列举如下：

idle task – called when there is nothing else to run init – used to start up the system

px4io – handle the communication with the PX4IO co-processor

hpwork – PX4稍低优先级驱动线程。handle thread based PX4 drivers (mainly I2C drivers) lpwork –PX4非常低优先级驱动线程。handle thread based low priority work (eg. IO) fmuservo – AUX输出。handle talking to the auxillary PWM outputs on the FMU uavcan – handle the uavcan CANBUS protocol

这些任务的启动，由rc.APM脚本文件(ardupilot\\mk\\PX4\\ROMFS\\init.d\\rc.APM)指定。PX4启动时，会读取该文件。rc.APM属于nsh 类型脚本。作为练习，你可以修改rc.APM脚本文件，增加一些sleep和echo命令，那么当PX4启动时，通过debug console（也就是serial 5）可以显示出来。

更多内容，可以参考英文原版。 原生线程的启动代码如下：

hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);

等同于AP_HAL中的hal.scheduler->register_timer_process()。上述代码的意思是，HRT （high resolution timer）高精度定时器，以1000微妙的周期调用MPU6000::measure_trampoline函数。这些操作是禁止中断的，最多占用数十微妙的时间。 上面的优先级非常高。下面的方法，是稍低优先级。

work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);

用于处理I2C设备。大概花几百微妙的操作时间。是可以被中断的任务。如果是最低优先级，那么参数改为LPWORK，这样的任务一般需要花费更长的时间。 6、AP_Scheduler任务调度系统

用于飞行器主线程，提供了简单的机制控制每个操作花费了多少时间。例如：1、等待一个新IMU采样；2、在每一个IMU采样周期之间调用一系列其他任务。

每一个飞行器都有一个AP_Scheduler::Task table任务列表，参考代码（ardupilot\\libraries\\AP_Scheduler\\ Scheduler_test.pde ）类似如下：

static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { ins_update, 1, 1000 }, { one_hz_print, 50, 1000 }, { five_second_call, 250, 1800 }, };

结构体第1列，循环调用的任务函数。第2列，调用频率（也叫tick，一个tick，就是一个最小时间单元，pixhawk为2.5ms）。第3列为最大可能占用的操作时间，scheduler.run()会传递当前可用的时间（微秒）,如果时间不够，那么这个任务就pass掉了，不执行。 注意，AP_Scheduler::Task table列表必须具备以下条件： 1、 他们不能被阻塞。

2、 在飞行时，他们不能调用sleep function 3、 他们必须有可预估的最坏的运行时间。 你可以修改Scheduler_test.pde，加入自己的代码来读取气压计、罗盘、GPS、更新AHRS输出roll/pitch。 7、信号灯

有3种方法可以避免多线程访问冲突：1、信号灯；2、lockless data；3、PX4 ORB。 例如：I2C驱动可以通过信号灯，确保同一时间，只有一个I2C设备被使用。可以查看ardupilot\\libraries\\AP_Compass\\AP_Compass_HMC5843.cpp了解： 获得信号灯：_i2c_sem->take(1); 释放信号灯：_i2c_sem->give(); 8、Lockless Data Structures

Lockless Data Structures比信号灯要方便，例子见：

the _shared_data structure in libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp the ring buffers used in numerous places. A good example is libraries/DataFlash/DataFlash_File.cpp

Go and have a look at these two examples, and prove to yourself that they are safe for concurrent access. For DataFlash_File look at the use of the _writebuf_head and _writebuf_tail variables. 9、PX4 ORB

ORB(Object Request Broker)是PX4的互斥机制。

另外两种PX4驱动通信机制，列举如下：

ioctl calls (see the examples in AP_HAL_PX4/RCOutput.cpp)

/dev/xxx read/write calls (see _timer_tick in AP_HAL_PX4/RCOutput.cpp) 想要学习Pixhawk源码的朋友有福了，后边我会陆续的将Pixhawk的源码学习笔记整理出来分享给大家。敬请关注：新浪微博@WalkAnt，3017224@qq.com。欢迎交流。

Pixhawk源码笔记三：串行接口UART和Console

这里，我们对 APM UART Console 接口进行讲解。如有问题，可以交流30175224@qq.com。新浪@WalkAnt，转载本博客文章，请注明出处，以便更大范围的交流，谢谢。 第四部分 串行接口UART和Console

详细参考：http://dev.ardupilot.com/wiki/learning-ardupilot-uarts-and-the-console/ UART很重要，用于调试输出，数传、GPS模块等。 1、5个UART

目前共定义了5个UART，他们的用途分别是：

uartA – 串行终端，通常是Micro USB接口，运行MAVLink协议。 uartB – GPS1模块。

uartC – 主数传接口，也就是Pixhawk telem1接口。 uartD – 次数传接口，也就是telem2接口。 uartE – GPS2模块。

有些UART具备双重角色，比如通过修改SERIAL2_PROTOCOL参数，可以将uartD的Mavlink telemetry数传更改为Frsky telemetry数传（中国江苏产数传）。 测试 libraries/AP_HAL/examples/UART_test目录下的 example sketch，分别对5个UART都输出hello 消息。使用USB转串口工具，可以测试。 2、调试终端Debug console

作为5个UART的补充，有些平台额外的还有一个debug console调试终端。你可以通过检查HAL_OS_POSIX_IO宏定义来判断，诸如： #if HAL_OS_POSIX_IO ::printf(\ #endif

如果定义了HAL_OS_POSIX_IO，可以试着查看AP_HAL/AP_HAL_Boards.h代码。 3、UART函数

每个UART都一系列基本操作函数，主要有： 1. printf – formatted print

2. printf_P – formatted print with progmem string (saves memory on AVR boards) 3. println – print and line feed 4. write – write a bunch of bytes 5. read – read some bytes

6. available – check if any bytes are waiting

7. txspace – check how much outgoing buffer space is available

8. get_flow_control – check if the UART has flow control capabilities

可以到AP_HAL中查看他们的定义，并使用UART_TEST进行测试。 4、UART接口说明

众多UART接口，众多名称，他们的对应关系，我总结如下，如有问题，欢迎发邮件至30175224@qq.com 新浪@WalkAnt，欢迎指正。 代码定义 PCB电路表述 飞控板接口 Serial标号 说明 接USB，支持MAVLink协议 接GPS模块，另CAN2接口 接第1数传模块 接第2数传模块 一般接GPS2模块 Debug Console用于程序调试 APM代码中的电路板上的表Pixhawk外壳上串口序号 表述 述 的标识 uartA uartB uartC uartD uartE / Micro USB UART4 UART2 UART3 UART8 UART7 USB GPS Telem1 Telem2 SERIAL4/5 SERIAL4/5 USB Serial 3 Serial 1 Serial 2 Serial 4 Serial 5

前面的文章： Pixhawk源码笔记一：APM代码基本结构：http://blog.sina.com.cn/s/blog_402c071e0102v59r.html

Pixhawk源码笔记二：APM线程： http://blog.sina.com.cn/s/blog_402c071e0102v5br.html

经过对UART测试代码： libraries/AP_HAL/examples/UART_test）进行详细测试： void loop(void) {

// Micro USB口输出： Hello on UART uartA at 764.461 seconds test_uart(hal.uartA, \

// GPS接口输出：Hello on UART uartB at 91.845 seconds test_uart(hal.uartB, \

// 数传Telem1输出：Hello on UART uartC at 24.693 seconds test_uart(hal.uartC, \

// 数传Telem2输出：Hello on UART uartD at 805.557 seconds test_uart(hal.uartD, \

// SEIRAL 4口输出：Hello on UART uartE at 911.812 seconds test_uart(hal.uartE, \

// also do a raw printf() on some platforms, which prints to the // debug console #if HAL_OS_POSIX_IO

// SEIRAL 5口输出: Hello on debug console at 976.857 seconds

::printf(\on debug console at %.3f seconds\\n\hal.scheduler->millis()*0.001f); #endif

hal.scheduler->delay(1000); }

SERIAL 5 作为重要的调试口。Pixhawk启动时会通过该接口输出大量信息，可以用来对启动过程进行监控。

Pixhawk源码笔记四：学习RC Input and Output

这里，我们对 APM RC Input Output接口进行讲解。如有问题，可以交流30175224@qq.com。新浪@WalkAnt，转载本博客文章，请注明出处，以便更大范围的交流，谢谢。 第五部分 学习RC Input and Output

参考：http://dev.ardupilot.com/wiki/learning-ardupilot-rc-input-output/

RC Input，也就是遥控输入，用于控制飞行方向、改变飞行模式、控制摄像头等外围装置。ArduPilot支持集中不同RC input（取决于具体的硬件飞控板）： 1. PPMSum – on PX4, Pixhawk, Linux and APM2 2. SBUS – on PX4, Pixhawk and Linux

3. Spektrum/DSM – on PX4, Pixhawk and Linux 4. PWM – on APM1 and APM2

5. RC Override (MAVLink) – all boards

其中SBUS and Spektrum/DSM是串行协议，SBUS为100kbps反UART协议，Spektrum/DSM为115200bps UART协议。对于PX4，这些协议是通过硬件UARTs实现的，而有些Linux系统是通过软件UARTs实现的。（原文：Some boards implement these using hardware UARTs (such as on PX4) and some implement them as bit-banged software UARTs (on Linux).）

RC Output，是指飞控接受到RC输入后，再将其处理后，输出到伺服和电机（电调）上。RC Output默认50Hz PWM信号。对于ArduCopter多轴飞行器和直升机，输出频率为400Hz。

1、RCInput对象（AP_HAL） RCInput 对象声明： AP_HAL::RCInput* rcin;

相关例程： libraries/AP_HAL/examples/RCInput/RCInput.pde ，试着动动遥控器手柄，看看输出是否符合预期。

2、RCOutput对象（AP_HAL） RCOutput对象声明：

AP_HAL::RCOutput* rcout;

不同的飞控，代码实现有所不同，可能包含了片上定时器、I2C、经由协处理器(PX4IO)输出等程序。

相关例程： libraries/AP_HAL/examples/RCOutput/RCOutput.pde 这段程序从1通道到14通道，控制电机从最小转速到最大转速逐级变化。 3、RC_Channel对象

hal.rcin和hal.rcout对象，为低层次调用。最常用的是使用更高级封装的RC_Channel 对

象来实现RC input 和 output。它允许用户对参数进行配置，例如每个通道min/max/trim值，同时支持辅助AUX通道函数，还可对input output进行比例缩放处理等。

相关例程： libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde例程教你如何setup、read、copy input to output。

4、RC_Channel奇怪的input/output 设置

看代码时，有些地方程序会让你感到奇怪，有一些是由于程序代码的不完善产生的，有一些则不是。

例如，很多变量作用在input和output上：

radio_out = (pwm_out * _reverse) + radio_trim;

上述代码中的radio_trim，是一个trim叠加，用来修正遥控器的值。 又例如，对于固定翼飞行器，roll（横滚）输入，成为了steer（转向 yaw）。对于ArduCopter中的多轴飞行器，在处于Drift模式（漂移模式）时，我们看到，pitch用于前飞，roll用于转向（而不是传统yaw用于转向）。以后，APM团队会将其纠正过来，将这两个概念分开。大家知道这么回事就OK了。 5、RC_Channel_aux 对象

另一个非常重要的类：RC_Channel_aux class，它是RC_Channel的子类。它有很多特点可供用户使用。这个会有一点比较难以理解，举个例子：

用户想要使用通道6(Channel 6)对航拍设备的横向稳定进行控制，那么他可以将FUNCTION设置为21，枚举变量类型为” k_rudder”（偏航，偏转，转向的意思）。如下： AP_GROUPINFO(\ AP_GROUPEND

如果程序中调用此代码，RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, 4500);，那么所有FUNCTION设为21(k_rudder)的通道(channel)都将输出满偏(4500就是满偏最大值)。

在相应的update_aux_servo_function()代码中， case RC_Channel_aux::k_rudder:

_aux_channels[i]->set_angle(4500);// 设置最大角度。 break;

注意这是一对多的设置。就我的理解，其实也就是我们常说的混控输出。比如在辅助通道6中，我们可以将其他通道设置为使用function = 21。那么其他使用了21的通道，将会被通道6混控。（这个很复杂，我也没太明白，对这个有更好理解的，请一定告诉我，相互学习：30175224@qq.com。当然如果日后我能有更好的理解，我会更新本博客。） 下图，RC_Channel共4个通道，RC_Channel_aux共10个通道。 第一组：1、2、3、4、5、6、7、8、10、11（共10通道） 第二组：9、12 第三组：13、14

FUNCTION参数如下：

// FUNCTION 为1-27，function参数。 typedef enum {

k_none = 0, ///< disabled

k_manual = 1, ///< manual, just pass-thru the RC in signal k_flap = 2, ///< flap

k_flap_auto = 3, ///< flap automated k_aileron = 4, ///< aileron

k_unused1 = 5, ///< unused function k_mount_pan = 6, ///< mount yaw (pan) k_mount_tilt = 7, ///< mount pitch (tilt) k_mount_roll = 8, ///< mount roll

k_mount_open = 9, ///< mount open (deploy) / close (retract) k_cam_trigger = 10, ///< camera trigger k_egg_drop = 11, ///< egg drop

k_mount2_pan = 12, ///< mount2 yaw (pan) k_mount2_tilt = 13, ///< mount2 pitch (tilt) k_mount2_roll = 14, ///< mount2 roll

k_mount2_open = 15, ///< mount2 open (deploy) / close (retract) k_dspoiler1 = 16, ///< differential spoiler 1 (left wing) k_dspoiler2 = 17, ///< differential spoiler 2 (right wing) k_aileron_with_input = 18, ///< aileron, with rc input k_elevator = 19, ///< elevator

k_elevator_with_input = 20, ///< elevator, with rc input k_rudder = 21, ///< secondary rudder channel

k_sprayer_pump = 22, ///< crop sprayer pump channel k_sprayer_spinner = 23, ///< crop sprayer spinner channel k_flaperon1 = 24, ///< flaperon, left wing k_flaperon2 = 25, ///< flaperon, right wing

k_steering = 26, ///< ground steering, used to separate from rudder k_parachute_release = 27, ///< parachute release k_epm = 28, ///< epm gripper

k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one)

} Aux_servo_function_t;

AP_Int8 function; ///< see Aux_servo_function_t enum

这里，我们对 APM EEPROM存储接口进行讲解。如有问题，可以交流30175224@qq.com。新浪@WalkAnt，转载本博客文章，请注明出处，以便更大范围的交流，谢谢。 第六部分 存储与EEPROM管理 详细参考：http://dev.ardupilot.com/wiki/learning-ardupilot-storage-and-eeprom-management/

用户参数、航点、集结点、地图数据以及其他有用的信息需要存储。ArduPilot提供4个基本存储接口：

1、AP_HAL::Storage对象：hal.storage；

2、StorageManager库，是hal.storage更高级别的封装； 3、DataFlash用于日志存储；

4、Posix IO函数，是传统文件系统读写函数。

其他用于永久存储信息的函数库，都是基于以上4种实现。例如：AP_Param library（用于处理用户可配置参数）是建立在StorageManager库之上的，而StorageManager库则是基于AP_HAL::Storage之上。AP_Terrain library（用于处理地形数据）则是建立在Posix IO functions之上，用于操作地形数据库。 1、AP_HAL::Storage library

AP_HAL::Storage对象适用于所有ArduPilot硬件平台。最小支持4096字节空间的存储，一些类似PX4v1的板子有8K EEPROM，Pixhawk有16K FRAM。所有这些都封装在AP_HAL::Storage API中。

hal.storage API，非常简单，仅3个函数： 1、init()，初始化存储系统； 2、read_block()，读块数据； 3、write_block()，写块数据。

之所以这么简单，是因为APM团队鼓励开发者使用StorageManager API，而不是hal.storage。只有在代码移植或调试时，使用hal.storage会比较方便（原文：You should only be delving into hal.storage when doing bringup of a new board, or when debugging.）。 存储空间的大小，在AP_HAL/AP_HAL_Boards.h文件中的HAL_STORAGE_SIZE宏中定义，如下：

#define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_PX4_V2 #define HAL_STORAGE_SIZE 16384 // 存储空间 16KB #endif

也就是说，我们不支持动态存储空间的定义。如果希望使用动态存储空间，可以使用Posix IO。

2、StorageManager library

在将ArduPilot代码移植到一个新的硬件板上时，hal.storage API非常简单，但是在操作存储区时就不那么好使了。我们会采用StorageManager。StorageManager library提供对存储区域“伪连续块”（一般用作不同的功能和目的）的访问。正因此我们将存储区域分配了不同的功能：

1、参数区；

2、飞行区域限制点数据区； 3、航点数据区； 4、集结点数据区。

参见： libraries/StorageManager/StorageManager.cpp，我们可以看到存储区域的划分： const StorageManager::StorageArea StorageManager::layout_copter[STORAGE_NUM_AREAS] PROGMEM = {

// ------------------------ 0-4096 分配给了 AVR版本的 APM { StorageParam, 0, 1536}, // 0x600 param bytes { StorageMission, 1536, 2422},

{ StorageRally, 3958, 90}, // 6 rally points { StorageFence, 4048, 48}, // 6 fence points #if STORAGE_NUM_AREAS >= 8

// ------------------------ 4096-8192 分配给了PX4版本 { StorageParam, 4096, 1280}, { StorageRally, 5376, 300}, { StorageFence, 5676, 256},

{ StorageMission, 5932, 2132}, // leave 128 byte gap for // expansion and PX4 sentinal #endif

#if STORAGE_NUM_AREAS >= 12 // Pixhawk // ------------------------ 8192-16384 分配给了 Pixhawk版本 { StorageParam, 8192, 1280}, // 类型 偏移量 长度 { StorageRally, 9472, 300}, { StorageFence, 9772, 256},

{ StorageMission, 10028, 6228}, // leave 128 byte gap for expansion #endif };

对于上面的存储分布，我们可以观察到AVR版本用到存储地址是0-4095，而PX4用到地址是4096-8191，Pixhawk用到的地址是8192-16383。这样的结构，是为了更好的与之前的版本兼容。这样一来，用户在更新最新的固件时，所有之前配置的参数将不会改变，将继续起作用。

StorageManager API也提供对类似整型数的读写访问，AP_Mission中就会利用这个API来存储和恢复航点数据。

相关例程（libraries/StorageManager/examples/StorageTest.pde）对StoageManager layer和AP_HAL::Storage object进行了测试。它对随机的偏移量、随机的长度进行了随机的IO操作。这也就意味可能会出现跨边界访问。这个例程非常有用，它用于对StorageManager API进行严苛测试，同样对于移植ArduPilot到新硬件平台也是极为有用的，因为它对EEPROM的访问函数进行了很严格的测试。

注意StorageTest是一个毁坏性的测试，它将会删除你之前存储的参数和航点。一定要

记得测试之前，备份你的配置。 存储对象的声明，一般如下：

StorageAccess AP_Param::_storage(StorageManager::StorageParam); 又或者

StorageAccess AP_Rally::_storage(StorageManager::StorageRally);

StorageAccess AP_Mission::_storage(StorageManager::StorageMission);

StorageAccess AP_Limit_Geofence::_storage(StorageManager::StorageFence); 3、DataFlash library

另一类存储，就是飞行日志存储，这个基于DataFlash library。这个库的名字看上去有些怪怪的，实际上这个库最开始是为APM1的DataFlash芯片设计的，它原本是一个硬件驱动库，后来慢慢演变为一个通用日志系统，这个可以在代码中找到蛛丝马迹（这些都是以前的痕迹，不是最好的代码实现方式）。

现在DataFlash API主要用于实现日志存储。它允许你自定义日志消息的数据结构。例如GPS消息，用于记录GPS传感器的日志数据。它能够非常有效存储这些数据，它同时也对其他库提供相应的APIs，用来进行日志回传、下载。

LOG数据结构是自定义的，其结构可以查看日志文件的FMT消息。FMT消息地应以的其他数据的存储格式。

相关例程 libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde。这里描述了数据的存储结构和数据格式。简单列举如下：

第一点，在.log文件中，我们可以看到如下格式的表达：

FMT, 128, 89, FMT, BBnNZ, Type,Length,Name,Format,Columns FMT, 129, 23, PARM, Nf, Name,Value FMT, 130, 45, GPS, BIHBcLLeeEefI,Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs,VZ,T FMT, 131, 31, IMU, Iffffff, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ FMT, 132, 67, MSG, Z, Message

第二点，上述格式，对应的代码（参见DataFlash.h）： #define LOG_BASE_STRUCTURES \\

{ LOG_FORMAT_MSG, sizeof(log_Format), \\

\ { LOG_PARAMETER_MSG, sizeof(log_Parameter), \\ \ { LOG_GPS_MSG, sizeof(log_GPS), \\ \

\ { LOG_IMU_MSG, sizeof(log_IMU), \\

\ 上述结构，以 LOG_IMU_MSG为例讲解： 信息类型ID 数据大小 信息名称 数数据1 数据2 数据3 数据4 数据5 数据6 数据7 据类型 LOG_IMU_Msizeof(log_IIMIffffTimeSG MU) U ff MS 131 31(字节) GyrX GyrY GyrZ AccX AccY AccZ IMl:整整型 0.0007-0.0001-0.0003-0.13390.0342-9.7487U 型; 46481 03 90 59 95 36 02 f:浮点 第三点，日志文件(.log)的一条数据如下：

IMU, 46481, 0.000703, -0.000190, -0.000359, -0.133995, 0.034236, -9.748702

第四点，消息类型的定义：

// message types for common messages

// 消息类型，，，对应 FMT 中的消息类型，，，见日志文件 .log 文件。 #define LOG_FORMAT_MSG 128 #define LOG_PARAMETER_MSG 129 #define LOG_GPS_MSG 130 #define LOG_IMU_MSG 131 #define LOG_MESSAGE_MSG 132 #define LOG_RCIN_MSG 133 #define LOG_RCOUT_MSG 134 #define LOG_IMU2_MSG 135 ?

第五点, log_IMU的结构，共占用 3 + 4 + 12 + 12 = 31字节。 struct PACKED log_IMU {

LOG_PACKET_HEADER; // 3 uint32_t timestamp; // 4

float gyro_x, gyro_y, gyro_z; // 4*3 = 12 float accel_x, accel_y, accel_z; // 4*3 = 12 };

第六点：如果要增加自定义的数据结构，那么可以像以下代码一样增加。 #define LOG_TEST_MSG 1

struct PACKED log_Test { LOG_PACKET_HEADER; uint16_t v1, v2, v3, v4; int32_t l1, l2; };

static const struct LogStructure log_structure[] PROGMEM = { LOG_COMMON_STRUCTURES,

{ LOG_TEST_MSG, sizeof(log_Test), // 增加自定义格式数据 \增加自定义格式数据

};

第七点：具体的数据结构操作

DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0])); log_num = DataFlash.StartNewLog(); DataFlash.WriteBlock(&pkt, sizeof(pkt));

DataFlash API隐藏了底层如何存储log文件的细节。另外，对于Pixhawk or Linux这样的支持 Posix IO的系统，日志文件是存储在microSD卡的“LOGS”目录中的。用户可以直接抽出SD卡，直接拷贝到电脑中。 4、Posix IO

有些板子是带操作系统的，支持类似Posix API，如Linux和NuttX。AP_Terrain library就是一个典型的例子。地形数据对于EEPROM是非常的大，经常要随机的存储。DataFlash API就不够灵活了，同时又了Posix IO支持，也就没必要再用DataFlash了。 查看AP_HAL_Boards.h文件，确认HAL_OS_POSIX_IO宏已定义，如下： #define HAL_OS_POSIX_IO 1 // 带文件系统，has posix-like filesystem IO 下面给出了LOG 和 TERRAIN 文件存放路径：

#define HAL_BOARD_LOG_DIRECTORY \日志文件地址

#define HAL_BOARD_TERRAIN_DIRECTORY \// 地表、地形文件地址

有上述信息，就表示支持Posix IO 功能，另外需要说明的是：

1、Posix IO函数，智能通过IO timer定时器，或者其他低优先级线程调用。IO线程优先级59。

2、不要通过其他API直接调用，哪怕是简单stat()函数，都不可以，除非你长得太帅。 3、尽量少存储，存储数据长度小，尽量少用seek（搜寻）功能。

很简单，一个原则，不要太耗时，影响飞控代码执行。一个简单的针对SD卡的IO操作有可能花上一秒钟，这段时间足够让你的飞行器翻转，垂直掉落，直接炸鸡了。Pixhawk SD卡读写操作一般几毫秒，偶尔花费的时间会很长。现在在你知道这么做了？

相关例程 libraries/AP_Terrain/TerrainIO.cpp，我们会发现处理IO的状态机都是通过AP_Terrain::io_timer调用的。

这里，我们对 APM 源码调用做一个简要介绍，并对APM 程序库做详细讲解。如有问题，可以交流30175224@qq.com。新浪@WalkAnt，转载本博客文章，请注明出处，以便更大范围的交流，谢谢。

第七部分 源代码预览与APM:Copter程序库

英文参考：http://dev.ardupilot.com/wiki/apmcopter-code-overview/

本节来源：http://liung.github.io/blog/apm/2014-08-30-APM-Arducopter代码预览.html

APM::Copter代码主要放在ArduCopter文件夹中，并且和ArduPlane和ArduRover使用同样的库文件。

下面这张图展示了从飞行模式到电机输出的调用关系：

APM:Copter程序库

http://dev.ardupilot.com/wiki/apmcopter-programming-libraries/

http://liung.github.io/blog/apm/2014-08-30-APM-Arducopter相关库介绍.html

这些库文件也同样被ArduPlane和ArduRover所使用。下面将列出一系列高层次的库的说明和它们的函数说明。 1 核心库

AP_AHRS：采用DCM（方向余弦矩阵方法）或EKF（扩展卡尔曼滤波方法）预估飞行器姿态。

AP_Common：所有执行文件（sketch格式，arduino IDE的文件）和其他库都需要的基础核心库。

AP_Math：包含了许多数学函数，特别对于矢量运算 AC_PID：PID控制器库

AP_InertialNav：扩展带有gps和气压计数据的惯性导航库 AC_AttitudeControl：姿态控制相关库 AP_WPNav：航点相关的导航库

AP_Motors：多旋翼和传统直升机混合的电机库

RC_Channel：更多的关于从APM_RC的PWM输入/输出数据转换到内部通用单位的库，比

如角度

AP_HAL，AP_HAL_AVR，AP_HAL_PX4:硬件抽象层库，提供给其他高级控制代码一致的接口，而不必担心底层不同的硬件。 2 传感器相关库

AP_InertialSensor：读取陀螺仪和加速度计数据，并向主程序执行标准程序和提供标准单位数据（deg/s，m/s）。

AP_RangerFinder：声呐和红外测距传感器的交互库 AP_Baro：气压计相关库 AP_GPS：GPS相关库

AP_Compass：三轴罗盘相关库

AP_OpticalFlow：光流传感器相关库 3 其他库

AP_Mount，AP_Camera, AP_Relay：相机安装控制库，相机快门控制库 AP_Mission： 从eeprom（电可擦只读存储器）存储/读取飞行指令相关库 AP_Buffer：惯性导航时所用到的一个简单的堆栈（FIFO，先进先出）缓冲区 关于库的导航图，如下：

这里，我们以高度保持模式为例，对姿态控制程序调用关系进行了详细介绍。如有问题，可以交流30175224@qq.com。新浪@WalkAnt，转载本博客文章，请注明出处，以便更大范围的交流，谢谢。

第八部分 姿态控制预览

英文参考：http://dev.ardupilot.com/wiki/apmcopter-programming-attitude-control-2/

本节源自：http://liung.github.io/blog/apm/2014-08-31-APM-ArduCopter姿态控制概览.html

手动飞行模式，诸如自稳模式（Stabilize Mode）、特技模式（Acro Mode）、飘逸模式（Drift Mode），其程序结构如下图：

在主循环执行过程中（比如Pixhawk的任务调度周期2.5ms，400Hz；APM2.x为10ms，100Hz），每一个周期，程序会按下述步骤执行：

首先，高层次文件flight_mode.pde中的update_flight_mode()函数被调用。通过检查control_mode变量，前飞行器的飞行模式（使用变量），然后执行相应飞行模式下的_run()函数（如自稳模式的stabilize_run，返航模式（RTL）的rtl_run等）。执行_run的结果是，系统将会找到与飞行模式相对应的命名为control_.pde飞行控制文件（比如：control_stabilize.pde，control_rtl.pde等）。

_run函数负责将用户的输入（从g.rc_1.control_in，g.rc_2.control_in等读入）转换为此时飞行模式下的倾斜角（lean angle）、滚转速率（rotation rate）、爬升率（climb rate）等（也就是设置目标值roll\\pitch\\yaw\\throttle）。举个例子：AltHold(定高，altitude hold)模式中将用户的滚转和俯仰输入转换为倾斜角（单位：角度/°），将偏航输入转换为滚转速率（单位：°/s），将油门输入转换为爬升率（单位：cm/s）。

_run函数最后还必须要完成的就是将预期角度、速率等参数传送给姿态控制和/或方位控制库（它们都放在AC_AttitiudeControl文件夹内）。

AC_AttitiudeControl库提供了5种可能的方法来调整飞行器的姿态，下面来说明最通用的三种方法：

1) angle_ef_roll_pitch_rate_ef_yaw():该函数需要一个地轴系坐标下滚转和偏航角度，一个地轴系坐标下的偏航速率。例如：传递给该函数三个参数分别为，roll = -1000， pitch = -1500， yaw = 500代表飞行器此时向左倾斜10°，低头15°，向右偏航速率为5°/s。 2) angle_ef_roll_pitch_yaw():该函数接受地轴系下的滚转、俯仰和偏航角。和上面的函数类似，不过参数yaw = 500代表飞行器北偏东5°

3) rate_bf_roll_pitch_yaw():该函数接受一个体轴系下的滚转、俯仰和偏航角速率（°/s）。例如：传递给该函数三个参数：roll = -1000， pitch = -1500， yaw = 500代表飞行器此时左倾速率10°/s，低头速率15°/s，绕Z轴速率为5°/s。

当上述这些函数调用之后，就会接着调用AC_AttitudeControl::rate_controller_run()函数，将上面所列举的函数的输出转化为滚转、偏航和俯仰输入，并使用set_roll,set_pitch,set_yaw 和 set_throttle方法将这些输入发送给AP_Motors库。

另外，

AC_PosControl库用来控制飞行器的3D方位。不过通常只用来调整比较简单的Z轴方向（如姿态控制），这是因为许多需要复杂3D方位调整的飞行模式(例如悬停Loiter)使用的是“AC_WPNav库”。总之，AC_PosControl库中常用的方法有：

1) set_alt_target_from_climb_rate():将爬升率（cm/s）作为参数，用来更新一个需要调整的相对高度目标。

2) set_pos_target():接受一个以系统中的home位置作为参考点的3D位置矢量（单位：cm）。

如果调用了AC_PosControl中的任何一个方法，那么在该飞行模式下就必须调用函数AC_PosControl::update_z_controller()。这样的话，就可以启用Z轴的方位控制PID循环，并向AP_Motors库发送低级别的油门信息。同样，如果调用了xy轴的函数，那就就必须调用AC_PosControl::update_xy_controller()函数。

AP_Motors库含有“电机混合模式”代码。这些代码负责将从AC_AttitudeControl和AC_PosControl库发送过来的滚转、俯仰、偏航角度和油门值信息转换为电机的相对输出值（例如：PWM值）。因此，这样高级别的库就必须要使用如下函数：

1) set_roll(),set_pitch(),set_yaw()：接受在[-4500,4500]角度范围内的滚转、俯仰和偏航角。这些参数不是期望角度或者速率，更准确的讲，它仅仅是一个数值。例如，set_roll(-4500)将代表飞行器尽可能快的向左滚转。

2) set_throttle():接受一个范围在[0,1000]的相对油门值。0代表电机关闭，1000代表满油门状态。

虽然对于不同飞行器构型（如四旋翼，Y6，传统直升机等）的控制代码中有许多不同的类，但这些类中都有一个相同的函数output_armed，负责将这些滚转、俯仰、偏航和油门值转换为PWM类型输入值。这转换的过程中，会应用到stability patch，用来控制由于飞行器构型限制所引起的轴系的优先级问题（例如四旋翼的四个电机不可能在做最大速度滚转时四个电机的油门同时达到最大，因为必须一部分电机输出小于另一部分才能引起滚转）。在执行函数output_armed的最后，还将调用hal.rcout->write()，把期望PWM值传递给AP_HAL层。 AP_HAL库（硬件抽象层）提供了针对所有飞控板统一的接口。实际控制中，hal.rc_out->write()函数将接受到的来自于AP_Motors类中指定的PWM值，输出至飞控板对应的PWM端口（pin端）。

这一节讲述如何向APM代码中添加新的飞行模式。通过这里我们可以对飞行模式相关的几个文件有一个非常清晰的认识。如有问题，可以交流30175224@qq.com。新浪长沙@WalkAnt，转载本博客文章，请注明出处，谢谢。 第十部分 添加新的飞行模式

英文参考：http://dev.ardupilot.com/wiki/apmcopter-adding-a-new-flight-mode/

本节源自：http://liung.github.io/blog/apm/2014-09-05-APM-ArduCopter添加新的飞行模式.html

这部分将涵盖一些怎样创建一个新的高级别的飞行模式的基本操作步骤（类似于自稳，悬停等），这些新模式处于“the onion”（洋葱头工程）中的高级别代码控制部分，如之前姿态控制页面描述的一样。不过遗憾的是本页面并没有提供给你关于所创建的理想飞行模式需要的所有信息，但是希望这将是一个好的开始。

Step #1：在文件defines.h中用#define定义你自己新的飞行模式,然后将飞行模式数量NUM_MODES加1。 // Auto Pilot modes // ----------------

#define STABILIZE 0 // hold level position #define ACRO 1 // rate control

#define ALT_HOLD 2 // AUTO control #define AUTO 3 // AUTO control #define GUIDED 4 // AUTO control

#define LOITER 5 // Hold a single location #define RTL 6 // AUTO control #define CIRCLE 7 // AUTO control #define LAND 9 // AUTO control

#define OF_LOITER 10 // Hold a single location using optical flow sensor #define DRIFT 11 // DRIFT mode (Note: 12 is no longer used) #define SPORT 13 // earth frame rate control #define FLIP 14 // flip the vehicle on the roll axis

#define AUTOTUNE 15 // autotune the vehicle's roll and pitch gains #define POSHOLD 16 // position hold with manual override

#define NEWFLIGHTMODE 17 // new flight mode description #define NUM_MODES 18

Step #2：类似于相似的飞行模式的control_stabilize.pde或者control_loiter.pde文件，创建新的飞行模式的.pde控制sketch文件。该文件中必须包含一个_init()初始化函数和_run()运行函数，类似于static boolalthold_init(bool ignore_checks)和static void althold_run() /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // newflightmode_init - initialise flight mode

static bool newflightmode_init(bool ignore_checks) {

// do any required initialisation of the flight mode here

// this code will be called whenever the operator switches into this mode

// return true initialisation is successful, false if it fails

// if false is returned here the vehicle will remain in the previous flight mode return true; }

// newflightmode_run - runs the main controller // will be called at 100hz or more static void newflightmode_run() {

// if not armed or throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || g.rc_3.control_in <= 0) { attitude_control.relax_bf_rate_controller();

attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; }

// convert pilot input into desired vehicle angles or rotation rates // g.rc_1.control_in : pilots roll input in the range -4500 ~ 4500 // g.rc_2.control_in : pilot pitch input in the range -4500 ~ 4500 // g.rc_3.control_in : pilot's throttle input in the range 0 ~ 1000 // g.rc_4.control_in : pilot's yaw input in the range -4500 ~ 4500

// call one of attitude controller's attitude control functions like

// attitude_control.angle_ef_roll_pitch_rate_yaw(roll angle, pitch angle, yaw rate);

// call position controller's z-axis controller or simply pass through throttle // attitude_control.set_throttle_out(desired throttle, true); }

Step #3：在文件flight_mode.pde文件的set_mode()函数中增加一个新飞行模式的case（C++中switch..case语法）选项，然后调用上面的_init()函数。 // set_mode - change flight mode and perform any necessary initialisation static bool set_mode(uint8_t mode) {

// boolean to record if flight mode could be set bool success = false;

bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform

// return immediately if we are already in the desired mode if (mode == control_mode) { return true; }

switch(mode) { case ACRO:

#if FRAME_CONFIG == HELI_FRAME success = heli_acro_init(ignore_checks); #else

success = acro_init(ignore_checks); #endif break;

case NEWFLIGHTMODE:

success = newflightmode_init(ignore_checks); break; } }

Step #4:在文件flight_mode.pde文件的update_flight_mode()函数中增加一个新飞行模式的case选项，然后调用上面的_run()函数。

// update_flight_mode - calls the appropriate attitude controllers based on flight mode // called at 100hz or more

static void update_flight_mode() {

switch (control_mode) { case ACRO:

#if FRAME_CONFIG == HELI_FRAME heli_acro_run(); #else

acro_run(); #endif break;

case NEWFLIGHTMODE:

success = newflightmode_run(); break; } }

Step #5: 在文件flight_mode.pde文件的print_flight_mode()函数中增加可以输出新飞行模式字符串的case选项。

static void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) {

switch (mode) { case STABILIZE:

port->print_P(PSTR(\ break;

case NEWFLIGHTMODE:

port->print_P(PSTR(\

break;

Step #6：在文件Parameters.pde中向FLTMODE1 ~ FLTMODE6参数中正确的增加你的新飞行模式到@Values列表中。 // @Param: FLTMODE1

// @DisplayName: Flight Mode 1

// @Description: Flight mode when Channel 5 pwm is 1230, <= 1360 // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport,17:NewFlightMode // @User: Standard

GSCALAR(flight_mode1, \

// @Param: FLTMODE2

// @DisplayName: Flight Mode 2

// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport,17:NewFlightMode // @User: Standard

GSCALAR(flight_mode2, \

Step #7：如果你想让你的新飞行模式出现的Mission Planner的平视显示器HUD和飞行模式组件中，你需要相应修改Mission Planner代码。关于Mission Planner如何编译的问题，请参考我的另外一篇文章：http://blog.sina.com.cn/s/blog_402c071e0102v4kx.html。有任何问题可以@WalkAnt，30175224@qq.com与我交流，如果有湖南长沙星沙的朋友想要相约一起放飞机的，再高兴不过了。每每假日，一个人独自在长沙县政府通程广场放飞机，好不孤单~~~~不过那里天空中的风筝倒是不少，也不觉形单影只了。

这一节将向你介绍如何规划你的新代码块,，使之可以按需运行。实际上在本节内容在本博客《Pixhawk源码笔记二：APM线程》的第6节“AP_Scheduler任务调度系统”中已有讲述，这里再做进一步介绍。如有问题，可以交流30175224@qq.com。新浪长沙@WalkAnt，转载本博客文章，请注明出处，谢谢。 第十一部分 调用代码，使之定时运行 英文参考：http://dev.ardupilot.com/wiki/code-overview-scheduling-your-new-code-to-run-intermittently/ 本节源自：http://liung.github.io/blog/apm/2014-09-05-APM-ArduCopter规划新代码使之按一定频率运行.html

1、用代码调度器（scheduler）运行你的代码

在给定时间间隔内来运行你的代码的最灵活的方式就是使用调度器。这可以通过将你的

函数添加到文件ArduCopter.pde中的scheduler_tasks数组来完成。需要表明的是：实际上该文件中有两个任务列表，上面的任务列表是针对高频CPUs（如Pixhawk），对应的调度频率是400Hz，下面的是针对低频CPUs（如APM2），对应的调度频率是100Hz。 添加一个任务是相当的简单，你只要在列表添加新的一行代码就可以了（列表中位置越靠前意味着拥有更高的级别）。任务项中的第一列代表了函数名，第二列是以2.5ms为单位的数字（或者APM2中以10ms为单位）。所以，如果你想要你的函数执行频率为400Hz，那么该列就需要填写为“1”，如果想要50Hz，那么就需要改为“8”。任务项的最后一列代表该函数预计运行花费的微秒（百万分之一秒）时间。这可以帮助调度器来预估在下一个主循环开始之前有否有足够的时间来运行你的函数。

static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { update_GPS, 2, 900 }, { update_nav_mode, 1, 400 }, { medium_loop, 2, 700 }, { update_altitude, 10, 1000 }, { fifty_hz_loop, 2, 950 }, { run_nav_updates, 10, 800 }, { slow_loop, 10, 500 },

{ gcs_check_input, 2, 700 }, { gcs_send_heartbeat, 100, 700 }, { gcs_data_stream_send, 2, 1500 }, { gcs_send_deferred, 2, 1200 }, { compass_accumulate, 2, 700 }, { barometer_accumulate, 2, 900 }, { super_slow_loop, 100, 1100 }, { my_new_function, 10, 200 }, { perf_update, 1000, 500 } };

2、作为循环的一部分运行你的代码

为了代替在代码调度器中加入一个新的函数入口，你还可以在现有的任何时间循环事件中添加你的函数。除了在fast-loop循环中添加外，这种方法对比起上面的代码调度器方法并没有什么实质性好处。但当你的代码添加到fast-loop循环中时，就意味着它将以最高的优先级别来执行（它几乎能100%达到所确保的400hz运行速度）。 fast_loop:APM2上运行频率100hz，Pixhawk上400Hz fifty_hz_loop:运行频率50hz

ten_hz_logging_loop:运行频率10hz three_hz_loop:运行频率3.3hz on_hz_loop:运行频率1hz

所以举个例子，如果你想让你的代码运行频率为10hz，那么你就要将它添加到ArduCopter.pde文件的ten_hz_logging_loop()函数声明中。 // ten_hz_logging_loop // should be run at 10hz

static void ten_hz_logging_loop() {

if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) {

Log_Write_Attitude(); }

if (g.log_bitmask & MASK_LOG_RCIN) { DataFlash.Log_Write_RCIN(); }

if (g.log_bitmask & MASK_LOG_RCOUT) { DataFlash.Log_Write_RCOUT(); }

if ((g.log_bitmask & MASK_LOG_NTUN) && mode_requires_GPS(control_mode)) { Log_Write_Nav_Tuning(); }

// your new function call here my_new_function(); }

这一节将向你介绍如何增加新的MAVLink消息。（上图以自动起飞为例，简单说明了函数之间的调用关系）如有问题，可以交流30175224@qq.com。新浪长沙@WalkAnt，转载本博客文章，请注明出处，谢谢。 第十二部分 增加新的MAVLink消息

英文参考：http://dev.ardupilot.com/wiki/code-overview-adding-a-new-mavlink-message/

本节源自：http://liung.github.io/blog/apm/2014-09-05-APM-增加新的MAVLink通讯协议消息.html

MavLink协议：https://pixhawk.ethz.ch/mavlink/

地面站之间的数据和指令通信都是通过串行接口使用MAVLink协议来传递的。本页面将提供关于添加新的MAVLink信息的一些高级建议。

这些指令仅在Liunx上测试完成（准确的说，是在Windows上运行的Ubuntu虚拟机上测试完成的）。关于设置虚拟机的方法在SITL(软件层面仿真)页面有相关介绍。如果你要运行SITL，你最好遵循下面的一些建议。这些指令不能直接在Windows或者Mac平台上本地运行。

Step #1：确保你已经安装了最新的ardupilot代码，同时也检查一下mavproxy是否是最新版本。mavproxy工具可以通过在终端窗口运行下面命令进行升级。 sudo pip install --upgrade mavproxy

Step #2：先确定你所要添加的信息的类型，以及如何和已有的MavLink消息兼容。

比如：你可能会想要向飞行器发送一个新的导航指令，让它可以在任务中期（自动模式中）模仿一个特技动作（比如翻筋斗）。在这个例子中，你需要一个类似于MAV_CMD_NAV_WAYPOINT（可以在MAVLink消息页面搜索MAV_CMD_NAV_WAYPOINT）一样的新的导航指令MAV_CMD_NAV_TRICK。

又或者你想要从飞行器发送一个新的传感器数据类型到地面站,可能类似于SCALED_PRESSURE消息。

Step #3：在common.xml和ardupilotmega.xml文件中添加你的信息的定义声明。

如果你希望将该指令添加到MAVLink协议中，那么你应该添加该指令到../ardupilot/libraries/GCS_MAVLink/message_definitions/common.xml文件中。如果你仅仅个人使用或者仅仅和ArduCopter，ArduPlane，ArduRover搭配使用，那么它就应该被添加到ardupimega.xml文件中。

Step #4：重新生成你的所有inlcude文件，确保添加的信息在主代码中可以被识别。 首先将目录切换到ardupilot文件夹下，然后执行下面命令： ./libraries/GCS_MAVLink/generate.sh

成功执行后，你应该看到下面这些文件都应经被更新。

../libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h ../libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h ../libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h

文件version.h仅简单的更新了文件的日期和时间，但是ardupilotmega.h文件已经应该有了你的新消息的定义声明。

Step #5：在飞行器主代码中添加函数方法用来控制向/从地面站发送/接收指令。

这些顶层代码指令绝大部分包含在飞行器的GCS_MAVLink.pde文件中或在../libraries/GCS_MAVLink/GCS类中。

在我们想要添加一个新的导航指令的例子中（比如执行特技动作），应该需要下面信息： 扩展AP_Mission库中的mission_cmd_to_mavlink()和mavlink_to_mission_cmd()方法，将

mavproxy的指定转换到一个AP_Mission::Mission_Command结构体中。

// 将 Mission_Command 对象 转换到 mavlink消息，该消息能够被发送到 GCS 地面站。

bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd, mavlink_mission_item_t& packet) {? ?}

// 将 mavlink消息 转换到 Mission_Command 对象，该对象可以被存储到 eeprom

bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP_Mission::Mission_Command& cmd) {? ?}

在飞行器的commands_logic.pde文件中分别添加start_command()函数和verify_command()函数的一个case分支，用来校验新的消息指令MAV_CMD_NAV_TRICK是否接收到。这些需要你调用自己创建的两个新函数do_trick()和verify_trick()（具体参考下面）。

创建两个新函数do_trick()和verify_trick()，用来控制飞行器如何执行特技动作（这可能需要调用control_auto.pde中的另一个函数来设置auto_mode变量，然后调用新方法auto_trick_start()）。当指令第一次被唤醒时将使用do_trick()函数。verify_trick()函数将会以10hz频率（或者更高）被重复调用直到特技动作完成，当特技动作执行完毕之后verify_trick()函数应该返回True。

本文来源：https://www.bwwdw.com/article/fsug.html