在嘗試制作這個四旋翼飛控的過程中，我感觸頗多，在整理了思緒之后，把重要的點一一記下來，希望給同樣喜歡飛行器的朋友以借鑒。

這個飛控是基于STM32，整合了MPU6050，即陀螺儀和重力加速計，但沒有融合電子羅盤。

另外，四旋翼飛行器的運動方式請百度百科，不太復雜，具體不再贅述。

這是飛控程序的控制流程（一個執行周期）：

比較重要的地方：

1.i2c通信方式

因為我不是學電類專業，最開始對i2c這些是沒有一點概念，最后通過Google了解了一些原理，然后發現STM32的開發庫是帶有i2c通信的相關函數的，但是我最后還是沒有用這些函數。

我通過GPIO模擬i2c,這樣也能獲得mpu6050的數據，雖然代碼多了一些，但是比較好的理解i2c的原理。

STM32庫實現的模擬i2c代碼（注釋好像因為編碼問題跪了）：

2.mpu6050

然后用寫好的模擬i2c函數讀取mpu6050，根據mpu6050手冊的各寄存器地址，讀取到了重力加速計和陀螺儀的各分量;

傳感器采樣率設置為200Hz,這個值是因為我電調頻率為200Hz，也就是說，我的程序循環一次0.005s,一般來說，采樣率高點沒問題，別比執行一次閉環控制的周期長就行了；

陀螺儀量程±2000°/s，加速計量程±2g， 量程越大，取值越不精確；

這里注意，由于我們沒有采用磁力計，而陀螺儀存在零偏，所以最終在yaw方向上沒有絕對的參考系，不能建立絕對的地理坐標系，這樣最好的結果也僅僅是在yaw上存在緩慢漂移。

3.互補濾波

融合時，陀螺儀的積分運算很大程度上決定了飛行器的瞬時運動情況，而重力加速計通過長時間的累積不斷矯正陀螺儀產生的誤差，最終得到準確的機體姿態。

這里我們采用Madgwick提供的UpdateIMU算法來得到姿態角所對應的四元數，之后只需要經過簡單運算便可轉換為實時歐拉角。感謝Madgwick大大為開源做出的貢獻。

4.獲取期望姿態

也就是遙控部分了，讓用戶介入控制。

本著拿來主義的原則，用上”圓點博士開源項目”提供的安卓的開源藍牙控制端。

圓點博士給出了數據包格式，同過HC-06藍牙模塊接連到STM32串口1，再無線連接到控制端，這樣我們就可以獲得控制端不斷發送的數據包了，并實時更新期望姿態角，這里只需要注意輸出的姿態角和實時姿態角方向一致以及數據包的校驗就行了。

5.PID控制算法

由于簡單的線性控制不可能滿足四軸飛行器這個靈敏的系統，引入PID控制器來更好的糾正系統。

簡介：PID實指“比例proporTIonal”、“積分integral”、“微分derivaTIve”，這三項構成PID基本要素。每一項完成不同任務，對系統功能產生不同的影響。

以Pitch為例：

error為期望角減去實時角度得到的誤差；

iState為積分i參數對應累積過去時間里的誤差總和；

if語句限定iState范圍，繁殖修正過度；

微分d參數為當前姿態減去上次姿態，估算當前速度（瞬間速度）；

總調整量為p,i,d三者之和；

這樣，P代表控制系統的響應速度，越大，響應越快。

I，用來累積過去時間內的誤差，修正P無法達到的期望姿態值（靜差）；

D，加強對機體變化的快速響應，對P有抑制作用。

PID各參數的整定需要綜合考慮控制系統的各個方面，才能達到最佳效果。

輸出PWM信號：

PID計算完成之后，便可以通過STM32自帶的定時資源很容易的調制出四路pwm信號，采用的電調pwm格式為50Hz，高電平持續時間0.5ms-2.5ms；

我以1.0ms-2.0ms為每個電機的油門行程，這樣，1ms的寬度均勻的對應電調的從最低到最高轉速。

至此，一個用stm32和mpu6050搭建的飛控系統就算實現了。