QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
MAVLinkMessage.cc
Go to the documentation of this file.
1#include "MAVLinkMessage.h"
2#include "MAVLinkMessageField.h"
3#include "QGCLoggingCategory.h"
4#include "QmlObjectListModel.h"
5
6#include <QtCore/QTimeZone>
7
8QGC_LOGGING_CATEGORY(MAVLinkMessageLog, "AnalyzeView.MAVLinkMessage")
9
10QGCMAVLinkMessage::QGCMAVLinkMessage(const mavlink_message_t &message, QObject *parent)
11 : QObject(parent)
12 , _message(message)
13 , _fields(new QmlObjectListModel(this))
14
15{
16 qCDebug(MAVLinkMessageLog) << this;
17
18 const mavlink_message_info_t *const msgInfo = mavlink_get_message_info(&message);
19 if (!msgInfo) {
20 qCWarning(MAVLinkMessageLog) << QStringLiteral("QGCMAVLinkMessage NULL msgInfo msgid(%1)").arg(message.msgid);
21 return;
22 }
23
24 _name = QString(msgInfo->name);
25 qCDebug(MAVLinkMessageLog) << "New Message:" << _name;
26
27 for (unsigned int i = 0; i < msgInfo->num_fields; ++i) {
28 QString type = QStringLiteral("?");
29 switch (msgInfo->fields[i].type) {
30 case MAVLINK_TYPE_CHAR: type = QString("char"); break;
31 case MAVLINK_TYPE_UINT8_T: type = QString("uint8_t"); break;
32 case MAVLINK_TYPE_INT8_T: type = QString("int8_t"); break;
33 case MAVLINK_TYPE_UINT16_T: type = QString("uint16_t"); break;
34 case MAVLINK_TYPE_INT16_T: type = QString("int16_t"); break;
35 case MAVLINK_TYPE_UINT32_T: type = QString("uint32_t"); break;
36 case MAVLINK_TYPE_INT32_T: type = QString("int32_t"); break;
37 case MAVLINK_TYPE_FLOAT: type = QString("float"); break;
38 case MAVLINK_TYPE_DOUBLE: type = QString("double"); break;
39 case MAVLINK_TYPE_UINT64_T: type = QString("uint64_t"); break;
40 case MAVLINK_TYPE_INT64_T: type = QString("int64_t"); break;
41 }
42
43 QGCMAVLinkMessageField *const field = new QGCMAVLinkMessageField(msgInfo->fields[i].name, type, this);
44 _fields->append(field);
45 }
46}
47
48QGCMAVLinkMessage::~QGCMAVLinkMessage()
49{
50 _fields->clearAndDeleteContents();
51
52 qCDebug(MAVLinkMessageLog) << this;
53}
54
55void QGCMAVLinkMessage::updateFieldSelection()
56{
57 bool sel = false;
58
59 for (int i = 0; i < _fields->count(); ++i) {
60 const QGCMAVLinkMessageField *const field = qobject_cast<const QGCMAVLinkMessageField*>(_fields->get(i));
61 if (field && field->selected()) {
62 sel = true;
63 break;
64 }
65 }
66
67 if (sel != _fieldSelected) {
68 _fieldSelected = sel;
69 emit fieldSelectedChanged();
70 }
71}
72
73void QGCMAVLinkMessage::updateFreq()
74{
75 const quint64 msgCount = _count - _lastCount;
76 const qreal lastRateHz = _actualRateHz;
77 _actualRateHz = (0.2 * _actualRateHz) + (0.8 * msgCount);
78 _lastCount = _count;
79 if (_actualRateHz != lastRateHz) {
80 emit actualRateHzChanged();
81 }
82}
83
84void QGCMAVLinkMessage::setSelected(bool sel)
85{
86 if (sel != _selected) {
87 _selected = sel;
88 _updateFields();
89 emit selectedChanged();
90 }
91}
92
93void QGCMAVLinkMessage::setTargetRateHz(int32_t rate)
94{
95 if (rate != _targetRateHz) {
96 _targetRateHz = rate;
97 emit targetRateHzChanged();
98 }
99}
100
101void QGCMAVLinkMessage::update(const mavlink_message_t &message)
102{
103 _count++;
104 _message = message;
105
106 if (_selected || _fieldSelected) {
107 // Don't update field info unless selected to reduce perf hit of message processing
108 _updateFields();
109 }
110 emit countChanged();
111}
112
113void QGCMAVLinkMessage::_updateFields()
114{
115 const mavlink_message_info_t *msgInfo = mavlink_get_message_info(&_message);
116 if (!msgInfo) {
117 qCWarning(MAVLinkMessageLog) << "QGCMAVLinkMessage::update NULL msgInfo msgid" << _message.msgid;
118 return;
119 }
120
121 if (_fields->count() != static_cast<int>(msgInfo->num_fields)) {
122 qCWarning(MAVLinkMessageLog) << "QGCMAVLinkMessage::update msgInfo field count mismatch msgid" << _message.msgid;
123 return;
124 }
125
126 uint8_t *const msg = reinterpret_cast<uint8_t*>(&_message.payload64[0]);
127 for (unsigned int i = 0; i < msgInfo->num_fields; ++i) {
128 QGCMAVLinkMessageField *const field = qobject_cast<QGCMAVLinkMessageField*>(_fields->get(static_cast<int>(i)));
129 if (!field) {
130 continue;
131 }
132
133 const unsigned int offset = msgInfo->fields[i].wire_offset;
134 const unsigned int array_length = msgInfo->fields[i].array_length;
135 static const unsigned int array_buffer_length = (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_CHECKSUM_BYTES + 7);
136
137 switch (msgInfo->fields[i].type) {
138 case MAVLINK_TYPE_CHAR:
139 field->setSelectable(false);
140 if (array_length > 0) {
141 char *const str = reinterpret_cast<char*>(msg + offset);
142 str[array_length - 1] = '\0';
143 const QString v(str);
144 field->updateValue(v, 0);
145 } else {
146 char b = *(reinterpret_cast<char*>(msg + offset));
147 const QString v(b);
148 field->updateValue(v, 0);
149 }
150 break;
151 case MAVLINK_TYPE_UINT8_T:
152 if (array_length > 0) {
153 const uint8_t *const nums = msg + offset;
154 const QString tmp("%1, ");
155 QString string;
156 for (unsigned int j = 0; j < array_length - 1; ++j) {
157 string += tmp.arg(nums[j]);
158 }
159 string += QString::number(nums[array_length - 1]);
160 field->updateValue(string, static_cast<qreal>(nums[0]));
161 } else {
162 const uint8_t u = *(msg + offset);
163 field->updateValue(QString::number(u), static_cast<qreal>(u));
164 }
165 break;
166 case MAVLINK_TYPE_INT8_T:
167 if (array_length > 0) {
168 const int8_t *const nums = reinterpret_cast<int8_t*>(msg + offset);
169 const QString tmp("%1, ");
170 QString string;
171 for (unsigned int j = 0; j < array_length - 1; ++j) {
172 string += tmp.arg(nums[j]);
173 }
174 string += QString::number(nums[array_length - 1]);
175 field->updateValue(string, static_cast<qreal>(nums[0]));
176 } else {
177 const int8_t n = *(reinterpret_cast<int8_t*>(msg + offset));
178 field->updateValue(QString::number(n), static_cast<qreal>(n));
179 }
180 break;
181 case MAVLINK_TYPE_UINT16_T:
182 if (array_length > 0) {
183 uint16_t nums[array_buffer_length / sizeof(uint16_t)]{};
184 (void) memcpy(nums, msg + offset, sizeof(uint16_t) * array_length);
185 const QString tmp("%1, ");
186 QString string;
187 for (unsigned int j = 0; j < array_length - 1; ++j) {
188 string += tmp.arg(nums[j]);
189 }
190 string += QString::number(nums[array_length - 1]);
191 field->updateValue(string, static_cast<qreal>(nums[0]));
192 } else {
193 uint16_t n = 0;
194 (void) memcpy(&n, msg + offset, sizeof(uint16_t));
195 field->updateValue(QString::number(n), static_cast<qreal>(n));
196 }
197 break;
198 case MAVLINK_TYPE_INT16_T:
199 if (array_length > 0) {
200 int16_t nums[array_buffer_length / sizeof(int16_t)]{};
201 (void) memcpy(nums, msg + offset, sizeof(int16_t) * array_length);
202 const QString tmp("%1, ");
203 QString string;
204 for (unsigned int j = 0; j < array_length - 1; ++j) {
205 string += tmp.arg(nums[j]);
206 }
207 string += QString::number(nums[array_length - 1]);
208 field->updateValue(string, static_cast<qreal>(nums[0]));
209 } else {
210 int16_t n;
211 memcpy(&n, msg + offset, sizeof(int16_t));
212 field->updateValue(QString::number(n), static_cast<qreal>(n));
213 }
214 break;
215 case MAVLINK_TYPE_UINT32_T:
216 if (array_length > 0) {
217 uint32_t nums[array_buffer_length / sizeof(uint32_t)]{};
218 (void) memcpy(nums, msg + offset, sizeof(uint32_t) * array_length);
219 const QString tmp("%1, ");
220 QString string;
221 for (unsigned int j = 0; j < array_length - 1; ++j) {
222 string += tmp.arg(nums[j]);
223 }
224 string += QString::number(nums[array_length - 1]);
225 field->updateValue(string, static_cast<qreal>(nums[0]));
226 } else {
227 uint32_t n;
228 (void) memcpy(&n, msg + offset, sizeof(uint32_t));
229 if (_message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) {
230 const QDateTime d = QDateTime::fromMSecsSinceEpoch(static_cast<qint64>(n), QTimeZone::utc());
231 field->updateValue(d.toString("HH:mm:ss"), static_cast<qreal>(n));
232 } else {
233 field->updateValue(QString::number(n), static_cast<qreal>(n));
234 }
235 }
236 break;
237 case MAVLINK_TYPE_INT32_T:
238 if (array_length > 0) {
239 int32_t nums[array_buffer_length / sizeof(int32_t)]{};
240 (void) memcpy(nums, msg + offset, sizeof(int32_t) * array_length);
241 const QString tmp("%1, ");
242 QString string;
243 for (unsigned int j = 0; j < array_length - 1; ++j) {
244 string += tmp.arg(nums[j]);
245 }
246 string += QString::number(nums[array_length - 1]);
247 field->updateValue(string, static_cast<qreal>(nums[0]));
248 } else {
249 int32_t n;
250 (void) memcpy(&n, msg + offset, sizeof(int32_t));
251 field->updateValue(QString::number(n), static_cast<qreal>(n));
252 }
253 break;
254 case MAVLINK_TYPE_FLOAT:
255 if (array_length > 0) {
256 float nums[array_buffer_length / sizeof(float)]{};
257 (void) memcpy(nums, msg + offset, sizeof(float) * array_length);
258 const QString tmp("%1, ");
259 QString string;
260 for (unsigned int j = 0; j < array_length - 1; ++j) {
261 string += tmp.arg(static_cast<double>(nums[j]));
262 }
263 string += QString::number(static_cast<double>(nums[array_length - 1]));
264 field->updateValue(string, static_cast<qreal>(nums[0]));
265 } else {
266 float fv;
267 (void) memcpy(&fv, msg + offset, sizeof(float));
268 field->updateValue(QString::number(static_cast<double>(fv)), static_cast<qreal>(fv));
269 }
270 break;
271 case MAVLINK_TYPE_DOUBLE:
272 if (array_length > 0) {
273 double nums[array_buffer_length / sizeof(double)]{};
274 (void) memcpy(nums, msg + offset, sizeof(double) * array_length);
275 const QString tmp("%1, ");
276 QString string;
277 for (unsigned int j = 0; j < array_length - 1; ++j) {
278 string += tmp.arg(nums[j]);
279 }
280 string += QString::number(static_cast<double>(nums[array_length - 1]));
281 field->updateValue(string, static_cast<qreal>(nums[0]));
282 } else {
283 double d;
284 (void) memcpy(&d, msg + offset, sizeof(double));
285 field->updateValue(QString::number(d), static_cast<qreal>(d));
286 }
287 break;
288 case MAVLINK_TYPE_UINT64_T:
289 if (array_length > 0) {
290 uint64_t nums[array_buffer_length / sizeof(uint64_t)]{};
291 (void) memcpy(nums, msg + offset, sizeof(uint64_t) * array_length);
292 const QString tmp("%1, ");
293 QString string;
294 for (unsigned int j = 0; j < array_length - 1; ++j) {
295 string += tmp.arg(nums[j]);
296 }
297 string += QString::number(nums[array_length - 1]);
298 field->updateValue(string, static_cast<qreal>(nums[0]));
299 } else {
300 uint64_t n;
301 (void) memcpy(&n, msg + offset, sizeof(uint64_t));
302 if(_message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) {
303 const QDateTime d = QDateTime::fromMSecsSinceEpoch(n / 1000, QTimeZone::utc());
304 field->updateValue(d.toString("yyyy MM dd HH:mm:ss"), static_cast<qreal>(n));
305 } else {
306 field->updateValue(QString::number(n), static_cast<qreal>(n));
307 }
308 }
309 break;
310 case MAVLINK_TYPE_INT64_T:
311 if (array_length > 0) {
312 int64_t nums[array_buffer_length / sizeof(int64_t)]{};
313 (void) memcpy(nums, msg + offset, sizeof(int64_t) * array_length);
314 const QString tmp("%1, ");
315 QString string;
316 for (unsigned int j = 0; j < array_length - 1; ++j) {
317 string += tmp.arg(nums[j]);
318 }
319 string += QString::number(nums[array_length - 1]);
320 field->updateValue(string, static_cast<qreal>(nums[0]));
321 } else {
322 int64_t n;
323 (void) memcpy(&n, msg + offset, sizeof(int64_t));
324 field->updateValue(QString::number(n), static_cast<qreal>(n));
325 }
326 break;
327 default:
328 break;
329 }
330 }
331}
mavlink_message_t
struct __mavlink_message mavlink_message_t
Definition QGCCorePlugin.h:23
QGC_LOGGING_CATEGORY
#define QGC_LOGGING_CATEGORY(name, categoryStr)
Definition QGCLoggingCategory.h:14
QGCMAVLinkMessage::targetRateHzChanged
void targetRateHzChanged()
QGCMAVLinkMessage::actualRateHzChanged
void actualRateHzChanged()
QGCMAVLinkMessage::fieldSelectedChanged
void fieldSelectedChanged()
QmlObjectListModel::get
QObject * get(int index)
Definition QmlObjectListModel.cc:59
QmlObjectListModel::count
int count() const override final
Definition QmlObjectListModel.cc:252
QmlObjectListModel::clearAndDeleteContents
void clearAndDeleteContents() override final
Clears the list and calls deleteLater on each entry.
Definition QmlObjectListModel.cc:273