QGC添加自定义组件和发送自定义MAVLINK消息
本文实现版本： QGC：Stable_V4.0; QT:5.12.6。下载和安装方式这边就不再赘述。下文仅作自身学习记录用途，侵删。
一、添加自定义组件
1.1 在飞行界面添加组件
在QGC开发者文档中说明飞行视图的界面实现在FlightDisplayView.qml文件中
在FlightDisplayView.qml文件中添加如下，可以在ToolStrip下面添加
Rectangle{
anchors.left: toolStrip.left // 左侧对齐
anchors.top: toolStrip.bottom // 顶部位于toolStrip控件底部
anchors.topMargin: _margins * 10 // 设置间隙
width: 150 // 长和高设置
height: 30
color: "black" // 底色设置
radius: 8 // 矩形圆角半径
visible: true // 设置为可见
z: _panel.z + 4 // 设置层级
// 填充文本
Text {
anchors.fill: parent
text: qsTr("Request all Parameter")
color: "white"
}
// 设置鼠标点击事件
MouseArea{
anchors.fill: parent
onClicked: {
console.log("Request all Parameter is clicked!") // 在控制台打印log
// 在单例QGroundControl的MultiVehicleManager对象下有一个当前正处于活动状态的activeVehicle，调用该方法
QGroundControl.multiVehicleManager.activeVehicle.requestAllParameters()
}
}
}
节选自开发者文档
1.2 实现组件事件
在Vehicle.h中添加 requestAllParameters()函数实现。如果需要能够在qml中实现访问的话，就需要在函数声明之前加上Q_INVOKABLE。
Q_INVOKABLE void requestAllParameters(void);
然后在Vehicle.cc中对这个函数进行实现
void Vehicle::requestAllParameters()
{
mavlink_message_t msg; // 定义mavlink消息
mavlink_msg_param_request_list_pack_chan( // 在chan通道上打包消息
_mavlink->getSystemId(), // 本机系统id
_mavlink->getComponentId(), // 本机组件id
priorityLink()->mavlinkChannel(), // 选择通道
&msg, _id, MAV_COMP_ID_ALL); // 对应的消息，所要发送的消息的目标系统id，目标组件id
sendMessageOnLink(priorityLink(), msg); // 发送消息
qDebug() << "============= sned Vehicle::requestAllParameters ============ " << _id << MAV_COMP_ID_ALL;
}
mavlink_msg_param_request_list_pack_chan()函数解释如下：
/**
* @brief Pack a param_request_list message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t target_system,uint8_t target_component)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
#else
mavlink_param_request_list_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST;
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC);
}
1.3 在MOCK模拟链接中实现验证
在MockLink.cc文件中，找到
void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
并在其中添加代码段
qDebug() << "message with param request list is received, and message id: " << msg.msgid
<< " target system: " << request.target_system << "target_component: " << request.target_component;
如下所示
void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
{
if (_failureMode == MockConfiguration::FailParamNoReponseToRequestList) {
return;
}
mavlink_param_request_list_t request;
mavlink_msg_param_request_list_decode(&msg, &request);
qDebug() << "message with param request list is received, and message id: " << msg.msgid
<< " target system: " << request.target_system << "target_component: " << request.target_component;
Q_ASSERT(request.target_system == _vehicleSystemId);
Q_ASSERT(request.target_component == MAV_COMP_ID_ALL);
// Start the worker routine
_currentParamRequestListComponentIndex = 0;
_currentParamRequestListParamIndex = 0;
}
保存并且编译
1.4 验证
如下为正常显示结果
按下面步骤操作
QT控制台显示结果如下：系统ID=128，组件ID=0，消息ID=21
二、自定义MAVLINK消息的一些预备知识
关于mavlink消息的解释，网上已经有很多了，这里也就不再多说，推荐还是去官网仔细阅读一下。
其消息包封装过程如下（图片来源：MAVLink协议通信分析——（二）消息结构）：
由用户自定义MSG和PAYLOAD内容，其余部分则由MAVLINK自动添加封装成包。
在QGC中，自定义MAVLINK消息实现在如下路径
/home/你的用户名/qgroundcontrol/libs/mavlink/include/mavlink/v2.0/message_definitions
关于这个文件夹下各个文件的具体作用，建议查看这边：Dialects
这边主要来看一下common.xml文件内容，该文件主要实现的是关于MAVLINK各种消息结构的定义实现。我们以0号消息HEARTBEAT为例。在官网中，HEARTBEAT内部结构定义如下：
对应的common.xml中对其结构进行了具体的定义：
<message id="0" name="HEARTBEAT">
<description>The heartbeat message shows that a system or component is present and responding. The type and autopilot fields (along with the message component id), allow the receiving system to treat further messages from this system appropriately (e.g. by laying out the user interface based on the autopilot). This microservice is documented at https://mavlink.io/en/services/heartbeat.html</description>
<field type="uint8_t" name="type" enum="MAV_TYPE">Vehicle or component type. For a flight controller component the vehicle type (quadrotor, helicopter, etc.). For other components the component type (e.g. camera, gimbal, etc.). This should be used in preference to component id for identifying the component type.</field>
<field type="uint8_t" name="autopilot" enum="MAV_AUTOPILOT">Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers.</field>
<field type="uint8_t" name="base_mode" enum="MAV_MODE_FLAG" display="bitmask">System mode bitmap.</field>
<field type="uint32_t" name="custom_mode">A bitfield for use for autopilot-specific flags</field>
<field type="uint8_t" name="system_status" enum="MAV_STATE">System status flag.</field>
<field type="uint8_t_mavlink_version" name="mavlink_version">MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version</field>
</message>
可以看出与官网给出的结构一致。
MAVLINK消息通过mavgenerate生成具体的消息头文件，该头文件在QGC中会保存在下面这个路径
/home/你的用户名/qgroundcontrol/libs/mavlink/include/mavlink/v2.0/common
内部文件主要以mavlink_msg_xxx.h形式命名，找到对应的mavlink_msg_heartbeat.h。内部实现了关于HEARTBEAT这个消息的结构体以及消息长度等定义。
#define MAVLINK_MSG_ID_HEARTBEAT 0
MAVPACKED(
typedef struct __mavlink_heartbeat_t {
uint32_t custom_mode; /*< A bitfield for use for autopilot-specific flags*/
uint8_t type; /*< Vehicle or component type. For a flight controller component the vehicle type (quadrotor, helicopter, etc.). For other components the component type (e.g. camera, gimbal, etc.). This should be used in preference to component id for identifying the component type.*/
uint8_t autopilot; /*< Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers.*/
uint8_t base_mode; /*< System mode bitmap.*/
uint8_t system_status; /*< System status flag.*/
uint8_t mavlink_version; /*< MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version*/
}) mavlink_heartbeat_t;
#define MAVLINK_MSG_ID_HEARTBEAT_LEN 9
#define MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN 9
#define MAVLINK_MSG_ID_0_LEN 9
#define MAVLINK_MSG_ID_0_MIN_LEN 9
#define MAVLINK_MSG_ID_HEARTBEAT_CRC 50
#define MAVLINK_MSG_ID_0_CRC 50
除此之外，其内部定义了一些用于消息打包或者解析的函数：
mavlink_msg_heartbeat_pack(); // Pack a heartbeat message
mavlink_msg_heartbeat_pack_chan(); // Pack a heartbeat message on a channel
mavlink_msg_heartbeat_encode(); // Encode a heartbeat struct
mavlink_msg_heartbeat_encode_chan(); // Encode a heartbeat struct on a channel
mavlink_msg_heartbeat_send(); // Send a heartbeat message
mavlink_msg_heartbeat_send_struct(); // Send a heartbeat message
...
事实上，在每个mavlink_msg_xxx.h的mavlink消息定义中，都实现了以下一些函数（其中xxx为对应的消息名称，如heartbeat、local_position_ned_system_global_offset等等）
mavlink_msg_xxx_pack() // 打包消息
mavlink_msg_xxx_pack_chan() // 在通道上打包消息（较常用）
mavlink_msg_xxx_encode(); // 对消息进行编码
mavlink_msg_xxx_encode_chan(); // 在通道上对消息进行编码
mavlink_msg_xxx_send(); // 发送消息
mavlink_msg_xxx_send_struct(); // 发送消息
...
也就是说，在你用mavgenerate自动生成你定义好的消息时，相关的结构体定义以及对应的操作函数它也帮你实现好了，是不是很方便呢？
关于mavgenerate的下载和使用，请看这边：
源码下载地址
Generating MAVLink Libraries
了解完这些之后，我们就可以开始在QGC中自定义MAVLINK消息了。
三、QGC自定义MAVLINK消息
3.1 生成新的MAVLINK消息头文件
首先在如下路径文件中添加2条新的消息
/home/你的用户名/qgroundcontrol/libs/mavlink/include/mavlink/v2.0/message_definitions/common.xml
内容如下：
<message id="12" name="PREFLIGHT_SELFCHECK">
<description>Ask vechile to do self-check before flight.</description>
<field type="uint8_t" name="target_system">The system doing self-check</field>
</message>
<message id="13" name="PREFLIGHT_SELFCHECK_ACK">
<description>Vechile self-check ack.</description>
<field type="uint8_t" name="ack">The result of self-check ack.1:healthy 0:unhealthy</field>
</message>
然后，使用mavgenerate生成新的消息头文件，在如下路径下（下载的mavlink文件夹内，我是直接放在了home下面）
/home/你的用户名/mavlink
打开新的终端，输入
python3 -m mavgenerate
选择XML路径为：
/home/你的用户名/qgroundcontrol/libs/mavlink/include/mavlink/v2.0/message_definitions/ardupilotmega.xml
再选择输出路径（可以是自己定义的文件夹）
然后选择如下：
点击生成，成功后终端显示如下
在OUT选择的路径下的common文件夹中，会自动生成两个文件，分别为：mavlink_msg_preflight_selfcheck.h和mavlink_msg_preflight_selfcheck_ack.h。有兴趣的同学可以打开来看一下里面的内容是否和第2章中所述一致。
我们将这两个文件拷贝进QGC路径下，具体为：
/home/你的用户名/qgroundcontrol/libs/mavlink/include/mavlink/v2.0/common
3.2 QGC源码内添加内容
用QT打开QGC源码文件，利用下方的查找框可以快速查找对应的文件
首先，我们在common.h文件中，添加头文件（大概是在2000行左右的位置，这段全是头文件包含）
#include "./mavlink_msg_preflight_selfcheck.h"
#include "./mavlink_msg_preflight_selfcheck_ack.h"
然后快速查找到MAVLINK_MESSAGE_INFO定义处，添加内容如下：
MAVLINK_MESSAGE_INFO_PREFLIGHT_SELFCHECK,
MAVLINK_MESSAGE_INFO_PREFLIGHT_SELFCHECK_ACK,
具体位置为
#define MAVLINK_MESSAGE_INFO{
…
MAVLINK_MESSAGE_INFO_AUTH_KEY,MAVLINK_MESSAGE_INFO_OPERATION,
MAVLINK_MESSAGE_INFO_SET_MODE,
MAVLINK_MESSAGE_INFO_PREFLIGHT_SELFCHECK,
MAVLINK_MESSAGE_INFO_PREFLIGHT_SELFCHECK_ACK,
MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ,
...
}
同样的，在MAVLINK_MESSAGE_NAMES中进行添加
{
“PREFLIGHT_SELFCHECK”,12},
{
“PREFLIGHT_SELFCHECK_ACK”,13}
具体位置为
MAVLINK_MESSAGE_NAMES{
…
{
“POWER_STATUS”, 125}, {
“PREFLIGHT_SELFCHECK”,12},
{
“PREFLIGHT_SELFCHECK_ACK”,13},{
“PROTOCOL_VERSION”,300},
...
}
再找到ardupilotmega.h文件，在内部添加程序如下：
#define MAVLINK_MESSAGE_CRCS{
…
{
11, 89, 6, 1, 4, 0},
{
12, 219, 1, 1, 0, 0}, {
13, 199, 1, 0, 0, 0}, // 这行是添加的内容
{
20, 214, 20, 3, 2, 3}
}
3.3 QGC源码内实现消息发送和接收
首先在Vehicle.h中添加两个接口
Q_INVOKABLE void selfCheck(void);
void _handleSelfCheckAck(mavlink_message_t& message);
并在Vehicle.cc文件中添加如下实现：
void Vehicle::selfCheck()
{
mavlink_message_t msg;
mavlink_msg_preflight_selfcheck_pack_chan(_mavlink->getSystemId(),
_mavlink->getComponentId(),
priorityLink()->mavlinkChannel(),
&msg, _id);
sendMessageOnLink(priorityLink(), msg);
qDebug() << "=== vehivle send preflight_selfcheck message === " << _id;
}
void Vehicle::_handleSelfCheckAck(mavlink_message_t &message)
{
mavlink_preflight_selfcheck_ack_t ack;
mavlink_msg_preflight_selfcheck_ack_decode(&message, &ack);
qDebug() << "=== Vehicle receive selfcheck_ack_t === " << ack.ack;
}
然后同样还是在Vehicle.cc文件中，找到_mavlinkMessageReceived()函数，在其内部的switch-case语句中添加：
case MAVLINK_MSG_ID_PREFLIGHT_SELFCHECK_ACK:
_handleSelfCheckAck(message);
break;
实现对接收到的MAVLINK消息的解析。
3.4 Mock内部模拟消息处理
在MockLink.h文件中添加如下函数声明
void _handleSelfCheck(const mavlink_message_t& msg);
然后在对应的MockLink.cc文件中，添加函数实现
void MockLink::_handleSelfCheck(const mavlink_message_t& msg)
{
mavlink_preflight_selfcheck_t selfCheck;
mavlink_msg_preflight_selfcheck_decode(&msg, &selfCheck);
if (selfCheck.target_system == _vehicleSystemId){
mavlink_message_t msg;
mavlink_msg_preflight_selfcheck_ack_pack_chan(_vehicleSystemId,
_vehicleComponentId,
_mavlinkChannel,
&msg, 1);
respondWithMavlinkMessage(msg);
}
}
在同一文件内部找到_handleIncomingMavlinkBytes()函数，在内部的switch-case语句中添加如下消息处理
case MAVLINK_MSG_ID_PREFLIGHT_SELFCHECK:
_handleSelfCheck(msg);
break;
然后在第1章中的FlightDisplayView.qml文件中，对原控件内容进行修改，修改内容如下：
Rectangle{
anchors.left: toolStrip.left // 左侧对齐
anchors.top: toolStrip.bottom // 顶部位于toolStrip控件底部
anchors.topMargin: _margins * 10 // 设置间隙
width: 150 // 长和高设置
height: 30
color: "black" // 底色设置
radius: 8 // 矩形圆角半径
visible: true // 设置为可见
z: _panel.z + 4 // 设置层级
// 填充文本
Text {
anchors.fill: parent
text: qsTr("SelfCheck")//qsTr("Request all Parameter")
color: "white"
}
// 设置鼠标点击事件
MouseArea{
anchors.fill: parent
onClicked: {
QGroundControl.multiVehicleManager.activeVehicle.selfCheck();
}
}
}
保存并编译，后续操作过程和第一章一致，结果实现如下：
以上内容仅作学习分享用途，侵删