每天读一点儿APM(PIX)代码之三：飞行控制
本期话题以ArduPlane代码为例分析了Ardupilot飞控体系对飞行器的反馈控制过程，并在实例中添加了一个直接易用的固定翼起飞辅助模式，供大家参考。
姿态控制是飞控的核心功能，不论固定翼还是多旋翼，都需要维持飞行器本身的稳定。ArduPilot采用反馈修正的方式进行姿态控制，以固定翼ArduPlane为例，其工作流程如下：
1.计算目标姿态
该部分功能位于APMPlane的update_flight_mode函数，根据当前飞行器所处于的飞行模式以及状态，计算得出飞行器的目标姿态,将目标俯仰和横滚角度保存在nav_roll_cd和nav_pitch_cd中。
2.计算自稳控制值
stabilize函数的主要功能是根据飞行器目标姿态，计算在俯仰和横滚方向的修正控制量。其内部调用AP_RollController，AP_PitchController，AP_YawController等姿态控制类，根据飞行器当前姿态到目标姿态的差距，进行PID反馈控制，计算得出各通道的控制值，存储在相应通道的servo_out参数中。
遥控器此时的输入也会影响上述控制值结果，因此我们在stablize，FlyByWire，auto等飞行模式时，仍可以由遥控器对飞行进行干预。
3.计算各舵机数值
Set_Servos函数把各通道的控制输出值直接映射到相关舵机上。这里主要考虑的问题是当混控存在时（三角翼模式或者V尾模式），roll,pitch，yaw的输出均与若干舵机相关，因此需要根据混控方式的不同，计算各舵机的输出量。
4.输出到舵机
将舵机输出量转换为pwm脉冲，直接输出到已定义针脚。
飞行控制过程代码相对繁杂，不过大多数情况我们对飞控的开发不需要关注上述全部代码，如果以自动化导航为开发目的，不关心飞行器的自稳的算法细节，可以只关心计算目标姿态；如果关心飞行器的反馈控制，可以重点研究计算自稳控制值所涉及的各种controller；如果开发特殊机械结构的飞行器，则可以在计算舵机数值的功能块内，根据特殊的舵机、喷气孔特点修改各动态结构的状态。
为了进一步说明arduplane的控制过程，在如下例子中，填加了一个简单、直接的起飞模式。
如果需要通过手抛起飞的方式飞固定翼，可以使用飞控提供的自动起飞功能，在自动起飞阶段，飞行器的油门，副翼，平尾等可以自动调节，以保证飞机的姿态。众所周知，apm和pix提供的起飞辅助功能是在AUTO飞行模式下的，使用起来有如下不便：
首先，必须通过地面站，设置航点，添加一个名为Takeoff的任务后，切换到auto模式，才可以激活起飞辅助功能，过程繁琐。
其次，起飞辅助是自动航点中的一种状态，会带来操纵者的迷惑，很多时候，并不能通过osd界面获知当前是否处于准备起飞的状态，进退两难。
最后，起飞模式要设置最大油门，最大爬升角，脱手检测速度，马达起动延迟等若干参数，对于新手，如果很多参数设置不合适，会导致动力和反馈不足，起飞失败。
因此，我定义了一个新的takeoff起飞模式，它具有非常简单的逻辑:
1.是一个独立的飞行模式，通过遥杆切换到起飞模式后，飞行器马上处于起飞辅助状态。
2.起飞模式中，油门由遥控器手动控制；飞控仅尝试将飞机稳定到起飞俯仰值（20度），并保证飞机平衡。
3.当飞控检测到遥控器水平位移的输入大于正负10度，或者飞行器相对高度大于50米时，自动切换到STABILIZE模式。
这种起飞辅助，相比于市面常见商品飞控，缺少了自动的油门控制防打手功能，但是却带来了更为简单可靠的起飞体验。事实上，在我和周围朋友们手抛起飞时，往往也都是带着飞机助跑一段儿，待飞机达到一定速度，马达起动后再抛出手(各种前拉，腰推机毫无问题；天捷力小胖子、大胖子也完全不担心打手，尽情放心；x5类飞翼，请推满油门侧手抛，注意脱手时飞行器的姿态，防止倾覆)。
首先，添加飞行模式（参见http://dev.ardupilot.com/wiki/apmcopter-adding-a-new-flight-mode/）。在defines.h中添加TAKEOFF模式，序号17：
enum FlightMode {
MANUAL = 0,
CIRCLE = 1,
STABILIZE = 2,
TRAINING = 3,
ACRO = 4,
FLY_BY_WIRE_A = 5,
FLY_BY_WIRE_B = 6,
CRUISE = 7,
AUTOTUNE = 8,
AUTO = 10,
RTL = 11,
LOITER = 12,
GUIDED = 15,
INITIALISING = 16,
TAKEOFF = 17
};case TAKEOFF:
auto_throttle_mode = false;
takeoff_pitch = aparm.pitch_limit_max_cd.get();
break;其中，auto_throttle_mode=false，表示这种模式下，油门由遥控器控制。
Takeoff_pitch是APM参数LIM_PITCH_MAX定义的最大俯仰角度，此处用来用作起飞模式的抬头角度。
然后，在arduplane.ino的update_flight_mode()函数中添加TAKEOFF模式的控制逻辑：
nav_roll_cd = 0;
nav_pitch_cd = takeoff_pitch;
int16_t pitchinput = channel_pitch->pwm_to_angle();
if (pitchinput > 1000 || pitchinput < -1000 || relative_altitude_abs_cm()>5000)
{
set_mode(STABILIZE);
}其中，前两行代码将会使飞行器永远保持横滚方向的水平，以及一个固定的抬头俯仰角。
pitchinput是从遥控器获取的俯仰通道相对值，一旦其绝对值大于1000(相当于10度)，就会推出起飞模式，切换到平衡模式。
如果飞行器的相对高度大于50米，也会切换到平衡模式。
最后，需要修改逻辑，使得在TAKEOFF模式下，仍然可以通过遥控器通道对飞行器的飞行进行微调，在Attitude.ino的stabilize()函数作修改，允许TAKEOFF模式下进行stabilize_stick_mixing_direct()直接混控。
（注，这里有两种混控方式：
stabilize_stick_mixing_fbw()：遥控器的输入影响飞行器的目的姿态，由姿态控制模块根据目的姿态计算舵机输出位置。如ACRO,FLY_BY_WIRE_A AND b,AUTOTUNE,CRUISE,AUTO等飞行模式。
stabilize_stick_mixing_direct()：首先根据目的姿态计算舵机输出位置，然后将遥控器的输入作 在实际使用中，fbw类混控给人感觉飞行器非常平稳，对于遥控器的输入反映迟缓，而direct类则操纵十分灵活，任何些微改动都可以直接体现出来。
）
此次修改的源码以及修改后重新编译的rom可以参见github：。https://github.com/xttyyy/arduplane3.2.1-takeoff-8mode/。