QGCMAVLinkInspector.cc 23.7 KB
Newer Older
Don Gagne's avatar
Don Gagne committed
1 2
#define MAVLINK_USE_MESSAGE_INFO
#include <stddef.h>         // Hack workaround for Mav 2.0 header problem with respect to offsetof usage
lm's avatar
lm committed
3
#include "QGCMAVLink.h"
lm's avatar
lm committed
4
#include "QGCMAVLinkInspector.h"
5
#include "MultiVehicleManager.h"
6
#include "UAS.h"
7
#include "QGCApplication.h"
8

lm's avatar
lm committed
9 10
#include "ui_QGCMAVLinkInspector.h"

Don Gagne's avatar
Don Gagne committed
11
#include <QList>
lm's avatar
lm committed
12 13
#include <QDebug>

14 15 16
const float QGCMAVLinkInspector::updateHzLowpass = 0.2f;
const unsigned int QGCMAVLinkInspector::updateInterval = 1000U;

17 18
QGCMAVLinkInspector::QGCMAVLinkInspector(const QString& title, QAction* action, MAVLinkProtocol* protocol, QWidget *parent) :
    QGCDockWidget(title, action, parent),
19
    _protocol(protocol),
LM's avatar
LM committed
20 21
    selectedSystemID(0),
    selectedComponentID(0),
lm's avatar
lm committed
22 23 24
    ui(new Ui::QGCMAVLinkInspector)
{
    ui->setupUi(this);
lm's avatar
lm committed
25

26
    // Make sure "All" is an option for both the system and components
27 28
    ui->systemComboBox->addItem(tr("All"), 0);
    ui->componentComboBox->addItem(tr("All"), 0);
lm's avatar
lm committed
29

30
    // Set up the column headers for the message listing
lm's avatar
lm committed
31 32 33 34 35
    QStringList header;
    header << tr("Name");
    header << tr("Value");
    header << tr("Type");
    ui->treeWidget->setHeaderLabels(header);
LM's avatar
LM committed
36

37
    // Connect the UI
38 39 40 41 42 43
    connect(ui->systemComboBox, static_cast<void (QComboBox::*)(int)>(&QComboBox::currentIndexChanged),
            this, &QGCMAVLinkInspector::selectDropDownMenuSystem);
    connect(ui->componentComboBox, static_cast<void (QComboBox::*)(int)>(&QComboBox::currentIndexChanged),
            this, &QGCMAVLinkInspector::selectDropDownMenuComponent);

    connect(ui->clearButton, &QPushButton::clicked, this, &QGCMAVLinkInspector::clearView);
LM's avatar
LM committed
44

45
    // Connect external connections
46
    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &QGCMAVLinkInspector::_vehicleAdded);
47
    connect(protocol, &MAVLinkProtocol::messageReceived, this, &QGCMAVLinkInspector::receiveMessage);
LM's avatar
LM committed
48

49
    // Attach the UI's refresh rate to a timer.
50
    connect(&updateTimer, &QTimer::timeout, this, &QGCMAVLinkInspector::refreshView);
LM's avatar
LM committed
51
    updateTimer.start(updateInterval);
52 53
    
    loadSettings();
lm's avatar
lm committed
54 55
}

56
void QGCMAVLinkInspector::_vehicleAdded(Vehicle* vehicle)
LM's avatar
LM committed
57
{
58
    ui->systemComboBox->addItem(QString("Vehicle %1").arg(vehicle->id()), vehicle->id());
59 60

    // Add a tree for a new UAS
61
    addUAStoTree(vehicle->id());
LM's avatar
LM committed
62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77
}

void QGCMAVLinkInspector::selectDropDownMenuSystem(int dropdownid)
{
    selectedSystemID = ui->systemComboBox->itemData(dropdownid).toInt();
    rebuildComponentList();
}

void QGCMAVLinkInspector::selectDropDownMenuComponent(int dropdownid)
{
    selectedComponentID = ui->componentComboBox->itemData(dropdownid).toInt();
}

void QGCMAVLinkInspector::rebuildComponentList()
{
    ui->componentComboBox->clear();
78
    components.clear();
LM's avatar
LM committed
79

80
    ui->componentComboBox->addItem(tr("All"), 0);
81 82

    // Fill
83
    Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->getVehicleById(selectedSystemID);
84
    if (vehicle)
85
    {
86
        UASInterface* uas = vehicle->uas();
87 88 89 90 91 92 93
        QMap<int, QString> components = uas->getComponents();
        foreach (int id, components.keys())
        {
            QString name = components.value(id);
            ui->componentComboBox->addItem(name, id);
        }
    }
LM's avatar
LM committed
94 95 96 97 98 99
}

void QGCMAVLinkInspector::addComponent(int uas, int component, const QString& name)
{
    Q_UNUSED(component);
    Q_UNUSED(name);
100
    
LM's avatar
LM committed
101 102 103 104 105
    if (uas != selectedSystemID) return;

    rebuildComponentList();
}

106 107 108 109
/**
 * Reset the view. This entails clearing all data structures and resetting data from already-
 * received messages.
 */
110 111
void QGCMAVLinkInspector::clearView()
{
112 113 114 115 116 117
    QMap<int, mavlink_message_t* >::iterator ite;
    for(ite=uasMessageStorage.begin(); ite!=uasMessageStorage.end();++ite)
    {
        delete ite.value();
        ite.value() = NULL;
    }
118
    uasMessageStorage.clear();
119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149

    QMap<int, QMap<int, QTreeWidgetItem*>* >::iterator iteMsg;
    for (iteMsg=uasMsgTreeItems.begin(); iteMsg!=uasMsgTreeItems.end();++iteMsg)
    {
        QMap<int, QTreeWidgetItem*>* msgTreeItems = iteMsg.value();

        QList<int> groupKeys = msgTreeItems->uniqueKeys();
        QList<int>::iterator listKeys;
        for (listKeys=groupKeys.begin();listKeys!=groupKeys.end();++listKeys)
        {
            delete msgTreeItems->take(*listKeys);
        }
    }
    uasMsgTreeItems.clear();

    QMap<int, QTreeWidgetItem* >::iterator iteTree;
    for(iteTree=uasTreeWidgetItems.begin(); iteTree!=uasTreeWidgetItems.end();++iteTree)
    {
        delete iteTree.value();
        iteTree.value() = NULL;
    }
    uasTreeWidgetItems.clear();
    
    QMap<int, QMap<int, float>* >::iterator iteHz;
    for (iteHz=uasMessageHz.begin(); iteHz!=uasMessageHz.end();++iteHz)
    {

        iteHz.value()->clear();
        delete iteHz.value();
        iteHz.value() = NULL;
    }
150
    uasMessageHz.clear();
151 152 153 154 155 156 157 158

    QMap<int, QMap<int, unsigned int>* >::iterator iteCount;
    for(iteCount=uasMessageCount.begin(); iteCount!=uasMessageCount.end();++iteCount)
    {
        iteCount.value()->clear();
        delete iteCount.value();
        iteCount.value() = NULL;
    }
159
    uasMessageCount.clear();
160 161 162 163 164 165 166 167

    QMap<int, QMap<int, quint64>* >::iterator iteLast;
    for(iteLast=uasLastMessageUpdate.begin(); iteLast!=uasLastMessageUpdate.end();++iteLast)
    {
        iteLast.value()->clear();
        delete iteLast.value();
        iteLast.value() = NULL;
    }
168 169
    uasLastMessageUpdate.clear();

170
    onboardMessageInterval.clear();
171

172 173 174
    ui->treeWidget->clear();
}

lm's avatar
lm committed
175 176
void QGCMAVLinkInspector::refreshView()
{
177
    QMap<int, mavlink_message_t* >::const_iterator ite;
178

179
    for(ite=uasMessageStorage.constBegin(); ite!=uasMessageStorage.constEnd();++ite)
lm's avatar
lm committed
180
    {
181
        mavlink_message_t* msg = ite.value();
Don Gagne's avatar
Don Gagne committed
182 183
        const mavlink_message_info_t* msgInfo = mavlink_get_message_info(msg);

184 185 186 187
        // Ignore NULL values
        if (msg->msgid == 0xFF) continue;

        // Update the message frenquency
188

189 190 191 192
        // Get the previous frequency for low-pass filtering
        float msgHz = 0.0f;
        QMap<int, QMap<int, float>* >::const_iterator iteHz = uasMessageHz.find(msg->sysid);
        QMap<int, float>* uasMsgHz = iteHz.value();
lm's avatar
lm committed
193

194 195 196
        while((iteHz != uasMessageHz.end()) && (iteHz.key() == msg->sysid))
        {
            if(iteHz.value()->contains(msg->msgid))
lm's avatar
lm committed
197
            {
198 199 200
                uasMsgHz = iteHz.value();
                msgHz = iteHz.value()->value(msg->msgid);
                break;
lm's avatar
lm committed
201
            }
202 203
            ++iteHz;
        }
lm's avatar
lm committed
204

205 206 207 208
        // Get the number of message received
        float msgCount = 0;
        QMap<int, QMap<int, unsigned int> * >::const_iterator iter = uasMessageCount.find(msg->sysid);
        QMap<int, unsigned int>* uasMsgCount = iter.value();
209

210 211 212
        while((iter != uasMessageCount.end()) && (iter.key()==msg->sysid))
        {
            if(iter.value()->contains(msg->msgid))
213
            {
214 215 216
                msgCount = (float) iter.value()->value(msg->msgid);
                uasMsgCount = iter.value();
                break;
217
            }
218 219
            ++iter;
        }
lm's avatar
lm committed
220

221 222 223 224
        // Compute the new low-pass filtered frequency and update the message count
        msgHz = (1.0f-updateHzLowpass)* msgHz + updateHzLowpass*msgCount/((float)updateInterval/1000.0f);
        uasMsgHz->insert(msg->msgid,msgHz);
        uasMsgCount->insert(msg->msgid,(unsigned int) 0);
225

226 227
        // Update the tree view
        QString messageName("%1 (%2 Hz, #%3)");
Don Gagne's avatar
Don Gagne committed
228
        messageName = messageName.arg(msgInfo->name).arg(msgHz, 3, 'f', 1).arg(msg->msgid);
229

230
        addUAStoTree(msg->sysid);
231

232 233 234 235 236 237 238
        // Look for the tree for the UAS sysid
        QMap<int, QTreeWidgetItem*>* msgTreeItems = uasMsgTreeItems.value(msg->sysid);
        if (!msgTreeItems)
        {
            // The UAS tree has not been created yet, no update
            return;
        }
239

240 241 242 243 244 245
        // Add the message with msgid to the tree if not done yet
        if(!msgTreeItems->contains(msg->msgid))
        {
            QStringList fields;
            fields << messageName;
            QTreeWidgetItem* widget = new QTreeWidgetItem();
Don Gagne's avatar
Don Gagne committed
246
            for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
247
            {
248 249
                QTreeWidgetItem* field = new QTreeWidgetItem();
                widget->addChild(field);
250
            }
251 252 253 254 255
            msgTreeItems->insert(msg->msgid,widget);
            QList<int> groupKeys = msgTreeItems->uniqueKeys();
            int insertIndex = groupKeys.indexOf(msg->msgid);
            uasTreeWidgetItems.value(msg->sysid)->insertChild(insertIndex,widget);
        }
256

257 258 259 260 261 262
        // Update the message
        QTreeWidgetItem* message = msgTreeItems->value(msg->msgid);
        if(message)
        {
            message->setFirstColumnSpanned(true);
            message->setData(0, Qt::DisplayRole, QVariant(messageName));
Don Gagne's avatar
Don Gagne committed
263
            for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
264
            {
Don Gagne's avatar
Don Gagne committed
265
                updateField(msg, msgInfo, i, message->child(i));
266
            }
lm's avatar
lm committed
267 268
        }
    }
lm's avatar
lm committed
269 270
}

271 272 273 274 275
void QGCMAVLinkInspector::addUAStoTree(int sysId)
{
    if(!uasTreeWidgetItems.contains(sysId))
    {
        // Add the UAS to the main tree after it has been created
276
        Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->getVehicleById(sysId);
277
        if (vehicle)
278
        {
279
            UASInterface* uas = vehicle->uas();
280
            QStringList idstring;
281
            idstring << QString("Vehicle %1").arg(uas->getUASID());
282 283 284 285 286 287 288 289 290
            QTreeWidgetItem* uasWidget = new QTreeWidgetItem(idstring);
            uasWidget->setFirstColumnSpanned(true);
            uasTreeWidgetItems.insert(sysId,uasWidget);
            ui->treeWidget->addTopLevelItem(uasWidget);
            uasMsgTreeItems.insert(sysId,new QMap<int, QTreeWidgetItem*>());
        }
    }
}

lm's avatar
lm committed
291 292 293
void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t message)
{
    Q_UNUSED(link);
294 295

    quint64 receiveTime;
296 297 298 299
    
    if (selectedSystemID != 0 && selectedSystemID != message.sysid) return;
    if (selectedComponentID != 0 && selectedComponentID != message.compid) return;

300
    // Create dynamically an array to store the messages for each UAS
301
    if (!uasMessageStorage.contains(message.sysid))
302
    {
303 304 305
        mavlink_message_t* msg = new mavlink_message_t;
        *msg = message;
        uasMessageStorage.insertMulti(message.sysid,msg);
306 307
    }

308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330
    bool msgFound = false;
    QMap<int, mavlink_message_t* >::const_iterator iteMsg = uasMessageStorage.find(message.sysid);
    mavlink_message_t* uasMessage = iteMsg.value();
    while((iteMsg != uasMessageStorage.end()) && (iteMsg.key() == message.sysid))
    {
        if (iteMsg.value()->msgid == message.msgid)
        {
            msgFound = true;
            uasMessage = iteMsg.value();
            break;
        }
        ++iteMsg;
    }
    if (!msgFound)
    {
        mavlink_message_t* msgIdMessage = new mavlink_message_t;
        *msgIdMessage = message;
        uasMessageStorage.insertMulti(message.sysid,msgIdMessage);
    }
    else
    {
        *uasMessage = message;
    }
331 332

    // Looking if this message has already been received once
333
    msgFound = false;
334 335 336 337 338 339 340 341 342 343 344 345 346
    QMap<int, QMap<int, quint64>* >::const_iterator ite = uasLastMessageUpdate.find(message.sysid);
    QMap<int, quint64>* lastMsgUpdate = ite.value();
    while((ite != uasLastMessageUpdate.end()) && (ite.key() == message.sysid))
    {   
        if(ite.value()->contains(message.msgid))
        {
            msgFound = true;

            // Point to the found message
            lastMsgUpdate = ite.value();
            break;
        }
        ++ite;
347 348
    }

349 350 351 352
    receiveTime = QGC::groundTimeMilliseconds();

    // If the message doesn't exist, create a map for the frequency, message count and time of reception
    if(!msgFound)
353
    {
354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370
        // Create a map for the message frequency
        QMap<int, float>* messageHz = new QMap<int,float>;
        messageHz->insert(message.msgid,0.0f);
        uasMessageHz.insertMulti(message.sysid,messageHz);

        // Create a map for the message count
        QMap<int, unsigned int>* messagesCount = new QMap<int, unsigned int>;
        messagesCount->insert(message.msgid,0);
        uasMessageCount.insertMulti(message.sysid,messagesCount);

        // Create a map for the time of reception of the message
        QMap<int, quint64>* lastMessage = new QMap<int, quint64>;
        lastMessage->insert(message.msgid,receiveTime);
        uasLastMessageUpdate.insertMulti(message.sysid,lastMessage);

        // Point to the created message
        lastMsgUpdate = lastMessage;
371
    }
372
    else
373
    {
374 375
        // The message has been found/created
        if ((lastMsgUpdate->contains(message.msgid))&&(uasMessageCount.contains(message.sysid)))
376
        {
377 378 379 380 381
            // Looking for and updating the message count
            unsigned int count = 0;
            QMap<int, QMap<int, unsigned int>* >::const_iterator iter = uasMessageCount.find(message.sysid);
            QMap<int, unsigned int> * uasMsgCount = iter.value();
            while((iter != uasMessageCount.end()) && (iter.key() == message.sysid))
382
            {
383 384 385 386 387 388 389 390
                if(iter.value()->contains(message.msgid))
                {
                    uasMsgCount = iter.value();
                    count = uasMsgCount->value(message.msgid,0);
                    uasMsgCount->insert(message.msgid,count+1);
                    break;
                }
                ++iter;
391 392
            }
        }
393
        lastMsgUpdate->insert(message.msgid,receiveTime);
394
    }
395

396 397
    if (selectedSystemID == 0 || selectedComponentID == 0)
    {
398 399 400
        return;
    }

401 402
    switch (message.msgid)
    {
403 404 405 406 407 408 409 410 411 412 413
        case MAVLINK_MSG_ID_DATA_STREAM:
            {
                mavlink_data_stream_t stream;
                mavlink_msg_data_stream_decode(&message, &stream);
                onboardMessageInterval.insert(stream.stream_id, stream.message_rate);
            }
            break;

    }
}

lm's avatar
lm committed
414 415
QGCMAVLinkInspector::~QGCMAVLinkInspector()
{
416
    clearView();
lm's avatar
lm committed
417 418
    delete ui;
}
lm's avatar
lm committed
419

Don Gagne's avatar
Don Gagne committed
420
void QGCMAVLinkInspector::updateField(mavlink_message_t* msg, const mavlink_message_info_t* msgInfo, int fieldid, QTreeWidgetItem* item)
lm's avatar
lm committed
421 422
{
    // Add field tree widget item
Don Gagne's avatar
Don Gagne committed
423
    item->setData(0, Qt::DisplayRole, QVariant(msgInfo->fields[fieldid].name));
424 425
    
    bool msgFound = false;
Don Gagne's avatar
Don Gagne committed
426
    QMap<int, mavlink_message_t* >::const_iterator iteMsg = uasMessageStorage.find(msg->sysid);
427
    mavlink_message_t* uasMessage = iteMsg.value();
Don Gagne's avatar
Don Gagne committed
428
    while((iteMsg != uasMessageStorage.end()) && (iteMsg.key() == msg->sysid))
429
    {
Don Gagne's avatar
Don Gagne committed
430
        if (iteMsg.value()->msgid == msg->msgid)
431 432 433 434 435 436 437 438 439 440 441 442
        {
            msgFound = true;
            uasMessage = iteMsg.value();
            break;
        }
        ++iteMsg;
    }

    if (!msgFound)
    {
        return;
    }
443

444 445 446
    uint8_t* m = ((uint8_t*)uasMessage)+8;


Don Gagne's avatar
Don Gagne committed
447
    switch (msgInfo->fields[fieldid].type)
lm's avatar
lm committed
448 449
    {
    case MAVLINK_TYPE_CHAR:
Don Gagne's avatar
Don Gagne committed
450
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
451
        {
Don Gagne's avatar
Don Gagne committed
452
            char* str = (char*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
453
            // Enforce null termination
Don Gagne's avatar
Don Gagne committed
454
            str[msgInfo->fields[fieldid].array_length-1] = '\0';
lm's avatar
lm committed
455 456 457 458 459 460 461
            QString string(str);
            item->setData(2, Qt::DisplayRole, "char");
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single char
Don Gagne's avatar
Don Gagne committed
462 463
            char b = *((char*)(m+msgInfo->fields[fieldid].wire_offset));
            item->setData(2, Qt::DisplayRole, QString("char[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
464 465 466 467
            item->setData(1, Qt::DisplayRole, b);
        }
        break;
    case MAVLINK_TYPE_UINT8_T:
Don Gagne's avatar
Don Gagne committed
468
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
469
        {
Don Gagne's avatar
Don Gagne committed
470
            uint8_t* nums = m+msgInfo->fields[fieldid].wire_offset;
lm's avatar
lm committed
471 472 473
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
474
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
475 476 477
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
478
            item->setData(2, Qt::DisplayRole, QString("uint8_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
479 480 481 482 483
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
484
            uint8_t u = *(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
485 486 487 488 489
            item->setData(2, Qt::DisplayRole, "uint8_t");
            item->setData(1, Qt::DisplayRole, u);
        }
        break;
    case MAVLINK_TYPE_INT8_T:
Don Gagne's avatar
Don Gagne committed
490
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
491
        {
Don Gagne's avatar
Don Gagne committed
492
            int8_t* nums = (int8_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
493 494 495
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
496
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
497 498 499
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
500
            item->setData(2, Qt::DisplayRole, QString("int8_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
501 502 503 504 505
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
506
            int8_t n = *((int8_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
507 508 509 510 511
            item->setData(2, Qt::DisplayRole, "int8_t");
            item->setData(1, Qt::DisplayRole, n);
        }
        break;
    case MAVLINK_TYPE_UINT16_T:
Don Gagne's avatar
Don Gagne committed
512
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
513
        {
Don Gagne's avatar
Don Gagne committed
514
            uint16_t* nums = (uint16_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
515 516 517
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
518
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
519 520 521
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
522
            item->setData(2, Qt::DisplayRole, QString("uint16_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
523 524 525 526 527
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
528
            uint16_t n = *((uint16_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
529 530 531 532 533
            item->setData(2, Qt::DisplayRole, "uint16_t");
            item->setData(1, Qt::DisplayRole, n);
        }
        break;
    case MAVLINK_TYPE_INT16_T:
Don Gagne's avatar
Don Gagne committed
534
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
535
        {
Don Gagne's avatar
Don Gagne committed
536
            int16_t* nums = (int16_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
537 538 539
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
540
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
541 542 543
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
544
            item->setData(2, Qt::DisplayRole, QString("int16_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
545 546 547 548 549
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
550
            int16_t n = *((int16_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
551 552 553 554 555
            item->setData(2, Qt::DisplayRole, "int16_t");
            item->setData(1, Qt::DisplayRole, n);
        }
        break;
    case MAVLINK_TYPE_UINT32_T:
Don Gagne's avatar
Don Gagne committed
556
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
557
        {
Don Gagne's avatar
Don Gagne committed
558
            uint32_t* nums = (uint32_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
559 560 561
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
562
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
563 564 565
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
566
            item->setData(2, Qt::DisplayRole, QString("uint32_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
567 568 569 570 571
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
572
            float n = *((uint32_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
573 574 575 576 577
            item->setData(2, Qt::DisplayRole, "uint32_t");
            item->setData(1, Qt::DisplayRole, n);
        }
        break;
    case MAVLINK_TYPE_INT32_T:
Don Gagne's avatar
Don Gagne committed
578
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
579
        {
Don Gagne's avatar
Don Gagne committed
580
            int32_t* nums = (int32_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
581 582 583
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
584
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
585 586 587
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
588
            item->setData(2, Qt::DisplayRole, QString("int32_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
589 590 591 592 593
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
594
            int32_t n = *((int32_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
595 596 597 598 599
            item->setData(2, Qt::DisplayRole, "int32_t");
            item->setData(1, Qt::DisplayRole, n);
        }
        break;
    case MAVLINK_TYPE_FLOAT:
Don Gagne's avatar
Don Gagne committed
600
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
601
        {
Don Gagne's avatar
Don Gagne committed
602
            float* nums = (float*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
603 604 605
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
606
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
607
            {
Geoff Fink's avatar
Geoff Fink committed
608
               string += tmp.arg(nums[j]);
lm's avatar
lm committed
609
            }
Don Gagne's avatar
Don Gagne committed
610
            item->setData(2, Qt::DisplayRole, QString("float[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
611 612 613 614 615
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
616
            float f = *((float*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
617 618 619 620 621
            item->setData(2, Qt::DisplayRole, "float");
            item->setData(1, Qt::DisplayRole, f);
        }
        break;
    case MAVLINK_TYPE_DOUBLE:
Don Gagne's avatar
Don Gagne committed
622
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
623
        {
Don Gagne's avatar
Don Gagne committed
624
            double* nums = (double*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
625 626 627
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
628
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
629 630 631
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
632
            item->setData(2, Qt::DisplayRole, QString("double[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
633 634 635 636 637
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
638
            double f = *((double*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
639 640 641 642 643
            item->setData(2, Qt::DisplayRole, "double");
            item->setData(1, Qt::DisplayRole, f);
        }
        break;
    case MAVLINK_TYPE_UINT64_T:
Don Gagne's avatar
Don Gagne committed
644
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
645
        {
Don Gagne's avatar
Don Gagne committed
646
            uint64_t* nums = (uint64_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
647 648 649
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
650
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
651 652 653
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
654
            item->setData(2, Qt::DisplayRole, QString("uint64_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
655 656 657 658 659
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
660
            uint64_t n = *((uint64_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
661
            item->setData(2, Qt::DisplayRole, "uint64_t");
pixhawk's avatar
pixhawk committed
662
            item->setData(1, Qt::DisplayRole, (quint64) n);
lm's avatar
lm committed
663 664 665
        }
        break;
    case MAVLINK_TYPE_INT64_T:
Don Gagne's avatar
Don Gagne committed
666
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
667
        {
Don Gagne's avatar
Don Gagne committed
668
            int64_t* nums = (int64_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
669 670 671
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
672
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
673 674 675
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
676
            item->setData(2, Qt::DisplayRole, QString("int64_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
677 678 679 680 681
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
682
            int64_t n = *((int64_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
683
            item->setData(2, Qt::DisplayRole, "int64_t");
pixhawk's avatar
pixhawk committed
684
            item->setData(1, Qt::DisplayRole, (qint64) n);
lm's avatar
lm committed
685 686 687 688
        }
        break;
    }
}