2. 自定义Mavlink消息实现QGC和PX4 mavlink守护进程通信

设计构想

QGC <--mavlink(数传)--> PX4(mavlink守护进程) <--uORB--> raw_serial_rtx <--UART--> device

• 上一节实现了raw_serial_rtx <--UART--> device
• 本节实现 QGC <--mavlink(数传)--> PX4(mavlink守护进程)
• 功能：QGC向PX4发送数据包，PX4收到后，原样返回，QGC再实现，实现收发完整过程。
• 网上一些前辈，将PX官网自定义消息跑了一遍，没有自定义消息环节，也没有GCS接收环节，还和uORB混在一起，着实看不懂，特别是自定义消息接收环节有坑，希望这篇文章可以给大家帮上忙。

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 创建消息定义XML

• 创建方法参见How to Define MAVLink Messages & Enums
• 创建rsrtx.xml,其实一个消息也可解决问题，但在Mavlink转发可能不出问题，还是写两个吧，一个上行，一个下行

<?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字节的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>
      <!--需要根据实际调整，如果是数据大，实时性要求不高，尽量调大，反之亦然，整个包不能大于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创建消息头文件,xml就是上面定义的xml文件，OUT是一个文件夹，就是头文件的输出位置，最好是个空目录。语言选C，协议选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数据包中有一个校验和字段，如果不把校验和说明添加进去怎么发都接不到正常的包（呵呵，为了这个经验，折腾两天，代码看得眼睛都红了，笑着笑着就哭了）
• 在mavlink/include/mavlink/v2.0/standard/standard.h添加校验和说明，注意一定要按照msgid顺序在原有的基础上插入(注意查看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 编写收发代码

• 参考src/modules/mavlink/mavlink_parameters.h
  创建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
  创建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);

            //收到之后原样回发
            sendData(pkg.dev,pkg.len,pkg.data);

        }
        default:
            break;
    }
}

//向地面站直接发送消息
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

//...
//在构造函数中初始化_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);

    /* 处理地面站向飞控发送的透明串口数据，在消息转发前处理 */
    _mavlink_rawSerialRTX.handle_message(&msg);

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

• 在src/modules/mavlink/CMakeLists.txt > SRC节下
  添加mavlink_raw_serial_rtx.cpp
• 编译查看是否有语法错误 make px4fmu-v2_default

2.3 将消息添加到QGC上并实现收发

本节中所有路径都是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 > 构建步骤中为make测试多线程编译，我的笔记本是6核12线程的，设置为-j12，具体设置参考make命令，完全编译时间缩短到5分半，这就可以接受了，抽根烟喝口水的功夫。

QGCMAKE.png

2.3.2 设计一个简单的qml界面

• 考虑到QGC的Mavlink控制台结构比较独立，功能也和我们要做的差不多，就仿造它写界面和功能代码，只需要少量修改就可以完成功能。其实界面和功能是协同开发的，大家在看界面的QML代码的时候要结合功能代码看。
• 注意Mavlink控制台只在桌面版出现，按照这个流程相信大家可以一直到其他地方
• 参考src/AnalyzeView/MavlinkConsolePage.qml
  创建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:        "板载串口调试"
    pageDescription: "通过Mavlink远程调试飞控板载串口外接设备"
    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:  "在此键入要发送的数据"
                    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:         "板载串口调试"
            pageSource:         "RawSerilaRtxTest.qml"
        }
    }
 }
 //...

2.3.2 编写收发逻辑代码

• 编写代码之前请仔细分析数据链路，特别是接收链路。QT通过 信号 emit / connect实现不同类的解耦合调用。当一个类emit一个信号，在其他语言里可能称之为接口调用，这个类不需要知道到底有那些地方会实现，相当于发射信号但是不知道谁会去就收它；需要接收这个信号的类，按照信号的声明，创建一个参数表相同的方法，使用connect方法去连接（接收）信号。一个信号可以被多个类接收，非常灵活。
• Vehicle::_mavlinkReceivied()接收MalinkProtocol::messageReceived()信号，判断消息ID为MAVLINK_MSG_ID_SERIAL_CONTROL，发出Vehicle::mavlinkSerialControl()信号，最后被MavlinkConsoleController::_receiveData()捕捉，完成数据链路。
• 发现MAVLINK_MSG_SERIAL_CONTROL这个消息有意思，里面带的device，baudrate数据段俨然就是专门为串口设计的，事实上也真是，奈何PX4中只使用了SERIAL_CONTROL_DEV_SHELL，其他设备也不转发，空高兴~~~。
• 参考MAVLINK_MSG_SERIAL_CONTROL消息在src/Vehicle/Vehicle.h中申明信号的函数根。

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

//Raw Sersial Data 添加的信号函数根
void rawSerialDataReceived(uint8_t dev,uint8_t len,QByteArray data);
//...

• 在src/Vehicle/Vehicle.cc中过滤MAVLINK_MSG_ID_RTX_UAV2GCS消息，并发射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消息，并发射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
  创建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);
    //修改符合信号参数表
    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
  创建src/AnalyzeView/RawSerialRtxTestController.cc
  将类名简单替换,主要修改
  • 发送函数 _sendSerialData(QByteArray data, bool close)
  • 接收函数 _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);
    }
}


//接收数据
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;
        }
    }
}

//发送数据
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;
        //使用定义消息时的生成的编码函数，简单测试这里把dev数据段写死
        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);

        //发送
        _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数传连接（57600波特）来回耗时大致在 300+ms，可能是定义数据包定义太长，传短报文不占优势，。对于偶发丢包情况，可以在串口协议上加ACK，或者在发送时mavlink上加ack，还有就是要进一步优化代码，mavlink发送缓存不足（其它消息排队还没发完），导致根本没发送出去，如果要做成产品，需要后期进一步优化

QGCSERTEST1.png

• MavlinkInspector 查看消息接收

MavlinkInspector.png

传送门: PX4 QGC透明串口转发

1. PX4串口读写
2.自定义Mavlink消息实现QGC和PX4 mavlink守护进程通信
3.自定义uORB消息实现，实现PX4模块间数据传递