Fixed a number of small bugs and issues, WP handling makes more sense now

src/uas/UASWaypointManager.cc

@@ -46,7 +46,8 @@ UASWaypointManager::UASWaypointManager(UAS &_uas)
       current_state(WP_IDLE),
       current_partner_systemid(0),
       current_partner_compid(0),
-      protocol_timer(this)
+      protocol_timer(this),
+      currentWaypoint(NULL)
 {
     connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout()));
     connect(&uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleLocalPositionChanged(UASInterface*,double,double,double,quint64)));
@@ -92,11 +93,11 @@ void UASWaypointManager::handleLocalPositionChanged(UASInterface* mav, double x,
 {
     Q_UNUSED(mav);
     Q_UNUSED(time);
-    if (waypoints.count() > 0 && (waypoints[current_wp_id]->getFrame() == MAV_FRAME_LOCAL || waypoints[current_wp_id]->getFrame() == MAV_FRAME_LOCAL_ENU))
+    if (waypoints.count() > 0 && currentWaypoint && (currentWaypoint->getFrame() == MAV_FRAME_LOCAL || currentWaypoint->getFrame() == MAV_FRAME_LOCAL_ENU))
     {
-        double xdiff = x-waypoints[current_wp_id]->getX();
-        double ydiff = y-waypoints[current_wp_id]->getY();
-        double zdiff = z-waypoints[current_wp_id]->getZ();
+        double xdiff = x-currentWaypoint->getX();
+        double ydiff = y-currentWaypoint->getY();
+        double zdiff = z-currentWaypoint->getZ();
         double dist = sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff);
         emit waypointDistanceChanged(dist);
     }
@@ -146,6 +147,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
             //// qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << "Frame:"<< (MAV_FRAME) wp->frame << "Command:" << (MAV_CMD) wp->command;
             Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command);
             addWaypoint(lwp, false);
+            if (wp->current == 1) currentWaypoint = lwp;
 
             //get next waypoint
             current_wp_id++;
@@ -164,6 +166,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
 
                 protocol_timer.stop();
                 emit readGlobalWPFromUAS(false);
+                if (currentWaypoint) emit currentWaypointChanged(currentWaypoint->getId());
                 emit updateStatusString("done.");
 
                 // qDebug() << "got all waypoints from ID " << systemId;
@@ -232,6 +235,8 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
                 for(int i = 0; i < waypoints.size(); i++) {
                     if (waypoints[i]->getId() == wpc->seq) {
                         waypoints[i]->setCurrent(true);
+                        currentWaypoint = waypoints[i];
+
                     } else {
                         waypoints[i]->setCurrent(false);
                     }
@@ -267,6 +272,7 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq)
             for(int i = 0; i < waypoints.size(); i++) {
                 if (waypoints[i]->getId() == seq) {
                     waypoints[i]->setCurrent(true);
+                    currentWaypoint = waypoints[i];
                 } else {
                     waypoints[i]->setCurrent(false);
                 }
@@ -302,7 +308,11 @@ void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive)
 {
     if (wp) {
         wp->setId(waypoints.size());
-        if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true);
+        if (enforceFirstActive && waypoints.size() == 0)
+        {
+            wp->setCurrent(true);
+            currentWaypoint = wp;
+        }
         waypoints.insert(waypoints.size(), wp);
         connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*)));
 
@@ -318,7 +328,11 @@ Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive)
 {
     Waypoint* wp = new Waypoint();
     wp->setId(waypoints.size());
-    if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true);
+    if (enforceFirstActive && waypoints.size() == 0)
+    {
+        wp->setCurrent(true);
+        currentWaypoint = wp;
+    }
     waypoints.insert(waypoints.size(), wp);
     connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*)));
 

src/uas/UASWaypointManager.h

@@ -163,6 +163,7 @@ private:
     quint8 current_partner_compid;                  ///< The current protocol communication target component
 
     QVector<Waypoint *> waypoints;                  ///< local waypoint list (main storage)
+    Waypoint* currentWaypoint;                      ///< The currently used waypoint
     QVector<mavlink_waypoint_t *> waypoint_buffer;  ///< buffer for waypoints during communication
     QTimer protocol_timer;                          ///< Timer to catch timeouts
 };

src/ui/HSIDisplay.cc

@@ -42,7 +42,7 @@ This file is part of the QGROUNDCONTROL project
 #include "UASWaypointManager.h"
 #include <qmath.h>
 //#include "Waypoint2DIcon.h"
-//#include "MAV2DIcon.h"
+#include "MAV2DIcon.h"
 
 #include <QDebug>
 
@@ -115,24 +115,6 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
 
     xCenterPos = vwidth/2.0f;
     yCenterPos = vheight/2.0f + topMargin - bottomMargin;
-    //qDebug() << "CENTER" << xCenterPos << yCenterPos;
-
-    // Add interaction elements
-    QHBoxLayout* layout = new QHBoxLayout(this);
-    layout->setMargin(0);
-    layout->setSpacing(12);
-    QDoubleSpinBox* spinBox = new QDoubleSpinBox(this);
-    spinBox->setMinimum(0.1);
-    spinBox->setMaximum(9999);
-    spinBox->setMaximumWidth(50);
-    spinBox->setValue(metricWidth);
-    spinBox->setToolTip(tr("Ground width in meters shown on instrument"));
-    spinBox->setStatusTip(tr("Ground width in meters shown on instrument"));
-    connect(spinBox, SIGNAL(valueChanged(double)), this, SLOT(setMetricWidth(double)));
-    connect(this, SIGNAL(metricWidthChanged(double)), spinBox, SLOT(setValue(double)));
-    layout->addWidget(spinBox);
-    layout->setAlignment(spinBox, Qt::AlignBottom | Qt::AlignRight);
-    this->setLayout(layout);
 
     uas = NULL;
     resetMAVState();
@@ -246,6 +228,7 @@ void HSIDisplay::renderOverlay()
         // Translate to center
         painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
         QColor uasColor = uas->getColor();
+        MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
         //MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
         // Translate back
         painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
@@ -309,8 +292,8 @@ void HSIDisplay::renderOverlay()
 
     // Draw crosstrack error to top right
     float crossTrackError = 0;
-    paintText(tr("XTRACK"), QGC::colorCyan, 2.2f, 57, 11, &painter);
-    paintText(tr("%1 m").arg(crossTrackError, 5, 'f', 2, '0'), Qt::white, 2.2f, 70, 11, &painter);
+    paintText(tr("XTRACK"), QGC::colorCyan, 2.2f, 54, 11, &painter);
+    paintText(tr("%1 m").arg(crossTrackError, 5, 'f', 2, '0'), Qt::white, 2.2f, 67, 11, &painter);
 
     // Draw position to bottom left
     if (localAvailable > 0 && globalAvailable == 0) {
@@ -330,7 +313,7 @@ void HSIDisplay::renderOverlay()
     }
 
     // Draw Field of view to bottom right
-    paintText(tr("FOV"), QGC::colorCyan, 2.6f, 62, vheight- 5.0f, &painter);
+    paintText(tr("FOV %1 m").arg(metricWidth, 5, 'f', 1, ' '), QGC::colorCyan, 2.6f, 55, vheight- 5.0f, &painter);
 }
 
 void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter)

src/ui/MainWindow.cc

@@ -134,7 +134,7 @@ MainWindow::MainWindow(QWidget *parent):
     toolBar->addPerspectiveChangeAction(ui.actionOperatorsView);
     toolBar->addPerspectiveChangeAction(ui.actionEngineersView);
     toolBar->addPerspectiveChangeAction(ui.actionPilotsView);
-    toolBar->addPerspectiveChangeAction(ui.actionUnconnectedView);
+//    toolBar->addPerspectiveChangeAction(ui.actionUnconnectedView);
 
     buildCommonWidgets();
     connectCommonWidgets();
@@ -296,7 +296,6 @@ void MainWindow::setDefaultSettingsForAp()
 
 void MainWindow::resizeEvent(QResizeEvent * event)
 {
-    Q_UNUSED(event);
     if (height() < 800)
     {
         ui.statusBar->setVisible(false);
@@ -306,6 +305,17 @@ void MainWindow::resizeEvent(QResizeEvent * event)
         ui.statusBar->setVisible(true);
         ui.statusBar->setSizeGripEnabled(true);
     }
+
+    if (width() > 1200)
+    {
+        toolBar->setToolButtonStyle(Qt::ToolButtonTextBesideIcon);
+    }
+    else
+    {
+        toolBar->setToolButtonStyle(Qt::ToolButtonIconOnly);
+    }
+
+    QMainWindow::resizeEvent(event);
 }
 
 QString MainWindow::getWindowStateKey()
@@ -730,8 +740,8 @@ void MainWindow::loadCustomWidgetsFromDefaults(const QString& systemType, const
         {
             // Will only be loaded if not already a custom widget with
             // the same name is present
-            qDebug() << "Tried to load file: " << file;
-            loadCustomWidget(defaultsDir+"/"+file, true);
+            qDebug() << "Tried to load file: " << defaultsDir+file;
+            loadCustomWidget(defaultsDir+file, true);
         }
     }
 }

src/ui/QGCToolBar.cc

@@ -41,10 +41,9 @@ QGCToolBar::QGCToolBar(QWidget *parent) :
     logReplayAction = new QAction(QIcon(":"), "Replay", this);
     logReplayAction->setCheckable(true);
 
-    addSeparator();
-
     addAction(toggleLoggingAction);
     addAction(logReplayAction);
+//    addSeparator();
 
     // CREATE TOOLBAR ITEMS
     // Add internal actions
@@ -64,11 +63,10 @@ QGCToolBar::QGCToolBar(QWidget *parent) :
     toolBarBatteryBar->setStyleSheet("QProgressBar:horizontal { margin: 0px 4px 0px 0px; border: 1px solid #4A4A4F; border-radius: 4px; text-align: center; padding: 2px; color: #111111; background-color: #111118; height: 10px; } QProgressBar:horizontal QLabel { font-size: 9px; color: #111111; } QProgressBar::chunk { background-color: green; }");
     toolBarBatteryBar->setMinimum(0);
     toolBarBatteryBar->setMaximum(100);
-    toolBarBatteryBar->setMinimumWidth(200);
+    toolBarBatteryBar->setMinimumWidth(20);
     toolBarBatteryBar->setMaximumWidth(200);
     toolBarBatteryVoltageLabel = new QLabel("xx.x V");
     toolBarBatteryVoltageLabel->setStyleSheet(QString("QLabel { margin: 0px 0px 0px 4px; font: 14px; color: %1; }").arg(QColor(Qt::green).name()));
-    //symbolButton->setIcon(":");
     symbolButton->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px 0px 0px 20px; background-color: none; }");
     addWidget(symbolButton);
     addWidget(toolBarNameLabel);

src/ui/linechart/LinechartPlot.cc

@@ -126,10 +126,7 @@ LinechartPlot::LinechartPlot(QWidget *parent, int plotid, quint64 interval): Qwt
     connect(updateTimer, SIGNAL(timeout()), this, SLOT(paintRealtime()));
     //updateTimer->start(DEFAULT_REFRESH_RATE);
 
-    //    QwtPlot::setAutoReplot();
-
-    //    canvas()->setPaintAttribute(QwtPlotCanvas::PaintCached, false);
-    //    canvas()->setPaintAttribute(QwtPlotCanvas::PaintPacked, false);
+    connect(&timeoutTimer, SIGNAL(timeout()), this, SLOT(removeTimedOutCurves()));
 }
 
 LinechartPlot::~LinechartPlot()
@@ -206,6 +203,34 @@ void LinechartPlot::setActive(bool active)
     m_active = active;
 }
 
+void LinechartPlot::removeTimedOutCurves()
+{
+    foreach(QString key, lastUpdate.keys())
+    {
+        quint64 time = lastUpdate.value(key);
+        if (QGC::groundTimeMilliseconds() - time > 20000)
+        {
+            // Remove this curve
+            // Delete curves
+            QwtPlotCurve* curve = curves.take(key);
+            // Delete the object
+            delete curve;
+            // Set the pointer null
+            curve = NULL;
+
+            // Notify connected components about the removal
+            emit curveRemoved(key);
+
+            // Remove from data list
+            TimeSeriesData* d = data.take(key);
+            // Delete the object
+            delete d;
+            // Set the pointer null
+            d = NULL;
+        }
+    }
+}
+
 /**
  * @brief Set the zero (center line) value
  * The zero value defines the centerline of the plot.
@@ -235,18 +260,18 @@ void LinechartPlot::appendData(QString dataname, quint64 ms, double value)
     // Add new value
     TimeSeriesData* dataset = data.value(dataname);
 
-    quint64 time;
+    quint64 time = QGC::groundTimeMilliseconds();
 
     // Append data
-    if (m_groundTime) {
-        // Use the current (receive) time
-        time = QGC::groundTimeUsecs()/1000;
-    } else {
+    if (!m_groundTime)
+    {
         // Use timestamp from dataset
         time = ms;
     }
     dataset->append(time, value);
 
+    lastUpdate.insert(dataname, time);
+
     // Scaling values
     if(ms < minTime) minTime = ms;
     if(ms > maxTime) maxTime = ms;

src/ui/linechart/LinechartPlot.h

@@ -39,6 +39,7 @@ This file is part of the PIXHAWK project
 #include <QList>
 #include <QMutex>
 #include <QTime>
+#include <QTimer>
 #include <qwt_plot_panner.h>
 #include <qwt_plot_curve.h>
 #include <qwt_scale_draw.h>
@@ -47,7 +48,7 @@ This file is part of the PIXHAWK project
 #include <qwt_array.h>
 #include <qwt_plot.h>
 #include <ScrollZoomer.h>
-#include <MG.h>
+#include "MG.h"
 
 class TimeScaleDraw: public QwtScaleDraw
 {
@@ -266,13 +267,16 @@ public slots:
 
     /** @brief Set the number of values to average over */
     void setAverageWindow(int windowSize);
+    void removeTimedOutCurves();
 
+    public:
     QColor getColorForCurve(QString id);
 
 protected:
     QMap<QString, QwtPlotCurve*> curves;
     QMap<QString, TimeSeriesData*> data;
     QMap<QString, QwtScaleMap*> scaleMaps;
+    QMap<QString, quint64> lastUpdate;
     ScrollZoomer* zoomer;
 
     QList<QColor> colors;
@@ -306,6 +310,7 @@ protected:
     int plotid;
     bool m_active; ///< Decides wether the plot is active or not
     bool m_groundTime; ///< Enforce the use of the receive timestamp instead of the data timestamp
+    QTimer timeoutTimer;
 
     // Methods
     void addCurve(QString id);