PX4实战之旅（二）：通过自定义mavlink消息和QGC通信
PX4 1.13.0QGC 4.0.11雷讯X7飞控PX4 1.13.0以前版本的固件生成自定义mavlink消息文件的方法可以参考本文。修改下图路径中的common.xml文件在文件的下图位置加上注意id不能与其他的消息相同，我这里取的是12921。然后编译先make clean再编译完成后会在下图的路径中生成假设PX4源码的路径为~/PX4-Autopilot在下创建文件夹，在该文件夹中创建，
·
文章目录
前言
PX4 1.13.2
QGC 4.2.4
pix2.4.8飞控
PX4二次开发快速入门（八）：自定义mavlink消息和QGC通信
一、PX4生成自定义mavlink消息文件
PX4 1.13.0以及更新版本的固件生成自定义mavlink消息文件的方法老版本固件不同，老版本固件生成自定义MAVLINk消息可以参考：
https://mbot1.blog.csdn.net/article/details/108802979
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_mavlink
MAIN test_mavlink
COMPILE_FLAGS
#-DDEBUG_BUILD # uncomment for PX4_DEBUG output
#-O0 # uncomment when debugging
SRCS
test_mavlink.cpp
DEPENDS
px4_work_queue
)
编辑Kconfig文件
menuconfig MODULES_TEST_MAVLINK
bool "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 module
return 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 arguments
while ((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 parameters
parameters_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 updates
if (_parameter_update_sub.updated() || force)
{
// clear update
parameter_update_s update;
_parameter_update_sub.copy(&update);
// update parameters from storage
updateParams();
}
}
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;
// Subscriptions
uORB::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 https://github.com/mavlink/mavlink.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.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: mavlinkConsolePage
pageComponent: pageComponent
pageName: qsTr("Mavlink Test")
Mavlinktest {
id: test_mavlink
}
TextEdit {
id: textEdit
x: 285
y: 189
width: 81
height: 25
color: "#f5f3f3"
text: qsTr("TEST1")
font.family: "Times New Roman"
font.pixelSize: 12
}
Button {
id: button
x: 372
y: 189
text: qsTr("发送")
onClicked: {
test_mavlink._sendcom(textEdit.text,textEdit1.text,textEdit2.text)
}
}
TextEdit {
id: textEdit1
x: 285
y: 220
width: 81
height: 25
color: "#f5f3f3"
text: qsTr("TEST2")
font.pixelSize: 12
}
TextEdit {
id: textEdit2
x: 285
y: 251
width: 81
height: 25
color: "#f5f3f3"
text: qsTr("TEST3")
font.pixelSize: 12
}
Label {
id: label
x: 285
y: 317
width: 30
height: 19
text: qsTr("接收")
}
TextEdit {
id: textEdit3
x: 286
y: 354
width: 80
height: 19
color: "#f5f3f3"
text: test_mavlink.test1
font.pixelSize: 12
}
TextEdit {
id: textEdit4
x: 286
y: 391
width: 80
height: 19
color: "#f5f3f3"
text: test_mavlink.test2
font.pixelSize: 12
}
TextEdit {
id: textEdit5
x: 286
y: 424
width: 80
height: 19
color: "#f5f3f3"
text:test_mavlink.test3
font.pixelSize: 12
}
// Component
} // AnalyzePage
在qgroundcontrol.qrc中将上面创建的QML资源添加进去，取别名叫MavlinkTestPage.qml。注意添加完后一定不要忘了把下面别名那个输入框填上别名，注意别名不要写错，不要忘了加上后缀.qml。否则如果后面QGCCorePlugin.cc引用的时候和别名不一样，会导致MavlinkTestPage页面里是空的，不是MavlinkTestPage.qml设计的页面
然后修改QGCCorePlugin.cc文件
添加一行
_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.cc
创建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 model
setStringList(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 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
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 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
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 duplicates
if (_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_OBJECT
public:
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.cc注册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发送给地面站，并在地面站上显示。
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
19
0- 0
已为社区贡献24条内容
所有评论(0)