基于OSG和OSDK的無人機標校系統(tǒng)設計及實現(xiàn)

虞炳文，蔡紅維，丁思煒，楊 波，王 康

(西昌衛(wèi)星發(fā)射中心，四川 西昌 615000)

0 引言

目前的標校，根據(jù)跟蹤目標的屬性，可以區(qū)分為對靜態(tài)目標的標校以及對動態(tài)目標的標校。

對靜態(tài)目標的跟蹤標校實現(xiàn)較為簡單，但是需要標校塔或者標校桿配合進行，因此受到場地限制的約束較為明顯。

通過對動態(tài)目標的跟蹤標校，較為常見的有三種方式，分別是利用標校衛(wèi)星、利用飛機、利用無人機進行標校。

利用標校衛(wèi)星，即利用天平衛(wèi)星進行標校，優(yōu)點是簡單可行，但是此種方法只能對測控設備的跟蹤性能(基帶指標)進行標校，對設備的動態(tài)性能(伺服指標)則無法進行有效標校。

利用飛機標校，優(yōu)點是對設備的跟蹤性能和動態(tài)性能都能進行有效的檢驗，但缺點也很明顯，就是成本高昂。

利用無人機進行標校，優(yōu)點是成本較低，起飛在一定程度上不受場地限制，對測控設備的跟蹤和動態(tài)性能在理論上都能進行有效的驗證，但是缺點是，目前所使用的無人機上可攜帶的應答機較為簡單，以及無人機容易受到天氣等因素影響，驗證精度無法得到有效保證。

而文中通過對無人機、應答機等的硬件指標設計，對標校相關(guān)軟件設計，提高無人機的標?？煽啃裕ＷC其精度，從而保證無人機標校的可行性。通過對無人機標校系統(tǒng)進行功能需求分析，提出了對于標校軟件功能和無人機、應答機等硬件指標的設計與實現(xiàn)，對于進一步提高無人機標校精度與可靠性，推動無人機標校的應用有較強的現(xiàn)實意義。

1 無人機標校存在的問題

在前期開展的無人機標校過程中，暴露出一些問題。

1)無人機在高空中穩(wěn)定性差，容易受到高空風等因素影響，導致不能按照既定飛行軌跡飛行。

2)目前基地正在使用的標校無人機，攜帶的是信標機，并非應答機，無應答模式。

3)無人機攜帶的信標機的發(fā)射頻點與信號強度不支持修改。

4)因機載信標機較為簡易，未配備頻標設備，頻率的穩(wěn)定性較差。

5)飛行軌跡的設計依靠無人機產(chǎn)家提供的商業(yè)軟件，例如DJI Pilot，并非專業(yè)的標校軟件，因此使用上有很多限制，例如在手動添加飛行軌跡的路徑點時，難以避免地會出現(xiàn)點密度較低，點分布不均勻等情況。另外，在飛行軌跡的導入導出格式上，也會出現(xiàn)不匹配的情況。

6)專業(yè)的無人機標校軟件的缺失，導致無人機的飛行軌跡的設計無法考慮到很多重要因素，例如被標校設備的指標參數(shù)，當時當?shù)氐淖匀灰蛩氐取?/p>

2 無人機標校軟硬件分析設計

在此對無人機標校系統(tǒng)進行一個分析及設計，涉及硬件及軟件內(nèi)容。

2.1 應答機需求分析及設計

應答機應包含六個模塊，依次為：控制天線、無線通信模塊、控制模塊、應答模塊、射頻天線、供電模塊，應答機示意圖見圖1。

圖1 應答機平面示意圖

對機載應答機作如下要求：

1)應答機的重量主要來源于應答機保護外殼、無線通信模塊、應答模塊、電池。應答機總重量小于15 kg。

2)供電模塊的輸入端電壓為12 V，電流為2 A，即功率為24 W。

3)應答機外接3S電池。

4)可通過地面控制臺無線控制應答機信號開關(guān)，調(diào)整信號頻點、調(diào)整信號強度，切換應答及信標模式等。

2.2 標校無人機需求分析及設計

標校無人機的重要指標為：無人機載重與飛行穩(wěn)定性。

1)無人機載重量與無人機的電機、螺旋槳槳葉長度、螺距等密切相關(guān)。電池、電機、螺旋槳選型與搭配很重要，一般情況下，整機重量，應該小于電機最大動力的2/5。

2)飛行穩(wěn)定性受飛控影響較大。選擇大疆A3 Pro飛控，搭載D-RTK-GNSS，可使得飛行懸停精度達到垂直方向約0.02 m，水平方向約0.01 m。

3)機架選用六旋翼機架，材質(zhì)輕且堅固。

4)電池的選擇需要綜合考慮電池的重量及容量。無人機電池容量保證載重10 kg下，可飛行30 min。

5)無人機遙控天線與應答機射頻天線、應答機通信控制天線區(qū)分開。

6)無人機可同時接收來自遙控器及地面控制臺的控制信號。遙控器控制無人機進行飛行操作，地面控制臺通過上傳規(guī)劃好的航線驅(qū)動無人機啟動航線飛行。

無人機示意圖見圖2。

圖2 無人機俯視圖

2.3 無人機標校軟件需求分析及設計

無人機標校軟件是為標校無人機專用設計的桌面端應用軟件，運行在地面控制臺服務器上，其作用是能夠根據(jù)測控設備及機載應答機的性能參數(shù)、指標要求以及當?shù)丨h(huán)境因素，綜合生成一條滿足標校需求的飛行軌跡，并且能夠?qū)w行軌跡上傳至無人機飛控，驅(qū)動標校無人機實施飛行計劃。同時，無人機標校軟件可以實現(xiàn)對機載應答機的遠程控制。

2.3.1 軟件功能

無人機標校軟件需要實現(xiàn)以下三個功能：

1)生成一條可靠的飛行軌跡；

2)將飛行軌跡導入飛控，并實施；

3)遠程控制機載應答機。

2.3.2 開發(fā)環(huán)境

軟件部分使用較為成熟的C++開發(fā)框架：

1)操作系統(tǒng)：Linux操作系統(tǒng)。

2)開發(fā)語言：C++。

3)開發(fā)工具：Qt及QtCreator。

4)編譯器：GCC 5.4.0/5.5.0以上版本。

2.3.3 功能設計

軟件功能設計分為三部分進行闡述。

2.3.3.1 生成飛行軌跡功能

生成飛行軌跡的功能是整個軟件部分的基礎內(nèi)容。

2.3.3.1.1 功能分析

飛行軌跡的規(guī)劃，需要考慮以下幾個方面的情況：

1)標校的目的。跟蹤靜態(tài)目標與跟蹤動態(tài)目標所要達成的目標是不同的。跟蹤靜態(tài)目標的通常是為了檢查測控設備的極性、AGC曲線標定。跟蹤動態(tài)目標通常是為了測試設備的伺服性能，即設備方位俯仰上的速度、加速度、加加速度等，能否達到指標要求，因此在設計飛行軌跡時，需考慮其極限值。

2)飛行軌跡的約束條件。標校無人機允許飛行的最大高度與起飛點位置相關(guān)。測控設備與應答機之間的最小距離與電磁波傳輸特性有關(guān)，即電磁波遠場距離公式，見公式(1)，其中D為天線口徑，λ為電磁波頻率；最遠距離與測控設備與應答機的發(fā)射信號強度、接收靈敏度有關(guān)。

3)無人機飛行的干擾因素。主要是環(huán)境因素，包括高空風、大氣折射率、溫濕壓、太陽夾角、月球夾角等。

4)飛行軌跡的模擬仿真。搭建三維立體場景進行模擬仿真，一方面實現(xiàn)飛行軌跡可視化，一方面驗證各參數(shù)的正確性，可選用OpenSceneGraph及osgEarth函數(shù)庫實現(xiàn)。

2.3.3.1.2 程序設計

根據(jù)上述影響分析，對飛行軌跡的功能作如下設計。

1)設計飛行軌跡生成函數(shù)類，QGenerateTraj。

2)設計一個結(jié)構(gòu)體trajStruct，包含每個飛行軌跡跡點的數(shù)據(jù)格式值，見代碼 1，其中T為相對時間，X、Y、Z分別為地心固定坐標系下的X軸值、Y軸值、Z軸值，Vx、Vy、Vz分別為在X軸、Y軸、Z軸方向的速度分量。

代碼1:飛行軌跡點結(jié)構(gòu)體

struct trajStruct{

int T;

int X;

int Y;

int Z;

int Vx;

int Vy;

int Vz;

}

3)函數(shù)emulateTrajectory()，無返回值，功能為根據(jù)生成的飛行軌跡在三維環(huán)境中進行仿真。

4)函數(shù)getFlyTrajectory()，返回值為保存為QList的飛行軌跡，即保存為每個跡點的數(shù)據(jù)的容器，功能為根據(jù)輸入的參數(shù)生成飛行軌跡，該函數(shù)在emulateTrajectory()中被調(diào)用。

5)setOsgSceneParaAndDisplay(QList traj)函數(shù)，返回值為QList的飛行軌跡，輸入值為getFlyTrajectory()輸出的結(jié)果數(shù)據(jù)，功能為在三維空間中演示輸入的飛行軌跡。該函數(shù)在emulateTrajectory()中被調(diào)用。

6)函數(shù)setFlyTrajPara()，無返回值，輸入值為各指標參數(shù)，功能為設置飛行軌跡設計中需要用到的基本參數(shù)，如測站地理坐標、測試指標、飛行限制條件、環(huán)境干擾因素、數(shù)據(jù)輸出格式等。該函數(shù)在getFlyTrajectory()函數(shù)中被調(diào)用。

7)函數(shù)setFlyTrajPara()，無返回值，輸入值為測試指標參數(shù)，功能為設置測試期望達到的指標參數(shù)。該函數(shù)在setFlyTrajPara()被調(diào)用。在該函數(shù)下，根據(jù)指標參數(shù)的不同，分別調(diào)用setElSpeed(int val)設置方位速度，調(diào)用setElAcceleration(int val)設置方位加速度，調(diào)用setElJerk(int val)設置方位加加速度，調(diào)用setAzSpeed(int val)設置方位速度，調(diào)用setAzAcceleration(int val)設置方位加速度，調(diào)用setAzJerk(int val)設置方位加加速度。

8)函數(shù)setFlyLimitPara()，無返回值，輸入值為飛行軌跡的限制條件，功能為設置因設備性能帶來的限制條件。該函數(shù)在setFlyTrajPara()被調(diào)用。在該函數(shù)下，根據(jù)指標參數(shù)的不同，分別調(diào)用setRadarEmitPower (int val)設置測控設備的發(fā)送功率，調(diào)用setRadarRecvMinVal (int val)設置測控設備的接收靈敏度，調(diào)用setRespEmitPower (int val)設置應答機的發(fā)送功率，調(diào)用setRespRecvMinVal (int val)設置應答機的接收靈敏度，調(diào)用setRemoteDist (int val,int fr)設置并計算遠場距離，其中val為天線口徑，fr為電磁波頻率。

9)函數(shù)setFlyInterPara()，無返回值，輸入值為環(huán)境因素帶來的干擾參數(shù)，功能為設置因環(huán)境因素帶來的干擾。該函數(shù)在setFlyTrajPara()被調(diào)用。在該函數(shù)下，根據(jù)指標參數(shù)的不同，分別調(diào)用ifUpperAirWinds(bool ok)設置是否啟用高空風誤差修正，調(diào)用ifAtmosRef(bool ok)設置是否啟用大氣折射修正，調(diào)用ifTempHumiPressure(bool ok)設置是否啟用溫濕壓修正，調(diào)用ifSunAngle(bool ok)設置是否避開太陽夾角干擾，調(diào)用ifMoonAngle(bool ok)設置是否避開月球夾角干擾。

函數(shù)調(diào)用關(guān)系見圖3。

圖3 生成飛行軌跡函數(shù)設計

2.3.3.1.3 關(guān)鍵技術(shù)實現(xiàn)

關(guān)鍵技術(shù)主要圍繞與OSG相關(guān)的內(nèi)容。

2.3.3.1.3.1 三維環(huán)境的搭建

為搭建一個高還原度、可測量、準確可靠的三維世界，選用開源的osgEarth函數(shù)庫來實現(xiàn)。osgEarth是基于OSG函數(shù)庫二次開發(fā)的，專門用于地理信息系統(tǒng)建模的函數(shù)庫，使用范圍廣，可配置性強。選用osgEarth2.10版本及OSG3.6.4版本進行二次開發(fā)。

通過osgEarth實現(xiàn)三維建模主要有兩種方法，一種是純代碼形式，所有的配置項通過代碼中調(diào)用函數(shù)庫實現(xiàn)；另一種是代碼加配置文件形式，三維地理模型的絕大部分配置內(nèi)容，都可以通過*.earth文件中的配置項實現(xiàn)配置。在無人機標校軟件的設計中使用第二種方式。

2.3.3.1.3.1.1 編寫*.earth文件

編寫*.earth文件，可以將其命名為simple.earth配置文件，earth后綴的文件格式，類似于*.xml文件，是用作配置三維地理模型的配置文件。定義地理信息模型，需要最少包含五部分內(nèi)容，一是確定空間參考系，二是確定地形渲染參數(shù)，三是加載高程數(shù)據(jù)，四是加載紋理信息，五是確定數(shù)據(jù)緩存方式。

1)確定空間參考系。空間參考系配置選用geocentric，地固系坐標系，即地心為質(zhì)心的空間直角坐標系。

代碼2:空間參考系配置項

2)定義地形引擎如何渲染影像數(shù)據(jù)和高程數(shù)據(jù)?？蓪Φ乩砟Ｐ偷牟逯捣绞?、是否開啟地形表面的光照、加載地形數(shù)據(jù)(瓦片)的策略、多影像數(shù)據(jù)疊加時集成最終影像數(shù)據(jù)的方式、地形瓦片分割的最大層數(shù)、瓦片范圍系數(shù)、瓦片的采樣率、瓦片邊緣率、高程夸張系數(shù)、邊緣緩沖率等參數(shù)進行設置。

代碼3:地形渲染方式配置項

3)加載高程數(shù)據(jù)。使用加載DEM圖的方式加載高程數(shù)據(jù)，DEM圖是一種每個圖元像素都包含了地理信息，如經(jīng)緯度、高程信息的TIF格式圖。在此使用的DEM圖為包含了中國范圍內(nèi)的1Km精度的高程信息。加載高程數(shù)據(jù)的操作，可以先貼上一張高程精度較低的DEM圖，然后在此基礎上，根據(jù)需求在特定區(qū)域貼上精度更高的DEM圖，如下圖所示。之所以要選擇不同精度的不同范圍的DEM圖，是因為精度如果太高，就會導致DEM圖非常大，加載起來就會非常困難，因此要合理安排DEM圖的精度。

代碼4:加載高程數(shù)據(jù)配置項

4)加載紋理信息，即貼圖操作。是為了使得地理模型可以更直觀且美觀的展現(xiàn)，如果沒有貼圖操作，則顯示的地理模型就是一個密密麻麻的點云。貼圖操作可以先貼上一張分辨率較低的全球影像圖，然后在此基礎上，根據(jù)需求在特定區(qū)域貼上分辨率更高的影像圖。原因同高程數(shù)據(jù)。

代碼5:加載紋理信息配置項

5)確定數(shù)據(jù)緩存的方式和存儲位置。緩存在三維建模中顯得較為重要，因為加載一次高程和紋理圖的數(shù)據(jù)量較大，需要較長時間，如果在每一次運行時都去重新加載，那顯然是不可接受的，因此osgEarth給出的一種方案是，第一次加載完成后，就會生成緩存數(shù)據(jù)，以后每次加載，則從緩存數(shù)據(jù)進行加載，這樣會顯著提升效率。

代碼6:數(shù)據(jù)緩存的配置項

/media/ybw/SSD/OSG_DATA/MAIN_DATA/FileCache

2.3.3.1.3.1.2 運行并顯示三維世界

因為選用Qt作為開發(fā)工具，要將osgEarth顯示的地理模型，顯示在Qt的窗口中。在osgEarth開發(fā)中，至少需要配置兩部分內(nèi)容，一是加載三維模型，二是設置相機操作器。

1)加載*.earth模型。

代碼7:讀取模型的代碼

osg::Node* pNode =

osgDB::readNodeFile("./Data/simple.earth");

osgEarth::MapNode* mapNode =

osgEarth::MapNode::findMapNode(pNode.get());

2)初始化相機操作器，即相機參數(shù)。osgEarth的界面顯示，實際上是將顯示界面當作一個攝像頭，顯示的是攝像機視角。

代碼8:設置相機操作器的代碼

osgEarth::Util::EarthManipulator *em = new osgEarth::Util::EarthManipulator;

if(mapNode.valid())

{

em.get()->setNode(mapNode);

}

em.get()->getSettings()->setArcViewpointTransitions(true);

osgViewer::Viewer * pViewer=this->getOsgViewer();

pViewer->setCameraManipulator(em.get());

pViewer->setSceneData(mRoot.get());//添加到場景

pViewer->realize();

2.3.3.2 航線導入飛控并實施功能

在生成了航線之后，就需要考慮如何將該航線轉(zhuǎn)換為無人機可識別的格式并上傳使用。

2.3.3.2.1 功能分析

實現(xiàn)飛行軌跡的導入，需要解決幾方面的問題。

1)將生成的飛行軌跡數(shù)據(jù)整理成飛控程序能識別的格式。

2)將整理后的飛行軌跡數(shù)據(jù)上傳至飛控。

3)驅(qū)動飛行器按既定飛行軌跡飛行。

2.3.3.2.2 程序設計

大疆無人機設備基于C++的二次開發(fā)，大疆官方提供了OSDK開發(fā)工具包，目前最新的版本為V4.0.0。

2.3.3.2.2.1 DJI OSDK簡介

在此介紹OSDK開發(fā)包。

1)DJI OSDK是由大疆自主開發(fā)的一個運行于桌面端的軟件庫，貼合用戶自主進行二次開發(fā)的需要，可個性定制對無人機在飛行中與自動化相關(guān)的的需求，其提供了一些基礎的API，用于實現(xiàn)基本的運動規(guī)劃功能；開發(fā)者在使用調(diào)用規(guī)劃相關(guān)的庫的同時，可以根據(jù)個人需要，對功能進一步擴展，可根據(jù)實際的使用需求，設計并編寫相應的航點任務以及熱點任務，制定一套個性化控制無人機自動化飛行的控制邏輯。

2)在無人機標校中主要用到的功能是航點規(guī)劃的功能。航點規(guī)劃，簡單來說就是通過設置一系列的軌跡點，連接成線，然后將航跡上傳給無人機，指令控制無人機按照指定的航線啟動飛行，實現(xiàn)無人機根據(jù)既定航線自動化飛行的控制功能。開發(fā)者通過調(diào)用DJI OSDK 的API接口，能夠控制無人機以指定的高度、方向飛往既定的位置，并且執(zhí)行約定好的動作，根據(jù)任務需求，還可以編寫更多的任務動作，控制無人機多次重復執(zhí)行該任務，以此實現(xiàn)多輪次標校的功能。

3)開發(fā)者在代碼開發(fā)中，調(diào)用航點任務相關(guān)的API，需要指定的參數(shù)為航點數(shù)量和對應的航點類型。航點數(shù)量在最新版本的API中，最多可設置65535個點。航點類型主要包含三種飛行模式，分別為曲率飛行、直線飛行和協(xié)調(diào)轉(zhuǎn)彎。

4)OSDK庫文件同時還為開發(fā)者提供了更多方便簡潔的功能，以便直接調(diào)用，比如對速度的控制功能，開發(fā)者能夠按照既定任務要求，為不同的航點配置不同的速度，當然，也可以為同一個航點設置多個速度。另外，在無人機執(zhí)行航線飛行任務時，修改或查詢無人機全局巡航速度等的狀態(tài)數(shù)據(jù)的函數(shù)，也是事先寫好，可以直接調(diào)用的。

2.3.3.2.2.2 主要函數(shù)類

為實現(xiàn)航點任務功能，利用OSDK函數(shù)庫，主要將設計到以下幾個代碼文件：

1)dji_mission_type.hpp文件。該文件主要用于定義在航線自動飛行中會用到的一些數(shù)據(jù)的結(jié)構(gòu)體文件，主要的內(nèi)容為WaypointV2結(jié)構(gòu)體，其中主要涉及到的內(nèi)容有：longitude為精度值，latitude為緯度值，relativeHeight為高程值，maxFlightSpeed為最大速度值，autoFlightSpeed為自動巡航速度值。

代碼9:WaypointV2結(jié)構(gòu)體內(nèi)容

typedef struct WaypointV2

{

float64_t longitude;

float64_t latitude;

float32_t relativeHeight; /*! relative to takeoff height*/

DJIWaypointV2FlightPathMode waypointType;

DJIWaypointV2HeadingMode headingMode;

WaypointV2Config config;

uint16_t dampingDistance;

float32_t heading;

DJIWaypointV2TurnMode turnMode;

RelativePosition pointOfInterest;

float32_t maxFlightSpeed;

float32_t autoFlightSpeed;

}WaypointV2;

2)dji_mission_base.hpp文件。在這部分代碼中，主要實現(xiàn)的是對航線任務的控制函數(shù)，start()實現(xiàn)啟動任務，stop()實現(xiàn)停止任務，pause()實現(xiàn)暫停任務，resume()實現(xiàn)重啟任務。

代碼10:dji_mission_base.hpp主要代碼

virtual void start(VehicleCallBack callback = 0, UserData userData=0)=0;

virtual ACK::ErrorCode start(int timer) = 0;

virtual void stop(VehicleCallBack callback = 0, UserData userData = 0) = 0;

virtual ACK::ErrorCode stop(int timer) = 0;

virtual void pause(VehicleCallBack callback = 0, UserData userData = 0) = 0;

virtual ACK::ErrorCode pause(int timer) = 0;

virtual void resume(VehicleCallBack callback = 0, UserData userData = 0)=0;

virtual ACK::ErrorCode resume(int timer) = 0;

3)dji_waypoint_v2.hpp、dji_waypoint_v2.cpp代碼文件。其中的WaypointV2MissionOperator類為主要功能實現(xiàn)的類，在該類中主要涉及到的函數(shù)有，init(WayPointV2InitSettings *info,int timeout)函數(shù)為實現(xiàn)該類函數(shù)的初始化操作，其中WayPointV2InitSettings為航線任務結(jié)構(gòu)體，在該結(jié)構(gòu)體中主要規(guī)定了任務ID值，missionID，任務重復次數(shù)repeatTimes，航線結(jié)束時的動作finishedAction，最大飛行速度maxFlightSpeed，自動飛行速度autoFlightSpeed，前往第一個任務點的方式gotoFirstWaypointMode，以及裝載了任務航點的容器mission。downloadInitSetting()即從飛控設備下載航跡任務的初始化配置信息。start()函數(shù)用于啟動航線任務，stop()為停止航線任務，pause()函數(shù)用于暫停飛行任務，resume()用于重啟飛行任務，注意到，start()、stop()、pause()、resume()四個函數(shù)都繼承自dji_mission_base.hpp代碼。uploadMission()函數(shù)用于上傳任務航線，downloadMission()用于下載在飛控中的任務航線。uploadAction()用于上傳飛行器動作。RegisterMissionEventCallback()和RegisterMissionStateCallback()分別用于反饋目前的動作和狀態(tài)信息。

代碼11:dji_waypoint_v2.hpp主要代碼:

class WaypointV2MissionOperator

{

public:

const uint16_t MAX_WAYPOINT_NUM_SIGNAL_PUSH = 260;

WaypointV2MissionOperator(Vehicle* vehiclePtr);

～WaypointV2MissionOperator();

ErrorCode::ErrorCodeType init(WayPointV2InitSettings *info, int timeout);

ErrorCode::ErrorCodeType downloadInitSetting(WayPointV2InitSettingsInternal &info, int timeout);

ErrorCode::ErrorCodeType start(int timeout);

ErrorCode::ErrorCodeType stop(int timeout);

ErrorCode::ErrorCodeType pause(int timeout);

ErrorCode::ErrorCodeType resume(int timeout);

ErrorCode::ErrorCodeType uploadMission(int timeout);

ErrorCode::ErrorCodeType downloadMission(std::vector&mission, int timeout);

ErrorCode::ErrorCodeType getGlobalCruiseSpeed(GlobalCruiseSpeed &cruiseSpeed, int timeout);

ErrorCode::ErrorCodeType setGlobalCruiseSpeed(const GlobalCruiseSpeed &cruiseSpeed, int timeout);

ErrorCode::ErrorCodeType uploadAction(std::vector&actions, int timeout);

ErrorCode::ErrorCodeType getActionRemainMemory(GetRemainRamAck &remainRamAck, int timeout);

ErrorCode::ErrorCodeType getWaypointIndexInList(GetWaypontStartEndIndexAck &startEndIndexAck, int timeout);

inline DJIWaypointV2MissionState getCurrentState() { return currentState; }

inline DJIWaypointV2MissionState getPrevState() { return prevState; }

void setPrevState(DJIWaypointV2MissionState state) {prevState = state; }

void setCurrentState(DJIWaypointV2MissionState state) {currentState = state; }

float32_t getTakeoffAltitude(){return takeoffAltitude;};

void setTakeoffAltitude(float32_t altitude){ takeoffAltitude = altitude;};

void RegisterMissionEventCallback(void *userData, PushCallback cb = NULL);

void RegisterMissionStateCallback(void *userData, PushCallback cb = NULL) ;

private:

std::vector missionV2;

DJIWaypointV2MissionState currentState;

DJIWaypointV2MissionState prevState;

Vehicle *vehiclePtr;

float32_t takeoffAltitude;

void RegisterOSDInfoCallback(Vehicle *vehiclePtr);

};

2.3.3.2.2.3 流程設計

航線導入并啟動的功能實施流程如下：

1)在之前已經(jīng)設計好航線的前提下，需要將該航行的數(shù)據(jù)點存儲模式修改為大疆指定的數(shù)據(jù)點存儲方式；

2)將生成的航線通過API調(diào)用上傳航線任務的整體信息。

3)一個航點任務包含航線飛行任務的ID、航點任務的航點數(shù)、任務重復次數(shù)、航點任務結(jié)束后的動作、最大飛行速度和巡航速度。

4)上傳航點信息基礎參數(shù)：航點坐標(設置航點的經(jīng)度、緯度和相對于起飛點的高度)、航點類型、航向類型和飛行速度?？蛇x參數(shù)：緩沖距離、航向角度、轉(zhuǎn)向模式、興趣點、單點最大飛行速度、單點巡航速度。

5)將航線任務上傳至標校無人機飛控。

6)控制無人機執(zhí)行航點任務。上傳完成后，標校無人機可通過開發(fā)者之前預設的動作，如開始、停止或暫停任務、設置或獲取巡航速度等。

2.3.3.2.2.4 程序?qū)崿F(xiàn)

本章節(jié)內(nèi)容旨在將上一章節(jié)內(nèi)容以代碼實現(xiàn)。

1)首先判斷是否支持航線任務規(guī)劃的功能，通過如下代碼。

代碼12:判斷函數(shù)是否支持功能

if (!vehiclePtr->isM300()) {

DSTATUS("This sample only supports M300!"); /*當前waypoint v2 僅支持M300機型*/

return false;

}

int timeout = 1;

GetRemainRamAck actionMemory = {0};

ErrorCode::ErrorCodeType ret;

if(!setUpSubscription(timeout))

{

DERROR("Failed to set up subscription!");

return -1;

}

else

{

DSTATUS("Set up subscription successfully!");

}

/*! wait for subscription data come*/

sleep(timeout);

2)而后創(chuàng)建mission任務，初始化mission任務，用到initMissionSetting()函數(shù)，該函數(shù)實現(xiàn)了任務初始化配置的功能，部分代碼見代碼7內(nèi)容。

代碼13:上傳任務的代碼

/*! init mission */ /* 初始化mission任務 */

ret = initMissionSetting(timeout);

if(ret != ErrorCode::SysCommonErr::Success)

return ret;

sleep(timeout);

3)其中代碼8中為添加航跡點的核心代碼。是initMissionSetting代碼中內(nèi)容，通過這段代碼，添加每個航點信息。

代碼14:上傳任務的代碼

/*! Init waypoint settings*/

WayPointV2InitSettings missionInitSettings;

missionInitSettings.missionID = rand();

missionInitSettings.repeatTimes = 1;

missionInitSettings.finishedAction = DJIWaypointV2MissionFinishedGoHome;

missionInitSettings.maxFlightSpeed = 10;

missionInitSettings.autoFlightSpeed = 2;

missionInitSettings.exitMissionOnRCSignalLost = 1;

missionInitSettings.gotoFirstWaypointMode = DJIWaypointV2MissionGotoFirstWaypointModePointToPoint;

missionInitSettings.mission = generatePolygonWaypoints(radius, polygonNum);

missionInitSettings.missTotalLen = missionInitSettings.mission.size();

ErrorCode::ErrorCodeType ret = vehiclePtr->waypointV2Mission->init(&missionInitSettings,timeout);

4)代碼9的內(nèi)容是在代碼8中進一步展開，特別將generatePolygonWaypoints()函數(shù)單獨講解，這部分內(nèi)容是generatePolygonWaypoints()函數(shù)的內(nèi)容。該函數(shù)實現(xiàn)了航點數(shù)據(jù)的添加功能。這一步函數(shù)調(diào)用，也是實現(xiàn)在飛行軌跡設計完成之后，將設計的航跡進行轉(zhuǎn)換，使得無人機飛控可以使用的關(guān)鍵。

代碼15:上傳任務的代碼

std::vector WaypointV2MissionSample::generatePolygonWaypoints(float32_t radius, uint16_t polygonNum) {

// Let's create a vector to store our waypoints in.

std::vector waypointList;

WaypointV2 startPoint;

WaypointV2 waypointV2;

Telemetry::TypeMap::type subscribeGPosition = vehiclePtr->subscribe->getValue();

startPoint.latitude = subscribeGPosition.latitude;

startPoint.longitude = subscribeGPosition.longitude;

startPoint.relativeHeight = 15;

setWaypointV2Defaults(startPoint);

waypointList.push_back(startPoint);

// Iterative algorithm

for (int i = 0; i < polygonNum; i++) {

float32_t angle = i * 2 * M_PI / polygonNum;

setWaypointV2Defaults(waypointV2);

float32_t X = radius * cos(angle);

float32_t Y = radius * sin(angle);

waypointV2.latitude = X/EARTH_RADIUS + startPoint.latitude;

waypointV2.longitude = Y/(EARTH_RADIUS * cos(startPoint.latitude)) + startPoint.longitude;

waypointV2.relativeHeight = startPoint.relativeHeight ;

waypointList.push_back(waypointV2);

waypointList.push_back(startPoint);

return waypointList;

}

5)而后將自定義的航線任務上傳至無人機飛控。見代碼8內(nèi)容。

代碼16:上傳任務的代碼

/*! upload mission */

/*! upload mission's timeout need to be longer than 2s*/

int uploadMissionTimeOut = 3;

ret = uploadWaypointMission(uploadMissionTimeOut); /* 上傳任務，注意注釋部分，這里的超時需要設置為3s, 設置為1時，可能因為上傳任務過多，報index錯誤*/

if(ret != ErrorCode::SysCommonErr::Success)

return ret;

sleep(timeout);

6)從飛控中下載航線，這一步可以起到檢查航線的作用。

代碼17:下載并檢查任務的代碼

/*! download mission */ /* 下載任務，可以查看對比上傳的任務 debug的時候可以用來對比排查問題 */

std::vector mission;

ret = downloadWaypointMission(mission, timeout);

if(ret != ErrorCode::SysCommonErr::Success)

return ret;

sleep(timeout);

7)而后上傳在航線進行過程中，需要涉及的一些飛行器動作。

代碼18:上傳飛行器動作的代碼

/*! upload actions */ /* 上傳actions：*/

/*! check action memory */

ret = getActionRemainMemory(actionMemory, timeout); //上傳action前先檢查action容量，避免上傳的任務過多導致action被擠掉了，執(zhí)行動作可能不完整。最大貌似為65535 byte

if (actionMemory.remainMemory <= 0)

{

DSTATUS("action memory is not enough.Can not upload more action!");

return ErrorCode::SysCommonErr::UndefinedError;

}

ret = uploadWapointActions(timeout);

if(ret != ErrorCode::SysCommonErr::Success)

return ret;

ret = getActionRemainMemory(actionMemory, timeout);

sleep(timeout);

8)開始執(zhí)行飛行任務。

代碼19:開始執(zhí)行飛行任務的代碼

/*! start mission */

ret = startWaypointMission(timeout); //開始執(zhí)行

if(ret != ErrorCode::SysCommonErr::Success)

return ret;

sleep(20);

9)設置飛行過程中的一些參數(shù)，比如最大飛行速度等。

代碼20:設置飛行參數(shù)的代碼

/*! set global cruise speed */

setGlobalCruiseSpeed(1.5, timeout); //設置巡航速度，巡航速度為全程航線的巡航速度，不是通過單點航點任務來設置的?？梢栽趩吸c航點中更改，間接實現(xiàn)不同的航點以不同的速度運行。

sleep(timeout);

10)獲取巡航速度，以判斷飛行器飛行狀態(tài)。

代碼21:獲取飛行參數(shù)的代碼

/*! get global cruise speed */

getGlobalCruiseSpeed(timeout);

sleep(timeout);

11)可以測試暫停飛行。

代碼22:暫停飛行的代碼

/*! pause the mission*/

ret = pauseWaypointMission(timeout);

if(ret != ErrorCode::SysCommonErr::Success)

return ret;

sleep(5);

12)測試重啟飛行任務的功能。

代碼23:重啟飛行的代碼

printf("kyle test: pauseWaypointMission and resumeWaypointMission/n");

/*! resume the mission*/

ret = resumeWaypointMission(timeout); //暫停及回復航線指令

if(ret != ErrorCode::SysCommonErr::Success)

return ret;

sleep(50);

2.3.3.3 遠控機載應答機功能

當無人機按照既定設計的航線自動飛行后，就需要考慮標校無人機機載應答機的控制問題。

2.3.3.3.1 功能分析

這部分功能需要根據(jù)應答機廠家所提供的API接口進行開發(fā)，但是無論是哪家廠家，根據(jù)使用者的需求對接，軟件接口至少包含以下內(nèi)容。

1)開關(guān)機控制；

2)信號頻率的選擇；

3)應答機信號發(fā)送強度的衰減值；

4)在應答和信標模式之間切換；

5)射頻信號是否允許輸出。

6)網(wǎng)絡通信功能。可以將這部分功能設計為網(wǎng)絡通信的形式，與近端的應答機控制模塊進行通信，使其控制遠端的應答機。

2.3.3.3.2 程序設計

根據(jù)需求分析，可以將功能分為兩部分，一部分是參數(shù)設置模塊，一部分是網(wǎng)絡通信模塊。

1)參數(shù)設置模塊。設計一個結(jié)構(gòu)體，將所有的參數(shù)包含在內(nèi)，以方便使用和查看。

代碼24:遠控應答機參數(shù)設置模塊

Typedef struct

{

unsigned char OnOff;//開關(guān)機控制

unsigned char FreqChoose;//頻率選擇

unsigned char Attenuator;//衰減器控制

unsigned char WorkMode;//工作方式

unsigned char SPout;//射頻輸出允許

}

2)網(wǎng)絡通信模塊。

代碼25:遠控應答機網(wǎng)絡通信模塊

QudpSocket *SendudpSocket;

SendudpSocket->writeDatagram(buf,len,QhostAddress(FSendIp),FSendPort);

3 無人機標校應用場景

在某次車載測控設備轉(zhuǎn)場后，受場地限制，沒有標校桿或者標校塔可供其進行標校，此時，需要起降標校無人機，進行標校。

1)利用無人機標校系統(tǒng)生成飛行軌跡功能，輸入當?shù)氐沫h(huán)境因素，如溫濕壓，海拔，以及當?shù)刈鴺宋恢玫葏?shù)，利用OSG在三維仿真空間中，生成飛行軌跡；

2)利用無人機標校系統(tǒng)導入航線的功能，通過OSDK上傳飛行軌跡；

3)利用無人機標校系統(tǒng)控制無人機的功能，控制無人機起飛，并飛行至等待點位置；

4)通過無人機標校系統(tǒng)遠控機載應答機的功能，設置合適的應答機頻點及強度，并開啟應答機；

5)標校無人機根據(jù)飛行軌跡實施完畢靜態(tài)及動態(tài)標校，并返航。

4 結(jié)束語

通過設計從生成飛行軌跡、到將飛行軌跡導入飛控并實施以及遠控應答機完整功能的軟件，并配套相應的硬件模塊，提升了無人機標校的可靠性、為后續(xù)無人機標校工作的推動提供了一套可行的技術(shù)方案。