四轴项目总结之二-姿态，数据篇

本来想一篇博文写完的，不知怎的，卡死了，遂另开一篇，接下来还有一篇，分三篇吧，这张主要介绍飞行，飞控原理。
先给大家分享一个空间角度上的概念。
roll翻滚角

pitch俯仰角

yaw 航向角

飞行器能上下前后左右飞行,甚至一些高难度的动作都是依赖于这几个角度的精准控制，试着想象一下，前后左右飞，遥控器的各个遥感应该怎样打。你要实现的一些基本操作包括：
自稳
释放操纵杆之后飞机将会停止原有的飞行模式并悬停在一个稳定的位置。
电传操纵训练
某些飞行姿态可以直接通过飞控设定的，而避免了各种机械传动装置的复杂性。比如你可以自行设定横滚、航向和俯仰角度的最大最小值及其变化速率的快慢
自动飞行
模型可以按照你预先设定好的路径飞行，当然，你可以手动干预飞行路线。
自动返航（RTL
飞行器可以按照预设的路线自动返航至起始飞行点并保持在预设高度悬停，这个功能需要GPS支持。
悬停
如果不给飞行器任何的控制信号，那么飞行器将会尽可能保持原有的飞行姿态并保持原有的位置不变
自动启动和着陆
或许这是避免坠机的最好方式了。。。
利用一些外部模块或者采用其他飞控都可以实现更多不同的功能，采用什么飞控和外设取决于你需要什么功能

要实现上面的功能自然少不了各种各样的传感器，
基本的有AHRS采集加速度和角速度，超声波或气压计采集高度信息，GPS采集位置信息。其中前两种楼主有研究过，GPS暂时木有研究，先不予讨论。
AHRS
首先，它能采集到xyz三个轴的角速度和角加速度，至于xy轴和roll,pitch,yaw是怎样对应的，依赖于软件的实现，典型的有x型和+型，其中xy轴用的较多，z轴的数据主要用来防止飞行器发生自旋。有了这些参数之后，如果pid调的较好就能实现自稳了，那怎么来实现呢？要点在于怎么合成角度，有人说对角速度进行积分不就得到角度了嘛，对，也不对，原因在于只用角度误差太大了，于是就有各种滤波算法，典型的有卡尔曼滤波，互补滤波。下面给一个卡尔曼滤波的例子：

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
            float norm;
            float hx, hy, hz, bx, bz;
            float vx, vy, vz, wx, wy, wz;
            float ex, ey, ez;

            // auxiliary variables to reduce number of repeated operations
            float q0q0 = q0*q0;
            float q0q1 = q