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);
Don Gagne's avatar
Don Gagne committed
18
    messageFilter.insert(MAVLINK_MSG_ID_STATUSTEXT_LONG, false);
19
    messageFilter.insert(MAVLINK_MSG_ID_COMMAND_LONG, false);
20 21 22 23 24 25
    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);
26
    messageFilter.insert(MAVLINK_MSG_ID_DATA_STREAM, false);
27
    messageFilter.insert(MAVLINK_MSG_ID_GPS_STATUS, false);
28
    messageFilter.insert(MAVLINK_MSG_ID_RC_CHANNELS_RAW, false);
29
    messageFilter.insert(MAVLINK_MSG_ID_LOG_DATA, false);
30
    #ifdef MAVLINK_MSG_ID_ENCAPSULATED_DATA
31
    messageFilter.insert(MAVLINK_MSG_ID_ENCAPSULATED_DATA, false);
32 33
    #endif
    #ifdef MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE
34
    messageFilter.insert(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, false);
35
    #endif
36
    messageFilter.insert(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, false);
37

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

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

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

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

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

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

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

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

77
    // The SYSTEM_TIME message is special, in that it's handled here for synchronizing the QGC time with the remote time.
78
    if (message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME)
lm's avatar
lm committed
79
    {
pixhawk's avatar
pixhawk committed
80 81
        mavlink_system_time_t timebase;
        mavlink_msg_system_time_decode(&message, &timebase);
82 83
        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
84 85 86 87
    }
    else
    {

88
        // 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
89
        uint8_t fieldid = 0;
90
        uint8_t* m = (uint8_t*)(msgDict[msgid].payload64);
91

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

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

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

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

pixhawk's avatar
pixhawk committed
116
quint64 MAVLinkDecoder::getUnixTimeFromMs(int systemID, quint64 time)
117
{
pixhawk's avatar
pixhawk committed
118 119 120
    quint64 ret = 0;
    if (time == 0)
    {
121
        ret = QGC::groundTimeMilliseconds() - sysDict[systemID].onboardToGCSUnixTimeOffsetAndDelay;
pixhawk's avatar
pixhawk committed
122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144
    }
    // 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
    {
145
        if (sysDict[systemID].onboardTimeOffset == 0 || time < (sysDict[systemID].firstOnboardTime -100))
pixhawk's avatar
pixhawk committed
146
        {
147 148
            sysDict[systemID].firstOnboardTime = time;
            sysDict[systemID].onboardTimeOffset = QGC::groundTimeMilliseconds() - time;
pixhawk's avatar
pixhawk committed
149
        }
150

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

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

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 190

//    // 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
191 192 193
    return ret;
}

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

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

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

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

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

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

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

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

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

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