Skip to content

Q
qgroundcontrol
Commit b4dc5f3c authored by Don Gagne's avatar Don Gagne
Browse files
Options

Mavlink 2.0 support 

No signing support yet
parent 531b9ac3
Hide whitespace changes
Inline Side-by-side
Showing with 155 additions and 166 deletions
QGCExternalLibs.pri
View file @ b4dc5f3c
......@@ -17,7 +17,7 @@ WindowsBuild {
# a single compiled codebase this hardwiring of dialect can go away. But until then
# this "workaround" is needed.
MAVLINKPATH_REL = libs/mavlink/include/mavlink/v1.0
MAVLINKPATH_REL = libs/mavlink/include/mavlink/v2.0
MAVLINKPATH = $$BASEDIR/$$MAVLINKPATH_REL
MAVLINK_CONF = ardupilotmega
DEFINES += MAVLINK_NO_DATA
......
v2.0 @ 36f37bde
Subproject commit 1ae7a11de2d17ee620edf71a9d7342e601eec347
Subproject commit 36f37bde1df2dd36661fbcbd6ed5aaf0424c8269
src/comm/LinkManager.cc
View file @ b4dc5f3c
......@@ -191,6 +191,8 @@ void LinkManager::_addLink(LinkInterface* link)
if (!(_mavlinkChannelsUsedBitMask & 1 << i)) {
mavlink_reset_channel_status(i);
link->_setMavlinkChannel(i);
// Start the channel on Mav 1 protocol
mavlink_get_channel_status(i)->flags = mavlink_get_channel_status(i)->flags | MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
_mavlinkChannelsUsedBitMask |= 1 << i;
channelSet = true;
break;
......
src/comm/MAVLinkProtocol.cc
View file @ b4dc5f3c
......@@ -214,6 +214,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
decodedFirstPacket = 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)) {
qDebug() << "switch to mavlink 2.0" << mavlinkStatus->flags;
mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
} else if ((mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && !(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
qDebug() << "switch to mavlink 1.0" << mavlinkStatus->flags;
mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
if(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS)
{
// process telemetry status message
......
src/ui/MAVLinkDecoder.cc
View file @ b4dc5f3c
#define MAVLINK_USE_MESSAGE_INFO
#include <stddef.h> // Hack workaround for Mav 2.0 header problem with respect to offsetof usage
#include "QGCMAVLink.h"
#include "MAVLinkDecoder.h"
#include <QDebug>
......@@ -10,8 +13,6 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
// http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
moveToThread(this);
mavlink_message_info_t msg[256] = MAVLINK_MESSAGE_INFO;
memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256);
memset(receivedMessages, 0, sizeof(mavlink_message_t)*256);
for (unsigned int i = 0; i<255;++i)
{
......@@ -44,7 +45,6 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
#ifdef MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE
messageFilter.insert(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, false);
#endif
messageFilter.insert(MAVLINK_MSG_ID_EXTENDED_MESSAGE, false);
messageFilter.insert(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, false);
textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG, false);
......@@ -73,6 +73,7 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t));
uint8_t msgid = message.msgid;
const mavlink_message_info_t* msgInfo = mavlink_get_message_info(&message);
// Store an arrival time for this message. This value ends up being calculated later.
quint64 time = 0;
......@@ -91,13 +92,13 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
// See if first value is a time value and if it is, use that as the arrival time for this data.
uint8_t fieldid = 0;
uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8;
if (QString(messageInfo[msgid].fields[fieldid].name) == QString("time_boot_ms") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT32_T)
if (QString(msgInfo->fields[fieldid].name) == QString("time_boot_ms") && msgInfo->fields[fieldid].type == MAVLINK_TYPE_UINT32_T)
{
time = *((quint32*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
time = *((quint32*)(m+msgInfo->fields[fieldid].wire_offset));
}
else if (QString(messageInfo[msgid].fields[fieldid].name).contains("usec") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT64_T)
else if (QString(msgInfo->fields[fieldid].name).contains("usec") && msgInfo->fields[fieldid].type == MAVLINK_TYPE_UINT64_T)
{
time = *((quint64*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
time = *((quint64*)(m+msgInfo->fields[fieldid].wire_offset));
time = (time+500)/1000; // Scale to milliseconds, round up/down correctly
}
}
......@@ -106,7 +107,7 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
time = getUnixTimeFromMs(message.sysid, time);
// Send out all field values for this message
for (unsigned int i = 0; i < messageInfo[msgid].num_fields; ++i)
for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
{
emitFieldValue(&message, i, time);
}
......@@ -196,6 +197,7 @@ quint64 MAVLinkDecoder::getUnixTimeFromMs(int systemID, quint64 time)
void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time)
{
bool multiComponentSourceDetected = false;
const mavlink_message_info_t* msgInfo = mavlink_get_message_info(msg);
// Store component ID
if (componentID[msg->msgid] == -1)
......@@ -216,7 +218,7 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
// Add field tree widget item
uint8_t msgid = msg->msgid;
if (messageFilter.contains(msgid)) return;
QString fieldName(messageInfo[msgid].fields[fieldid].name);
QString fieldName(msgInfo->fields[fieldid].name);
QString fieldType;
uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8;
QString name("%1.%2");
......@@ -265,7 +267,7 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
// XXX this is really ugly, but we do not know a better way to do this
mavlink_rc_channels_raw_t raw;
mavlink_msg_rc_channels_raw_decode(msg, &raw);
name = name.arg(messageInfo[msgid].name).arg(fieldName);
name = name.arg(msgInfo->name).arg(fieldName);
name.prepend(QString("port%1_").arg(raw.port));
}
else if (msgid == MAVLINK_MSG_ID_RC_CHANNELS_SCALED)
......@@ -273,7 +275,7 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
// XXX this is really ugly, but we do not know a better way to do this
mavlink_rc_channels_scaled_t scaled;
mavlink_msg_rc_channels_scaled_decode(msg, &scaled);
name = name.arg(messageInfo[msgid].name).arg(fieldName);
name = name.arg(msgInfo->name).arg(fieldName);
name.prepend(QString("port%1_").arg(scaled.port));
}
else if (msgid == MAVLINK_MSG_ID_SERVO_OUTPUT_RAW)
......@@ -281,12 +283,12 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
// XXX this is really ugly, but we do not know a better way to do this
mavlink_servo_output_raw_t servo;
mavlink_msg_servo_output_raw_decode(msg, &servo);
name = name.arg(messageInfo[msgid].name).arg(fieldName);
name = name.arg(msgInfo->name).arg(fieldName);
name.prepend(QString("port%1_").arg(servo.port));
}
else
{
name = name.arg(messageInfo[msgid].name).arg(fieldName);
name = name.arg(msgInfo->name).arg(fieldName);
}
if (multiComponentSourceDetected)
......@@ -296,31 +298,31 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
name = name.prepend(QString("M%1:").arg(msg->sysid));
switch (messageInfo[msgid].fields[fieldid].type)
switch (msgInfo->fields[fieldid].type)
{
case MAVLINK_TYPE_CHAR:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
char* str = (char*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
char* str = (char*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
str[messageInfo[msgid].fields[fieldid].array_length-1] = '\0';
str[msgInfo->fields[fieldid].array_length-1] = '\0';
QString string(name + ": " + str);
if (!textMessageFilter.contains(msgid)) emit textMessageReceived(msg->sysid, msg->compid, MAV_SEVERITY_INFO, string);
}
else
{
// Single char
char b = *((char*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
unit = QString("char[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
char b = *((char*)(m+msgInfo->fields[fieldid].wire_offset));
unit = QString("char[%1]").arg(msgInfo->fields[fieldid].array_length);
emit valueChanged(msg->sysid, name, unit, b, time);
}
break;
case MAVLINK_TYPE_UINT8_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
uint8_t* nums = m+messageInfo[msgid].fields[fieldid].wire_offset;
fieldType = QString("uint8_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
uint8_t* nums = m+msgInfo->fields[fieldid].wire_offset;
fieldType = QString("uint8_t[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
}
......@@ -328,17 +330,17 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
uint8_t u = *(m+messageInfo[msgid].fields[fieldid].wire_offset);
uint8_t u = *(m+msgInfo->fields[fieldid].wire_offset);
fieldType = "uint8_t";
emit valueChanged(msg->sysid, name, fieldType, u, time);
}
break;
case MAVLINK_TYPE_INT8_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
int8_t* nums = (int8_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
fieldType = QString("int8_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
int8_t* nums = (int8_t*)(m+msgInfo->fields[fieldid].wire_offset);
fieldType = QString("int8_t[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
}
......@@ -346,17 +348,17 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
int8_t n = *((int8_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
int8_t n = *((int8_t*)(m+msgInfo->fields[fieldid].wire_offset));
fieldType = "int8_t";
emit valueChanged(msg->sysid, name, fieldType, n, time);
}
break;
case MAVLINK_TYPE_UINT16_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
uint16_t* nums = (uint16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
fieldType = QString("uint16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
uint16_t* nums = (uint16_t*)(m+msgInfo->fields[fieldid].wire_offset);
fieldType = QString("uint16_t[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
}
......@@ -364,17 +366,17 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
uint16_t n = *((uint16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
uint16_t n = *((uint16_t*)(m+msgInfo->fields[fieldid].wire_offset));
fieldType = "uint16_t";
emit valueChanged(msg->sysid, name, fieldType, n, time);
}
break;
case MAVLINK_TYPE_INT16_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
int16_t* nums = (int16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
fieldType = QString("int16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
int16_t* nums = (int16_t*)(m+msgInfo->fields[fieldid].wire_offset);
fieldType = QString("int16_t[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
}
......@@ -382,17 +384,17 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
int16_t n = *((int16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
int16_t n = *((int16_t*)(m+msgInfo->fields[fieldid].wire_offset));
fieldType = "int16_t";
emit valueChanged(msg->sysid, name, fieldType, n, time);
}
break;
case MAVLINK_TYPE_UINT32_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
uint32_t* nums = (uint32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
fieldType = QString("uint32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
uint32_t* nums = (uint32_t*)(m+msgInfo->fields[fieldid].wire_offset);
fieldType = QString("uint32_t[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
}
......@@ -400,17 +402,17 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
uint32_t n = *((uint32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
uint32_t n = *((uint32_t*)(m+msgInfo->fields[fieldid].wire_offset));
fieldType = "uint32_t";
emit valueChanged(msg->sysid, name, fieldType, n, time);
}
break;
case MAVLINK_TYPE_INT32_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
int32_t* nums = (int32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
fieldType = QString("int32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
int32_t* nums = (int32_t*)(m+msgInfo->fields[fieldid].wire_offset);
fieldType = QString("int32_t[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
}
......@@ -418,17 +420,17 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
int32_t n = *((int32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
int32_t n = *((int32_t*)(m+msgInfo->fields[fieldid].wire_offset));
fieldType = "int32_t";
emit valueChanged(msg->sysid, name, fieldType, n, time);
}
break;
case MAVLINK_TYPE_FLOAT:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
float* nums = (float*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
fieldType = QString("float[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
float* nums = (float*)(m+msgInfo->fields[fieldid].wire_offset);
fieldType = QString("float[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, (float)(nums[j]), time);
}
......@@ -436,17 +438,17 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
float f = *((float*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
float f = *((float*)(m+msgInfo->fields[fieldid].wire_offset));
fieldType = "float";
emit valueChanged(msg->sysid, name, fieldType, f, time);
}
break;
case MAVLINK_TYPE_DOUBLE:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
double* nums = (double*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
fieldType = QString("double[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
double* nums = (double*)(m+msgInfo->fields[fieldid].wire_offset);
fieldType = QString("double[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
}
......@@ -454,17 +456,17 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
double f = *((double*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
double f = *((double*)(m+msgInfo->fields[fieldid].wire_offset));
fieldType = "double";
emit valueChanged(msg->sysid, name, fieldType, f, time);
}
break;
case MAVLINK_TYPE_UINT64_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
uint64_t* nums = (uint64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
fieldType = QString("uint64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
uint64_t* nums = (uint64_t*)(m+msgInfo->fields[fieldid].wire_offset);
fieldType = QString("uint64_t[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, (quint64) nums[j], time);
}
......@@ -472,17 +474,17 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
uint64_t n = *((uint64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
uint64_t n = *((uint64_t*)(m+msgInfo->fields[fieldid].wire_offset));
fieldType = "uint64_t";
emit valueChanged(msg->sysid, name, fieldType, (quint64) n, time);
}
break;
case MAVLINK_TYPE_INT64_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
int64_t* nums = (int64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
fieldType = QString("int64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length);
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
int64_t* nums = (int64_t*)(m+msgInfo->fields[fieldid].wire_offset);
fieldType = QString("int64_t[%1]").arg(msgInfo->fields[fieldid].array_length);
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, (qint64) nums[j], time);
}
......@@ -490,7 +492,7 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
else
{
// Single value
int64_t n = *((int64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
int64_t n = *((int64_t*)(m+msgInfo->fields[fieldid].wire_offset));
fieldType = "int64_t";
emit valueChanged(msg->sysid, name, fieldType, (qint64) n, time);
}
......
src/ui/MAVLinkDecoder.h
View file @ b4dc5f3c
......@@ -26,7 +26,6 @@ protected:
quint64 getUnixTimeFromMs(int systemID, quint64 time);
mavlink_message_t receivedMessages[256]; ///< Available / known messages
mavlink_message_info_t messageInfo[256]; ///< Message information
QMap<uint16_t, bool> messageFilter; ///< Message/field names not to emit
QMap<uint16_t, bool> textMessageFilter; ///< Message/field names not to emit in text mode
int componentID[256]; ///< Multi component detection
......
src/ui/QGCMAVLinkInspector.cc
View file @ b4dc5f3c
#include <QList>
#define MAVLINK_USE_MESSAGE_INFO
#include <stddef.h> // Hack workaround for Mav 2.0 header problem with respect to offsetof usage
#include "QGCMAVLink.h"
#include "QGCMAVLinkInspector.h"
#include "MultiVehicleManager.h"
......@@ -8,6 +8,7 @@
#include "ui_QGCMAVLinkInspector.h"
#include <QList>
#include <QDebug>
const float QGCMAVLinkInspector::updateHzLowpass = 0.2f;
......@@ -26,10 +27,6 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(const QString& title, QAction* action,
ui->systemComboBox->addItem(tr("All"), 0);
ui->componentComboBox->addItem(tr("All"), 0);
// Store metadata for all MAVLink messages.
mavlink_message_info_t msg_infos[256] = MAVLINK_MESSAGE_INFO;
memcpy(messageInfo, msg_infos, sizeof(mavlink_message_info_t)*256);
// Set up the column headers for the message listing
QStringList header;
header << tr("Name");
......@@ -181,8 +178,9 @@ void QGCMAVLinkInspector::refreshView()
for(ite=uasMessageStorage.constBegin(); ite!=uasMessageStorage.constEnd();++ite)
{
mavlink_message_t* msg = ite.value();
const mavlink_message_info_t* msgInfo = mavlink_get_message_info(msg);
// Ignore NULL values
if (msg->msgid == 0xFF) continue;
......@@ -227,7 +225,7 @@ void QGCMAVLinkInspector::refreshView()
// Update the tree view
QString messageName("%1 (%2 Hz, #%3)");
messageName = messageName.arg(messageInfo[msg->msgid].name).arg(msgHz, 3, 'f', 1).arg(msg->msgid);
messageName = messageName.arg(msgInfo->name).arg(msgHz, 3, 'f', 1).arg(msg->msgid);
addUAStoTree(msg->sysid);
......@@ -245,7 +243,7 @@ void QGCMAVLinkInspector::refreshView()
QStringList fields;
fields << messageName;
QTreeWidgetItem* widget = new QTreeWidgetItem();
for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i)
for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
{
QTreeWidgetItem* field = new QTreeWidgetItem();
widget->addChild(field);
......@@ -262,32 +260,12 @@ void QGCMAVLinkInspector::refreshView()
{
message->setFirstColumnSpanned(true);
message->setData(0, Qt::DisplayRole, QVariant(messageName));
for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i)
for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
{
updateField(msg->sysid,msg->msgid, i, message->child(i));
updateField(msg, msgInfo, i, message->child(i));
}
}
}
if (selectedSystemID == 0 || selectedComponentID == 0)
{
return;
}
for (int i = 0; i < 256; ++i)//mavlink_message_t msg, receivedMessages)
{
const char* msgname = messageInfo[i].name;
size_t namelen = strnlen(msgname, 5);
if (namelen < 3) {
continue;
}
if (!strcmp(msgname, "EMPTY")) {
continue;
}
}
}
void QGCMAVLinkInspector::addUAStoTree(int sysId)
......@@ -439,17 +417,17 @@ QGCMAVLinkInspector::~QGCMAVLinkInspector()
delete ui;
}
void QGCMAVLinkInspector::updateField(int sysid, int msgid, int fieldid, QTreeWidgetItem* item)
void QGCMAVLinkInspector::updateField(mavlink_message_t* msg, const mavlink_message_info_t* msgInfo, int fieldid, QTreeWidgetItem* item)
{
// Add field tree widget item
item->setData(0, Qt::DisplayRole, QVariant(messageInfo[msgid].fields[fieldid].name));
item->setData(0, Qt::DisplayRole, QVariant(msgInfo->fields[fieldid].name));
bool msgFound = false;
QMap<int, mavlink_message_t* >::const_iterator iteMsg = uasMessageStorage.find(sysid);
QMap<int, mavlink_message_t* >::const_iterator iteMsg = uasMessageStorage.find(msg->sysid);
mavlink_message_t* uasMessage = iteMsg.value();
while((iteMsg != uasMessageStorage.end()) && (iteMsg.key() == sysid))
while((iteMsg != uasMessageStorage.end()) && (iteMsg.key() == msg->sysid))
{
if (iteMsg.value()->msgid == msgid)
if (iteMsg.value()->msgid == msg->msgid)
{
msgFound = true;
uasMessage = iteMsg.value();
......@@ -466,14 +444,14 @@ void QGCMAVLinkInspector::updateField(int sysid, int msgid, int fieldid, QTreeWi
uint8_t* m = ((uint8_t*)uasMessage)+8;
switch (messageInfo[msgid].fields[fieldid].type)
switch (msgInfo->fields[fieldid].type)
{
case MAVLINK_TYPE_CHAR:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
char* str = (char*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
char* str = (char*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
str[messageInfo[msgid].fields[fieldid].array_length-1] = '\0';
str[msgInfo->fields[fieldid].array_length-1] = '\0';
QString string(str);
item->setData(2, Qt::DisplayRole, "char");
item->setData(1, Qt::DisplayRole, string);
......@@ -481,227 +459,227 @@ void QGCMAVLinkInspector::updateField(int sysid, int msgid, int fieldid, QTreeWi
else
{
// Single char
char b = *((char*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, QString("char[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
char b = *((char*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, QString("char[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, b);
}
break;
case MAVLINK_TYPE_UINT8_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
uint8_t* nums = m+messageInfo[msgid].fields[fieldid].wire_offset;
uint8_t* nums = m+msgInfo->fields[fieldid].wire_offset;
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("uint8_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("uint8_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
uint8_t u = *(m+messageInfo[msgid].fields[fieldid].wire_offset);
uint8_t u = *(m+msgInfo->fields[fieldid].wire_offset);
item->setData(2, Qt::DisplayRole, "uint8_t");
item->setData(1, Qt::DisplayRole, u);
}
break;
case MAVLINK_TYPE_INT8_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
int8_t* nums = (int8_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
int8_t* nums = (int8_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("int8_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("int8_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
int8_t n = *((int8_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
int8_t n = *((int8_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "int8_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_UINT16_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
uint16_t* nums = (uint16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
uint16_t* nums = (uint16_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("uint16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("uint16_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
uint16_t n = *((uint16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
uint16_t n = *((uint16_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "uint16_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_INT16_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
int16_t* nums = (int16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
int16_t* nums = (int16_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("int16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("int16_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
int16_t n = *((int16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
int16_t n = *((int16_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "int16_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_UINT32_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
uint32_t* nums = (uint32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
uint32_t* nums = (uint32_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("uint32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("uint32_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
float n = *((uint32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
float n = *((uint32_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "uint32_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_INT32_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
int32_t* nums = (int32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
int32_t* nums = (int32_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("int32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("int32_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
int32_t n = *((int32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
int32_t n = *((int32_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "int32_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_FLOAT:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
float* nums = (float*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
float* nums = (float*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("float[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("float[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
float f = *((float*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
float f = *((float*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "float");
item->setData(1, Qt::DisplayRole, f);
}
break;
case MAVLINK_TYPE_DOUBLE:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
double* nums = (double*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
double* nums = (double*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("double[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("double[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
double f = *((double*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
double f = *((double*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "double");
item->setData(1, Qt::DisplayRole, f);
}
break;
case MAVLINK_TYPE_UINT64_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
uint64_t* nums = (uint64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
uint64_t* nums = (uint64_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("uint64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("uint64_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
uint64_t n = *((uint64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
uint64_t n = *((uint64_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "uint64_t");
item->setData(1, Qt::DisplayRole, (quint64) n);
}
break;
case MAVLINK_TYPE_INT64_T:
if (messageInfo[msgid].fields[fieldid].array_length > 0)
if (msgInfo->fields[fieldid].array_length > 0)
{
int64_t* nums = (int64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset);
int64_t* nums = (int64_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("int64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length));
item->setData(2, Qt::DisplayRole, QString("int64_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
int64_t n = *((int64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
int64_t n = *((int64_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "int64_t");
item->setData(1, Qt::DisplayRole, (qint64) n);
}
......
src/ui/QGCMAVLinkInspector.h
View file @ b4dc5f3c
......@@ -44,7 +44,6 @@ protected:
QMap<int, int> components; ///< Already observed components
QMap<int, float> onboardMessageInterval; ///< Stores the onboard selected data rate
QTimer updateTimer; ///< Only update at 1 Hz to not overload the GUI
mavlink_message_info_t messageInfo[256]; // Store the metadata for all available MAVLink messages.
QMap<int, QTreeWidgetItem* > uasTreeWidgetItems; ///< Tree of available uas with their widget
QMap<int, QMap<int, QTreeWidgetItem*>* > uasMsgTreeItems; ///< Stores the widget of the received message for each UAS
......@@ -57,7 +56,7 @@ protected:
QMap<int, QMap<int, quint64>* > uasLastMessageUpdate; ///< Stores the time of the last message for each message of each UAS
/* @brief Update one message field */
void updateField(int sysid, int msgid, int fieldid, QTreeWidgetItem* item);
void updateField(mavlink_message_t* msg, const mavlink_message_info_t* msgInfo, int fieldid, QTreeWidgetItem* item);
/** @brief Rebuild the list of components */
void rebuildComponentList();
/* @brief Create a new tree for a new UAS */
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment