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
    uint8_t* m = (uint8_t*)&uasMessage->payload64[0];
445

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