浅谈飞控的软件设计（across写的）
關注across很久了，最近發現了它得CSDN，發現了這篇文章，感覺不錯，轉載保存。
摘自：https://blog.csdn.net/hz770495569/article/details/86570978
淺談飛控的軟件設計
?
across_drone 2019-01-21 11:12:34 1923 收藏 12
分類專欄： 飛控那些事兒
最后發布:2019-01-21 11:12:34首發:2019-01-21 11:12:34
版權聲明：本文為博主原創文章，遵循 CC 4.0 BY-SA 版權協議，轉載請附上原文出處鏈接和本聲明。
本文鏈接：https://blog.csdn.net/hz770495569/article/details/86570978
版權
寫在前面
開這個專欄的目的主要是深感自己對飛控軟件、算法的知識點過于雜亂，很久沒有進行系統的總結了，因此決定寫幾篇文章記錄一些飛控開發過程的知識點。主要是針對一些軟件、算法部分進行討論，如內容有錯誤，歡迎指出。
2019.03.02更新
距離專欄的第一篇文章已經快2年半了，最近在回看文章的時候，發現有些地方寫得不盡如人意，或亦是之前的技術水平不足，導致對一些問題的理解不深刻。由此，萌生了將已有文章進行更新的想法，同時也不忘初衷，自己能同時回顧一些知識，畢竟年紀大，工作中不常使用便會遺忘。
?
1 飛控軟件的基本模塊
無人機能夠飛行主要是依靠傳感器系統獲取位姿信息并反饋到微處理器進行控制系統的運算。所以飛控軟件設計主要負責搭建合理軟件流程，使各功能模塊協調有效的工作。
一個飛控系統的基本工作主要有：
1、CPU接收遙控器的操作指令和傳感器信號；
2、傳感器的數據處理和數據融合算法運算，得到位置、姿態信息；
3、根據控制指令完成相應的控制器（姿態、位置）計算，得出控制量并輸出到電機驅動；
?
2 軟件設計方法的討論
剛接觸飛控的時候，實驗室在設計之初，為了方便快捷，軟件系統的編寫采用前后臺操作的方式。這個方式的應用程序是在放在mian主函數里面無限循環，調用相應的處理子函數。這稱為后臺程序。而前臺程序指的就是中斷程序處理異步觸發事件的程序。故前臺程序稱為中斷級程序，而后臺程序稱為任務級程序。因此有些固定周期執行的任務都要靠中斷服務程序來完成，以保證時間的精確性。但是在中斷處理程序中只標記事件的發生，不做任何處理，轉而由后臺系統調度處理，這是為了避免在中斷程序執行時間過長影響后續和其他中斷事件。
這種設計方法的優點：
1、實現簡單，特別是對于筆者這樣的編程渣，照著stm32的庫函數寫代碼，也可簡單實現；
2、類似單片機的編程，沒有OS，因此對CPU的性能要求不算高，不太關注ROM/RAM；
3、如果設計得當，相較于帶OS的飛控，系統運行更加穩定，聽說很多工業級的飛控是不帶OS的；
缺點：
由于是用在飛行控制系統中，對整個系統的實時性有著很高的要求，如果邏輯和時序出現偏差，將出現無法估計的嚴重后果。而在初始開發過程中，發現采用此前后臺系統帶來兩大問題：
1、設計不當的話，比如某個周期的函數執行超時，后面所有的程序都會受到影響。如果飛控程序執行時間變得不夠準確，不利于對飛行器的控制，嚴重時發生飛機失控的現象。
2、移植性和擴展性差，給整個程序后續改動和維護帶來不便，由于各種任務都是相關的子函數，往往一個任務需要調用多個子函數。在程序改動或者維護的時候變得非常繁瑣復雜。經常由于忽略某一細節而導致功能無法實現，最后導致程序的可讀性降低，不利于他人做程序修改。
最近幾年也接觸了一些開源飛控，看了有關帶OS的飛控設計。這種設計方法是在某一操作系統上進行二次開發，OS通過一個內核的調度來管理CPU，使得所有的模塊也就是任務都能正常運行，達到相對意義的“并行”。同時采用基于優先級的可剝奪性調度算法來保證實時性。RTOS 將應用層軟件分成多個任務，簡化了應用軟件的設計，同時使得飛行控制的實時性得到保證。
個人偏好，其實比較喜歡裸機的方案。飛控其實說到底不是一個軟件功能特別復雜的產品，所以從開發角度來講，事先定義好相應的功能，沒必要經常的擴展設計，裸機代碼在平時的調試也有一定的優勢。飛控在整個飛行平臺的開發中，可以專注于負責飛行部分，可與其他設備進行通信，接收飛行指令即可。
?
3 完整的飛控系統組成模塊
當設計一個商業飛控的軟件時，就不僅僅是讓飛機飛起來那么簡單了，也就是說軟件模塊除了基本要素外，還需有其他擴展，如下圖所示。
?
4 飛控數據流分析
當熟悉了飛控系統中常用的軟件模塊，那這些模塊互相之間又是怎樣的關系？又是如何互相通信，各自需要什么數據來完成飛行任務？
以pixhawk飛控的原生固件為例，如下圖所示。這圖是很早期的代碼記錄下來的，與最新的代碼相比，有些誤差，不過差不多，能解釋問題，所以筆者懶得更新，重新畫圖了。
這里補充一下，
5 示例分析
掌握了以上所描述的幾個知識點，這樣基本上在初次閱讀一份飛控代碼時，能有起碼的認知了。下面簡單介紹兩款開源的飛控代碼，都是網上找的代碼，主要看下軟件架構。
5.1 恒拓開源飛控
基于MDK的開發環境，使用C語言，基于STM32的官方庫。
代碼結構：
STARTUPCODE：stm32的啟動文件；
StdPeriph_Driver：基于3.5版本的庫函數的驅動文件；
USB-FS-Device_Driver：USB設備驅動文件；
usb_virture_com：USB的板級支持驅動；
Driver：板級驅動層，包含一些總線和外設的驅動程序；
Modules：傳感器模塊的驅動程序；
Algorithm：算法程序，包含濾波、數學庫等；
Function：飛行應用層，關鍵模塊，比如姿態估計、姿態控制等；
User：主程序和中斷應用程序；
ANO_DT：支持匿名地面站協議；
Heigh：高度控制程序；
整個代碼的模塊化非常細致，比較清晰。
?
代碼設計就是前面所講的裸機代碼的一般實現方法。
先看mian文件：
非常簡單，上電進行各種初始化，然后大循環，循環執行任務調度。
下面看下loop的函數內容。
將整個飛控代碼分成了幾個周期分別為5ms，10ms，25ms、50ms和100ms的任務。而每個任務的時間標志flag是由一個時間片函數進行管理的。設了一個tick節拍，2.5ms一次，所以比如計數達到2次，則5ms的定時任務即可執行。
而這個時間片函數是一個定時中斷，每隔2.5ms執行一次。
這種程序設計方法如下圖所示。定時中斷的影響只在任務調度模塊里起作用，依次讓不同的任務按不同的周期進行執行。要注意的是所設計的每個任務運行時間不能超過設定的周期。
???????? 筆者也看了國內有名的匿名飛控，也是同樣的調度設計方法，另開源ardupilot飛控，因歷史原因，是繼承APM飛控而來，也是采用這種類OS的偽調度器方式，代碼全部順序執行，根據定時的計數標志去分別安排飛控任務。
?
5.2 PX4飛控 - Pixhawk原生固件
開源PX4飛控相對復雜多了，很多軟件的細節筆者也不甚了解，所以就簡單描述下。
PX4飛控從軟件架構上可以分為四層。在每一層里，各個驅動程序或上層的控制/估計算法都是一個獨立模塊，能夠在運行期間互相通信。這種模塊化的設計不僅有助于支持更多機型(因為不存在特定機型的主循環)，同時使得代碼具有高度的可移植性。
?
?
綜上，飛控的裸機方案設計，常用的是分時調度機制。將整個系統時間分成若干時間片，用ID進行標識（通常用頻率標志），每個時間片內執行相應的功能模塊。這就需要事先合理的分配各個軟件任務，保證在相應的時間片內不能超時。
?
飛行模式與狀態機設計
以上描述了飛控設計必要的功能模塊，以及互相之間的數據流是如何工作的。接下來就是有關飛行模式與狀態機的設計。
飛行模式，按照功能需求，進行自定義設計。飛行模式與飛控內部的狀態切換以及控制器切換具有密切聯系，因此需要事先擬定好，有哪些飛行模式。進而，根據飛行模式，設計狀態機，即各種飛行模式之間如何進行切換，包括由地面的加鎖狀態，到空中的狀態如何進行切換。
有關飛行模式與狀態機的設計，有個視頻講解：
https://www.bilibili.com/video/av45087166/
6 幾個思考
飛控軟件中哪些任務優先級高？
很顯然就是前面所講的基本模塊，包括遙控輸入、傳感器數據讀取、姿態估計/控制這一類。當然這也設計到飛控的各個控制回路所需的更新速率問題等，后續會詳細闡述下飛控中的各個控制回路。關于優先級可以參看px4飛控：
1.（中斷級）快速傳感器驅動程序
2.看門狗/系統狀態監控
3.驅動器輸出（PWM輸出驅動器線程，IO COMMS發送命令線程）
4.姿態控制器
5.更新速率慢的傳感器驅動程序（不能阻塞姿態控制器）
6.航路/位置控制器
7.默認優先級 - 通用用戶代碼，shell命令等
8.日志記錄，參數同步程序
9.空閑進程
綜上，越底層的控制器和關鍵數據的獲取部分，優先級最高，因為這個是飛行器安全的保障。
?
飛控軟件的更新周期設計？
飛控中有兩個基本的計時：更新周期和延遲。要想獲得良好的系統性能，就必須減少延遲。一般情況下，延遲甚至比更新周期更重要—因為大延遲產生相移。延遲造成的相移會讓你輸出錯誤的控制量。
所以飛控中，除了設計一個固定的更新周期，還需關注延遲。一個固定頻率的控制循環，有時會因為延遲，導致性能很差。舉個例子：假設你以100Hz采樣速率讀取傳感器，狀態估計和控制器也都是100Hz。那每一步輸入的最大延遲/延遲是10ms？?????
另外，如果分別設計3個循環，傳感器讀取，狀態估計，控制或者將3者放在一個任務循環里，有什么區別？
500hz的讀取數據，狀態估計，再進行控制和500hz讀取數據，狀態估計，250hz控制兩種方式有區別嗎？這3個問題，目前筆者也沒有確切的答案，因此就不寫了，留作大家思考，可以互相交流看法。
有關更新周期，主要部分是數據融合算法和控制器的頻率設計。基于以下原則。
一般來說，傳感器的采樣頻率盡可能越高越好，比如gyro數據1000hz采樣，實際在姿態估計算法中是200hz讀取，這樣多出來的數據點能用來做濾波處理，盡可能的保存真實信息。
控制器頻率則一般根據系統的帶寬決定，一般來說，控制頻率是帶寬的5倍左右（理論推導不詳述了，請自行查閱書籍）。
在多旋翼飛行器中，一般包含了4個控制回路，角速度控制、角度控制、速度控制、位置控制。以角速度控制為例，前面gyro濾波器設置的截止頻率為30hz，所以最好角速度控制達到150hz以上。當然，這些沒有標準的設置，基本合理就可以，底層的角速度控制頻率盡可能高些，200~400左右即可
測量函數運行時間？
在飛控算法中，需要用到更新周期這個變量，當然，如果算法運行是固定周期，則變量的值就是所設定的周期，還可以代碼自行測量，如下所示。
???????? 當然，對算法而言，采用固定的采樣率還是計算出來的采樣率，個人覺得理論上來講，計算出來的更準確，應該是更好，比如對控制器的積分計算等會有影響。當然如果固定的周期不出錯，并無太大影響，因為時間即使有些許誤差，也是比較小的數值。
測量固定周期？
筆者曾經在編寫裸機飛控代碼時，想確認下所設計的固定周期是否準確，采用了一個笨辦法。在周期里，配置一個輸出引腳，每執行一次，引腳的電平取反，然后用示波器觀察波形，看顯示的周期是否與設定的一致，是否會波動變化。如果波動變化很大，說明代碼中有任務超時了，整個代碼運行出錯。
還有一個簡單的辦法，就是在任務中設置一個計數標志，然后計時，看兩者數值是否相差很多。比如50hz的計數標志，計時10s，則計數標志應該是500左右，考慮人工的誤差，相差不會很大，如果相差過大，則說明任務超時。
總結
以上是生活随笔為你收集整理的浅谈飞控的软件设计（across写的）的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： Fineplus 1.0(QQ完美助手)
- 下一篇： CE中搜索汉字 + VB 转换汉字Uni