用了好些天才看完一個工程代碼，北京中科浩電（好像是這個名字）的無人機源碼，msp430的闆子，imu是mpu6050，定高用了氣壓計spl06，遙控傳輸資料用的nrf24l01，開發平台IAR，小型的無人機沒有拍攝功能，沒有磁力計，沒有光流傳感，應該可以作為無人機控制的入門手段，用來了解控制流程和方法，對時序和原理的了解，以及一些必要的算法的運用。
不知道該按照什麼順序來整理，因為自己現在也還順不清，隻能想到什麼寫什麼了。。。盡量通俗
一、主函數、核心函數和各個部分的初始化
主函數就很簡單，雖然看過實際工程不多，但好像主函數都要很簡潔？
首先關中斷，然後運作硬體初始化函數，然後開中斷
再然後就是while 1的一個循環，循環裡隻有一個函數PollingKernel()，意思是對核心函數進行輪詢。
然後就沒了。
先看初始化函數裡幹了啥，複制代碼看起來太麻煩，直接上圖了：
可以看到也是調用了各個部分的初始化函數，具體包括：
系統時鐘（統一時鐘系統，一般不需要我們改動）、
IIC通信協定（IO口配置）、
電機（其實就是四個引腳和時鐘是PWM輸出配置）、
LED（LED主要作為狀态訓示燈，比如準備狀态、鎖定狀态等等）、
MPU6050（對裡面的一些配置寄存器指派，一般是預設值，應該不需要我們改）
spl0601（配置各種寄存器，同時對管理氣壓資料的全局變量進行初始化，比如校準參數、對地面海拔進行讀取等等）
NRF24l01也就是無線收發部分（這個子產品就有點複雜，寄存器控制字和位址我現在都還沒看懂，而且也懶得看（）直接看代碼就是設定首發位址、設定模式、以及管理nrf子產品的全局變量的初始化等等。。。）
地面工作站初始化（對一個地面資訊結構體進行一些初始化，好像所有資訊在這個裡面都有提及）
PID初始化（對角度角速度pid的一些參數進行設定）
這裡還把模式設定成了Altitude_Hold，定高模式？
序列槽和時鐘（就不說了）
好初始化完了，開始進入主輪詢函數PollingKernel（）。
還是貼代碼吧：
void PollingKernel()
{
if (Thread_3MS)
{
Thread_3MS = false;
GetMPU6050Data();
WZ_Est_Calcu(0.003f);
ATT_Update(&g_MPUManager,&g_Attitude, 0.003f);
FlightPidControl(0.003f);
MotorControl();
}
if (Thread_8MS)
{
Thread_8MS = false;
GetAngle(&g_Attitude);
}
if (Thread_10MS)
{
Thread_10MS = false;
PollingNRF();
PollingGCS();
}
if (Thread_20MS)
{
Thread_20MS = false;
// //氣壓計資料采集OK
UpdateSPL06Info();
// //更新和選擇Z軸觀測量
// WZ_Obs_Calcu(0.02f);
// //資料融合
WZ_Fix_Calcu(0.02f);
// //高度控制
ALT_Ctrl(0.02f);
}
}
實質上是
1、每3ms：
更新一次mpu6050的資料，
預估下一時刻的高度和速度資料（沒太看懂這個操作），
解算出目前姿态資訊（三個歐拉角），以及更新出目前Z軸上的加速度（這裡沒看懂怎麼做的），
接下來是個狀态機，狀态轉換大概流程是這樣的：
a、開始是等待狀态，如果解鎖了，（解鎖标志位為1），進入下一狀态，
b、下一狀态即準備狀态，在這個狀态中，重置pid，把偏置、偏差、積分、期望值、測量值什麼的統統置零，航向角因為沒有磁力計，怎麼放的哪裡就是基準（0），代表姿态的四元數也統統置零。這些東西重置好之後，脫離準備狀态，進入下一狀态，姑且叫進行狀态，
c、進行狀态中，主要任務也是對pid的一些量進行設定：
裝入三個角速度的測量值（物坐标系，直接陀螺儀測出來的）、裝入橫滾角和俯仰角的角度值（解算之後得到的）、裝入航向角的期望值（這裡還沒太明白，不過應該不怎麼變）、利用串級pid處理三個角度和角速度（等下再講什麼是串級pid）、
d、最後一個狀态是結束狀态，在這個狀态裡pid被置零，然後跳回等待狀态。在進行狀态中，是不會自動跳進結束狀态的，結束狀态的跳轉條件由其他條件觸發，比如突然鎖定等等
最後一步，前面姿态更新了，pid也算出來了，接下來要去改變電機狀态了。這裡仍然是狀态機，甚至跟其前一個函數非常類似，流程如下：
a、等待狀态1，如果解鎖了，就跳到等待狀态2中，一鍵起飛（應該是這個意思）标志置零，高度鎖定标志置零（這些标志是一組全局變量，用來改變和描述飛機狀态的），
b、進入等待狀态2中，如果此時并未設定一鍵起飛，也沒動油門，那就說明還不想起飛，進入進行狀态。否則如果設定了一鍵起飛，那麼就把高度鎖定标志位置位，意思是自動定高，也進入進行狀态
c、進行狀态中，先設定一個臨時的變量temp，然後會判斷飛機自身的狀态，如果是Stabilize_Mode（自穩模式），油門-1000作為準備給pwm輸出的值，也就是傳給temp。如果是Altitude_Hold（定高模式），将一個全局變量中的油門值作為準備給pwm輸出的值，一樣是傳給temp。這兩者的差別在于，自穩模式是自己控制高度，即取決于你遙控的油門，定高模式是自動控制高度，使用者無法去控制，它會自己計算出油門值以到達設定的高度。
然後四個螺旋槳處加的pwm怎麼确定呢？四個值都要以這個temp為基礎，加以微調。為什麼要微調？因為temp隻跟高度有關，想要無人機姿态平穩或者姿态依據控制做出改變，那麼必須各自在temp上疊加一部分，原理是橫滾和俯仰運動要相鄰轉速改變，航向角變化要對角的轉速一起改變，有大體的了解就夠了，具體是這樣的：
這樣一來四個電機要輸出的值（也就是占空比）就确定了，隻要按照這個去改變TIM的比較寄存器就可以了。。。
d、同樣是退出狀态，進入這個狀态後，電機輸出清零，無人機停止工作。
2、每8ms，
運作一次GetAngle函數，看名字也知道，是對目前無人機三個姿态角進行解算，以及求物坐标系三個坐标軸相對于地坐标系Z軸的分量，後者對了解飛行狀态沒有啥意義，不懂也沒關系，實質上是程式中用來求解無人機在地坐标系Z軸方向的加速度的過程變量。
3、每10ms
運作一次地面工作站和NRF的輪詢函數：
PollingNRF();
PollingGCS();
至于輪詢裡幹了些什麼，就不仔細寫了，隻需要知道是和通信有關的，地面站和無人機肯定要在不斷地交換資料對吧。
4、每20ms
更新一次目前的高度資訊，也就是目前高度height
然後調用WZ_Fix_Calcu（）函數，算是濾一下波？用估計量修正一下觀測量，這部分才注意到，具體等下次再看高度控制的時候詳細分析。這部分确實算是目前遇到最難的地方，甚至比姿态解算還難一點，涉及到高度速度和加速度的測量、估計、修正，各種觀測值估計值等等，過程中還加了莫名其妙的pid。。。
但總之，這部分主要與定高有關，給定期望高度，算法會不斷更新油門值，motorcontrol函數輪詢時，如果模式是定高，就會把這個值輸出給電機，達到了自動控制高度的目的。
嗯。。。第一部分算是整理完了，但實際上整個控制流程也基本全了（？），剩下就想到什麼寫什麼了，目前還存在的問題就有1、剛剛提到的高度控制部分的代碼，要仔細分析出來，2、然後就是通信部分還比較薄弱，序列槽通信還好，但無線通信這部分，子產品太難上手，代碼也很迷，有機會要動手做實驗，把各種通信都能熟練使用，3、循迹和識别部分，内容估計還是會有一些，但應該不會太難了，先看一遍新飛機的代碼再說