玩转四旋翼无人机（姿态控制 in ArduPilot）

本文主要介绍关于ardupliot project中关于四旋翼的姿态控制的

首先给出姿态控制的结构图



顶层文件flight-mode.的飞行模式，然后调用合适的

<flight mode>_run()

函数(i.e. stabilize_run for stabilize mode, rtl_run for RTL mode, etc).

<flight mode>_run()

函数可以在文件

control_<flight mode>.cpp

(i.e. control_stabilize.cpp, control_rtl.cpp, etc)中找到。该函数负责转换当前用户的输入（如rc_1.control_in, g.rc_2.control_in）为一个针对飞行模式的倾斜角，旋转速度，攀升速度。如在定高模式下，yaw输入转换为旋转速度。油门输入转化为攀升速度。该函数的最后要做的就是将这些的道德角度速度传递给姿态控制和位置控制器。

姿态控制库提供了5中飞行器的控制方式。最常用的3种是

- angle_ef_roll_pitch_rate_ef_yaw() : 该函数支持一个地球坐标系下的滚转和俯仰角度、yaw速度。

- angle_ef_roll_pitch_yaw() : 该函数支持一个地球坐标系下的滚转、俯仰、偏航角度。

- rate_bf_roll_pitch_yaw() : 该函数支持集体坐标系下的滚转、俯仰、偏航角度。

调用上述angle_ef_roll_pitch_rate_ef_yaw等函数之后，在执行中调用了 AC_AttitudeControl::rate_controller_run()。该函数转换上述方法计算的输出为roll pitch yaw，并使用函数AP_Motors library中的函数 set_roll, set_pitch, set_yaw and set_throttle

AC_PosControl库支持飞行器的3D位置控制，一般只是简单的高度z轴控制，因为更复杂的3D位置模式控制（如Loiter模式）使用了AC_WPNav库，这样该库中常用的方法包括

- set_alt_target_from_climb_rate() : this accepts a climb rate in cm/s and updates an absolute altitude target

- set_pos_target() : this accepts a 3D position vector which is an offset from home in cm

如果AC_PosControl中的方法被调用，飞行模式的代码一定会调用AC_PosControl::update_z_controller(). 执行的z轴的位置PID控制环并发送低级的油门大小到AP_Motors库。如果任意xy轴的控制方法被调用，那么随后会调用AC_PosControl::update_xy_controller()。

The AP_Motors库保留了“motor mixing”的代码。这些代码负责转换来自 AC_AttitudeControl and AC_PosControl库的roll, pitch, yaw, 油门大小为绝对的电机输出(i.e. PWM values)。level因此高层库会用到这些函数

- set_roll(), set_pitch(), set_yaw() : 输入参数为roll, pitch and yaw，值范围为-4500 ~ 4500。这些值并不对应角度和角速度仅仅是一个值。例如set_roll(-4500)，roll偏到左极限。

- set_throttle() : 输入参数为一个绝对油门值，范围是0~1000. 0是电机不转，1000是电机全速。

对于不同的机架类型还有不同类的定义，但是所有的都会有一个output_armed的函数，该函数负责实现roll， pitch， yaw和油门的值为PWM。转换中包含stability patch的实现，当输入需求超过了飞机可以达到的物理极限，例如(i.e. throttle的最大值和roll的最大值对于一个四旋翼是无法同时实现的，因为roll要求一些电机必须小于其他的电机。“output_armed”函数中会调用hal.rcout->write()并传递一个想要的pwm值给AP_HAL层。

AP_HAL（硬件抽象层）库提供了对所有的板子的一个统一的接口。