XBee模块实现QGC与PX4飞控的组网通信连接

本篇博客介绍如何利用XBee模块实现QGC地面站与飞控的通信

一、问题的提出

正如上一篇博客指出，PX4飞控原装数传模块（3DR Radio）只能一对一通信，并不能实现多机组网通信，而XBee模块可以弥补以上不足，因此，为实现后续多机编队分布式控制，用XBee模块来替代原装数传进行无线通信势在必行。鉴于地面站具备友好的显示功能，此项任务的核心工作就是实现基于XBee模块的QGC地面站与飞控之间的通信，这样就能为后续多机组网通信的调试奠定基础。

二、基本思路

从上篇博客中可以看出XBee模块有自己的通信协议，与PX4飞控原装的mavlink协议有较大的差异，因而可以想到修改与mavlink通信相关代码是实现XBee模块通信的关键。另外模块之间的通信，除去中间传输过程，两端节点要处理的其实就是根据通信协议对信息进行解码和编码。因此，添加XBee模块的基本思路如下：

找到QGC地面站中涉及到mavlink通信解码和编码部分程序，修改使之兼容XBee模块的通信协议；
同样地，找到PX4飞控源码中涉及到mavlink通信解码和编码部分程序，修改使之兼容XBee模块的通信协议；
通信测试和功能完善。

三、开发过程

如下：

3-1 QGC地面站部分修改

QGC地面站部分源码的学习主要参考B站UP主胡萝卜科技上传的教学视频。根据视频中的相关介绍，与mavlink通信协议的编/解码相关的程序代码主要位于以下三个文件中（位于文件夹：../qgroundcontrol/libs/mavlink/include/mavlink/v2.0/）：

mavlink_helpers.h
mavlink_types.h
checksum.h

其中， mavlink_helpers.h文件中有解码和编码的函数，mavlink_types.h给出了与通信协议相关的变量定义（如数据包帧头，编/解码序列格式）, checksum.h文件则定义了校验码生成函数（源码中使用的是哈希校验，与XBee的求和校验并不相同）。

(1) mavlink_helpers.h

该文件中编/解码的函数分别为

mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)//编码函数
mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, mavlink_status_t* status, uint8_t min_length, uint8_t length uint8_t crc_extra)

通过这两个函数，我们可以知道mavlink通信协议有两个版本，数据包帧头分别为：

MAVLINK_STX_MAVLINK1：0xFE
MAVLINK_STX: 0xFD

当前用的是2.0的版本，协议格式如下

【数据包字节序号】	字节含义	值
1	帧头：开始标志	0xFD
2	有效载荷长度	0-255
3	incompat_flags	暂时不了解，似乎常取0
4	compat_flags	暂时不了解，似乎常取0
5	消息序列（SEQ）	0-255，传输计数，用于计算丢包率
6	系统ID	1-255，区分不同的飞控
7	组件ID	0-255, 区分同一飞控里不同的组件，如相机
8~10	消息ID	三个字节，0～ 2 24 2^{24} 224，标记本数据包类型
11~N+10	消息内容数据	最多253字节， N ≥ 1 N\geq1 N≥1
N+11~N+12	校验码	2字节

注意，mavlink的发送协议和接收协议是一致的，但是XBee的不同，根据上一篇博客所述，其发送协议为：

【数据包字节序号】	字节含义	值
1	帧头：开始标志	0x7E
2～3	有效载荷长度	2个字节
4	发送协议类型	0x10
5	发送协议ID	0x01
6-13	64位目标地址	8个字节
14～15	16位目标地址	2个字节，0xFFFE
16	广播半径	0x00
17	选项	0x00
18~N+17	消息内容数据	N个字节
N+18	校验码	1个字节

接收协议为：

【数据包字节序号】	字节含义	值
1	帧头：开始标志	0x7E
2～3	有效载荷长度	2个字节
4	接收协议	0x90
5-12	64位来源地址	8个字节
13～14	16位来源地址	2个字节，0xFFFE
15	选项	0xC1/0xC2
16~N+15	消息内容数据	N个字节
N+16	校验码	1个字节

为了尽可能与mavlink统一，我们可以对XBee通信协议中的消息内容数据进行进一步结构化处理，以发送协议为例，可划分如下：

【18～N+17 】	消息内容数据	共N个字节
18	消息序列（SEQ）	0-255，传输计数，用于计算丢包率
19	系统ID	1-255，区分不同的飞控
20	组件ID	0-255, 区分同一飞控里不同的组件，如相机
21~23	消息ID	三个字节，0～ 2 24 2^{24} 224，标记本数据包类型
24~N+10	消息内容数据	N-6个字节

相应的，mavlink_helpers.h中的编码函数（发送对应编码）可添加XBee发送协议编码内容：

    if(status->flags==MAVLINK_STATUS_FLAG_IN_XBEE){msg->magic = MAVLINK_STX_XBEE;header_len = MAVLINK_XBEE_HEADER_LEN+1;msg->len = _mav_trim_payload(_MAV_PAYLOAD(msg), length);msg->transfer_type=MAVLINK_SENDTYPE_XBEE;msg->transfer_ID=MAVLINK_SENDID_XBEE;if(msg->addr64==0){msg->addr64=0x000000000000FFFF;}if(msg->addr16==0){msg->addr16=0xFFFE;}msg->opt_xbee=OPT_SEND_BYTE;msg->brdcst_r=BRDCST_R;}...if(status->flags==MAVLINK_STATUS_FLAG_IN_XBEE){signature_len=0;signing=false;buf[0] = msg->magic;buf[1] = 0;buf[2] = length+14+6;buf[3] = msg->transfer_type;buf[4] = msg->transfer_ID;buf[12] = msg->addr64 & 0xFF;buf[11] = (msg->addr64>>8)&0xFF;buf[10] = (msg->addr64>>16)&0xFF;buf[9] = (msg->addr64>>24)&0xFF;buf[8] = (msg->addr64>>32)&0xFF;buf[7] = (msg->addr64>>40)&0xFF;buf[6] = (msg->addr64>>48)&0xFF;buf[5] = (msg->addr64>>56)&0xFF;buf[14] = msg->addr16&0xFF;buf[13] = (msg->addr16>>8)&0xFF;buf[15] = msg->brdcst_r;buf[16] = msg->opt_xbee;buf[17] = msg->seq;buf[18] = msg->sysid;buf[19] = msg->compid;buf[20] = msg->msgid & 0xFF;buf[21] = (msg->msgid >> 8) & 0xFF;buf[22] = (msg->msgid >> 16) & 0xFF;msg->checksum = crc_calculate2(&buf[3], header_len-3);crc_accumulate_buffer2(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);msg->checksum=msg->checksum & 0xFF;msg->checksum=0xFF-msg->checksum;checksum_len=1;}

接收协议也可做类似的处理。修改后的 mavlink_helpers.h参考资源文件。

(2) mavlink_types.h

该文件中定义了与通信协议相关的变量，主要有：

mavlink_parse_state_t：解码次序标记
mavlink_framing_t解码结果反馈
协议类型宏定义：

#define MAVLINK_STATUS_FLAG_IN_MAVLINK1  1 // last incoming packet was MAVLink1
#define MAVLINK_STATUS_FLAG_OUT_MAVLINK1 2 // generate MAVLink1 by default
#define MAVLINK_STATUS_FLAG_IN_SIGNED    4 // last incoming packet was signed and validated
#define MAVLINK_STATUS_FLAG_IN_BADSIG    8 // last incoming packet had a bad signature

mavlink_status_t：协议状态结构体变量
mavlink_message_tmavlink消息结构体

为使之兼容XBee，可增加如下内容：

//mavlink_parse_state_t
typedef enum {MAVLINK_PARSE_STATE_UNINIT=0,...MAVLINK_PARSE_STATE_GOT_RCVTYPE,//7MAVLINK_PARSE_STATE_GOT_SRCADDR64,//8MAVLINK_PARSE_STATE_GOT_SRCADDR16,//9MAVLINK_PARSE_STATE_GOT_OPTTYPE,//10...MAVLINK_PARSE_STATE_SIGNATURE_WAIT
} mavlink_parse_state_t; ///< The state machine for the comm parser//mavlink_message_t
MAVPACKED(
typedef struct __mavlink_message {uint16_t checksum;      ///< sent at end of packet
...uint8_t transfer_type; ///< for XBEEuint8_t transfer_ID; ///<  for XBEEuint8_t brdcst_r; ///<  for XBEEuint8_t opt_xbee; ///<  for XBEE
...uint64_t addr64;uint16_t addr16;
...
}) mavlink_message_t;//新增宏定义
//XBEE protocol
#define MAVLINK_STX_XBEE 0x7E          // 1. marker for XBEE protocol
//2.LENGTH 2 BYTES
#define MAVLINK_SENDTYPE_XBEE 0x10          // 3. Send type
#define MAVLINK_RECEIVETYPE_XBEE 0x90          // 3. receive type
#define MAVLINK_SENDID_XBEE 0x01          // 4. Send ID
// 5. Destination/Source Adress  8bytes/* 主机与从机地址 */#define Addr_Coordinate 0x0013A20041080112#define Addr_QuadRotor1 0x0013A2004108010B#define Addr_QuadRotor2 0x0013A20041080102#define Addr_ALL 0x000000000000FFFF
//6. 16bit address
#define Addr_16BIT 0xFFFE
//7.Broadcast radius 1BYTE
#define BRDCST_R 0x00
//8.Option byte
#define OPT_SEND_BYTE 0x00
#define OPT_RECEIVE_BYTE 0xC2

修改完的文件可参见已上传资源文件 mavlink_types.h.

(3) checksum.h

由于XBee校验方式不同，因此在该文件中仿照mavlink校验函数增加了适合XBee协议的校验函数，主要有：

仿照函数crc_accumulate(uint8_t data, uint16_t *crcAccum)构造了适用于XBee数据校验的累和函数：

static inline void crc_accumulate2(uint8_t data, uint16_t *crcAccum)
{/*Accumulate one byte of data into the CRC*/*crcAccum = *crcAccum+data;
}

适用于XBee的初始化函数

static inline void crc_init2(uint16_t* crcAccum)
{*crcAccum = 0x0000;
}

适用于XBee的计算函数

static inline uint16_t crc_calculate2(const uint8_t* pBuffer, uint16_t length)
{uint16_t crcTmp=0x00;crc_init2(&crcTmp);while (length--) {crc_accumulate2(*pBuffer++, &crcTmp);}return crcTmp;
}

修改完的文件可参考已上传资源文件 checksum.h.

3-2 PX4飞控部分的修改

和QGC部分类似，只要将上面修改的 mavlink_helpers.h、 mavlink_types.h、 checksum.h这三个文件复制到文件夹./Firmware/mavlink/include/mavlink/v2.0/中即可。

此外，还需要注意的是，我们应当配置一个串口给XBee模块使用，主要是飞控板的 TELEM 1 端口(注册路径为:/dev/ttyS1)和 TELEM2 端口(注册路径为:/dev/ttyS2)，我们可以选用TELEM2 端口，在rcS文件中默认开启为：

mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m xbee -r 1000

然后在mavlink_main.cpp文件中添加端口参数设置程序：

     case 'm':if (strcmp(myoptarg, "custom") == 0) {_mode = MAVLINK_MODE_CUSTOM;
...}else if (strcmp(myoptarg, "xbee") == 0) {_mode = MAVLINK_MODE_XBEE;_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_XBEE;int32_t proto =MAVLINK_STATUS_FLAG_IN_XBEE;if (_protocol_version_switch != proto) {_protocol_version_switch = proto;//默认是0set_proto_version(proto);//转化为zigebee协议}}break;

以及设置需要的发送消息：

switch (_mode) {case MAVLINK_MODE_XBEE:configure_stream("HEARTBEAT", 0.75f);/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */configure_stream("STATUSTEXT", 3.0f);/* COMMAND_LONG stream: use unlimited rate to send all commands  */configure_stream("COMMAND_LONG");configure_stream("ALTITUDE", 1.0f);configure_stream("ATTITUDE", 5.0f);configure_stream("ATTITUDE_TARGET", 2.0f);configure_stream("DEBUG", 1.0f);configure_stream("DEBUG_VECT", 1.0f);configure_stream("DISTANCE_SENSOR", 0.5f);configure_stream("ESTIMATOR_STATUS", 0.5f);configure_stream("EXTENDED_SYS_STATE", 1.0f);configure_stream("GLOBAL_POSITION_INT", 5.0f);configure_stream("GPS_RAW_INT", 1.0f);//configure_stream("HIGHRES_IMU", 1.5f);configure_stream("HOME_POSITION", 0.5f);configure_stream("LOCAL_POSITION_NED", 1.0f);      configure_stream("NAMED_VALUE_FLOAT", 1.0f);configure_stream("RC_CHANNELS", 0.5f);configure_stream("SYS_STATUS", 0.1f);configure_stream("VFR_HUD", 1.0f);break;

3-3 通信测试及功能完善

通信测试主要通过以下两步完成：

给飞控和电脑端连接好各自的XBee模块，打开地面站并给飞控上电，观察地面站有无收到飞控发送的消息数据；
给地面站配置一个指令发送按钮，并在飞控源码中编写针对该指令的应答程序，然后测试地面站在点击按钮时，有没有收到飞控反馈的应答信号。

其中，第一步主要测试地面站的接收解码和飞控的发送编码功能是否正常；第二步主要测试地面站的发送编码和飞控端的接收解码。

第一步基本无需新增代码，地面站收到飞控端发送的心跳包configure_stream("HEARTBEAT", 0.75f);即可显示连接；下面重点考虑第二步的实现流程，依次为：

在 Analyze 页面左栏的console选项下新增一个MyTest项；
在MyTest项右栏空页中添加一个按钮，名称记为My Flight Command；
在My Flight Command右边增加一个选项下拉列表（即combobox类型的控件），用于选择不同的指令；
在该combobox控件右侧再添加一个label标签，用于显示接收到的应答信号。

正如下图所示：

图1 MyTestPage

如图1所示，需要实现的功能为：通过选项下拉列表控件（图1中的3）选择指定的命令，点击按钮My Flight Command，地面站即发送该指令给飞控，飞控收到指令后反馈回一个应答信号到地面站，通过标签（图中的4）显示该应答信号。

具体实现方法为：

仿照Analyze页面下的Mavlink Console子页面对应的qml文件来构造一个MyTestPage页面，并在该新页面中从左至右加入QGC的按钮、选项下拉列表、标签；
定义对应于按钮的触发函数；
定义对应于下拉列表选项的变量；
定义对应于标签内容的变量；
编写C++文件代码，建立页面中的控件与相关命令发送及接收之间的关系。

（1）创建`MyTestPage`页面

Mavlink Console子页面对应的qml文件为MavlinkConsolePage.qml，仿照其写法，创建的MavlinkConsolePage.qml文件为：

/*****************************************************************************  myTestPage****************************************************************************/import QtQuick          2.3
import QtQuick.Controls 1.2
import QtQuick.Dialogs  1.2
...
import QGroundControl.Vehicle       1.0AnalyzePage {id:        myTestPagepageComponent:      pageComponentpageName:           qsTr("MyTestPage")pageDescription:    qsTr("MyTestPage  is used to test some function write by myself.")property real _margin: ScreenTools.defaultFontPixelWidth * 2Component {id:                 pageComponentColumn {id:         mainColumnwidth:      availableWidthspacing:    _marginRow {spacing: _marginQGCButton {text:       qsTr("My Flight Command")width:      ScreenTools.defaultFontPixelWidth * 30anchors.verticalCenter:   parent.verticalCenter}QGCComboBox {id:             modeChannelCombowidth:          ScreenTools.defaultFontPixelWidth * 20model:          [ qsTr("Not assigned"), qsTr("Takeoff"), qsTr("Land"),qsTr("Fly a circle"), qsTr("Fly a rectangle"), qsTr("swarm")]}    QGCLabel {text: "Waiting for an ACK from PX4!"anchors.verticalCenter:   parent.verticalCenter}}} // Column} // Component
} // AnalyzePage

创建好后，需要添加到qgroundcontrol.qrc文件和 AnalyzeView.qml文件中，依次如下：

# qgroundcontrol.qrc
...<file alias="GeoTagPage.qml">src/AnalyzeView/GeoTagPage.qml</file><file alias="MyTestPage.qml">src/AnalyzeView/ MyTestPage.qml</file><file alias="HealthPageWidget.qml">src/FlightMap/Widgets/HealthPageWidget.qml</file>...#  AnalyzeView.qml...ListElement {buttonImage:        "/qmlimages/MavlinkConsoleIcon"buttonText:         qsTr("Mavlink Console")pageSource:         "MavlinkConsolePage.qml"}ListElement {buttonImage:        "/qmlimages/MavlinkConsoleIcon"buttonText:         qsTr("MyTestPage")pageSource:         "MyTestPage.qml"}
...

(2)定义对应于按钮控件的触发函数

c++中的函数都是某个类的方法，若要定义一个能被qml调用的类的方法，还需要该类声明为qml可以访问的类型，具体有两种方式：

用qmlRegisterUncreatableType进行声明，如：

   qmlRegisterUncreatableType<MultiVehicleManager>("QGroundControl.MultiVehicleManager", 1, 0, "MultiVehicleManager", "Reference only");

然后在头文件中声明类的属性：

Q_PROPERTY(MultiVehicleManager* multiVehicleManager READ multiVehicleManager CONSTANT)

最后qml文件可以通过QGroundControl.multiVehicleManager 来访问其中的属性和方法。

用qmlRegisterType对类进行声明，在qml中通过id来访问属性和方法：

// --QGCApplication.cc
qmlRegisterType<PlanMasterController>("QGroundControl.Controllers", 1, 0, "PlanMasterController");
///PlanView.qmlPlanMasterController {id:                     masterControllerComponent.onCompleted:  start(true /* flyView */)}

解决类的问题后，对于类中需要被qml使用的方法，则在函数前加上Q_INVOKABLE即可，如：

Q_INVOKABLE void emergencyStop(void);

可以看出按钮My Flight Command的触发函数在c++文件中被定义，然后在qml文件中被按钮控件调用，函数可在头文件中通过Q_INVOKABLE关键字来声明：

Q_INVOKABLE void MyFlightCMD(void);

那么究竟选用那个类的头文件呢？由于我们是要发送指令给飞控，所以头文件对应的c++文件必须能发送mavlink消息给飞控。这里先简单对QGC地面站的工作流程进行简要分析，QGC地面站的启动包含两条流程线，如下图所示：

图2 QGC地面站启动流程图

左边是显示的页面，也就是软件的前端，右边是由c++运行计算的数据流，也就是软件后台。地面站启动后，后台会通过LinkManager.cc文件相关函数找到计算机当前存在的端口，然后MultiVehicleManager.cc的相关函数会不断发心跳包给这个端口，MAVLinkProtocol.cc负责检查是否收到PX4的发送回的心跳包，若收到，则告诉MultiVehicleManager.cc，然后MultiVehicleManager.cc为该收到心跳包的端口建立一个Vehicle的类对象，标记为activeVehicle。

因此，最终与PX4建立通信连接的是该Vehicle类对象，所以前面的按钮触发函数应在Vehicle.h里进行声明，然后在Vehicle.cc里进行定义。
qml中的调用如下：

 QGCButton {text:       qsTr("My Flight Command")width:      ScreenTools.defaultFontPixelWidth * 30onClicked:  QGroundControl.multiVehicleManager.activeVehicle.MyFlightCMD()//触发函数anchors.verticalCenter:   parent.verticalCenter}

（3）定义对应于下拉列表选项的变量

下拉列表的选项一般都是整数型的，为了定义一个可同时被qml文件和cpp类修改的整数变量，可按以下步骤实现：

在vehicle.h里声明该整数型变量(public)：

Q_PROPERTY(int  Flight_Command   READ Flight_Command  WRITE setFlight_Command  NOTIFY Flight_CommandChanged)

此外，对于仅在cpp文件中需要修改的变量，可以按以下方式声明：

 Q_PROPERTY(int      firmwareMajorVersion        READ firmwareMajorVersion       NOTIFY firmwareVersionChanged)

若变量为仅初始赋值一次，后续不再允许改动的常量，则声明格式为：

Q_PROPERTY(int id READ id CONSTANT)

定义一个对应的全局变量(private)：

  int  _Flight_Command;

定义一个取值函数(public)：

int Flight_Command(void){ return _Flight_Command; }

声明及定义写值函数(public):

void setFlight_Command(int Flight_Command)；
...void Vehicle::setFlight_Command(int Flight_Command)
{..._Flight_Command=Flight_Command;qDebug("Flight_Command=%d",Flight_Command);
}

声明信号传递函数（传递到qml文件中，无需定义内容，signals:）

void Flight_CommandChanged(int Flight_Command);

使用时，在C++中采用如下方式修改变量值：

..._Flight_Command = 1;emit Flight_CommandChanged(_Flight_Command);
...

qml文件中，则通过调用类对象方法修改：

...property var    _activeVehicle:         QGroundControl.multiVehicleManager.activeVehicle
...        QGCComboBox {id:             modeFlightCmdCombowidth:          ScreenTools.defaultFontPixelWidth * 20model:          [ qsTr("Not assigned"), qsTr("Takeoff"), qsTr("Land"),qsTr("Fly a circle"), qsTr("Fly a rectangle"), qsTr("swarm")]currentIndex:  _activeVehicle.Flight_CommandonActivated: {_activeVehicle.Flight_Command=index//相当于调用了函数setFlight_Command(index)}                    }
...

（4）定义对应于标签内容的变量

标签中的内容只需要显示，不需要从界面中去更改它，因此我们可以声明一个字符串变量，属性如下：

Q_PROPERTY(int  ackString   READ ackString  NOTIFY ackStringChanged)

然后声明和定义函数：

QString         ackString        ();
...
QString Vehicle::ackString()
{QString messages;switch(_CommandACK){case 0:messages="Waiting for an ACK from PX4!";break;case 1:messages="ACK:Success!";break;case 2:messages="ACK:Failed!";break;default:messages="ACK:Waiting!";break;}return messages;
}

声明信号传递函数：

 void ackStringChanged    ();

qml文件中调用：

QGCLabel {text:  qsTr(_activeVehicle.ackString)anchors.verticalCenter:   parent.verticalCenter
}

（5）建立页面中的控件与指令发送/消息接收之间的关系

我们首先需要通过mavlink-generator 的 mavlink 消息生成器:mavgenerate.py生成一个新的mavlink消息：mavlink_msg_set_my_command用于我们发送/接收我们的指令：

#define MAVLINK_MSG_ID_SET_MY_COMMAND 197MAVPACKED(
typedef struct __mavlink_set_my_command_t {int16_t data1; /*<  not specific defined*/int16_t data2; /*<  not specific defined*/int16_t data3; /*<  not specific defined*/int16_t data4; /*<  not specific defined*/int16_t data5; /*<  not specific defined*/int16_t data6; /*<  not specific defined*/int16_t data7; /*<  not specific defined*/int16_t data8; /*<  not specific defined*/
}) mavlink_set_my_command_t;

在QGC地面站的common.h文件（libs/mavlink/include/mavlink/v2.0/common/common.h）中添加该消息头文件：

...
#include "./mavlink_msg_obstacle_distance.h"
#include "./mavlink_msg_set_my_command.h"
...

同时在该文件下的 MAVLINK_MESSAGE_INFO 和 MAVLINK_MESSAGE_NAMES 宏定义中添加：

#define MAVLINK_MESSAGE_INFO{... ,MAVLINK_MESSAGE_INFO_SET_MY_COMMAND,...}
#define MAVLINK_MESSAGE_NAMES {{ "ACTUATOR_CONTROL_TARGET", 140 }, ...,{ "SET_MY_COMMAND", 197 }, ...}

在 ardupilotmega.h 需要添加消息的校验数组 {197, 9, 16, 0, 0, 0},：

#define MAVLINK_MESSAGE_CRCS {...{195, 120, 37, 0, 0, 0}, {197, 9, 16, 0, 0, 0}, {200, 134, 42, 3, 40, 41}...

数组的第一个数197就是消息ID，注意这个ID的大小也决定了数组在 MAVLINK_MESSAGE_CRCS中所放的位置，因为没有196号消息，所以放在195号消息之后。数组其它元素，可从mavgenerate.py生成的其它文件（set_my_command.h）中找到，原本为：

#define MAVLINK_MESSAGE_CRCS {{197, 9, 16, 16,0, 0, 0}}

我们去掉中间多余的一个16即可，然后在按钮触发函数中添加打包发送函数：

void Vehicle::MyFlightCMD()
{qDebug("_Flight_Command=%d",_Flight_Command);mavlink_message_t msg={};mavlink_msg_set_my_command_pack_chan(_mavlink->getSystemId(),_mavlink->getComponentId(),priorityLink()->mavlinkChannel(),&msg,_Flight_Command,0,0,0,0,0,0,0);sendMessageOnLink(priorityLink(), msg);
}

同时在PX4飞控源码中也要按以上步骤添加新的mavlink消息：mavlink_msg_set_my_command，然后在消息处理函数中添加对该消息的处理：

void MavlinkReceiver::handle_message(mavlink_message_t *msg)
{...switch(msg->msgid){...case MAVLINK_MSG_ID_SET_MY_COMMAND:handle_message_set_my_command(msg);break;...}
}
...
void MavlinkReceiver::handle_message_set_my_command(mavlink_message_t *msg)
{mavlink_set_my_command_t cmd_mavlinkmavlink_set_my_command_decode(msg, &cmd_mavlink);acknowledge(msg->sysid,msg->compid,cmd_mavlink.data1,cmd_mavlink.data1);
}

也就是说，我们利用PX4飞控中acknowledge应答函数反馈接收到的消息给QGC地面站，所以需要在地面站处理应答消息的函数里添加对反馈的处理：

void Vehicle::_handleCommandAck(mavlink_message_t& message)
{bool showError = false;mavlink_command_ack_t ack;mavlink_msg_command_ack_decode(&message, &ack);_CommandACK=ack.result;emit ackStringChanged();...
}

最后分别连接XBee模块到飞控和QGC地面站，开机测试，得到我们想要的结果。

图3 XBee通信测试