匿名四軸前段時間開源了，還出來不少好文章，轉載給大家共享
/******************** (C) COPYRIGHT 2014 ANO Tech ***************************
* 文件名 ：ANO_FlyControl.cpp* 描述 ：飛行控制
**********************************************************************************/
include
"ANO_FlyControl.h"ANO_FlyControl fc;
/*
先整定內環，後整定外環。
參數整定找最佳，從小到大順序查
先是比例後積分，最後再把微分加
曲線振盪很頻繁，比例度盤要放大
曲線漂浮繞大灣，比例度盤往小扳
曲線偏離回覆慢，積分時間往下降
曲線波動週期長，積分時間再加長
曲線振盪頻率快，先把微分降下來
動差大來波動慢。微分時間應加長
理想曲線兩個波，前高後低4比1
*/
/*
ROLL和PIT軸向按照以上公式計算PID輸出，但YAW軸比較特殊，因爲偏航角法線方向剛好和地球重力平行，
這個方向的角度無法由加速度計直接測得，需要增加一個電子羅盤來替代加速度計。如果不使用羅盤的話，
我們可以單純的通過角速度積分來測得偏航角，缺點是由於積分環節中存在積分漂移，偏航角隨着時間的推移
會偏差越來越大。我們不使用羅盤就沒有比例項，只僅使用微分環節來控制。
*/
ANO_FlyControl::ANO_FlyControl()
{
yawRate = 120;
//重置PID參數
PID_Reset();
}
//重置PID參數
void ANO_FlyControl:
ID_Reset(void)
{
//因爲YAW角度會漂移，所以參數和ROLL、PITCH不一樣
pid[PIDROLL].set_pid(70, 15, 120, 2000000); //ROLL角度的內環控制係數,20000:積分上限
pid[PIDPITCH].set_pid(70, 30, 120, 2000000);//PITCH角度的內環控制係數
pid[PIDYAW].set_pid(100, 50, 0, 2000000); //YAW角度的內環控制係數
pid[PIDLEVEL].set_pid(280, 0, 0, 0); //外環控制係數
pid[PIDMAG].set_pid(15, 0, 0, 0); //電子羅盤控制係數
}
/*
【掃盲知識】
串級PID：採用的角度P和角速度PID的雙閉環PID算法------>角度的誤差被作爲期望輸入到角速度控制器中 （角度的微分就是角速度）
對於本系統則採用了將角度控制與角速度控制級聯的方式組成整個串級 PID 控制器。
串級 PID 算法中，角速度內環佔着極爲重要的地位。在對四旋翼飛行的物理模型進
行分析後，可以知道造成系統不穩定的物理表現之一就是不穩定的角速度。
因此，若能夠直接對系統的角速度進行較好的閉環控制，必然會改善系統的動態特性
及其穩定性，通常也把角速度內環稱爲增穩環節。而角度外環的作用則體現在對四旋翼飛
行器的姿態角的精確控制。
外環：輸入爲角度,輸出爲角速度
內環：輸入爲角速度，輸出爲PWM增量
使用串級pid，分爲：角度環控制pid環，和角速度控制環穩定環。主調爲角度環（外環），副調爲角速度環（內環）。
參數整定原則爲先內後外，故在整定內環時將外環的PID均設爲0
所謂外環就是隻是一個P在起作用，也就是比例在起作用；P也就是修正力度，越大越容易使飛機震盪。
震盪的特點是：頻率小、幅度大
*/
/*
【橫滾（Roll）和俯仰（Pitch）的控制算法】
橫滾（Roll）和俯仰（Pitch）的控制算法是一樣的，控制參數也比較接近。
首先得到軸姿態的角度差（angle
error），將這個值乘以角度係數p後限幅（限幅必須有，否則劇烈打舵時容易引發震盪）作爲角速度控制器期望值（target_rate）。target_rate與陀螺儀得到的當前角速度作差，得到角速度誤差（rate_error）乘以kp得到P。在I值小於限幅值（這個值大概在5%油門）或者rate_error與i值異號時將rate_error累加到I中。前後兩次rate_error的差作爲D項，值得注意的是加需要入20hz（也可以採用其它合適頻率）濾波，以避免震盪。將P,I,D三者相加並限幅（50%油門）得到最終PID輸出。
*/
//串環PID調節詳情參見：http://blog.csdn.net/super_mic
... 36723
//飛行器姿態外環控制（角度控制）
void ANO_FlyControl::Attitude_Outter_Loop(void)
{
int32_t errorAngle[2];
Vector3f Gyro_ADC;
//計算角度誤差值,
角度誤差值=期望值-此刻姿態值
//constrain_int32作用：32位整型數限幅，使其控制輸入的最大飛行傾角不大於25度（如果控制量比25度大，飛機早就墜毀了）
//rc.Command[ROLL]：遙控數據 imu.angle.x ：此刻姿態(角度)
//1.得到軸姿態的角度差（errorAngle）
//2.這個角度差值進行限幅(constrain_int32)（正負FLYANGLE_MAX）（限幅必須有，否則劇烈打舵時容易引發震盪）作爲角速度控制器期望值（target_rate）
errorAngle[ROLL] = constrain_int32((rc.Command[ROLL] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.x
* 10;
errorAngle[PITCH] = constrain_int32((rc.Command[PITCH] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.y
* 10;
//獲取此時陀螺儀上的角速度，取角速度的四次平均值
Gyro_ADC = mpu6050.Get_Gyro() / 4;
/*
得到外環PID輸出（角速度的差值）(實質是相當於內環的P比例項)-------->
3.target_rate與陀螺儀得到的當前角速度作差，得到角速度誤差（RateError）乘以kp（外環控制係數 pid[PIDLEVEL]--->(280, 0, 0, 0)）得到給內環的P。
*/
//橫滾roll：外環控制。輸入爲角度,輸出爲角速度。RateError[ROLL]
作爲內環的輸入。
RateError[ROLL] = pid[PIDLEVEL].get_p(errorAngle[ROLL]) - Gyro_ADC.x; //Gyro_ADC.x:陀螺儀X軸的值
//俯仰pitch：外環控制。輸入爲角度,輸出爲角速度。RateError[PITCH]
作爲內環的輸入。
RateError[PITCH] = pid[PIDLEVEL].get_p(errorAngle[PITCH]) - Gyro_ADC.y;//Gyro_ADC.y:陀螺儀Y軸的值
/*
偏航（Yaw）的控制算法和前兩者略有不同，是將打舵量（遙控數據量rc.Command[YAW]）和角度誤差的和作爲角速度內環的期望值，
這樣可以獲得更好的動態響應。角速度內環和橫滾與俯仰的控制方法一致，參數（積分限幅值會很小，默認只有萬分之8）上有不同。*/
//航向yaw：外環控制。輸入爲角度,輸出爲角速度。 RateError[YAW] 作爲內環的輸入。
RateError[YAW] = ((int32_t)(yawRate) * rc.Command[YAW]) / 32 - Gyro_ADC.z; //Gyro_ADC.z:陀螺儀Z軸的值
}
//飛行器姿態內環控制:
輸入爲角速度，輸出爲PWM增量
//內環的效果就是：減小
P比例控制帶來的震盪
void ANO_FlyControl::Attitude_Inner_Loop(void)
{
int32_t PIDTerm[3];
//注意這裏是i的值是0到2
//PIDROLL、PIDPITCH、PIDYAW是枚舉類型，也就是0、1、2，也就是下面的pid 、PIDTerm就是3組PID
for(u8 i=0; i<3;i++)
{
//現象：當油門低於檢查值時積分清零，重新積分
//猜測：這裏應該是擔心飛機沒飛起來時就開始有積分，會導致起飛時不穩定
if ((rc.rawData[THROTTLE]) < RC_MINCHECK)
pid.reset_I();
//get_pid函數：return get_p(error) + get_i(error, dt) + get_d(error, dt);-------->這裏實際就是一個完整的PID
//PID_INNER_LOOP_TIME：2000us--->0.2ms 積分微分時間，每隔0.2ms操作積分和微分,RateError是外環計算的結果（從外環算出）
//得到內環PID輸出，直接輸出轉爲電機控制量
PIDTerm = pid.get_pid(RateError, PID_INNER_LOOP_TIME);
}
//對YAW角繼續處理，加入遙控控制
//在I值小於限幅值（這個值大概在5%油門）或者rate_error與i值異號時將rate_error累加到I中。
PIDTerm[YAW] = -constrain_int32(PIDTerm[YAW], -300 - abs(rc.Command[YAW]), +300 + abs(rc.Command[YAW]));
//PID輸出轉爲電機控制量
motor.writeMotor(rc.Command[THROTTLE], PIDTerm[ROLL], PIDTerm[PITCH], PIDTerm[YAW]);
}
/*
【調節串環PID大概過程（注意修正反向）】
1、估計大概的起飛油門。
2、調整角速度內環參數。
3、將角度外環加上，調整外環參數。
4、橫滾俯仰參數一般可取一致，將飛機解綁，抓在手中測試兩個軸混合控制的效果（注意安全），有問題回到“烤四軸”繼續調整，直至飛機在手中不會抽搐。
5、大概設置偏航參數（不追求動態響應，起飛後頭不偏即可），起飛後再觀察橫滾和俯仰軸向打舵的反應，如有問題回到“烤四軸”。
6、橫滾和俯仰ok以後，再調整偏航軸參數以達到好的動態效果。
*/
/*
【過程詳解】
1、要在飛機的起飛油門基礎上進行PID參數的調整，否則“烤四軸”的時候調試穩定了，飛起來很可能又會晃盪。
2、內環的參數最爲關鍵！理想的內環參數能夠很好地跟隨打舵（角速度控制模式下的打舵）控制量。
在平衡位置附近（正負30度左右），舵量突加，飛機快速響應；舵量回中，飛機立刻停止運動（幾乎沒有回彈和震盪）。
2.1首先改變程序，將角度外環去掉，將打舵量作爲內環的期望（角速度模式，在APM中叫ACRO模式，在大疆中叫手動模式）。
2.2加上P，P太小，不能修正角速度誤差表現爲很“軟”傾斜後難以修正，打舵響應也差。P太大，在平衡位置容易震盪，
打舵回中或給干擾（用手突加干擾）時會震盪。合適的P能較好的對打舵進行響應，又不太會震盪，但是舵量回中後會回彈好幾下才能停止（沒有D）。
2.3加上D，D的效果十分明顯，加快打舵響應，最大的作用是能很好地抑制舵量回中後的震盪，可謂立竿見影。
太大的D會在橫滾俯仰混控時表現出來（儘管在“烤四軸”時的表現可能很好），具體表現是四軸抓在手裏推油門會抽搐。
如果這樣，只能回到“烤四軸”降低D，同時P也只能跟着降低。D調整完後可以再次加大P值，以能夠跟隨打舵爲判斷標準。
2.4加上I，會發現手感變得柔和了些。由於筆者“烤四軸”的裝置中四軸的重心高於旋轉軸，這決定了在四軸偏離水平位置後
會有重力分量使得四軸會繼續偏離平衡位置。I的作用就可以使得在一定角度範圍內（30度左右）可以修正重力帶來的影響。
表現打舵使得飛機偏離平衡位置，舵量回中後飛機立刻停止轉動，若沒有I或太小，飛機會由於重力繼續轉動。
3、角度外環只有一個參數P。將外環加上（在APM中叫Stabilize模式，在大疆中叫姿態模式）。打舵會對應到期望的角度。
P的參數比較簡單。太小，打舵不靈敏，太大，打舵回中易震盪。以合適的打舵反應速度爲準。
4、至此，烤四軸”效果應該會很好了，但是兩個軸混控的效果如何還不一定，有可能會抽（兩個軸的控制量疊加起來，
特別是較大的D，會引起抽搐）。如果抽了，降低PD的值，I基本不用變。
5、加上偏航的修正參數後（直接給雙環參數，角度外環P和橫滾差不多，內環P比橫滾大些，I和橫滾差不多，D可以先不加），
拿在手上試過修正和打舵方向正確後可以試飛了（試飛很危險！！！！選擇在寬敞、無風的室內，1米的高度（高度太低會有地面效應干擾，
太高不容易看清姿態且容易摔壞），避開人羣的地方比較適合，如有意外情況，立刻關閉油門！！！
5.1試飛時主要觀察這麼幾個方面的情況，一般經過調整的參數在平衡位置不會大幅度震盪，需要觀察：
5.1.1在平衡位置有沒有小幅度震盪（可能是由於機架震動太大導致姿態解算錯誤造成。也可能是角速度內環D的波動過大，
前者可以加強減震措施，傳感器下貼上3M膠，必要時在兩層3M泡沫膠中夾上“減震板”，注意：鐵磁性的減震板會干擾磁力計讀數；
後者可以嘗試降低D項濾波的截止頻率）。
5.1.2觀察打舵響應的速度和舵量回中後飛機的回覆速度。
5.1.3各個方向（記得測試右前，左後等方向）大舵量突加輸入並回中時是否會引起震盪。
如有，嘗試減小內環PD也可能是由於“右前”等混控方向上的舵量太大造成。
6、橫滾和俯仰調好後就可以調整偏航的參數了。合適參數的判斷標準和之前一樣，打舵快速響應，舵量回中飛機立刻停止轉動（參數D的作用）。
至此，雙環PID參數調節完畢！祝爽飛！
*/