QGCMAVLinkInspector.cc 23.8 KB
Newer Older
lm's avatar
lm committed
1
#include "QGCMAVLink.h"
lm's avatar
lm committed
2
#include "QGCMAVLinkInspector.h"
3
#include "MultiVehicleManager.h"
4
#include "UAS.h"
5
#include "QGCApplication.h"
6

lm's avatar
lm committed
7 8
#include "ui_QGCMAVLinkInspector.h"

Don Gagne's avatar
Don Gagne committed
9
#include <QList>
lm's avatar
lm committed
10 11
#include <QDebug>

12 13 14
const float QGCMAVLinkInspector::updateHzLowpass = 0.2f;
const unsigned int QGCMAVLinkInspector::updateInterval = 1000U;

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

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

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

35
    // Connect the UI
36 37 38 39 40 41
    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
42

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

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

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

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

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();
76
    components.clear();
LM's avatar
LM committed
77

78
    ui->componentComboBox->addItem(tr("All"), 0);
79 80

    // Fill
81
    Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->getVehicleById(selectedSystemID);
82
    if (vehicle)
83
    {
84
        UASInterface* uas = vehicle->uas();
85 86 87 88 89 90 91
        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
92 93 94 95 96 97
}

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

    rebuildComponentList();
}

104 105 106 107
/**
 * Reset the view. This entails clearing all data structures and resetting data from already-
 * received messages.
 */
108 109
void QGCMAVLinkInspector::clearView()
{
110 111 112 113 114 115
    QMap<int, mavlink_message_t* >::iterator ite;
    for(ite=uasMessageStorage.begin(); ite!=uasMessageStorage.end();++ite)
    {
        delete ite.value();
        ite.value() = NULL;
    }
116
    uasMessageStorage.clear();
117 118 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

    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;
    }
148
    uasMessageHz.clear();
149 150 151 152 153 154 155 156

    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;
    }
157
    uasMessageCount.clear();
158 159 160 161 162 163 164 165

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

168
    onboardMessageInterval.clear();
169

170 171 172
    ui->treeWidget->clear();
}

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

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

182 183 184 185 186
        if (!msgInfo) {
            qWarning() << QStringLiteral("QGCMAVLinkInspector::refreshView NULL msgInfo msgid(%1)").arg(msg->msgid);
            continue;
        }

187 188 189 190
        // Ignore NULL values
        if (msg->msgid == 0xFF) continue;

        // Update the message frenquency
191

192 193 194 195
        // 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
196

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

208 209 210 211
        // 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();
212

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

224 225 226 227
        // 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);
228

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

233
        addUAStoTree(msg->sysid);
234

235 236 237 238 239 240 241
        // 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;
        }
242

243 244 245 246 247 248
        // 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
249
            for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
250
            {
251 252
                QTreeWidgetItem* field = new QTreeWidgetItem();
                widget->addChild(field);
253
            }
254 255 256 257 258
            msgTreeItems->insert(msg->msgid,widget);
            QList<int> groupKeys = msgTreeItems->uniqueKeys();
            int insertIndex = groupKeys.indexOf(msg->msgid);
            uasTreeWidgetItems.value(msg->sysid)->insertChild(insertIndex,widget);
        }
259

260 261 262 263 264 265
        // 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
266
            for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
267
            {
Don Gagne's avatar
Don Gagne committed
268
                updateField(msg, msgInfo, i, message->child(i));
269
            }
lm's avatar
lm committed
270 271
        }
    }
lm's avatar
lm committed
272 273
}

274 275 276 277 278
void QGCMAVLinkInspector::addUAStoTree(int sysId)
{
    if(!uasTreeWidgetItems.contains(sysId))
    {
        // Add the UAS to the main tree after it has been created
279
        Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->getVehicleById(sysId);
280
        if (vehicle)
281
        {
282
            UASInterface* uas = vehicle->uas();
283
            QStringList idstring;
284
            idstring << QString("Vehicle %1").arg(uas->getUASID());
285 286 287 288 289 290 291 292 293
            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
294 295 296
void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t message)
{
    Q_UNUSED(link);
297 298

    quint64 receiveTime;
299 300 301 302
    
    if (selectedSystemID != 0 && selectedSystemID != message.sysid) return;
    if (selectedComponentID != 0 && selectedComponentID != message.compid) return;

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

311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333
    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;
    }
334 335

    // Looking if this message has already been received once
336
    msgFound = false;
337 338 339 340 341 342 343 344 345 346 347 348 349
    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;
350 351
    }

352 353 354 355
    receiveTime = QGC::groundTimeMilliseconds();

    // If the message doesn't exist, create a map for the frequency, message count and time of reception
    if(!msgFound)
356
    {
357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373
        // 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;
374
    }
375
    else
376
    {
377 378
        // The message has been found/created
        if ((lastMsgUpdate->contains(message.msgid))&&(uasMessageCount.contains(message.sysid)))
379
        {
380 381 382 383 384
            // 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))
385
            {
386 387 388 389 390 391 392 393
                if(iter.value()->contains(message.msgid))
                {
                    uasMsgCount = iter.value();
                    count = uasMsgCount->value(message.msgid,0);
                    uasMsgCount->insert(message.msgid,count+1);
                    break;
                }
                ++iter;
394 395
            }
        }
396
        lastMsgUpdate->insert(message.msgid,receiveTime);
397
    }
398

399 400
    if (selectedSystemID == 0 || selectedComponentID == 0)
    {
401 402 403
        return;
    }

404 405
    switch (message.msgid)
    {
406 407 408 409 410 411 412 413 414 415 416
        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
417 418
QGCMAVLinkInspector::~QGCMAVLinkInspector()
{
419
    clearView();
lm's avatar
lm committed
420 421
    delete ui;
}
lm's avatar
lm committed
422

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

    if (!msgFound)
    {
        return;
    }
446

447
    uint8_t* m = (uint8_t*)&uasMessage->payload64[0];
448

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