QGCMAVLinkInspector.cc 23.7 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 44 45 46
    MultiVehicleManager* multiVehicleManager = qgcApp()->toolbox()->multiVehicleManager();
    connect(multiVehicleManager,    &MultiVehicleManager::vehicleAdded,     this, &QGCMAVLinkInspector::_vehicleAdded);
    connect(multiVehicleManager,    &MultiVehicleManager::vehicleRemoved,   this, &QGCMAVLinkInspector::_vehicleRemoved);
    connect(protocol,               &MAVLinkProtocol::messageReceived,      this, &QGCMAVLinkInspector::receiveMessage);
LM's avatar
LM committed
47

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

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

    // Add a tree for a new UAS
60 61 62 63 64 65
    addVehicleToTree(vehicle->id());
}

void QGCMAVLinkInspector::_vehicleRemoved(Vehicle* vehicle)
{
    removeVehicleFromTree(vehicle->id());
LM's avatar
LM committed
66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81
}

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

84
    ui->componentComboBox->addItem(tr("All"), 0);
85 86

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

void QGCMAVLinkInspector::addComponent(int uas, int component, const QString& name)
{
    Q_UNUSED(component);
    Q_UNUSED(name);
104
    
LM's avatar
LM committed
105 106 107 108 109
    if (uas != selectedSystemID) return;

    rebuildComponentList();
}

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

    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;
    }
154
    uasMessageHz.clear();
155 156 157 158 159 160 161 162

    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;
    }
163
    uasMessageCount.clear();
164 165 166 167 168 169 170 171

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

174 175 176
    ui->treeWidget->clear();
}

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

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

186 187 188 189 190
        if (!msgInfo) {
            qWarning() << QStringLiteral("QGCMAVLinkInspector::refreshView NULL msgInfo msgid(%1)").arg(msg->msgid);
            continue;
        }

191 192 193 194
        // Ignore NULL values
        if (msg->msgid == 0xFF) continue;

        // Update the message frenquency
195

196 197 198 199
        // 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
200

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

212 213 214 215
        // 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();
216

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

228 229 230 231
        // 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);
232

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

237
        addVehicleToTree(msg->sysid);
238

239 240 241 242 243 244 245
        // 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;
        }
246

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

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

278
void QGCMAVLinkInspector::addVehicleToTree(int vehicleId)
279
{
280 281 282 283 284 285 286 287
    if (!uasTreeWidgetItems.contains(vehicleId)) {
        QStringList idstring;
        idstring << tr("Vehicle %1").arg(vehicleId);
        QTreeWidgetItem* uasWidget = new QTreeWidgetItem(idstring);
        uasWidget->setFirstColumnSpanned(true);
        uasTreeWidgetItems.insert(vehicleId, uasWidget);
        ui->treeWidget->addTopLevelItem(uasWidget);
        uasMsgTreeItems.insert(vehicleId, new QMap<int, QTreeWidgetItem*>());
288 289 290
    }
}

291 292 293 294 295 296 297 298 299
void QGCMAVLinkInspector::removeVehicleFromTree(int vehicleId)
{
    Q_UNUSED(vehicleId);

    // This doesn't work with multi-vehicle. But this code is so screwed up and crufty it's not worth the effort making that work.
    // Especially since mult-vehicle support here has been broken for ages. Better to at least get single vehicle working.
    clearView();
}

lm's avatar
lm committed
300 301 302
void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t message)
{
    Q_UNUSED(link);
303 304

    quint64 receiveTime;
305 306 307 308
    
    if (selectedSystemID != 0 && selectedSystemID != message.sysid) return;
    if (selectedComponentID != 0 && selectedComponentID != message.compid) return;

309
    // Create dynamically an array to store the messages for each UAS
310
    if (!uasMessageStorage.contains(message.sysid))
311
    {
312 313 314
        mavlink_message_t* msg = new mavlink_message_t;
        *msg = message;
        uasMessageStorage.insertMulti(message.sysid,msg);
315 316
    }

317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339
    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;
    }
340 341

    // Looking if this message has already been received once
342
    msgFound = false;
343 344 345 346 347 348 349 350 351 352 353 354 355
    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;
356 357
    }

358 359 360 361
    receiveTime = QGC::groundTimeMilliseconds();

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

}

lm's avatar
lm committed
407 408
QGCMAVLinkInspector::~QGCMAVLinkInspector()
{
409
    clearView();
lm's avatar
lm committed
410 411
    delete ui;
}
lm's avatar
lm committed
412

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

    if (!msgFound)
    {
        return;
    }
436

437
    uint8_t* m = (uint8_t*)&uasMessage->payload64[0];
438

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