寫在前面

因為項目原因是嗜，需要無人機在事先指定的飛行任務(wù)（Mission）中加入特定的指令，控制外部設(shè)備完成一些工作氨肌。

最初是想在地面站和飛控之間打一條通道捂寿，這樣地面站就可以控制外部設(shè)備了，詳見我之前的博客魁瞪。但是很難做到事先規(guī)劃穆律，在指定航點（WAYPOINT）上做計劃的操作，當(dāng)然手動改控制是一點問題沒有导俘，就是有點考驗用戶的耐心和注意力峦耘，而且我們也不能指望用戶有多高的水平（只有甲方爸爸挑你），即使用戶水平再高旅薄，當(dāng)一件事變成工作也就沒意思了辅髓。

最后就想能不能事先在任務(wù)中規(guī)劃好指令泣崩，到一個航點就自動完成工作呢，這樣一旦任務(wù)做好洛口，就可以一鍵起飛矫付，完成指定工作后自動返回，把用戶解放出來第焰。

反復(fù)查看QGC和PX4代碼這個功能在最近終于基本完成了买优，特地寫下來給大家分享，就當(dāng)給自己做個備忘錄吧挺举。

本來想直接粘代碼的但是有些給客戶做的東西不能往外放杀赢，重寫個demo有點費事，好在PX4湘纵，QGC有現(xiàn)成的功能實現(xiàn)脂崔，我也是照著思路寫出來的，大家可以照貓畫虎梧喷。

不賣關(guān)子了脱篙，這個功能就是相機控制（Camera），下面就以Camera為例伤柄，一步一步講講具體實現(xiàn)。

1.自定義MavLink命令

1.1 MavLink 消息和命令

大家可以在Mavlink的官網(wǎng)上找到它們的定義文搂。簡單的說MavLink協(xié)議上傳輸?shù)亩际歉鞣N消息(Message)适刀，消息其實就是一種數(shù)據(jù)包格式，是為了通信而準(zhǔn)備的煤蹭，消息的格式多種多樣笔喉。而命令(Command)是一種數(shù)據(jù)，格式完全統(tǒng)一硝皂，通過命令號來解釋不同的意義常挚，這也就方便在飛行任務(wù)中定義、存儲稽物、解釋奄毡、執(zhí)行。

1.2 MavLink命令（MAV_CMD）的格式

命令由命令號和七個參數(shù)組成加上其他我們不需要關(guān)心控制的字段贝或，下面貼出來的是MavLink傳輸命令的消息所定義的結(jié)構(gòu)體吼过。

//mavlink_msg_command_long.h
#define MAVLINK_MSG_ID_COMMAND_LONG 76
MAVPACKED(
typedef struct __mavlink_command_long_t {
 float param1; /*<  Parameter 1 (for the specific command).*/
 float param2; /*<  Parameter 2 (for the specific command).*/
 float param3; /*<  Parameter 3 (for the specific command).*/
 float param4; /*<  Parameter 4 (for the specific command).*/
 float param5; /*<  Parameter 5 (for the specific command).*/
 float param6; /*<  Parameter 6 (for the specific command).*/
 float param7; /*<  Parameter 7 (for the specific command).*/
 uint16_t command; /*<  Command ID (of command to send).*/
 uint8_t target_system; /*<  System which should execute the command*/
 uint8_t target_component; /*<  Component which should execute the command, 0 for all components*/
 uint8_t confirmation; /*<  0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)*/
}) mavlink_command_long_t;

1.3 自定義消息號

可以使用Mavlink官方提供的生成器，也可以直接在頭文件中直接加上咪奖，其實就是個16位以內(nèi)不與已有重復(fù)的數(shù)字盗忱。

1.3.1 安裝Mavlink生成器

#下載源碼
git clone https://github.com/mavlink/mavlink.git

#進(jìn)入代碼目錄并更新子模塊
cd mavlink
git submodule update --init --recursive

#可能報錯需要安裝，如果以前沒有安裝的話羊赵，目前python3的腳本還不怎么好使
sudo apt install python-tk
pip install future
#運行python腳本
python -m mavgenerate

1.3.2 定義命令

• 創(chuàng)建一個XML趟佃，如xxx.xml,cmd號隨便定義，16位無符號整型范圍內(nèi)（ < 65536）,不和已有的沖突即可,已有的在QGC那個ardupilotmega.h定義得比較全。

<mavlink>
    <enums>
        <enum name="MAV_CMD">
            <description>Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data.</description>

            <entry value="56302" name="MAV_CMD_XXXX_AMP_SINGLE">
                <description>single XXXX power control</description>
                <param index="1" label="index">XXXX index</param>
                <param index="2" label="power">if 1 poweron the XXXX,0 poweroff XXXX</param>
                <param index="3" label="amp">XXXX power amp,0~1,0</param>
                <param index="4">Reserved (all remaining params)</param>
            </entry>

            <entry value="56340" name="MAV_CMD_XXXX_PARAM_MODULATE">
                <description>MODULATE parameters</description>
                <param index="1" label="index">XXXX index</param>
                <param index="2" label="modulateType"></param>
                <param index="3" label="modulateBandWidth"></param>
                <param index="4" label="modulateMode"></param>
                <param index="5" label="modulatePulseNum"></param>
                <param index="6" label="modulateSubcodeWidth"></param>
                <param index="7" label="modulateCodeLen"></param>
            </entry>

            <!-- ... 可以加很多   -->
           </enum>
    </enums>
</mavlink>  

• 生成

python mavgenerate.py
#進(jìn)入界面后選擇xml位置闲昭，生成位置
#語言選C
#Protocol選2.0

然后就可以在你生成的位置找到xxx.h文件了

typedef enum MAV_CMD
{
    MAV_CMD_XXXX_AMP_SINGLE=56302, /* single XXXX power control |XXXX index| if 1 poweron the XXXX,0 poweroff XXXX| XXXX power amp,0~1,0| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)|  */
    MAV_CMD_XXXX_PARAM_MODULATE=56340, /* MODULATE parameters |XXXX index| | | | | | |  */

    //...

    MAV_CMD_ENUM_END=56362, /*  | */
}

呵呵罐寨，搞了半天就是個數(shù)字，所以直接在用的頭文件里直接加就好了汤纸，寫個xml可能更好在團隊中溝通吧衩茸。

1.4 頭文件中添加命令

QGC 在libs/mavlink/include/mavlink/v2.0/ardupilotmega/ardupilotmega.h

PX4 在mavlink/include/mavlink/v2.0/common/common.h

//...
typedef enum MAV_CMD
{
    //...
    //MAV_CMD_ENUM_END=42651, /*  | */ 將以前尾部注釋掉

    MAV_CMD_XXXX_AMP_SINGLE=56302, /* single XXXX power control |XXXX index| if 1 poweron the XXXX,0 poweroff XXXX| XXXX power amp,0~1,0| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)|  */
    MAV_CMD_XXXX_PARAM_MODULATE=56340, /* MODULATE parameters |XXXX index| | | | | | |  */

    //...

    MAV_CMD_ENUM_END=56362, /*  | */ //加上我們定義最大的號+1
}
//...

2 QGC中相機控制任務(wù)功能實現(xiàn)分析

2.1 任務(wù)命令的傳輸存儲和處理在數(shù)據(jù)結(jié)構(gòu)上的區(qū)別

2.1.1 任務(wù)命令的傳輸和存儲的數(shù)據(jù)結(jié)構(gòu)

就和所有通信協(xié)議一樣，數(shù)據(jù)都是串行的贮泞，任務(wù)將所有命令挨個存放楞慈，是一個線性列表，命令與命令之間只有先后關(guān)系沒有從屬關(guān)系啃擦。你在QGC的任務(wù)界面下隨便編輯一個任務(wù)囊蓝，加上幾個航點，在航點上對相機進(jìn)行操作令蛉，然后導(dǎo)出任務(wù)聚霜，其實就是一個json文件了，用文本編輯器看看就知道了珠叔。但是奇怪的是相機操作明明就是依附在航點上的呀蝎宇，直覺上應(yīng)該是個樹狀關(guān)系，后面在處理環(huán)節(jié)上說祷安。

這些命令被 MAVLINK_MSG_ID_COMMAND_LONG 消息發(fā)送到PX4上存到SD卡上姥芥，即使飛控關(guān)機也不會丟失。飛控與QGC連接后汇鞭，也是通過剛才那個消息凉唐，GQC會從飛控上下載任務(wù)，顯示到飛行界面上霍骄。飛控接到任務(wù)模式開始指令之后台囱，讀取任務(wù)并執(zhí)行，執(zhí)行過程在后面PX4修改中詳解读整。

2.1.2 任務(wù)命令在處理過程中的數(shù)據(jù)結(jié)構(gòu)

通過下面的說明可以看出這個數(shù)據(jù)結(jié)構(gòu)是以基本航點為線性列表簿训，其他命令存儲在航點下的section中就是樹的關(guān)系

PlanMasterController
|__ GeoFenceController //圍欄
|__ RallyPointController //集結(jié)
|__ MissionController //任務(wù)控制器
    |__ List<VisualMissionItem*>* visualItems; //這就是任務(wù)處理過程中的任務(wù)的儲存容器,下面兩個類都是VisualMissionItem的子類
        |__ ComplexMissionItem //復(fù)雜任務(wù)吧，沒怎么研究它
        |__ SimpleMissionItem  //這就是我們?nèi)蝿?wù)處理中航點的記錄對象
            |__ MissionItem   missionItem //就是CMD的的表達(dá)米间，即命令號加7個參數(shù)煎楣，正常情況下存儲任務(wù)的經(jīng)緯度高度，可以是WAYPOINT或者其他的飛行命令
            |__ SpeedSection*   speedSection  //航速控制節(jié) 繼承Section车伞，實現(xiàn)方法和Camera相同择懂，可作為對照，相對簡單
            |__ CameraSection*  cameraSection //相機控制節(jié) 繼承Section
            |__ YourSection*    yourSection   //如果要添加功能就要自己實現(xiàn)Section另玖，來存儲自定義命令困曙，在原版中是沒有的

2.2 數(shù)據(jù)處理過程

2.2.1 從文件或飛控中讀取任務(wù)

前面說了任務(wù)是線性列表結(jié)構(gòu)表伦，讀取的過程就是就是要轉(zhuǎn)換樹狀結(jié)構(gòu)，簡單說一下這個過程,代碼大家自行在項目中搜索慷丽。

1. 將所有任務(wù)都存到到 MissionController.visualItems 中蹦哼，只不過這個時候沒有處理，里面全部都是SimpleMissionItem要糊，任務(wù)數(shù)據(jù)對應(yīng)的是 SimpleMissionItem.missionItem, section中沒有數(shù)據(jù)
2. MissionController._scanForAdditionalSettings()順序遍歷 MissionController.visualItems 纲熏，如果是SimpleMissionItem,調(diào)用SimpleMissionItem.scanForMissionSettings(),調(diào)用成員對象speedSection.scanForSection(visualItems, scanIndex),cameraSection.scanForSection(visualItems, scanIndex)，你也要修改SimpleMissionItem調(diào)用你yourSection.scanForSection(visualItems, scanIndex)
3. 在Section.scanForSection(visualItems, scanIndex),查找scanIndex對應(yīng)的SimpleMissionItem.missionItem.command是否是自己需要的锄俄，如果是想辦法存起來局劲，刪除當(dāng)前item，返回true奶赠，如果不是返回false鱼填。注意一個section可以有多個命令共同組成。
4. 這樣就將線性結(jié)構(gòu)變成了樹狀結(jié)構(gòu)毅戈。這一段要仔細(xì)看碼才能讀懂苹丸。

還是用代看代碼比較清晰

void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle)
{
    int scanIndex = 0;
    while (scanIndex < visualItems->count()) {
        VisualMissionItem* visualItem = visualItems->value<VisualMissionItem*>(scanIndex);
        SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
        if (simpleItem) {
            scanIndex++;
            simpleItem->scanForSections(visualItems, scanIndex, vehicle);
        } else {
            // Complex item, can't have sections
            scanIndex++;
        }
    }
}

bool SimpleMissionItem::scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle)
{
    bool sectionFound = false;
    if (_cameraSection->available()) {
        sectionFound |= _cameraSection->scanForSection(visualItems, scanIndex);
    }
    if (_speedSection->available()) {
        sectionFound |= _speedSection->scanForSection(visualItems, scanIndex);
    }
    return sectionFound;
}

//如果時多個命令，一定在這將下面連續(xù)幾個屬于自己的命令都找到并刪除掉苇经，應(yīng)為命令都是連續(xù)的赘理，而且本次不讀完，就會混到航點中去扇单，污染結(jié)構(gòu)
bool SpeedSection::scanForSection(QmlObjectListModel* visualItems, int scanIndex)
{
    SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(scanIndex);
    MissionItem& missionItem = item->missionItem();
    // See SpeedSection::appendMissionItems for specs on what consitutes a known speed setting
    if (missionItem.command() == MAV_CMD_DO_CHANGE_SPEED && missionItem.param3() == -1 && ...) {
        visualItems->removeAt(scanIndex)->deleteLater();
        _flightSpeedFact.setRawValue(missionItem.param2());
        setSpecifyFlightSpeed(true);
        return true;
    }
    return false;
}

//CammeraSection 不貼太長了,道理一樣

2.2.2 轉(zhuǎn)換成任務(wù)文件或發(fā)送給飛控

相比起前面的識別這部分相對簡單,同樣代碼大家自行在項目中搜索商模。

1. MissionController._convertToMissionItems()遍歷visualItems，如果是SimpleMissionItem令花，調(diào)用SimpleMissionItem.appendMissionItems (QList<MissionItem>& items, QObject missionItemParent)
2. SimpleMissionItem 將自己的missionItem，append到items中凉倚。并且調(diào)用所有section->appendSectionItems(items, missionItemParent, seqNum);
3. Section根據(jù)自己的實際情況在appendSectionItems(QList<MissionItem>& items, QObject missionItemParent, int& nextSequenceNumber)函數(shù)中將任務(wù)轉(zhuǎn)換為一個或多個命令兼都，append到list中。

上碼

bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMissionItems, QList<MissionItem*>& rgMissionItems, QObject* missionItemParent)
{
    bool endActionSet = false;
    int lastSeqNum = 0;
    for (int i=0; i<visualMissionItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(visualMissionItems->get(i));
        lastSeqNum = visualItem->lastSequenceNumber();
        visualItem->appendMissionItems(rgMissionItems, missionItemParent);
    }
    // Mission settings has a special case for end mission action
    MissionSettingsItem* settingsItem = visualMissionItems->value<MissionSettingsItem*>(0);
    if (settingsItem) {
        endActionSet = settingsItem->addMissionEndAction(rgMissionItems, lastSeqNum + 1, missionItemParent);
    }

    return endActionSet;
}

void SimpleMissionItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    int seqNum = sequenceNumber();
    items.append(new MissionItem(missionItem(), missionItemParent));
    seqNum++;
    _cameraSection->appendSectionItems(items, missionItemParent, seqNum);
    _speedSection->appendSectionItems(items, missionItemParent, seqNum);
}

//自己定義時可以有多個稽寒，因為復(fù)雜命令一個7個參數(shù)可能不夠用
void SpeedSection::appendSectionItems(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum)
{
    if (_specifyFlightSpeed) {
        MissionItem* item = 
            new MissionItem(seqNum++,
                    MAV_CMD_DO_CHANGE_SPEED, //這是命令號
                    MAV_FRAME_MISSION, //這個地方一定寫成這個值扮碧，表示隨航點一同執(zhí)行，不然飛控不會執(zhí)行
                    _vehicle->multiRotor() ? 1 /* groundspeed */ : 0 /* airspeed */,    // p1 Change airspeed or groundspeed
                    _flightSpeedFact.rawValue().toDouble(),                             // p2
                    -1,                                                                 // p3 No throttle change
                    0,                                                                  // p4 Absolute speed change
                    0, 0, 0,                                                            // param 5-7 not used
                    true,                                                               // autoContinue
                    false,                                                              // isCurrentItem
                    missionItemParent);
        items.append(item);
}
//CammeraSection 不貼太長了,道理一樣

2.2.3 添加自己的Section

當(dāng)然是要創(chuàng)建一個類實現(xiàn) Section 了杏糙，最重要的是實現(xiàn)解析函數(shù) scanForSection 和生成函數(shù) appendSectionItems 慎王，當(dāng)然也要有自己的數(shù)據(jù)內(nèi)容，這是我們加這個東西的核心目的宏侍。

還有要注意的是一旦在界面上修改核心數(shù)據(jù)時一定要 emit Section 定義的一些信號赖淤，以便更新這個任務(wù)鏈的序號以及更新前臺顯示的序號。

最后手動搜索 cammeraSection或 speedSection ,在它們出現(xiàn)的地方谅河，加上我們定義的section對應(yīng)操作咱旱。當(dāng)然要區(qū)分是不是它們自己專有的确丢，標(biāo)準(zhǔn)就是看Section中有沒有定義，沒有就是專有的吐限。

還是有必要把Section.h拉出來看看

class Section : public QObject
{
    Q_OBJECT

public:
    Section(Vehicle* vehicle, QObject* parent = NULL)
        : QObject(parent)
        , _vehicle(vehicle)
    {

    }

    Q_PROPERTY(bool     available           READ available          WRITE setAvailable  NOTIFY availableChanged)
    Q_PROPERTY(bool     settingsSpecified   READ settingsSpecified                      NOTIFY settingsSpecifiedChanged)
    Q_PROPERTY(bool     dirty               READ dirty              WRITE setDirty      NOTIFY availableChanged)

    virtual bool available          (void) const = 0;
    virtual bool settingsSpecified  (void) const = 0;
    virtual bool dirty              (void) const = 0;

    virtual void setAvailable       (bool available) = 0;
    virtual void setDirty           (bool dirty) = 0;

    /// Scans the loaded items for the section items
    ///     @param visualItems Item list
    ///     @param scanIndex Index to start scanning from
    /// @return true: section found, items added, scanIndex updated
    virtual bool scanForSection(QmlObjectListModel* visualItems, int scanIndex) = 0;

    /// Appends the mission items associated with this section
    ///     @param items List to append to
    ///     @param missionItemParent QObject parent for created MissionItems
    ///     @param nextSequenceNumber[in,out] Sequence number for first item, updated as items are added
    virtual void appendSectionItems(QList<MissionItem*>& items, QObject* missionItemParent, int& nextSequenceNumber) = 0;

    /// Returns the number of mission items represented by this section.
    ///     Signals: itemCountChanged
    virtual int itemCount(void) const = 0; //沒數(shù)據(jù)返回0鲜侥，需要多少命令就返回多少以appendSectionItems()產(chǎn)生的為準(zhǔn)

signals:
    void availableChanged           (bool available);
    void settingsSpecifiedChanged   (bool settingsSpecified);
    void dirtyChanged               (bool dirty); //好像沒啥用
    void itemCountChanged           (int itemCount); //這是我覺得最重要的信號量，一旦改數(shù)據(jù)一般都要emit它

protected:
    Vehicle* _vehicle;
};

2.3 修改前臺

Talk is cheap诸典， show me your code.

//planView.qml ====================================================================================
QGCView {
    //...
    // Mission Item Editor
    Item {
        id:                     missionItemEditor
        anchors.left:           parent.left
        anchors.right:          parent.right
        anchors.top:            rightControls.bottom
        anchors.topMargin:      ScreenTools.defaultFontPixelHeight * 0.5
        anchors.bottom:         parent.bottom
        anchors.bottomMargin:   ScreenTools.defaultFontPixelHeight * 0.25
        visible:                _editingLayer == _layerMission && !planControlColapsed
        QGCListView {
            id:             missionItemEditorListView
            anchors.fill:   parent
            spacing:        ScreenTools.defaultFontPixelHeight / 4
            orientation:    ListView.Vertical
            model:          _missionController.visualItems //所有的航點數(shù)據(jù)
            cacheBuffer:    Math.max(height * 2, 0)
            clip:           true
            currentIndex:   _missionController.currentPlanViewIndex
            highlightMoveDuration: 250
            visible:        _editingLayer == _layerMission && !planControlColapsed
            //-- List Elements
            delegate: MissionItemEditor { //所有航點的編輯界面描函，不僅僅是WAYPOINT
                map:                editorMap
                masterController:  _planMasterController
                missionItem:        object
                width:              parent.width
                readOnly:           false
                rootQgcView:        _qgcView
                onClicked:  {
                    _missionController.setCurrentPlanViewIndex(object.sequenceNumber, false);
                }
                onRemove: {
                    var removeIndex = index
                    _missionController.removeMissionItem(removeIndex)
                    if (removeIndex >= _missionController.visualItems.count) {
                        removeIndex--
                    }
                    _missionController.setCurrentPlanViewIndex(removeIndex, true)
                }
                onInsertWaypoint:       insertSimpleMissionItem(editorMap.center, index)
                onInsertComplexItem:    insertComplexMissionItem(complexItemName, editorMap.center, index)
            }
        }
    }
    //...
}

//MissionItemEditor.qml ==================================================================================
Rectangle {
    property var    missionItem  
    // ...
    //這個地方就是加載具體編輯器的地方
    Loader {
        id:                 editorLoader
        anchors.leftMargin: _margin
        anchors.topMargin:  _margin
        anchors.left:       parent.left
        anchors.top:        commandPicker.bottom
        source:             missionItem.editorQml //在scanForSections中定義的是qrc:/qml/SimpleItemEditor.qml
        visible:            _currentItem

        property var    masterController:   _masterController
        property real   availableWidth:     _root.width - (_margin * 2) ///< How wide the editor should be
        property var    editorRoot:         _root
    }
}

//SimpleItemEditor.qml ================================================================================
Rectangle {
    // ...
    CameraSection {
        checked:    missionItem.cameraSection.settingsSpecified
        visible:    missionItem.cameraSection.available
    }
    // ...
    //如果自定義section就要自定一個QML來編輯，可以是使用Loader
    //或許定義單獨的組件就像CameraSection狐粱，需要在src/QmlControls/QGroundControl.Controls.qmldir中添加
    //具體怎么添加舀寓，項目搜索關(guān)鍵字CameraSection就知道了
}

//CameraSection.qml =================================================================================
Column {
    property var    _camera:        missionItem.cameraSection //竟然可以直接讀取包含它框架的屬性，見識了

    // ... 對代碼做了些簡化脑奠，看得清楚
    SectionHeader {
        id:             cameraSectionHeader
        text:           qsTr("Camera")
        checked:        false
    }

    FactComboBox {
        id:             cameraActionCombo
        anchors.left:   parent.left
        anchors.right:  parent.right
        fact:           _camera.cameraAction
        indexModel:     false
    }

    FactTextField {
        fact:                   _camera.cameraPhotoIntervalTime
        Layout.preferredWidth:  _fieldWidth
    }

    FactTextField {
        fact:                   _camera.cameraPhotoIntervalDistance
        Layout.preferredWidth:  _fieldWidth
    }
    // ...
    // 都是一些屬性數(shù)據(jù)的綁定基公，綁定也有技巧，一般是屬性綁定到編輯控件的值上宋欺，初始化的時候?qū)懙娇丶?    // QGC專門定制了一套FactXXX控件如果可以用的話用起來也挺好
}

• 關(guān)于界面轰豆，如果是命令與前序點的命令有關(guān)聯(lián)，可能需要做很多事齿诞，需要遍歷前序節(jié)點酸休；
• 如果控制參數(shù)比較復(fù)雜，就像我這個項目有幾百個參數(shù)就需要在PlanView寫專用界面來控制祷杈，Section中直接就是存的就是missionItem列表斑司，至于怎么生成它們，怎么將他們變成我要的參數(shù)但汞，都在這個控制器中宿刮。參數(shù)也是增量生成的，只發(fā)送和前序節(jié)點不一樣的數(shù)據(jù)私蕾。這樣據(jù)就需要知道MissionController.visualItems,和當(dāng)前的航點序號僵缺。而且每當(dāng) 當(dāng)前航點序號發(fā)生該改變時就要遍歷前序節(jié)點适荣，把前序節(jié)點的的狀態(tài)保存再來為當(dāng)前節(jié)點的改變做對比啡专，每當(dāng)用戶改變參數(shù)時都要把missionitem重新生成，防止用戶點下一個航點丟失數(shù)據(jù)器紧。

3 PX4中相機控制任務(wù)功能實現(xiàn)分析

3.1 上傳/下載命令

當(dāng)任務(wù)從QGC上傳到PX4上容贝，判斷是否支持當(dāng)前命令自脯，然后存儲到SD卡中，這步最重要的時要放行我們自定義的命令斤富，下載同理膏潮。

• src/modules/mavlink/mavlink_mission.cpp

//上傳解析
int
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item,
        struct mission_item_s *mission_item)
{
    //...
    if (mavlink_mission_item->frame == MAV_FRAME_MISSION) {
        switch (mavlink_mission_item->command) {
            //...
            case MAV_CMD_IMAGE_START_CAPTURE:
            case MAV_CMD_IMAGE_STOP_CAPTURE:
            case MAV_CMD_VIDEO_START_CAPTURE:
            case MAV_CMD_VIDEO_STOP_CAPTURE:
            case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
            case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
            case MAV_CMD_SET_CAMERA_MODE:
            //...
            mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
                break;
            //..
        }
    }
}

//下載編碼
int
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item,
        mavlink_mission_item_t *mavlink_mission_item){
    //...
    if (mission_item->frame == MAV_FRAME_MISSION) {
        switch (mavlink_mission_item->command) {
            //...
            case NAV_CMD_IMAGE_START_CAPTURE:
            case NAV_CMD_IMAGE_STOP_CAPTURE:
            case NAV_CMD_VIDEO_START_CAPTURE:
            case NAV_CMD_VIDEO_STOP_CAPTURE:
            case NAV_CMD_DO_MOUNT_CONFIGURE:
            case NAV_CMD_DO_MOUNT_CONTROL:
            case NAV_CMD_DO_SET_ROI:
            case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
            case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
            case NAV_CMD_SET_CAMERA_MODE:
            //...
                break;
        }
    }
}

3.2 在navigator模塊中放行

• 添加 NAV_CMD,在src/modules/navigator/navigation.h中添加命令，注意把'MAV'改為'NAV'開頭

/* compatible to mavlink MAV_CMD */
enum NAV_CMD {

    //...
    NAV_CMD_DO_MOUNT_CONFIGURE = 204,
    NAV_CMD_DO_MOUNT_CONTROL = 205,
    NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214,
    NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
    NAV_CMD_SET_CAMERA_MODE = 530,
    NAV_CMD_IMAGE_START_CAPTURE = 2000,
    NAV_CMD_IMAGE_STOP_CAPTURE = 2001,

    //...
    NAV_CMD_XXXX_AMP_SINGLE=56302, /* single XXXX power control |XXXX index| if 1 poweron the XXXX,0 poweroff XXXX| XXXX power amp,0~1,0| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)|  */
    NAV_CMD_XXXX_PARAM_MODULATE=56340, /* MODULATE parameters |XXXX index| | | | | | |  */
    //...
    NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
};

• 在導(dǎo)航模塊中放行命令满力，src/modules/navigator/mission_feasibility_checker.cpp

bool
MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
{
    bool
    MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
    {
        // check if we find unsupported items and reject mission if so
        if (missionitem.nav_cmd != NAV_CMD_IDLE &&
            missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
            //...
            missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
            missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
            missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
            missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
            missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
            missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
            missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
            //..
        ){
            mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1),
                         (int)missionitem.nav_cmd);
            return false;
        }
        //...
    }

bool
MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt)
{
    if (takeoff_index != -1) {
        // checks if all the mission items before the first takeoff waypoint
        // are not waypoints or position-related items;
        // this means that, before a takeoff waypoint, one can set
        // one of the bellow mission items
        for (size_t i = 0; i < (size_t)takeoff_index; i++) {
            if (missionitem.nav_cmd != NAV_CMD_IDLE &&
                //...
                missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
                missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
                missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
                missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
                missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
                missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
                missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
                //...
                ) {
                takeoff_first = false;

            } else {
                takeoff_first = true;

            }
        }

    }
}

3.3 到達(dá)航點發(fā)送任務(wù)

• 判斷是否達(dá)到目標(biāo)位置戏罢，表示任務(wù)可以執(zhí)行了屋谭，全部都可以發(fā)送了

src/modules/navigator/mission_block.cpp

bool
MissionBlock::is_mission_item_reached()
{
    /* handle non-navigation or indefinite waypoints */

    switch (_mission_item.nav_cmd) {
        //...
        case NAV_CMD_IMAGE_START_CAPTURE:
        case NAV_CMD_IMAGE_STOP_CAPTURE:
        case NAV_CMD_VIDEO_START_CAPTURE:
        case NAV_CMD_VIDEO_STOP_CAPTURE:
        case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
        case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
        case NAV_CMD_SET_CAMERA_MODE:
            return true;
        //...
    }    
}

• 定義一個模塊號,不和以往沖突就行

mavlink/include/mavlink/v2.0/common/common.h

typedef enum MAV_COMPONENT
{
    //...
        MAV_COMP_ID_CAMERA=100, /* Camera #1. | */
        MAV_COMP_ID_CAMERA2=101, /* Camera #2. | */
        MAV_COMP_ID_CAMERA3=102, /* Camera #3. | */
        MAV_COMP_ID_CAMERA4=103, /* Camera #4. | */
        MAV_COMP_ID_CAMERA5=104, /* Camera #5. | */
        MAV_COMP_ID_CAMERA6=105, /* Camera #6. | */
    //...
} MAV_COMPONENT;

• 發(fā)布uORB消息

src/modules/navigator/navigator_main.cpp

void
Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
{
    switch (vcmd->command) {
        //...
        case NAV_CMD_IMAGE_START_CAPTURE:
        case NAV_CMD_IMAGE_STOP_CAPTURE:
        case NAV_CMD_VIDEO_START_CAPTURE:
        case NAV_CMD_VIDEO_STOP_CAPTURE:
        case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
        case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
        case NAV_CMD_SET_CAMERA_MODE:

            vcmd->target_component = 100; // Camera #1，接受時需要通過這個值來判斷自己是不是需要處理
            break;
        //...
    }

    //發(fā)布消息
    if (_vehicle_cmd_pub != nullptr) {
        orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, vcmd);

    } else {
        _vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
    }
}

3.4 命令的最終執(zhí)行模塊/驅(qū)動

• 建立目錄
  src/drivers/camera_capture

• 創(chuàng)建頭h和cpp文件龟糕，這個文件我做了一些簡化

src/drivers/camera_capture/camera_capture.cpp

namespace camera_capture
{
CameraCapture   *g_camera_capture;
}

CameraCapture::CameraCapture() :
{

}

CameraCapture::~CameraCapture()
{
    camera_capture::g_camera_capture = nullptr;
}

void
CameraCapture::cycle_trampoline(void *arg)
{
    CameraCapture *dev = reinterpret_cast<CameraCapture *>(arg);
    dev->cycle();
}


void
CameraCapture::cycle()
{
    //訂閱命令消息桐磁，處理命令
    if (_command_sub < 0) {
        _command_sub = orb_subscribe(ORB_ID(vehicle_command));
    }

    bool updated = false;
    orb_check(_command_sub, &updated);
    // Command handling
    if (updated) {
        vehicle_command_s cmd;
        orb_copy(ORB_ID(vehicle_command), _command_sub, &cmd);
        // TODO : this should eventuallly be a capture control command
        if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { 
            // Enable/disable signal capture
            if (commandParamToInt(cmd.param1) == 1) {
                set_capture_control(true);
            } else if (commandParamToInt(cmd.param1) == 0) {
                set_capture_control(false);
            }
            // Reset capture sequence
            if (commandParamToInt(cmd.param2) == 1) {
                reset_statistics(true);
            }
        }
    }

    //低速循環(huán)隊列，比開進(jìn)程好多了
    work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, camera_capture::g_camera_capture,
           USEC2TICK(100000)); // 100ms
}


int
CameraCapture::start()
{
    /* allocate basic report buffers */
    _trig_buffer = new ringbuffer::RingBuffer(2, sizeof(_trig_s));

    if (_trig_buffer == nullptr) {
        return PX4_ERROR;
    }

    // start to monitor at low rates for capture control commands
    work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, this,
           USEC2TICK(1));

    return PX4_OK;
}

void
CameraCapture::stop()
{
    work_cancel(LPWORK, &_work);
    work_cancel(HPWORK, &_work_publisher);

    if (camera_capture::g_camera_capture != nullptr) {
        delete (camera_capture::g_camera_capture);
    }
}


extern "C" __EXPORT int camera_capture_main(int argc, char *argv[]);

int camera_capture_main(int argc, char *argv[])
{
    if (argc < 2) {
        return usage();
    }

    if (!strcmp(argv[1], "start")) {
        camera_capture::g_camera_capture = new CameraCapture();
        return 0;
    } else if (!strcmp(argv[1], "stop")) {
        camera_capture::g_camera_capture->stop();

    } else {
        return usage();
    }
    return 0;
}

• 創(chuàng)建編寫CMakeList.txt

px4_add_module(
    MODULE drivers__camera_capture
    MAIN camera_capture
    COMPILE_FLAGS
    SRCS
        camera_capture.cpp
)

• 在要生成的bord上加上這個驅(qū)動

boards/px4/fmu-vX/default.cmake

DRIVERS
        //...
        camera_capture
        //...

• 如果要自啟動需要在ROMFS/px4fmu_common/init.d/rcS上添加啟動命令

4 在飛行模式中的改進(jìn)

前面任務(wù)都完成了讲岁，但是用戶可能想知道任務(wù)執(zhí)行過程中設(shè)備的狀態(tài)我擂，這時就需要在QGC上修改，下面只有這些狀態(tài)的數(shù)據(jù)來源缓艳，怎樣處理仁者見仁了校摩。

基本都在下面QML上下功夫

/home/ted/src/qgroundcontrol/src/FlightDisplay/FlightDisplayView.qml

QGCView {
    property bool vehicleArmed:                 _activeVehicle ? _activeVehicle.armed : true // true here prevents pop up from showing during shutdown
    property bool vehicleWasArmed:              false
    property bool vehicleInMissionFlightMode:   _activeVehicle ? (_activeVehicle.flightMode === _activeVehicle.missionFlightMode) : false
    property bool promptForMissionRemove:       false

    //里面有任務(wù)數(shù)據(jù)
    property var    _missionController:     _planMasterController.missionController
    //當(dāng)前連接的飛機，非常有用阶淘，可以收發(fā)mavlink消息
    property var    _activeVehicle:         QGroundControl.multiVehicleManager.activeVehicle

    //通過它可以知道當(dāng)前飛到哪個航點
    property int currentMissionIndex : flightMissionController?(flightMissionController.currentMissionIndex):0
}

• 設(shè)備反饋模式衙吩，設(shè)備由串口通過PX4向QGC發(fā)送當(dāng)前的狀態(tài)，這個比較靠譜溪窒，方法請參見之前博文坤塞。

• 用戶還能可能需要手動操作，這就要定義新的MavLink 消息了澈蚌，個人覺得摹芙，還是以直接發(fā)送MAV_CMD同樣的數(shù)據(jù)比較好，QGC端命令生成不用變宛瞄，PX4只需要在mavlink模塊上加上接受這個自定義消息的處理浮禾，并像navigator模塊一樣發(fā)布UORB消息，執(zhí)行也完全不用動份汗。

傳送門: PX4 QGC透明串口轉(zhuǎn)發(fā)

1. PX4串口讀寫
2.自定義Mavlink消息實現(xiàn)QGC和PX4 mavlink守護進(jìn)程通信
3.自定義uORB消息實現(xiàn)盈电，實現(xiàn)PX4模塊間數(shù)據(jù)傳遞

最后再吐槽下CSDN，這篇文章在CSDN上的MarkDown編輯器下死活格式不對杯活，呵呵匆帚，搞副業(yè)的老板