QGC接收PX4自定义Mavlink消息（二）qgc接收

这里补充一些，我的px4版本是1.11.0dev，在ubuntu18.04上开发，qgc为目前官网最新的版本，在windows上上开发，大家的源码会因为版本差异而有少许区别，所以代码复制粘贴编译肯定会报错，所以得根据具体情况去改，我也会大概描述一下关键mavlink收发得逻辑。

第一步，将上一篇文章生成器生成得mavlink_msg_vehicle_zkrt.h头文件放在此路径下：C:\Users\Sense\Desktop\QGC_Source\QGroundControl\libs\mavlink\include\mavlink\v2.0\ardupilotmega

我的源码时放在桌面的，这里有人可能会有疑问，为什么不是放在v2.0\common文件夹下，毕竟ardupilotmege明显是apm的固件，下面会说明我放在common文件夹下踩坑的经历。。。大家先这样做，现实性能收到数据；

在ardurpilotmega.h需要修改三处地方，第一处：

上图这串数据是qgc用来校验消息id的，注释的是apm原来自带的msg，重新添加自己定义的166msg的校验数据，大家可以在mavlink_types.h文件中找到如下定义：

typedef struct __mavlink_msg_entry {uint32_t msgid;uint8_t crc_extra;uint8_t min_msg_len;       // minimum message lengthuint8_t max_msg_len;       // maximum message length (e.g. including mavlink2 extensions)uint8_t flags;             // MAV_MSG_ENTRY_FLAG_*uint8_t target_system_ofs; // payload offset to target_system, or 0uint8_t target_component_ofs; // payload offset to target_component, or 0
} mavlink_msg_entry_t;

如上图，第一个元素为消息id ：166，第二至四个元素可以在生成器生成的mavlink_msg_vehicle_zkrt.h头文件中查看,如图:

需要修改的第二处:

需要修改的第三处：

大功告成，我们可以在Vehicle.cc文件中加入qDebug来测试qgc有没有接收成功：

测试结果如下：

很明显，自定义的消息接收比心跳包接收要多的多，因为心跳包为1hz，自定义的为30hz，接下来就可以在switch里面写自己的处理函数了！（这个将放到下一篇文章）

踩坑经历及mavlink接收代码分析:

一开始说了，我的mavlink_msg_vehicle_zkrt.h头文件是放在common文件夹中，且修改的三处地方都是common文件夹中的对应地方，一开始在Vehicle.cc中的switch那怎么都接收不倒自定义的数据，研究了半天，下面说一下mavlink接收的大概流程：

在switch的最上面，可以发现mavlink_message_t message是已经解析好了传进来的，这里我们可以访问message的成员，例如msgid，但经过debug发现msgid没有等于166，于是继续向前追踪：

void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{// If the link is already running at Mavlink V2 set our max proto version to it.unsigned mavlinkVersion = _mavlink->getCurrentVersion();if (_maxProtoVersion != mavlinkVersion && mavlinkVersion >= 200) {_maxProtoVersion = mavlinkVersion;qCDebug(VehicleLog) << "_mavlinkMessageReceived Link already running Mavlink v2. Setting _maxProtoVersion" << _maxProtoVersion;}if (message.sysid != _id && message.sysid != 0) {// We allow RADIO_STATUS messages which come from a link the vehicle is using to pass through and be handledif (!(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS && _vehicleLinkManager->containsLink(link))) {return;}}

connect(_mavlink, &MAVLinkProtocol::messageReceived,        this, &Vehicle::_mavlinkMessageReceived);
//这里全局收索发现这个函数绑定了一个信号，继续收索看看在哪触发了

MAVLinkProtocol::messageReceived信号在下面函数触发了：

void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{   //在这qDebug 变量b，会发现b就是qgc刚接到的数据，没有经过解析，在这我发现msgid确实有166//但是经过解析之后就没有了    // Since receiveBytes signals cross threads we can end up with signals in the queue// that come through after the link is disconnected. For these we just drop the data// since the link is closed.SharedLinkInterfacePtr linkPtr = _linkMgr->sharedLinkInterfacePointerForLink(link, true);if (!linkPtr) {qCDebug(MAVLinkProtocolLog) << "receiveBytes: link gone!" << b.size() << " bytes arrived too late";return;}uint8_t mavlinkChannel = link->mavlinkChannel();//这里就是解析mavlink的函数mavlink_parse_char(mavlinkChannel, static_cast<uint8_t>(b[position]//请注意这里是一个个字节传进去循环解析的，所以继续追踪该函数for (int position = 0; position < b.size(); position++) {if (mavlink_parse_char(mavlinkChannel, static_cast<uint8_t>(b[position]), &_message, &_status)) {// Got a valid messageif (!link->decodedFirstMavlinkPacket()) {link->setDecodedFirstMavlinkPacket(true);mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {qCDebug(MAVLinkProtocolLog) << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;// Set all links to v2setVersion(200);}}//-----------------------------------------------------------------// MAVLink Statusuint8_t lastSeq = lastIndex[_message.sysid][_message.compid];uint8_t expectedSeq = lastSeq + 1;// Increase receive countertotalReceiveCounter[mavlinkChannel]++;

最终定位到mavlink_helpers.h 中的这个函数：（代码中有中文解析）

MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_status_t* status,uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{int bufferIndex = 0;status->msg_received = MAVLINK_FRAMING_INCOMPLETE;switch (status->parse_state){case MAVLINK_PARSE_STATE_UNINIT:case MAVLINK_PARSE_STATE_IDLE:if (c == MAVLINK_STX) //可以看到，第一个字节，传进来会先判断是不是起始字节，这里要注意mavlink1.0和mavlink2.0的结构是不一样的，但是向下兼容，我px4发出来的是1.0，但是到这里变成了2.0，不知道咋回事{status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;//这里状态变了，用于第二个字节进来rxmsg->len = 0;rxmsg->magic = c;//赋值status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;mavlink_start_checksum(rxmsg);} else if (c == MAVLINK_STX_MAVLINK1){status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;rxmsg->len = 0;rxmsg->magic = c;status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;mavlink_start_checksum(rxmsg);}break;case MAVLINK_PARSE_STATE_GOT_STX://由于状态变成了MAVLINK_PARSE_STATE_GOT_STX，所以第二个字节走了这一步；if (status->msg_received
/* Support shorter buffers than thedefault maximum packet size */
#if (MAVLINK_MAX_PAYLOAD_LEN < 255)|| c > MAVLINK_MAX_PAYLOAD_LEN
#endif){status->buffer_overrun++;_mav_parse_error(status);status->msg_received = 0;status->parse_state = MAVLINK_PARSE_STATE_IDLE;}else{// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2rxmsg->len = c;//赋值status->packet_idx = 0;mavlink_update_checksum(rxmsg, c);if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {rxmsg->incompat_flags = 0;rxmsg->compat_flags = 0;status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;} else {status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;//这里读取到playload长度字节，状态变成了MAVLINK_PARSE_STATE_GOT_LENGTH，就这样循环，每读一个就给rxmsg的成员赋值一次，直到把字节读完，形成完整的消息结构体}}break;case MAVLINK_PARSE_STATE_GOT_LENGTH:rxmsg->incompat_flags = c;if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {// message includes an incompatible feature flag_mav_parse_error(status);status->msg_received = 0;status->parse_state = MAVLINK_PARSE_STATE_IDLE;break;}mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;break;case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:rxmsg->compat_flags = c;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;break;case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:rxmsg->seq = c;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;break;case MAVLINK_PARSE_STATE_GOT_SEQ:rxmsg->sysid = c;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;break;case MAVLINK_PARSE_STATE_GOT_SYSID:rxmsg->compid = c;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;break;case MAVLINK_PARSE_STATE_GOT_COMPID:rxmsg->msgid = c;      mavlink_update_checksum(rxmsg, c);if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {if(rxmsg->len > 0) {status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;} else {status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;}
#ifdef MAVLINK_CHECK_MESSAGE_LENGTHif (rxmsg->len < mavlink_min_message_length(rxmsg) ||rxmsg->len > mavlink_max_message_length(rxmsg)) {_mav_parse_error(status);status->parse_state = MAVLINK_PARSE_STATE_IDLE;break;}
#endif} else {status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;}break;case MAVLINK_PARSE_STATE_GOT_MSGID1:rxmsg->msgid |= c<<8;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;break;case MAVLINK_PARSE_STATE_GOT_MSGID2:rxmsg->msgid |= ((uint32_t)c)<<16;mavlink_update_checksum(rxmsg, c);if(rxmsg->len > 0){status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;} else {status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;}
#ifdef MAVLINK_CHECK_MESSAGE_LENGTHif (rxmsg->len < mavlink_min_message_length(rxmsg) ||rxmsg->len > mavlink_max_message_length(rxmsg)){_mav_parse_error(status);status->parse_state = MAVLINK_PARSE_STATE_IDLE;break;}
#endifbreak;case MAVLINK_PARSE_STATE_GOT_MSGID3:_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;mavlink_update_checksum(rxmsg, c);if (status->packet_idx == rxmsg->len){status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;}break;case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {      const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);uint8_t crc_extra = e?e->crc_extra:0;mavlink_update_checksum(rxmsg, crc_extra);//我在每一次的case语句里面都qDebug了msgid，我的直觉告诉我这个校验有问题，我查看了 mavlink_get_msg_entry函数的定义；        if (c != (rxmsg->checksum & 0xFF)) {status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;} else {status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;}rxmsg->ck[0] = c;// zero-fill the packet to cope with short incoming packetsif (e && status->packet_idx < e->max_msg_len) {memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx);}break;}case MAVLINK_PARSE_STATE_GOT_CRC1:case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {// got a bad CRC messagestatus->msg_received = MAVLINK_FRAMING_BAD_CRC;} else {// Successfully got messagestatus->msg_received = MAVLINK_FRAMING_OK;}rxmsg->ck[1] = c;if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;// If the CRC is already wrong, don't overwrite msg_received,// otherwise we can end up with garbage flagged as valid.if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {status->msg_received = MAVLINK_FRAMING_INCOMPLETE;}} else {if (status->signing &&(status->signing->accept_unsigned_callback == NULL ||!status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {// If the CRC is already wrong, don't overwrite msg_received.if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;}}status->parse_state = MAVLINK_PARSE_STATE_IDLE;if (r_message != NULL) {memcpy(r_message, rxmsg, sizeof(mavlink_message_t));}}break;case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c;status->signature_wait--;if (status->signature_wait == 0) {// we have the whole signature, check it is OKbool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);if (!sig_ok &&(status->signing->accept_unsigned_callback &&status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {// accepted via application level overridesig_ok = true;}if (sig_ok) {status->msg_received = MAVLINK_FRAMING_OK;} else {status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;}status->parse_state = MAVLINK_PARSE_STATE_IDLE;if (r_message !=NULL) {memcpy(r_message, rxmsg, sizeof(mavlink_message_t));}}break;}bufferIndex++;// If a message has been sucessfully decoded, check indexif (status->msg_received == MAVLINK_FRAMING_OK){//while(status->current_seq != rxmsg->seq)//{//    status->packet_rx_drop_count++;//               status->current_seq++;//}status->current_rx_seq = rxmsg->seq;// Initial condition: If no packet has been received so far, drop count is undefinedif (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;// Count this packet as receivedstatus->packet_rx_success_count++;}if (r_message != NULL) {r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg}if (r_mavlink_status != NULL) {    r_mavlink_status->parse_state = status->parse_state;r_mavlink_status->packet_idx = status->packet_idx;r_mavlink_status->current_rx_seq = status->current_rx_seq+1;r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;r_mavlink_status->packet_rx_drop_count = status->parse_error;r_mavlink_status->flags = status->flags;}status->parse_error = 0;if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {/*the CRC came out wrong. We now need to overwrite themsg CRC with the one on the wire so that if thecaller decides to forward the message anyway thatmavlink_msg_to_send_buffer() won't overwrite thechecksum*/if (r_message != NULL) {r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);}}return status->msg_received;
}

感觉case MAVLINK_PARSE_STATE_GOT_PAYLOAD里面的校验可能有问题，因为我在每个case里面都qDebug了msgid，166都在，所以有可能是校验除了问题，我查看了mavlink_get_msg_entry的代码：

#ifndef MAVLINK_GET_MSG_ENTRY
MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid)
{//下面这个MAVLINK_MESSAGE_CRCS就是common.h和ardupilotmega.h的宏定义//下面回去比对msgid和message_crcs是否匹配，也就是我们一开始修改ardupilotmega.h的第一处地方；static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS;/*use a bisection search to find the right entry. A perfect hash may be betterNote that this assumes the table is sorted by msgid*/uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]) - 1;while (low < high) {uint32_t mid = (low+1+high)/2;if (msgid < mavlink_message_crcs[mid].msgid) {high = mid-1;continue;}if (msgid > mavlink_message_crcs[mid].msgid) {low = mid;continue;}low = mid;break;}if (mavlink_message_crcs[low].msgid != msgid) {// msgid is not in the tablereturn NULL;}return &mavlink_message_crcs[low];
}
#endif // MAVLINK_GET_MSG_ENTRY

直到这里我才明白为什么不能在common.h里面修改了，打开ardupilotmega.h文件：

#ifndef MAVLINK_MESSAGE_CRCS
#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {1, 124, 31, 31, 0, 0, 0}, {2, 137, 12, 12, 0, 0, 0}, {4, 237, 14, 14, 3, 12, 13}, {5, 217, 28, 28, 1, 0, 0}, {6, 104, 3, 3, 0, 0, 0}, {7, 119, 32, 32, 0, 0, 0}, {8, 117, 36, 36, 0, 0, 0}, {11, 89, 6, 6, 1, 4, 0}, {20, 214, 20, 20, 3, 2, 3}, {21, 159, 2, 2, 3, 0, 1}, {22, 220, 25, 25, 0, 0, 0}, {23, 168, 23, 23, 3, 4, 5}, {24, 24, 30, 52, 0, 0, 0}, {25, 23, 101, 101, 0, 0, 0}, {26, 170, 22, 24, 0, 0, 0}, {27, 144, 26, 29, 0, 0, 0}, {28, 67, 16, 16, 0, 0, 0}, {29, 115, 14, 16, 0, 0, 0}, {30, 39, 28, 28, 0, 0, 0}, {31, 246, 32, 48, 0, 0, 0}, {32, 185, 28, 28, 0, 0, 0}, {33, 104, 28, 28, 0, 0, 0}, {34, 237, 22, 22, 0, 0, 0}, {35, 244, 22, 22, 0, 0, 0}, {36, 222, 21, 37, 0, 0, 0}, {37, 212, 6, 7, 3, 4, 5}, {38, 9, 6, 7, 3, 4, 5}, {39, 254, 37, 38, 3, 32, 33}, {40, 230,

#include "./mavlink_msg_water_depth.h"
#include "./mavlink_msg_mcu_status.h"
#include "./mavlink_msg_vehicle_zkrt.h"// base include
#include "../common/common.h"
#include "../uAvionix/uAvionix.h"
#include "../icarous/icarous.h"#undef MAVLINK_THIS_XML_IDX
#define MAVLINK_THIS_XML_IDX 0#if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX
# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL,

可以看到：ardupilotmega.h里面先定义了MAVLINK_MESSAGE_CRCS，然后在后面才#include "../common/common.h"，所以在预编译common.h里面的时候就已经定义了MAVLINK_MESSAGE_CRCS，宏定义也就不起作用了，所以在qgc接收的时候就解析不了，而可以发现，common和ardupilotmega的宏定义，是ardupilotmega包含了common里面的宏定义，所以为什么qgc用ardupilotmega.h头文件也可收到mavlink，所以改掉ardupilotmega.h里面注释掉的166号消息对于接收px4的消息是没有影响的，因为px4里面根本就没有用到166号msgid，这也是为什么网上大部分的教程都用166（虽然他们本来就是赋值粘贴代码。。。。。。）

下次有空把qgc解析自定义的消息包流程也发布出来，记录下自己的学习过程，不得不感叹px4和qgc的工程之大，弄个mavlink消息都弄了好几天！

QGC接收PX4自定义Mavlink消息（二）qgc接收相关推荐

QGC添加自定义组件和发送自定义MAVLINK消息
QGC添加自定义组件和发送自定义MAVLINK消息一.添加自定义组件 1.1 在飞行界面添加组件 1.2 实现组件事件 1.3 在MOCK模拟链接中实现验证 1.4 验证二.自定义MAVLINK消 ...
px4使用mavlink和其他无人机通信并实现跟踪
交流学习加qq:2096723956 PX4固件版本1.11.0 以位置信息为例,本文直接使用MAVLINK库中预定义的位置消息,如果想自定义MAVLINK消息,参考 https://blog.csd ...
新建Mavlink消息
1.下载Mavlink生成器 1.1 Git clone //需要在翻墙的网络环境下下载 git clone https://github.com/mavlink/mavlink.git 1.2 进入 ...
XBee模块实现QGC与PX4飞控的组网通信连接
本篇博客介绍如何利用XBee模块实现QGC地面站与飞控的通信一.问题的提出正如上一篇博客指出,PX4飞控原装数传模块(3DR Radio)只能一对一通信,并不能实现多机组网通信,而XBee模块 ...
PX4读取串口消息，并通过MAVLINK发送给地面站
参考:(131条消息) PX4飞控读取UART串口信息通过Mavlink传给QGC地面站显示_XXX_UUU_XXX的博客-CSDN博客_px4串口2 PX4版本:1.12.1-3 QGC版本: 4. ...
PX4模块设计之二：uORB消息代理
PX4模块设计之二:uORB消息代理 1. uORB模块接口 1.1. uORB服务接口 1.2. uORB消息注册/去注册接口 1.3. uORB消息发布接口 1.4. uORB消息订阅/去订阅接口 ...
PX4串口添加传感器—在QGC上添加串口数据显示
前言因为项目要求,(在PX4上添加拉力传感器,并把数据显示在QGC的地图上),本人开始了苦皮的生活.从未接触飞控的我,一来就是开发..烧脑掉发啊.. 但人生是无所畏惧的.在学习的路途中有幸遇见我的师 ...
Java微信公众平台开发(二)--微信服务器post消息体的接收
转自: http://www.cuiyongzhi.com/post/39.html 在上一篇的文章中我们详细讲述了如何将我们的应用服务器和微信腾讯服务器之间的对接操作,最后接入成功,不知道你有没有发 ...
【一学就会的ROS基础入门教程】03-1 ROS基础编程：ROS工作空间的创建、话题topic的发布与接收、以及话题消息的自定义使用
[一学就会的ROS基础入门教程 ]03-1 ROS基础编程:ROS工作空间的创建.话题topic的发布与接收.以及话题消息的自定义使用文前白话 1.创建工作空间与功能包关于工作空间的介绍创建开发 ...

QGC接收PX4自定义Mavlink消息（二）qgc接收

QGC接收PX4自定义Mavlink消息（二）qgc接收相关推荐

最新文章

热门文章