本來想一篇博文寫完的，不知怎的，卡死了，遂另開一篇，接下來還有一篇，分三篇吧，這張主要介紹飛行，飛控原理。
先給大家分享一個空間角度上的概念。
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 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
// compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}有時，考慮到有零飄，我們還要用磁力計來進行校準。關於算法之類的講解，不在這裏進行，感興趣的可以自行推導，網上有很多的介紹和源碼，當然了，還有相關參數的調試，如果不想自己進行，也可以在網上購買姿態結算模塊，150左右，結算後的姿態uart或iic輸出的都有，精度還蠻高的。
超聲波，氣壓計
一般超聲波模塊適用於低空（<3m）,精度可達到cm級，氣壓計適用於高空，精度在0.5m左右，在後面的高度pid整定時，我們會提到，懸停時上下波動控制在20cm左右算是比較正常的範圍。用定時器去採，或者直接用串口收就可以了，注意在後面數據的使用時需要加入姿態進行校準（×sin(theta)）,還有就是要對數據進行必要的濾波，將波動範圍控制在一個合理的範圍，以防出現異常。
攝像頭
主要用來循跡，我主要參考的是飛思卡爾小車中用來循跡的算法，效果不是很理想，注意圖像的數據量非常之大，要進行合理的壓縮，二值化是必不可少的一個過程，同時，調攝像頭也需要花費不少的功夫，建議一定要結合相應的可視調試助手進行調試。
自此，飛行器姿態結算和數據採集預處理算是有個小結了，姿態解算講的不是很詳細，請再參考其它博文和源碼研究。後面主要進行pid的控制和調試。