QGCMAVLinkInspector.cc 23.6 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
        // Ignore NULL values
        if (msg->msgid == 0xFF) continue;

        // Update the message frenquency
186

187 188 189 190
        // 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
191

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

203 204 205 206
        // 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();
207

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

219 220 221 222
        // 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);
223

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

228
        addUAStoTree(msg->sysid);
229

230 231 232 233 234 235 236
        // 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;
        }
237

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

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

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

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

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

306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328
    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;
    }
329 330

    // Looking if this message has already been received once
331
    msgFound = false;
332 333 334 335 336 337 338 339 340 341 342 343 344
    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;
345 346
    }

347 348 349 350
    receiveTime = QGC::groundTimeMilliseconds();

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

394 395
    if (selectedSystemID == 0 || selectedComponentID == 0)
    {
396 397 398
        return;
    }

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

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

    if (!msgFound)
    {
        return;
    }
441

442
    uint8_t* m = (uint8_t*)&uasMessage->payload64[0];
443

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