Here are some additions. My px4 version is 1.11 0dev, in Ubuntu 18 Developed on 04, qgc is the latest version of the current official website. Developed on windows, your source code will be slightly different due to version differences, so code copy, paste and compilation will certainly report errors, so you have to change it according to the specific situation. I will also briefly describe the logic of key mavlink sending and receiving.
The first step is to create the mavlink generated by the previous article generator_ msg_ vehicle_ zkrt. H header files are placed in this path: C:\Users\Sense\Desktop\QGC_Source\QGroundControl\libs\mavlink\include\mavlink\v2.0\ardupilotmega
My source code is on the desktop. Some people may have questions here. Why not put it in V2 0 \ common folder. After all, ardupilotmege is obviously the firmware of apm. The following will explain my experience of stepping on the pit under the common folder... Let's do this first, and the actual performance will receive data;
At ardurpilotmega H. three places need to be modified, the first:
The string of data in the figure above is used by qgc to verify the message id. the annotation is the original MSG of apm. Re add the self-defined 166msg verification data, which can be found in mavlink_ types. The following definitions are found in the H file:
typedef struct __mavlink_msg_entry {
uint32_t msgid;
uint8_t crc_extra;
uint8_t min_msg_len; // minimum message length
uint8_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 0
uint8_t target_component_ofs; // payload offset to target_component, or 0
} mavlink_msg_entry_t;As shown in the figure above, the first element is message id: 166, and the second to fourth elements can be in the mavlink generated by the generator_ msg_ vehicle_ zkrt. H header file, as shown:
The second point to be modified:
The third place to be modified:
It's done. We can go to vehicle Add qDebug to the CC file to test whether qgc has received successfully:
The test results are as follows:
Obviously, the customized message reception is much more than the heartbeat packet reception, because the heartbeat packet is 1hz and the customized is 30hz. Next, you can write your own processing function in switch! (this will be put in the next article)
Pit stepping experience and mavlink receiving code analysis:
At the beginning, my mavlink_ msg_ vehicle_ zkrt. The H header file is placed in the common folder, and the three modified places are the corresponding places in the common folder, starting with vehicle The switch in CC can't receive custom data. After studying for a long time, let's talk about the general process of mavlink receiving:
At the top of the switch, you can find mavlink_message_t message has been parsed and passed in. Here we can access message members, such as msgid, but after debug ging, we find that msgid is not equal to 166, so we continue to track forward:
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 handled
if (!(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS && _vehicleLinkManager->containsLink(link))) {
return;
}
}connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); //Here, it is found that this function is bound with a signal. Continue to collect the query to see where it is triggered
The MAVLinkProtocol::messageReceived signal is triggered in the following function:
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
//In the qDebug variable b, you will find that b is the data just received by qgc, which has not been parsed. Here, I find that msgid does have 166
//But after parsing, it's gone
// 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();
//Here is the function to parse mavlink_ parse_ char(mavlinkChannel, static_cast<uint8_t>(b[position]
//Please note that bytes are passed into the loop for parsing, so continue to track the function
for (int position = 0; position < b.size(); position++) {
if (mavlink_parse_char(mavlinkChannel, static_cast<uint8_t>(b[position]), &_message, &_status)) {
// Got a valid message
if (!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 v2
setVersion(200);
}
}
//-----------------------------------------------------------------
// MAVLink Status
uint8_t lastSeq = lastIndex[_message.sysid][_message.compid];
uint8_t expectedSeq = lastSeq + 1;
// Increase receive counter
totalReceiveCounter[mavlinkChannel]++;Finally locate to mavlink_ helpers. This function in H: (Chinese parsing in the code)
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) //As you can see, the first byte will be passed in to judge whether it is the starting byte. Here, pay attention to mavlink 1 0 and mavlink 2 The structure of 0 is different, but it is downward compatible. I px4 sent 1.0, but here it has become 2.0. I don't know what's going on
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;//Here the state changes for the second byte to come in
rxmsg->len = 0;
rxmsg->magic = c;//assignment
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://Since the state becomes MAVLINK_PARSE_STATE_GOT_STX, so the second byte takes this step;
if (status->msg_received
/* Support shorter buffers than the
default 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 CRC2
rxmsg->len = c;//assignment
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;//Here, the length byte of the payload is read, and the status becomes MAVLINK_PARSE_STATE_GOT_LENGTH, a loop like this, assigns a value to the member of rxmsg every time it is read, until the bytes are read, forming a complete message structure
}
}
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_LENGTH
if (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_LENGTH
if (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
break;
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);
//I qDebug msgid in every case statement. My intuition told me that there was a problem with this verification. I checked mavlink_ get_ msg_ Definition of entry function;
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 packets
if (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 message
status->msg_received = MAVLINK_FRAMING_BAD_CRC;
} else {
// Successfully got message
status->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 OK
bool 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 override
sig_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 index
if (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 undefined
if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
// Count this packet as received
status->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 the
msg CRC with the one on the wire so that if the
caller decides to forward the message anyway that
mavlink_msg_to_send_buffer() won't overwrite the
checksum
*/
if (r_message != NULL) {
r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);
}
}
return status->msg_received;
}Feel case mavlink_ PARSE_ STATE_ GOT_ There may be a problem with the verification in payload, because I have qDebug in every case, msgid and 166, so there may be a problem with the verification. In addition to the problem, I checked mavlink_get_msg_entry code:
#ifndef MAVLINK_GET_MSG_ENTRY
MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid)
{
//The following MAVLINK_MESSAGE_CRCS is common H and ardupilotmega Macro definition of H
//Let's go back and compare msgid and message_ Whether the CRCs matches, that is, we modified ardupilotmega at the beginning The first place of 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 better
Note 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 table
return NULL;
}
return &mavlink_message_crcs[low];
}
#endif // MAVLINK_GET_MSG_ENTRYOnly here do I understand why I can't be in common H, open ardupilotmega H documents:
#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,You can see: ardupilotmega Mavlink is defined first in H_ MESSAGE_ CRCs, and then #include "../common/common.h" later, so common.h is precompiled Mavlink has been defined in H_ MESSAGE_ In CRCs, the macro definition will not work, so it cannot be parsed when qgc receives it. It can be found that the macro definitions of common and ardupilotmega contain the macro definitions in common, so why does qgc use ardupilotmega H header file can also receive mavlink, so change ardupilotmega The message No. 166 commented out in H has no impact on receiving px4 messages, because msgid No. 166 is not used in px4 at all, which is why most online tutorials use 166 (although they are assignment and paste code...)
The next time I have time to publish the customized message package process of qgc parsing, record my learning process, and have to lament the big project of px4 and qgc. It took me several days to get a mavlink message!