MAVLinkDecoder.cc 19 KB
Newer Older
Don Gagne's avatar
Don Gagne committed
1
#include "QGCMAVLink.h"
lm's avatar
lm committed
2
#include "MAVLinkDecoder.h"
Don Gagne's avatar
Don Gagne committed
3 4

#include <QDebug>
lm's avatar
lm committed
5

6 7
MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol) :
    QThread(), creationThread(QThread::currentThread())
lm's avatar
lm committed
8
{
9 10 11 12
    // We're doing it wrong - because the Qt folks got the API wrong:
    // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
    moveToThread(this);

13
    // Fill filter
14 15 16
    // Allow system status
//    messageFilter.insert(MAVLINK_MSG_ID_HEARTBEAT, false);
//    messageFilter.insert(MAVLINK_MSG_ID_SYS_STATUS, false);
17
    messageFilter.insert(MAVLINK_MSG_ID_STATUSTEXT, false);
18
    messageFilter.insert(MAVLINK_MSG_ID_COMMAND_LONG, false);
19 20 21 22 23 24
    messageFilter.insert(MAVLINK_MSG_ID_COMMAND_ACK, false);
    messageFilter.insert(MAVLINK_MSG_ID_PARAM_SET, false);
    messageFilter.insert(MAVLINK_MSG_ID_PARAM_VALUE, false);
    messageFilter.insert(MAVLINK_MSG_ID_MISSION_ITEM, false);
    messageFilter.insert(MAVLINK_MSG_ID_MISSION_COUNT, false);
    messageFilter.insert(MAVLINK_MSG_ID_MISSION_ACK, false);
25
    messageFilter.insert(MAVLINK_MSG_ID_DATA_STREAM, false);
26
    messageFilter.insert(MAVLINK_MSG_ID_GPS_STATUS, false);
27
    messageFilter.insert(MAVLINK_MSG_ID_RC_CHANNELS_RAW, false);
28
    messageFilter.insert(MAVLINK_MSG_ID_LOG_DATA, false);
29
    #ifdef MAVLINK_MSG_ID_ENCAPSULATED_DATA
30
    messageFilter.insert(MAVLINK_MSG_ID_ENCAPSULATED_DATA, false);
31 32
    #endif
    #ifdef MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE
33
    messageFilter.insert(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, false);
34
    #endif
35
    messageFilter.insert(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, false);
36

37 38
    textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG, false);
    textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG_VECT, false);
39
    textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, false);
40 41
    textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, false);
    textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_INT, false);
42
//    textMessageFilter.insert(MAVLINK_MSG_ID_HIGHRES_IMU, false);
43

44
    connect(protocol, &MAVLinkProtocol::messageReceived, this, &MAVLinkDecoder::receiveMessage);
45
    connect(this, &MAVLinkDecoder::finish, this, &QThread::quit);
46

47
    start(LowestPriority);
48 49 50 51 52 53 54 55 56
}

/**
 * @brief Runs the thread
 *
 **/
void MAVLinkDecoder::run()
{
    exec();
57
    moveToThread(creationThread);
lm's avatar
lm committed
58 59 60 61 62 63
}

void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t message)
{
    Q_UNUSED(link);

64
    uint32_t msgid = message.msgid;
Don Gagne's avatar
Don Gagne committed
65
    const mavlink_message_info_t* msgInfo = mavlink_get_message_info(&message);
66 67 68 69 70 71
    if(!msgInfo) {
        qWarning() << "Invalid MAVLink message received. ID:" << msgid;
        return;
    }

    msgDict[message.msgid] = message;
lm's avatar
lm committed
72

73 74
    // Store an arrival time for this message. This value ends up being calculated later.
    quint64 time = 0;
75

76
    // The SYSTEM_TIME message is special, in that it's handled here for synchronizing the QGC time with the remote time.
77
    if (message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME)
lm's avatar
lm committed
78
    {
pixhawk's avatar
pixhawk committed
79 80
        mavlink_system_time_t timebase;
        mavlink_msg_system_time_decode(&message, &timebase);
81 82
        sysDict[msgid].onboardTimeOffset = (timebase.time_unix_usec+500)/1000 - timebase.time_boot_ms;
        sysDict[msgid].onboardToGCSUnixTimeOffsetAndDelay  = static_cast<qint64>(QGC::groundTimeMilliseconds() - (timebase.time_unix_usec+500)/1000);
lm's avatar
lm committed
83 84 85 86
    }
    else
    {

87
        // See if first value is a time value and if it is, use that as the arrival time for this data.
pixhawk's avatar
pixhawk committed
88
        uint8_t fieldid = 0;
89
        uint8_t* m = (uint8_t*)(msgDict[msgid].payload64);
90

Don Gagne's avatar
Don Gagne committed
91
        if (QString(msgInfo->fields[fieldid].name) == QString("time_boot_ms") && msgInfo->fields[fieldid].type == MAVLINK_TYPE_UINT32_T)
pixhawk's avatar
pixhawk committed
92
        {
Don Gagne's avatar
Don Gagne committed
93
            time = *((quint32*)(m+msgInfo->fields[fieldid].wire_offset));
pixhawk's avatar
pixhawk committed
94
        }
Don Gagne's avatar
Don Gagne committed
95
        else if (QString(msgInfo->fields[fieldid].name).contains("usec") && msgInfo->fields[fieldid].type == MAVLINK_TYPE_UINT64_T)
pixhawk's avatar
pixhawk committed
96
        {
Don Gagne's avatar
Don Gagne committed
97
            time = *((quint64*)(m+msgInfo->fields[fieldid].wire_offset));
98
            time = (time+500)/1000; // Scale to milliseconds, round up/down correctly
pixhawk's avatar
pixhawk committed
99
        }
100
    }
pixhawk's avatar
pixhawk committed
101

102 103
    // Align UAS time to global time
    time = getUnixTimeFromMs(message.sysid, time);
pixhawk's avatar
pixhawk committed
104

105
    // Send out all field values for this message
Don Gagne's avatar
Don Gagne committed
106
    for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
107 108
    {
        emitFieldValue(&message, i, time);
lm's avatar
lm committed
109 110 111 112 113 114
    }

    // Send out combined math expressions
    // FIXME XXX TODO
}

pixhawk's avatar
pixhawk committed
115
quint64 MAVLinkDecoder::getUnixTimeFromMs(int systemID, quint64 time)
116
{
pixhawk's avatar
pixhawk committed
117 118 119
    quint64 ret = 0;
    if (time == 0)
    {
120
        ret = QGC::groundTimeMilliseconds() - sysDict[systemID].onboardToGCSUnixTimeOffsetAndDelay;
pixhawk's avatar
pixhawk committed
121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143
    }
    // Check if time is smaller than 40 years,
    // assuming no system without Unix timestamp
    // runs longer than 40 years continuously without
    // reboot. In worst case this will add/subtract the
    // communication delay between GCS and MAV,
    // it will never alter the timestamp in a safety
    // critical way.
    //
    // Calculation:
    // 40 years
    // 365 days
    // 24 hours
    // 60 minutes
    // 60 seconds
    // 1000 milliseconds
    // 1000 microseconds
#ifndef _MSC_VER
    else if (time < 1261440000000LLU)
#else
    else if (time < 1261440000000)
#endif
    {
144
        if (sysDict[systemID].onboardTimeOffset == 0 || time < (sysDict[systemID].firstOnboardTime -100))
pixhawk's avatar
pixhawk committed
145
        {
146 147
            sysDict[systemID].firstOnboardTime = time;
            sysDict[systemID].onboardTimeOffset = QGC::groundTimeMilliseconds() - time;
pixhawk's avatar
pixhawk committed
148
        }
149

150
        if (time > sysDict[systemID].firstOnboardTime ) sysDict[systemID].firstOnboardTime = time;
151

152
        ret = time + sysDict[systemID].onboardTimeOffset;
pixhawk's avatar
pixhawk committed
153 154 155 156 157 158 159 160
    }
    else
    {
        // Time is not zero and larger than 40 years -> has to be
        // a Unix epoch timestamp. Do nothing.
        ret = time;
    }

161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189

//    // Check if the offset estimation likely went wrong
//    // and we're talking to a new instance / the system
//    // has rebooted. Only reset if this is consistent.
//    if (!isNull && lastNonNullTime > ret)
//    {
//        onboardTimeOffsetInvalidCount++;
//    }
//    else if (!isNull && lastNonNullTime < ret)
//    {
//        onboardTimeOffsetInvalidCount = 0;
//    }

//    // Reset onboard time offset estimation, since it seems to be really off
//    if (onboardTimeOffsetInvalidCount > 20)
//    {
//        onboardTimeOffset = 0;
//        onboardTimeOffsetInvalidCount = 0;
//        lastNonNullTime = 0;
//        qDebug() << "RESETTET ONBOARD TIME OFFSET";
//    }

//    // If we're progressing in time, set it
//    // else wait for the reboot detection to
//    // catch the timestamp wrap / reset
//    if (!isNull && (lastNonNullTime < ret)) {
//        lastNonNullTime = ret;
//    }

pixhawk's avatar
pixhawk committed
190 191 192
    return ret;
}

lm's avatar
lm committed
193 194
void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time)
{
195
    bool multiComponentSourceDetected = false;
Don Gagne's avatar
Don Gagne committed
196
    const mavlink_message_info_t* msgInfo = mavlink_get_message_info(msg);
197

198 199 200 201 202 203 204
    uint32_t msgid = msg->msgid;

    // create new system data if it wasn't dectected yet
    if (!(sysDict.keys().contains(msgid))) {
        sysDict[msgid] = SystemData();
    }

205
    // Store component ID
206
    if (sysDict[msgid].componentID == -1)
207
    {
208
        sysDict[msgid].componentID = msg->compid;
209 210 211 212
    }
    else
    {
        // Got this message already
213
        if (sysDict[msgid].componentID != msg->compid)
214
        {
215
            sysDict[msgid].componentMulti = true;
216 217 218
        }
    }

219 220 221
    if (sysDict[msgid].componentMulti == true) {
        multiComponentSourceDetected = true;
    }
222

lm's avatar
lm committed
223
    // Add field tree widget item
224
    if (messageFilter.contains(msgid)) return;
Don Gagne's avatar
Don Gagne committed
225
    QString fieldName(msgInfo->fields[fieldid].name);
lm's avatar
lm committed
226
    QString fieldType;
227
    uint8_t* m = (uint8_t*)(msgDict[msgid].payload64);
lm's avatar
lm committed
228 229
    QString name("%1.%2");
    QString unit("");
pixhawk's avatar
pixhawk committed
230 231 232 233 234 235

    // Debug vector messages
    if (msgid == MAVLINK_MSG_ID_DEBUG_VECT)
    {
        mavlink_debug_vect_t debug;
        mavlink_msg_debug_vect_decode(msg, &debug);
236 237 238
        char buf[11];
        strncpy(buf, debug.name, 10);
        buf[10] = '\0';
239
        name = QString("%1.%2").arg(buf, fieldName);
240
        time = getUnixTimeFromMs(msg->sysid, (debug.time_usec+500)/1000); // Scale to milliseconds, round up/down correctly
pixhawk's avatar
pixhawk committed
241
    }
242 243 244 245 246 247 248
    else if (msgid == MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY)
    {
        mavlink_debug_float_array_t debug;
        mavlink_msg_debug_float_array_decode(msg, &debug);
        char buf[11];
        strncpy(buf, debug.name, 10);
        buf[10] = '\0';
249
        name = QString("%1.%2").arg(buf, fieldName);
250 251
        time = getUnixTimeFromMs(msg->sysid, (debug.time_usec+500)/1000); // Scale to milliseconds, round up/down correctly
    }
252 253 254 255 256
    else if (msgid == MAVLINK_MSG_ID_DEBUG)
    {
        mavlink_debug_t debug;
        mavlink_msg_debug_decode(msg, &debug);
        name = name.arg(QString("debug")).arg(debug.ind);
257
        time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms);
258 259 260 261 262
    }
    else if (msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT)
    {
        mavlink_named_value_float_t debug;
        mavlink_msg_named_value_float_decode(msg, &debug);
263 264 265 266
        char buf[11];
        strncpy(buf, debug.name, 10);
        buf[10] = '\0';
        name = QString(buf);
267
        time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms);
268 269 270 271 272
    }
    else if (msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT)
    {
        mavlink_named_value_int_t debug;
        mavlink_msg_named_value_int_decode(msg, &debug);
273 274 275 276
        char buf[11];
        strncpy(buf, debug.name, 10);
        buf[10] = '\0';
        name = QString(buf);
277
        time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms);
278
    }
279 280 281 282 283
    else if (msgid == MAVLINK_MSG_ID_RC_CHANNELS_RAW)
    {
        // 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);
284
        name = name.arg(msgInfo->name, fieldName);
285 286 287 288 289 290 291
        name.prepend(QString("port%1_").arg(raw.port));
    }
    else if (msgid == MAVLINK_MSG_ID_RC_CHANNELS_SCALED)
    {
        // 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);
292
        name = name.arg(msgInfo->name, fieldName);
293 294 295 296 297 298
        name.prepend(QString("port%1_").arg(scaled.port));
    }
    else if (msgid == MAVLINK_MSG_ID_SERVO_OUTPUT_RAW)
    {
        // XXX this is really ugly, but we do not know a better way to do this
        mavlink_servo_output_raw_t servo;
299
        mavlink_msg_servo_output_raw_decode(msg, &servo);
300
        name = name.arg(msgInfo->name, fieldName);
301 302
        name.prepend(QString("port%1_").arg(servo.port));
    }
pixhawk's avatar
pixhawk committed
303 304
    else
    {
305
        name = name.arg(msgInfo->name, fieldName);
pixhawk's avatar
pixhawk committed
306 307
    }

308 309
    if (multiComponentSourceDetected)
    {
310
        name = name.prepend(QString("C%1:").arg(msg->compid));
311 312
    }

313
    name = name.prepend(QString("M%1:").arg(msg->sysid));
314

Don Gagne's avatar
Don Gagne committed
315
    switch (msgInfo->fields[fieldid].type)
lm's avatar
lm committed
316 317
    {
    case MAVLINK_TYPE_CHAR:
Don Gagne's avatar
Don Gagne committed
318
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
319
        {
Don Gagne's avatar
Don Gagne committed
320
            char* str = (char*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
321
            // Enforce null termination
Don Gagne's avatar
Don Gagne committed
322
            str[msgInfo->fields[fieldid].array_length-1] = '\0';
lm's avatar
lm committed
323
            QString string(name + ": " + str);
324
            if (!textMessageFilter.contains(msgid)) emit textMessageReceived(msg->sysid, msg->compid, MAV_SEVERITY_INFO, string);
lm's avatar
lm committed
325 326 327 328
        }
        else
        {
            // Single char
Don Gagne's avatar
Don Gagne committed
329 330
            char b = *((char*)(m+msgInfo->fields[fieldid].wire_offset));
            unit = QString("char[%1]").arg(msgInfo->fields[fieldid].array_length);
lm's avatar
lm committed
331 332 333 334
            emit valueChanged(msg->sysid, name, unit, b, time);
        }
        break;
    case MAVLINK_TYPE_UINT8_T:
Don Gagne's avatar
Don Gagne committed
335
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
336
        {
Don Gagne's avatar
Don Gagne committed
337 338 339
            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)
lm's avatar
lm committed
340 341 342 343 344 345 346
            {
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
347
            uint8_t u = *(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
348 349 350 351 352
            fieldType = "uint8_t";
            emit valueChanged(msg->sysid, name, fieldType, u, time);
        }
        break;
    case MAVLINK_TYPE_INT8_T:
Don Gagne's avatar
Don Gagne committed
353
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
354
        {
Don Gagne's avatar
Don Gagne committed
355 356 357
            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)
lm's avatar
lm committed
358 359 360 361 362 363 364
            {
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
365
            int8_t n = *((int8_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
366 367 368 369 370
            fieldType = "int8_t";
            emit valueChanged(msg->sysid, name, fieldType, n, time);
        }
        break;
    case MAVLINK_TYPE_UINT16_T:
Don Gagne's avatar
Don Gagne committed
371
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
372
        {
Don Gagne's avatar
Don Gagne committed
373 374 375
            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)
lm's avatar
lm committed
376 377 378 379 380 381 382
            {
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
383
            uint16_t n = *((uint16_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
384 385 386 387 388
            fieldType = "uint16_t";
            emit valueChanged(msg->sysid, name, fieldType, n, time);
        }
        break;
    case MAVLINK_TYPE_INT16_T:
Don Gagne's avatar
Don Gagne committed
389
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
390
        {
Don Gagne's avatar
Don Gagne committed
391 392 393
            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)
lm's avatar
lm committed
394 395 396 397 398 399 400
            {
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
401
            int16_t n = *((int16_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
402 403 404 405 406
            fieldType = "int16_t";
            emit valueChanged(msg->sysid, name, fieldType, n, time);
        }
        break;
    case MAVLINK_TYPE_UINT32_T:
Don Gagne's avatar
Don Gagne committed
407
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
408
        {
Don Gagne's avatar
Don Gagne committed
409 410 411
            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)
lm's avatar
lm committed
412 413 414 415 416 417 418
            {
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
419
            uint32_t n = *((uint32_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
420 421 422 423 424
            fieldType = "uint32_t";
            emit valueChanged(msg->sysid, name, fieldType, n, time);
        }
        break;
    case MAVLINK_TYPE_INT32_T:
Don Gagne's avatar
Don Gagne committed
425
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
426
        {
Don Gagne's avatar
Don Gagne committed
427 428 429
            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)
lm's avatar
lm committed
430 431 432 433 434 435 436
            {
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
437
            int32_t n = *((int32_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
438 439 440 441 442
            fieldType = "int32_t";
            emit valueChanged(msg->sysid, name, fieldType, n, time);
        }
        break;
    case MAVLINK_TYPE_FLOAT:
Don Gagne's avatar
Don Gagne committed
443
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
444
        {
Don Gagne's avatar
Don Gagne committed
445 446 447
            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)
lm's avatar
lm committed
448
            {
449
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, (float)(nums[j]), time);
lm's avatar
lm committed
450 451 452 453 454
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
455
            float f = *((float*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
456 457 458 459 460
            fieldType = "float";
            emit valueChanged(msg->sysid, name, fieldType, f, time);
        }
        break;
    case MAVLINK_TYPE_DOUBLE:
Don Gagne's avatar
Don Gagne committed
461
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
462
        {
Don Gagne's avatar
Don Gagne committed
463 464 465
            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)
lm's avatar
lm committed
466 467 468 469 470 471 472
            {
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, nums[j], time);
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
473
            double f = *((double*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
474 475 476 477 478
            fieldType = "double";
            emit valueChanged(msg->sysid, name, fieldType, f, time);
        }
        break;
    case MAVLINK_TYPE_UINT64_T:
Don Gagne's avatar
Don Gagne committed
479
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
480
        {
Don Gagne's avatar
Don Gagne committed
481 482 483
            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)
lm's avatar
lm committed
484
            {
pixhawk's avatar
pixhawk committed
485
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, (quint64) nums[j], time);
lm's avatar
lm committed
486 487 488 489 490
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
491
            uint64_t n = *((uint64_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
492
            fieldType = "uint64_t";
pixhawk's avatar
pixhawk committed
493
            emit valueChanged(msg->sysid, name, fieldType, (quint64) n, time);
lm's avatar
lm committed
494 495 496
        }
        break;
    case MAVLINK_TYPE_INT64_T:
Don Gagne's avatar
Don Gagne committed
497
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
498
        {
Don Gagne's avatar
Don Gagne committed
499 500 501
            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)
lm's avatar
lm committed
502
            {
pixhawk's avatar
pixhawk committed
503
                emit valueChanged(msg->sysid, QString("%1.%2").arg(name).arg(j), fieldType, (qint64) nums[j], time);
lm's avatar
lm committed
504 505 506 507 508
            }
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
509
            int64_t n = *((int64_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
510
            fieldType = "int64_t";
pixhawk's avatar
pixhawk committed
511
            emit valueChanged(msg->sysid, name, fieldType, (qint64) n, time);
lm's avatar
lm committed
512 513
        }
        break;
514 515
    default:
        qDebug() << "WARNING: UNKNOWN MAVLINK TYPE";
lm's avatar
lm committed
516 517
    }
}