函数添加到文件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。