PX4实战之旅（二）：通过自定义mavlink消息和QGC通信

之旅（二）：通过自定义mavlink消息和QGC通信"/>

PX4实战之旅（二）：通过自定义mavlink消息和QGC通信

文章目录

• 前言
• 一、PX4生成自定义mavlink消息文件
• 二、PX4创建自定义进程和uorb消息
• • 2.1创建uorb消息
  • 2.2创建自定义进程
• 三、PX4接收QGC消息
• • 修改mavlink_receiver.h
  • 修改mavlink_receiver.cpp
• 四、PX4发送消息给QGC
• • 在`src/modules/mavlink/streams`下新建`MAVLINK_TEST.hpp`
  • 修改mavlink_messages.cpp
  • 修改mavlink_main.cpp
• 五、在QGC中添加自定义mavlink消息
• • 安装MAVLINK生成器mavgenerate
  • 修改XML文件并生成MAVLINK库文件
• 六、QGC消息处理
• • 设计界面
  • 消息处理
• 七、测试

前言

PX4 1.13.2
QGC 4.2.4
pix2.4.8飞控

一、PX4生成自定义mavlink消息文件

PX4 1.13.0以及更新版本的固件生成自定义mavlink消息文件的方法老版本固件不同，老版本固件生成自定义MAVLINk消息可以参考：

PX4 1.13.0以前版本的固件生成自定义mavlink消息文件的方法可以参考本文。
修改下图路径中的common.xml文件

在文件的下图位置加上

<message id="12921" name="TEST_MAVLINK"><description>Test Mavlink.</description><field type="uint8_t" name="test1"> </field><field type="int16_t" name="test2"> </field><field type="float" name="test3"> </field>
</message>

注意id不能与其他的消息相同，我这里取的是12921。
然后编译
先

make clean

再

make cuav_x7pro_default

编译完成后会在下图的路径中生成mavlink_msg_test_mavlink.h

二、PX4创建自定义进程和uorb消息

假设PX4源码的路径为~/PX4-Autopilot

2.1创建uorb消息

定义两个消息

gedit ~/PX4-Autopilot/msg/test_mavlink_rx.msg

gedit ~/PX4-Autopilot/msg/test_mavlink_tx.msg

都填入下面的内容

uint64 timestamp # time since system start (microseconds)uint8 test1
int16 test2
float32 test3

修改CMakeLists.txt

gedit ~/PX4-Autopilot/msg/CMakeLists.txt

加入两行

test_mavlink_tx.msg
test_mavlink_rx.msg

如下图

然后编译固件生成消息头文件

2.2创建自定义进程

在PX4-Autopilot/src/modules下创建test_mavlink文件夹，

在该文件夹中创建CMakeLists.txt，Kconfig，test_mavlink.cpp，test_mavlink.h四个文件
编辑CMakeLists，填入以下内容后保存退出

px4_add_module(MODULE modules__test_mavlinkMAIN test_mavlinkCOMPILE_FLAGS#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output#-O0             # uncomment when debuggingSRCStest_mavlink.cppDEPENDSpx4_work_queue)

编辑Kconfig文件

menuconfig MODULES_TEST_MAVLINKbool "test_mavlink"default n---help---Enable support for test_mavlink

编辑test_mavlink.cpp文件
填写内容如下：

#include "test_mavlink.h"#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>int test_mavlink::print_status()
{PX4_INFO("Running");// TODO: print additional runtime information about the state of the modulereturn 0;
}int test_mavlink::custom_command(int argc, char *argv[])
{/*if (!is_running()) {print_usage("not running");return 1;}// additional custom commands can be handled like this:if (!strcmp(argv[0], "do-something")) {get_instance()->do_something();return 0;}*/return print_usage("unknown command");
}int test_mavlink::task_spawn(int argc, char *argv[])
{_task_id = px4_task_spawn_cmd("module",SCHED_DEFAULT,SCHED_PRIORITY_DEFAULT,1024,(px4_main_t)&run_trampoline,(char *const *)argv);if (_task_id < 0){_task_id = -1;return -errno;}return 0;
}test_mavlink *test_mavlink::instantiate(int argc, char *argv[])
{int example_param = 0;bool example_flag = false;bool error_flag = false;int myoptind = 1;int ch;const char *myoptarg = nullptr;// parse CLI argumentswhile ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF){switch (ch){case 'p':example_param = (int)strtol(myoptarg, nullptr, 10);break;case 'f':example_flag = true;break;case '?':error_flag = true;break;default:PX4_WARN("unrecognized flag");error_flag = true;break;}}if (error_flag){return nullptr;}test_mavlink *instance = new test_mavlink(example_param, example_flag);if (instance == nullptr){PX4_ERR("alloc failed");}return instance;
}test_mavlink::test_mavlink(int example_param, bool example_flag): ModuleParams(nullptr)
{
}void test_mavlink::run()
{// initialize parametersparameters_update(true);while (!should_exit()){
//        PX4_INFO("run\n");if(test_mavlink_rx_sub.updated()){PX4_INFO("updated\n");test_mavlink_rx_sub.copy(&_test_mavlink_rx);
//            PX4_INFO("_test_mavlink_rx.test1=%c\n",_test_mavlink_rx.test1);
//            PX4_INFO("_test_mavlink_rx.test2=%d\n",_test_mavlink_rx.test2);
//            PX4_INFO("_test_mavlink_rx.test3=%f\n",(double)_test_mavlink_rx.test3);_test_mavlink_tx.test1=_test_mavlink_rx.test1+1;_test_mavlink_tx.test2=_test_mavlink_rx.test2+1;_test_mavlink_tx.test3=_test_mavlink_rx.test3+1;test_mavlink_tx_pub.publish(_test_mavlink_tx);}PX4_INFO("_test_mavlink_rx.test1=%d\n",_test_mavlink_rx.test1);PX4_INFO("_test_mavlink_rx.test2=%d\n",_test_mavlink_rx.test2);PX4_INFO("_test_mavlink_rx.test3=%f\n",(double)_test_mavlink_rx.test3);px4_usleep(50000);parameters_update();}
}void test_mavlink::parameters_update(bool force)
{// check for parameter updatesif (_parameter_update_sub.updated() || force){// clear updateparameter_update_s update;_parameter_update_sub.copy(&update);// update parameters from storageupdateParams();}
}int test_mavlink::print_usage(const char *reason)
{if (reason){PX4_WARN("%s\n", reason);}PRINT_MODULE_DESCRIPTION(R"DESCR_STR(
### Description
Section that describes the provided module functionality.This is a template for a module running as a task in the background with start/stop/status functionality.### Implementation
Section describing the high-level implementation of this module.### Examples
CLI usage example:
$ module start -f -p 42)DESCR_STR");PRINT_MODULE_USAGE_NAME("module", "template");PRINT_MODULE_USAGE_COMMAND("start");PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);PRINT_MODULE_USAGE_DEFAULT_COMMANDS();return 0;
}int test_mavlink_main(int argc, char *argv[])
{return test_mavlink::main(argc, argv);
}

编辑test_mavlink.h文件
填写内容如下：

#pragma once#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/test_mavlink_tx.h>
#include <uORB/topics/test_mavlink_rx.h>
using namespace time_literals;extern "C" __EXPORT int test_mavlink_main(int argc, char *argv[]);class test_mavlink : public ModuleBase<test_mavlink>, public ModuleParams
{
public:test_mavlink(int example_param, bool example_flag);virtual ~test_mavlink() = default;/** @see ModuleBase */static int task_spawn(int argc, char *argv[]);/** @see ModuleBase */static test_mavlink *instantiate(int argc, char *argv[]);/** @see ModuleBase */static int custom_command(int argc, char *argv[]);/** @see ModuleBase */static int print_usage(const char *reason = nullptr);/** @see ModuleBase::run() */void run() override;/** @see ModuleBase::print_status() */int print_status() override;private:/*** Check for parameter changes and update them if needed.* @param parameter_update_sub uorb subscription to parameter_update* @param force for a parameter update*/void parameters_update(bool force = false);DEFINE_PARAMETERS((ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */)test_mavlink_tx_s _test_mavlink_tx;test_mavlink_rx_s _test_mavlink_rx;// SubscriptionsuORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};uORB::Subscription test_mavlink_rx_sub{ORB_ID(test_mavlink_rx)};uORB::Publication<test_mavlink_tx_s>	test_mavlink_tx_pub{ORB_ID(test_mavlink_tx)};
};

编辑下图编译脚本

加入一行

CONFIG_MODULES_TEST_MAVLINK=y

如下：

三、PX4接收QGC消息

修改mavlink_receiver.h

添加自定义消息头文件：

#include <uORB/topics/test_mavlink_rx.h>

添加自定义消息处理函数

void handle_message_test_mavlink_rx(mavlink_message_t *msg);

添加话题发布

uORB::Publication<test_mavlink_rx_s> _test_mavlink_rx_pub{ORB_ID(test_mavlink_rx)};

修改mavlink_receiver.cpp

调用消息处理函数：

case MAVLINK_MSG_ID_TEST_MAVLINK:handle_message_test_mavlink_rx(msg);break;

实现消息处理函数，主要是解析mavlink消息并通过uorb发布：

void
MavlinkReceiver::handle_message_test_mavlink_rx(mavlink_message_t *msg)
{mavlink_test_mavlink_t mavlink_test_msg;mavlink_msg_test_mavlink_decode(msg, &mavlink_test_msg);test_mavlink_rx_s __test_mavlink_rx;__test_mavlink_rx.test1=mavlink_test_msg.test1;__test_mavlink_rx.test2=mavlink_test_msg.test2;__test_mavlink_rx.test3=mavlink_test_msg.test3;_test_mavlink_rx_pub.publish(__test_mavlink_rx);
}

四、PX4发送消息给QGC

在src/modules/mavlink/streams下新建MAVLINK_TEST.hpp

在MAVLINK_TEST.hpp中填入下面内容，主要是订阅test_mavlink_tx消息然后将消息内容打包为MAVLINK数据帧并发送：

#ifndef MAVLINK_TEST_HPP
#define MAVLINK_TEST_HPP#include <uORB/topics/test_mavlink_tx.h>
class MavlinkStreamMavlinktest : public MavlinkStream
{
public:static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamMavlinktest(mavlink); }static constexpr const char *get_name_static() { return "TEST_MAVLINK"; }static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TEST_MAVLINK; }const char *get_name() const override { return get_name_static(); }uint16_t get_id() override { return get_id_static(); }unsigned get_size() override{return test_mavlink_tx_sub.advertised() ? MAVLINK_MSG_ID_TEST_MAVLINK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;}private:explicit MavlinkStreamMavlinktest(Mavlink *mavlink) : MavlinkStream(mavlink) {}uORB::Subscription test_mavlink_tx_sub{ORB_ID(test_mavlink_tx)};bool send() override{test_mavlink_tx_s test_mavlink_tx;if (test_mavlink_tx_sub.update(&test_mavlink_tx)) {mavlink_test_mavlink_t msg{};
msg.test1=test_mavlink_tx.test1;
msg.test2=test_mavlink_tx.test2;
msg.test3=test_mavlink_tx.test3;			mavlink_msg_test_mavlink_send_struct(_mavlink->get_channel(), &msg);return true;}return false;}
};#endif // MOUNT_ORIENTATION

修改mavlink_messages.cpp

添加hpp头文件

#include "streams/MAVLINK_TEST.hpp"

添加接口

#if defined(MAVLINK_TEST_HPP)create_stream_list_item<MavlinkStreamMavlinktest>(),
#endif // MAVLINK_TEST_HPP

修改mavlink_main.cpp

配置MAVLINK发送速率，注意这里要加在switch (_mode)上面

configure_stream_local("TEST_MAVLINK", unlimited_rate);

注意双引号里面的是消息的名称，在前面的MAVLINK_TEST.hpp中定义：

五、在QGC中添加自定义mavlink消息

QGC中的MAVLINK库和PX4中的不一样，使用的方言也不一样，因此两者的库不能直接复制粘贴用，在QGC中也需要进行定义

安装MAVLINK生成器mavgenerate

sudo apt-get install python3-pip

pip3 install --user future

sudo apt-get install python3-tk

git clone .git --recursive

下载后进入到工程文件夹

进入这个文件夹路径下执行

python3 -m mavgenerate

会出现下面这个窗口

修改XML文件并生成MAVLINK库文件

QGC采用的是ardupilotmega方言
修改QGC下图目录的ardupilotmega.xml

在最后加上

<message id="12921" name="TEST_MAVLINK"><description>Test Mavlink.</description><field type="uint8_t" name="test1"> </field><field type="int16_t" name="test2"> </field><field type="float" name="test3"> </field>
</message>

打开上一节安装的mavgenerate
XML选择修改后的ardupilotmega.xml，Out选择库文件的输出目录，Language选择C，Protocol选择2.0。最后点Generate

然后会在指定的输出目录下生成消息库文件

将生成的这些文件全部复制并覆盖到qgroundcontrol/libs/mavlink/include/mavlink/v2.0目录下

六、QGC消息处理

设计界面

参考src/AnalyzeView/MavlinkConsolePage.qml 创建src/AnalyzeView/MavlinkTestPage.qml 。

MavlinkTestPage.qml内容如下：

import QtQuick          2.3
import QtQuick.Controls 1.2
import QtQuick.Dialogs  1.2
import QtQuick.Layouts      1.2import 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.0AnalyzePage {id:              mavlinkConsolePagepageComponent:   pageComponentpageName:        qsTr("Mavlink Test")Mavlinktest {id: test_mavlink}TextEdit {id: textEditx: 285y: 189width: 81height: 25color: "#f5f3f3"text: qsTr("TEST1")font.family: "Times New Roman"font.pixelSize: 12}Button {id: buttonx: 372y: 189text: qsTr("发送")onClicked: {test_mavlink._sendcom(textEdit.text,textEdit1.text,textEdit2.text)}}TextEdit {id: textEdit1x: 285y: 220width: 81height: 25color: "#f5f3f3"text: qsTr("TEST2")font.pixelSize: 12}TextEdit {id: textEdit2x: 285y: 251width: 81height: 25color: "#f5f3f3"text: qsTr("TEST3")font.pixelSize: 12}Label {id: labelx: 285y: 317width: 30height: 19text: qsTr("接收")}TextEdit {id: textEdit3x: 286y: 354width: 80height: 19color: "#f5f3f3"text: test_mavlink.test1font.pixelSize: 12}TextEdit {id: textEdit4x: 286y: 391width: 80height: 19color: "#f5f3f3"text: test_mavlink.test2font.pixelSize: 12}TextEdit {id: textEdit5x: 286y: 424width: 80height: 19color: "#f5f3f3"text:test_mavlink.test3font.pixelSize: 12}// Component
} // AnalyzePage

在qgroundcontrol.qrc中将上面创建的QML资源添加进去，取别名叫MavlinkTestPage.qml。注意添加完后一定不要忘了把下面别名那个输入框填上别名，注意别名不要写错，不要忘了加上后缀.qml。否则如果后面QGCCorePlugin引用的时候和别名不一样，会导致MavlinkTestPage页面里是空的，不是MavlinkTestPage.qml设计的页面



然后修改QGCCorePlugin文件
添加一行

_p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Mavlinktest"),     QUrl::fromUserInput("qrc:/qml/MavlinkTestPage.qml"),      QUrl::fromUserInput("qrc:/qmlimages/MavlinkConsoleIcon"))));

消息处理

参考src/AnalyzeView/MavlinkConsoleController.h
创建src/AnalyzeView/Mavlinktest.h
参考src/AnalyzeView/MavlinkConsoleController
创建src/AnalyzeView/Mavlinktest.cpp

Mavlinktest.cpp内容如下：

#include "Mavlinktest.h"
#include "QGCApplication.h"
#include "UAS.h"
#include "MAVLinkInspectorController.h"
#include "MultiVehicleManager.h"
#include <QtCharts/QLineSeries>
Mavlinktest::Mavlinktest(): QStringListModel(),_cursor_home_pos{-1},_cursor{0},_vehicle{nullptr}
{auto *manager = qgcApp()->toolbox()->multiVehicleManager();connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &Mavlinktest::_setActiveVehicle);_setActiveVehicle(manager->activeVehicle());MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &Mavlinktest::_receiveMessage);
}Mavlinktest::~Mavlinktest()
{if (_vehicle){QByteArray msg;_sendSerialData(msg, true);}
}void
Mavlinktest::sendCommand(QString command)
{_history.append(command);command.append("\n");_sendSerialData(qPrintable(command));_cursor_home_pos = -1;_cursor = rowCount();
}QString
Mavlinktest::historyUp(const QString& current)
{return _history.up(current);
}QString
Mavlinktest::historyDown(const QString& current)
{return _history.down(current);
}void
Mavlinktest::_setActiveVehicle(Vehicle* vehicle)
{for (auto &con : _uas_connections){disconnect(con);}_uas_connections.clear();_vehicle = vehicle;if (_vehicle){_incoming_buffer.clear();// Reset the modelsetStringList(QStringList());_cursor = 0;_cursor_home_pos = -1;_uas_connections << connect(_vehicle, &Vehicle::mavlinkSerialControl, this, &Mavlinktest::_receiveData);}
}void
Mavlinktest::_receiveData(uint8_t device, uint8_t, uint16_t, uint32_t, QByteArray data)
{if (device != SERIAL_CONTROL_DEV_SHELL){return;}// auto idx = index(_cursor);
//setData(idx,  QString("%1 ttyS6 -> * [%2]").arg(QTime::currentTime().toString("HH:mm:ss.zzz")).arg(12));// 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 bufferidx = _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 datareturn;}}
}void
Mavlinktest::_receiveMessage(LinkInterface*, mavlink_message_t message)
{if(message.msgid==MAVLINK_MSG_ID_TEST_MAVLINK){qDebug()<<"receive MAVLINK_MSG_ID_TEST_MAVLINK";mavlink_test_mavlink_t mavlink_test;mavlink_msg_test_mavlink_decode(&message, &mavlink_test);//id=pwm.base_mode;
//_name=QString::number(heartbeat.base_mode);_test1=QString::number(mavlink_test.test1);_test2=QString::number(mavlink_test.test2);_test3=QString::number(mavlink_test.test3);emit valueChanged();qDebug()<<_test1<<_test2<<_test3;
//        qDebug()<<"从模式"<<heartbeat.custom_mode;}
}void Mavlinktest::_sendcom(QString test1,QString test2,QString test3)
{if (!_vehicle){qWarning() << "Internal error";return;}WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();if (!weakLink.expired()) {SharedLinkInterfacePtr sharedLink = weakLink.lock();if (!sharedLink) {qCDebug(VehicleLog) << "_handlePing: primary link gone!";return;}
//            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();auto priority_link =sharedLink;uint8_t send_test1=test1.toUInt();int16_t send_test2=test2.toShort();float send_test3=test3.toFloat();mavlink_message_t msg;mavlink_msg_test_mavlink_pack_chan(_vehicle->id(),1,priority_link->mavlinkChannel(),&msg,send_test1,send_test2,send_test3);_vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);qDebug()<<"send"<<send_test1<<send_test2<<send_test3;}
}
void
Mavlinktest::_sendSerialData(QByteArray data, bool close)
{if (!_vehicle){qWarning() << "Internal error";return;}WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();if (!weakLink.expired()) {SharedLinkInterfacePtr sharedLink = weakLink.lock();if (!sharedLink) {qCDebug(VehicleLog) << "_handlePing: primary link gone!";return;}// Send maximum sized chunks until the complete buffer is transmitted
//        while(data.size())
//        {
//            QByteArray chunk{data.left(MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN)};
//            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 =sharedLink;
//            mavlink_message_t msg;//            mavlink_msg_serial_control_pack_chan(
//                protocol->getSystemId(),
//                protocol->getComponentId(),
//                priority_link->mavlinkChannel(),
//                &msg,
//                SERIAL_CONTROL_DEV_SHELL,
//                flags,
//                0,
//                0,
//                chunk.size(),
//                reinterpret_cast<uint8_t*>(chunk.data()));
//            _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
//            data.remove(0, chunk.size());
//        }}}bool
Mavlinktest::_processANSItext(QByteArray &line)
{// Iterate over the incoming buffer to parse off known ANSI control codesfor (int i = 0; i < line.size(); i++){if (line.at(i) == '\x1B'){// For ANSI codes we expect at least 3 incoming charsif (i < line.size() - 2 && line.at(i+1) == '['){// Parse ANSI codeswitch(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 endif (_cursor < rowCount()){setData(index(_cursor), "");}break;case '2':// Check for sufficient buffer sizeif ( i >= line.size() - 3){return false;}if (line.at(i+3) == 'J' && _cursor_home_pos != -1){// Erase everything and rewind to homebool 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 charline.remove(i+3,1);break;}// Remove the parsed ANSI code and decrement the bufferposline.remove(i, 3);i--;}else{// We can reasonably expect a control code was fragemented// Stop parsing here and wait for it to come inreturn false;}}}return true;
}void
Mavlinktest::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 Mavlinktest::CommandHistory::append(const QString& command)
{if (command.length() > 0){// do not append duplicatesif (_history.length() == 0 || _history.last() != command){if (_history.length() >= maxHistoryLength){_history.removeFirst();}_history.append(command);}}_index = _history.length();
}QString Mavlinktest::CommandHistory::up(const QString& current)
{if (_index <= 0){return current;}--_index;if (_index < _history.length()){return _history[_index];}return "";
}QString Mavlinktest::CommandHistory::down(const QString& current)
{if (_index >= _history.length()){return current;}++_index;if (_index < _history.length()){return _history[_index];}return "";
}

Mavlinktest.h内容如下：

#pragma once#include "MAVLinkProtocol.h"
#include "Vehicle.h"
#include "QmlObjectListModel.h"
#include "Fact.h"
#include "FactMetaData.h"
#include <QObject>
#include <QString>
#include <QMetaObject>
#include <QStringListModel>// Fordward decls
class Vehicle;/// Controller for MavlinkConsole.qml.
class Mavlinktest : public QStringListModel
{Q_OBJECTpublic:Mavlinktest();virtual ~Mavlinktest();Q_INVOKABLE void sendCommand(QString command);Q_INVOKABLE void _sendcom(QString test1,QString test2,QString test3);Q_INVOKABLE QString historyUp(const QString& current);Q_INVOKABLE QString historyDown(const QString& current);Q_PROPERTY(QString          test1       READ test1      NOTIFY valueChanged)QString         test1           (){return _test1;}Q_PROPERTY(QString          test2       READ test2      NOTIFY valueChanged)QString         test2           (){return _test2;}Q_PROPERTY(QString          test3       READ test3      NOTIFY valueChanged)QString         test3           (){return _test3;}private slots:void _setActiveVehicle  (Vehicle* vehicle);void _receiveData(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);void _receiveMessage            (LinkInterface* link, mavlink_message_t message);
signals:void            valueChanged        ();void selectedChanged                ();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;};QString     _test1,_test2,_test3;int           _cursor_home_pos;int           _cursor;QByteArray    _incoming_buffer;Vehicle*      _vehicle;QList<QMetaObject::Connection> _uas_connections;CommandHistory _history;};

将.h和.cpp源文件加入工程（如果QT Creator 没有自动添加的话），修改qgroundcontrol.pro

src/AnalyzeView/Mavlinktest.h\
src/AnalyzeView/Mavlinktest.cpp\

分别添加到下图的位置：

在src/QGCApplication注册QML

先添加头文件

#include "Mavlinktest.h"

然后添加

qmlRegisterType<Mavlinktest> (kQGCControllers, 1, 0, "Mavlinktest");

经过上面注册后就可以在ＱＭＬ中用双引号里的类名进行直接构造，通过id来访问属性和方法，例如：

Mavlinktest {id: testsa
}

七、测试

将编辑好的PX4固件编译烧入飞控，编译生成QGC地面站

编译QGC后，界面如下，上面有三个输入框，输入需要发送的数据，点击发送即可通过自定义MAVLINK消息发送给飞控，下面有显示接收数据的地方，会将飞控飞控QGC的自定义MAVLIK消息解析后在下面显示。

将飞控通过USB连接地面站。先在MAVLINK Console里面输入

test_mavlink start

然后打开Mavlinktest那一栏，在“发送”按钮左边的三个输入框输入发送的数据，正常的话飞控会通过自定义MAVLINK消息接收到这些数据然后加1发送给地面站，并在地面站上显示。