使用通用MCU實現(xiàn)無人機飛行任務(wù)的快速二次開發(fā)

---TIDronePilot外部控制offboard模式介紹

無名小哥 2024年1月1日

1. 傳統(tǒng)飛控二次開發(fā)方法和主要存在的問題簡介

通過對前面幾講中《零基礎(chǔ)競賽無人機積木式編程指南》系列開發(fā)教程的學(xué)習(xí)可知，在以往TI電賽真題的學(xué)習(xí)訓(xùn)練方案中飛行任務(wù)代碼開發(fā)主要集中在Substask_Demo.c和Developer_Mode.c兩個程序文件，其中在Substask_Demo.c內(nèi)負(fù)責(zé)對具體飛行任務(wù)中每個階段的無人機的飛行動作、航點位置、目標(biāo)追蹤、巡航速度、目標(biāo)姿態(tài)、執(zhí)行機構(gòu)驅(qū)動(如蜂鳴器、激光筆、舵機、電機)等進(jìn)行流程化的設(shè)計，Developer_Mode.c內(nèi)用于實現(xiàn)自動起飛、降落、不同的飛行任務(wù)切換與執(zhí)行。

盡管飛控代碼內(nèi)的相關(guān)的API函數(shù)接口已經(jīng)實現(xiàn)得相當(dāng)完備，但上述開發(fā)過程的對于初次接觸整個飛控代碼這一系統(tǒng)工程的學(xué)習(xí)者來講，想要直接上手去二次開發(fā)仍然還有一定的難度，特別是在對飛控系統(tǒng)代碼框架（硬件驅(qū)動層、傳感器驅(qū)動、傳感器濾波、姿態(tài)解算、慣性導(dǎo)航、機器視覺、基本飛行控制實現(xiàn)、API函數(shù)、導(dǎo)航控制、SDK自主任務(wù)等）沒有整體的把握的情況下。

僅有C語言+單片機常用基礎(chǔ)芯片資源使用知識的初學(xué)者按照提供的真題案例和二次開發(fā)教程“依葫蘆畫瓢”去實現(xiàn)特定任務(wù)往往容易會出現(xiàn)一些常識性錯誤，有些錯誤會直接導(dǎo)致整個飛控系統(tǒng)的“崩盤”。下面以實現(xiàn)無人機在前進(jìn)的過程中搜尋到色塊后，對色塊進(jìn)行跟蹤這一任務(wù)為例子，將新手易錯的問題整理如下:

• 不理解單片機通過定時器實現(xiàn)控制器需要周期性執(zhí)行這一基本要求的重要性，如下圖中在5ms周期性任務(wù)中引入了延時函數(shù)的飛行任務(wù)中，開發(fā)者OS是希望延時函數(shù)上方的速度控制執(zhí)行10S，認(rèn)為加了延時之后會速度控制函數(shù)就會一直執(zhí)行。實則是程序會運行一次速度控制函數(shù)后，程序會在此處空轉(zhuǎn)等待，后續(xù)的高度控制、姿態(tài)控制、PWM輸出等得不到及時的執(zhí)行，只有在10S等待空窗期后才會繼續(xù)執(zhí)行后續(xù)的控制，在10S等待時間內(nèi)，飛控不會對自身的位置、速度、姿態(tài)進(jìn)行即時高頻（200Hz相對0.1Hz來講）的調(diào)整與修正，無人機處于間歇性“失控”狀態(tài)，飛行器的控制周期從5ms變成了10S，用此段代碼去控制你的無人機，慘烈的炸機也就變得不可避免。上述問題的根源是延時函數(shù)破壞了控制器的周期性執(zhí)行，連最基本的姿態(tài)控制都得不到保障，動搖了飛控系統(tǒng)的“國本”。
• 對飛控代碼的執(zhí)行流程與代碼產(chǎn)生實際作用理解錯亂，不會對任務(wù)按流程進(jìn)行拆分，將部分重復(fù)作用的代碼“雜亂堆砌”在一起，造成邏輯混亂，實際作用不明。如在下方代碼中色塊對準(zhǔn)函數(shù)內(nèi)部實現(xiàn)有光流速度控制函數(shù)，當(dāng)視覺模塊識別到了色塊時，case 1內(nèi)部的速度控制函數(shù)和色塊控制內(nèi)部的速度控制函數(shù)會順序執(zhí)行，即同一個控制周期5ms內(nèi)，速度控制函數(shù)執(zhí)行了兩遍，第一次運行的控制器輸出會被后續(xù)第二次運行的結(jié)果覆蓋掉，似乎第一次可以視為“無用”代碼，看似不影響最終的控制效果。但是事實是我們需要考慮到PID控制器的運算過程，同一個控制器一個周期內(nèi)執(zhí)行兩遍，相當(dāng)于積分I做了兩次運算，微分項不起任何作用。因為兩次計算過程中，當(dāng)反饋和期望不變的情況下，第一次的運算過程的偏差和第二次運算的偏差相等，并且第一次的偏差賦值給了上次的偏差，即微分項會恒等于0，即不管微分項參數(shù)D為多少，最終計算出來的微分項結(jié)果恒等于0。在PID三個參數(shù)都存在的情況下，某個控制器重復(fù)執(zhí)行亦會導(dǎo)致災(zāi)難性的BUG。
• 
• 
• 認(rèn)為控制代碼只需要執(zhí)行一次就能達(dá)到期望的控制效果，如果從A飛到B，AB兩點的距離為100cm，試想無人機需要在一個控制周期5ms內(nèi)就能實現(xiàn)A到B，假設(shè)無人機做勻加速直線運動，零初始狀態(tài)下無人機的加速度要到多少才能完成這一目標(biāo)，無人機的實際推重比是否能達(dá)到這個要求？如果按照這個加速度進(jìn)行加速無人機1S后是否會脫離地球軌道？
• 

• 另外由于飛控自身硬件資源有限，飛控自身外界的外設(shè)占用比較多，可供用戶它用的串口、PWM、IO資源不再富裕，使用起來使用捉襟見肘，常常需要外部MCU進(jìn)行擴展，既然都引入了外部MCU到無人機平臺中了，很自然的會想到用自己熟悉的MCU飛無人機飛行任務(wù)進(jìn)行開發(fā)。

• 綜上所述，針對新手初學(xué)者來講，面對略顯龐雜的飛控系統(tǒng)代碼，在進(jìn)行二次開發(fā)時，若寫出某些天坑級的BUG會導(dǎo)致無人機系統(tǒng)的整體崩潰，基本的姿態(tài)自穩(wěn)都得不到保障，就會出現(xiàn)災(zāi)難級的炸機事故。因此為了使得二次開發(fā)更加簡單、開發(fā)更加安全，有必要將和用戶二次開發(fā)相關(guān)的API函數(shù)、導(dǎo)航控制、SDK自主任務(wù)控制部分代碼的實現(xiàn)，單獨用一個通用控制器去實現(xiàn)，我們只需要在飛控端將已有的API函數(shù)接口進(jìn)行簡單整合，在SDK模式中新增加外部控制offboard模式就可以，這部分工作量很少，很多都是現(xiàn)成的我們在后面的教程中進(jìn)行介紹。

• TIVA飛控的硬件組成與系統(tǒng)框架

• TIVA飛控硬件系統(tǒng)包括飛控主板與外設(shè)兩部分組成，其中飛控主板上板載加速度計陀螺儀（帶溫控電路）、氣壓計傳感器，帶獨立按鍵和五向按鍵，配備OLED顯示屏進(jìn)行人機交互，帶蜂鳴器和RGB燈提示，板載的擴展接口包括8路串口、16路PWM、8個預(yù)留IO口、2組I2C口等。擴展接口用于外接機載端SLAM定位、offboard外部控制、光流、對地激光測距、GPS、機器視覺（如OPENMV/K210/樹莓派OPENCV）、激光雷達(dá)點云數(shù)據(jù)等。

• TIVA飛控軟件系統(tǒng)包括芯片底層與應(yīng)用相關(guān)驅(qū)動、傳感器數(shù)據(jù)采集、傳感器濾波、姿態(tài)解算、慣性導(dǎo)航、基礎(chǔ)飛行控制、自主飛行API函數(shù)、SDK開發(fā)者模式等。

  

• 運用通用單片機開發(fā)板實現(xiàn)對飛控外部控制

• 在傳統(tǒng)的開發(fā)模式中主要是在飛控代碼內(nèi)部對SDK開發(fā)模式中編寫飛行任務(wù)代碼去實現(xiàn)特定任務(wù)，這些開發(fā)流程在之前按的教程中有詳細(xì)的闡明。本教程需要解決的問題是如何將飛控軟件框架中的自主飛行相關(guān)API函數(shù)和SDK開發(fā)者模式的應(yīng)用層開發(fā)代碼放在外部的通用的MCU中去實現(xiàn)，比如盤古TI MCU系統(tǒng)板(已完成)、STM32F103系列核心板(已完成)、STC32G12K128開發(fā)板(完成度90%)等。由于外部通用MCU中需要實現(xiàn)的代碼量很少，對單片機處理資源和性能要求并不高，因此用戶完全可以用任何一款自己熟悉的單片機，高效率得心應(yīng)手的去開發(fā)，參照上述已實現(xiàn)的三款核心板方案，去自己實現(xiàn)飛控的外部控制這部分設(shè)計。為了方便后面表述，我們將帶有offboard外部控制的飛控稱為TIDronePilot(簡稱TPT/下位機/飛控端)，將外部通用MCU開發(fā)板稱為TIDronePlanner(簡稱TPR/上位機/應(yīng)用端)。

• TIDronePilot中offboard外部控制相關(guān)函數(shù)的實現(xiàn)

• TPT的實現(xiàn)工作量很小，就是在保留原來TIVA飛控所有功能的前提（仍然可以用傳統(tǒng)方案開發(fā)）下，主要新增加了一下三點：

  TPT發(fā)送飛控內(nèi)部估計出的三維位置、速度、加速度、角速度、姿態(tài)四元數(shù)、融合標(biāo)志等信息給外部控制端，TPR端解析到上述反饋數(shù)據(jù)后用于對無人機實現(xiàn)位置、速度等控制，并將最終的控制量串口打包發(fā)送給飛控端。

  

• TPT解析來自TPR端的姿態(tài)和豎直方向速度控制信息，作為offboard外部控制模式下對應(yīng)項的期望值。

• 在SDK模式中額外新增加了offboard模式，在此模式下飛控端的姿態(tài)期望、豎直方向速度期望來源于外部控制器的給定（TPT串口解析來之TPR的控制指令），同時也可以通過遙控器手動介入接管無人機的控制權(quán)限，可以實現(xiàn)不切換遙控器開關(guān)擋位的情況下對無人機的直接控制。需要注意的是飛控端切入SDK后offboard_start_flag會置1，飛控端會將此標(biāo)志為發(fā)送給外部控制端用于觸發(fā)外部控制端執(zhí)行其內(nèi)部的SDK任務(wù)。

至此用于實現(xiàn)offboard外部控制飛控端的主要工作就全部實現(xiàn)了，單從控制部分抽象來看offboard模式下飛控端“退化”成只執(zhí)行基本的飛行控制部分，主要包括姿態(tài)自穩(wěn)、部分定高(豎直速度、加速度)功能，飛控內(nèi)部并沒有運行對無人機的位置、速度進(jìn)行直接的控制的代碼，相當(dāng)于飛控在offboard外部控制模式下變成了一個“指哪打哪”、“一切行動聽指揮”嚴(yán)格響應(yīng)外部控制端TPR命令的被控對象。飛控負(fù)責(zé)保持自身姿態(tài)和豎直方向速度的穩(wěn)定，其它應(yīng)用層開發(fā)全部放在外部控制端TPR端去實現(xiàn)，新的開發(fā)模式下用戶可以不用在飛控TPT端修改代碼，把無人機作為一個整體被控對象去開發(fā)，自然也就不會出現(xiàn)第一節(jié)中天坑級BUG導(dǎo)致災(zāi)難性炸機的情況，在TPR端開發(fā)應(yīng)用層代碼不會影響飛控TPT端內(nèi)部的控制，任何情況下遙控器都可以直接對無人機的控制進(jìn)行接管。?

1. TIDronePlanner中應(yīng)用開發(fā)層相關(guān)函數(shù)的實現(xiàn)

TIDronePlanner作為飛控offboard外部控制模式下的上層控制器，其主要作用是實現(xiàn)原飛控中用于自主飛行相關(guān)API函數(shù)、SDK開發(fā)模式的功能，由于TPR應(yīng)用端內(nèi)部需要實現(xiàn)無人機水平方向的位置、速度控制，豎直方向的高度位置控制，自然需要飛控端TPT給應(yīng)用端TPR發(fā)送自身的位置、速度、姿態(tài)等無人機飛行狀態(tài)的反饋信息，TPR接收到反饋信息后用于無人機的位置、速度閉環(huán)控制，最終將TPR控制器的輸出封裝成相關(guān)API接口變量，通過串口通訊打包發(fā)給TPT飛控端，從而實現(xiàn)了整個飛控位置、速度、加速度、姿態(tài)角度、姿態(tài)角速度的系統(tǒng)的完整閉環(huán)控制。

?

在通用MCU系統(tǒng)板中實現(xiàn)TIDronePlanner的工作量同樣很少，除了對應(yīng)單片機資源如串口通訊、PWM輸出、按鍵、顯示屏、定時器調(diào)度驅(qū)動實現(xiàn)外，剩下的工作只剩下解析飛控端發(fā)送過來的狀態(tài)反饋信息、移植原飛控端位置控制、速度控制、SDK開發(fā)模式、API接口函數(shù)、串口打包發(fā)送API接口變量。

• 解析來自飛控端的狀態(tài)反饋數(shù)據(jù)，按照飛控發(fā)送時對應(yīng)數(shù)據(jù)協(xié)議幀來解析。

?

• 實現(xiàn)位置控制、速度控制、SDK開發(fā)模式、API接口函數(shù)，由于這部分在飛控內(nèi)部均有實現(xiàn)，直接移植過來就可以，此處不作過多介紹，在之前的教程中有對相關(guān)函數(shù)實現(xiàn)進(jìn)行講解。

?

?

這里需要注意的是雖然Auto_Flight_Ctrl函數(shù)在定時器內(nèi)周期性執(zhí)行，但SDK任務(wù)執(zhí)行與否取決于offboard_start_flag變量的值，當(dāng)用遙控器/ADC按鍵操作飛控端切入offboard模式后，此標(biāo)志位會被置1，Auto_Flight_Ctrl中的自主飛行任務(wù)才會執(zhí)行。?

• 串口打包發(fā)送API接口變量

?

?TIDronePlanner中實現(xiàn)部分還包括外設(shè)如舵機/電機、激光筆、蜂鳴器等設(shè)備的驅(qū)動，輔助傳感器如激光雷達(dá)點云、機器視覺等數(shù)據(jù)的接入，具體根據(jù)實際飛行任務(wù)要求來，由于TIDronePlanner的實現(xiàn)僅需要占用一個串口資源就可以實現(xiàn)其應(yīng)用層，因此通用MCU核心板的其它預(yù)留出來的資源就可以供二次開發(fā)使用，此舉能有效解決傳統(tǒng)開發(fā)方案中，資源不夠用的情況。

1. 基于TIDronePilot和TIDronePlanner分層式開發(fā)Q&A

1. 上位機和下位機通過串口通訊的方式進(jìn)行數(shù)據(jù)交互，數(shù)據(jù)的實時性如何，如何保證二者通訊時數(shù)據(jù)的可靠性？

• 這里我們看下用于offboard外部控制的通訊串口波特率為2250000bps，傳輸帶個字節(jié)數(shù)據(jù)包括1個起始位、1個停止位、8個數(shù)據(jù)位共占用10個二進(jìn)制位，因此傳輸單個字節(jié)時間開銷為1/225000秒，即約為4.4us，可知傳輸100個字節(jié)時，時間開銷為約為0.44ms，實際當(dāng)前傳輸帶寬為68個字節(jié)，約為0.3ms。
• 串口通訊時為了確保串口不丟幀，需要考慮優(yōu)先級高于當(dāng)前串口中斷的任務(wù)的最大執(zhí)行時間+串口中斷函數(shù)執(zhí)行時間，務(wù)必小于串口接收單個字節(jié)所需的時間，才能確保不丟幀。
  1、多個串口通訊時，串口通訊波特率可以降低一點。
  2、合計設(shè)計優(yōu)先級，當(dāng)存在不同波特率通訊時，通訊波特率高的串口中斷優(yōu)先級要高于波特率低的。
  3、存在優(yōu)先級高于串口中斷的其它中斷任務(wù)時，其它中斷任務(wù)的總的最大時間開銷也要考慮。

1. 在上位機端開發(fā)應(yīng)用層代碼的情況下，是不是可以完全不用管飛控端、機載計算端的代碼實現(xiàn)？

• 新增offboard外部控制模式的主要目的是簡化開發(fā)過程，讓新手能更容易上手在飛控應(yīng)用層做二次開發(fā)，無需管飛控整個飛控系統(tǒng)代碼。因此用戶可以完全不應(yīng)管飛控各系統(tǒng)模塊的內(nèi)部具體的實現(xiàn)過程，新手用戶把飛控當(dāng)作“盲盒”也能實現(xiàn)二次開發(fā)，就像之前的教程中把機載端當(dāng)作一個能提供室內(nèi)高精度位姿數(shù)據(jù)的“GPS傳感器”一樣，只需要知道如何安裝、接線、設(shè)置、操控就可以。

1. 文檔中是以室內(nèi)激光雷達(dá)SLAM定位為例的，如果我需要在室外實現(xiàn)相關(guān)應(yīng)用層的二次開發(fā)，還能按照上述方式來開發(fā)嗎？另外外部通用MCU系統(tǒng)板能用樹莓派等機載端去實現(xiàn)嗎？

• 能，只需要修改飛控端發(fā)送的位置、速度數(shù)據(jù)融合來源就可以，比如在室外GPS定位下，飛控在給外部應(yīng)用層發(fā)反饋狀態(tài)數(shù)據(jù)時，將內(nèi)部GPS融合得到的位置、速度數(shù)據(jù)替換掉原來SLAM定位融合的數(shù)據(jù)，原來外部控制器的開發(fā)過程不需要做任何調(diào)整，同理UWB定位模式下也也是這個處理思路。
• 對于通用MCU的要求比較小，用樹莓派ROS端去做開發(fā)當(dāng)然可以，同時如果所使用的單片機波特率到不了2250000bps，可以適當(dāng)調(diào)小通訊波特率，但建議不要低于921600。

TIDronePilot和TIDronePlanner配合使用時的具體操作參見配套的視頻教程...