Skip to content
QGCMAVLinkInspector.cc 23.6 KiB
Newer Older
lm's avatar
lm committed
#include "QGCMAVLink.h"
lm's avatar
lm committed
#include "QGCMAVLinkInspector.h"
#include "MultiVehicleManager.h"
#include "UAS.h"
#include "QGCApplication.h"
lm's avatar
lm committed
#include "ui_QGCMAVLinkInspector.h"

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

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

QGCMAVLinkInspector::QGCMAVLinkInspector(const QString& title, QAction* action, MAVLinkProtocol* protocol, QWidget *parent) :
    QGCDockWidget(title, action, parent),
LM's avatar
LM committed
    selectedSystemID(0),
    selectedComponentID(0),
lm's avatar
lm committed
    ui(new Ui::QGCMAVLinkInspector)
{
    ui->setupUi(this);
lm's avatar
lm committed

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

    // Set up the column headers for the message listing
lm's avatar
lm committed
    QStringList header;
    header << tr("Name");
    header << tr("Value");
    header << tr("Type");
    ui->treeWidget->setHeaderLabels(header);
    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);
    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &QGCMAVLinkInspector::_vehicleAdded);
    connect(protocol, &MAVLinkProtocol::messageReceived, this, &QGCMAVLinkInspector::receiveMessage);
    // Attach the UI's refresh rate to a timer.
    connect(&updateTimer, &QTimer::timeout, this, &QGCMAVLinkInspector::refreshView);
LM's avatar
LM committed
    updateTimer.start(updateInterval);
    
    loadSettings();
lm's avatar
lm committed
}

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

    // Add a tree for a new UAS
    addUAStoTree(vehicle->id());
LM's avatar
LM committed
}

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();
    ui->componentComboBox->addItem(tr("All"), 0);
    Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->getVehicleById(selectedSystemID);
    if (vehicle)
        UASInterface* uas = vehicle->uas();
        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
}

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

    rebuildComponentList();
}

/**
 * Reset the view. This entails clearing all data structures and resetting data from already-
 * received messages.
 */
void QGCMAVLinkInspector::clearView()
{
    QMap<int, mavlink_message_t* >::iterator ite;
    for(ite=uasMessageStorage.begin(); ite!=uasMessageStorage.end();++ite)
    {
        delete ite.value();
        ite.value() = NULL;
    }

    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;
    }
    uasMessageHz.clear();

    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;
    }
    uasMessageCount.clear();

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

    ui->treeWidget->clear();
}

lm's avatar
lm committed
void QGCMAVLinkInspector::refreshView()
{
    QMap<int, mavlink_message_t* >::const_iterator ite;
    for(ite=uasMessageStorage.constBegin(); ite!=uasMessageStorage.constEnd();++ite)
lm's avatar
lm committed
    {
Don Gagne's avatar
Don Gagne committed
        const mavlink_message_info_t* msgInfo = mavlink_get_message_info(msg);

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

        // Update the message frenquency
        // 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

        while((iteHz != uasMessageHz.end()) && (iteHz.key() == msg->sysid))
        {
            if(iteHz.value()->contains(msg->msgid))
lm's avatar
lm committed
            {
                uasMsgHz = iteHz.value();
                msgHz = iteHz.value()->value(msg->msgid);
                break;
lm's avatar
lm committed
            }
lm's avatar
lm committed

        // 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();
        while((iter != uasMessageCount.end()) && (iter.key()==msg->sysid))
        {
            if(iter.value()->contains(msg->msgid))
                msgCount = (float) iter.value()->value(msg->msgid);
                uasMsgCount = iter.value();
                break;
lm's avatar
lm committed

        // 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);
        // Update the tree view
        QString messageName("%1 (%2 Hz, #%3)");
Don Gagne's avatar
Don Gagne committed
        messageName = messageName.arg(msgInfo->name).arg(msgHz, 3, 'f', 1).arg(msg->msgid);
        // 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;
        }
        // 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
            for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
                QTreeWidgetItem* field = new QTreeWidgetItem();
                widget->addChild(field);
            msgTreeItems->insert(msg->msgid,widget);
            QList<int> groupKeys = msgTreeItems->uniqueKeys();
            int insertIndex = groupKeys.indexOf(msg->msgid);
            uasTreeWidgetItems.value(msg->sysid)->insertChild(insertIndex,widget);
        }
        // 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
            for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
Don Gagne's avatar
Don Gagne committed
                updateField(msg, msgInfo, i, message->child(i));
lm's avatar
lm committed
        }
    }
void QGCMAVLinkInspector::addUAStoTree(int sysId)
{
    if(!uasTreeWidgetItems.contains(sysId))
    {
        // Add the UAS to the main tree after it has been created
        Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->getVehicleById(sysId);
        if (vehicle)
            UASInterface* uas = vehicle->uas();
            QStringList idstring;
            idstring << QString("Vehicle %1").arg(uas->getUASID());
            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
void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t message)
{
    Q_UNUSED(link);

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

    // Create dynamically an array to store the messages for each UAS
    if (!uasMessageStorage.contains(message.sysid))
        mavlink_message_t* msg = new mavlink_message_t;
        *msg = message;
        uasMessageStorage.insertMulti(message.sysid,msg);
    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;
    }

    // Looking if this message has already been received once
    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;
    receiveTime = QGC::groundTimeMilliseconds();

    // If the message doesn't exist, create a map for the frequency, message count and time of reception
    if(!msgFound)
        // 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;
        // The message has been found/created
        if ((lastMsgUpdate->contains(message.msgid))&&(uasMessageCount.contains(message.sysid)))
            // 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))
                if(iter.value()->contains(message.msgid))
                {
                    uasMsgCount = iter.value();
                    count = uasMsgCount->value(message.msgid,0);
                    uasMsgCount->insert(message.msgid,count+1);
                    break;
                }
                ++iter;
        lastMsgUpdate->insert(message.msgid,receiveTime);
    if (selectedSystemID == 0 || selectedComponentID == 0)
    {
    switch (message.msgid)
    {
        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
QGCMAVLinkInspector::~QGCMAVLinkInspector()
{
    clearView();
lm's avatar
lm committed
    delete ui;
}
lm's avatar
lm committed

Don Gagne's avatar
Don Gagne committed
void QGCMAVLinkInspector::updateField(mavlink_message_t* msg, const mavlink_message_info_t* msgInfo, int fieldid, QTreeWidgetItem* item)
lm's avatar
lm committed
{
    // Add field tree widget item
Don Gagne's avatar
Don Gagne committed
    item->setData(0, Qt::DisplayRole, QVariant(msgInfo->fields[fieldid].name));
Don Gagne's avatar
Don Gagne committed
    QMap<int, mavlink_message_t* >::const_iterator iteMsg = uasMessageStorage.find(msg->sysid);
    mavlink_message_t* uasMessage = iteMsg.value();
Don Gagne's avatar
Don Gagne committed
    while((iteMsg != uasMessageStorage.end()) && (iteMsg.key() == msg->sysid))
Don Gagne's avatar
Don Gagne committed
        if (iteMsg.value()->msgid == msg->msgid)
        {
            msgFound = true;
            uasMessage = iteMsg.value();
            break;
        }
        ++iteMsg;
    }

    if (!msgFound)
    {
        return;
    }
    uint8_t* m = (uint8_t*)&uasMessage->payload64[0];
Don Gagne's avatar
Don Gagne committed
    switch (msgInfo->fields[fieldid].type)
lm's avatar
lm committed
    {
    case MAVLINK_TYPE_CHAR:
Don Gagne's avatar
Don Gagne committed
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            char* str = (char*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
Don Gagne's avatar
Don Gagne committed
            str[msgInfo->fields[fieldid].array_length-1] = '\0';
lm's avatar
lm committed
            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
            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
            item->setData(1, Qt::DisplayRole, b);
        }
        break;
    case MAVLINK_TYPE_UINT8_T:
Don Gagne's avatar
Don Gagne committed
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            uint8_t* nums = m+msgInfo->fields[fieldid].wire_offset;
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("uint8_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            uint8_t u = *(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            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
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            int8_t* nums = (int8_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("int8_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            int8_t n = *((int8_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
            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
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            uint16_t* nums = (uint16_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("uint16_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            uint16_t n = *((uint16_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
            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
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            int16_t* nums = (int16_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("int16_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            int16_t n = *((int16_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
            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
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            uint32_t* nums = (uint32_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("uint32_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            float n = *((uint32_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
            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
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            int32_t* nums = (int32_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("int32_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            int32_t n = *((int32_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
            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
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            float* nums = (float*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
Geoff Fink's avatar
Geoff Fink committed
               string += tmp.arg(nums[j]);
lm's avatar
lm committed
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("float[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            float f = *((float*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
            item->setData(2, Qt::DisplayRole, "float");
            item->setData(1, Qt::DisplayRole, f);
        }
        break;
    case MAVLINK_TYPE_DOUBLE:
Don Gagne's avatar
Don Gagne committed
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            double* nums = (double*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("double[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            double f = *((double*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
            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
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            uint64_t* nums = (uint64_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("uint64_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            uint64_t n = *((uint64_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
            item->setData(2, Qt::DisplayRole, "uint64_t");
pixhawk's avatar
pixhawk committed
            item->setData(1, Qt::DisplayRole, (quint64) n);
lm's avatar
lm committed
        }
        break;
    case MAVLINK_TYPE_INT64_T:
Don Gagne's avatar
Don Gagne committed
        if (msgInfo->fields[fieldid].array_length > 0)
lm's avatar
lm committed
        {
Don Gagne's avatar
Don Gagne committed
            int64_t* nums = (int64_t*)(m+msgInfo->fields[fieldid].wire_offset);
lm's avatar
lm committed
            // Enforce null termination
            QString tmp("%1, ");
            QString string;
Don Gagne's avatar
Don Gagne committed
            for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
lm's avatar
lm committed
            {
                string += tmp.arg(nums[j]);
            }
Don Gagne's avatar
Don Gagne committed
            item->setData(2, Qt::DisplayRole, QString("int64_t[%1]").arg(msgInfo->fields[fieldid].array_length));
lm's avatar
lm committed
            item->setData(1, Qt::DisplayRole, string);
        }
        else
        {
            // Single value
Don Gagne's avatar
Don Gagne committed
            int64_t n = *((int64_t*)(m+msgInfo->fields[fieldid].wire_offset));
lm's avatar
lm committed
            item->setData(2, Qt::DisplayRole, "int64_t");
pixhawk's avatar
pixhawk committed
            item->setData(1, Qt::DisplayRole, (qint64) n);
lm's avatar
lm committed
        }
        break;
    }
}