diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 16f860b3aad4c26f235ad774129c37330822aff5..177e6cb5fbcd97fec8ac6983ddbb12396d4b3e15 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -542,19 +542,19 @@ TRANSLATIONS += es-MX.ts \ # xbee support # libxbee only supported by linux and windows systems -win32-msvc2008|win32-msvc2010|linux{ - HEADERS += src/comm/XbeeLinkInterface.h \ - src/comm/XbeeLink.h \ - src/comm/HexSpinBox.h \ - src/ui/XbeeConfigurationWindow.h \ - src/comm/CallConv.h - SOURCES += src/comm/XbeeLink.cpp \ - src/comm/HexSpinBox.cpp \ - src/ui/XbeeConfigurationWindow.cpp - DEFINES += XBEELINK - INCLUDEPATH += thirdParty/libxbee +#win32-msvc2008|win32-msvc2010|linux{ +# HEADERS += src/comm/XbeeLinkInterface.h \ +# src/comm/XbeeLink.h \ +# src/comm/HexSpinBox.h \ +# src/ui/XbeeConfigurationWindow.h \ +# src/comm/CallConv.h +# SOURCES += src/comm/XbeeLink.cpp \ +# src/comm/HexSpinBox.cpp \ +# src/ui/XbeeConfigurationWindow.cpp +# DEFINES += XBEELINK +# INCLUDEPATH += thirdParty/libxbee # TO DO: build library when it does not exists already - LIBS += -LthirdParty/libxbee/lib \ - -llibxbee - -} +# LIBS += -LthirdParty/libxbee/lib \ +# -llibxbee +# +#} diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc index 18b36bed1fbf171ded47ff6abd37a64404f63763..b356fd4eddf78f0d8e70cdf7da7e903619192732 100644 --- a/src/GAudioOutput.cc +++ b/src/GAudioOutput.cc @@ -119,9 +119,9 @@ GAudioOutput::GAudioOutput(QObject* parent) : QObject(parent), } #endif // Initialize audio output - m_media = new Phonon::MediaObject(this); - Phonon::AudioOutput *audioOutput = new Phonon::AudioOutput(Phonon::MusicCategory, this); - createPath(m_media, audioOutput); + //m_media = new Phonon::MediaObject(this); + //Phonon::AudioOutput *audioOutput = new Phonon::AudioOutput(Phonon::MusicCategory, this); + //createPath(m_media, audioOutput); // Prepare regular emergency signal, will be fired off on calling startEmergency() emergencyTimer = new QTimer(); @@ -184,8 +184,8 @@ bool GAudioOutput::say(QString text, int severity) cst_wave* wav = flite_text_to_wave(text.toStdString().c_str(), v); // file.fileName() returns the unique file name cst_wave_save(wav, file.fileName().toStdString().c_str(), "riff"); - m_media->setCurrentSource(Phonon::MediaSource(file.fileName().toStdString().c_str())); - m_media->play(); + //m_media->setCurrentSource(Phonon::MediaSource(file.fileName().toStdString().c_str())); + //m_media->play(); res = true; } #endif @@ -228,8 +228,8 @@ void GAudioOutput::notifyPositive() if (!muted) { // Use QFile to transform path for all OS QFile f(QCoreApplication::applicationDirPath()+QString("/audio/double_notify.wav")); - m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); - m_media->play(); + //m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); + //m_media->play(); } } @@ -238,8 +238,8 @@ void GAudioOutput::notifyNegative() if (!muted) { // Use QFile to transform path for all OS QFile f(QCoreApplication::applicationDirPath()+QString("/audio/flat_notify.wav")); - m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); - m_media->play(); + //m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); + //m_media->play(); } } @@ -283,8 +283,8 @@ void GAudioOutput::beep() // Use QFile to transform path for all OS QFile f(QCoreApplication::applicationDirPath()+QString("/audio/alert.wav")); qDebug() << "FILE:" << f.fileName(); - m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); - m_media->play(); + //m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); + //m_media->play(); } } diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 5ac86fabb6e59d06157b366bbfe7a6b2dcd1f0a1..64520a50a2f0f067cacc5e689907fd453edc71b6 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include #include "QGCMAVLink.h" diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 262141b107e6529b8ea8674f9c554cdf99bdfdeb..6a296e4236df6bc5162ba63c32d1af8f826a277d 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -211,8 +211,9 @@ using namespace TNX; SerialLink::SerialLink(QString portname, int baudRate, bool hardwareFlowControl, bool parity, int dataBits, int stopBits) : - port(NULL), m_stopp(false), - ports(new QVector()) + port(NULL), + ports(new QVector()), + m_stopp(false) { // Setup settings this->porthandle = portname.trimmed(); diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index c786af608f4e99d858b1351426ed591284370c12..020eae9959ef8733240d70a0986f84c5b8e1addb 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -216,7 +216,7 @@ void HDDisplay::saveState() // Restore instrument settings for (int i = 0; i < acceptList->count(); i++) { QString key = acceptList->at(i); - instruments += "|" + QString::number(minValues.value(key, -1.0))+","+key+","+acceptUnitList->at(i)+","+QString::number(maxValues.value(key, +1.0))+","+((symmetric.value(key, false)) ? "s" : ""); + instruments += "|" + QString::number(minValues.value(key, -1.0))+","+key+","+acceptUnitList->at(i)+","+QString::number(maxValues.value(key, +1.0))+","+customNames.value(key, "")+","+((symmetric.value(key, false)) ? "s" : ""); } // qDebug() << "Saving" << instruments; @@ -271,16 +271,23 @@ void HDDisplay::addGauge() QStringList items; for (int i = 0; i < values.count(); ++i) { QString key = values.keys().at(i); + QString label = key; + QStringList keySplit = key.split("."); + if (keySplit.size() > 1) + { + keySplit.removeFirst(); + label = keySplit.join("."); + } QString unit = units.value(key); if (unit.contains("deg") || unit.contains("rad")) { - items.append(QString("%1,%2,%3,%4,s").arg("-180").arg(key).arg(unit).arg("+180")); + items.append(QString("%1,%2,%3,%4,%5,s").arg("-180").arg(key).arg(unit).arg("+180").arg(label)); } else { - items.append(QString("%1,%2,%3,%4").arg("0").arg(key).arg(unit).arg("+100")); + items.append(QString("%1,%2,%3,%4,%5").arg("0").arg(key).arg(unit).arg("+100").arg(label)); } } bool ok; QString item = QInputDialog::getItem(this, tr("Add Gauge Instrument"), - tr("Format: min, curve name, unit, max[,s]"), items, 0, true, &ok); + tr("Format: min, data name, unit, max, label [,s]"), items, 0, true, &ok); if (ok && !item.isEmpty()) { addGauge(item); } @@ -307,9 +314,19 @@ void HDDisplay::addGauge(const QString& gauge) val = parts.at(3).toDouble(&ok); success &= ok; if (ok) maxValues.insert(key, val); + // Convert name + if (parts.length() >= 5) + { + if (parts.at(4).length() > 0) + { + customNames.insert(key, parts.at(4)); + } + } // Convert symmetric flag - if (parts.length() >= 5) { - if (parts.at(4).contains("s")) { + if (parts.length() >= 6) + { + if (parts.at(5).contains("s")) + { symmetric.insert(key, true); } } @@ -425,12 +442,15 @@ void HDDisplay::renderOverlay() float topSpacing = leftSpacing; float yCoord = topSpacing + gaugeWidth/2.0f; - for (int i = 0; i < acceptList->size(); ++i) { + for (int i = 0; i < acceptList->size(); ++i) + { QString value = acceptList->at(i); - drawGauge(xCoord, yCoord, gaugeWidth/2.0f, minValues.value(value, -1.0f), maxValues.value(value, 1.0f), value, values.value(value, minValues.value(value, 0.0f)), gaugeColor, &painter, symmetric.value(value, false), goodRanges.value(value, qMakePair(0.0f, 0.5f)), critRanges.value(value, qMakePair(0.7f, 1.0f)), true); + QString label = customNames.value(value); + drawGauge(xCoord, yCoord, gaugeWidth/2.0f, minValues.value(value, -1.0f), maxValues.value(value, 1.0f), label, values.value(value, minValues.value(value, 0.0f)), gaugeColor, &painter, symmetric.value(value, false), goodRanges.value(value, qMakePair(0.0f, 0.5f)), critRanges.value(value, qMakePair(0.7f, 1.0f)), true); xCoord += gaugeWidth + leftSpacing; // Move one row down if necessary - if (xCoord + gaugeWidth*0.9f > vwidth) { + if (xCoord + gaugeWidth*0.9f > vwidth) + { yCoord += topSpacing + gaugeWidth; xCoord = leftSpacing + gaugeWidth/2.0f; } @@ -807,12 +827,45 @@ float HDDisplay::refLineWidthToPen(float line) return line * 2.50f; } +void HDDisplay::addSource(QObject* obj) +{ + //genericSources.append(obj); + // FIXME XXX HACK +// if (plots.size() > 0) +// { + // Connect generic source + connect(obj, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,unsigned int,quint64)), this, SLOT(updateValue(int,QString,QString,unsigned int,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), this, SLOT(updateValue(int,QString,QString,quint64,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), this, SLOT(updateValue(int,QString,QString,qint64,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); +// } +} + void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec) { if (!intValues.contains(name)) intValues.insert(name, true); updateValue(uasId, name, unit, (double)value, msec); } +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const unsigned int value, const quint64 msec) +{ + if (!intValues.contains(name)) intValues.insert(name, true); + updateValue(uasId, name, unit, (double)value, msec); +} + +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec) +{ + if (!intValues.contains(name)) intValues.insert(name, true); + updateValue(uasId, name, unit, (double)value, msec); +} + +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec) +{ + if (!intValues.contains(name)) intValues.insert(name, true); + updateValue(uasId, name, unit, (double)value, msec); +} + void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec) { Q_UNUSED(uasId); diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index 15a2240d195cf42031d843d53abd38012867fe42..cdec1cc7483e7be6f62c04a2c5d4224196602d22 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -68,7 +68,14 @@ public slots: void updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); /** @brief Update a HDD integer value */ void updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec); + /** @brief Update a HDD integer value */ + void updateValue(const int uasId, const QString& name, const QString& unit, const unsigned int value, const quint64 msec); + /** @brief Update a HDD integer value */ + void updateValue(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec); + /** @brief Update a HDD integer value */ + void updateValue(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec); virtual void setActiveUAS(UASInterface* uas); + void addSource(QObject* obj); /** @brief Removes a plot item by the action data */ void removeItemByAction(); @@ -149,6 +156,7 @@ protected: QMap maxValues; ///< The maximum value this variable is assumed to have QMap symmetric; ///< Draw the gauge / dial symmetric bool = yes QMap intValues; ///< Is the gauge value an integer? + QMap customNames; ///< Custom names for the data names QMap > goodRanges; ///< The range of good values QMap > critRanges; ///< The range of critical values double scalingFactor; ///< Factor used to scale all absolute values to screen coordinates diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 9a4f6ecf54fdeba8cd4fceae10c907683680495c..ec0a309c23e26d4c76c7a627c8f52f35634d0cd6 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -99,8 +99,8 @@ HSIDisplay::HSIDisplay(QWidget *parent) : laserFix(0), mavInitialized(false), bottomMargin(10.0f), - topMargin(12.0f), dragStarted(false), + topMargin(12.0f), leftDragStarted(false), mouseHasMoved(false), actionPending(false), diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 1b45015713163fe6ac0fd61f0c16106c6f31f635..51858292dbe5986adc7734e6a6b2777d6fec3d0c 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -91,9 +91,9 @@ MainWindow::MainWindow(QWidget *parent): currentStyle(QGC_MAINWINDOW_STYLE_INDOOR), aboutToCloseFlag(false), changingViewsFlag(false), + centerStackActionGroup(this), styleFileName(QCoreApplication::applicationDirPath() + "/style-indoor.css"), autoReconnect(false), - centerStackActionGroup(this), lowPowerMode(false) { hide(); @@ -427,14 +427,18 @@ void MainWindow::buildCommonWidgets() if (!headDown1DockWidget) { headDown1DockWidget = new QDockWidget(tr("Flight Display"), this); - headDown1DockWidget->setWidget( new HDDisplay(acceptList, "Flight Display", this) ); + HDDisplay* hdDisplay = new HDDisplay(acceptList, "Flight Display", this); + hdDisplay->addSource(mavlinkDecoder); + headDown1DockWidget->setWidget(hdDisplay); headDown1DockWidget->setObjectName("HEAD_DOWN_DISPLAY_1_DOCK_WIDGET"); addTool(headDown1DockWidget, tr("Flight Display"), Qt::RightDockWidgetArea); } if (!headDown2DockWidget) { headDown2DockWidget = new QDockWidget(tr("Actuator Status"), this); - headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Actuator Status", this) ); + HDDisplay* hdDisplay = new HDDisplay(acceptList2, "Actuator Status", this); + hdDisplay->addSource(mavlinkDecoder); + headDown2DockWidget->setWidget(hdDisplay); headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET"); addTool(headDown2DockWidget, tr("Actuator Status"), Qt::RightDockWidgetArea); } diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index eb99b89cbf4073a78d96c6e6250b82bce78dd24f..10733f4f837e1bb8cb797e1319ae85cae9b44df2 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/src/ui/QGCMAVLinkInspector.cc @@ -37,10 +37,11 @@ void QGCMAVLinkInspector::refreshView() // Ignore NULL values if (!msg) continue; // Update the tree view + QString messageName("%1 (%2 Hz, #%3)"); + messageName = messageName.arg(messageInfo[msg->msgid].name).arg(messagesHz.value(msg->msgid, 0), 2, 'f', 0).arg(msg->msgid); if (!treeWidgetItems.contains(msg->msgid)) { - QString messageName("%1 (#%2)"); - messageName = messageName.arg(messageInfo[msg->msgid].name).arg(msg->msgid); + QStringList fields; fields << messageName; QTreeWidgetItem* widget = new QTreeWidgetItem(fields); @@ -56,7 +57,10 @@ void QGCMAVLinkInspector::refreshView() ui->treeWidget->addTopLevelItem(widget); } + // Set Hz QTreeWidgetItem* message = treeWidgetItems.value(msg->msgid); + message->setFirstColumnSpanned(true); + message->setData(0, Qt::DisplayRole, QVariant(messageName)); for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i) { updateField(msg->msgid, i, message->child(i)); @@ -71,6 +75,25 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m // int filterValue = ui->systemComboBox()->value().toInt(); // if (filterValue != ) memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t)); + + float msgHz = 0.0f; + quint64 receiveTime = QGC::groundTimeMilliseconds(); + if (lastMessageUpdate.contains(message.msgid)) + { + msgHz = 1000.0/(double)(receiveTime - lastMessageUpdate.value(message.msgid)); + if (isinf(msgHz) || isnan(msgHz) || msgHz < 0.0f) + { + msgHz = 1; + } + //qDebug() << "DIFF:" << receiveTime - lastMessageUpdate.value(message.msgid); + float newHz = 0.001f*msgHz+0.999f*messagesHz.value(message.msgid, 1); + qDebug() << "HZ" << newHz; + messagesHz.insert(message.msgid, newHz); + } + + //qDebug() << "MSGHZ:" << messagesHz.value(message.msgid, 1000); + + lastMessageUpdate.insert(message.msgid, receiveTime); } QGCMAVLinkInspector::~QGCMAVLinkInspector() diff --git a/src/ui/QGCMAVLinkInspector.h b/src/ui/QGCMAVLinkInspector.h index 996d74d185ec548894f7f4ab5e8555a18b5cafc9..56e3f2d03513daee7f26a09bc5c9f0f9e2a1ec98 100644 --- a/src/ui/QGCMAVLinkInspector.h +++ b/src/ui/QGCMAVLinkInspector.h @@ -26,7 +26,8 @@ public slots: void refreshView(); protected: - QMap lastFieldUpdate; ///< Used to switch between highlight and non-highlighting color + QMap lastMessageUpdate; ///< Used to switch between highlight and non-highlighting color + QMap messagesHz; ///< Used to store update rate in Hz mavlink_message_t receivedMessages[256]; ///< Available / known messages QMap treeWidgetItems; ///< Available tree widget items QTimer updateTimer; ///< Only update at 1 Hz to not overload the GUI diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 0739b558d8225244c35ad9aa0d3c7083b9d78ca1..e721c67317159c23ea8d95a73799b90b619346c6 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -315,6 +315,7 @@ void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 { if (activePlot->isVisible(curve+unit)) { + if (usec == 0) usec = QGC::groundTimeMilliseconds(); if (logStartTime == 0) logStartTime = usec; qint64 time = usec - logStartTime; if (time < 0) time = 0; @@ -347,6 +348,7 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString& { if (activePlot->isVisible(curve+unit)) { + if (usec == 0) usec = QGC::groundTimeMilliseconds(); if (logStartTime == 0) logStartTime = usec; qint64 time = usec - logStartTime; if (time < 0) time = 0; @@ -391,6 +393,7 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString& { if (activePlot->isVisible(curve+unit)) { + if (usec == 0) usec = QGC::groundTimeMilliseconds(); if (logStartTime == 0) logStartTime = usec; qint64 time = usec - logStartTime; if (time < 0) time = 0; @@ -425,6 +428,7 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString& { if (activePlot->isVisible(curve+unit)) { + if (usec == 0) usec = QGC::groundTimeMilliseconds(); if (logStartTime == 0) logStartTime = usec; qint64 time = usec - logStartTime; if (time < 0) time = 0;