通過部落格記錄分享自己的研究過程，這是第一篇，先寫過去的總結，再寫接下來的思路。

過去的總結

飛行控制器的選擇與學習

選擇TMS320F28335，主要是因為DSP執行速度快；整合FPU協處理，善於做大量浮點型資料運算；外設資源豐富，足夠滿足飛行器的需求。

底層驅動編寫

底層驅動程式碼編寫難點： TI官網為每個外設只提供一個或兩個例程，很難滿足實際需求；不提供庫函式，只能用暫存器配置外設工作；中文資料少，只能看官方的資料手冊。

底層驅動程式碼編寫收穫：英語閱讀能力，資料搜尋能力，除錯方法的積累，對硬體協議底層的認識。

①4路PWM輸出控制電機，週期400Hz，脈寬1000us-2000us變化

②捕獲八路遙控接收脈衝，週期20Hz，脈寬1000us-2000us

③I2C讀取MPU6050加速度計/陀螺儀

④SPI驅動NRF24L01

⑤定時器2.5ms程式執行週期

⑥SCI與上位機通訊

⑦cmd檔案編寫

以上每個驅動除錯至少1星期，主要方法是：將任務細化，逐步除錯、驗證；阿莫論壇、21IC論壇、TI技術支援論壇等各大論壇查詢資料；讀中文、英文手冊理解每個外設工作過程，進而理解如何配置暫存器；尋找快捷除錯方法。

例如SPI驅動NRF24L01。除錯思路是將問題細化為：第一層SPI協議的編寫(驗證方式是先寫NRF24L01一個暫存器，再讀NRF24L01暫存器，看是寫入和讀出是否一樣)和NRF24L01暫存器設定(驗證方式是傳送和接收的位元組一致)；第二層若寫進去和讀出來的資料不一樣，是寫有問題還是讀有問題？所以需要單獨的除錯寫和讀，讀可以用讀出來的資料與寫進去的資料對比，但如何證明寫沒有問題？所以需要明白SPI協議是什麼意思，SPI分四根線分別是使能、時鐘、輸出和輸入，那麼用示波器顯示時鐘和“寫”的波形，看每個時鐘對應的“寫”高低電平進而確定是否和寫入的位元組一致；第三層若接收不到位元組，是傳送有問題還是接收有問題？所以需要先確保有一方一定沒問題，NRF24L01能否能通訊與什麼晶片配置無關，至於怎麼配置NRF24L01內部暫存器有關，而stm32已經有了NRF24L01的例程，故先調通stm32發stm32收，再DSP發stm32收，接著stm32發DSP收，最後DSP發DSP收。第四層單個位元組除錯好了再除錯飛控放一直髮，與上位機連線方一直收。

再例如I2C讀取MPU6050姿態。除錯思路將問題細化為：第一層I2C協議編寫(驗證方式是寫一個位元組再讀一個位元組，看寫和讀的位元組是否一致)、MPU6050暫存器的設定(是否能讀取正確的加速度和陀螺儀輸出)、上位機觀察MPU6050的姿態；第二層是用示波器單獨除錯I2C的寫和讀，此處難點在於TI例程只給出了16位位元組的自檢例程，需要看英文中文手冊，理解每個暫存器的每個位的含義，對應的I2C配置的哪個具體過程，將程式改寫成只寫8位非自檢模式；第三層因為MPU6050已經很常用了，網上查詢MPU6050的例程，先用stm32配置成功，再用DSP模仿stm32配置，看讀取的資料是否隨模組轉動而變化；第四層用已經編寫好的SCI程式分別按照上位機規定的協議傳送連續位元組串，用已經編寫好的IMU姿態解算程式碼解算模組當前姿態，看能否MPU6050模組轉動，上位機的飛行模型是否一樣轉動。

CMD檔案的編寫：主要是因為用模擬器燒寫程式是放入RAM中直接執行，RAM掉電資料就會丟失，所以實際工程中需要燒寫進flash，但是flash的執行速度比RAM執行速度慢5倍左右，對於無人機這種實時控制系統是不可接受的。所以實際過程是先燒寫程式進flash，在程式執行之前先把flash中的程式全部copy到RAM中，最後直接在RAM中執行。要完成上述過程，需要對DSP儲存空間有個深入的認識。其實DSP將外設的暫存器全部儲存在一塊區域，不能改寫，而其他RAM和flash都可以由使用者規定程式碼、常數、靜態變數和動態變數放在哪個地址，最後可以由.map檔案檢視每個變數每段段程式碼實際儲存的地址。

姿態解算

這裡姿態解算演算法直接採用了網上開源的基於Mahony的互補濾波演算法，演算法原理是：加速度計提供長期穩定的線性加速度輸出，陀螺儀提供短期的快速角速度輸出，而姿態演算法的原理即為對三軸角速度進行積分得到尤拉角，由於陀螺儀的漂移和偏置特性，導致積分誤差不斷增大，因此需要加速度計來實時校正。姿態解算所要做的工作便是描述載體座標系與導航座標系之間的相對關係，而描述這種關係的方法目前來說有四種：四元數、方向餘弦矩陣 DCM、尤拉角和等效旋轉向量。其中以四元數的計算量小、無奇點問題而被廣泛應用在工程領域中。

飛控硬體設計

    先用DSP開發板作為飛控板，用杜邦線引出PWM(控制電機)、I2C介面(MPU6050讀取姿態)、CAP(遙控接收器)、NRF24L01(備用以後無線傳輸資料)、SCI(串列埠傳送資料給上位機)，將上述系統除錯到轉動飛機，上位機飛機模型可以一樣的轉動。將上述介面全部用排針引出，再查詢資料設計DSP最小系統，30MHz晶振、JATG口、電源晶片(選擇官方推薦的tps767d301)、復位電路(實驗證明這個可以不要)、DSP  bootload(燒寫)模式選擇電路，其他的還需要開關、濾波電容、led指示燈、AD取樣電壓的電路。以上均為飛控板上需要的東西，然後就是找封裝、畫原理圖、PCB佈局佈線(參考淘寶一種最省空間最容易佈線的DSP晶片佈局方式： DSP晶片的4角分別靠近飛控板的4邊)。考慮到電源晶片的散熱問題，特地將電源晶片底下的PCB銅箔裸露，以加大散熱面積。最後大小為6×4cm。PCB板打樣回來自己焊接，先焊接電源部分，以免焊接了其他部分電源電路出問題直接燒掉，電源沒問題後再焊接DSP晶片，因為DSP晶片176個引腳，非常難焊，所以優先焊接DSP，預防在焊接的過程中焊接時間過長把晶片燒壞，焊接過程是斷斷續續的(方便散熱)，並且在DSP晶片上蓋上棉花撒上水，焊好DSP最小系統後就可以下程式測試DSP有沒有工作。這個是第一版飛控，上面的感測器只有GY-86模組，可以用於飛行器飛行控制；第二版將加入GPS介面和光流介面，用於定高定點。最後經測試，第一版飛控第一塊PCB就成功，目前除錯的也是這塊飛控板。

除錯系統搭建

    因為是中型四旋翼飛行器，為了安全，先選用燒烤模式除錯，除錯飛行器邏輯(即飛行器倒下的那邊電機會轉，另一邊會停)。為了方便PID引數的除錯，選擇NRF24L01無線模組作為資料的“中轉站”，需要實現如下資訊流向功能：姿態資料流向DSP－＞NRF24L01(飛控板)－＞NRF24L01(stm32開發板) －＞stm32－＞上位機(串列埠)，PID引數流向上位機(串列埠)－＞stm32－＞ NRF24L01(stm32開發板) －＞NRF24L01(飛控板) －＞DSP。這個資料流的搭建主要在於資料通訊的協議不能亂，比如roll角度在DSP放在哪個數組裡寫入NRF24L01，在stm32通過NRF24L01接收在哪個數組裡，最後以上位機哪部分協議傳送給PC。

姿態控制

先是採用標準的PID控制方式，D項為誤差的微分項，當誤差定義為角度時，此時的D項即為角速度，而該類資訊恰好由陀螺儀提供，因此這種有陀螺儀參與的單環 PID 控制器也是一種常規 PID 變型寫法，但是飛行器無法穩定的飛行。

為了增加飛機的阻尼特性、提高飛機角度響應速度，便引入串級PID 控制器。該類串級 PID 控制器其實在很多國外開源飛控中已經被廣泛應用，比如Crazyflie、MWC、APM、PX4等姿態控制器無一例外都是串級PID。該串級PID控制器分為外環-角度控制器和內環-角速度控制器，分別控制了飛機的角度和角速度，而後者角速度相當於角度的導數，也就是說增加了系統的控制階次，使得飛機具有更強的阻尼效果。除錯串級 PID 的過程也更加深入理解了引數對於系統的效能影響：外環P 直接影響飛機跟蹤搖桿的速度，值越大，跟隨性越強，響應越快；內環 P 項直接影響飛機姿態的自調整速度，過大的 P 會導致飛機自振盪；內環 I 項與外環 I 項一般而言只保留一個，因為積分環節會破壞系統穩定性，當飛機存在角度靜差時，必須引入積分項消除靜差；內環 D 項屬於輔助項，過大的 D 項會導致 PID 輸出毛刺過多，直接導致電機頻換變速和高頻振盪。前述的姿態控制只限於水平面內的角度控制，偏航角的控制與姿態控制一致，用串級 PID 也可以達到較好的效果。但是實際飛行過程中發現偏航角的響應速度並沒有非常快，因此本人嘗試利用前饋控制提高響應速度：即遙控器輸入直連到執行機構輸出，實驗證明這種處理方法大幅度提升了偏航角的響應速度。

除錯定高

    定高常用的感測器為超聲波模組和氣壓計。雖然超聲波精度可達1-2cm，但只適應低空並且需要平面地形來反射超聲波，所以選用氣壓計MS5611。除錯主要分為3個步驟：①讀氣壓感測器資料②與加速度的二重積分互補融合得到可用的高度資料③用於串級PID控制模式得到PWM。因為純用加速度積分得到的高度長時間會漂移，所以需要氣壓計來補償矯正，主要控制思想是二階互補融合濾波：第一階是對融合後的高度微分得到速度與加速度計的一重積分得到的速度融合，得到可用的速度，作為串級PID內環控制輸入；第二階將氣壓計得到的高度與一階融合得到的速度進行積分再融合，得到可用的高度，作為串級PID的外環控制。主要程式框架已經搭建好了，主要問題是氣壓計會漂移，網上查詢原因，可能是氣壓計受光線影響或者氣壓計沒有被包起來，容易受周邊環境影響，所以下一步除錯將優化飛控板，將飛控板放在小盒子中，氣壓計上再蓋上黑色海綿。

除錯定點

    可選用的感測器有GPS和光流。GPS適用於開闊的環境，光流適用於近地面。想要實現的效果是飛行器高空用GPS定點，降落用光流定點，實現飛行器的自主飛行。目前定點仍在除錯，GPS選擇ublox公司的NEO-M8N模組，已用串列埠可以正確讀取GPS發回的經緯度等資訊。光流選用的是pixhawk的光流模組，上面集成了超聲波模組，光流處理演算法內部已經寫好了，只需要用I2C介面讀取速度和高度。控制方式也是串級PID：對水平速度積分得到水平位移作為外環輸入，外環輸出是期望姿態，速度作為內環輸入與外環輸出做差得到控制量。最重要的是算出相對地面位移，再補償回來。目前只獲取了光流模組的速度、位移(速度的積分)，這個也需要在下一版的飛控中做好介面。

除錯濾波

    現程式中的MPU6050、MS5611、HMC5883L等感測器原始資料濾波主要是滑動視窗濾波，還沒有對機械振動對感測器資料影響作頻域分析。已做好相關的準備工作：將需要分析的資料通過匿名上位機儲存成.CSV格式檔案，運用MATLAB匯入檔案資料，並畫出波形。進一步將使用MATLAB裡 的FFT函式分析機械振動帶來的是哪些頻率的干擾，進而針對此干擾設計相應的濾波器。

現在的研究思路

研究思路是基於pix開源平臺ardupilot程式碼繼續開發，學習C++、Nuttx作業系統，瞭解pix開源平臺結構和相關演算法，再做最新研究機器視覺、模式識別等。因為時間問題，不能面面俱到，把pix架構搞清楚後選擇想研究的部分深入。