zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Skip message parsing if the message is not being consumed. 

Work around message disassembly issue on ARM 32-bit.
Fix memory leaks.
QGC4.4
Gus Grubba 5 years ago committed by Lorenz Meier
parent
f320a7e4bc
commit
abff8929bf
2 changed files with 124 additions and 67 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 179
      src/AnalyzeView/MAVLinkInspectorController.cc
  2. 12
      src/AnalyzeView/MAVLinkInspectorController.h

179
src/AnalyzeView/MAVLinkInspectorController.cc
Unescape View File

@ -182,6 +182,12 @@ QGCMAVLinkMessage::QGCMAVLinkMessage(QObject *parent, mavlink_message_t* message @@ -182,6 +182,12 @@ QGCMAVLinkMessage::QGCMAVLinkMessage(QObject *parent, mavlink_message_t* message
}
//-----------------------------------------------------------------------------
QGCMAVLinkMessage::~QGCMAVLinkMessage()
{
_fields.clearAndDeleteContents();
}
//-----------------------------------------------------------------------------
void
QGCMAVLinkMessage::updateFieldSelection()
{
@ -215,8 +221,12 @@ QGCMAVLinkMessage::updateFreq() @@ -215,8 +221,12 @@ QGCMAVLinkMessage::updateFreq()
void
QGCMAVLinkMessage::update(mavlink_message_t* message)
{
_message = *message;
_count++;
//-- If we are not consuming this message, no need to parse it
if(!_selected && !_fieldSelected) {
return;
}
_message = *message;
const mavlink_message_info_t* msgInfo = mavlink_get_message_info(message);
if (!msgInfo) {
qWarning() << QStringLiteral("QGCMAVLinkMessage::update NULL msgInfo msgid(%1)").arg(message->msgid);
@ -230,104 +240,108 @@ QGCMAVLinkMessage::update(mavlink_message_t* message) @@ -230,104 +240,108 @@ QGCMAVLinkMessage::update(mavlink_message_t* message)
for (unsigned int i = 0; i < msgInfo->num_fields; ++i) {
QGCMAVLinkMessageField* f = qobject_cast<QGCMAVLinkMessageField*>(_fields.get(static_cast<int>(i)));
if(f) {
const unsigned int offset = msgInfo->fields[i].wire_offset;
const unsigned int array_length = msgInfo->fields[i].array_length;
static const unsigned int array_buffer_length = (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_CHECKSUM_BYTES + 7);
switch (msgInfo->fields[i].type) {
case MAVLINK_TYPE_CHAR:
f->setSelectable(false);
if (msgInfo->fields[i].array_length > 0) {
char* str = reinterpret_cast<char*>(m+ msgInfo->fields[i].wire_offset);
if (array_length > 0) {
char* str = reinterpret_cast<char*>(m + offset);
// Enforce null termination
str[msgInfo->fields[i].array_length - 1] = '\0';
str[array_length - 1] = '\0';
QString v(str);
f->updateValue(v, 0);
} else {
// Single char
char b = *(reinterpret_cast<char*>(m + msgInfo->fields[i].wire_offset));
char b = *(reinterpret_cast<char*>(m + offset));
QString v(b);
f->updateValue(v, 0);
}
break;
case MAVLINK_TYPE_UINT8_T:
if (msgInfo->fields[i].array_length > 0) {
uint8_t* nums = m+msgInfo->fields[i].wire_offset;
// Enforce null termination
if (array_length > 0) {
uint8_t* nums = m + offset;
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(nums[j]);
}
string += QString::number(nums[msgInfo->fields[i].array_length - 1]);
string += QString::number(nums[array_length - 1]);
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
uint8_t u = *(m+msgInfo->fields[i].wire_offset);
uint8_t u = *(m + offset);
f->updateValue(QString::number(u), static_cast<qreal>(u));
}
break;
case MAVLINK_TYPE_INT8_T:
if (msgInfo->fields[i].array_length > 0) {
int8_t* nums = reinterpret_cast<int8_t*>(m + msgInfo->fields[i].wire_offset);
// Enforce null termination
if (array_length > 0) {
int8_t* nums = reinterpret_cast<int8_t*>(m + offset);
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(nums[j]);
}
string += QString::number(nums[msgInfo->fields[i].array_length - 1]);
string += QString::number(nums[array_length - 1]);
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
int8_t n = *(reinterpret_cast<int8_t*>(m+msgInfo->fields[i].wire_offset));
int8_t n = *(reinterpret_cast<int8_t*>(m + offset));
f->updateValue(QString::number(n), static_cast<qreal>(n));
}
break;
case MAVLINK_TYPE_UINT16_T:
if (msgInfo->fields[i].array_length > 0) {
uint16_t* nums = reinterpret_cast<uint16_t*>(m + msgInfo->fields[i].wire_offset);
// Enforce null termination
if (array_length > 0) {
uint16_t nums[array_buffer_length / sizeof(uint16_t)];
memcpy(nums, m + offset, sizeof(uint16_t) * array_length);
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(nums[j]);
}
string += QString::number(nums[msgInfo->fields[i].array_length - 1]);
string += QString::number(nums[array_length - 1]);
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
uint16_t n = *(reinterpret_cast<uint16_t*>(m + msgInfo->fields[i].wire_offset));
uint16_t n;
memcpy(&n, m + offset, sizeof(uint16_t));
f->updateValue(QString::number(n), static_cast<qreal>(n));
}
break;
case MAVLINK_TYPE_INT16_T:
if (msgInfo->fields[i].array_length > 0) {
int16_t* nums = reinterpret_cast<int16_t*>(m + msgInfo->fields[i].wire_offset);
// Enforce null termination
if (array_length > 0) {
int16_t nums[array_buffer_length / sizeof(int16_t)];
memcpy(nums, m + offset, sizeof(int16_t) * array_length);
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(nums[j]);
}
string += QString::number(nums[msgInfo->fields[i].array_length - 1]);
string += QString::number(nums[array_length - 1]);
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
int16_t n = *(reinterpret_cast<int16_t*>(m + msgInfo->fields[i].wire_offset));
int16_t n;
memcpy(&n, m + offset, sizeof(int16_t));
f->updateValue(QString::number(n), static_cast<qreal>(n));
}
break;
case MAVLINK_TYPE_UINT32_T:
if (msgInfo->fields[i].array_length > 0) {
uint32_t* nums = reinterpret_cast<uint32_t*>(m + msgInfo->fields[i].wire_offset);
// Enforce null termination
if (array_length > 0) {
uint32_t nums[array_buffer_length / sizeof(uint32_t)];
memcpy(nums, m + offset, sizeof(uint32_t) * array_length);
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(nums[j]);
}
string += QString::number(nums[msgInfo->fields[i].array_length - 1]);
string += QString::number(nums[array_length - 1]);
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
uint32_t n = *(reinterpret_cast<uint32_t*>(m + msgInfo->fields[i].wire_offset));
uint32_t n;
memcpy(&n, m + offset, sizeof(uint32_t));
//-- Special case
if(_message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) {
QDateTime d = QDateTime::fromMSecsSinceEpoch(static_cast<qint64>(n),Qt::UTC,0);
@ -338,70 +352,74 @@ QGCMAVLinkMessage::update(mavlink_message_t* message) @@ -338,70 +352,74 @@ QGCMAVLinkMessage::update(mavlink_message_t* message)
}
break;
case MAVLINK_TYPE_INT32_T:
if (msgInfo->fields[i].array_length > 0) {
int32_t* nums = reinterpret_cast<int32_t*>(m + msgInfo->fields[i].wire_offset);
// Enforce null termination
if (array_length > 0) {
int32_t nums[array_buffer_length / sizeof(int32_t)];
memcpy(nums, m + offset, sizeof(int32_t) * array_length);
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(nums[j]);
}
string += QString::number(nums[msgInfo->fields[i].array_length - 1]);
string += QString::number(nums[array_length - 1]);
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
int32_t n = *(reinterpret_cast<int32_t*>(m + msgInfo->fields[i].wire_offset));
int32_t n;
memcpy(&n, m + offset, sizeof(int32_t));
f->updateValue(QString::number(n), static_cast<qreal>(n));
}
break;
case MAVLINK_TYPE_FLOAT:
if (msgInfo->fields[i].array_length > 0) {
float* nums = reinterpret_cast<float*>(m + msgInfo->fields[i].wire_offset);
// Enforce null termination
if (array_length > 0) {
float nums[array_buffer_length / sizeof(float)];
memcpy(nums, m + offset, sizeof(float) * array_length);
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(static_cast<double>(nums[j]));
}
string += QString::number(static_cast<double>(nums[msgInfo->fields[i].array_length - 1]));
string += QString::number(static_cast<double>(nums[array_length - 1]));
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
float fv = *(reinterpret_cast<float*>(m + msgInfo->fields[i].wire_offset));
float fv;
memcpy(&fv, m + offset, sizeof(float));
f->updateValue(QString::number(static_cast<double>(fv)), static_cast<qreal>(fv));
}
break;
case MAVLINK_TYPE_DOUBLE:
if (msgInfo->fields[i].array_length > 0) {
double* nums = reinterpret_cast<double*>(m + msgInfo->fields[i].wire_offset);
// Enforce null termination
if (array_length > 0) {
double nums[array_buffer_length / sizeof(double)];
memcpy(nums, m + offset, sizeof(double) * array_length);
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(nums[j]);
}
string += QString::number(static_cast<double>(nums[msgInfo->fields[i].array_length - 1]));
string += QString::number(static_cast<double>(nums[array_length - 1]));
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
double d = *(reinterpret_cast<double*>(m + msgInfo->fields[i].wire_offset));
double d;
memcpy(&d, m + offset, sizeof(double));
f->updateValue(QString::number(d), static_cast<qreal>(d));
}
break;
case MAVLINK_TYPE_UINT64_T:
if (msgInfo->fields[i].array_length > 0) {
uint64_t* nums = reinterpret_cast<uint64_t*>(m + msgInfo->fields[i].wire_offset);
// Enforce null termination
if (array_length > 0) {
uint64_t nums[array_buffer_length / sizeof(uint64_t)];
memcpy(nums, m + offset, sizeof(uint64_t) * array_length);
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(nums[j]);
}
string += QString::number(nums[msgInfo->fields[i].array_length - 1]);
string += QString::number(nums[array_length - 1]);
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
uint64_t n = *(reinterpret_cast<uint64_t*>(m + msgInfo->fields[i].wire_offset));
uint64_t n;
memcpy(&n, m + offset, sizeof(uint64_t));
//-- Special case
if(_message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) {
QDateTime d = QDateTime::fromMSecsSinceEpoch(n/1000,Qt::UTC,0);
@ -412,19 +430,20 @@ QGCMAVLinkMessage::update(mavlink_message_t* message) @@ -412,19 +430,20 @@ QGCMAVLinkMessage::update(mavlink_message_t* message)
}
break;
case MAVLINK_TYPE_INT64_T:
if (msgInfo->fields[i].array_length > 0) {
int64_t* nums = reinterpret_cast<int64_t*>(m + msgInfo->fields[i].wire_offset);
// Enforce null termination
if (array_length > 0) {
int64_t nums[array_buffer_length / sizeof(int64_t)];
memcpy(nums, m + offset, sizeof(int64_t) * array_length);
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[i].array_length - 1; ++j) {
for (unsigned int j = 0; j < array_length - 1; ++j) {
string += tmp.arg(nums[j]);
}
string += QString::number(nums[msgInfo->fields[i].array_length - 1]);
string += QString::number(nums[array_length - 1]);
f->updateValue(string, static_cast<qreal>(nums[0]));
} else {
// Single value
int64_t n = *(reinterpret_cast<int64_t*>(m + msgInfo->fields[i].wire_offset));
int64_t n;
memcpy(&n, m + offset, sizeof(int64_t));
f->updateValue(QString::number(n), static_cast<qreal>(n));
}
break;
@ -443,6 +462,12 @@ QGCMAVLinkVehicle::QGCMAVLinkVehicle(QObject* parent, quint8 id) @@ -443,6 +462,12 @@ QGCMAVLinkVehicle::QGCMAVLinkVehicle(QObject* parent, quint8 id)
}
//-----------------------------------------------------------------------------
QGCMAVLinkVehicle::~QGCMAVLinkVehicle()
{
_messages.clearAndDeleteContents();
}
//-----------------------------------------------------------------------------
QGCMAVLinkMessage*
QGCMAVLinkVehicle::findMessage(uint32_t id, uint8_t cid)
{
@ -472,11 +497,30 @@ QGCMAVLinkVehicle::findMessage(QGCMAVLinkMessage* message) @@ -472,11 +497,30 @@ QGCMAVLinkVehicle::findMessage(QGCMAVLinkMessage* message)
//-----------------------------------------------------------------------------
void
QGCMAVLinkVehicle::_resetSelection()
{
for(int i = 0; i < _messages.count(); i++) {
QGCMAVLinkMessage* m = qobject_cast<QGCMAVLinkMessage*>(_messages.get(i));
if(m && m->selected()) {
m->setSelected(false);
emit m->selectedChanged();
}
}
}
//-----------------------------------------------------------------------------
void
QGCMAVLinkVehicle::setSelected(int sel)
{
if(sel < _messages.count()) {
_selected = sel;
emit selectedChanged();
_resetSelection();
QGCMAVLinkMessage* m = qobject_cast<QGCMAVLinkMessage*>(_messages.get(sel));
if(m && !m->selected()) {
m->setSelected(true);
emit m->selectedChanged();
}
}
}
@ -499,6 +543,9 @@ QGCMAVLinkVehicle::append(QGCMAVLinkMessage* message) @@ -499,6 +543,9 @@ QGCMAVLinkVehicle::append(QGCMAVLinkMessage* message)
QGCMAVLinkMessage* selectedMsg = nullptr;
if(_messages.count()) {
selectedMsg = qobject_cast<QGCMAVLinkMessage*>(_messages.get(_selected));
} else {
//-- First message
message->setSelected(true);
}
_messages.append(message);
//-- Sort messages by id and then cid
@ -700,6 +747,8 @@ MAVLinkInspectorController::MAVLinkInspectorController() @@ -700,6 +747,8 @@ MAVLinkInspectorController::MAVLinkInspectorController()
//-----------------------------------------------------------------------------
MAVLinkInspectorController::~MAVLinkInspectorController()
{
_charts.clearAndDeleteContents();
_vehicles.clearAndDeleteContents();
}
//----------------------------------------------------------------------------------------

12
src/AnalyzeView/MAVLinkInspectorController.h
Unescape View File

@ -91,8 +91,10 @@ public: @@ -91,8 +91,10 @@ public:
Q_PROPERTY(quint64 count READ count NOTIFY messageChanged)
Q_PROPERTY(QmlObjectListModel* fields READ fields NOTIFY indexChanged)
Q_PROPERTY(bool fieldSelected READ fieldSelected NOTIFY fieldSelectedChanged)
Q_PROPERTY(bool selected READ selected NOTIFY selectedChanged)
QGCMAVLinkMessage(QObject* parent, mavlink_message_t* message);
QGCMAVLinkMessage (QObject* parent, mavlink_message_t* message);
~QGCMAVLinkMessage ();
quint32 id () { return _message.msgid; }
quint8 cid () { return _message.compid; }
@ -102,16 +104,19 @@ public: @@ -102,16 +104,19 @@ public:
quint64 lastCount () { return _lastCount; }
QmlObjectListModel* fields () { return &_fields; }
bool fieldSelected () { return _fieldSelected; }
bool selected () { return _selected; }
void updateFieldSelection();
void update (mavlink_message_t* message);
void updateFreq ();
void setSelected (bool sel) { _selected = sel; }
signals:
void messageChanged ();
void freqChanged ();
void indexChanged ();
void fieldSelectedChanged ();
void selectedChanged ();
private:
QmlObjectListModel _fields;
@ -121,6 +126,7 @@ private: @@ -121,6 +126,7 @@ private:
uint64_t _lastCount = 0;
mavlink_message_t _message; //-- List of QGCMAVLinkMessageField
bool _fieldSelected = false;
bool _selected = false;
};
//-----------------------------------------------------------------------------
@ -134,7 +140,8 @@ public: @@ -134,7 +140,8 @@ public:
Q_PROPERTY(int selected READ selected WRITE setSelected NOTIFY selectedChanged)
QGCMAVLinkVehicle(QObject* parent, quint8 id);
QGCMAVLinkVehicle (QObject* parent, quint8 id);
~QGCMAVLinkVehicle ();
quint8 id () { return _id; }
QmlObjectListModel* messages () { return &_messages; }
@ -154,6 +161,7 @@ signals: @@ -154,6 +161,7 @@ signals:
private:
void _checkCompID (QGCMAVLinkMessage *message);
void _resetSelection ();
private:
quint8 _id;

Write Preview
Loading…
Cancel
Save