diff --git a/images/style-mission.css b/images/style-mission.css index 5831e88853c9a6d8cd922d936800b7b87686907d..96ac39109a194522f8a35e5d0f4da29d1bbcb2e1 100644 --- a/images/style-mission.css +++ b/images/style-mission.css @@ -49,15 +49,15 @@ border: 1px solid #777777; } QCheckBox::indicator:checked { - background-color: #555555; + background-color: #379AC3; } QCheckBox::indicator:checked:hover { - background-color: #555555; + background-color: #379AC3; } QCheckBox::indicator:checked:pressed { - background-color: #555555; + background-color: #379AC3; } QCheckBox::indicator:indeterminate:hover { @@ -182,17 +182,19 @@ QPushButton { max-height: 18px; border: 2px solid #4A4A4F; border-radius: 5px; - padding-left: 10px; - padding-right: 10px; + padding-left: 5px; + padding-right: 5px; background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #232228, stop: 1 #020208); } QPushButton:checked { background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #404040, stop: 1 #808080); + border: 2px solid #379AC3; } QPushButton:pressed { background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #bbbbbb, stop: 1 #b0b0b0); + border: 2px solid #379AC3; } QToolButton { @@ -207,17 +209,49 @@ QToolButton { QToolButton:checked { background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #090909, stop: 1 #353535); + border: 2px solid #379AC3; } QToolButton:pressed { background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #bbbbbb, stop: 1 #b0b0b0); + border: 2px solid #379AC3; } QToolTip { - background-color: #404040; + background-color: #090909; + border: 1px solid #379AC3; border-radius: 3px; + color: #DDDDDF; +} + +QMenu { + border: 1px solid #379AC3; } +QMenu::separator { + height: 1px; + background: #379AC3; + margin-top: 8px; + margin-bottom: 4px; + margin-left: 5px; + margin-right: 5px; + } + +QSlider::groove:horizontal { + border: 1px solid #999999; + height: 4px; /* the groove expands to the size of the slider by default. by giving it a height, it has a fixed size */ + background: qlineargradient(x1:0, y1:0, x2:0, y2:1, stop:0 #4A4A4F, stop:1 #4A4A4F); + margin: 2px 0; + } + + QSlider::handle:horizontal { + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #232228, stop: 1 #020208); + border: 2px solid #379AC3; + width: 18px; + margin: -5px 0; /* handle is placed by default on the contents rect of the groove. Expand outside the groove */ + border-radius: 3px; + } + QPushButton#forceLandButton { font-weight: bold; min-height: 30px; diff --git a/images/style-outdoor.css b/images/style-outdoor.css new file mode 100644 index 0000000000000000000000000000000000000000..d765aa3845c55194f210e51708257e9a3a5dc5ee --- /dev/null +++ b/images/style-outdoor.css @@ -0,0 +1,309 @@ +* { font-family: "Bitstream Vera Sans"; font: "Roman"; font-size: 12px; } +QWidget#colorIcon {} + +QWidget { +background-color: #F6F6F6; +color: #000000; +background-clip: border; +font-size: 11px; +} + +QGroupBox { +border: 1px solid #222216; +border-radius: 3px; +padding: 10px 0px 0px 0px; +margin-top: 1ex; /* leave space at the top for the title */ +} + +QCheckBox { +/*background-color: #252528;*/ +color: #222221; +} + +QCheckBox::indicator { + border: 1px solid #111111; + border-radius: 2px; + color: #222221; + width: 10px; + height: 10px; +} + +QLineEdit { +border: 1px solid #111111; + border-radius: 2px; +} + +QTextEdit { +border: 1px solid #111111; + border-radius: 2px; +} + +QPlainTextEdit { +border: 1px solid #111111; + border-radius: 2px; +} + +QComboBox { +border: 1px solid #111111; + border-radius: 2px; + } + + QCheckBox::indicator:checked { + background-color: #222222; + } + + QCheckBox::indicator:checked:hover { + background-color: #222222; + } + + QCheckBox::indicator:checked:pressed { + background-color: #222222; + } + + QCheckBox::indicator:indeterminate:hover { + image: url(:/images/checkbox_indeterminate_hover.png); + } + + QCheckBox::indicator:indeterminate:pressed { + image: url(:/images/checkbox_indeterminate_pressed.png); + } + + QGroupBox::title { + subcontrol-origin: margin; + subcontrol-position: top center; /* position at the top center */ + margin: 0 3px 0px 3px; + padding: 0 3px 0px 0px; + font: bold 8px; + color: #DDDDDF; + } + + QMainWindow::separator { + background: #090909; + width: 2px; /* when vertical */ + height: 2px; /* when horizontal */ + } + + QMainWindow::separator:hover { + background: white; + } + + QDockWidget { + border: 1px solid #32345E; + /* titlebar-close-icon: url(close.png); + titlebar-normal-icon: url(undock.png);*/ + } + + QDockWidget::title { + text-align: left; /* align the text to the left */ + background: lightgray; + padding-left: 5px; + } + + QDockWidget::close-button, QDockWidget::float-button { + border: 1px solid transparent; + background: darkgray; + padding: 0px; + } + + QDockWidget::close-button:hover, QDockWidget::float-button:hover { + background: gray; + } + + QDockWidget::close-button:pressed, QDockWidget::float-button:pressed { + padding: 1px -1px -1px 1px; + } + + + +QDockWidget::close-button, QDockWidget::float-button { + background-color: #181820; + color: #EEEEEE; +} + +QDockWidget::title { + text-align: left; + background: #121214; + color: #4A4A4F; + padding-left: 5px; + height: 10px; + border-bottom: 1px solid #222222; +} + +QSeparator { + color: #EEEEEE; + } + + +QSpinBox { + min-height: 14px; + max-height: 18px; + border: 1px solid #4A4A4F; + border-radius: 5px; +} + +QSpinBox::up-button { + subcontrol-origin: border; + subcontrol-position: top right; /* position at the top right corner */ + border-image: url(:/images/actions/go-up.svg) 1; + border-width: 1px; +} +QSpinBox::down-button { + subcontrol-origin: border; + subcontrol-position: bottom right; /* position at the top right corner */ + border-image: url(:/images/actions/go-down.svg) 1; + border-width: 1px; +} + +QDoubleSpinBox { + min-height: 14px; + max-height: 18px; + border: 1px solid #4A4A4F; + border-radius: 5px; +} + +QDoubleSpinBox::up-button { + subcontrol-origin: border; + subcontrol-position: top right; /* position at the top right corner */ + border-image: url(:/images/actions/go-up.svg) 1; + border-width: 1px; + max-width: 5px; +} +QDoubleSpinBox::down-button { + subcontrol-origin: border; + subcontrol-position: bottom right; /* position at the top right corner */ + border-image: url(:/images/actions/go-down.svg) 1; + border-width: 1px; + max-width: 5px; +} + +QPushButton { + font-weight: bold; + min-height: 18px; + max-height: 18px; + border: 2px solid #4A4A4F; + border-radius: 5px; + padding-left: 10px; + padding-right: 10px; + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #232228, stop: 1 #020208); +} + +QPushButton:checked { + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #404040, stop: 1 #808080); +} + +QPushButton:pressed { + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #bbbbbb, stop: 1 #b0b0b0); +} + +QToolButton { + font-weight: bold; + min-height: 16px; + min-width: 24px; + max-height: 18px; + border: 2px solid #4A4A4F; + border-radius: 5px; + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #232228, stop: 1 #020208); +} + +QToolButton:checked { + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #090909, stop: 1 #353535); +} + +QToolButton:pressed { + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #bbbbbb, stop: 1 #b0b0b0); +} + +QToolTip { + background-color: #404040; + border-radius: 3px; +} + +QPushButton#forceLandButton { + font-weight: bold; + min-height: 30px; + color: #000000; + background: qlineargradient(x1:0, y1:0, x2:0, y2:1, + stop:0 #ffee01, stop:1 #ae8f00) url("ICONDIR/control/emergency-button.png"); + background-clip: border; + border-width: 1px; + border-color: #222222; + border-radius: 5px; +} + +QPushButton:pressed#forceLandButton { + font-weight: bold; + min-height: 30px; + color: #000000; + background: qlineargradient(x1:0, y1:0, x2:0, y2:1, + stop:0 #bbaa00, stop:1 #a05b00) url("ICONDIR/control/emergency-button.png"); + background-clip: border; + border-width: 1px; + border-color: #222222; + border-radius: 5px; +} + +QPushButton#killButton { + font-weight: bold; + min-height: 30px; + color: #000000; + background: qlineargradient(x1:0, y1:0, x2:0, y2:1, + stop:0 #ffb917, stop:1 #b37300) url("ICONDIR/control/emergency-button.png"); + background-clip: border; + border-width: 1px; + border-color: #222222; + border-radius: 5px; +} + +QPushButton:pressed#killButton { + font-weight: bold; + min-height: 30px; + color: #000000; + background: qlineargradient(x1:0, y1:0, x2:0, y2:1, + stop:0 #bb8500, stop:1 #903000) url("ICONDIR/control/emergency-button.png"); + background-clip: border; + border-width: 1px; + border-color: #222222; + border-radius: 5px; +} + +QPushButton#controlButton { + min-height: 25px; + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #A0AE00, stop: 1 #909E00); +} + +QPushButton:checked#controlButton { + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #b76f11, stop: 1 #e1a718); +} + +QProgressBar { + border: 1px solid #4A4A4F; + border-radius: 4px; + text-align: center; + padding: 2px; + color: #DDDDDF; + background-color: #111118; +} + +QProgressBar:horizontal { + height: 9px; +} + +QProgressBar:vertical { + width: 9px; +} + +QProgressBar::chunk { + background-color: #3C7B9E; +} + +QProgressBar::chunk#batteryBar { + background-color: green; +} + +QProgressBar::chunk#speedBar { + background-color: yellow; +} + +QProgressBar::chunk#thrustBar { + background-color: orange; +} diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index d3be98e82ed0bcfce2c5965c290637039163134d..f9725737bc3e8d51732ea873746b5ebca0509925 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -83,6 +83,9 @@ macx { QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/qgroundcontrol.app/Contents/MacOs # Copy google earth starter file QMAKE_POST_LINK += && cp -f $$BASEDIR/images/earth.html $$TARGETDIR/qgroundcontrol.app/Contents/MacOs + # Copy CSS stylesheets + QMAKE_POST_LINK += && cp -f $$BASEDIR/images/style-mission.css $$TARGETDIR/qgroundcontrol.app/Contents/MacOs/style-indoor.css + QMAKE_POST_LINK += && cp -f $$BASEDIR/images/style-outdoor.css $$TARGETDIR/qgroundcontrol.app/Contents/MacOs # Copy model files #QMAKE_POST_LINK += && cp -f $$BASEDIR/models/*.dae $$TARGETDIR/qgroundcontrol.app/Contents/MacOs @@ -189,7 +192,7 @@ linux-g++ { DEFINES += QGC_OSG_ENABLED } - exists(/usr/include/osgEarth) | exists(/usr/local/include/osgEarth) { + exists(/usr/include/osgEarth):exists(/usr/include/osg) | exists(/usr/local/include/osgEarth):exists(/usr/include/osg) { message("Building support for osgEarth") DEPENDENCIES_PRESENT += osgearth # Include osgEarth libraries diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index bcbcd69792443382a521b5f367d7735201370b6e..81789c578fd371c80201dc868715b0724ecb4b6a 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -153,7 +153,8 @@ FORMS += src/ui/MainWindow.ui \ src/ui/uas/QGCUnconnectedInfoWidget.ui \ src/ui/designer/QGCToolWidget.ui \ src/ui/designer/QGCParamSlider.ui \ - src/ui/designer/QGCActionButton.ui + src/ui/designer/QGCActionButton.ui \ + src/ui/QGCMAVLinkLogPlayer.ui INCLUDEPATH += src \ src/ui \ @@ -259,7 +260,8 @@ HEADERS += src/MG.h \ src/ui/designer/QGCToolWidget.h \ src/ui/designer/QGCParamSlider.h \ src/ui/designer/QGCActionButton.h \ - src/ui/designer/QGCToolWidgetItem.h + src/ui/designer/QGCToolWidgetItem.h \ + src/ui/QGCMAVLinkLogPlayer.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|win32-msvc2008: { @@ -380,7 +382,8 @@ SOURCES += src/main.cc \ src/ui/designer/QGCToolWidget.cc \ src/ui/designer/QGCParamSlider.cc \ src/ui/designer/QGCActionButton.cc \ - src/ui/designer/QGCToolWidgetItem.cc + src/ui/designer/QGCToolWidgetItem.cc \ + src/ui/QGCMAVLinkLogPlayer.cc macx|win32-msvc2008: { SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/Core.cc b/src/Core.cc index 3e5602704bb8413801254bca0d9ac592e3639df0..04645abcaadc8b7f4220cb803c96b163dac369ce 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -40,6 +40,7 @@ This file is part of the QGROUNDCONTROL project #include #include "configuration.h" +#include "QGC.h" #include "Core.h" #include "MG.h" #include "MainWindow.h" @@ -69,6 +70,26 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) this->setOrganizationName(QLatin1String("OPENMAV")); this->setOrganizationDomain("http://qgroundcontrol.org"); + // Check application settings + // clear them if they mismatch + // QGC then falls back to default + QSettings settings; + settings.sync(); + if (settings.contains("QGC_APPLICATION_VERSION_INT")) + { + QString qgcVersion = settings.value("QGC_APPLICATION_VERSION").toString(); + if (qgcVersion != QGC_APPLICATION_VERSION) + { + settings.clear(); + } + } + else + { + // If application version is not set, clear settings anyway + settings.clear(); + } + settings.sync(); + // Show splash screen QPixmap splashImage(":images/splash.png"); QSplashScreen* splashScreen = new QSplashScreen(splashImage, Qt::WindowStaysOnTopHint); diff --git a/src/LogCompressor.cc b/src/LogCompressor.cc index 0d4ebdb9f603042e2ce36fceabe4d684df77706d..84acff29396852c96cb435835d1b6d09c3e52436 100644 --- a/src/LogCompressor.cc +++ b/src/LogCompressor.cc @@ -214,7 +214,7 @@ void LogCompressor::run() } } - if (index % (dataLines/100) == 0) emit logProcessingStatusChanged(tr("Log compressor: Processed %1% of %2 lines").arg(index/(float)dataLines*100, 0, 'f', 2).arg(dataLines)); + if (dataLines > 100) if (index % (dataLines/100) == 0) emit logProcessingStatusChanged(tr("Log compressor: Processed %1% of %2 lines").arg(index/(float)dataLines*100, 0, 'f', 2).arg(dataLines)); if (!failed) { diff --git a/src/QGC.cc b/src/QGC.cc index 14da6ac60b88b0a8c3067cd9d8d8c8e7846553a9..087c4885742a48ca3efdc1ad61705f1ea64e4701 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -34,4 +34,9 @@ quint64 groundTimeUsecs() return static_cast(microseconds + (time.time().msec()*1000)); } +int applicationVersion() +{ + return APPLICATIONVERSION; +} + } diff --git a/src/QGC.h b/src/QGC.h index e21a17d105b80dc8839f1bd2f83cb0eabc8fbaf6..ebc390e7f621789af80d5e857c159ba20d314f17 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -3,6 +3,9 @@ #include #include +#include + +#include "configuration.h" namespace QGC { @@ -15,9 +18,39 @@ namespace QGC /** @brief Get the current ground time in microseconds */ quint64 groundTimeUsecs(); + int applicationVersion(); + + + + class SLEEP : public QThread + { + public: + /** + * @brief Set a thread to sleep for seconds + * @param s time in seconds to sleep + **/ + static void sleep(unsigned long s) + { + QThread::sleep(s); + } + /** + * @brief Set a thread to sleep for milliseconds + * @param ms time in milliseconds to sleep + **/ + static void msleep(unsigned long ms) + { + QThread::msleep(ms); + } + /** + * @brief Set a thread to sleep for microseconds + * @param us time in microseconds to sleep + **/ + static void usleep(unsigned long us) + { + QThread::usleep(us); + } + }; - const QString APPNAME = "QGROUNDCONTROL"; - const QString COMPANYNAME = "OPENMAV"; } #define QGC_EVENTLOOP_DEBUG 0 diff --git a/src/comm/AS4Protocol.cc b/src/comm/AS4Protocol.cc index 47e4ac2d9f7fa1a9ff8381a8ed17e0efc3c75d2b..3f8a02031f6ee2e2e040c132108051bd11554d99 100644 --- a/src/comm/AS4Protocol.cc +++ b/src/comm/AS4Protocol.cc @@ -43,6 +43,8 @@ This file is part of the QGROUNDCONTROL project #include #include +#include "QGC.h" + AS4Protocol::AS4Protocol() { @@ -84,6 +86,10 @@ AS4Protocol::~AS4Protocol() void AS4Protocol::run() { + forever + { + QGC::SLEEP::msleep(5000); + } } diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 39bc47bc1d7279c6f3f9e3b56dbd7ff15803f207..754871f43e31cd77691d00c9a3c925f85afa5184 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -1,24 +1,4 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - +/*=================================================================== ======================================================================*/ /** @@ -59,7 +39,7 @@ MAVLinkProtocol::MAVLinkProtocol() : heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), m_heartbeatsEnabled(false), m_loggingEnabled(false), - m_logfile(NULL), + m_logfile(new QFile(QCoreApplication::applicationDirPath()+"/mavlink_packetlog.mavlink")), m_enable_version_check(true), versionMismatchIgnore(false) { @@ -86,6 +66,7 @@ MAVLinkProtocol::~MAVLinkProtocol() { if (m_logfile) { + m_logfile->flush(); m_logfile->close(); delete m_logfile; } @@ -95,11 +76,15 @@ MAVLinkProtocol::~MAVLinkProtocol() void MAVLinkProtocol::run() { + forever + { + QGC::SLEEP::msleep(5000); + } } QString MAVLinkProtocol::getLogfileName() { - return QCoreApplication::applicationDirPath()+"/mavlink.log"; + return m_logfile->fileName(); } /** @@ -124,12 +109,18 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Log data if (m_loggingEnabled) { - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - quint64 time = MG::TIME::getGroundTimeNowUsecs(); + int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64); + uint8_t buf[len]; + quint64 time = QGC::groundTimeUsecs(); memcpy(buf, (void*)&time, sizeof(quint64)); + // int packetlen = +// quint64 checktime = *((quint64*)buf); +// qDebug() << "TIME" << time << "CHECKTIME:" << checktime; mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message); - m_logfile->write((const char*) buf); - qDebug() << "WROTE LOGFILE"; + QByteArray b((const char*)buf, len); + //int packetlen = + if(m_logfile->write(b) < MAVLINK_MAX_PACKET_LEN+sizeof(quint64)) qDebug() << "WRITING TO LOG FAILED!"; + //qDebug() << "WROTE LOGFILE"; } // ORDER MATTERS HERE! @@ -170,7 +161,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) if (!versionMismatchIgnore) { emit protocolStatusMessage(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"), - tr("It is unsafe to use different MAVLink versions. QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION)); + tr("It is unsafe to use different MAVLink versions. QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION)); versionMismatchIgnore = true; } @@ -189,37 +180,37 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) break; case MAV_AUTOPILOT_PIXHAWK: { - // Fixme differentiate between quadrotor and coaxial here - PxQuadMAV* mav = new PxQuadMAV(this, message.sysid); - // Connect this robot to the UAS object - // it is IMPORTANT here to use the right object type, - // else the slot of the parent object is called (and thus the special - // packets never reach their goal) - connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); - uas = mav; - } + // Fixme differentiate between quadrotor and coaxial here + PxQuadMAV* mav = new PxQuadMAV(this, message.sysid); + // Connect this robot to the UAS object + // it is IMPORTANT here to use the right object type, + // else the slot of the parent object is called (and thus the special + // packets never reach their goal) + connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } break; case MAV_AUTOPILOT_SLUGS: { - SlugsMAV* mav = new SlugsMAV(this, message.sysid); - // Connect this robot to the UAS object - // it is IMPORTANT here to use the right object type, - // else the slot of the parent object is called (and thus the special - // packets never reach their goal) - connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); - uas = mav; - } + SlugsMAV* mav = new SlugsMAV(this, message.sysid); + // Connect this robot to the UAS object + // it is IMPORTANT here to use the right object type, + // else the slot of the parent object is called (and thus the special + // packets never reach their goal) + connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } break; case MAV_AUTOPILOT_ARDUPILOTMEGA: { - ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid); - // Connect this robot to the UAS object - // it is IMPORTANT here to use the right object type, - // else the slot of the parent object is called (and thus the special - // packets never reach their goal) - connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); - uas = mav; - } + ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid); + // Connect this robot to the UAS object + // it is IMPORTANT here to use the right object type, + // else the slot of the parent object is called (and thus the special + // packets never reach their goal) + connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } break; default: uas = new UAS(this, message.sysid); @@ -390,23 +381,43 @@ void MAVLinkProtocol::enableHeartbeats(bool enabled) void MAVLinkProtocol::enableLogging(bool enabled) { - if (enabled && !m_loggingEnabled) + bool changed = false; + if (enabled != m_loggingEnabled) changed = true; + + if (enabled) { - m_logfile = new QFile(getLogfileName()); - m_logfile->open(QIODevice::WriteOnly | QIODevice::Append); + if (m_logfile->isOpen()) + { + m_logfile->flush(); + m_logfile->close(); + } + if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append)) + { + emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot log to the file %1, please choose a different file.").arg(m_logfile->fileName())); + qDebug() << "OPENING LOGFILE FAILED!"; + } } - else + else if (!enabled) { - m_logfile->close(); - delete m_logfile; - m_logfile = NULL; + m_logfile->flush(); + m_logfile->close(); } m_loggingEnabled = enabled; + if (changed) emit loggingChanged(enabled); +} + +void MAVLinkProtocol::setLogfileName(const QString& filename) +{ + m_logfile->flush(); + m_logfile->close(); + m_logfile->setFileName(filename); + enableLogging(m_loggingEnabled); } void MAVLinkProtocol::enableVersionCheck(bool enabled) { m_enable_version_check = enabled; + emit versionCheckChanged(enabled); } bool MAVLinkProtocol::heartbeatsEnabled(void) diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 76171e262d954e9d0f827fe5e87364e0aaeb8a2e..af1ba184078a927a3b468958c88281ec3539ed9b 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -70,8 +70,10 @@ public: bool loggingEnabled(void); /** @brief Get protocol version check state */ bool versionCheckEnabled(void); + /** @brief Get the protocol version */ + int getVersion() { return MAVLINK_VERSION; } /** @brief Get the name of the packet log file */ - static QString getLogfileName(); + QString getLogfileName(); public slots: /** @brief Receive bytes from a communication interface */ @@ -89,6 +91,9 @@ public slots: /** @brief Enable/disable binary packet logging */ void enableLogging(bool enabled); + /** @brief Set log file name */ + void setLogfileName(const QString& filename); + /** @brief Enable / disable version check */ void enableVersionCheck(bool enabled); diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index d697dabd3090b15af89253e459530e81a9fba427..952bffe2e6bd61b8de4496dffdec3059109bc887 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -97,8 +97,8 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile LinkManager::instance()->add(this); // Open packet log - mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName()); - mavlinkLogFile->open(QIODevice::ReadOnly); + mavlinkLogFile = new QFile(); + //mavlinkLogFile->open(QIODevice::ReadOnly); } MAVLinkSimulationLink::~MAVLinkSimulationLink() @@ -141,7 +141,7 @@ void MAVLinkSimulationLink::run() } last = MG::TIME::getGroundTimeNow(); } - MG::SLEEP::msleep(2); + QGC::SLEEP::msleep(3); } } @@ -375,12 +375,21 @@ void MAVLinkSimulationLink::mainloop() { rate10hzCounter = 1; - + float lastX = x; + float lastY = y; + float lastZ = z; + float hackDt = 0.1f; // 100 ms // Move X Position - x = 12.0*sin(((double)circleCounter)/100.0); - y = 5.0*cos(((double)circleCounter)/100.0); - z = 1.8 + 1.2*sin(((double)circleCounter)/60.0); + x = 12.0*sin(((double)circleCounter)/200.0); + y = 5.0*cos(((double)circleCounter)/200.0); + z = 1.8 + 1.2*sin(((double)circleCounter)/200.0); + + float xSpeed = (x - lastX)/hackDt; + float ySpeed = (y - lastY)/hackDt; + float zSpeed = (z - lastZ)/hackDt; + + circleCounter++; @@ -402,7 +411,7 @@ void MAVLinkSimulationLink::mainloop() streampointer += bufferlength; // Send back new position - mavlink_msg_local_position_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), 0, 0, 0); + mavlink_msg_local_position_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), xSpeed, ySpeed, zSpeed); bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); @@ -416,14 +425,14 @@ void MAVLinkSimulationLink::mainloop() // streampointer += bufferlength; // GLOBAL POSITION - mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); + mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, xSpeed, ySpeed, zSpeed); bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; // GLOBAL POSITION VEHICLE 2 - mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); + mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed); bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); @@ -447,7 +456,7 @@ void MAVLinkSimulationLink::mainloop() chan.chan5_raw = (chan.chan3_raw + chan.chan4_raw) / 2.0f; chan.chan6_raw = (chan.chan3_raw + chan.chan2_raw) / 2.0f; chan.chan7_raw = (chan.chan4_raw + chan.chan2_raw) / 2.0f; - chan.chan8_raw = (chan.chan6_raw + chan.chan2_raw) / 2.0f; + chan.chan8_raw = 0; chan.rssi = 100; messageSize = mavlink_msg_rc_channels_raw_encode(systemId, componentId, &msg, &chan); // Allocate buffer with packet data diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index cca446668350a87461cf71bd55112a4ab5c1afcd..34b830fe66579da12eb88d5dad3eda3255162f54 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -464,7 +464,7 @@ bool MAVLinkXMLParser::generate() // Add field to C structure cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); // Add pack line to message_xx_pack function - packLines += QString("\ti += put_array_by_index((int8_t*)%1, sizeof(%2)*%3, i, msg->payload); // %4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText); + packLines += QString("\ti += put_array_by_index((const int8_t*)%1, sizeof(%2)*%3, i, msg->payload); // %4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText); // Add decode function for this type decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index ee320e7363d3419c8faf027001e1f967de249e65..c59aad2654050744ed1cf60b02f4518a44925d37 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -108,8 +108,9 @@ void SerialLink::loadSettings() setPortName(settings.value("SERIALLINK_COMM_PORT").toString()); setBaudRateType(settings.value("SERIALLINK_COMM_BAUD").toInt()); setParityType(settings.value("SERIALLINK_COMM_PARITY").toInt()); - setStopBitsType(settings.value("SERIALLINK_COMM_STOPBITS").toInt()); - setDataBitsType(settings.value("SERIALLINK_COMM_DATABITS").toInt()); + setStopBits(settings.value("SERIALLINK_COMM_STOPBITS").toInt()); + setDataBits(settings.value("SERIALLINK_COMM_DATABITS").toInt()); + setFlowType(settings.value("SERIALLINK_COMM_FLOW_CONTROL").toInt()); } } @@ -120,8 +121,9 @@ void SerialLink::writeSettings() settings.setValue("SERIALLINK_COMM_PORT", this->porthandle); settings.setValue("SERIALLINK_COMM_BAUD", getBaudRateType()); settings.setValue("SERIALLINK_COMM_PARITY", getParityType()); - settings.setValue("SERIALLINK_COMM_STOPBITS", getStopBitsType()); - settings.setValue("SERIALLINK_COMM_DATABITS", getDataBitsType()); + settings.setValue("SERIALLINK_COMM_STOPBITS", getStopBits()); + settings.setValue("SERIALLINK_COMM_DATABITS", getDataBits()); + settings.setValue("SERIALLINK_COMM_FLOW_CONTROL", getFlowType()); settings.sync(); } @@ -316,10 +318,10 @@ bool SerialLink::hardwareConnect() { emit connected(); emit connected(true); - - writeSettings(); } + writeSettings(); + return connectionUp; } @@ -531,6 +533,48 @@ int SerialLink::getStopBitsType() return stopBits; } +int SerialLink::getDataBits() +{ + int ret; + switch (dataBits) + { + case DATA_5: + ret = 5; + break; + case DATA_6: + ret = 6; + break; + case DATA_7: + ret = 7; + break; + case DATA_8: + ret = 8; + break; + default: + ret = 0; + break; + } + return ret; +} + +int SerialLink::getStopBits() +{ + int ret; + switch (stopBits) + { + case STOP_1: + ret = 1; + break; + case STOP_2: + ret = 2; + break; + default: + ret = 0; + break; + } + return ret; +} + bool SerialLink::setPortName(QString portName) { if(portName.trimmed().length() > 0) @@ -845,8 +889,7 @@ bool SerialLink::setParityType(int parity) } -// FIXME Works not as anticipated by user! -bool SerialLink::setDataBitsType(int dataBits) +bool SerialLink::setDataBits(int dataBits) { bool accepted = true; @@ -879,12 +922,12 @@ bool SerialLink::setDataBitsType(int dataBits) return accepted; } -// FIXME WORKS NOT AS ANTICIPATED BY USER! -bool SerialLink::setStopBitsType(int stopBits) +bool SerialLink::setStopBits(int stopBits) { bool reconnect = false; bool accepted = true; - if(isConnected()) { + if(isConnected()) + { disconnect(); reconnect = true; } @@ -907,3 +950,51 @@ bool SerialLink::setStopBitsType(int stopBits) if(reconnect) connect(); return accepted; } + +bool SerialLink::setDataBitsType(int dataBits) +{ + bool reconnect = false; + bool accepted = false; + + if (isConnected()) + { + disconnect(); + reconnect = true; + } + + if (dataBits >= (int)DATA_5 && dataBits <= (int)DATA_8) + { + DataBitsType newBits = (DataBitsType) dataBits; + + port->setDataBits(newBits); + if(reconnect) + { + connect(); + } + accepted = true; + } + + return accepted; +} + +bool SerialLink::setStopBitsType(int stopBits) +{ + bool reconnect = false; + bool accepted = false; + if(isConnected()) + { + disconnect(); + reconnect = true; + } + + if (stopBits >= (int)STOP_1 && dataBits <= (int)STOP_2) + { + StopBitsType newBits = (StopBitsType) stopBits; + + port->setStopBits(newBits); + accepted = true; + } + + if(reconnect) connect(); + return accepted; +} diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index 3bac202b8cf753e0510663815de76b464dbbfc17..b352ad9329d2daf1976f556520416d5d54dda797 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -74,6 +74,10 @@ public: */ QString getName(); int getBaudRate(); + int getDataBits(); + int getStopBits(); + + // ENUM values int getBaudRateType(); int getFlowType(); int getParityType(); @@ -103,6 +107,10 @@ public: public slots: bool setPortName(QString portName); bool setBaudRate(int rate); + bool setDataBits(int dataBits); + bool setStopBits(int stopBits); + + // Set ENUM values bool setBaudRateType(int rateIndex); bool setFlowType(int flow); bool setParityType(int parity); diff --git a/src/comm/SerialLinkInterface.h b/src/comm/SerialLinkInterface.h index 49b5335f9811db78cd9873c505e9b14e0ed89f44..ff16a66b10d672b0539d9571c2924af0e4bf01ca 100644 --- a/src/comm/SerialLinkInterface.h +++ b/src/comm/SerialLinkInterface.h @@ -43,6 +43,9 @@ class SerialLinkInterface : public LinkInterface { public: virtual QString getPortName() = 0; virtual int getBaudRate() = 0; + virtual int getDataBits() = 0; + virtual int getStopBits() = 0; + virtual int getBaudRateType() = 0; virtual int getFlowType() = 0; virtual int getParityType() = 0; diff --git a/src/comm/SerialSimulationLink.cc b/src/comm/SerialSimulationLink.cc index e2e2e927b83dd47421b7ea57d6c6a27557d2f539..b769f57db763d43606a5f1f62b93acefb2927acd 100644 --- a/src/comm/SerialSimulationLink.cc +++ b/src/comm/SerialSimulationLink.cc @@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include "QGC.h" /** * Create a simulated link. This link is connected to an input and output file. @@ -92,6 +93,10 @@ void SerialSimulationLink::run() msleep(rate); }*/ + forever + { + QGC::SLEEP::msleep(5000); + } } void SerialSimulationLink::enableLoopBackMode(SerialLink* loop) diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 0574c1eff8a90a4b2da323fe4c2e46d19406c1a4..372886020b71be2fee948770ac36b3a14804d1b8 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -35,7 +35,7 @@ This file is part of the QGROUNDCONTROL project #include #include "UDPLink.h" #include "LinkManager.h" -#include "MG.h" +#include "QGC.h" //#include UDPLink::UDPLink(QHostAddress host, quint16 port) @@ -49,7 +49,7 @@ UDPLink::UDPLink(QHostAddress host, quint16 port) // Set unique ID and add link to the list of links this->id = getNextLinkId(); - this->name = tr("UDP link ") + QString::number(getId()); + this->name = tr("UDP Link (port:%1)").arg(14550); LinkManager::instance()->add(this); } @@ -64,6 +64,10 @@ UDPLink::~UDPLink() **/ void UDPLink::run() { + forever + { + QGC::SLEEP::msleep(5000); + } } void UDPLink::setAddress(QString address) @@ -242,7 +246,7 @@ bool UDPLink::connect() if (connectState) { emit connected(); - connectionStartTime = MG::TIME::getGroundTimeNow(); + connectionStartTime = QGC::groundTimeUsecs()/1000; } start(HighPriority); @@ -281,7 +285,7 @@ qint64 UDPLink::getNominalDataRate() { qint64 UDPLink::getTotalUpstream() { statisticsMutex.lock(); - qint64 totalUpstream = bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); + qint64 totalUpstream = bitsSentTotal / ((QGC::groundTimeUsecs()/1000 - connectionStartTime) / 1000); statisticsMutex.unlock(); return totalUpstream; } @@ -304,7 +308,7 @@ qint64 UDPLink::getBitsReceived() { qint64 UDPLink::getTotalDownstream() { statisticsMutex.lock(); - qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); + qint64 totalDownstream = bitsReceivedTotal / ((QGC::groundTimeUsecs()/1000 - connectionStartTime) / 1000); statisticsMutex.unlock(); return totalDownstream; } diff --git a/src/configuration.h b/src/configuration.h index 64a17c93f3cec5848b3f58224680536fb926dd5a..c91fae7efbcffd00cab6f9ef7ecefedc5c0fa230 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -2,6 +2,7 @@ #define CONFIGURATION_H #include "mavlink.h" +#include /** @brief Polling interval in ms */ #ifdef MAVLINK_ENABLED_SLUGS @@ -16,6 +17,14 @@ #define WITH_TEXT_TO_SPEECH 1 #define QGC_APPLICATION_NAME "QGroundControl" -#define QGC_APPLICATION_VERSION "v. 0.8.0 (Beta)" +#define QGC_APPLICATION_VERSION "v. 0.8.1 (Alpha)" + +namespace QGC + +{ + const QString APPNAME = "QGROUNDCONTROL"; + const QString COMPANYNAME = "OPENMAV"; + const int APPLICATIONVERSION = 80; // 0.8.0 +} #endif // CONFIGURATION_H diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index 8fb7618589efd8b9fb2f598dc7f56063917ad216..023c97d43966188b151dca7b9e539d7adbf13cd7 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -36,6 +36,7 @@ This file is part of the PIXHAWK project #include #include "UAS.h" #include "UASManager.h" +#include "QGC.h" /** * The coordinate frame of the joystick axis is the aeronautical frame like shown on this image: @@ -276,7 +277,7 @@ void JoystickInput::run() } // Sleep, update rate of joystick is approx. 50 Hz (1000 ms / 50 = 20 ms) - MG::SLEEP::msleep(20); + QGC::SLEEP::msleep(20); } diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index bde4e9e52b6be86f8d5796e961ad7abc535a7999..f23b0d16fbc2088c3b1b9ef6df8d6efbd85fff79 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -60,8 +60,8 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_raw_aux_t raw; mavlink_msg_raw_aux_decode(&message, &raw); quint64 time = getUnixTime(0); - emit valueChanged(uasId, "Pressure", raw.baro, time); - emit valueChanged(uasId, "Temperature", raw.temp, time); + emit valueChanged(uasId, "Pressure", "raw", raw.baro, time); + emit valueChanged(uasId, "Temperature", "raw", raw.temp, time); } break; case MAVLINK_MSG_ID_PATTERN_DETECTED: @@ -110,12 +110,12 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_vision_position_estimate_decode(&message, &pos); quint64 time = getUnixTime(pos.usec); //emit valueChanged(uasId, "vis. time", pos.usec, time); - emit valueChanged(uasId, "vis. roll", pos.roll, time); - emit valueChanged(uasId, "vis. pitch", pos.pitch, time); - emit valueChanged(uasId, "vis. yaw", pos.yaw, time); - emit valueChanged(uasId, "vis. x", pos.x, time); - emit valueChanged(uasId, "vis. y", pos.y, time); - emit valueChanged(uasId, "vis. z", pos.z, time); + emit valueChanged(uasId, "vis. roll", "rad", pos.roll, time); + emit valueChanged(uasId, "vis. pitch", "rad", pos.pitch, time); + emit valueChanged(uasId, "vis. yaw", "rad", pos.yaw, time); + emit valueChanged(uasId, "vis. x", "m", pos.x, time); + emit valueChanged(uasId, "vis. y", "m", pos.y, time); + emit valueChanged(uasId, "vis. z", "m", pos.z, time); } break; case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: @@ -124,12 +124,12 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_vicon_position_estimate_decode(&message, &pos); quint64 time = getUnixTime(pos.usec); //emit valueChanged(uasId, "vis. time", pos.usec, time); - emit valueChanged(uasId, "vicon roll", pos.roll, time); - emit valueChanged(uasId, "vicon pitch", pos.pitch, time); - emit valueChanged(uasId, "vicon yaw", pos.yaw, time); - emit valueChanged(uasId, "vicon x", pos.x, time); - emit valueChanged(uasId, "vicon y", pos.y, time); - emit valueChanged(uasId, "vicon z", pos.z, time); + emit valueChanged(uasId, "vicon roll", "rad", pos.roll, time); + emit valueChanged(uasId, "vicon pitch", "rad", pos.pitch, time); + emit valueChanged(uasId, "vicon yaw", "rad", pos.yaw, time); + emit valueChanged(uasId, "vicon x", "m", pos.x, time); + emit valueChanged(uasId, "vicon y", "m", pos.y, time); + emit valueChanged(uasId, "vicon z", "m", pos.z, time); emit localPositionChanged(this, pos.x, pos.y, pos.z, time); } break; @@ -143,7 +143,7 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count); emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count); emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count); - emit UAS::valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "Load", "%", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); } break; case MAVLINK_MSG_ID_CONTROL_STATUS: diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 39882258ea5bacfba6d44d239abd31752fb35c1e..c0510dbc7aa3e6521f347a43224eafc41c096682 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -181,7 +181,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit statusChanged(this, uasState, stateDescription); emit statusChanged(this->status); emit loadChanged(this,state.load/10.0f); - emit UAS::valueChanged(uasId, "Load", ((float)state.load)/1000.0f, MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "Load", "%", ((float)state.load)/1000.0f, MG::TIME::getGroundTimeNow()); stateAudio = " changed status to " + uasState; } @@ -279,15 +279,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_raw_imu_decode(&message, &raw); quint64 time = getUnixTime(raw.usec); - emit valueChanged(uasId, "Accel. X", raw.xacc, time); - emit valueChanged(uasId, "Accel. Y", raw.yacc, time); - emit valueChanged(uasId, "Accel. Z", raw.zacc, time); - emit valueChanged(uasId, "Gyro Phi", static_cast(raw.xgyro), time); - emit valueChanged(uasId, "Gyro Theta", static_cast(raw.ygyro), time); - emit valueChanged(uasId, "Gyro Psi", static_cast(raw.zgyro), time); - emit valueChanged(uasId, "Mag. X", raw.xmag, time); - emit valueChanged(uasId, "Mag. Y", raw.ymag, time); - emit valueChanged(uasId, "Mag. Z", raw.zmag, time); + emit valueChanged(uasId, "accel x", "raw", raw.xacc, time); + emit valueChanged(uasId, "accel y", "raw", raw.yacc, time); + emit valueChanged(uasId, "accel z", "raw", raw.zacc, time); + emit valueChanged(uasId, "gyro roll", "raw", static_cast(raw.xgyro), time); + emit valueChanged(uasId, "gyro pitch", "raw", static_cast(raw.ygyro), time); + emit valueChanged(uasId, "gyro yaw", "raw", static_cast(raw.zgyro), time); + emit valueChanged(uasId, "mag x", "raw", raw.xmag, time); + emit valueChanged(uasId, "mag y", "raw", raw.ymag, time); + emit valueChanged(uasId, "mag z", "raw", raw.zmag, time); } break; case MAVLINK_MSG_ID_ATTITUDE: @@ -303,20 +303,37 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); // emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); // emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); - emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); - emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); - emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); - emit valueChanged(uasId, "rollspeed IMU", attitude.rollspeed, time); - emit valueChanged(uasId, "pitchspeed IMU", attitude.pitchspeed, time); - emit valueChanged(uasId, "yawspeed IMU", attitude.yawspeed, time); + emit valueChanged(uasId, "roll", "rad", mavlink_msg_attitude_get_roll(&message), time); + emit valueChanged(uasId, "pitch", "rad", mavlink_msg_attitude_get_pitch(&message), time); + emit valueChanged(uasId, "yaw", "rad", mavlink_msg_attitude_get_yaw(&message), time); + emit valueChanged(uasId, "rollspeed", "rad/s", attitude.rollspeed, time); + emit valueChanged(uasId, "pitchspeed", "rad/s", attitude.pitchspeed, time); + emit valueChanged(uasId, "yawspeed", "rad/s", attitude.yawspeed, time); // Emit in angles - emit valueChanged(uasId, "roll (deg)", (attitude.roll/M_PI)*180.0, time); - emit valueChanged(uasId, "pitch (deg)", (attitude.pitch/M_PI)*180.0, time); - emit valueChanged(uasId, "yaw (deg)", (attitude.yaw/M_PI)*180.0, time); - emit valueChanged(uasId, "roll V (deg/s)", (attitude.rollspeed/M_PI)*180.0, time); - emit valueChanged(uasId, "pitch V (deg/s)", (attitude.pitchspeed/M_PI)*180.0, time); - emit valueChanged(uasId, "yaw V (deg/s)", (attitude.yawspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "roll", "deg", (attitude.roll/M_PI)*180.0, time); + emit valueChanged(uasId, "pitch", "deg", (attitude.pitch/M_PI)*180.0, time); + + emit valueChanged(uasId, "rollspeed", "deg/s", (attitude.rollspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "pitchspeed", "deg/s", (attitude.pitchspeed/M_PI)*180.0, time); + + // Force yaw to 180 deg range + double yaw = ((attitude.yaw/M_PI)*180.0); + double sign = 1.0; + if (yaw < 0) + { + sign = -1.0; + yaw = -yaw; + } + while (yaw > 180.0) + { + yaw -= 180.0; + } + + yaw *= sign; + + emit valueChanged(uasId, "yaw", "deg", yaw, time); + emit valueChanged(uasId, "yawspeed", "deg/s", (attitude.yawspeed/M_PI)*180.0, time); emit attitudeChanged(this, attitude.roll, attitude.pitch, attitude.yaw, time); } @@ -331,12 +348,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) localX = pos.x; localY = pos.y; localZ = pos.z; - emit valueChanged(uasId, "x", pos.x, time); - emit valueChanged(uasId, "y", pos.y, time); - emit valueChanged(uasId, "z", pos.z, time); - emit valueChanged(uasId, "Vx", pos.vx, time); - emit valueChanged(uasId, "Vy", pos.vy, time); - emit valueChanged(uasId, "Vz", pos.vz, time); + emit valueChanged(uasId, "x", "m", pos.x, time); + emit valueChanged(uasId, "y", "m", pos.y, time); + emit valueChanged(uasId, "z", "m", pos.z, time); + emit valueChanged(uasId, "x speed", "m/s", pos.vx, time); + emit valueChanged(uasId, "y speed", "m/s", pos.vy, time); + emit valueChanged(uasId, "z speed", "m/s", pos.vz, time); emit localPositionChanged(this, pos.x, pos.y, pos.z, time); emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); @@ -366,12 +383,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) speedX = pos.vx/100.0; speedY = pos.vy/100.0; speedZ = pos.vz/100.0; - emit valueChanged(uasId, "lat", pos.lat, time); - emit valueChanged(uasId, "lon", pos.lon, time); - emit valueChanged(uasId, "alt", pos.alt, time); - emit valueChanged(uasId, "g-vx", speedX, time); - emit valueChanged(uasId, "g-vy", speedY, time); - emit valueChanged(uasId, "g-vz", speedZ, time); + emit valueChanged(uasId, "latitude", "deg", latitude, time); + emit valueChanged(uasId, "longitude", "deg", longitude, time); + emit valueChanged(uasId, "altitude", "m", altitude, time); + emit valueChanged(uasId, "gps x speed", "m/s", speedX, time); + emit valueChanged(uasId, "gps y speed", "m/s", speedY, time); + emit valueChanged(uasId, "gps z speed", "m/s", speedZ, time); emit globalPositionChanged(this, longitude, latitude, altitude, time); emit speedChanged(this, speedX, speedY, speedZ, time); // Set internal state @@ -397,8 +414,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // quint64 time = getUnixTime(pos.usec); quint64 time = MG::TIME::getGroundTimeNow(); - emit valueChanged(uasId, "lat", pos.lat, time); - emit valueChanged(uasId, "lon", pos.lon, time); + emit valueChanged(uasId, "latitude", "deg", pos.lat, time); + emit valueChanged(uasId, "longitude", "deg", pos.lon, time); if (pos.fix_type > 0) { @@ -411,13 +428,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) alt = 0; emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE"); } - emit valueChanged(uasId, "alt", pos.alt, time); + emit valueChanged(uasId, "altitude", "m", pos.alt, time); // Smaller than threshold and not NaN if (pos.v < 1000000 && pos.v == pos.v) { - emit valueChanged(uasId, "speed", pos.v, time); + emit valueChanged(uasId, "speed", "m/s", pos.v, time); //qDebug() << "GOT GPS RAW"; - emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); + // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); } else { @@ -447,9 +464,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_raw_pressure_t pressure; mavlink_msg_raw_pressure_decode(&message, &pressure); - emit valueChanged(uasId, "Abs pressure", pressure.press_abs, this->getUnixTime(0)); - emit valueChanged(uasId, "Diff pressure 1", pressure.press_diff1, this->getUnixTime(0)); - emit valueChanged(uasId, "Diff pressure 2", pressure.press_diff2, this->getUnixTime(0)); + quint64 time = this->getUnixTime(0); + emit valueChanged(uasId, "abs pressure", "hP", pressure.press_abs, time); + emit valueChanged(uasId, "diff pressure 1", "hP", pressure.press_diff1, time); + emit valueChanged(uasId, "diff pressure 2", "hP", pressure.press_diff2, time); } break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: @@ -506,7 +524,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; case MAVLINK_MSG_ID_DEBUG: - emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), "raw", mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow()); break; case MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT: { @@ -514,9 +532,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_attitude_controller_output_decode(&message, &out); quint64 time = MG::TIME::getGroundTimeNowUsecs(); emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time); - emit valueChanged(uasId, "att control roll", out.roll, time/1000.0f); - emit valueChanged(uasId, "att control pitch", out.pitch, time/1000.0f); - emit valueChanged(uasId, "att control yaw", out.yaw, time/1000.0f); + emit valueChanged(uasId, "att control roll", "raw", out.roll, time/1000.0f); + emit valueChanged(uasId, "att control pitch", "raw", out.pitch, time/1000.0f); + emit valueChanged(uasId, "att control yaw", "raw", out.yaw, time/1000.0f); } break; case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT: @@ -525,9 +543,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_position_controller_output_decode(&message, &out); quint64 time = MG::TIME::getGroundTimeNow(); //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time); - emit valueChanged(uasId, "pos control x", out.x, time); - emit valueChanged(uasId, "pos control y", out.y, time); - emit valueChanged(uasId, "pos control z", out.z, time); + emit valueChanged(uasId, "pos control x", "raw", out.x, time); + emit valueChanged(uasId, "pos control y", "raw", out.y, time); + emit valueChanged(uasId, "pos control z", "raw", out.z, time); } break; case MAVLINK_MSG_ID_WAYPOINT_COUNT: @@ -618,9 +636,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_debug_vect_decode(&message, &vect); QString str((const char*)vect.name); quint64 time = getUnixTime(vect.usec); - emit valueChanged(uasId, str+".x", vect.x, time); - emit valueChanged(uasId, str+".y", vect.y, time); - emit valueChanged(uasId, str+".z", vect.z, time); + emit valueChanged(uasId, str+".x", "raw", vect.x, time); + emit valueChanged(uasId, str+".y", "raw", vect.y, time); + emit valueChanged(uasId, str+".z", "raw", vect.z, time); } break; //#ifdef MAVLINK_ENABLED_PIXHAWK @@ -645,12 +663,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_nav_filter_bias_t bias; mavlink_msg_nav_filter_bias_decode(&message, &bias); quint64 time = MG::TIME::getGroundTimeNow(); - emit valueChanged(uasId, "b_f[0]", bias.accel_0, time); - emit valueChanged(uasId, "b_f[1]", bias.accel_1, time); - emit valueChanged(uasId, "b_f[2]", bias.accel_2, time); - emit valueChanged(uasId, "b_w[0]", bias.gyro_0, time); - emit valueChanged(uasId, "b_w[1]", bias.gyro_1, time); - emit valueChanged(uasId, "b_w[2]", bias.gyro_2, time); + emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time); + emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time); + emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time); + emit valueChanged(uasId, "b_w[0]", "raw", bias.gyro_0, time); + emit valueChanged(uasId, "b_w[1]", "raw", bias.gyro_1, time); + emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time); } break; case MAVLINK_MSG_ID_RADIO_CALIBRATION: diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 0b00419907cbe0d5b7f3af47a2f9250683bbb325..a35dd96d5bbb7ed2f76fe95c48158ea42d964de1 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -328,9 +328,9 @@ signals: * @param msec the timestamp of the message, in milliseconds */ - // FIXME Exchange the lines below against the commented ones - void valueChanged(int uasId, QString name, double value, quint64 msec); - void valueChanged(UASInterface* uas, QString name, double value, quint64 msec); + //void valueChanged(const int uasId, const QString& name, const double value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec); // void valueChanged(const int uasId, const QString& name, const double value, const quint64 msec); // //void valueChanged(UASInterface* uas, QString name, double value, quint64 msec); diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index 37f02e79c827e0ae10f3af57a2c21583a82ac4b2..ceb9a2a417cc4d049801b2d2574715c3da52588a 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -33,10 +33,12 @@ This file is part of the QGROUNDCONTROL project #include #include #include "UAS.h" -#include -#include +#include "UASInterface.h" +#include "UASManager.h" +#include "QGC.h" -UASManager* UASManager::instance() { +UASManager* UASManager::instance() +{ static UASManager* _instance = 0; if(_instance == 0) { _instance = new UASManager(); @@ -67,6 +69,10 @@ UASManager::~UASManager() void UASManager::run() { + forever + { + QGC::SLEEP::msleep(5000); + } } void UASManager::addUAS(UASInterface* uas) @@ -104,12 +110,6 @@ QList UASManager::getUASList() UASInterface* UASManager::getActiveUAS() { -// if(!activeUAS) -// { -// QMessageBox msgBox; -// msgBox.setText(tr("No Micro Air Vehicle connected. Please connect one first.")); -// msgBox.exec(); -// } return activeUAS; ///< Return zero pointer if no UAS has been loaded } @@ -193,8 +193,6 @@ void UASManager::setActiveUAS(UASInterface* uas) activeUAS = uas; activeUASMutex.unlock(); - qDebug() << __FILE__ << ":" << __LINE__ << " ACTIVE UAS SET TO: " << uas->getUASName(); - emit activeUASSet(uas); emit activeUASSet(uas->getUASID()); emit activeUASStatusChanged(uas, true); diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 57ed24d31fd218914d44e7f7147f46d95b6b5a6a..19ec8d878df689e90e0dd806813912a7360dd4c9 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -56,6 +56,7 @@ void UASWaypointManager::timeout() { protocol_timer.start(PROTOCOL_TIMEOUT_MS); current_retries--; + emit updateStatusString(tr("Timeout, retrying (retries left: %1)").arg(current_retries)); qDebug() << "Timeout, retrying (retries left:" << current_retries << ")"; if (current_state == WP_GETLIST) { @@ -518,7 +519,7 @@ void UASWaypointManager::sendWaypointClearAll() wpca.target_system = uas.getUASID(); wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER; - emit updateStatusString(QString("clearing waypoint list...")); + emit updateStatusString(QString("Clearing waypoint list...")); mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca); uas.sendMessage(message); @@ -555,7 +556,7 @@ void UASWaypointManager::sendWaypointCount() wpc.count = current_count; qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system; - emit updateStatusString(QString("start transmitting waypoints...")); + emit updateStatusString(QString("Starting to transmit waypoints...")); mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc); uas.sendMessage(message); @@ -572,7 +573,7 @@ void UASWaypointManager::sendWaypointRequestList() wprl.target_system = uas.getUASID(); wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER; - emit updateStatusString(QString("requesting waypoint list...")); + emit updateStatusString(QString("Requesting waypoint list...")); mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl); uas.sendMessage(message); @@ -592,7 +593,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq) wpr.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wpr.seq = seq; - emit updateStatusString(QString("retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count)); + emit updateStatusString(QString("Retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count)); mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr); uas.sendMessage(message); @@ -616,7 +617,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq) wp->target_system = uas.getUASID(); wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER; - emit updateStatusString(QString("sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count)); + emit updateStatusString(QString("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count)); qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system<<" WP Buffer count: "<addWidget(conf); ui.linkGroupBox->setLayout(layout); - ui.linkGroupBox->setTitle(tr("serial link")); + ui.linkGroupBox->setTitle(tr("Serial Link")); //ui.linkGroupBox->setTitle(link->getName()); //connect(link, SIGNAL(nameChanged(QString)), ui.linkGroupBox, SLOT(setTitle(QString))); } @@ -142,13 +142,13 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn // Open details pane for MAVLink if necessary MAVLinkProtocol* mavlink = dynamic_cast(protocol); - if(mavlink != 0) + if (mavlink != 0) { QWidget* conf = new MAVLinkSettingsWidget(mavlink, this); QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.protocolGroupBox); layout->addWidget(conf); ui.protocolGroupBox->setLayout(layout); - ui.protocolGroupBox->setTitle(protocol->getName()); + ui.protocolGroupBox->setTitle(protocol->getName()+" (Global Settings)"); } else { diff --git a/src/ui/DebugConsole.cc b/src/ui/DebugConsole.cc index f5af15c5f4a2b250b798883d1a3680b81dfd4f53..9bf157a16d005a9885cbbb2253a7bf5479eb8a53 100644 --- a/src/ui/DebugConsole.cc +++ b/src/ui/DebugConsole.cc @@ -62,6 +62,8 @@ DebugConsole::DebugConsole(QWidget *parent) : { // Setup basic user interface m_ui->setupUi(this); + // Hide sent text field - it is only useful after send has been hit + m_ui->sentText->setVisible(false); // Make text area not editable m_ui->receiveText->setReadOnly(true); // Limit to 500 lines @@ -105,6 +107,8 @@ DebugConsole::DebugConsole(QWidget *parent) : connect(m_ui->holdButton, SIGNAL(toggled(bool)), this, SLOT(hold(bool))); // Connect connect button connect(m_ui->connectButton, SIGNAL(clicked()), this, SLOT(handleConnectButton())); + // Connect the special chars combo box + connect(m_ui->specialComboBox, SIGNAL(activated(QString)), this, SLOT(appendSpecialSymbol(QString))); this->setVisible(false); } @@ -327,8 +331,83 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) } } +QByteArray DebugConsole::symbolNameToBytes(const QString& text) +{ + QByteArray b; + if (text == "LF") + { + b.append(static_cast(0x0A)); + } + else if (text == "FF") + { + b.append(static_cast(0x0C)); + } + else if (text == "CR") + { + b.append(static_cast(0x0D)); + } + else if (text == "CR+LF") + { + b.append(static_cast(0x0D)); + b.append(static_cast(0x0A)); + } + else if (text == "TAB") + { + b.append(static_cast(0x09)); + } + else if (text == "NUL") + { + b.append(static_cast(0x00)); + } + else if (text == "ESC") + { + b.append(static_cast(0x1B)); + } + else if (text == "~") + { + b.append(static_cast(0x7E)); + } + else if (text == "") + { + b.append(static_cast(0x20)); + } + return b; +} + +void DebugConsole::appendSpecialSymbol(const QString& text) +{ + QString line = m_ui->sendText->text(); + QByteArray symbols = symbolNameToBytes(text); + // The text is appended to the enter field + if (convertToAscii) + { + line.append(symbols); + } + else + { + + for (int i = 0; i < symbols.size(); i++) + { + QString add(" 0x%1"); + line.append(add.arg(static_cast(symbols.at(i)), 2, 16, QChar('0'))); + } + } + m_ui->sendText->setText(line); +} + void DebugConsole::sendBytes() { + if (!m_ui->sentText->isVisible()) + { + m_ui->sentText->setVisible(true); + } + + if (!currLink->isConnected()) + { + m_ui->sentText->setText(tr("Nothing sent. The link %1 is unconnected. Please connect first.").arg(currLink->getName())); + return; + } + QByteArray transmit; QString feedback; bool ok = true; @@ -382,8 +461,16 @@ void DebugConsole::sendBytes() if (ok && m_ui->sendText->text().toLatin1().size() > 0) { // Transmit only if conversion succeeded - currLink->writeBytes(transmit, transmit.size()); - m_ui->sentText->setText(tr("Sent: ") + feedback); +// int transmitted = + currLink->writeBytes(transmit, transmit.size()); +// if (transmit.size() == transmitted) +// { + m_ui->sentText->setText(tr("Sent: ") + feedback); +// } +// else +// { +// m_ui->sentText->setText(tr("Error during sending: Transmitted only %1 bytes instead of %2.").arg(transmitted, transmit.size())); +// } } else if (m_ui->sendText->text().toLatin1().size() > 0) { @@ -403,6 +490,8 @@ void DebugConsole::hexModeEnabled(bool mode) { convertToAscii = !mode; m_ui->receiveText->clear(); + m_ui->sendText->clear(); + m_ui->sentText->clear(); } /** diff --git a/src/ui/DebugConsole.h b/src/ui/DebugConsole.h index e3622eefddc5309c7857062bb1ada551d21d78c6..be064feb716378a0adf2227e407faeb3f9e0ef22 100644 --- a/src/ui/DebugConsole.h +++ b/src/ui/DebugConsole.h @@ -83,6 +83,8 @@ public slots: void setAutoHold(bool hold); /** @brief Receive plain text message to output to the user */ void receiveTextMessage(int id, int component, int severity, QString text); + /** @brief Append a special symbol */ + void appendSpecialSymbol(const QString& text); protected slots: /** @brief Draw information overlay */ @@ -92,6 +94,7 @@ public slots: protected: void changeEvent(QEvent *e); + QByteArray symbolNameToBytes(const QString& symbol); QList links; LinkInterface* currLink; diff --git a/src/ui/DebugConsole.ui b/src/ui/DebugConsole.ui index 6a32b3ed399815500438964c973cee5ec932017f..e22d2f22c33d52a70e88da849d4a9692c75dcfa5 100644 --- a/src/ui/DebugConsole.ui +++ b/src/ui/DebugConsole.ui @@ -6,8 +6,8 @@ 0 0 - 463 - 159 + 447 + 181 @@ -106,24 +106,74 @@ - - - - - 80 - 0 - - - - Type the bytes to send here, use 0xAA format for HEX (Check HEX checkbox above) - - - - - + + 5 + + + + 10 + + + QComboBox::AdjustToContentsOnFirstShow + + + 1 + + + + Add.. + + + + + CR+LF + + + + + LF + + + + + FF + + + + + CR + + + + + TAB + + + + + NUL + + + + + ESC + + + + + ~ + + + + + <Space> + + + + @@ -164,6 +214,19 @@ + + + + + 80 + 0 + + + + Type the bytes to send here, use 0xAA format for HEX (Check HEX checkbox above) + + + diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index 2d73917fb5178042d5d940ed7e39e2dd2cebf746..fc365fd162492dd77f27be60241478f02cfe8057 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -58,6 +58,7 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : normalStrokeWidth(1.0f), fineStrokeWidth(0.5f), acceptList(new QStringList()), + acceptUnitList(new QStringList()), lastPaintTime(0), columns(3), m_ui(new Ui::HDDisplay) @@ -65,6 +66,8 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : setWindowTitle(title); //m_ui->setupUi(this); + setAutoFillBackground(true); + // Add all items in accept list to gauge if (plotList) { @@ -77,7 +80,7 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : restoreState(); // Set minimum size - setMinimumSize(80, 80); + setMinimumSize(60, 60); // Set preferred size setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); @@ -139,8 +142,6 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : // Connect with UAS connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); //start(); - - this->setVisible(false); } HDDisplay::~HDDisplay() @@ -151,7 +152,7 @@ HDDisplay::~HDDisplay() QSize HDDisplay::sizeHint() const { - return QSize(400, 400*(vwidth/vheight)); + return QSize(400, 400.0f*(vwidth/vheight)*1.1f); } void HDDisplay::enableGLRendering(bool enable) @@ -165,6 +166,35 @@ void HDDisplay::triggerUpdate() update(this->geometry()); } +//void HDDisplay::updateValue(UASInterface* uas, const QString& name, const QString& unit, double value, quint64 msec) +//{ +// // UAS is not needed +// Q_UNUSED(uas); + +// if (!isnan(value) && !isinf(value)) +// { +// // Update mean +// const float oldMean = valuesMean.value(name, 0.0f); +// const int meanCount = valuesCount.value(name, 0); +// double mean = (oldMean * meanCount + value) / (meanCount + 1); +// if (isnan(mean) || isinf(mean)) mean = 0.0; +// valuesMean.insert(name, mean); +// valuesCount.insert(name, meanCount + 1); +// // Two-value sliding average +// double dot = (valuesDot.value(name) + (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f))/2.0f; +// if (isnan(dot) || isinf(dot)) +// { +// dot = 0.0; +// } +// valuesDot.insert(name, dot); +// values.insert(name, value); +// lastUpdate.insert(name, msec); +// //} + +// //qDebug() << __FILE__ << __LINE__ << "VALUE:" << value << "MEAN:" << mean << "DOT:" << dot << "COUNT:" << meanCount; +// } +//} + void HDDisplay::paintEvent(QPaintEvent * event) { Q_UNUSED(event); @@ -181,7 +211,12 @@ void HDDisplay::contextMenuEvent (QContextMenuEvent* event) menu.addActions(getItemRemoveActions()); menu.addSeparator(); menu.addAction(setColumnsAction); - menu.addAction(setTitleAction); + // Title change would ruin settings + // this can only be allowed once + // HDDisplays are instantiated + // by a factory method based on + // QSettings + //menu.addAction(setTitleAction); menu.exec(event->globalPos()); } @@ -194,10 +229,10 @@ void HDDisplay::saveState() for (int i = 0; i < acceptList->count(); i++) { QString key = acceptList->at(i); - instruments += "|" + QString::number(minValues.value(key, -1.0))+","+key+","+QString::number(maxValues.value(key, +1.0)); + instruments += "|" + QString::number(minValues.value(key, -1.0))+","+key+","+acceptUnitList->at(i)+","+QString::number(maxValues.value(key, +1.0))+","+((symmetric.value(key, false)) ? "s" : ""); } - qDebug() << "Saving" << instruments; + // qDebug() << "Saving" << instruments; settings.setValue(windowTitle()+"_gauges", instruments); settings.sync(); @@ -240,6 +275,8 @@ void HDDisplay::removeItemByAction() acceptList->removeAt(index); minValues.remove(item); maxValues.remove(item); + symmetric.remove(item); + adjustGaugeAspectRatio(); } } @@ -248,11 +285,20 @@ void HDDisplay::addGauge() QStringList items; for (int i = 0; i < values.count(); ++i) { - items.append(QString("%1,%2,%3").arg("-180").arg(values.keys().at(i)).arg("+180")); + QString key = values.keys().at(i); + 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")); + } + else + { + items.append(QString("%1,%2,%3,%4").arg("-180").arg(key).arg(unit).arg("+180")); + } } bool ok; QString item = QInputDialog::getItem(this, tr("Add Gauge Instrument"), - tr("Format: min, curve name, max"), items, 0, true, &ok); + tr("Format: min, curve name, max[,s]"), items, 0, true, &ok); if (ok && !item.isEmpty()) { addGauge(item); @@ -264,30 +310,47 @@ void HDDisplay::addGauge(const QString& gauge) if (gauge.length() > 0) { QStringList parts = gauge.split(','); - if (parts.count() > 1) + if (parts.count() > 2) { double val; bool ok; + bool success = true; QString key = parts.at(1); + QString unit = parts.at(2); if (!acceptList->contains(key)) { // Convert min to double number val = parts.first().toDouble(&ok); + success &= ok; if (ok) minValues.insert(key, val); // Convert max to double number - val = parts.last().toDouble(&ok); + val = parts.at(3).toDouble(&ok); + success &= ok; if (ok) maxValues.insert(key, val); - // Add value to acceptlist - acceptList->append(key); + // Convert symmetric flag + if (parts.length() >= 5) + { + if (parts.at(4).contains("s")) + { + symmetric.insert(key, true); + } + } + if (success) + { + // Add value to acceptlist + acceptList->append(key); + acceptUnitList->append(unit); + } } } - else + else if (parts.count() > 1) { if (!acceptList->contains(gauge)) { - acceptList->append(parts.first()); + acceptList->append(parts.at(0)); + acceptUnitList->append(parts.at(1)); } } } @@ -394,7 +457,7 @@ void HDDisplay::renderOverlay() 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, goodRanges.value(value, qMakePair(0.0f, 0.5f)), critRanges.value(value, qMakePair(0.7f, 1.0f)), true); + 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); xCoord += gaugeWidth + leftSpacing; // Move one row down if necessary if (xCoord + gaugeWidth*0.9f > vwidth) @@ -415,14 +478,14 @@ void HDDisplay::setActiveUAS(UASInterface* uas) if (this->uas != NULL) { // Disconnect any previously connected active MAV - disconnect(this->uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(updateValue(int,QString,double,quint64))); + disconnect(this->uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); } // Now connect the new UAS //qDebug() << "UAS SET!" << "ID:" << uas->getUASID(); // Setup communication - connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(updateValue(int,QString,double,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); this->uas = uas; } @@ -513,17 +576,33 @@ void HDDisplay::drawChangeRateStrip(float xRef, float yRef, float height, float paintText(label, defaultColor, 3.0f, xRef+width/2.0f, yRef+height-((scaledValue - minRate)/(maxRate-minRate))*height - 1.6f, painter); } -void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float max, QString name, float value, const QColor& color, QPainter* painter, QPair goodRange, QPair criticalRange, bool solid) +void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float max, QString name, float value, const QColor& color, QPainter* painter, bool symmetric, QPair goodRange, QPair criticalRange, bool solid) { // Draw the circle QPen circlePen(Qt::SolidLine); // Rotate the whole gauge with this angle (in radians) for the zero position - const float zeroRotation = 0.49f; + float zeroRotation; + if (symmetric) + { + zeroRotation = 1.35f; + } + else + { + zeroRotation = 0.49f; + } // Scale the rotation so that the gauge does one revolution // per max. change - const float rangeScale = ((2.0f * M_PI) / (max - min)) * 0.72f; + float rangeScale; + if (symmetric) + { + rangeScale = ((2.0f * M_PI) / (max - min)) * 0.57f; + } + else + { + rangeScale = ((2.0f * M_PI) / (max - min)) * 0.72f; + } const float scaledValue = (value-min)*rangeScale; @@ -539,6 +618,11 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float } circlePen.setWidth(refLineWidthToPen(radius/12.0f)); circlePen.setColor(color); + + if (symmetric) + { + circlePen.setStyle(Qt::DashLine); + } painter->setBrush(Qt::NoBrush); painter->setPen(circlePen); drawCircle(xRef, yRef+nameHeight, radius, 0.0f, color, painter); @@ -558,7 +642,15 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float QBrush brush(QGC::colorBackground, Qt::SolidPattern); painter->setBrush(brush); painter->setPen(Qt::NoPen); - painter->drawRect(refToScreenX(xRef-radius/2.5f), refToScreenY(yRef+nameHeight+radius/4.0f), refToScreenX(radius+radius/2.0f), refToScreenY((radius - radius/4.0f)*1.2f)); + + if (symmetric) + { + painter->drawRect(refToScreenX(xRef-radius), refToScreenY(yRef+nameHeight+radius/4.0f), refToScreenX(radius+radius), refToScreenY((radius - radius/4.0f)*1.2f)); + } + else + { + painter->drawRect(refToScreenX(xRef-radius/2.5f), refToScreenY(yRef+nameHeight+radius/4.0f), refToScreenX(radius+radius/2.0f), refToScreenY((radius - radius/4.0f)*1.2f)); + } // Draw good value and crit. value markers if (goodRange.first != goodRange.second) @@ -592,8 +684,8 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float QPolygonF p(6); p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef+nameHeight+radius * 0.05f)); - p.replace(1, QPointF(xRef-minWidth/2.0f, yRef+nameHeight+radius * 0.9f)); - p.replace(2, QPointF(xRef+minWidth/2.0f, yRef+nameHeight+radius * 0.9f)); + p.replace(1, QPointF(xRef-minWidth/2.0f, yRef+nameHeight+radius * 0.89f)); + p.replace(2, QPointF(xRef+minWidth/2.0f, yRef+nameHeight+radius * 0.89f)); p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef+nameHeight+radius * 0.05f)); p.replace(4, QPointF(xRef, yRef+nameHeight+radius * 0.0f)); p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef+nameHeight+radius * 0.05f)); @@ -765,9 +857,10 @@ float HDDisplay::refLineWidthToPen(float line) return line * 2.50f; } -void HDDisplay::updateValue(int uasId, QString name, double value, quint64 msec) +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec) { Q_UNUSED(uasId); + Q_UNUSED(unit); // Update mean const float oldMean = valuesMean.value(name, 0.0f); const int meanCount = valuesCount.value(name, 0); @@ -775,6 +868,7 @@ void HDDisplay::updateValue(int uasId, QString name, double value, quint64 msec) valuesCount.insert(name, meanCount + 1); valuesDot.insert(name, (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f)); values.insert(name, value); + units.insert(name, unit); lastUpdate.insert(name, msec); } diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index 02de21d0ff6f4ae632925ef98dc070afeff9e502..ee148629bd0a21435c389d5d2cb6fe9d98c543bc 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -64,7 +64,7 @@ public: public slots: /** @brief Update a HDD value */ - void updateValue(int uasId, QString name, double value, quint64 msec); + void updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); void setActiveUAS(UASInterface* uas); /** @brief Removes a plot item by the action data */ @@ -114,7 +114,7 @@ protected: void drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter); void drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid=true); - void drawGauge(float xRef, float yRef, float radius, float min, float max, const QString name, float value, const QColor& color, QPainter* painter, QPair goodRange, QPair criticalRange, bool solid=true); + void drawGauge(float xRef, float yRef, float radius, float min, float max, const QString name, float value, const QColor& color, QPainter* painter, bool symmetric, QPair goodRange, QPair criticalRange, bool solid=true); void drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter); void paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter); @@ -137,12 +137,14 @@ protected: UASInterface* uas; ///< The uas currently monitored QMap values; ///< The variables this HUD displays + QMap units; ///< The units QMap valuesDot; ///< First derivative of the variable QMap valuesMean; ///< Mean since system startup for this variable QMap valuesCount; ///< Number of values received so far QMap lastUpdate; ///< The last update time for this variable QMap minValues; ///< The minimum value this variable is assumed to have QMap maxValues; ///< The maximum value this variable is assumed to have + QMap symmetric; ///< Draw the gauge / dial symmetric bool = yes 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 @@ -176,7 +178,8 @@ protected: float normalStrokeWidth; ///< Normal line stroke width, used throughout the HUD float fineStrokeWidth; ///< Fine line stroke width, used throughout the HUD - QStringList* acceptList; ///< Variable names to plot + QStringList* acceptList; ///< Variable names to plot + QStringList* acceptUnitList; ///< Unit names to plot quint64 lastPaintTime; ///< Last time this widget was refreshed int columns; ///< Number of instrument columns diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index f8c73ce12d7cb08df7af9d411722465d94dce290..d7a203ccc0386e7cfb2d2c5bfd3ca77b309d10f9 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -46,7 +46,7 @@ This file is part of the QGROUNDCONTROL project #include HSIDisplay::HSIDisplay(QWidget *parent) : - HDDisplay(NULL, "", parent), + HDDisplay(NULL, "HSI", parent), gpsSatellites(), satellitesUsed(0), attXSet(0), @@ -102,11 +102,17 @@ HSIDisplay::HSIDisplay(QWidget *parent) : connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); refreshTimer->setInterval(updateInterval); + columns = 1; + // this->setScene(new QGraphicsScene(-metricWidth/2.0f, -metricWidth/2.0f, metricWidth, metricWidth, this)); + vwidth = 80; + vheight = 80; + xCenterPos = vwidth/2.0f; yCenterPos = vheight/2.0f + topMargin - bottomMargin; + qDebug() << "CENTER" << xCenterPos << yCenterPos; // Add interaction elements QHBoxLayout* layout = new QHBoxLayout(this); diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 2d57262366913602c610f7d03373abf9178f9230..90c22ae57fe383cd2be7c465f694170a27e3e8b9 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -73,11 +73,6 @@ inline bool isinf(T value) HUD::HUD(int width, int height, QWidget* parent) : QGLWidget(QGLFormat(QGL::SampleBuffers), parent), uas(NULL), - values(QMap()), - valuesDot(QMap()), - valuesMean(QMap()), - valuesCount(QMap()), - lastUpdate(QMap()), yawInt(0.0f), mode(tr("UNKNOWN MODE")), state(tr("UNKNOWN STATE")), @@ -115,7 +110,21 @@ HUD::HUD(int width, int height, QWidget* parent) waypointName(""), roll(0.0f), pitch(0.0f), - yaw(0.0f) + yaw(0.0f), + rollLP(0.0f), + pitchLP(0.0f), + yawLP(0.0f), + yawDiff(0.0f), + xPos(0.0), + yPos(0.0), + zPos(0.0), + xSpeed(0.0), + ySpeed(0.0), + zSpeed(0.0), + lat(0.0), + lon(0.0), + alt(0.0), + load(0.0f) { // Set auto fill to false setAutoFillBackground(false); @@ -173,8 +182,9 @@ HUD::HUD(int width, int height, QWidget* parent) } // Connect with UAS - UASManager* manager = UASManager::instance(); - connect(manager, SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + + setVisible(false); } HUD::~HUD() @@ -192,48 +202,15 @@ void HUD::showEvent(QShowEvent* event) // React only to internal (pre-display) // events Q_UNUSED(event) - { - refreshTimer->start(updateInterval); - } + refreshTimer->start(updateInterval); } void HUD::hideEvent(QHideEvent* event) { // React only to internal (pre-display) // events - Q_UNUSED(event) - { - refreshTimer->stop(); - } -} - -void HUD::updateValue(UASInterface* uas, QString name, double value, quint64 msec) -{ - // UAS is not needed - Q_UNUSED(uas); - - if (!isnan(value) && !isinf(value)) - { - // Update mean - const float oldMean = valuesMean.value(name, 0.0f); - const int meanCount = valuesCount.value(name, 0); - double mean = (oldMean * meanCount + value) / (meanCount + 1); - if (isnan(mean) || isinf(mean)) mean = 0.0; - valuesMean.insert(name, mean); - valuesCount.insert(name, meanCount + 1); - // Two-value sliding average - double dot = (valuesDot.value(name) + (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f))/2.0f; - if (isnan(dot) || isinf(dot)) - { - dot = 0.0; - } - valuesDot.insert(name, dot); - values.insert(name, value); - lastUpdate.insert(name, msec); - //} - - //qDebug() << __FILE__ << __LINE__ << "VALUE:" << value << "MEAN:" << mean << "DOT:" << dot << "COUNT:" << meanCount; - } + Q_UNUSED(event); + refreshTimer->stop(); } /** @@ -242,8 +219,7 @@ void HUD::updateValue(UASInterface* uas, QString name, double value, quint64 mse */ void HUD::setActiveUAS(UASInterface* uas) { - qDebug() << "ATTEMPTING TO SET UAS"; - if (this->uas != NULL && this->uas != uas) + if (this->uas != NULL) { // Disconnect any previously connected active MAV disconnect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); @@ -251,51 +227,51 @@ void HUD::setActiveUAS(UASInterface* uas) disconnect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); disconnect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); + + disconnect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + disconnect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + disconnect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); } // Now connect the new UAS - - //if (this->uas != uas) - // { - qDebug() << "UAS SET!" << "ID:" << uas->getUASID(); // Setup communication connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int))); connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); - //connect(uas, SIGNAL(thrustChanged(UASInterface*, double)), this, SLOT(updateThrust(UASInterface*, double))); - //connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); - //connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - //connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); - //connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString,QString))); - //connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double))); - //connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeThrustSetPoint(UASInterface*,double,double,double,double,quint64))); - //connect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); - //} -} -void HUD::updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec) -{ - updateValue(uas, "roll desired", rollDesired, msec); - updateValue(uas, "pitch desired", pitchDesired, msec); - updateValue(uas, "yaw desired", yawDesired, msec); - updateValue(uas, "thrust desired", thrustDesired, msec); + connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); + + // Set new UAS + this->uas = uas; } +//void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec) +//{ +//// updateValue(uas, "roll desired", rollDesired, msec); +//// updateValue(uas, "pitch desired", pitchDesired, msec); +//// updateValue(uas, "yaw desired", yawDesired, msec); +//// updateValue(uas, "thrust desired", thrustDesired, msec); +//} + void HUD::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp) { - //qDebug() << __FILE__ << __LINE__ << "YAW" << yaw; - updateValue(uas, "roll", roll, timestamp); - updateValue(uas, "pitch", pitch, timestamp); - updateValue(uas, "yaw", yaw, timestamp); + Q_UNUSED(uas); + Q_UNUSED(timestamp); + this->roll = roll; + this->pitch = pitch; + this->yaw = yaw; } void HUD::updateBattery(UASInterface* uas, double voltage, double percent, int seconds) { - updateValue(uas, "voltage", voltage, MG::TIME::getGroundTimeNow()); - updateValue(uas, "time remaining", seconds, MG::TIME::getGroundTimeNow()); - updateValue(uas, "charge level", percent, MG::TIME::getGroundTimeNow()); + Q_UNUSED(uas); +// this->voltage = voltage; +// this->timeRemaining = seconds; +// this->percentRemaining = percent; fuelStatus.sprintf("BAT [%02.0f \%% | %05.2fV] (%02dm:%02ds)", percent, voltage, seconds/60, seconds%60); @@ -317,30 +293,38 @@ void HUD::receiveHeartbeat(UASInterface*) { } -void HUD::updateThrust(UASInterface*, double thrust) +void HUD::updateThrust(UASInterface* uas, double thrust) { - updateValue(uas, "thrust", thrust, MG::TIME::getGroundTimeNow()); + Q_UNUSED(uas); + Q_UNUSED(thrust); +// updateValue(uas, "thrust", thrust, MG::TIME::getGroundTimeNow()); } void HUD::updateLocalPosition(UASInterface* uas,double x,double y,double z,quint64 timestamp) { - updateValue(uas, "x", x, timestamp); - updateValue(uas, "y", y, timestamp); - updateValue(uas, "z", z, timestamp); + Q_UNUSED(uas); + Q_UNUSED(timestamp); + this->xPos = x; + this->yPos = y; + this->zPos = z; } -void HUD::updateGlobalPosition(UASInterface*,double lat, double lon, double altitude, quint64 timestamp) +void HUD::updateGlobalPosition(UASInterface* uas,double lat, double lon, double altitude, quint64 timestamp) { - updateValue(uas, "lat", lat, timestamp); - updateValue(uas, "lon", lon, timestamp); - updateValue(uas, "altitude", altitude, timestamp); + Q_UNUSED(uas); + Q_UNUSED(timestamp); + this->lat = lat; + this->lon = lon; + this->alt = altitude; } void HUD::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 timestamp) { - updateValue(uas, "xSpeed", x, timestamp); - updateValue(uas, "ySpeed", y, timestamp); - updateValue(uas, "zSpeed", z, timestamp); + Q_UNUSED(uas); + Q_UNUSED(timestamp); + this->xSpeed = x; + this->ySpeed = y; + this->zSpeed = z; } /** @@ -372,7 +356,9 @@ void HUD::updateMode(int id,QString mode, QString description) void HUD::updateLoad(UASInterface* uas, double load) { - updateValue(uas, "load", load, MG::TIME::getGroundTimeNow()); + Q_UNUSED(uas); + this->load = load; + //updateValue(uas, "load", load, MG::TIME::getGroundTimeNow()); } /** @@ -575,14 +561,14 @@ void HUD::paintHUD() // Read out most important values to limit hash table lookups // Low-pass roll, pitch and yaw - roll = roll * 0.2f + 0.8f * values.value("roll", 0.0f); - pitch = pitch * 0.2f + 0.8f * values.value("pitch", 0.0f); - yaw = yaw * 0.2f + 0.8f * values.value("yaw", 0.0f); + rollLP = rollLP * 0.2f + 0.8f * roll; + pitchLP = pitchLP * 0.2f + 0.8f * pitch; + yawLP = yawLP * 0.2f + 0.8f * yaw; // Translate for yaw const float maxYawTrans = 60.0f; - static float yawDiff = 0.0f; - float newYawDiff = valuesDot.value("yaw", 0.0f); + + float newYawDiff = yawDiff; if (isinf(newYawDiff)) newYawDiff = yawDiff; if (newYawDiff > M_PI) newYawDiff = newYawDiff - M_PI; @@ -722,24 +708,29 @@ void HUD::paintHUD() QString yawAngle; // const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f; - const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f); + + // YAW is in compass-human readable format, so 0 - 360deg. This is normal in aviation, not -180 - +180. + const float yawDeg = ((yawLP/M_PI)*180.0f)+180.0f; yawAngle.sprintf("%03d", (int)yawDeg); paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter); // CHANGE RATE STRIPS - drawChangeRateStrip(-51.0f, -50.0f, 15.0f, -1.0f, 1.0f, valuesDot.value("z", 0.0f), &painter); + drawChangeRateStrip(-51.0f, -50.0f, 15.0f, -1.0f, 1.0f, -zSpeed, &painter); // CHANGE RATE STRIPS - drawChangeRateStrip(49.0f, -50.0f, 15.0f, -1.0f, 1.0f, valuesDot.value("x", 0.0f), &painter); + drawChangeRateStrip(49.0f, -50.0f, 15.0f, -1.0f, 1.0f, xSpeed, &painter); // GAUGES // Left altitude gauge - drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, -values.value("z", 0.0f), defaultColor, &painter, false); + drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, -zPos, defaultColor, &painter, false); // Right speed gauge - drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, values.value("xSpeed", 0.0f), defaultColor, &painter, false); + drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, xSpeed, defaultColor, &painter, false); + + // Waypoint name + if (waypointName != "") paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter); // MOVING PARTS @@ -747,15 +738,15 @@ void HUD::paintHUD() painter.translate(refToScreenX(yawTrans), 0); // Rotate view and draw all roll-dependent indicators - painter.rotate((roll/M_PI)* -180.0f); + painter.rotate((rollLP/M_PI)* -180.0f); - painter.translate(0, (-pitch/M_PI)* -180.0f * refToScreenY(1.8)); + painter.translate(0, (-pitchLP/M_PI)* -180.0f * refToScreenY(1.8)); //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f); // PITCH - paintPitchLines(pitch, &painter); + paintPitchLines(pitchLP, &painter); painter.end(); //glDisable(GL_MULTISAMPLE); @@ -1204,82 +1195,82 @@ void HUD::drawChangeRateStrip(float xRef, float yRef, float height, float minRat paintText(label, defaultColor, 3.0f, xRef+width/2.0f, yRef+height-((scaledValue - minRate)/(maxRate-minRate))*height - 1.6f, painter); } -void HUD::drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter) -{ - Q_UNUSED(maxWidth); - Q_UNUSED(maxHeight); - if (values.size() > 0) - { - QString selectedKey = values.begin().key(); - // | | | | | | - // | | | | | | - // x speed: 2.54 - - // One column per value - QMapIterator value(values); - - float x = xRef; - float y = yRef; - - const float vspacing = 1.0f; - float width = 1.5f; - float height = 1.5f; - const float hspacing = 0.6f; - - // TODO ensure that instrument stays smaller than maxWidth and maxHeight - - - int i = 0; - while (value.hasNext() && i < maxNum) - { - value.next(); - QBrush brush(Qt::SolidPattern); - - - if (value.value() < 0.01f && value.value() > -0.01f) - { - brush.setColor(Qt::gray); - } - else if (value.value() > 0.01f) - { - brush.setColor(Qt::blue); - } - else - { - brush.setColor(Qt::yellow); - } - - painter->setBrush(brush); - painter->setPen(Qt::NoPen); - - // Draw current value colormap - painter->drawRect(refToScreenX(x), refToScreenY(y), refToScreenX(width), refToScreenY(height)); - - // Draw change rate colormap - painter->drawRect(refToScreenX(x), refToScreenY(y+height+hspacing), refToScreenX(width), refToScreenY(height)); - - // Draw mean value colormap - painter->drawRect(refToScreenX(x), refToScreenY(y+2.0f*(height+hspacing)), refToScreenX(width), refToScreenY(height)); - - // Add spacing - x += width+vspacing; - - // Iterate - i++; - } - - // Draw detail label - QString detail = "NO DATA AVAILABLE"; - - if (values.contains(selectedKey)) - { - detail = values.find(selectedKey).key(); - detail.append(": "); - detail.append(QString::number(values.find(selectedKey).value())); - } - paintText(detail, QColor(255, 255, 255), 3.0f, xRef, yRef+3.0f*(height+hspacing)+1.0f, painter); - } -} +//void HUD::drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter) +//{ +// Q_UNUSED(maxWidth); +// Q_UNUSED(maxHeight); +// if (values.size() > 0) +// { +// QString selectedKey = values.begin().key(); +// // | | | | | | +// // | | | | | | +// // x speed: 2.54 + +// // One column per value +// QMapIterator value(values); + +// float x = xRef; +// float y = yRef; + +// const float vspacing = 1.0f; +// float width = 1.5f; +// float height = 1.5f; +// const float hspacing = 0.6f; + +// // TODO ensure that instrument stays smaller than maxWidth and maxHeight + + +// int i = 0; +// while (value.hasNext() && i < maxNum) +// { +// value.next(); +// QBrush brush(Qt::SolidPattern); + + +// if (value.value() < 0.01f && value.value() > -0.01f) +// { +// brush.setColor(Qt::gray); +// } +// else if (value.value() > 0.01f) +// { +// brush.setColor(Qt::blue); +// } +// else +// { +// brush.setColor(Qt::yellow); +// } + +// painter->setBrush(brush); +// painter->setPen(Qt::NoPen); + +// // Draw current value colormap +// painter->drawRect(refToScreenX(x), refToScreenY(y), refToScreenX(width), refToScreenY(height)); + +// // Draw change rate colormap +// painter->drawRect(refToScreenX(x), refToScreenY(y+height+hspacing), refToScreenX(width), refToScreenY(height)); + +// // Draw mean value colormap +// painter->drawRect(refToScreenX(x), refToScreenY(y+2.0f*(height+hspacing)), refToScreenX(width), refToScreenY(height)); + +// // Add spacing +// x += width+vspacing; + +// // Iterate +// i++; +// } + +// // Draw detail label +// QString detail = "NO DATA AVAILABLE"; + +// if (values.contains(selectedKey)) +// { +// detail = values.find(selectedKey).key(); +// detail.append(": "); +// detail.append(QString::number(values.find(selectedKey).value())); +// } +// paintText(detail, QColor(255, 255, 255), 3.0f, xRef, yRef+3.0f*(height+hspacing)+1.0f, painter); +// } +//} void HUD::drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid) { @@ -1362,12 +1353,10 @@ void HUD::resizeGL(int w, int h) paintHUD(); } -void HUD::selectWaypoint(UASInterface* uas, int id) +void HUD::selectWaypoint(int uasId, int id) { - if (uas == this->uas) - { - waypointName = tr("WP") + QString::number(id); - } + Q_UNUSED(uasId); + waypointName = tr("WP") + QString::number(id); } void HUD::setImageSize(int width, int height, int depth, int channels) diff --git a/src/ui/HUD.h b/src/ui/HUD.h index 70e5572f21dd33cf52cb51e0f3827c1557280b50..d9c89f73999d38e43e2de0d34051a9ed5f521e8d 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -63,11 +63,8 @@ public slots: /** @brief Set the currently monitored UAS */ void setActiveUAS(UASInterface* uas); - /** @brief Update a HUD value */ - void updateValue(UASInterface* uas, QString name, double value, quint64 msec); - void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp); - void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); +// void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); void updateBattery(UASInterface*, double, double, int); void receiveHeartbeat(UASInterface*); void updateThrust(UASInterface*, double); @@ -77,7 +74,7 @@ public slots: void updateState(UASInterface*,QString); void updateMode(int id,QString mode, QString description); void updateLoad(UASInterface*, double); - void selectWaypoint(UASInterface* uas, int id); + void selectWaypoint(int uasId, int id); void startImage(int imgid, int width, int height, int depth, int channels); void setPixels(int imgid, const unsigned char* imageData, int length, int startIndex); @@ -103,8 +100,6 @@ protected slots: void drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter); void drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid=true); - void drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter); - void drawPolygon(QPolygonF refPolygon, QPainter* painter); @@ -130,11 +125,6 @@ protected: QImage* image; ///< Double buffer image QImage glImage; ///< The background / camera image UASInterface* uas; ///< The uas currently monitored - QMap values; ///< The variables this HUD displays - QMap valuesDot; ///< First derivative of the variable - QMap valuesMean; ///< Mean since system startup for this variable - QMap valuesCount; ///< Number of values received so far - QMap lastUpdate; ///< The last update time for this variable float yawInt; ///< The yaw integral. Used to damp the yaw indication. QString mode; ///< The current vehicle mode QString state; ///< The current vehicle state @@ -188,6 +178,20 @@ protected: float roll; float pitch; float yaw; + float rollLP; + float pitchLP; + float yawLP; + double yawDiff; + double xPos; + double yPos; + double zPos; + double xSpeed; + double ySpeed; + double zSpeed; + double lat; + double lon; + double alt; + float load; void paintEvent(QPaintEvent *event); }; diff --git a/src/ui/MAVLinkSettingsWidget.cc b/src/ui/MAVLinkSettingsWidget.cc index 054ff10a5a73ddf40017c24edfc23a00c3aaef2c..7cb49ec6d639341bb0d004ae507de37c5d982c27 100644 --- a/src/ui/MAVLinkSettingsWidget.cc +++ b/src/ui/MAVLinkSettingsWidget.cc @@ -27,6 +27,11 @@ This file is part of the QGROUNDCONTROL project * @author Lorenz Meier */ +#include +#include +#include +#include + #include "MAVLinkSettingsWidget.h" #include "ui_MAVLinkSettingsWidget.h" @@ -37,6 +42,8 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget { m_ui->setupUi(this); + m_ui->gridLayout->setAlignment(Qt::AlignTop); + // Initialize state m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled()); @@ -49,7 +56,59 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget connect(m_ui->loggingCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableLogging(bool))); connect(protocol, SIGNAL(versionCheckChanged(bool)), m_ui->versionCheckBox, SLOT(setChecked(bool))); connect(m_ui->versionCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableVersionCheck(bool))); + connect(m_ui->logFileButton, SIGNAL(clicked()), this, SLOT(chooseLogfileName())); + + // Update values + m_ui->versionLabel->setText(tr("MAVLINK_VERSION: %1").arg(protocol->getVersion())); + updateLogfileName(protocol->getLogfileName()); + + // Connect visibility updates + connect(protocol, SIGNAL(versionCheckChanged(bool)), m_ui->versionLabel, SLOT(setVisible(bool))); + m_ui->versionLabel->setVisible(protocol->versionCheckEnabled()); + //connect(m_ui->versionCheckBox, SIGNAL(toggled(bool)), m_ui->versionSpacer, SLOT(setVisible(bool))); + //connect(m_ui->loggingCheckBox, SIGNAL(toggled(bool)), m_ui->logFileSpacer, SLOT(setVisible(bool))); + connect(protocol, SIGNAL(loggingChanged(bool)), m_ui->logFileLabel, SLOT(setVisible(bool))); + m_ui->logFileLabel->setVisible(protocol->loggingEnabled()); + connect(protocol, SIGNAL(loggingChanged(bool)), m_ui->logFileButton, SLOT(setVisible(bool))); + m_ui->logFileButton->setVisible(protocol->loggingEnabled()); + + // Update settings + m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled()); + m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); + m_ui->versionCheckBox->setChecked(protocol->versionCheckEnabled()); +} +void MAVLinkSettingsWidget::updateLogfileName(const QString& fileName) +{ + QFileInfo file(fileName); + m_ui->logFileLabel->setText(file.fileName()); +} + +void MAVLinkSettingsWidget::chooseLogfileName() +{ + QString fileName = QFileDialog::getSaveFileName(this, tr("Specify MAVLink log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink Logfile (*.mavlink);;")); + + if (!fileName.endsWith(".mavlink")) + { + fileName.append(".mavlink"); + } + + QFileInfo file(fileName); + if (file.exists() && !file.isWritable()) + { + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText(tr("The selected logfile is not writable")); + msgBox.setInformativeText(tr("Please make sure that the file %1 is writable or select a different file").arg(fileName)); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + } + else + { + updateLogfileName(fileName); + protocol->setLogfileName(fileName); + } } MAVLinkSettingsWidget::~MAVLinkSettingsWidget() diff --git a/src/ui/MAVLinkSettingsWidget.h b/src/ui/MAVLinkSettingsWidget.h index 0941b2b5dbaf03337921a4bb487c383c0f17d9b6..c6f28c7b913abd76dd5a5bc44c2244282591d3e3 100644 --- a/src/ui/MAVLinkSettingsWidget.h +++ b/src/ui/MAVLinkSettingsWidget.h @@ -15,6 +15,12 @@ public: MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget *parent = 0); ~MAVLinkSettingsWidget(); +public slots: + /** @brief Update the log file name display */ + void updateLogfileName(const QString& fileName); + /** @brief Start the file select dialog for the log file */ + void chooseLogfileName(); + protected: MAVLinkProtocol* protocol; void changeEvent(QEvent *e); diff --git a/src/ui/MAVLinkSettingsWidget.ui b/src/ui/MAVLinkSettingsWidget.ui index c24aaddf2099bd0c80b773b9dc1f8c77c08bd7cb..5f96e685ae3652ee9413b42bddcc58f8d3e84ed7 100644 --- a/src/ui/MAVLinkSettingsWidget.ui +++ b/src/ui/MAVLinkSettingsWidget.ui @@ -6,51 +6,85 @@ 0 0 - 267 - 123 + 361 + 145 Form - - - 6 - - + + Emit heartbeat - + Log all MAVLink packets - + + + + Qt::Horizontal + + + QSizePolicy::MinimumExpanding + + + + 8 + 0 + + + + + Only accept MAVs with same protocol version - - + + - Qt::Vertical + Qt::Horizontal - 20 - 84 + 8 + 0 + + + + MAVLINK_VERSION: + + + + + + + Logfile name + + + + + + + Select.. + + + diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 75a115091e6eac95dca05a4dafc1bca97ec31d94..134d68fd1768c4ae9ff320f727f9212a73c37be5 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -27,6 +27,7 @@ #include "JoystickWidget.h" #include "GAudioOutput.h" #include "QGCToolWidget.h" +#include "QGCMAVLinkLogPlayer.h" #ifdef QGC_OSG_ENABLED #include "Q3DWidgetFactory.h" @@ -63,15 +64,59 @@ MainWindow* MainWindow::instance() MainWindow::MainWindow(QWidget *parent): QMainWindow(parent), toolsMenuActions(), - currentView(VIEW_OPERATOR), + currentView(VIEW_ENGINEER), aboutToCloseFlag(false), + changingViewsFlag(false), settings() { // Get current settings settings.sync(); + if (!settings.contains("CURRENT_VIEW")) + { + // Set this view as default view + settings.setValue("CURRENT_VIEW", currentView); + + // Enable default tools + + // ENABLE UAS LIST + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,currentView), true); + // ENABLE COMMUNICATION CONSOLE + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_DEBUG_CONSOLE,currentView), true); + } + else + { + if (settings.value("CURRENT_VIEW", VIEW_PILOT) != VIEW_PILOT) + { + currentView = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt(); + } + } + // Check if the settings exist, instantiate defaults if necessary - QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, currentView); + + // OPERATOR VIEW DEFAULT + QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_OPERATOR); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + } + + // ENGINEER VIEW DEFAULT + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_LINECHART, VIEW_ENGINEER); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + } + + // MAVLINK VIEW DEFAULT + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_PROTOCOL, VIEW_MAVLINK); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + } + + // PILOT VIEW DEFAULT + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_HUD, VIEW_PILOT); if (!settings.contains(centralKey)) { settings.setValue(centralKey,true); @@ -88,6 +133,25 @@ MainWindow::MainWindow(QWidget *parent): // Setup user interface ui.setupUi(this); + setVisible(false); + + // Bind together the perspective actions + QActionGroup* perspectives = new QActionGroup(ui.menuPerspectives); + perspectives->addAction(ui.actionEngineersView); + perspectives->addAction(ui.actionMavlinkView); + perspectives->addAction(ui.actionPilotsView); + perspectives->addAction(ui.actionOperatorsView); + perspectives->setExclusive(true); + + // Mark the right one as selected + if (currentView == VIEW_ENGINEER) ui.actionEngineersView->setChecked(true); + if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true); + if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true); + if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true); + + // The pilot view is not available on startup + ui.actionPilotsView->setEnabled(false); + buildCommonWidgets(); connectCommonWidgets(); @@ -96,15 +160,18 @@ MainWindow::MainWindow(QWidget *parent): configureWindowName(); - // Add status bar - //setStatusBar(createStatusBar()); - // Set the application style (not the same as a style sheet) // Set the style to Plastique qApp->setStyle("plastique"); // Set style sheet as last step - reloadStylesheet(); + QFile* styleSheet = new QFile(":/images/style-mission.css"); + if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) + { + QString style = QString(styleSheet->readAll()); + style.replace("ICONDIR", QCoreApplication::applicationDirPath()+ "/images/"); + qApp->setStyleSheet(style); + } // Create actions connectCommonActions(); @@ -117,15 +184,16 @@ MainWindow::MainWindow(QWidget *parent): statusBar()->setSizeGripEnabled(true); - if (settings.contains("geometry")) + // Restore the window position and size + if (settings.contains(getWindowGeometryKey())) { - // Restore the window geometry - restoreGeometry(settings.value("geometry").toByteArray()); + // Restore the window geometry + restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray()); } else { - // Adjust the size - adjustSize(); + // Adjust the size + adjustSize(); } // Populate link menu @@ -135,6 +203,8 @@ MainWindow::MainWindow(QWidget *parent): this->addLink(link); } + connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addLink(LinkInterface*))); + // Enable and update view presentView(); } @@ -144,6 +214,16 @@ MainWindow::~MainWindow() } +QString MainWindow::getWindowStateKey() +{ + return QString::number(currentView)+"/windowstate"; +} + +QString MainWindow::getWindowGeometryKey() +{ + return QString::number(currentView)+"/geometry"; +} + void MainWindow::buildCommonWidgets() { //TODO: move protocol outside UI @@ -153,50 +233,63 @@ void MainWindow::buildCommonWidgets() // Dock widgets if (!controlDockWidget) { - controlDockWidget = new QDockWidget(tr("Control"), this); - controlDockWidget->setWidget( new UASControlWidget(this) ); + controlDockWidget = new QDockWidget(tr("Control"), this); + controlDockWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_DOCKWIDGET"); + controlDockWidget->setWidget( new UASControlWidget(this) ); addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget()), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); } if (!listDockWidget) { - listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); - listDockWidget->setWidget( new UASListWidget(this) ); + listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); + listDockWidget->setWidget( new UASListWidget(this) ); + listDockWidget->setObjectName("UNMANNED_SYSTEMS_LIST_DOCKWIDGET"); addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget()), MENU_UAS_LIST, Qt::RightDockWidgetArea); } if (!waypointsDockWidget) { - waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); - waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); - addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget()), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); + waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); + waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); + waypointsDockWidget->setObjectName("WAYPOINT_LIST_DOCKWIDGET"); + addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget()), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); } if (!infoDockWidget) { - infoDockWidget = new QDockWidget(tr("Status Details"), this); - infoDockWidget->setWidget( new UASInfoWidget(this) ); - addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget()), MENU_STATUS, Qt::RightDockWidgetArea); + infoDockWidget = new QDockWidget(tr("Status Details"), this); + infoDockWidget->setWidget( new UASInfoWidget(this) ); + infoDockWidget->setObjectName("UAS_STATUS_DETAILS_DOCKWIDGET"); + addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget()), MENU_STATUS, Qt::RightDockWidgetArea); } if (!debugConsoleDockWidget) { - debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); - debugConsoleDockWidget->setWidget( new DebugConsole(this) ); - addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget()), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea); + debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); + debugConsoleDockWidget->setWidget( new DebugConsole(this) ); + debugConsoleDockWidget->setObjectName("COMMUNICATION_DEBUG_CONSOLE_DOCKWIDGET"); + addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget()), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea); + } + + if (!logPlayerDockWidget) + { + logPlayerDockWidget = new QDockWidget(tr("MAVLink Log Player"), this); + logPlayerDockWidget->setWidget( new QGCMAVLinkLogPlayer(mavlink, this) ); + logPlayerDockWidget->setObjectName("MAVLINK_LOG_PLAYER_DOCKWIDGET"); + addToToolsMenu(logPlayerDockWidget, tr("MAVLink Log Replay"), SLOT(showToolWidget()), MENU_MAVLINK_LOG_PLAYER, Qt::RightDockWidgetArea); } // Center widgets if (!mapWidget) { - mapWidget = new MapWidget(this); - addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP); + mapWidget = new MapWidget(this); + addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP); } if (!protocolWidget) { - protocolWidget = new XMLCommProtocolWidget(this); - addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); + protocolWidget = new XMLCommProtocolWidget(this); + addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); } if (!dataplotWidget) @@ -210,61 +303,61 @@ void MainWindow::buildPxWidgets() { //FIXME: memory of acceptList will never be freed again QStringList* acceptList = new QStringList(); - acceptList->append("-180,roll (deg),+180"); - acceptList->append("-180,pitch (deg),+180"); - acceptList->append("-180,yaw (deg),+180"); + acceptList->append("-105,roll,deg,+105,s"); + acceptList->append("-105,pitch,deg,+105,s"); + acceptList->append("-105,yaw,deg,+105,s"); - acceptList->append("-500,roll V (deg/s),+500"); - acceptList->append("-500,pitch V (deg/s),+500"); - acceptList->append("-500,yaw V (deg/s),+500"); + acceptList->append("-260,rollspeed,deg/s,+260,s"); + acceptList->append("-260,pitchspeed,deg/s,+260,s"); + acceptList->append("-260,yawspeed,deg/s,+260,s"); //FIXME: memory of acceptList2 will never be freed again QStringList* acceptList2 = new QStringList(); - acceptList2->append("0,Abs pressure,65500"); - acceptList2->append("-2000,Accel. X, 2000"); - acceptList2->append("-2000,Accel. Y, 2000"); + acceptList2->append("0,abs pressure,hPa,65500"); + acceptList2->append("-999,accel. X,raw,999,s"); + acceptList2->append("-999,accel. Y,raw,999,s"); if (!linechartWidget) { - // Center widgets - linechartWidget = new Linecharts(this); - addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART); + // Center widgets + linechartWidget = new Linecharts(this); + addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART); } if (!hudWidget) { - hudWidget = new HUD(320, 240, this); - addToCentralWidgetsMenu(hudWidget, "HUD", SLOT(showCentralWidget()), CENTRAL_HUD); + hudWidget = new HUD(320, 240, this); + addToCentralWidgetsMenu(hudWidget, "HUD", SLOT(showCentralWidget()), CENTRAL_HUD); } if (!dataplotWidget) { - dataplotWidget = new QGCDataPlot2D(this); - addToCentralWidgetsMenu(dataplotWidget, "Data Plots", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT); + dataplotWidget = new QGCDataPlot2D(this); + addToCentralWidgetsMenu(dataplotWidget, "Data Plots", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT); } #ifdef QGC_OSG_ENABLED if (!_3DWidget) { - _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); - addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL); + _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); + addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL); } #endif #ifdef QGC_OSGEARTH_ENABLED if (!_3DMapWidget) { - _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); - addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH); + _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); + addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH); } #endif #if (defined _MSC_VER) | (defined Q_OS_MAC) if (!gEarthWidget) { - gEarthWidget = new QGCGoogleEarthView(this); - addToCentralWidgetsMenu(gEarthWidget, "Google Earth", SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); + gEarthWidget = new QGCGoogleEarthView(this); + addToCentralWidgetsMenu(gEarthWidget, "Google Earth", SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); } #endif @@ -273,65 +366,73 @@ void MainWindow::buildPxWidgets() if (!detectionDockWidget) { - detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); - detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); - addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget()), MENU_DETECTION, Qt::RightDockWidgetArea); + detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); + detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); + detectionDockWidget->setObjectName("OBJECT_DETECTION_DOCK_WIDGET"); + addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget()), MENU_DETECTION, Qt::RightDockWidgetArea); } if (!parametersDockWidget) { - parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); - parametersDockWidget->setWidget( new ParameterInterface(this) ); - addToToolsMenu (parametersDockWidget, tr("Onboard Parameters"), SLOT(showToolWidget()), MENU_PARAMETERS, Qt::RightDockWidgetArea); + parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); + parametersDockWidget->setWidget( new ParameterInterface(this) ); + parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET"); + addToToolsMenu (parametersDockWidget, tr("Onboard Parameters"), SLOT(showToolWidget()), MENU_PARAMETERS, Qt::RightDockWidgetArea); } if (!watchdogControlDockWidget) { - watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); - watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); - addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget()), MENU_WATCHDOG, Qt::BottomDockWidgetArea); + watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); + watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); + watchdogControlDockWidget->setObjectName("WATCHDOG_CONTROL_DOCKWIDGET"); + addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget()), MENU_WATCHDOG, Qt::BottomDockWidgetArea); } if (!hsiDockWidget) { - hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); - hsiDockWidget->setWidget( new HSIDisplay(this) ); - addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget()), MENU_HSI, Qt::BottomDockWidgetArea); + hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); + hsiDockWidget->setWidget( new HSIDisplay(this) ); + hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET"); + addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget()), MENU_HSI, Qt::BottomDockWidgetArea); } if (!headDown1DockWidget) { headDown1DockWidget = new QDockWidget(tr("Flight Display"), this); headDown1DockWidget->setWidget( new HDDisplay(acceptList, "Flight Display", this) ); - addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget()), MENU_HDD_1, Qt::RightDockWidgetArea); + headDown1DockWidget->setObjectName("HEAD_DOWN_DISPLAY_1_DOCK_WIDGET"); + addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget()), MENU_HDD_1, Qt::RightDockWidgetArea); } if (!headDown2DockWidget) { - headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); + headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Payload Status", this) ); - addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget()), MENU_HDD_2, Qt::RightDockWidgetArea); + headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET"); + addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget()), MENU_HDD_2, Qt::RightDockWidgetArea); } if (!rcViewDockWidget) { - rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); - rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); - addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); + rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET"); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); } if (!headUpDockWidget) { - headUpDockWidget = new QDockWidget(tr("HUD"), this); - headUpDockWidget->setWidget( new HUD(320, 240, this)); - addToToolsMenu (headUpDockWidget, tr("Control Indicator"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea); + headUpDockWidget = new QDockWidget(tr("HUD"), this); + headUpDockWidget->setWidget( new HUD(320, 240, this)); + headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); + addToToolsMenu (headUpDockWidget, tr("Control Indicator"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea); } // Dialogue widgets //FIXME: free memory in destructor if (!joystick) { - joystick = new JoystickInput(); + joystick = new JoystickInput(); } } @@ -340,52 +441,58 @@ void MainWindow::buildSlugsWidgets() { if (!linechartWidget) { - // Center widgets + // Center widgets linechartWidget = new Linecharts(this); } if (!headUpDockWidget) { - // Dock widgets - headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); - headUpDockWidget->setWidget( new HUD(320, 240, this)); - addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea); + // Dock widgets + headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); + headUpDockWidget->setWidget( new HUD(320, 240, this)); + headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); + addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea); } if (!rcViewDockWidget) { - rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); - rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); - addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); + rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET"); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); } if (!slugsDataWidget) { - // Dialog widgets - slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); - slugsDataWidget->setWidget( new SlugsDataSensorView(this)); - addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget()), MENU_SLUGS_DATA, Qt::RightDockWidgetArea); + // Dialog widgets + slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); + slugsDataWidget->setWidget( new SlugsDataSensorView(this)); + slugsDataWidget->setObjectName("SLUGS_DATA_DOCK_WIDGET"); + addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget()), MENU_SLUGS_DATA, Qt::RightDockWidgetArea); } if (!slugsPIDControlWidget) { slugsPIDControlWidget = new QDockWidget(tr("Slugs PID Control"), this); - slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); - addToToolsMenu (slugsPIDControlWidget, tr("PID Configuration"), SLOT(showToolWidget()), MENU_SLUGS_PID, Qt::LeftDockWidgetArea); + slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); + slugsPIDControlWidget->setObjectName("SLUGS_PID_CONTROL_DOCK_WIDGET"); + addToToolsMenu (slugsPIDControlWidget, tr("PID Configuration"), SLOT(showToolWidget()), MENU_SLUGS_PID, Qt::LeftDockWidgetArea); } if (!slugsHilSimWidget) { - slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); - slugsHilSimWidget->setWidget( new SlugsHilSim(this)); - addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); + slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); + slugsHilSimWidget->setWidget( new SlugsHilSim(this)); + slugsHilSimWidget->setObjectName("SLUGS_HIL_SIM_DOCK_WIDGET"); + addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); } if (!slugsCamControlWidget) { slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this); - slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); - addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); + slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); + slugsCamControlWidget->setObjectName("SLUGS_CAM_CONTROL_DOCK_WIDGET"); + addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); } } @@ -395,82 +502,83 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, const char * slotName, TOOLS_WIDGET_NAMES centralWidget) { - QAction* tempAction; + QAction* tempAction; - // Add the separator that will separate tools from central Widgets + // Add the separator that will separate tools from central Widgets if (!toolsMenuActions[CENTRAL_SEPARATOR]) { - tempAction = ui.menuTools->addSeparator(); - toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; - tempAction->setData(CENTRAL_SEPARATOR); - } + tempAction = ui.menuTools->addSeparator(); + toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; + tempAction->setData(CENTRAL_SEPARATOR); + } - tempAction = ui.menuTools->addAction(title); + tempAction = ui.menuTools->addAction(title); - tempAction->setCheckable(true); - tempAction->setData(centralWidget); + tempAction->setCheckable(true); + tempAction->setData(centralWidget); - // populate the Hashes - toolsMenuActions[centralWidget] = tempAction; - dockWidgets[centralWidget] = widget; + // populate the Hashes + toolsMenuActions[centralWidget] = tempAction; + dockWidgets[centralWidget] = widget; - QString chKey = buildMenuKey(SUB_SECTION_CHECKED, centralWidget, currentView); + QString chKey = buildMenuKey(SUB_SECTION_CHECKED, centralWidget, currentView); if (!settings.contains(chKey)) { - settings.setValue(chKey,false); - tempAction->setChecked(false); - } + settings.setValue(chKey,false); + tempAction->setChecked(false); + } else { tempAction->setChecked(settings.value(chKey).toBool()); } - // connect the action - connect(tempAction,SIGNAL(triggered()),this, slotName); + // connect the action + connect(tempAction,SIGNAL(triggered()),this, slotName); } void MainWindow::showCentralWidget() { - QAction* senderAction = qobject_cast(sender()); - int tool = senderAction->data().toInt(); - QString chKey; + QAction* senderAction = qobject_cast(sender()); + int tool = senderAction->data().toInt(); + QString chKey; - // check the current action + // check the current action if (senderAction && dockWidgets[tool]) { - // uncheck all central widget actions - QHashIterator i(toolsMenuActions); - while (i.hasNext()) { - i.next(); - qDebug() << "shCW" << i.key() << "read"; - if (i.value() && i.value()->data().toInt() > 255){ - i.value()->setChecked(false); + // uncheck all central widget actions + QHashIterator i(toolsMenuActions); + while (i.hasNext()) + { + i.next(); + //qDebug() << "shCW" << i.key() << "read"; + if (i.value() && i.value()->data().toInt() > 255) + { + i.value()->setChecked(false); - // update the settings - chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.value()->data().toInt()), currentView); - settings.setValue(chKey,false); - } - } + // update the settings + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.value()->data().toInt()), currentView); + settings.setValue(chKey,false); + } + } - // check the current action - qDebug() << senderAction->text(); - senderAction->setChecked(true); + // check the current action + //qDebug() << senderAction->text(); + senderAction->setChecked(true); - // update the central widget - centerStack->setCurrentWidget(dockWidgets[tool]); + // update the central widget + centerStack->setCurrentWidget(dockWidgets[tool]); - // store the selected central widget - chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); - settings.setValue(chKey,true); - settings.sync(); + // store the selected central widget + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); + settings.setValue(chKey,true); - presentView(); - } + presentView(); + } } /** @@ -478,122 +586,107 @@ void MainWindow::showCentralWidget() * enabled last time. */ void MainWindow::addToToolsMenu ( QWidget* widget, - const QString title, - const char * slotName, - TOOLS_WIDGET_NAMES tool, + const QString title, + const char * slotName, + TOOLS_WIDGET_NAMES tool, Qt::DockWidgetArea location) { - QAction* tempAction; - QString posKey, chKey; + QAction* tempAction; + QString posKey, chKey; if (toolsMenuActions[CENTRAL_SEPARATOR]) { - tempAction = new QAction(title, this); - ui.menuTools->insertAction(toolsMenuActions[CENTRAL_SEPARATOR], - tempAction); + tempAction = new QAction(title, this); + ui.menuTools->insertAction(toolsMenuActions[CENTRAL_SEPARATOR], + tempAction); } else { - tempAction = ui.menuTools->addAction(title); - } + tempAction = ui.menuTools->addAction(title); + } - tempAction->setCheckable(true); - tempAction->setData(tool); + tempAction->setCheckable(true); + tempAction->setData(tool); - // populate the Hashes - toolsMenuActions[tool] = tempAction; - dockWidgets[tool] = widget; - qDebug() << widget; + // populate the Hashes + toolsMenuActions[tool] = tempAction; + dockWidgets[tool] = widget; + //qDebug() << widget; - posKey = buildMenuKey (SUB_SECTION_LOCATION,tool, currentView); + posKey = buildMenuKey (SUB_SECTION_LOCATION,tool, currentView); if (!settings.contains(posKey)) { - settings.setValue(posKey,location); - dockWidgetLocations[tool] = location; + settings.setValue(posKey,location); + dockWidgetLocations[tool] = location; } else { dockWidgetLocations[tool] = static_cast (settings.value(posKey, Qt::RightDockWidgetArea).toInt()); - } + } - chKey = buildMenuKey(SUB_SECTION_CHECKED,tool, currentView); + chKey = buildMenuKey(SUB_SECTION_CHECKED,tool, currentView); if (!settings.contains(chKey)) { - settings.setValue(chKey,false); - tempAction->setChecked(false); + settings.setValue(chKey,false); + tempAction->setChecked(false); widget->setVisible(false); } else { - tempAction->setChecked(settings.value(chKey).toBool()); + tempAction->setChecked(settings.value(chKey).toBool()); widget->setVisible(settings.value(chKey, false).toBool()); - } + } - // connect the action - connect(tempAction,SIGNAL(triggered()),this, slotName); + // connect the action + connect(tempAction,SIGNAL(triggered()),this, slotName); // connect(qobject_cast (dockWidgets[tool]), // SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool))); - connect(qobject_cast (dockWidgets[tool]), - SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea))); + connect(qobject_cast (dockWidgets[tool]), + SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea))); } void MainWindow::showToolWidget() { - QAction* temp = qobject_cast(sender()); - int tool = temp->data().toInt(); - + QAction* temp = qobject_cast(sender()); + int tool = temp->data().toInt(); if (temp && dockWidgets[tool]) { if (temp->isChecked()) { - addDockWidget(dockWidgetLocations[tool], qobject_cast (dockWidgets[tool])); - qobject_cast(dockWidgets[tool])->show(); + addDockWidget(dockWidgetLocations[tool], qobject_cast (dockWidgets[tool])); + qobject_cast(dockWidgets[tool])->show(); } else { - removeDockWidget(qobject_cast(dockWidgets[tool])); + removeDockWidget(qobject_cast(dockWidgets[tool])); + } } - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); - settings.setValue(chKey,temp->isChecked()); - settings.sync(); - } } void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view) { - bool tempVisible; - Qt::DockWidgetArea tempLocation; - QDockWidget* tempWidget = static_cast (dockWidgets[widget]); - - tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,widget,view), false).toBool(); - -// qDebug() << buildMenuKey (SUB_SECTION_CHECKED, widget,view) << tempVisible; + bool tempVisible; + Qt::DockWidgetArea tempLocation; + QDockWidget* tempWidget = static_cast (dockWidgets[widget]); - // Some widgets are per default visible. Overwrite the settings value if not present. - if (widget == MainWindow::MENU_UAS_LIST) - { - if (!settings.contains(buildMenuKey (SUB_SECTION_CHECKED,widget,view))) - { - tempVisible = true; - } - } + tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,widget,view), false).toBool(); if (tempWidget) { - toolsMenuActions[widget]->setChecked(tempVisible); - } + toolsMenuActions[widget]->setChecked(tempVisible); + } - //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,widget,view) << tempVisible; + //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,widget,view) << tempVisible; tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view), QVariant(Qt::RightDockWidgetArea)).toInt()); @@ -607,80 +700,81 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view) if ((tempWidget != NULL) && tempVisible) { - addDockWidget(tempLocation, tempWidget); - tempWidget->show(); - } + addDockWidget(tempLocation, tempWidget); + tempWidget->show(); + } } QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view) { - // Key is built as follows: autopilot_type/section_menu/view/tool/section - int apType; - - apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())? - UASManager::instance()->getActiveUAS()->getAutopilotType(): - -1; - - return (QString::number(apType) + "/" + - QString::number(SECTION_MENU) + "/" + - QString::number(view) + "/" + - QString::number(tool) + "/" + - QString::number(section) + "/" ); + // Key is built as follows: autopilot_type/section_menu/view/tool/section + int apType; + + apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())? + UASManager::instance()->getActiveUAS()->getAutopilotType(): + -1; + + return (QString::number(apType) + "/" + + QString::number(SECTION_MENU) + "/" + + QString::number(view) + "/" + + QString::number(tool) + "/" + + QString::number(section) + "/" ); } void MainWindow::closeEvent(QCloseEvent *event) { - settings.setValue("geometry", saveGeometry()); + settings.setValue(getWindowGeometryKey(), saveGeometry()); //settings.setValue("windowState", saveState()); aboutToCloseFlag = true; - settings.setValue("VIEW_ON_APPLICATION_CLOSE", currentView); + // Save the last current view in any case + settings.setValue("CURRENT_VIEW", currentView); + // Save the current window state, but only if a system is connected (else no real number of widgets would be present) + if (UASManager::instance()->getActiveUAS()) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); + // Save the current view only if a UAS is connected + if (UASManager::instance()->getUASList().length() > 0) settings.setValue("CURRENT_VIEW_WITH_UAS_CONNECTED", currentView); settings.sync(); QMainWindow::closeEvent(event); } -/** - * Stores the visibility setting of each widget. This method - * will only change the settings if the application is not - * about to close. - */ + void MainWindow::updateVisibilitySettings (bool vis) { - if (!aboutToCloseFlag) { - - QDockWidget* temp = qobject_cast(sender()); + if (!aboutToCloseFlag && !changingViewsFlag) + { + QDockWidget* temp = qobject_cast(sender()); - if (temp) { - QHashIterator i(dockWidgets); + if (temp) + { + QHashIterator i(dockWidgets); while (i.hasNext()) { - i.next(); + i.next(); if ((static_cast (dockWidgets[i.key()])) == temp) { - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); - settings.setValue(chKey,vis); - settings.sync(); - toolsMenuActions[i.key()]->setChecked(vis); - break; - } + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); + settings.setValue(chKey,vis); + toolsMenuActions[i.key()]->setChecked(vis); + break; + } + } } - } } } void MainWindow::updateLocationSettings (Qt::DockWidgetArea location) { - QDockWidget* temp = qobject_cast(sender()); + QDockWidget* temp = qobject_cast(sender()); - QHashIterator i(dockWidgets); + QHashIterator i(dockWidgets); while (i.hasNext()) { - i.next(); + i.next(); if ((static_cast (dockWidgets[i.key()])) == temp) { - QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast(i.key()), currentView); - settings.setValue(posKey,location); - break; - } - } + QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast(i.key()), currentView); + settings.setValue(posKey,location); + break; + } + } } /** @@ -708,7 +802,7 @@ void MainWindow::connectCommonWidgets() void MainWindow::createCustomWidget() { - qDebug() << "ADDING CUSTOM WIDGET"; + //qDebug() << "ADDING CUSTOM WIDGET"; QGCToolWidget* tool = new QGCToolWidget(this); QDockWidget* dock = new QDockWidget("Unnamed Tool", this); dock->setWidget(tool); @@ -724,13 +818,13 @@ void MainWindow::connectPxWidgets() void MainWindow::connectSlugsWidgets() { if (slugsHilSimWidget && slugsHilSimWidget->widget()){ - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), - slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); } if (slugsDataWidget && slugsDataWidget->widget()){ - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), - slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*))); } @@ -753,10 +847,10 @@ void MainWindow::arrangeCommonCenterStack() void MainWindow::arrangePxCenterStack() { - if (!centerStack) { - qDebug() << "Center Stack not Created!"; - return; - } + if (!centerStack) { + qDebug() << "Center Stack not Created!"; + return; + } if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget); @@ -777,10 +871,10 @@ void MainWindow::arrangePxCenterStack() void MainWindow::arrangeSlugsCenterStack() { - if (!centerStack) { - qDebug() << "Center Stack not Created!"; - return; - } + if (!centerStack) { + qDebug() << "Center Stack not Created!"; + return; + } if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget); if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget); @@ -858,8 +952,25 @@ void MainWindow::saveScreen() */ void MainWindow::reloadStylesheet() { + QString fileName = QCoreApplication::applicationDirPath() + "/style-indoor.css"; + + // Let user select style sheet + fileName = QFileDialog::getOpenFileName(this, tr("Specify stylesheet"), fileName, tr("CSS Stylesheet (*.css);;")); + + if (!fileName.endsWith(".css")) + { + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Information); + msgBox.setText(tr("QGroundControl did lot load a new style")); + msgBox.setInformativeText(tr("No suitable .css file selected. Please select a valid .css file.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + return; + } + // Load style sheet - QFile* styleSheet = new QFile(QCoreApplication::applicationDirPath() + "/qgroundcontrol.css"); + QFile* styleSheet = new QFile(fileName); if (!styleSheet->exists()) { styleSheet = new QFile(":/images/style-mission.css"); @@ -872,7 +983,13 @@ void MainWindow::reloadStylesheet() } else { - qDebug() << "Style not set:" << styleSheet->fileName() << "opened: " << styleSheet->isOpen(); + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Information); + msgBox.setText(tr("QGroundControl did lot load a new style")); + msgBox.setInformativeText(tr("Stylesheet file %1 was not readable").arg(fileName)); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); } delete styleSheet; } @@ -932,6 +1049,7 @@ void MainWindow::connectCommonActions() // Connect internal actions connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(UASCreated(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); // Unmanned System controls connect(ui.actionLiftoff, SIGNAL(triggered()), UASManager::instance(), SLOT(launchActiveUAS())); @@ -950,25 +1068,17 @@ void MainWindow::connectCommonActions() connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); // Help Actions - connect(ui.actionOnline_documentation, SIGNAL(triggered()), this, SLOT(showHelp())); + connect(ui.actionOnline_Documentation, SIGNAL(triggered()), this, SLOT(showHelp())); connect(ui.actionDeveloper_Credits, SIGNAL(triggered()), this, SLOT(showCredits())); - connect(ui.actionProject_Roadmap, SIGNAL(triggered()), this, SLOT(showRoadMap())); - - // Add option for custom widgets - connect(ui.actionNewCustomWidget, SIGNAL(triggered()), this, SLOT(createCustomWidget())); - - // Allow to mute audio - ui.actionMuteAudioOutput->setChecked(GAudioOutput::instance()->isMuted()); - connect(ui.actionMuteAudioOutput, SIGNAL(triggered(bool)), GAudioOutput::instance(), SLOT(mute(bool))); - + connect(ui.actionProject_Roadmap_2, SIGNAL(triggered()), this, SLOT(showRoadMap())); } void MainWindow::connectPxActions() { - ui.actionJoystickSettings->setVisible(true); + ui.actionJoystickSettings->setVisible(true); - // Joystick configuration - connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); + // Joystick configuration + connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); } @@ -979,7 +1089,7 @@ void MainWindow::connectSlugsActions() void MainWindow::showHelp() { - if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/user_guide"))) + if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/users/"))) { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); @@ -993,7 +1103,7 @@ void MainWindow::showHelp() void MainWindow::showCredits() { - if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/credits"))) + if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/credits/"))) { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); @@ -1007,7 +1117,7 @@ void MainWindow::showCredits() void MainWindow::showRoadMap() { - if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/roadmap"))) + if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/roadmap/"))) { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); @@ -1044,6 +1154,8 @@ void MainWindow::addLink(LinkInterface *link) CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this); ui.menuNetwork->addAction(commWidget->getAction()); + //qDebug() << "ADDING LINK:" << link->getName() << "ACTION IS: " << commWidget->getAction(); + // Special case for simulationlink MAVLinkSimulationLink* sim = dynamic_cast(link); if (sim) @@ -1053,12 +1165,23 @@ void MainWindow::addLink(LinkInterface *link) } } +void MainWindow::setActiveUAS(UASInterface* uas) +{ + // Enable and rename menu + ui.menuUnmanned_System->setTitle(uas->getUASName()); + if (!ui.menuUnmanned_System->isEnabled()) ui.menuUnmanned_System->setEnabled(true); +} + void MainWindow::UASCreated(UASInterface* uas) { + // Connect the UAS to the full user interface if (uas != NULL) { + // The pilot view was not available on startup, enable it now + ui.actionPilotsView->setEnabled(true); + QIcon icon; // Set matching icon switch (uas->getSystemType()) @@ -1119,42 +1242,35 @@ void MainWindow::UASCreated(UASInterface* uas) } } - // Camera view - //camera->addUAS(uas); - - // Revalidate UI - // TODO Stylesheet reloading should in theory not be necessary - reloadStylesheet(); - switch (uas->getAutopilotType()) - { - case (MAV_AUTOPILOT_SLUGS): - { - // Build Slugs Widgets - buildSlugsWidgets(); + { + case (MAV_AUTOPILOT_SLUGS): + { + // Build Slugs Widgets + buildSlugsWidgets(); - // Connect Slugs Widgets - connectSlugsWidgets(); + // Connect Slugs Widgets + connectSlugsWidgets(); - // Arrange Slugs Centerstack - arrangeSlugsCenterStack(); + // Arrange Slugs Centerstack + arrangeSlugsCenterStack(); - // Connect Slugs Actions - connectSlugsActions(); + // Connect Slugs Actions + connectSlugsActions(); - // FIXME: This type checking might be redundant + // FIXME: This type checking might be redundant // if (slugsDataWidget) { // SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); // if (dataWidget) { // SlugsMAV* mav2 = dynamic_cast(uas); // if (mav2) { - (dynamic_cast(slugsDataWidget->widget()))->addUAS(uas); + (dynamic_cast(slugsDataWidget->widget()))->addUAS(uas); // //loadSlugsView(); // loadGlobalOperatorView(); // } // } // } - } + } break; default: case (MAV_AUTOPILOT_GENERIC): @@ -1172,11 +1288,11 @@ void MainWindow::UASCreated(UASInterface* uas) // Connect Pixhawk Actions connectPxActions(); - } + } break; loadOperatorView(); - } + } // Change the view only if this is the first UAS @@ -1184,22 +1300,31 @@ void MainWindow::UASCreated(UASInterface* uas) // the currently active UAS if (UASManager::instance()->getActiveUAS() == uas) { - qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM"; + //qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM"; // Load last view if setting is present - if (settings.contains("VIEW_ON_APPLICATION_CLOSE")) - { - int view = settings.value("VIEW_ON_APPLICATION_CLOSE").toInt(); + if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED")) + { + int view = settings.value("CURRENT_VIEW_WITH_UAS_CONNECTED").toInt(); currentView = (VIEW_SECTIONS) view; presentView(); - } + + // Restore the widget positions and size + if (settings.contains(getWindowStateKey())) + { + restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion()); + } + + } else - { + { loadEngineerView(); - } - } + } + } } + + if (!ui.menuConnected_Systems->isEnabled()) ui.menuConnected_Systems->setEnabled(true); } /** @@ -1207,6 +1332,31 @@ void MainWindow::UASCreated(UASInterface* uas) */ void MainWindow::clearView() { + // Save current state + if (UASManager::instance()->getActiveUAS()) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); + settings.setValue(getWindowGeometryKey(), saveGeometry()); + + QAction* temp; + + // Set tool widget visibility settings for this view + foreach (int key, toolsMenuActions.keys()) + { + temp = toolsMenuActions[key]; + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(key), currentView); + + if (temp) + { + //qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked(); + settings.setValue(chKey,temp->isChecked()); + } + else + { + //qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED"; + settings.setValue(chKey,false); + } + } + + changingViewsFlag = true; // Remove all dock widgets from main window QObjectList childList( this->children() ); @@ -1225,60 +1375,70 @@ void MainWindow::clearView() // Is there a way to unset a widget from QDockWidget? } } + changingViewsFlag = false; } void MainWindow::loadEngineerView() { - clearView(); + if (currentView != VIEW_ENGINEER) + { + clearView(); - currentView = VIEW_ENGINEER; + currentView = VIEW_ENGINEER; - presentView(); + presentView(); + } } void MainWindow::loadOperatorView() { + if (currentView != VIEW_OPERATOR) + { clearView(); currentView = VIEW_OPERATOR; presentView(); + } } void MainWindow::loadPilotView() { + if (currentView != VIEW_PILOT) + { clearView(); currentView = VIEW_PILOT; presentView(); + } } void MainWindow::loadMAVLinkView() { + if (currentView != VIEW_MAVLINK) + { clearView(); currentView = VIEW_MAVLINK; presentView(); + } } void MainWindow::presentView() { + // LINE CHART + showTheCentralWidget(CENTRAL_LINECHART, currentView); - qDebug() << "LC"; - showTheCentralWidget(CENTRAL_LINECHART, currentView); + // MAP + showTheCentralWidget(CENTRAL_MAP, currentView); - // MAP - qDebug() << "MAP"; - showTheCentralWidget(CENTRAL_MAP, currentView); + // PROTOCOL + showTheCentralWidget(CENTRAL_PROTOCOL, currentView); - // PROTOCOL - qDebug() << "CP"; - showTheCentralWidget(CENTRAL_PROTOCOL, currentView); - - // HEAD UP DISPLAY - showTheCentralWidget(CENTRAL_HUD, currentView); + // HEAD UP DISPLAY + showTheCentralWidget(CENTRAL_HUD, currentView); // GOOGLE EARTH showTheCentralWidget(CENTRAL_GOOGLE_EARTH, currentView); @@ -1289,209 +1449,133 @@ void MainWindow::presentView() // GLOBAL 3D VIEW showTheCentralWidget(CENTRAL_3D_MAP, currentView); + // DATA PLOT + showTheCentralWidget(CENTRAL_DATA_PLOT, currentView); + - // Show docked widgets based on current view and autopilot type + // Show docked widgets based on current view and autopilot type - // UAS CONTROL - showTheWidget(MENU_UAS_CONTROL, currentView); + // UAS CONTROL + showTheWidget(MENU_UAS_CONTROL, currentView); - // UAS LIST - showTheWidget(MENU_UAS_LIST, currentView); + // UAS LIST + showTheWidget(MENU_UAS_LIST, currentView); - // WAYPOINT LIST - showTheWidget(MENU_WAYPOINTS, currentView); + // WAYPOINT LIST + showTheWidget(MENU_WAYPOINTS, currentView); - // UAS STATUS - showTheWidget(MENU_STATUS, currentView); + // UAS STATUS + showTheWidget(MENU_STATUS, currentView); - // DETECTION - showTheWidget(MENU_DETECTION, currentView); + // DETECTION + showTheWidget(MENU_DETECTION, currentView); - // DEBUG CONSOLE - showTheWidget(MENU_DEBUG_CONSOLE, currentView); + // DEBUG CONSOLE + showTheWidget(MENU_DEBUG_CONSOLE, currentView); - // ONBOARD PARAMETERS - showTheWidget(MENU_PARAMETERS, currentView); + // ONBOARD PARAMETERS + showTheWidget(MENU_PARAMETERS, currentView); - // WATCHDOG - showTheWidget(MENU_WATCHDOG, currentView); + // WATCHDOG + showTheWidget(MENU_WATCHDOG, currentView); - // HUD - showTheWidget(MENU_HUD, currentView); - if (headUpDockWidget) - { - HUD* tmpHud = dynamic_cast( headUpDockWidget->widget() ); - if (tmpHud){ - if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HUD,currentView)).toBool()){ - addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HUD, currentView)).toInt()), + // HUD + showTheWidget(MENU_HUD, currentView); + if (headUpDockWidget) + { + HUD* tmpHud = dynamic_cast( headUpDockWidget->widget() ); + if (tmpHud) + { + if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HUD,currentView)).toBool()) + { + addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HUD, currentView)).toInt()), headUpDockWidget); - headUpDockWidget->show(); - } else { - headUpDockWidget->hide(); + headUpDockWidget->show(); + } + else + { + headUpDockWidget->hide(); + } } - } - } + } - // RC View - showTheWidget(MENU_RC_VIEW, currentView); + // RC View + showTheWidget(MENU_RC_VIEW, currentView); - // SLUGS DATA - showTheWidget(MENU_SLUGS_DATA, currentView); + // SLUGS DATA + showTheWidget(MENU_SLUGS_DATA, currentView); - // SLUGS PID - showTheWidget(MENU_SLUGS_PID, currentView); + // SLUGS PID + showTheWidget(MENU_SLUGS_PID, currentView); - // SLUGS HIL - showTheWidget(MENU_SLUGS_HIL, currentView); + // SLUGS HIL + showTheWidget(MENU_SLUGS_HIL, currentView); - // SLUGS CAMERA - showTheWidget(MENU_SLUGS_CAMERA, currentView); + // SLUGS CAMERA + showTheWidget(MENU_SLUGS_CAMERA, currentView); - // HORIZONTAL SITUATION INDICATOR - showTheWidget(MENU_HSI, currentView); - // if (hsiDockWidget) - // { - // HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - // if (hsi){ - // if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HSI,currentView)).toBool()){ - // addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HSI, currentView)).toInt()), - // hsiDockWidget); - // } - // } - // } + // HORIZONTAL SITUATION INDICATOR + showTheWidget(MENU_HSI, currentView); - // HEAD DOWN 1 - showTheWidget(MENU_HDD_1, currentView); - // if (headDown1DockWidget) - // { - // HDDisplay *hdd = dynamic_cast(headDown1DockWidget->widget()); - // if (hdd) { - // if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HDD_1,currentView)).toBool()) { - // addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HDD_1, currentView)).toInt()), - // headDown1DockWidget); - // headDown1DockWidget->show(); - // hdd->start(); - // } else { - // headDown1DockWidget->hide();; - // hdd->stop(); - // } - // } - // } + // HEAD DOWN 1 + showTheWidget(MENU_HDD_1, currentView); - // HEAD DOWN 2 - showTheWidget(MENU_HDD_2, currentView); - // if (headDown2DockWidget) - // { - // HDDisplay *hdd = dynamic_cast(headDown2DockWidget->widget()); - // if (hdd){ - // if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HDD_2,currentView)).toBool()){ - // addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HDD_2, currentView)).toInt()), - // headDown2DockWidget); - // headDown2DockWidget->show(); - // hdd->start(); - // } else { - // headDown2DockWidget->hide(); - // hdd->stop(); - // } - // } - // } + // HEAD DOWN 2 + showTheWidget(MENU_HDD_2, currentView); - this->show(); + // MAVLINK LOG PLAYER + showTheWidget(MENU_MAVLINK_LOG_PLAYER, currentView); + + this->show(); } void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view) { - bool tempVisible; - QWidget* tempWidget = dockWidgets[centralWidget]; + bool tempVisible; + QWidget* tempWidget = dockWidgets[centralWidget]; - tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view)).toBool(); -// qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; + tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view), false).toBool(); + //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; if (toolsMenuActions[centralWidget]) { - toolsMenuActions[centralWidget]->setChecked(tempVisible); - } + toolsMenuActions[centralWidget]->setChecked(tempVisible); + } if (centerStack && tempWidget && tempVisible) { - centerStack->setCurrentWidget(tempWidget); - } -} - -void MainWindow::loadWidgets() -{ - loadOperatorView(); - //loadMAVLinkView(); - //loadPilotView(); -} - -void MainWindow::loadDataView(QString fileName) -{ - // DATAPLOT - if (dataplotWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(dataplotWidget); - dataplotWidget->loadFile(fileName); - } + centerStack->setCurrentWidget(tempWidget); } } -void MainWindow::load3DMapView() +void MainWindow::loadDataView(QString fileName) { -#ifdef QGC_OSGEARTH_ENABLED - clearView(); + clearView(); - // 3D map - if (_3DMapWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DMapWidget); - } - } -#endif -} + // Unload line chart + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(CENTRAL_LINECHART), currentView); + settings.setValue(chKey,false); -void MainWindow::loadGoogleEarthView() -{ -#if (defined _MSC_VER) | (defined Q_OS_MAC) - clearView(); + // Set data plot in settings as current widget and then run usual update procedure + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(CENTRAL_DATA_PLOT), currentView); + settings.setValue(chKey,true); - // 3D map - if (gEarthWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(gEarthWidget); - } - } -#endif + presentView(); + // Plot is now selected, now load data from file + if (dataplotWidget) + { + dataplotWidget->loadFile(fileName); + } +// QStackedWidget *centerStack = dynamic_cast(centralWidget()); +// if (centerStack) +// { +// centerStack->setCurrentWidget(dataplotWidget); +// dataplotWidget->loadFile(fileName); +// } +// } } -void MainWindow::load3DView() -{ -#ifdef QGC_OSG_ENABLED - clearView(); - - // 3D map - if (_3DWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DWidget); - } - } -#endif -} diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 1d9e61d707cfa1612f82c977745796f8c5aec6cd..62416469c2b6097dad5d761f8cd41977e5e2603f 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -104,6 +104,9 @@ public slots: void addLink(); void addLink(LinkInterface* link); void configure(); + /** @brief Set the currently controlled UAS */ + void setActiveUAS(UASInterface* uas); + /** @brief Add a new UAS */ void UASCreated(UASInterface* uas); void startVideoCapture(); void stopVideoCapture(); @@ -142,19 +145,19 @@ public slots: ========================================================== */ - void loadWidgets(); +// void loadWidgets(); /** @brief Load data view, allowing to plot flight data */ void loadDataView(QString fileName); - /** @brief Load 3D map view */ - void load3DMapView(); +// /** @brief Load 3D map view */ +// void load3DMapView(); - /** @brief Load 3D Google Earth view */ - void loadGoogleEarthView(); +// /** @brief Load 3D Google Earth view */ +// void loadGoogleEarthView(); - /** @brief Load 3D view */ - void load3DView(); +// /** @brief Load 3D view */ +// void load3DView(); /** * @brief Shows a Docked Widget based on the action sender @@ -208,6 +211,7 @@ protected: MENU_SLUGS_PID, MENU_SLUGS_HIL, MENU_SLUGS_CAMERA, + MENU_MAVLINK_LOG_PLAYER, CENTRAL_SEPARATOR= 255, // do not change CENTRAL_LINECHART, CENTRAL_PROTOCOL, @@ -297,6 +301,7 @@ protected: /** @brief Keeps track of the current view */ VIEW_SECTIONS currentView; bool aboutToCloseFlag; + bool changingViewsFlag; void clearView(); @@ -360,6 +365,7 @@ protected: QPointer watchdogControlDockWidget; QPointer headUpDockWidget; + QPointer logPlayerDockWidget; QPointer hsiDockWidget; QPointer rcViewDockWidget; @@ -393,6 +399,8 @@ private: Ui::MainWindow ui; QString buildMenuKey (SETTINGS_SECTIONS section , TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view); + QString getWindowStateKey(); + QString getWindowGeometryKey(); }; diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index 285640565f732a0be70aadf02632ec65e7d95955..c8165aea35e0cb3570a85358432334a3e21071a0 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -38,7 +38,7 @@ 0 0 1000 - 25 + 22 @@ -46,25 +46,33 @@ File - - + + + + - Network + Communication + + false + Select System + + false + Unmanned System @@ -75,13 +83,12 @@ - - Tools + @@ -100,7 +107,6 @@ - @@ -124,6 +130,9 @@ + + true + :/images/control/launch.svg @@ -143,6 +152,10 @@ + + + :/images/actions/process-stop.svg:/images/actions/process-stop.svg + Emergency Land @@ -151,6 +164,10 @@ + + + :/images/actions/process-stop.svg:/images/actions/process-stop.svg + Kill UAS @@ -179,33 +196,6 @@ Open UAS Preferences - - - - :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg - - - Show engineer view - - - - - - :/images/status/weather-overcast.svg:/images/status/weather-overcast.svg - - - Show pilot view - - - - - - :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg - - - Reload visual style - - @@ -218,30 +208,6 @@ true - - - - :/images/status/network-wireless-encrypted.svg:/images/status/network-wireless-encrypted.svg - - - Show operator view - - - Shop the 2D map and system status - - - - - - :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg - - - Show 3D Local View - - - Show 3D view - - true @@ -257,87 +223,6 @@ Simulate one vehicle to test and evaluate this application - - - - :/images/status/network-transmit-receive.svg:/images/status/network-transmit-receive.svg - - - Show full view - - - - - - :/images/devices/network-wired.svg:/images/devices/network-wired.svg - - - Show MAVLink view - - - - - - :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg - - - Online documentation - - - - - - :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg - - - Show data analysis view - - - - - - :/images/status/software-update-available.svg:/images/status/software-update-available.svg - - - Project Roadmap - - - - - - :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg - - - Credits / Developers - - - - - - :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg - - - Show 2D Map View - - - - - - :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg - - - Show 3D Global View - - - - - - :/images/mapproviders/googleearth.svg:/images/mapproviders/googleearth.svg - - - Google Earth View - - @@ -387,6 +272,9 @@ + + true + :/images/status/weather-overcast.svg:/images/status/weather-overcast.svg @@ -396,6 +284,9 @@ + + true + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg @@ -405,6 +296,9 @@ + + true + :/images/devices/network-wired.svg:/images/devices/network-wired.svg @@ -419,10 +313,13 @@ :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg - Reload Style + Load Stylesheet + + true + :/images/status/network-wireless-encrypted.svg:/images/status/network-wireless-encrypted.svg @@ -432,6 +329,10 @@ + + + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg + New Custom Widget @@ -452,6 +353,18 @@ Mute Audio Output + + + + :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg + + + Preferences + + + QGroundControl global settings + + diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc new file mode 100644 index 0000000000000000000000000000000000000000..1a4364fd36d35828d5bf974604f7d4c210330520 --- /dev/null +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -0,0 +1,284 @@ +#include +#include +#include + +#include "MainWindow.h" +#include "QGCMAVLinkLogPlayer.h" +#include "QGC.h" +#include "ui_QGCMAVLinkLogPlayer.h" + +QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *parent) : + QWidget(parent), + lineCounter(0), + totalLines(0), + startTime(0), + endTime(0), + currentStartTime(0), + accelerationFactor(1.0f), + mavlink(mavlink), + logLink(NULL), + loopCounter(0), + ui(new Ui::QGCMAVLinkLogPlayer) +{ + ui->setupUi(this); + ui->gridLayout->setAlignment(Qt::AlignTop); + + // Setup timer + connect(&loopTimer, SIGNAL(timeout()), this, SLOT(logLoop())); + + // Setup buttons + connect(ui->selectFileButton, SIGNAL(clicked()), this, SLOT(selectLogFile())); + connect(ui->pauseButton, SIGNAL(clicked()), this, SLOT(pause())); + connect(ui->playButton, SIGNAL(clicked()), this, SLOT(play())); + connect(ui->speedSlider, SIGNAL(valueChanged(int)), this, SLOT(setAccelerationFactorInt(int))); + + setAccelerationFactorInt(49); + ui->speedSlider->setValue(49); +} + +QGCMAVLinkLogPlayer::~QGCMAVLinkLogPlayer() +{ + delete ui; +} + +void QGCMAVLinkLogPlayer::play() +{ + if (logFile.isOpen()) + { + ui->pauseButton->setChecked(false); + ui->selectFileButton->setEnabled(false); + if (logLink != NULL) + { + delete logLink; + } + logLink = new MAVLinkSimulationLink(""); + + // Start timer + loopTimer.start(1); + } + else + { + ui->playButton->setChecked(false); + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Information); + msgBox.setText(tr("No logfile selected")); + msgBox.setInformativeText(tr("Please select first a MAVLink log file before playing it.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + } +} + +void QGCMAVLinkLogPlayer::pause() +{ + loopTimer.stop(); + ui->playButton->setChecked(false); + ui->selectFileButton->setEnabled(true); + delete logLink; + logLink = NULL; +} + +void QGCMAVLinkLogPlayer::reset() +{ + pause(); + loopCounter = 0; + logFile.reset(); + ui->pauseButton->setChecked(true); + ui->progressBar->setValue(0); + startTime = 0; +} + +void QGCMAVLinkLogPlayer::selectLogFile() +{ + QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink Logfile (*.mavlink);;")); + + loadLogFile(fileName); +} + +/** + * @param factor 0: 0.01X, 50: 1.0X, 100: 100.0X + */ +void QGCMAVLinkLogPlayer::setAccelerationFactorInt(int factor) +{ + float f = factor+1.0f; + f -= 50.0f; + + if (f < 0.0f) + { + accelerationFactor = 1.0f / (-f/2.0f); + } + else + { + accelerationFactor = 1+(f/2.0f); + } + + //qDebug() << "FACTOR:" << accelerationFactor; + + ui->speedLabel->setText(tr("Speed: %1X").arg(accelerationFactor, 5, 'f', 2, '0')); +} + +void QGCMAVLinkLogPlayer::loadLogFile(const QString& file) +{ + // Ensure that the playback process is stopped + if (logFile.isOpen()) + { + loopTimer.stop(); + logFile.reset(); + logFile.close(); + } + logFile.setFileName(file); + + if (!logFile.open(QFile::ReadOnly)) + { + MainWindow::instance()->showCriticalMessage(tr("The selected logfile is unreadable"), tr("Please make sure that the file %1 is readable or select a different file").arg(file)); + logFile.setFileName(""); + } + else + { + QFileInfo logFileInfo(file); + logFile.reset(); + startTime = 0; + ui->logFileNameLabel->setText(tr("%1").arg(logFileInfo.baseName())); + ui->logStatsLabel->setText(tr("Log: %2 MB, %3 packets").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64)))); + } +} + +union log64 +{ + quint64 q; + const char* b; +}; + +/** + * This function is the "mainloop" of the log player, reading one line + * and adjusting the mainloop timer to read the next line in time. + * It might not perfectly match the timing of the log file, + * but it will never induce a static drift into the log file replay. + * For scientific logging, the use of onboard timestamps and the log + * functionality of the line chart plot is recommended. + */ +void QGCMAVLinkLogPlayer::logLoop() +{ + const int packetLen = MAVLINK_MAX_PACKET_LEN; + const int timeLen = sizeof(quint64); + bool ok; + + // First check initialization + if (startTime == 0) + { + QByteArray startBytes = logFile.read(timeLen); + + // Check if the correct number of bytes could be read + if (startBytes.length() != timeLen) + { + ui->logStatsLabel->setText(tr("Error reading first %1 bytes").arg(timeLen)); + MainWindow::instance()->showCriticalMessage(tr("Failed loading MAVLink Logfile"), tr("Error reading first %1 bytes from logfile. Got %2 instead of %1 bytes. Is the logfile readable?").arg(timeLen).arg(startBytes.length())); + reset(); + return; + } + + // Convert data to timestamp + startTime = *((quint64*)(startBytes.constData())); + currentStartTime = QGC::groundTimeUsecs(); + ok = true; + + //qDebug() << "START TIME: " << startTime; + + // Check if these bytes could be correctly decoded + if (!ok) + { + ui->logStatsLabel->setText(tr("Error decoding first timestamp, aborting.")); + MainWindow::instance()->showCriticalMessage(tr("Failed loading MAVLink Logfile"), tr("Could not load initial timestamp from file %1. Is the file corrupted?").arg(logFile.fileName())); + reset(); + return; + } + } + + + // Initialization seems fine, load next chunk + QByteArray chunk = logFile.read(timeLen+packetLen); + QByteArray packet = chunk.mid(0, packetLen); + + // Emit this packet + mavlink->receiveBytes(logLink, packet); + + // Check if reached end of file before reading next timestamp + if (chunk.length() < timeLen+packetLen || logFile.atEnd()) + { + // Reached end of file + reset(); + + QString status = tr("Reached end of MAVLink log file."); + ui->logStatsLabel->setText(status); + ui->progressBar->setValue(100); + MainWindow::instance()->showStatusMessage(status); + return; + } + + // End of file not reached, read next timestamp + QByteArray rawTime = chunk.mid(packetLen); + + // This is the timestamp of the next packet + quint64 time = *((quint64*)(rawTime.constData())); + ok = true; + if (!ok) + { + // Convert it to 64bit number + QString status = tr("Time conversion error during log replay. Continuing.."); + ui->logStatsLabel->setText(status); + MainWindow::instance()->showStatusMessage(status); + } + else + { + // Normal processing, passed all checks + // start timer to match time offset between + // this and next packet + + + // Offset in ms + qint64 timediff = (time - startTime)/accelerationFactor; + + // Immediately load any data within + // a 3 ms interval + + int nextExecutionTime = (((qint64)currentStartTime + (qint64)timediff) - (qint64)QGC::groundTimeUsecs())/1000; + + //qDebug() << "nextExecutionTime:" << nextExecutionTime << "QGC START TIME:" << currentStartTime << "LOG START TIME:" << startTime; + + if (nextExecutionTime < 5) + { + logLoop(); + } + else + { + loopTimer.start(nextExecutionTime); + } +// loopTimer.start(20); + + if (loopCounter % 40 == 0) + { + QFileInfo logFileInfo(logFile); + int progress = 100.0f*(loopCounter/(float)(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64)))); + //qDebug() << "Progress:" << progress; + ui->progressBar->setValue(progress); + } + loopCounter++; + // Ui update: Only every 20 messages + // to prevent flickering and high CPU load + + // Update status label + // Update progress bar + } +} + +void QGCMAVLinkLogPlayer::changeEvent(QEvent *e) +{ + QWidget::changeEvent(e); + switch (e->type()) { + case QEvent::LanguageChange: + ui->retranslateUi(this); + break; + default: + break; + } +} diff --git a/src/ui/QGCMAVLinkLogPlayer.h b/src/ui/QGCMAVLinkLogPlayer.h new file mode 100644 index 0000000000000000000000000000000000000000..eb0ea53cce3d54e5d281ef4a0be157c2be234eee --- /dev/null +++ b/src/ui/QGCMAVLinkLogPlayer.h @@ -0,0 +1,63 @@ +#ifndef QGCMAVLINKLOGPLAYER_H +#define QGCMAVLINKLOGPLAYER_H + +#include +#include + +#include "MAVLinkProtocol.h" +#include "MAVLinkSimulationLink.h" + +namespace Ui { + class QGCMAVLinkLogPlayer; +} + +/** + * @brief Replays MAVLink log files + * + * This class allows to replay MAVLink logs at varying speeds. + * captured flights can be replayed, shown to others and analyzed + * in-depth later on. + */ +class QGCMAVLinkLogPlayer : public QWidget +{ + Q_OBJECT + +public: + explicit QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *parent = 0); + ~QGCMAVLinkLogPlayer(); + +public slots: + /** @brief Replay the logfile */ + void play(); + /** @brief Pause the logfile */ + void pause(); + /** @brief Reset the logfile */ + void reset(); + /** @brief Select logfile */ + void selectLogFile(); + /** @brief Load log file */ + void loadLogFile(const QString& file); + /** @brief The logging mainloop */ + void logLoop(); + /** @brief Set acceleration factor in percent */ + void setAccelerationFactorInt(int factor); + +protected: + int lineCounter; + int totalLines; + quint64 startTime; + quint64 endTime; + quint64 currentStartTime; + float accelerationFactor; + MAVLinkProtocol* mavlink; + MAVLinkSimulationLink* logLink; + QFile logFile; + QTimer loopTimer; + int loopCounter; + void changeEvent(QEvent *e); + +private: + Ui::QGCMAVLinkLogPlayer *ui; +}; + +#endif // QGCMAVLINKLOGPLAYER_H diff --git a/src/ui/QGCMAVLinkLogPlayer.ui b/src/ui/QGCMAVLinkLogPlayer.ui new file mode 100644 index 0000000000000000000000000000000000000000..780c3ba4d9eaf363994c05f22e76252093b98aa3 --- /dev/null +++ b/src/ui/QGCMAVLinkLogPlayer.ui @@ -0,0 +1,165 @@ + + + QGCMAVLinkLogPlayer + + + + 0 + 0 + 407 + 144 + + + + Form + + + + 12 + + + + + Please choose logfile + + + + + + + Select the logfile to replay + + + Select the logfile to replay + + + Select the logfile to replay + + + Select File + + + + + + + Set the replay speed + + + Set the replay speed + + + Set the replay speed + + + 1 + + + 100 + + + 50 + + + 50 + + + Qt::Horizontal + + + + + + + No logfile selected.. + + + + + + + Pause the logfile + + + Pause the logfile + + + Pause the logfile + + + ... + + + + :/images/actions/media-playback-pause.svg:/images/actions/media-playback-pause.svg + + + true + + + true + + + + + + + Percent of the logfile replayed + + + Percent of the logfile replayed + + + Percent of the logfile replayed + + + 0 + + + + + + + Current replay speed + + + Current replay speed + + + Current replay speed + + + Speed + + + + + + + Start to replay the logfile + + + Start to replay the logfile + + + Start to replay the logfile + + + ... + + + + :/images/actions/media-playback-start.svg:/images/actions/media-playback-start.svg + + + true + + + + + + + + + + diff --git a/src/ui/QGCRemoteControlView.cc b/src/ui/QGCRemoteControlView.cc index a00c83ff50e6a2b272f7a7947c6117a44a6b63fd..519598ab2fba406020f7285509006fae1c60814c 100644 --- a/src/ui/QGCRemoteControlView.cc +++ b/src/ui/QGCRemoteControlView.cc @@ -197,7 +197,7 @@ void QGCRemoteControlView::redraw() // Update raw values for(int i = 0; i < rawLabels.count(); i++) { - rawLabels.at(i)->setText(QString("%1 us").arg(raw.at(i), 4)); + rawLabels.at(i)->setText(QString("%1 us").arg(raw.at(i), 4, 10, QChar('0'))); } // Update percent bars diff --git a/src/ui/SerialConfigurationWindow.cc b/src/ui/SerialConfigurationWindow.cc index 11bd3dd915c661b4d4b2095f1eda438290193ace..f7305bc37b03c82617aa7380ba0cd9396de9f0b4 100644 --- a/src/ui/SerialConfigurationWindow.cc +++ b/src/ui/SerialConfigurationWindow.cc @@ -239,12 +239,17 @@ userConfigured(false) // Setup the user interface according to link type ui.setupUi(this); + //this->setVisible(false); + //this->hide(); // Create action to open this menu // Create configuration action for this link // Connect the current UAS action = new QAction(QIcon(":/images/devices/network-wireless.svg"), "", link); setLinkName(link->getName()); + + setupPortList(); + connect(action, SIGNAL(triggered()), this, SLOT(configureCommunication())); // Make sure that a change in the link name will be reflected in the UI @@ -258,10 +263,8 @@ userConfigured(false) connect(ui.parNone, SIGNAL(toggled(bool)), this, SLOT(setParityNone())); connect(ui.parOdd, SIGNAL(toggled(bool)), this, SLOT(setParityOdd())); connect(ui.parEven, SIGNAL(toggled(bool)), this, SLOT(setParityEven())); - connect(ui.dataBitsSpinBox, SIGNAL(valueChanged(int)), this->link, SLOT(setDataBitsType(int))); - connect(ui.stopBitsSpinBox, SIGNAL(valueChanged(int)), this->link, SLOT(setStopBitsType(int))); - - setupPortList(); + connect(ui.dataBitsSpinBox, SIGNAL(valueChanged(int)), this->link, SLOT(setDataBits(int))); + connect(ui.stopBitsSpinBox, SIGNAL(valueChanged(int)), this->link, SLOT(setStopBits(int))); //connect(this->link, SIGNAL(connected(bool)), this, SLOT()); ui.portName->setSizeAdjustPolicy(QComboBox::AdjustToContentsOnFirstShow); @@ -299,8 +302,8 @@ userConfigured(false) ui.baudRate->setCurrentIndex(this->link->getBaudRateType()); - ui.dataBitsSpinBox->setValue(this->link->getDataBitsType() + 5); - ui.stopBitsSpinBox->setValue(this->link->getStopBitsType() + 1); + ui.dataBitsSpinBox->setValue(this->link->getDataBits()); + ui.stopBitsSpinBox->setValue(this->link->getStopBits()); portCheckTimer = new QTimer(this); portCheckTimer->setInterval(1000); @@ -308,7 +311,7 @@ userConfigured(false) // Display the widget this->window()->setWindowTitle(tr("Serial Communication Settings")); - this->show(); + //this->show(); } else { @@ -323,18 +326,14 @@ SerialConfigurationWindow::~SerialConfigurationWindow() { void SerialConfigurationWindow::showEvent(QShowEvent* event) { - if (event->isAccepted()) - { - portCheckTimer->start(); - } + Q_UNUSED(event); + portCheckTimer->start(); } void SerialConfigurationWindow::hideEvent(QHideEvent* event) { - if (event->isAccepted()) - { - portCheckTimer->stop(); - } + Q_UNUSED(event); + portCheckTimer->stop(); } QAction* SerialConfigurationWindow::getAction() diff --git a/src/ui/SerialSettings.ui b/src/ui/SerialSettings.ui index b116400b26115baed98620bef520ee674324b5ce..491078404740c467bd335e700ce5be20a171b2b9 100644 --- a/src/ui/SerialSettings.ui +++ b/src/ui/SerialSettings.ui @@ -26,6 +26,15 @@ + + The serial port to which the system is connected. All ports listed here should work. + + + The serial port to which the system is connected. All ports listed here should work. + + + The serial port to which the system is connected. All ports listed here should work. + true @@ -48,6 +57,15 @@ + + The data transmission rate. If unsure 57600 and 115200 are very common rates. + + + The data transmission rate. If unsure 57600 and 115200 are very common rates. + + + The data transmission rate. If unsure 57600 and 115200 are very common rates. + false @@ -190,6 +208,15 @@ + + Activate / deactivate hardware flow control. Commonly deactivated + + + Activate / deactivate hardware flow control. Commonly deactivated + + + Activate / deactivate hardware flow control. Commonly deactivated + Active @@ -206,6 +233,15 @@ + + Set the parity. In most cases no parity (None) is used. + + + Set the parity. In most cases no parity (None) is used. + + + Set the parity. In most cases no parity (None) is used. + None @@ -216,6 +252,15 @@ + + Set the parity. In most cases no parity (None) is used. + + + Set the parity. In most cases no parity (None) is used. + + + Set the parity. In most cases no parity (None) is used. + Odd @@ -223,6 +268,15 @@ + + Set the parity. In most cases no parity (None) is used. + + + Set the parity. In most cases no parity (None) is used. + + + Set the parity. In most cases no parity (None) is used. + Even @@ -251,6 +305,15 @@ 0 + + Number of data bits per symbol. This is almost always 8. + + + Number of data bits per symbol. This is almost always 8. + + + Number of data bits per symbol. This is almost always 8. + 5 @@ -277,6 +340,15 @@ 0 + + Number of stop bits per symbol. This is almost always 2. + + + Number of stop bits per symbol. This is almost always 2. + + + Number of stop bits per symbol. This is almost always 2. + true @@ -315,6 +387,12 @@ Delete this link + + Delete this link + + + Link delete button + @@ -323,6 +401,12 @@ Connect this link + + Connect this link + + + Connect this link + @@ -331,6 +415,12 @@ Close the configuration window + + Close the configuration window + + + Close the configuration window + diff --git a/src/ui/WaypointList.ui b/src/ui/WaypointList.ui index 587a8c929dea70c5c6745fbb82d424f6e66025a9..c00a83ca2b503274df6a39e6486a6c748ba7a4d4 100644 --- a/src/ui/WaypointList.ui +++ b/src/ui/WaypointList.ui @@ -36,8 +36,8 @@ 0 0 - 466 - 163 + 464 + 158 @@ -48,7 +48,17 @@ 4 - + + + Waypoint list. The list is empty until you issue a read command or add waypoints. + + + Waypoint list. The list is empty until you issue a read command or add waypoints. + + + Waypoint list. The list is empty until you issue a read command or add waypoints. + + @@ -56,6 +66,15 @@ + + Read all waypoints from the MAV. Clears the list on this computer. + + + Read all waypoints from the MAV. Clears the list on this computer. + + + Read all waypoints from the MAV. Clears the list on this computer. + Read @@ -67,6 +86,15 @@ + + Transmit all waypoints on this list to the MAV. + + + Transmit all waypoints on this list to the MAV. + + + Transmit all waypoints on this list to the MAV. + Write @@ -78,6 +106,15 @@ + + The current waypoint transmission status + + + The current waypoint transmission status + + + The current waypoint transmission status + TextLabel @@ -85,6 +122,15 @@ + + Add a new waypoint to this list. Transmit via write to the MAV. + + + Add a new waypoint to this list. Transmit via write to the MAV. + + + Add a new waypoint to this list. Transmit via write to the MAV. + ... @@ -96,6 +142,15 @@ + + Load waypoints from a file on the local harddisk. Does not load the waypoints on the MAV. + + + Load waypoints from a file on the local harddisk. Does not load the waypoints on the MAV. + + + Load waypoints from a file on the local harddisk. Does not load the waypoints on the MAV. + Load WPs @@ -103,6 +158,15 @@ + + Save waypoints to a file on the local harddisk. Does not save them on the MAV. + + + Save waypoints to a file on the local harddisk. Does not save them on the MAV. + + + Save waypoints to a file on the local harddisk. Does not save them on the MAV. + Save WPs @@ -126,6 +190,12 @@ Set the current vehicle position as new waypoint + + Set the current vehicle position as new waypoint + + + Set the current vehicle position as new waypoint + ... @@ -136,9 +206,18 @@ - + + + Delete all waypoints from this list. You have to click write to clear the list on the MAV as well. + + + Delete all waypoints from this list. You have to click write to clear the list on the MAV as well. + + + Delete all waypoints from this list. You have to click write to clear the list on the MAV as well. + - + ... diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 606c3bddf61be9313659c276b2e481b0c196b077..85af32ffe6d14cba76bfb6499ebc5d4d69a0ced8 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -228,7 +228,8 @@ void LinechartPlot::appendData(QString dataname, quint64 ms, double value) datalock.lock(); /* Check if dataset identifier already exists */ - if(!data.contains(dataname)) { + if(!data.contains(dataname)) + { addCurve(dataname); } diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 14c42cec50a18ec9e26ffbd4a131ce4d05353a59..704b83bb2eb46d19f6eec49077c034917d16ad3b 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -35,6 +35,7 @@ This file is part of the PIXHAWK project #include #include #include +#include #include #include #include @@ -69,56 +70,66 @@ curveMenu(new QMenu(this)), logFile(new QFile()), logindex(1), logging(false), +logStartTime(0), updateTimer(new QTimer()) { // Add elements defined in Qt Designer ui.setupUi(this); - this->setMinimumSize(400, 250); + this->setMinimumSize(300, 200); // Add and customize curve list elements (left side) curvesWidget = new QWidget(ui.curveListWidget); ui.curveListWidget->setWidget(curvesWidget); - curvesWidgetLayout = new QVBoxLayout(curvesWidget); + curvesWidgetLayout = new QGridLayout(curvesWidget); curvesWidgetLayout->setMargin(2); curvesWidgetLayout->setSpacing(4); - curvesWidgetLayout->setSizeConstraint(QLayout::SetMinimumSize); + //curvesWidgetLayout->setSizeConstraint(QSizePolicy::Expanding); curvesWidgetLayout->setAlignment(Qt::AlignTop); + + curvesWidgetLayout->setColumnStretch(0, 0); + curvesWidgetLayout->setColumnStretch(1, 0); + curvesWidgetLayout->setColumnStretch(2, 80); + curvesWidgetLayout->setColumnStretch(3, 50); + curvesWidgetLayout->setColumnStretch(4, 50); + curvesWidgetLayout->setColumnStretch(5, 50); +// horizontalLayout->setColumnStretch(median, 50); + curvesWidgetLayout->setColumnStretch(6, 50); + curvesWidget->setLayout(curvesWidgetLayout); // Create curve list headings - QWidget* form = new QWidget(this); - QHBoxLayout *horizontalLayout; QLabel* label; QLabel* value; QLabel* mean; QLabel* variance; - form->setAutoFillBackground(false); - horizontalLayout = new QHBoxLayout(form); - horizontalLayout->setSpacing(5); - horizontalLayout->setMargin(0); - horizontalLayout->setSizeConstraint(QLayout::SetMinimumSize); //horizontalLayout->addWidget(checkBox); - label = new QLabel(form); + int labelRow = curvesWidgetLayout->rowCount(); + + curvesWidgetLayout->addWidget(new QLabel(tr("On")), labelRow, 0, 1, 2); + + label = new QLabel(this); label->setText("Name"); - horizontalLayout->addWidget(label); + curvesWidgetLayout->addWidget(label, labelRow, 2); // Value - value = new QLabel(form); + value = new QLabel(this); value->setText("Val"); - horizontalLayout->addWidget(value); + curvesWidgetLayout->addWidget(value, labelRow, 3); + + // Unit + //curvesWidgetLayout->addWidget(new QLabel(tr("Unit")), labelRow, 4); // Mean - mean = new QLabel(form); + mean = new QLabel(this); mean->setText("Mean"); - horizontalLayout->addWidget(mean); + curvesWidgetLayout->addWidget(mean, labelRow, 5); // Variance - variance = new QLabel(form); + variance = new QLabel(this); variance->setText("Variance"); - horizontalLayout->addWidget(variance); - curvesWidgetLayout->addWidget(form); + curvesWidgetLayout->addWidget(variance, labelRow, 6); // Add and customize plot elements (right side) @@ -131,15 +142,41 @@ updateTimer(new QTimer()) updateTimer->setInterval(300); connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh())); - updateTimer->start(); + readSettings(); } -LinechartWidget::~LinechartWidget() { +LinechartWidget::~LinechartWidget() +{ + writeSettings(); stopLogging(); delete listedCurves; listedCurves = NULL; } +void LinechartWidget::writeSettings() +{ + QSettings settings; + settings.beginGroup("LINECHART"); + if (timeButton) settings.setValue("ENFORCE_GROUNDTIME", timeButton->isChecked()); + if (unitsCheckBox) settings.setValue("SHOW_UNITS", unitsCheckBox->isChecked()); + settings.endGroup(); + settings.sync(); +} + +void LinechartWidget::readSettings() +{ + QSettings settings; + settings.sync(); + settings.beginGroup("LINECHART"); + if (activePlot) + { + timeButton->setChecked(settings.value("ENFORCE_GROUNDTIME", timeButton->isChecked()).toBool()); + activePlot->enforceGroundTime(settings.value("ENFORCE_GROUNDTIME", timeButton->isChecked()).toBool()); + } + if (unitsCheckBox) unitsCheckBox->setChecked(settings.value("SHOW_UNITS").toBool()); + settings.endGroup(); +} + void LinechartWidget::createLayout() { // Create actions @@ -204,9 +241,8 @@ void LinechartWidget::createLayout() connect(logButton, SIGNAL(clicked()), this, SLOT(startLogging())); // Ground time button - QToolButton* timeButton = new QToolButton(this); + timeButton = new QCheckBox(this); timeButton->setText(tr("Ground Time")); - timeButton->setCheckable(true); timeButton->setToolTip(tr("Overwrite timestamp of data from vehicle with ground receive time. Helps if the plots are not visible because of missing or invalid onboard time.")); timeButton->setWhatsThis(tr("Overwrite timestamp of data from vehicle with ground receive time. Helps if the plots are not visible because of missing or invalid onboard time.")); bool gTimeDefault = true; @@ -215,21 +251,15 @@ void LinechartWidget::createLayout() layout->addWidget(timeButton, 1, 4); layout->setColumnStretch(4, 0); connect(timeButton, SIGNAL(clicked(bool)), activePlot, SLOT(enforceGroundTime(bool))); + connect(timeButton, SIGNAL(clicked()), this, SLOT(writeSettings())); - // Create the scroll bar - //scrollbar = new QScrollBar(Qt::Horizontal, ui.diagramGroupBox); - //scrollbar->setMinimum(MIN_TIME_SCROLLBAR_VALUE); - //scrollbar->setMaximum(MAX_TIME_SCROLLBAR_VALUE); - //scrollbar->setPageStep(PAGESTEP_TIME_SCROLLBAR_VALUE); - // Set scrollbar to maximum and disable it - //scrollbar->setValue(MIN_TIME_SCROLLBAR_VALUE); - //scrollbar->setDisabled(true); - // scrollbar->setFixedHeight(20); - - - // Add scroll bar to layout and make sure it gets all available space - //layout->addWidget(scrollbar, 1, 5); - layout->setColumnStretch(5, 10); + unitsCheckBox = new QCheckBox(this); + unitsCheckBox->setText(tr("Show units")); + unitsCheckBox->setChecked(true); + unitsCheckBox->setToolTip(tr("Enable unit display in curve list")); + unitsCheckBox->setWhatsThis(tr("Enable unit display in curve list")); + layout->addWidget(unitsCheckBox, 1, 5); + connect(unitsCheckBox, SIGNAL(clicked()), this, SLOT(writeSettings())); ui.diagramGroupBox->setLayout(layout); @@ -238,14 +268,6 @@ void LinechartWidget::createLayout() // Connect notifications from the user interface to the plot connect(this, SIGNAL(curveRemoved(QString)), activePlot, SLOT(hideCurve(QString))); - //connect(this, SIGNAL(curveSet(QString, int)), activePlot, SLOT(showshowCurveCurve(QString, int))); - // FIXME - - // Connect notifications from the plot to the user interface - connect(activePlot, SIGNAL(curveAdded(QString)), this, SLOT(addCurve(QString))); - connect(activePlot, SIGNAL(curveRemoved(QString)), this, SLOT(removeCurve(QString))); - - // Scrollbar // Update scrollbar when plot window changes (via translator method setPlotWindowPosition() connect(activePlot, SIGNAL(windowPositionChanged(quint64)), this, SLOT(setPlotWindowPosition(quint64))); @@ -260,14 +282,18 @@ void LinechartWidget::createLayout() void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 usec) { - // Order matters here, first append to plot, then update curve list - activePlot->appendData(curve, usec, value); - // Store data - QLabel* label = curveLabels->value(curve, NULL); - // Make sure the curve will be created if it does not yet exist - if(!label) + static const QString unit("-"); + if (isVisible()) { - addCurve(curve); + // Order matters here, first append to plot, then update curve list + activePlot->appendData(curve+unit, usec, value); + // Store data + QLabel* label = curveLabels->value(curve+unit, NULL); + // Make sure the curve will be created if it does not yet exist + if(!label) + { + addCurve(curve, unit); + } } // Log data @@ -275,16 +301,75 @@ void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 { if (activePlot->isVisible(curve)) { - quint64 time = 0; - // Adjust time - if (activePlot->groundTime()) - { - time = QGC::groundTimeUsecs() - logStartTime; - } - else - { - time = usec - logStartTime; - } + if (logStartTime == 0) logStartTime = usec; + qint64 time = usec - logStartTime; + if (time < 0) time = 0; + + logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); + logFile->flush(); + } + } +} + + +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec) +{ + if (isVisible()) + { + // Order matters here, first append to plot, then update curve list + activePlot->appendData(curve+unit, usec, value); + // Store data + QLabel* label = curveLabels->value(curve+unit, NULL); + // Make sure the curve will be created if it does not yet exist + if(!label) + { + //qDebug() << "ADDING CURVE IN APPENDDATE DOUBLE"; + addCurve(curve, unit); + } + } + + // Log data + if (logging) + { + if (activePlot->isVisible(curve+unit)) + { + if (logStartTime == 0) logStartTime = usec; + qint64 time = usec - logStartTime; + if (time < 0) time = 0; + + logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); + logFile->flush(); + } + } +} + +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec) +{ + if (isVisible()) + { + // Order matters here, first append to plot, then update curve list + activePlot->appendData(curve+unit, usec, value); + // Store data + QLabel* label = curveLabels->value(curve+unit, NULL); + // Make sure the curve will be created if it does not yet exist + if(!label) + { + intData.insert(curve+unit, 0); + addCurve(curve, unit); + } + + // Add int data + intData.insert(curve+unit, value); + } + + // Log data + if (logging) + { + if (activePlot->isVisible(curve)) + { + if (logStartTime == 0) logStartTime = usec; + qint64 time = usec - logStartTime; + if (time < 0) time = 0; logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); logFile->flush(); @@ -295,11 +380,35 @@ void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 void LinechartWidget::refresh() { QString str; - + // Value QMap::iterator i; for (i = curveLabels->begin(); i != curveLabels->end(); ++i) { - str.sprintf("%+.2f", activePlot->getCurrentValue(i.key())); + if (intData.contains(i.key())) + { + str.sprintf("% 11i", intData.value(i.key())); + } + else + { + double val = activePlot->getCurrentValue(i.key()); + int intval = static_cast(val); + if (intval >= 100000 || intval <= -100000) + { + str.sprintf("% 11i", intval); + } + else if (intval >= 10000 || intval <= -10000) + { + str.sprintf("% 11.2f", val); + } + else if (intval >= 1000 || intval <= -1000) + { + str.sprintf("% 11.4f", val); + } + else + { + str.sprintf("% 11.6f", val); + } + } // Value i.value()->setText(str); } @@ -307,7 +416,24 @@ void LinechartWidget::refresh() QMap::iterator j; for (j = curveMeans->begin(); j != curveMeans->end(); ++j) { - str.sprintf("%+.2f", activePlot->getMean(j.key())); + double val = activePlot->getCurrentValue(j.key()); + int intval = static_cast(val); + if (intval >= 100000 || intval <= -100000) + { + str.sprintf("% 11i", intval); + } + else if (intval >= 10000 || intval <= -10000) + { + str.sprintf("% 11.2f", val); + } + else if (intval >= 1000 || intval <= -1000) + { + str.sprintf("% 11.4f", val); + } + else + { + str.sprintf("% 11.6f", val); + } j.value()->setText(str); } // QMap::iterator k; @@ -321,7 +447,7 @@ void LinechartWidget::refresh() for (l = curveVariances->begin(); l != curveVariances->end(); ++l) { // Variance - str.sprintf("%.2e", activePlot->getVariance(l.key())); + str.sprintf("% 8.3e", activePlot->getVariance(l.key())); l.value()->setText(str); } } @@ -381,7 +507,8 @@ void LinechartWidget::startLogging() if (logFile->open(QIODevice::WriteOnly | QIODevice::Text)) { logging = true; - logStartTime = QGC::groundTimeUsecs(); + logStartTime = 0; + curvesWidget->setEnabled(false); logindex++; logButton->setText(tr("Stop logging")); disconnect(logButton, SIGNAL(clicked()), this, SLOT(startLogging())); @@ -393,6 +520,7 @@ void LinechartWidget::startLogging() void LinechartWidget::stopLogging() { logging = false; + curvesWidget->setEnabled(true); if (logFile->isOpen()) { logFile->flush(); @@ -432,47 +560,39 @@ void LinechartWidget::createActions() * @param curve The id-string of the curve * @see removeCurve() **/ -void LinechartWidget::addCurve(QString curve) -{ - curvesWidgetLayout->addWidget(createCurveItem(curve)); -} - -QWidget* LinechartWidget::createCurveItem(QString curve) +void LinechartWidget::addCurve(const QString& curve, const QString& unit) { LinechartPlot* plot = activePlot; - QWidget* form = new QWidget(this); - QHBoxLayout *horizontalLayout; +// QHBoxLayout *horizontalLayout; QCheckBox *checkBox; QLabel* label; QLabel* value; + QLabel* unitLabel; QLabel* mean; QLabel* variance; - form->setAutoFillBackground(false); - horizontalLayout = new QHBoxLayout(form); - horizontalLayout->setSpacing(5); - horizontalLayout->setMargin(0); - horizontalLayout->setSizeConstraint(QLayout::SetMinimumSize); - checkBox = new QCheckBox(form); + int labelRow = curvesWidgetLayout->rowCount(); + + checkBox = new QCheckBox(this); checkBox->setCheckable(true); - checkBox->setObjectName(curve); + checkBox->setObjectName(curve+unit); checkBox->setToolTip(tr("Enable the curve in the graph window")); checkBox->setWhatsThis(tr("Enable the curve in the graph window")); - horizontalLayout->addWidget(checkBox); + curvesWidgetLayout->addWidget(checkBox, labelRow, 0); - QWidget* colorIcon = new QWidget(form); + QWidget* colorIcon = new QWidget(this); colorIcon->setMinimumSize(QSize(5, 14)); colorIcon->setMaximumSize(4, 14); - horizontalLayout->addWidget(colorIcon); + curvesWidgetLayout->addWidget(colorIcon, labelRow, 1); - label = new QLabel(form); - horizontalLayout->addWidget(label); + label = new QLabel(this); + curvesWidgetLayout->addWidget(label, labelRow, 2); //checkBox->setText(QString()); label->setText(curve); - QColor color = plot->getColorForCurve(curve); + QColor color = plot->getColorForCurve(curve+unit); if(color.isValid()) { QString colorstyle; colorstyle = colorstyle.sprintf("QWidget { background-color: #%X%X%X; }", color.red(), color.green(), color.blue()); @@ -481,20 +601,33 @@ QWidget* LinechartWidget::createCurveItem(QString curve) } // Value - value = new QLabel(form); + value = new QLabel(this); value->setNum(0.00); - value->setToolTip(tr("Current value of ") + curve); - value->setWhatsThis(tr("Current value of ") + curve); - curveLabels->insert(curve, value); - horizontalLayout->addWidget(value); + value->setStyleSheet(QString("QLabel {font-family:\"Courier\"; font-weight: bold;}")); + value->setToolTip(tr("Current value of %1 in %2 units").arg(curve, unit)); + value->setWhatsThis(tr("Current value of %1 in %2 units").arg(curve, unit)); + curveLabels->insert(curve+unit, value); + curvesWidgetLayout->addWidget(value, labelRow, 3); + + // Unit + unitLabel = new QLabel(this); + unitLabel->setText(unit); + unitLabel->setStyleSheet(QString("QLabel {color: %1;}").arg("#AAAAAA")); + //qDebug() << "UNIT" << unit; + unitLabel->setToolTip(tr("Unit of ") + curve); + unitLabel->setWhatsThis(tr("Unit of ") + curve); + curvesWidgetLayout->addWidget(unitLabel, labelRow, 4); + unitLabel->setVisible(unitsCheckBox->isChecked()); + connect(unitsCheckBox, SIGNAL(clicked(bool)), unitLabel, SLOT(setVisible(bool))); // Mean - mean = new QLabel(form); + mean = new QLabel(this); mean->setNum(0.00); - mean->setToolTip(tr("Arithmetic mean of ") + curve); - mean->setWhatsThis(tr("Arithmetic mean of ") + curve); - curveMeans->insert(curve, mean); - horizontalLayout->addWidget(mean); + mean->setStyleSheet(QString("QLabel {font-family:\"Courier\"; font-weight: bold;}")); + mean->setToolTip(tr("Arithmetic mean of %1 in %2 units").arg(curve, unit)); + mean->setWhatsThis(tr("Arithmetic mean of %1 in %2 units").arg(curve, unit)); + curveMeans->insert(curve+unit, mean); + curvesWidgetLayout->addWidget(mean, labelRow, 5); // // Median // median = new QLabel(form); @@ -503,12 +636,13 @@ QWidget* LinechartWidget::createCurveItem(QString curve) // horizontalLayout->addWidget(median); // Variance - variance = new QLabel(form); + variance = new QLabel(this); variance->setNum(0.00); - variance->setToolTip(tr("Variance of ") + curve); - variance->setWhatsThis(tr("Variance of ") + curve); - curveVariances->insert(curve, variance); - horizontalLayout->addWidget(variance); + variance->setStyleSheet(QString("QLabel {font-family:\"Courier\"; font-weight: bold;}")); + variance->setToolTip(tr("Variance of %1 in (%2)^2 units").arg(curve, unit)); + variance->setWhatsThis(tr("Variance of %1 in (%2)^2 units").arg(curve, unit)); + curveVariances->insert(curve+unit, variance); + curvesWidgetLayout->addWidget(variance, labelRow, 6); /* Color picker QColor color = QColorDialog::getColor(Qt::green, this); @@ -520,13 +654,10 @@ QWidget* LinechartWidget::createCurveItem(QString curve) */ // Set stretch factors so that the label gets the whole space - horizontalLayout->setStretchFactor(checkBox, 0); - horizontalLayout->setStretchFactor(colorIcon, 0); - horizontalLayout->setStretchFactor(label, 80); - horizontalLayout->setStretchFactor(value, 50); - horizontalLayout->setStretchFactor(mean, 50); -// horizontalLayout->setStretchFactor(median, 50); - horizontalLayout->setStretchFactor(variance, 50); + + + // Load visibility settings + // TODO // Connect actions QObject::connect(checkBox, SIGNAL(clicked(bool)), this, SLOT(takeButtonClick(bool))); @@ -534,9 +665,7 @@ QWidget* LinechartWidget::createCurveItem(QString curve) // Set UI components to initial state checkBox->setChecked(false); - plot->setVisible(curve, false); - - return form; + plot->setVisible(curve+unit, false); } /** @@ -572,7 +701,7 @@ void LinechartWidget::setActive(bool active) } if (active) { - updateTimer->start(); + updateTimer->start(updateInterval); } else { diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index dbffd698ffc4c460fb85464170d3a223e93a7519..d654a12c0952af285463915f8d975c3580ba2ac4 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -70,9 +70,14 @@ public: static const int MAX_TIME_SCROLLBAR_VALUE = 16383; ///< The maximum scrollbar value public slots: - void addCurve(QString curve); + void addCurve(const QString& curve, const QString& unit); void removeCurve(QString curve); + /** @brief Append data without unit */ void appendData(int uasId, QString curve, double data, quint64 usec); + /** @brief Append data with unit */ + void appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec); + /** @brief Append data as int with unit */ + void appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec); void takeButtonClick(bool checked); void setPlotWindowPosition(int scrollBarValue); void setPlotWindowPosition(quint64 position); @@ -90,12 +95,16 @@ public slots: void stopLogging(); /** @brief Refresh the view */ void refresh(); + /** @brief Write the current configuration to disk */ + void writeSettings(); + /** @brief Read the current configuration from disk */ + void readSettings(); protected: void addCurveToList(QString curve); void removeCurveFromList(QString curve); QToolButton* createButton(QWidget* parent); - QWidget* createCurveItem(QString curve); + void createCurveItem(QString curve); void createLayout(); int sysid; ///< ID of the unmanned system this plot belongs to @@ -110,9 +119,10 @@ protected: QMap* curveMeans; ///< References to the curve means QMap* curveMedians; ///< References to the curve medians QMap* curveVariances; ///< References to the curve variances + QMap intData; ///< Current values for integer-valued curves QWidget* curvesWidget; ///< The QWidget containing the curve selection button - QVBoxLayout* curvesWidgetLayout; ///< The layout for the curvesWidget QWidget + QGridLayout* curvesWidgetLayout; ///< The layout for the curvesWidget QWidget QScrollBar* scrollbar; ///< The plot window scroll bar QSpinBox* averageSpinBox; ///< Spin box to setup average window filter size @@ -126,13 +136,16 @@ protected: QToolButton* scalingLinearButton; QToolButton* scalingLogButton; QToolButton* logButton; + QPointer unitsCheckBox; + QPointer timeButton; QFile* logFile; unsigned int logindex; bool logging; + quint64 logStartTime; QTimer* updateTimer; LogCompressor* compressor; - quint64 logStartTime; + static const int updateInterval = 400; ///< Time between number updates, in milliseconds static const int MAX_CURVE_MENUITEM_NUMBER = 8; static const int PAGESTEP_TIME_SCROLLBAR_VALUE = (MAX_TIME_SCROLLBAR_VALUE - MIN_TIME_SCROLLBAR_VALUE) / 10; diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index aba663f58159953e6962d899abe421c7defad9bc..6fc366537a0e6501937a9393ed2bb498435fcca2 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -96,7 +96,12 @@ void Linecharts::addSystem(UASInterface* uas) addWidget(widget); plots.insert(uas->getUASID(), widget); #ifndef MAVLINK_ENABLED_SLUGS - connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), widget, SLOT(appendData(int,QString,double,quint64))); + // Values without unit + //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), widget, SLOT(appendData(int,QString,double,quint64))); + // Values with unit as double + connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); + // Values with unit as integer + connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); #endif connect(widget, SIGNAL(logfileWritten(QString)), this, SIGNAL(logfileWritten(QString))); // Set system active if this is the only system diff --git a/src/ui/map3D/QGCGoogleEarthView.ui b/src/ui/map3D/QGCGoogleEarthView.ui index 53269ab8f5c67477b0cd32ac7a50666e7e130fd2..62ed1354b22f3e691b0e054ead26dbf001918605 100644 --- a/src/ui/map3D/QGCGoogleEarthView.ui +++ b/src/ui/map3D/QGCGoogleEarthView.ui @@ -15,49 +15,50 @@ - 10 + 8 - 5 + 2 2 - + - - - - - Home + + Go to home location + + + Go to home location + + + Go to home location - - - - - Follow + Home - - Trail + + Show MAV trajectories + + + Show MAV trajectories + + + Show MAV trajectories - - - - - Clear WPs + Trails - + Qt::Horizontal @@ -70,8 +71,17 @@ - + + + Distance of the chase camera to the MAV + + + Distance of the chase camera to the MAV + + + Distance of the chase camera to the MAV + 30 @@ -86,30 +96,87 @@ - + + + Distance of the chase camera to the MAV + + + Distance of the chase camera to the MAV + + + Distance of the chase camera to the MAV + + + Dist + + + + + + + Show waypoints + + + Show waypoints + + + Show waypoints + + + WPs + + + + + + + Chase the MAV + + + Chase the MAV + + + Chase the MAV + - Cam Distance + Follow - - - Qt::Horizontal + + + Select the MAV to chase - - - 40 - 20 - + + Select the MAV to chase - + + Select the MAV to chase + + - - + + + + Delete all waypoints + + + Delete all waypoints + + + Delete all waypoints + - Waypoints + Clear WPs + + + + + + + Qt::Vertical diff --git a/src/ui/uas/QGCUnconnectedInfoWidget.ui b/src/ui/uas/QGCUnconnectedInfoWidget.ui index 4eaac71aab3a8d00bbcdcb9fee01101470b6835e..f7e30998cb458bd9241713b99a0c1964c2cbfb0e 100644 --- a/src/ui/uas/QGCUnconnectedInfoWidget.ui +++ b/src/ui/uas/QGCUnconnectedInfoWidget.ui @@ -39,16 +39,15 @@ p, li { white-space: pre-wrap; } <table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> <tr> <td style="border: none;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600;">Unmanned System List</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:12pt; font-weight:600;">Unmanned System List</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:6pt; font-weight:600;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">No Systems are connected yet.</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> Please either connect a link or use the simulation function to see QGroundControl in action.</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-style:italic;">Click on the simulation button below to simulate a micro air vehicle or on the connect link button to connect a serial port link. A UDP link is already open for connections on port </span><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600; font-style:italic;">14550</span><span style=" font-family:'Ubuntu'; font-size:11pt; font-style:italic;">.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:10pt; font-weight:600;">No Systems are connected yet.</span><span style=" font-family:'Ubuntu'; font-size:10pt;"> Please either connect a link or use the simulation function to see QGroundControl in action.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:10pt;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:10pt; font-style:italic;">Click on the simulation button below to simulate a micro air vehicle or on the connect link button to connect a serial port link. A UDP link is already open for connections on port </span><span style=" font-family:'Ubuntu'; font-size:10pt; font-weight:600; font-style:italic;">14550</span><span style=" font-family:'Ubuntu'; font-size:10pt; font-style:italic;">.</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt; font-style:italic;"></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt; font-style:italic;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Communication Link Help:</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt; font-style:italic;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">If you encounter communication problems on your link (e.g. no MAV is shown in the list after connecting the link), please check if you receive data on the link using the communication console. Select </span><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Tools -&gt; Communication Console</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> to enable it. The console should show incoming traffic and some used bandwidth (e.g. 1.43 kB/s on the indicator).</span></p></td></tr></table></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:10pt; font-weight:600;">Communication Link Help:</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:6pt; font-style:italic;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:10pt;">If you encounter communication problems on your link (e.g. no MAV is shown in the list after connecting the link), please check if you receive data on the link using the communication console. Select </span><span style=" font-family:'Ubuntu'; font-size:10pt; font-weight:600;">Tools -&gt; Communication Console</span><span style=" font-family:'Ubuntu'; font-size:10pt;"> to enable it. The console should show incoming traffic and some used bandwidth (e.g. 1.43 kB/s on the indicator).</span></p></td></tr></table></body></html> diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index 842c15046dddcab5fed21d94239b855d8cbdc404..9de5dd170b841cfb3cea9346ff9ffd1025e564c0 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -84,6 +84,8 @@ engineOn(false) connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode())); ui.modeComboBox->setCurrentIndex(0); + + ui.gridLayout->setAlignment(Qt::AlignTop); } void UASControlWidget::setUAS(UASInterface* uas) diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc index 498c3ff0f60e81bd7dcbe137747440108680ea10..e8a4f4e28d9953ff31c2ce1eb0ec3cfa9bf36588 100644 --- a/src/ui/uas/UASListWidget.cc +++ b/src/ui/uas/UASListWidget.cc @@ -52,6 +52,7 @@ UASListWidget::UASListWidget(QWidget *parent) : QWidget(parent), m_ui(new Ui::UA listLayout = new QVBoxLayout(this); listLayout->setAlignment(Qt::AlignTop); this->setLayout(listLayout); + setObjectName("UNMANNED_SYSTEMS_LIST"); // Construct initial widget uWidget = new QGCUnconnectedInfoWidget(this);