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）运行你的代码
在给定时间间隔内来运行你的代码的最灵活的方式就是使用调度器。这可以通过将你的