2. 自定義Mavlink消息實現(xiàn)QGC和PX4 mavlink守護進程通信

設計構(gòu)想

QGC <--mavlink(數(shù)傳)--> PX4(mavlink守護進程) <--uORB--> raw_serial_rtx <--UART--> device

• 上一節(jié)實現(xiàn)了raw_serial_rtx <--UART--> device
• 本節(jié)實現(xiàn) QGC <--mavlink(數(shù)傳)--> PX4(mavlink守護進程)
• 功能：QGC向PX4發(fā)送數(shù)據(jù)包哈雏，PX4收到后诫钓，原樣返回辽话，QGC再實現(xiàn)康聂，實現(xiàn)收發(fā)完整過程荔棉。
• 網(wǎng)上一些前輩濒募，將PX官網(wǎng)自定義消息跑了一遍坪郭，沒有自定義消息環(huán)節(jié)吭产，也沒有GCS接收環(huán)節(jié)侣监，還和uORB混在一起，著實看不懂臣淤，特別是自定義消息接收環(huán)節(jié)有坑橄霉，希望這篇文章可以給大家?guī)蜕厦Α?/li>

2.1 自定義Mavlink消息

2.1.1 安裝mavlink消息生成器

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

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

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

2.1.2 創(chuàng)建消息定義XML

• 創(chuàng)建方法參見How to Define MAVLink Messages & Enums
• 創(chuàng)建rsrtx.xml,其實一個消息也可解決問題，但在Mavlink轉(zhuǎn)發(fā)可能不出問題伐谈，還是寫兩個吧纲菌，一個上行，一個下行

<?xml version="1.0"?>
<mavlink>
  <enums>
    <enum name="RSRTX_OPT_DEV_ENUM">
      <description>which port send to or receive </description>
      <entry value="0" name="DEV_TTYS0">
        <description>/dev/ttyS0</description>
      </entry>
      <entry value="1" name="DEV_TTYS1">
        <description>/dev/ttyS1</description>
      </entry>
      <entry value="2" name="DEV_TTYS2">
        <description>/dev/ttyS2</description>
      </entry>
      <entry value="3" name="DEV_TTYS3">
        <description>/dev/ttyS3</description>
      </entry>
      <entry value="4" name="DEV_TTYS4">
        <description>/dev/ttyS4</description>
      </entry>
      <entry value="5" name="DEV_TTYS5">
        <description>/dev/ttyS5</description>
      </entry>
      <entry value="6" name="DEV_TTYS6">
        <description>/dev/ttyS6</description>
      </entry>
    </enum>
  </enums>
  <!--ID不能與其他消息重復刻剥，v2使用24字節(jié)的ID號，因為前面踩坑的原因把97611滩字，97612改成了211和213造虏，應該不影響-->
  <messages>
    <!--從地面站到無人機-->
    <message id="211" name="RTX_GCS2UAV">
      <description>Message form GCS to UAV</description>
      <field type="uint8_t" name="dev">which port send to ,see RSRTX_OPT_DEV_ENUM</field>
      <field type="uint8_t" name="len">pkg lenght , 250 max</field>
      <!--需要根據(jù)實際調(diào)整，如果是數(shù)據(jù)大麦箍，實時性要求不高漓藕，盡量調(diào)大，反之亦然挟裂，整個包不能大于255享钞，下同-->
      <field type="uint8_t[250]" name="data">data payload</field>
    </message>
    <!--從無人機到地面站-->
    <message id="213" name="RTX_UAV2GCS">
      <description>Message form UAV to GCS</description>
      <field type="uint8_t" name="dev">which port send this pkg,see RSRTX_OPT_DEV_ENUM</field>
      <field type="uint8_t" name="len">pkg lenght , 250 max</field>
      <field type="uint8_t[250]" name="data">data payload</field>
    </message>
  </messages>
</mavlink>

• 使用python -m mavgenerate創(chuàng)建消息頭文件,xml就是上面定義的xml文件，OUT是一個文件夾诀蓉，就是頭文件的輸出位置栗竖，最好是個空目錄。語言選C渠啤，協(xié)議選2.0

mavgen.png

• 生成一些文件和一個目錄rsrtx狐肢，在rsrtx目錄下有三個文件比較重要，已用紅線標注(開始在XML中將GCS敲成CGS沥曹，XML已修改份名，截圖沒改，應該不影響閱讀)

  
  
  mavgen1.png

mavgen2.png

• 在生成的rsrtx.h中注釋一些沖突

/*
#if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX
# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_RTX_GCS2UAV, MAVLINK_MESSAGE_INFO_RTX_UAV2GCS}
# define MAVLINK_MESSAGE_NAMES {{ "RTX_GCS2UAV", 211 }, { "RTX_UAV2GCS", 213 }}
# if MAVLINK_COMMAND_24BIT
#  include "../mavlink_get_info.h"
# endif
#endif
*/

2.2 將消息添加到PX4上

2.2.1 拷貝頭文件

• 將上面生成rsrtx文件夾拷貝到mavlink/include/mavlink/v2.0目錄下
• 在/mavlink/include/mavlink/v2.0/common/common.h中添加

...
#include "./mavlink_msg_smart_battery_status.h"
#include "./mavlink_msg_time_estimate_to_target.h"
#include "./mavlink_msg_wheel_distance.h"
//添加生成的頭文件
#include "../rsrtx/rsrtx.h"

• 編譯一遍看能否通過make px4fmu-v2_default
• 坑點1妓美，px4使用的是引用common.xml的standard.xml空方言（別問我怎么知道的僵腺，填坑的崩潰我自己知道就好），而在mavlink數(shù)據(jù)包中有一個校驗和字段壶栋，如果不把校驗和說明添加進去怎么發(fā)都接不到正常的包（呵呵辰如，為了這個經(jīng)驗，折騰兩天贵试，代碼看得眼睛都紅了琉兜，笑著笑著就哭了）
• 在mavlink/include/mavlink/v2.0/standard/standard.h添加校驗和說明，注意一定要按照msgid順序在原有的基礎(chǔ)上插入(注意查看211锡移，213呕童，太長了用省略號)

//在rsrtx.h 找到對應的校驗和說明，按照由小自大的順序插到原來的table中,mavlink_helper會使用排序二分法查找淆珊，如果不按順序就找不到夺饲。
#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0},...{149, 200, 30, 60, 0, 0, 0},{211, 35, 252, 252, 0, 0, 0}, {213, 62, 252, 252, 0, 0, 0},{230, 163, 42, 42, 0, 0, 0},...}

//在rsrtx.h 找到對應宏定義，按照id號排序，在QGC里拍了往声，這里也跟著按順序排吧
#define MAVLINK_MESSAGE_INFO {...,MAVLINK_MESSAGE_INFO_LANDING_TARGET,MAVLINK_MESSAGE_INFO_RTX_GCS2UAV, MAVLINK_MESSAGE_INFO_RTX_UAV2GCS, MAVLINK_MESSAGE_INFO_ESTIMATOR_STATUS,...}

//在rsrtx.h 找到對應宏定義擂找，插到最后，應該是按字母表排序浩销，沒找到地方使用
#define MAVLINK_MESSAGE_NAMES {...{ "RTX_GCS2UAV", 211 }, { "RTX_UAV2GCS", 213 }}

2.2.2 編寫收發(fā)代碼

• 參考src/modules/mavlink/mavlink_parameters.h
  創(chuàng)建src/modules/mavlink/mavlink_raw_serial_rtx.h

#pragma once
#include "mavlink_bridge_header.h"
#include <drivers/drv_hrt.h>

class Mavlink;

class MavlinkRawSerialRTX
{
public:
    explicit MavlinkRawSerialRTX(Mavlink *mavlink);
    ~MavlinkRawSerialRTX();
    void handle_message(const mavlink_message_t *msg);
private:
    MavlinkRawSerialRTX(MavlinkRawSerialRTX &);
    MavlinkRawSerialRTX &operator = (const MavlinkRawSerialRTX &);

    int sendData(uint8_t dev,uint8_t len,uint8_t * data );

    Mavlink *_mavlink;
};

• 參考src/modules/mavlink/mavlink_parameters.cpp
  創(chuàng)建src/modules/mavlink/mavlink_raw_serial_rtx.cpp

#include <stdio.h>
#include "mavlink_raw_serial_rtx.h"
#include "mavlink_main.h"

MavlinkRawSerialRTX::MavlinkRawSerialRTX(Mavlink *mavlink) :
    _mavlink(mavlink)
{
}

MavlinkRawSerialRTX::~MavlinkRawSerialRTX()
{
}

void
MavlinkRawSerialRTX::handle_message(const mavlink_message_t *msg)
{
    //接收消息
    switch (msg->msgid){
        case MAVLINK_MSG_ID_RTX_GCS2UAV:{
            mavlink_rtx_gcs2uav_t pkg ;
            mavlink_msg_rtx_gcs2uav_decode(msg,&pkg);

            //收到之后原樣回發(fā)
            sendData(pkg.dev,pkg.len,pkg.data);

        }
        default:
            break;
    }
}

//向地面站直接發(fā)送消息
int
MavlinkRawSerialRTX::sendData(uint8_t dev,uint8_t len,uint8_t * data ){
    if(len>sizeof(mavlink_rtx_uav2gcs_t::data) || _mavlink==nullptr )
        return -1;

    mavlink_rtx_uav2gcs_t pkg;
    pkg.dev = dev;
    pkg.len = len;
    memcpy(pkg.data,data,len);

    mavlink_msg_rtx_uav2gcs_send_struct(_mavlink->get_channel(),&pkg);

    return 0;
}

• 修改src/modules/mavlink/mavlink_receiver.h

...
//加頭文件
#include "mavlink_raw_serial_rtx.h"
//...

class MavlinkReceiver
{
public:
    //...
    MavlinkFTP          _mavlink_ftp;
    MavlinkLogHandler       _mavlink_log_handler;
    MavlinkTimesync         _mavlink_timesync;
    MavlinkMissionManager       _mission_manager;
    MavlinkParametersManager    _parameters_manager;
    //添加一個public成員
    MavlinkRawSerialRTX _mavlink_rawSerialRTX;
    //...
};

• 修改src/modules/mavlink/mavlink_receiver.cpp

//...
//在構(gòu)造函數(shù)中初始化_mavlink_rawSerialRTX成員
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
    _mavlink(parent),
    _mavlink_ftp(parent),
    _mavlink_log_handler(parent),
    _mavlink_timesync(parent),
    _mission_manager(parent),
    _parameters_manager(parent),
    _mavlink_rawSerialRTX(parent),
    _p_bat_emergen_thr(param_find("BAT_EMERGEN_THR")),
    _p_bat_crit_thr(param_find("BAT_CRIT_THR")),
    _p_bat_low_thr(param_find("BAT_LOW_THR")),
    _p_flow_rot(param_find("SENS_FLOW_ROT")),
    _p_flow_maxr(param_find("SENS_FLOW_MAXR")),
    _p_flow_minhgt(param_find("SENS_FLOW_MINHGT")),
    _p_flow_maxhgt(param_find("SENS_FLOW_MAXHGT"))
{
    /* Make the attitude quaternion valid */
    _att.q[0] = 1.0f;
}
//...
void *
MavlinkReceiver::receive_thread(void *arg)
{
//...
    /* handle packet with log component */
    _mavlink_log_handler.handle_message(&msg);

    /* handle packet with timesync component */
    _mavlink_timesync.handle_message(&msg);

    /* 處理地面站向飛控發(fā)送的透明串口數(shù)據(jù)贯涎，在消息轉(zhuǎn)發(fā)前處理 */
    _mavlink_rawSerialRTX.handle_message(&msg);

    /* handle packet with parent object */
    _mavlink->handle_message(&msg);
//...
}

• 在src/modules/mavlink/CMakeLists.txt > SRC節(jié)下
  添加mavlink_raw_serial_rtx.cpp
• 編譯查看是否有語法錯誤 make px4fmu-v2_default

2.3 將消息添加到QGC上并實現(xiàn)收發(fā)

本節(jié)中所有路徑都是QGC工程下的相對路徑

2.3.1 拷貝頭文件

• 方法基本和 2.2.1 相同
• 將生成的rsrtx目錄拷貝到libs/mavlink/include/mavlink/v2.0下
• 在libs/mavlink/include/mavlink/v2.0/common/common.h中添加

...
#include "./mavlink_msg_smart_battery_status.h"
#include "./mavlink_msg_time_estimate_to_target.h"
#include "./mavlink_msg_wheel_distance.h"
//添加生成的頭文件
#include "../rsrtx/rsrtx.h"

• 坑點2，和上面PX4坑點相同慢洋，但是QGC中使用的是ardupilotmega方言塘雳，可能是為了兼容Ardupilot飛控，所以在QGC里需要修改libs/mavlink/include/mavlink/v2.0/ardupilotmega/ardupilotmega.h

//在rsrtx.h 找到對應的校驗和說明普筹，按照由小自大的順序插到原來的table中,mavlink_helper會使用排序二分法查找败明，如果不按順序就找不到。
#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0},...{149, 200, 30, 60, 0, 0, 0},{211, 35, 252, 252, 0, 0, 0}, {213, 62, 252, 252, 0, 0, 0},{230, 163, 42, 42, 0, 0, 0},...}

//在rsrtx.h 找到對應宏定義太防，也是要插入到149和230之間妻顶，位置基本不影響主要功能，但是MAvLinkeInspector會排序查找蜒车，找不到看不到
#define MAVLINK_MESSAGE_INFO {...,, MAVLINK_MESSAGE_INFO_GIMBAL_CONTROL,MAVLINK_MESSAGE_INFO_RTX_GCS2UAV, MAVLINK_MESSAGE_INFO_RTX_UAV2GCS, MAVLINK_MESSAGE_INFO_GIMBAL_TORQUE_CMD_REPORT,...}

//在rsrtx.h 找到對應宏定義讳嘱，插到最后，應該是按字母表排序酿愧，沒找到地方使用
#define MAVLINK_MESSAGE_NAMES {...{ "RTX_GCS2UAV", 211 }, { "RTX_UAV2GCS", 213 }}

• 使用QT Creator編譯一遍看能否通過沥潭，這次編譯耗時24分鐘...
• 可以在QT Creator項目 build > 構(gòu)建步驟中為make測試多線程編譯，我的筆記本是6核12線程的寓娩，設置為-j12叛氨，具體設置參考make命令呼渣，完全編譯時間縮短到5分半棘伴，這就可以接受了，抽根煙喝口水的功夫屁置。

QGCMAKE.png

2.3.2 設計一個簡單的qml界面

• 考慮到QGC的Mavlink控制臺結(jié)構(gòu)比較獨立焊夸，功能也和我們要做的差不多，就仿造它寫界面和功能代碼蓝角，只需要少量修改就可以完成功能阱穗。其實界面和功能是協(xié)同開發(fā)的，大家在看界面的QML代碼的時候要結(jié)合功能代碼看使鹅。
• 注意Mavlink控制臺只在桌面版出現(xiàn)揪阶，按照這個流程相信大家可以一直到其他地方
• 參考src/AnalyzeView/MavlinkConsolePage.qml
  創(chuàng)建src/AnalyzeView/RawSerilaRtxTest.qml

import QtQuick          2.3
import QtQuick.Controls 1.2
import QtQuick.Dialogs  1.2
import QtQuick.Layouts      1.2
import QGroundControl               1.0
import QGroundControl.Palette       1.0
import QGroundControl.FactSystem    1.0
import QGroundControl.FactControls  1.0
import QGroundControl.Controls      1.0
import QGroundControl.ScreenTools   1.0
import QGroundControl.Controllers   1.0
AnalyzePage {
    id:              serialTestPage
    pageComponent:   pageComponent
    pageName:        "板載串口調(diào)試"
    pageDescription: "通過Mavlink遠程調(diào)試飛控板載串口外接設備"
    property bool isLoaded: false

    Component {
        id: pageComponent

        ColumnLayout {
            id:     consoleColumn
            height: availableHeight
            width:  availableWidth

            Connections {
                target: rawSerialRtxTestController

                onDataChanged: {
                    // Keep the view in sync if the button is checked
                    if (isLoaded) {
                        if (followTail.checked) {
                            listview.positionViewAtEnd();
                        }
                    }
                }
            }

            Component {
                id: delegateItem
                Rectangle {
                    color:  qgcPal.windowShade
                    height: Math.round(ScreenTools.defaultFontPixelHeight * 0.1 + field.height)
                    width:  listview.width

                    QGCLabel {
                        id:          field
                        text:        display
                        width:       parent.width
                        wrapMode:    Text.NoWrap
                        font.family: ScreenTools.fixedFontFamily
                        anchors.verticalCenter: parent.verticalCenter
                    }
                }
            }

            QGCListView {
                Component.onCompleted: {
                    isLoaded = true
                }
                Layout.fillHeight: true
                Layout.fillWidth:  true
                clip:              true
                id:                listview
                model:             rawSerialRtxTestController
                delegate:          delegateItem

                onMovementStarted: {
                    followTail.checked = false
                }
            }

            RowLayout {
                Layout.fillWidth:   true

                QGCTextField {
                    id:               serialString
                    Layout.fillWidth: true
                    placeholderText:  "在此鍵入要發(fā)送的數(shù)據(jù)"
                    onAccepted: {
                        rawSerialRtxTestController.sendCommand(text)
                        text = ""
                    }
                    Keys.onPressed: {
                        if (event.key == Qt.Key_Up) {
                            text = rawSerialRtxTestController.historyUp(text);
                            event.accepted = true;
                        } else if (event.key == Qt.Key_Down) {
                            text = rawSerialRtxTestController.historyDown(text);
                            event.accepted = true;
                        }
                    }
                }

                QGCButton {
                    id:        followTail
                    text:      "顯示最新"
                    checkable: true
                    checked:   true

                    onCheckedChanged: {
                        if (checked && isLoaded) {
                            listview.positionViewAtEnd();
                        }
                    }
                }
            }
        }
    } // Component
} // AnalyzePage

• 在qgroundcontrol.qrc中添加一個QML資源,取別名叫RawSerilaRtxTest.qml,已用綠線標出。
  

  ADDRAWQML.png

• 在src/AnalyzeView/AnalyzeView.qm添加一個按鈕患朱，鏈接RawSerilaRtxTest.qml

//...
MavlinkConsoleController {
    id: conController
}

RawSerialRtxTestController {
    id: rawSerialRtxTestController
}
//...
 Repeater {
    id: buttonRepeater
    model: ListModel {
        ListElement {
            buttonImage:        "/qmlimages/LogDownloadIcon"
            buttonText:         qsTr("Log Download")
            pageSource:         "LogDownloadPage.qml"
        }
        ListElement {
            buttonImage:        "/qmlimages/GeoTagIcon"
            buttonText:         qsTr("GeoTag Images")
            pageSource:         "GeoTagPage.qml"
        }
        ListElement {
            buttonImage:        "/qmlimages/MavlinkConsoleIcon"
            buttonText:         qsTr("Mavlink Console")
            pageSource:         "MavlinkConsolePage.qml"
        }
        //添加的連接,圖標懶得改了
        ListElement {
            buttonImage:        "/qmlimages/MavlinkConsoleIcon"
            buttonText:         "板載串口調(diào)試"
            pageSource:         "RawSerilaRtxTest.qml"
        }
    }
 }
 //...

2.3.2 編寫收發(fā)邏輯代碼

• 編寫代碼之前請仔細分析數(shù)據(jù)鏈路鲁僚，特別是接收鏈路。QT通過 信號 emit / connect實現(xiàn)不同類的解耦合調(diào)用。當一個類emit一個信號冰沙，在其他語言里可能稱之為接口調(diào)用侨艾，這個類不需要知道到底有那些地方會實現(xiàn)，相當于發(fā)射信號但是不知道誰會去就收它拓挥；需要接收這個信號的類唠梨，按照信號的聲明，創(chuàng)建一個參數(shù)表相同的方法侥啤，使用connect方法去連接（接收）信號当叭。一個信號可以被多個類接收，非常靈活盖灸。
• Vehicle::_mavlinkReceivied()接收MalinkProtocol::messageReceived()信號科展，判斷消息ID為MAVLINK_MSG_ID_SERIAL_CONTROL，發(fā)出Vehicle::mavlinkSerialControl()信號糠雨，最后被MavlinkConsoleController::_receiveData()捕捉才睹，完成數(shù)據(jù)鏈路。
• 發(fā)現(xiàn)MAVLINK_MSG_SERIAL_CONTROL這個消息有意思甘邀，里面帶的device琅攘，baudrate數(shù)據(jù)段儼然就是專門為串口設計的，事實上也真是松邪，奈何PX4中只使用了SERIAL_CONTROL_DEV_SHELL坞琴，其他設備也不轉(zhuǎn)發(fā)，空高興~~~逗抑。
• 參考MAVLINK_MSG_SERIAL_CONTROL消息在src/Vehicle/Vehicle.h中申明信號的函數(shù)根剧辐。

//...
// MAVlink Serial Data
void mavlinkSerialControl(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);

//Raw Sersial Data 添加的信號函數(shù)根
void rawSerialDataReceived(uint8_t dev,uint8_t len,QByteArray data);
//...

• 在src/Vehicle/Vehicle.cc中過濾MAVLINK_MSG_ID_RTX_UAV2GCS消息，并發(fā)射rawSerialDataReceived信號

//...
case MAVLINK_MSG_ID_SERIAL_CONTROL:
{
    mavlink_serial_control_t ser;
    mavlink_msg_serial_control_decode(&message, &ser);
    emit mavlinkSerialControl(ser.device, ser.flags, ser.timeout, ser.baudrate, QByteArray(reinterpret_cast<const char*>(ser.data), ser.count));
}
    break;
//過濾MAVLINK_MSG_ID_RTX_UAV2GCS消息邮府，并發(fā)射rawSerialDataReceived信號
case MAVLINK_MSG_ID_RTX_UAV2GCS:
{
    mavlink_rtx_uav2gcs_t pkg;
    mavlink_msg_rtx_uav2gcs_decode(&message,&pkg);
    emit rawSerialDataReceived(pkg.dev,pkg.len,QByteArray(reinterpret_cast<const char*>(pkg.data),pkg.len));
}
    break;
//...

• 參考src/AnalyzeView/MavlinkConsoleController.h
  創(chuàng)建src/AnalyzeView/RawSerialRtxTestController.h
  看上去挺長的荧关，其實就是考過來沒改幾個字，下面cpp同理

#pragma once
#include "QmlObjectListModel.h"
#include "Fact.h"
#include "FactMetaData.h"
#include <QObject>
#include <QString>
#include <QMetaObject>
#include <QStringListModel>
// Fordward decls
class Vehicle;
class RawSerialRtxTestController : public QStringListModel
{
    Q_OBJECT
public:
    RawSerialRtxTestController();
    virtual ~RawSerialRtxTestController();
    Q_INVOKABLE void sendCommand(QString command);
    Q_INVOKABLE QString historyUp(const QString& current);
    Q_INVOKABLE QString historyDown(const QString& current);
private slots:
    void _setActiveVehicle  (Vehicle* vehicle);
    //修改符合信號參數(shù)表
    void _receiveData(uint8_t dev,uint8_t len,QByteArray data);
private:
    bool _processANSItext(QByteArray &line);
    void _sendSerialData(QByteArray, bool close = false);
    void writeLine(int line, const QByteArray &text);
    class CommandHistory
    {
    public:
        void append(const QString& command);
        QString up(const QString& current);
        QString down(const QString& current);
    private:
        static constexpr int maxHistoryLength = 100;
        QList<QString> _history;
        int _index = 0;
    };
    int           _cursor_home_pos;
    int           _cursor;
    QByteArray    _incoming_buffer;
    Vehicle*      _vehicle;
    QList<QMetaObject::Connection> _uas_connections;
    CommandHistory _history;
};

• 參考src/AnalyzeView/MavlinkConsoleController.cc
  創(chuàng)建src/AnalyzeView/RawSerialRtxTestController.cc
  將類名簡單替換,主要修改
  • 發(fā)送函數(shù) _sendSerialData(QByteArray data, bool close)
  • 接收函數(shù) _receiveData(uint8_t dev,uint8_t len,QByteArray data)

#include "RawSerialRtxTestController.h"
#include "QGCApplication.h"
#include "UAS.h"

RawSerialRtxTestController::RawSerialRtxTestController()
    : QStringListModel(),
      _cursor_home_pos{-1},
      _cursor{0},
      _vehicle{nullptr}
{
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    //連接信號activeVehicleChanged
    connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &RawSerialRtxTestController::_setActiveVehicle);
    _setActiveVehicle(manager->activeVehicle());
}

RawSerialRtxTestController::~RawSerialRtxTestController()
{
    if (_vehicle) {
        QByteArray msg("");
        _sendSerialData(msg, true);
    }
}

void
RawSerialRtxTestController::sendCommand(QString command)
{
    _history.append(command);
    command.append("\n");
    _sendSerialData(qPrintable(command));
    _cursor_home_pos = -1;
    _cursor = rowCount();
}

QString
RawSerialRtxTestController::historyUp(const QString& current)
{
    return _history.up(current);
}

QString
RawSerialRtxTestController::historyDown(const QString& current)
{
    return _history.down(current);
}

void
RawSerialRtxTestController::_setActiveVehicle(Vehicle* vehicle)
{
    for (auto &con : _uas_connections)
        disconnect(con);
    _uas_connections.clear();

    _vehicle = vehicle;

    if (_vehicle) {
        _incoming_buffer.clear();
        // Reset the model
        setStringList(QStringList());
        _cursor = 0;
        _cursor_home_pos = -1;
        //將Vehicle::rawSerialDataReceived連接到_receiveData
        _uas_connections << connect(_vehicle, &Vehicle::rawSerialDataReceived, this, &RawSerialRtxTestController::_receiveData);
    }
}


//接收數(shù)據(jù)
void
RawSerialRtxTestController::_receiveData(uint8_t dev,uint8_t len,QByteArray data)
{

    auto rc = rowCount();
    if (_cursor >= rc) {
        insertRows(rc, 1 + _cursor - rc);
    }
    auto idx = index(_cursor);
    setData(idx,  QString("%1 ttyS6 -> * [%2]").arg(QTime::currentTime().toString("HH:mm:ss.zzz")).arg(len));
    _cursor++;

    // Append incoming data and parse for ANSI codes
    _incoming_buffer.append(data);
    while(!_incoming_buffer.isEmpty()) {
        bool newline = false;
        int idx = _incoming_buffer.indexOf('\n');
        if (idx == -1) {
            // Read the whole incoming buffer
            idx = _incoming_buffer.size();
        } else {
            newline = true;
        }

        QByteArray fragment = _incoming_buffer.mid(0, idx);
        if (_processANSItext(fragment)) {
            writeLine(_cursor, fragment);
            if (newline)
                _cursor++;
            _incoming_buffer.remove(0, idx + (newline ? 1 : 0));
        } else {
            // ANSI processing failed, need more data
            return;
        }
    }
}

//發(fā)送數(shù)據(jù)
void
RawSerialRtxTestController::_sendSerialData(QByteArray data, bool close)
{
    auto rc = rowCount();
    if (_cursor >= rc) {
        insertRows(rc, 1 + _cursor - rc);
    }
    auto idx = index(_cursor);

    if (!_vehicle) {
         setData(idx,  "飛控未連接");
        _cursor++;
        return;
    }

    // Send maximum sized chunks until the complete buffer is transmitted
    while(data.size()) {
        QByteArray chunk{data.left(sizeof(mavlink_rtx_gcs2uav_t::data))};
        uint8_t flags = SERIAL_CONTROL_FLAG_EXCLUSIVE |  SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI;
        if (close) flags = 0;
        auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
        auto priority_link = _vehicle->priorityLink();
        mavlink_message_t msg;
        //使用定義消息時的生成的編碼函數(shù)褂傀，簡單測試這里把dev數(shù)據(jù)段寫死
        mavlink_msg_rtx_gcs2uav_pack_chan(
                   protocol->getSystemId(),
                   protocol->getComponentId(),
                   priority_link->mavlinkChannel(),
                   &msg,
                   RSRTX_OPT_DEV_ENUM::DEV_TTYS6,
                   chunk.size(),
                   reinterpret_cast<uint8_t*>(chunk.data()));

        setData(idx,  QString("%1 * -> ttyS6 [%2] ").arg(QTime::currentTime().toString("HH:mm:ss.zzz")).arg(chunk.size()));
         _cursor++;
        writeLine(_cursor, chunk);

        //發(fā)送
        _vehicle->sendMessageOnLink(priority_link, msg);
        data.remove(0, chunk.size());
    }

    _history.append(data);
}

bool
RawSerialRtxTestController::_processANSItext(QByteArray &line)
{
    // Iterate over the incoming buffer to parse off known ANSI control codes
    for (int i = 0; i < line.size(); i++) {
        if (line.at(i) == '\x1B') {
            // For ANSI codes we expect at least 3 incoming chars
            if (i < line.size() - 2 && line.at(i+1) == '[') {
                // Parse ANSI code
                switch(line.at(i+2)) {
                    default:
                        continue;
                    case 'H':
                        if (_cursor_home_pos == -1) {
                            // Assign new home position if home is unset
                            _cursor_home_pos = _cursor;
                        } else {
                            // Rewind write cursor position to home
                            _cursor = _cursor_home_pos;
                        }
                        break;
                    case 'K':
                        // Erase the current line to the end
                        if (_cursor < rowCount()) {
                            setData(index(_cursor), "");
                        }
                        break;
                    case '2':
                        // Check for sufficient buffer size
                        if ( i >= line.size() - 3)
                            return false;

                        if (line.at(i+3) == 'J' && _cursor_home_pos != -1) {
                            // Erase everything and rewind to home
                            bool blocked = blockSignals(true);
                            for (int j = _cursor_home_pos; j < rowCount(); j++)
                                setData(index(j), "");
                            blockSignals(blocked);
                            QVector<int> roles;
                            roles.reserve(2);
                            roles.append(Qt::DisplayRole);
                            roles.append(Qt::EditRole);
                            emit dataChanged(index(_cursor), index(rowCount()), roles);
                        }
                        // Even if we didn't understand this ANSI code, remove the 4th char
                        line.remove(i+3,1);
                        break;
                }
                // Remove the parsed ANSI code and decrement the bufferpos
                line.remove(i, 3);
                i--;
            } else {
                // We can reasonably expect a control code was fragemented
                // Stop parsing here and wait for it to come in
                return false;
            }
        }
    }
    return true;
}

void
RawSerialRtxTestController::writeLine(int line, const QByteArray &text)
{
    auto rc = rowCount();
    if (line >= rc) {
        insertRows(rc, 1 + line - rc);
    }
    auto idx = index(line);
    setData(idx, data(idx, Qt::DisplayRole).toString() + text);
}

void RawSerialRtxTestController::CommandHistory::append(const QString& command)
{
    if (command.length() > 0) {

        // do not append duplicates
        if (_history.length() == 0 || _history.last() != command) {

            if (_history.length() >= maxHistoryLength) {
                _history.removeFirst();
            }
            _history.append(command);
        }
    }
    _index = _history.length();
}

QString RawSerialRtxTestController::CommandHistory::up(const QString& current)
{
    if (_index <= 0)
        return current;

    --_index;
    if (_index < _history.length()) {
        return _history[_index];
    }
    return "";
}

QString RawSerialRtxTestController::CommandHistory::down(const QString& current)
{
    if (_index >= _history.length())
        return current;

    ++_index;
    if (_index < _history.length()) {
        return _history[_index];
    }
    return "";
}

• 將h和cpp源文件加入工程（如果QT Creator 沒有自動添加的話）qgroundcontrol.pro
• 在src/QGCApplication.cc注冊QML

//...
#include "MavlinkConsoleController.h"
//引入頭文件
#include "RawSerialRtxTestController.h"
//...
qmlRegisterType<MavlinkConsoleController>       (kQGCControllers,                       1, 0, "MavlinkConsoleController");
//注冊RawSerialRtxTestController為QML對象
qmlRegisterType<RawSerialRtxTestController>     (kQGCControllers,                       1, 0, "RawSerialRtxTestController");
//...

• QT Creator 編譯

2.4 功能測試

• 在QT Creator中運行QGC

• 使用QGC上傳編譯好的PX4固件

• 打開我們編寫的串口測試界面忍啤，輸入幾行

• USB連接來回耗時大致在 30+ms

QGCSERTEST0.png

• SIK數(shù)傳連接（57600波特）來回耗時大致在 300+ms，可能是定義數(shù)據(jù)包定義太長仙辟，傳短報文不占優(yōu)勢同波，。對于偶發(fā)丟包情況叠国，可以在串口協(xié)議上加ACK未檩，或者在發(fā)送時mavlink上加ack，還有就是要進一步優(yōu)化代碼粟焊，mavlink發(fā)送緩存不足（其它消息排隊還沒發(fā)完）冤狡，導致根本沒發(fā)送出去校赤，如果要做成產(chǎn)品，需要后期進一步優(yōu)化

QGCSERTEST1.png

• MavlinkInspector 查看消息接收

MavlinkInspector.png

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

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