程序员经验分享-hwhale

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

2023-05-16

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

一、问题的提出

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

二、基本思路

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

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

三、开发过程

如下：

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
3incompat_flags暂时不了解，似乎常取0
4compat_flags暂时不了解，似乎常取0
5消息序列（SEQ）0-255，传输计数，用于计算丢包率
6系统ID1-255，区分不同的飞控
7组件ID0-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发送协议ID0x01
6-1364位目标地址8个字节
14～1516位目标地址2个字节，0xFFFE
16广播半径0x00
17选项0x00
18~N+17消息内容数据N个字节
N+18校验码1个字节

接收协议为：

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

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

【18～N+17 】消息内容数据共N个字节
18消息序列（SEQ）0-255，传输计数，用于计算丢包率
19系统ID1-255，区分不同的飞控
20组件ID0-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,//7
    MAVLINK_PARSE_STATE_GOT_SRCADDR64,//8
    MAVLINK_PARSE_STATE_GOT_SRCADDR16,//9
    MAVLINK_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 XBEE
    uint8_t transfer_ID; ///<  for XBEE
    uint8_t brdcst_r; ///<  for XBEE
    uint8_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;//默认是0
					set_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);即可显示连接；下面重点考虑第二步的实现流程，依次为：

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

正如下图所示：
在这里插入图片描述

图1 MyTestPage

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

具体实现方法为：

  1. 仿照Analyze页面下的Mavlink Console子页面对应的qml文件来构造一个MyTestPage页面，并在该新页面中从左至右加入QGC的按钮、选项下拉列表、标签；
  2. 定义对应于按钮的触发函数；
  3. 定义对应于下拉列表选项的变量；
  4. 定义对应于标签内容的变量；
  5. 编写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.0

AnalyzePage {
    id:        myTestPage
    pageComponent:      pageComponent
    pageName:           qsTr("MyTestPage")
    pageDescription:    qsTr("MyTestPage  is used to test some function write by myself.")

    property real _margin: ScreenTools.defaultFontPixelWidth * 2

    Component {
        id:                 pageComponent

        Column {
            id:         mainColumn
            width:      availableWidth
            spacing:    _margin

            Row {
                spacing: _margin

                QGCButton {
                    text:       qsTr("My Flight Command")
                    width:      ScreenTools.defaultFontPixelWidth * 30
                    anchors.verticalCenter:   parent.verticalCenter
                }

                QGCComboBox {
                    id:             modeChannelCombo
                    width:          ScreenTools.defaultFontPixelWidth * 20
                    model:          [ 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可以访问的类型，具体有两种方式：

  1. 用qmlRegisterUncreatableType进行声明，如：
   qmlRegisterUncreatableType<MultiVehicleManager>("QGroundControl.MultiVehicleManager", 1, 0, "MultiVehicleManager", "Reference only");

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

Q_PROPERTY(MultiVehicleManager* multiVehicleManager READ multiVehicleManager CONSTANT)

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

  1. 用qmlRegisterType对类进行声明，在qml中通过id来访问属性和方法：
// --QGCApplication.cc
qmlRegisterType<PlanMasterController>("QGroundControl.Controllers", 1, 0, "PlanMasterController");
///PlanView.qml
    PlanMasterController {
        id:                     masterController
        Component.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 * 30

                    onClicked:  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:             modeFlightCmdCombo
            width:          ScreenTools.defaultFontPixelWidth * 20
            model:          [ qsTr("Not assigned"), qsTr("Takeoff"), qsTr("Land"),qsTr("Fly a circle"), qsTr("Fly a rectangle"), qsTr("swarm")]
            
            currentIndex:  _activeVehicle.Flight_Command
            onActivated: {
                           _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 197

MAVPACKED(
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_mavlink
    mavlink_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通信测试
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

XBee

QGC

PX4

模块实现

飞控的组网通信连接

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

  • 编译PX4固件

    PX4编译 文章目录 PX4编译疑难杂症bug1bug2catkin build isolated 官方脚本Step1Step2 安装常用依赖Step3 创建并运行脚本Step4 补全代码子模块Step5 验证仿真 官方offboard 例

  • 【2020-8-9】APM,PX4,GAZEBO,MAVLINK,MAVROS,ROS之间的关系以及科研设备选型

    0 概述 无人机自主飞行平台可以分为四个部分 xff1a 动力平台 xff0c 飞行控制器 xff0c 机载电脑和模拟平台 动力平台 xff1a 负责执行飞行任务 xff0c 包括螺旋桨 电机 机架等 xff0c 用于科研的一般都是F380

  • QGC开发 显示双GPS/RTK信息以及自定义页面（ubuntu）

    一 QGC开发 显示双GPS RTK信息 1 在sitl中进行仿真 xff0c 虚拟出第二个GPS mavlink发送到地面站 如下图中 xff0c 在mavlink msg gps2 raw h中找到发送第二组gps rtk数据函数mav

  • 基于F4/F7/H7飞控硬件和px4飞控固件的廉价自主无人机系统（1）-飞控

    前言 穿越机F4 F7 H7飞控是一系列采用stm32系列F4xx和F7xx处理器的飞控的统称 xff0c 是目前穿越机爱好者非常喜欢使用的飞控硬件 xff0c 其价格也非常便宜180 xff5e 410 而px4则是一款常见的开源飞控固件

  • PX4代码学习系列博客(5)——在px4中添加自己的模块

    怎么在px4中添加自己的模块 在 px4固件目录结构和代码风格 这一节 xff0c 曾经说过NuttX是一个实时的嵌入式系统 xff0c 上面可以像windows那样运行程序 那既然是应用程序 xff0c 那我们应该也能写一些可以在Nutt

  • QGC4.1.2二次开发（2）QGC连接与数据收发

    文章目录 前言一 连接原理二 连接过程与数据收发1 连接过程 xff08 以串口为例 xff09 2 数据发送 总结 前言 QGC连接无人机飞控时支持多种连接方式 xff0c 并且可以自动连接 xff0c 不由让人好奇它的实现原理 xff0

  • PX4 -- EKF2

    文章目录 EKF2参数高度估计Range Finder滤波 单变量更新单变量更新对多变量的影响 EKF2 参数 EKF2 中有一类 GATE 参数 当测量值在 VAR GATE 范围内才会更新值 高度估计 四种高度控制方法 xff1a 气压

  • PX4模块设计之四：MAVLink简介

    PX4模块设计之四 xff1a MAVLink简介 1 MAVLink PX4 应用简介2 MAVLink v2 0新特性3 MAVLink协议版本4 MAVLink通信协议帧4 1 MAVLink v1 0 帧格式4 2 MAVLink

  • PX4模块设计之二十一：uORB消息管理模块

    PX4模块设计之二十一 xff1a uORB消息管理模块 1 uORB模块构建模式2 uORB消息管理函数2 1 状态查询2 2 资源利用2 3 模块启动2 4 模块停止3 uORB消息接口3 1 消息主题注册3 2 消息主题去注册3 3

  • PX4模块设计之二十六：BatteryStatus模块

    PX4模块设计之二十六 xff1a BatteryStatus模块 1 BatteryStatus模块简介2 模块入口函数2 1 主入口battery status main2 2 自定义子命令custom command 3 Batter

  • PX4模块设计之三十：Hysteresis类

    PX4模块设计之三十 xff1a Hysteresis类 1 Hysteresis类简介2 Hysteresis类成员变量介绍3 Hysteresis类迟滞逻辑4 Hysteresis类重要方法4 1 Hysteresis bool ini

  • PX4模块设计之三十六：MulticopterPositionControl模块

    PX4模块设计之三十六 xff1a MulticopterPositionControl模块 1 MulticopterPositionControl模块简介2 模块入口函数2 1 主入口mc pos control main2 2 自定义

  • mavros连接px4失败的usb-ttl原因

    问题描述 xff1a 最近在搞mavros xff0c 以方便协处理器和pixhawk通讯 xff0c 在按照官网教程安装mavros xff0c 设置px4 xff0c 连接硬件之后发现mavros卡在中间下不去 xff1a MAVROS

  • px4无人机常识介绍（固件，px4等）

    专业名词解释 aircraft 任何可以飞或者可以携带物品还是搭载旅客的飞行器统称为飞机 航空器 uav 无人驾驶飞机 vehicle 飞行器 airplane plane aero plane 有机翼和一个或多个引擎的飞行器统称为飞机 D

  • PX4中自定义MAVLink消息（记录）

    简单记录一下这个过程 一 自定义uORB消息 这一步比较简单 xff0c 首先在msg 中新建ca trajectory msg文件 uint64 timestamp time since system start span class t

  • PX4——Range Finder 篇

    Range Finder 此处选用的是 Benewake 下的 Lidar 参数设置 General Configuration 除了官方的参数设置外 xff0c 我在 EKF2 中还找到了 EKF2 RNG AID 参数 xff0c 用来

  • PX4项目学习::（七）飞控栈：commander

    PX4的飞行控制程序通过模块来实现 xff0c 与飞控相关的模块主要有commander xff0c navigator xff0c pos control xff0c att control这几个 xff0c 分别可以在src modul

  • 飞行姿态解算（三）

    继之前研究了一些飞行姿态理论方面的问题后 又找到了之前很流行的一段外国大神写的代码 来分析分析 第二篇文章的最后 讲到了文章中的算法在实际使用中有重大缺陷 大家都知道 分析算法理论的时候很多情况下我们没有考虑太多外界干扰的情况 原因是很多情

  • Nacos使用教程

    Spring Cloud是一个基于Spring Boot的微服务框架 它提供了一系列的组件和工具 可以帮助开发人员快速构建和部署分布式应用程序 其中 Nacos是一个新兴的服务发现和配置管理组件 可以帮助开发人员轻松地管理微服务的注册 发现

  • XBee3 协调器在网络发现期间找不到 End_Device

    目前 我正在运行 XBee3 International Mesh Kit 并尝试按照文档中给定的示例进行操作 我尝试使用 Micropython REPL 为三个设备实现网络发现 以下是我的 3 个 XBee 设备的配置 3 个 XBee

随机推荐

  • Python问题&解决

    python pycharm小技巧 1 要学会使用Debug pycharm Debug使用 Debug记得打断点 xff01 Pycharm debug技巧 在debug的时候python console可以直接敲变量显示 在安装Pych

  • Linux系统安装ClamAV的详细步骤

    ClamAV是一款开源免费的杀毒软件 xff0c 它可以在Linux系统上运行 以下是在Linux系统上安装ClamAV的步骤 xff1a 打开终端并更新软件包列表 xff1a sudo apt update 安装ClamAV xff1a

  • 为什么 0.1 + 0.2 不等于0.3？如何解决这个问题？

    一 开头 我们都知道0 1 43 0 2 61 61 0 3 xff0c 而是0 30000000000000004 xff0c 那么是为什么 xff1f 我们都知道计算机在内部实现中使用的是二进制 xff0c 0 1也是不例外的 xff0

  • 前端手写(十八)——Promise并行限制

    一 写在前面 一般我们做多个异步请求 xff0c 此时我们常常采用的是Promise all来进行处理 xff0c Promise all会全部的一起执行 xff0c 但是如果存在一些并行的限制 xff0c 也就是说一次最多只能执行固定的数

  • 深度学习环境安装(VMware)-Miniconda-pytorch

    提示 xff1a 最近要要跑一些算法 xff0c 用的linux系统一直是在服务器上进行开发 xff08 无GUI界面 xff09 xff0c 双系统又懒得开关机 xff0c 虚拟机还不能调用gpu xff0c 真无了个大语 对于文章中出现

  • novnc安装

    ubuntu22 04 span class token comment 安装软件 span span class token function sudo span span class token function apt span sp

  • python扫描端口

    什么是端口扫描 定义 xff1a 对一段端口或指定的端口进行扫描 目的 xff1a 通过扫描结果可以知道一台计算机上都提供了哪些服务 xff0c 然后就可以通过所提供的这些服务的己知漏洞就可进行攻击 原理 xff1a 当一个主机向远端一个服

  • HTML_移动端界面

    homework8 移动端界面 注 点击图标放大 点击图片旋转180度 ydd html span class token doctype lt DOCTYPE html gt span span class token tag span

  • Windows11安装与使用初体验

    Windows11安装 因为下载的是美国镜像 xff0c 所以系统语言是英文的 xff0c 但是这么多年的使用 xff0c 还是能够看懂一二的 xff0c 一步步操作就好了呗 xff0c 随缘点击 xff0c 无脑下一步 不知是我没有选择对

  • 基于51单片机的智能窗帘仿真方案原理图设计

    系统总体方案 xff08 附文件 xff09 通过上述对各个模块介绍 xff0c 我们最终选择了采用STC89C52作为的主控芯片 xff0c 采用光敏电阻采集环境光强通过ADC0832转换成数字信息然后由单片机处理得出环境光强的情况 xf

  • 基于RNN-LSTM模型的诗词生成/TensorFlow

    1 研究任务一介绍 1 1 研究任务 给定诗词数据集poems xff0c 采用基于循环神经网络 xff08 RNN xff09 的LSTM模型实现古诗词自动生成 xff0c 调整参数实现五言诗 七言诗 五言藏头诗 七言藏头诗和词的自动生成

  • PX4飞控学习与开发（三）-PX4+ROS开发环境搭建

    PX4开发环境搭建 主要步骤如下 xff1a 第一步 xff0c 设置用户组 在终端输入命令 xff1a sudo usermod a G dialout USER xff0c 然后登出 xff0c 重启 xff1b 第二步 xff0c P

  • PX4飞控学习与开发（五）-Pixhawk固件Firmware源码结构分析

    Pixhawk固件Firmware源码结构分析 Pixhawk源码Firmware是一个内容庞大的文件夹 xff0c 里面有许多的子文夹 xff0c 代表着不同的功能模块 文件夹结构如下图所示 xff1a 图1 Firmware源码结构 图

  • Latex的一些排版技巧

    Latex是科研论文写作的必备工具之一 xff0c 学会一些常用的排版指令有助于快速提高论文的排版质量 本篇博客的主要内容就是总结一些排版技巧 xff0c 方便后续查找使用 当然 xff0c 随着latex排版相关知识的进一步学习和使用 x

  • PX4飞控学习与开发（六）-利用 VScode 修改源码

    努力学习 xff0c 珍惜时间 xff1b 全力以赴 xff0c 创造未来 克制欲望 xff0c 摒除心魔 xff1b 心向何处 xff0c 往来圣贤 功崇惟志 xff0c 业广惟勤 xff1b 惟克果断 xff0c 乃罔后艰 面临困难 x

  • 基于VSCode软件的markdown笔记环境配置

    前期在CSDN上用markdown写了一些博客 xff0c 使用时还是觉得不太方便 xff0c 尤其是在编写公式时 xff0c 效率十分低下 但Markdown本身还是一款非常不错的笔记撰写工具 xff0c 所以一直琢磨着怎么改善其使用体验

  • Ubuntu 主机单系统 安装

    首先是安装系统 xff0c 启动盘是USB HDD模式 xff0c 其他基本和下面这篇文章一样 xff0c 除了安装时候没有Install 然后按e什么的 xff0c 应该是因为我的是20 04吧 史上最全Ubuntu18 04单系统安装教

  • 竞拍算法(Auction Algorithm)原理及工作过程分析

    这几天因一些项目工作 xff0c 需要对竞拍算法进行学习 但百度了大部分资料都未找到一篇文章对此算法有着较为深入的介绍 在一番努力之下 xff0c 终于找到了最初提出该算法的论文 xff0c 本文内容主要结合该论文对竞拍算法进行分析 竞拍算

  • PX4飞控学习与开发（七）-Pixhawk源码中的功能模块分析

    本篇博客主要介绍Firmware固件中各功能模块的基本结构 功能模块的编译 从上篇博客内容中的demo我们可以发现 xff0c 如果我们需要给Pixhawk模块新增一个功能模块 xff0c 一般的做法是新建一个文件夹 xff0c 所有这个功

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

    本篇博客介绍如何利用XBee模块实现QGC地面站与飞控的通信 一 问题的提出 正如 上一篇博客 指出 xff0c PX4飞控原装数传模块 xff08 3DR Radio xff09 只能一对一通信 xff0c 并不能实现多机组网通信 xff

热门标签