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