Merge branch 'experimental' of github.com:pixhawk/qgroundcontrol into experimental

Conflicts:
	src/ui/MainWindow.cc

images/earth.html

@@ -192,40 +192,38 @@ function setViewRange(dist)
 function addTrailPosition(id, lat, lon, alt)
 {
 	trails[id].setExtrude(false);
-trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
+    trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
 
-// Add LineString points
+    // Add LineString points
 	trails[id].getCoordinates().pushLatLngAlt(lat, lon, alt);
 
-// Create a style and set width and color of line
-trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
-lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
-lineStyle.setWidth(5);
-lineStyle.getColor().set(trailColors[id]);  // aabbggrr format
-//lineStyle.getColor().set(color);  // aabbggrr format
+    // Create a style and set width and color of line
+    trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
+    lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
+    lineStyle.setWidth(5);
+    lineStyle.getColor().set(trailColors[id]);  // aabbggrr format
+    //lineStyle.getColor().set(color);  // aabbggrr format
 
-// Add the feature to Earth
-if (trailsVisible[id] == true) ge.getFeatures().replaceChild(trailPlacemarks[id], trailPlacemarks[id]);
+    // Add the feature to Earth
+    if (trailsVisible[id] == true) ge.getFeatures().replaceChild(trailPlacemarks[id], trailPlacemarks[id]);
 }
 
 function initCallback(object)
 {
-  ge = object;
-  ge.getWindow().setVisibility(true);
-          ge.getOptions().setStatusBarVisibility(true); 
-        ge.getOptions().setScaleLegendVisibility(true);
-		ge.getOptions().setFlyToSpeed(5.0); 
-        //ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); 
-        ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO);
+    ge = object;
+    ge.getWindow().setVisibility(true);
+    ge.getOptions().setStatusBarVisibility(true); 
+    ge.getOptions().setScaleLegendVisibility(true);
+    ge.getOptions().setFlyToSpeed(5.0); 
+    //ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); 
+    ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO);
         
-        ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true); 
-        ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true); 
-        ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true); 
-        ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
+    ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true); 
+    ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true); 
+    ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true); 
+    ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
 
-        initialized = true;
-        
-        
+    initialized = true;
 }
 
 function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
@@ -247,13 +245,12 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 	planeOrient.setRoll(((roll/M_PI)+1.0)*360.0);
 	planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0);
 	planeOrient.setHeading(((yaw/M_PI)+1.0)*360.0);
-    }
-        
-                
+    
     planeLoc.setLatitude(lat);
     planeLoc.setLongitude(lon);
     planeLoc.setAltitude(alt);
-	
+    planeModel.setLocation(planeLoc);
+    }
 }
 
 function enableDaylight(enabled)

src/configuration.h

@@ -17,14 +17,14 @@
 #define WITH_TEXT_TO_SPEECH 1
 
 #define QGC_APPLICATION_NAME "QGroundControl"
-#define QGC_APPLICATION_VERSION "v. 0.8.1 (Alpha)"
+#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha)"
 
 namespace QGC
 
 {
     const QString APPNAME = "QGROUNDCONTROL";
     const QString COMPANYNAME = "OPENMAV";
-    const int APPLICATIONVERSION = 80; // 0.8.0
+    const int APPLICATIONVERSION = 83; // 0.8.0
 }
 
 #endif // CONFIGURATION_H

src/uas/PxQuadMAV.cc

@@ -64,6 +64,15 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "Temperature", "raw", raw.temp, time);
             }
             break;
+        case MAVLINK_MSG_ID_IMAGE_TRIGGERED:
+            {
+                // FIXME Kind of a hack to load data from disk
+                mavlink_image_triggered_t img;
+                mavlink_msg_image_triggered_decode(&message, &img);
+                qDebug() << "IMAGE AVAILABLE:" << img.timestamp;
+                emit imageStarted(img.timestamp);
+            }
+            break;
         case MAVLINK_MSG_ID_PATTERN_DETECTED:
             {
                 mavlink_pattern_detected_t detected;

src/uas/UAS.cc

@@ -464,6 +464,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "latitude", "deg", pos.lat, time);
                 emit valueChanged(uasId, "longitude", "deg", pos.lon, time);
 
+                // FIXME REMOVE
+                longitude = pos.lon;
+                latitude = pos.lat;
+                altitude = pos.alt;
+                emit globalPositionChanged(this, longitude, latitude, altitude, time);
+
                 if (pos.fix_type > 0)
                 {
                     emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);

src/uas/UAS.h

@@ -169,7 +169,7 @@ public:
     bool isAuto();
 
 public:
-    UASWaypointManager &getWaypointManager(void) { return waypointManager; }
+    UASWaypointManager* getWaypointManager() { return &waypointManager; }
     int getSystemType();
     int getAutopilotType() {return autopilot;}
 
@@ -303,6 +303,7 @@ signals:
     void loadChanged(UASInterface* uas, double load);
     /** @brief Propagate a heartbeat received from the system */
     void heartbeat(UASInterface* uas);
+    void imageStarted(quint64 timestamp);
 
     protected:
     /** @brief Get the UNIX timestamp in microseconds */

src/uas/UASInterface.h

@@ -79,7 +79,7 @@ public:
     virtual double getYaw() const = 0;
 
     /** @brief Get reference to the waypoint manager **/
-    virtual UASWaypointManager &getWaypointManager(void) = 0;
+    virtual UASWaypointManager* getWaypointManager(void) = 0;
 
     /* COMMUNICATION FLAGS */
 

src/uas/UASWaypointManager.cc

@@ -265,6 +265,13 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
     }
 }
 
+void UASWaypointManager::notifyOfChange(Waypoint* wp)
+{
+    Q_UNUSED(wp);
+    emit waypointListChanged();
+    emit waypointListChanged(uas.getUASID());
+}
+
 int UASWaypointManager::setCurrentWaypoint(quint16 seq)
 {
     if (seq < waypoints.size())
@@ -313,6 +320,9 @@ void UASWaypointManager::addWaypoint(Waypoint *wp)
         waypoints.insert(waypoints.size(), wp);
 
         emit waypointListChanged();
+        emit waypointListChanged(uas.getUASID());
+
+        qDebug() << "ADDED WAYPOINT WITH ID:" << wp->getId();
     }
 }
 
@@ -329,6 +339,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
             waypoints[i]->setId(i);
         }
         emit waypointListChanged();
+        emit waypointListChanged(uas.getUASID());
         return 0;
     }
     return -1;
@@ -359,6 +370,7 @@ void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
         //waypoints[new_seq]->setId(new_seq);
 
         emit waypointListChanged();
+        emit waypointListChanged(uas.getUASID());
     }
 }
 
@@ -425,21 +437,28 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
 
     emit loadWPFile();
     emit waypointListChanged();
+    emit waypointListChanged(uas.getUASID());
 }
 
 
-void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
-{
-    // FIXME Will be removed
-    Q_UNUSED(wp);
-}
+//void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
+//{
+//    // FIXME Will be removed
+//    Q_UNUSED(wp);
+//}
 
-int UASWaypointManager::globalRemoveWaypoint(quint16 seq)
-{
-    // FIXME Will be removed
-    Q_UNUSED(seq);
-    return 0;
-}
+//int UASWaypointManager::globalRemoveWaypoint(quint16 seq)
+//{
+//    // FIXME Will be removed
+//    Q_UNUSED(seq);
+//    return 0;
+//}
+
+//void UASWaypointManager::waypointUpdated(Waypoint*)
+//{
+//    // FIXME Add rate limiter
+//    emit waypointListChanged(uas.getUASID());
+//}
 
 void UASWaypointManager::clearWaypointList()
 {
@@ -457,6 +476,11 @@ void UASWaypointManager::clearWaypointList()
     }
 }
 
+int UASWaypointManager::getIndexOf(Waypoint* wp)
+{
+    return waypoints.indexOf(wp);
+}
+
 void UASWaypointManager::readWaypoints()
 {
     emit readGlobalWPFromUAS(true);

src/uas/UASWaypointManager.h

@@ -86,25 +86,27 @@ public:
     /** @name Waypoint list operations */
     /*@{*/
     const QVector<Waypoint *> &getWaypointList(void) { return waypoints; }  ///< Returns a const reference to the waypoint list.
-    void addWaypoint(Waypoint *wp);                        ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
     int removeWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
     void moveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
     void saveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile
     void loadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
+    void notifyOfChange(Waypoint* wp);                     ///< Notifies manager to changes to a waypoint
     void localAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
     int localRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
     void localMoveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
     void localSaveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile
     void localLoadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
-
+    int getIndexOf(Waypoint* wp);                               ///< Get the index of a waypoint in the list
     /*@}*/
 
-    /** @name Global waypoint list operations */
-    /*@{*/
-    const QVector<Waypoint *> &getGlobalWaypointList(void) { return waypoints; }  ///< Returns a const reference to the global waypoint list.
-    void globalAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
-    int globalRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
-    /*@}*/
+    UAS& getUAS() { return this->uas; }                         ///< Returns the owning UAS
+
+//    /** @name Global waypoint list operations */
+//    /*@{*/
+//    const QVector<Waypoint *> &getGlobalWaypointList(void) { return waypoints; }  ///< Returns a const reference to the global waypoint list.
+//    void globalAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
+//    int globalRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
+//    /*@}*/
 
 private:
     /** @name Message send functions */
@@ -120,14 +122,17 @@ private:
 
 public slots:
     void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
+    void addWaypoint(Waypoint *wp);                 ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
 
 signals:
-    void waypointListChanged(void);                 ///< emits signal that the local waypoint list has been changed
+    void waypointListChanged(void);                 ///< emits signal that the waypoint list has been changed
+    void waypointListChanged(int uasid);            ///< Emits signal that list has been changed
+    void waypointChanged(int uasid, Waypoint* wp);  ///< emits signal that waypoint has been changed
     void currentWaypointChanged(quint16);           ///< emits the new current waypoint sequence number
     void updateStatusString(const QString &);       ///< emits the current status string
 
-    void loadWPFile();                                ///< emits signal that a file wp has been load
-    void readGlobalWPFromUAS(bool value);                            ///< emits signal when finish to read Global WP from UAS
+    void loadWPFile();                              ///< emits signal that a file wp has been load
+    void readGlobalWPFromUAS(bool value);           ///< emits signal when finish to read Global WP from UAS
 
 private:
     UAS &uas;                                       ///< Reference to the corresponding UAS

src/ui/HDDisplay.cc

@@ -152,7 +152,7 @@ HDDisplay::~HDDisplay()
 
 QSize HDDisplay::sizeHint() const
 {
-    return QSize(400, 400.0f*(vwidth/vheight)*1.1f);
+    return QSize(400, 400.0f*(vwidth/vheight)*1.2f);
 }
 
 void HDDisplay::enableGLRendering(bool enable)

src/ui/HSIDisplay.cc

@@ -678,7 +678,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
 {
     if (uas)
     {
-        const QVector<Waypoint*>& list = uas->getWaypointManager().getWaypointList();
+		const QVector<Waypoint*>& list = uas->getWaypointManager()->getWaypointList();
         //        for (int i = 0; i < list.size(); i++)
         //        {
         //            QPointF in(list.at(i)->getX(), list.at(i)->getY());

src/ui/HUD.cc

@@ -30,6 +30,10 @@ This file is part of the QGROUNDCONTROL project
  */
 
 #include <QShowEvent>
+#include <QContextMenuEvent>
+#include <QMenu>
+#include <QDesktopServices>
+#include <QFileDialog>
 
 #include <QDebug>
 #include <cmath>
@@ -39,6 +43,7 @@ This file is part of the QGROUNDCONTROL project
 #include <limits>
 
 #include "UASManager.h"
+#include "UAS.h"
 #include "HUD.h"
 #include "MG.h"
 #include "QGC.h"
@@ -124,7 +129,13 @@ HUD::HUD(int width, int height, QWidget* parent)
     lat(0.0),
     lon(0.0),
     alt(0.0),
-    load(0.0f)
+    load(0.0f),
+    offlineDirectory(""),
+    nextOfflineImage(""),
+    hudInstrumentsEnabled(true),
+    videoEnabled(false),
+    xImageFactor(1.0),
+    yImageFactor(1.0)
 {
     // Set auto fill to false
     setAutoFillBackground(false);
@@ -183,6 +194,10 @@ HUD::HUD(int width, int height, QWidget* parent)
     // Connect with UAS
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
 
+    createActions();
+
+    if (UASManager::instance()->getActiveUAS() != NULL) setActiveUAS(UASManager::instance()->getActiveUAS());
+
     setVisible(false);
 }
 
@@ -212,6 +227,40 @@ void HUD::hideEvent(QHideEvent* event)
     refreshTimer->stop();
 }
 
+void HUD::contextMenuEvent (QContextMenuEvent* event)
+{
+    QMenu menu(this);
+    // Update actions
+    enableHUDAction->setChecked(hudInstrumentsEnabled);
+    enableVideoAction->setChecked(videoEnabled);
+
+    menu.addAction(enableHUDAction);
+    //menu.addAction(selectHUDColorAction);
+    menu.addAction(enableVideoAction);
+    menu.addAction(selectOfflineDirectoryAction);
+    //menu.addAction(selectVideoChannelAction);
+    menu.exec(event->globalPos());
+}
+
+void HUD::createActions()
+{
+    enableHUDAction = new QAction(tr("Enable HUD"), this);
+    enableHUDAction->setStatusTip(tr("Show the HUD instruments in this window"));
+    enableHUDAction->setCheckable(true);
+    enableHUDAction->setChecked(hudInstrumentsEnabled);
+    connect(enableHUDAction, SIGNAL(triggered(bool)), this, SLOT(enableHUDInstruments(bool)));
+
+    enableVideoAction = new QAction(tr("Enable Video Live feed"), this);
+    enableVideoAction->setStatusTip(tr("Show the video live feed"));
+    enableVideoAction->setCheckable(true);
+    enableVideoAction->setChecked(videoEnabled);
+    connect(enableVideoAction, SIGNAL(triggered(bool)), this, SLOT(enableVideo(bool)));
+
+    selectOfflineDirectoryAction = new QAction(tr("Select image log"), this);
+    selectOfflineDirectoryAction->setStatusTip(tr("Load previously logged images into simulation / replay"));
+    connect(selectOfflineDirectoryAction, SIGNAL(triggered()), this, SLOT(selectOfflineDirectory()));
+}
+
 /**
  *
  * @param uas the UAS/MAV to monitor/display with the HUD
@@ -230,22 +279,39 @@ void HUD::setActiveUAS(UASInterface* uas)
         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
-    // 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*)));
+        // Try to disconnect the image link
+        UAS* u = dynamic_cast<UAS*>(uas);
+        if (u)
+        {
+            disconnect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
+        }
+    }
 
-    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)));
+    if (uas)
+    {
+        // Now connect the new UAS
+        // 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(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)));
+
+        // Try to connect the image link
+        UAS* u = dynamic_cast<UAS*>(uas);
+        if (u)
+        {
+            connect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
+        }
 
-    // Set new UAS
-    this->uas = uas;
+        // Set new UAS
+        this->uas = uas;
+    }
 }
 
 //void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec)
@@ -609,174 +675,183 @@ void HUD::paintHUD()
         glPushMatrix();
         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
-        // Blue / Brown background
-        if (noCamera)
-        {
-            paintCenterBackground(roll, pitch, yawTrans);
-        }
-        // else {
         // Fill with black background
-        QString imagePath = "/Users/user/Desktop/frame0000.png";
-        if (QFileInfo(imagePath).exists())
+        if (videoEnabled)
         {
-            //qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
-            QImage fill = QImage(imagePath);
+            if (nextOfflineImage != "" && QFileInfo(nextOfflineImage).exists())
+            {
+                qDebug() << __FILE__ << __LINE__ << "template image:" << nextOfflineImage;
+                QImage fill = QImage(nextOfflineImage);
 
-            glImage = QGLWidget::convertToGLFormat(fill);
+                xImageFactor = width() / (float)fill.width();
+                yImageFactor = height() / (float)fill.height();
 
-            float xFactor;
-            float yFactor;
+                glImage = QGLWidget::convertToGLFormat(fill);
 
-            xFactor = width() / (float)fill.width();
-            yFactor = height() / (float)fill.height();
+                // Reset to save load efforts
+                nextOfflineImage = "";
+            }
 
-            glPixelZoom(xFactor, yFactor);
+            glPixelZoom(xImageFactor, yImageFactor);
             // Resize to correct size and fill with image
             glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());
-            // FIXME Adjust viewport to scale image into view
+        }
+        else
+        {
+            // Blue / brown background
+            paintCenterBackground(roll, pitch, yawTrans);
         }
 
-        // }
         glMatrixMode(GL_MODELVIEW);
         glPopMatrix();
 
         // END OF OPENGL PAINTING
 
+        if (hudInstrumentsEnabled)
+        {
 
+            //glEnable(GL_MULTISAMPLE);
+
+            // QT PAINTING
+            //makeCurrent();
+            QPainter painter;
+            painter.begin(this);
+            painter.setRenderHint(QPainter::Antialiasing, true);
+            painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
+            painter.translate((this->vwidth/2.0+xCenterOffset)*scalingFactor, (this->vheight/2.0+yCenterOffset)*scalingFactor);
+
+
+
+            // COORDINATE FRAME IS NOW (0,0) at CENTER OF WIDGET
+
+
+            // Draw all fixed indicators
+            // MODE
+            paintText(mode, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 10, &painter);
+            // STATE
+            paintText(state, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 15, &painter);
+            // BATTERY
+            paintText(fuelStatus, fuelColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 20, &painter);
+            // Waypoint
+            paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter);
 
-        //glEnable(GL_MULTISAMPLE);
-
-        // QT PAINTING
-        //makeCurrent();
-        QPainter painter;
-        painter.begin(this);
-        painter.setRenderHint(QPainter::Antialiasing, true);
-        painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
-        painter.translate((this->vwidth/2.0+xCenterOffset)*scalingFactor, (this->vheight/2.0+yCenterOffset)*scalingFactor);
-
-
-
-        // COORDINATE FRAME IS NOW (0,0) at CENTER OF WIDGET
-
-
-        // Draw all fixed indicators
-        // MODE
-        paintText(mode, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 10, &painter);
-        // STATE
-        paintText(state, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 15, &painter);
-        // BATTERY
-        paintText(fuelStatus, fuelColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 20, &painter);
-        // Waypoint
-        paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter);
-
-        // YAW INDICATOR
-        //
-        //      .
-        //    .   .
-        //   .......
-        //
-        const float yawIndicatorWidth = 4.0f;
-        const float yawIndicatorY = vheight/2.0f - 10.0f;
-        QPolygon yawIndicator(4);
-        yawIndicator.setPoint(0, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
-        yawIndicator.setPoint(1, QPoint(refToScreenX(yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
-        yawIndicator.setPoint(2, QPoint(refToScreenX(-yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
-        yawIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
-        painter.setPen(defaultColor);
-        painter.drawPolyline(yawIndicator);
+            // YAW INDICATOR
+            //
+            //      .
+            //    .   .
+            //   .......
+            //
+            const float yawIndicatorWidth = 4.0f;
+            const float yawIndicatorY = vheight/2.0f - 10.0f;
+            QPolygon yawIndicator(4);
+            yawIndicator.setPoint(0, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
+            yawIndicator.setPoint(1, QPoint(refToScreenX(yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
+            yawIndicator.setPoint(2, QPoint(refToScreenX(-yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
+            yawIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
+            painter.setPen(defaultColor);
+            painter.drawPolyline(yawIndicator);
 
-        // CENTER
+            // CENTER
 
-        // HEADING INDICATOR
-        //
-        //    __      __
-        //       \/\/
-        //
-        const float hIndicatorWidth = 7.0f;
-        const float hIndicatorY = -25.0f;
-        const float hIndicatorYLow = hIndicatorY + hIndicatorWidth / 6.0f;
-        const float hIndicatorSegmentWidth = hIndicatorWidth / 7.0f;
-        QPolygon hIndicator(7);
-        hIndicator.setPoint(0, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
-        hIndicator.setPoint(1, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f+hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
-        hIndicator.setPoint(2, QPoint(refToScreenX(0.0f-hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
-        hIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(hIndicatorY)));
-        hIndicator.setPoint(4, QPoint(refToScreenX(0.0f+hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
-        hIndicator.setPoint(5, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f-hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
-        hIndicator.setPoint(6, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
-        painter.setPen(defaultColor);
-        painter.drawPolyline(hIndicator);
+            // HEADING INDICATOR
+            //
+            //    __      __
+            //       \/\/
+            //
+            const float hIndicatorWidth = 7.0f;
+            const float hIndicatorY = -25.0f;
+            const float hIndicatorYLow = hIndicatorY + hIndicatorWidth / 6.0f;
+            const float hIndicatorSegmentWidth = hIndicatorWidth / 7.0f;
+            QPolygon hIndicator(7);
+            hIndicator.setPoint(0, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
+            hIndicator.setPoint(1, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f+hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
+            hIndicator.setPoint(2, QPoint(refToScreenX(0.0f-hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
+            hIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(hIndicatorY)));
+            hIndicator.setPoint(4, QPoint(refToScreenX(0.0f+hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
+            hIndicator.setPoint(5, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f-hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
+            hIndicator.setPoint(6, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
+            painter.setPen(defaultColor);
+            painter.drawPolyline(hIndicator);
 
 
-        // SETPOINT
-        const float centerWidth = 4.0f;
-        painter.setPen(defaultColor);
-        painter.setBrush(Qt::NoBrush);
-        // TODO
-        //painter.drawEllipse(QPointF(refToScreenX(qMin(10.0f, values.value("roll desired", 0.0f) * 10.0f)), refToScreenY(qMin(10.0f, values.value("pitch desired", 0.0f) * 10.0f))), refToScreenX(centerWidth/2.0f), refToScreenX(centerWidth/2.0f));
+            // SETPOINT
+            const float centerWidth = 4.0f;
+            painter.setPen(defaultColor);
+            painter.setBrush(Qt::NoBrush);
+            // TODO
+            //painter.drawEllipse(QPointF(refToScreenX(qMin(10.0f, values.value("roll desired", 0.0f) * 10.0f)), refToScreenY(qMin(10.0f, values.value("pitch desired", 0.0f) * 10.0f))), refToScreenX(centerWidth/2.0f), refToScreenX(centerWidth/2.0f));
 
-        const float centerCrossWidth = 10.0f;
-        // left
-        painter.drawLine(QPointF(refToScreenX(-centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(-centerCrossWidth / 2.0f), refToScreenY(0.0f)));
-        // right
-        painter.drawLine(QPointF(refToScreenX(centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(centerCrossWidth / 2.0f), refToScreenY(0.0f)));
-        // top
-        painter.drawLine(QPointF(refToScreenX(0.0f), refToScreenY(-centerWidth / 2.0f)), QPointF(refToScreenX(0.0f), refToScreenY(-centerCrossWidth / 2.0f)));
+            const float centerCrossWidth = 10.0f;
+            // left
+            painter.drawLine(QPointF(refToScreenX(-centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(-centerCrossWidth / 2.0f), refToScreenY(0.0f)));
+            // right
+            painter.drawLine(QPointF(refToScreenX(centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(centerCrossWidth / 2.0f), refToScreenY(0.0f)));
+            // top
+            painter.drawLine(QPointF(refToScreenX(0.0f), refToScreenY(-centerWidth / 2.0f)), QPointF(refToScreenX(0.0f), refToScreenY(-centerCrossWidth / 2.0f)));
 
 
 
-        // COMPASS
-        const float compassY = -vheight/2.0f + 10.0f;
-        QRectF compassRect(QPointF(refToScreenX(-5.0f), refToScreenY(compassY)), QSizeF(refToScreenX(10.0f), refToScreenY(5.0f)));
-        painter.setBrush(Qt::NoBrush);
-        painter.setPen(Qt::SolidLine);
-        painter.setPen(defaultColor);
-        painter.drawRoundedRect(compassRect, 2, 2);
-        QString yawAngle;
+            // COMPASS
+            const float compassY = -vheight/2.0f + 10.0f;
+            QRectF compassRect(QPointF(refToScreenX(-5.0f), refToScreenY(compassY)), QSizeF(refToScreenX(10.0f), refToScreenY(5.0f)));
+            painter.setBrush(Qt::NoBrush);
+            painter.setPen(Qt::SolidLine);
+            painter.setPen(defaultColor);
+            painter.drawRoundedRect(compassRect, 2, 2);
+            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)+180.f;
 
-        // 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);
+            // 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, -zSpeed, &painter);
+            // CHANGE RATE STRIPS
+            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, xSpeed, &painter);
-
-        // GAUGES
+            // CHANGE RATE STRIPS
+            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, -zPos, defaultColor, &painter, false);
+            // Left altitude gauge
+            drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, -zPos, defaultColor, &painter, false);
 
-        // Right speed gauge
-        drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, xSpeed, defaultColor, &painter, false);
+            // Right speed gauge
+            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
-
-
-        painter.translate(refToScreenX(yawTrans), 0);
-
-        // Rotate view and draw all roll-dependent indicators
-        painter.rotate((rollLP/M_PI)* -180.0f);
-
-        painter.translate(0, (-pitchLP/M_PI)* -180.0f * refToScreenY(1.8));
-
-        //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f);
-
-        // PITCH
+            // Waypoint name
+            if (waypointName != "") paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter);
+
+            // MOVING PARTS
+
+
+            painter.translate(refToScreenX(yawTrans), 0);
+
+            // Rotate view and draw all roll-dependent indicators
+            painter.rotate((rollLP/M_PI)* -180.0f);
+
+            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(pitchLP, &painter);
-        painter.end();
+            paintPitchLines(pitchLP, &painter);
+            painter.end();
+        }
+        else
+        {
+            QPainter painter;
+            painter.begin(this);
+            painter.end();
+        }
         //glDisable(GL_MULTISAMPLE);
 
+
+
         //glFlush();
     }
 }
@@ -1517,6 +1592,34 @@ void HUD::saveImage()
     saveImage(fileName);
 }
 
+void HUD::startImage(quint64 timestamp)
+{
+    if (videoEnabled && offlineDirectory != "")
+    {
+        // Load and diplay image file
+        nextOfflineImage = QString(offlineDirectory + "/%1.bmp").arg(timestamp);
+    }
+}
+
+void HUD::selectOfflineDirectory()
+{
+    QString fileName = QFileDialog::getExistingDirectory(this, tr("Select image directory"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation));
+    if (fileName != "")
+    {
+        offlineDirectory = fileName;
+    }
+}
+
+void HUD::enableHUDInstruments(bool enabled)
+{
+    hudInstrumentsEnabled = enabled;
+}
+
+void HUD::enableVideo(bool enabled)
+{
+    videoEnabled = enabled;
+}
+
 void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int startIndex)
 {
     Q_UNUSED(imgid);

src/ui/HUD.h

@@ -76,11 +76,19 @@ public slots:
     void updateLoad(UASInterface*, double);
     void selectWaypoint(int uasId, int id);
 
+    void startImage(quint64 timestamp);
     void startImage(int imgid, int width, int height, int depth, int channels);
     void setPixels(int imgid, const unsigned char* imageData, int length, int startIndex);
     void finishImage();
     void saveImage();
     void saveImage(QString fileName);
+    /** @brief Select directory where to load the offline files from */
+    void selectOfflineDirectory();
+    /** @brief Enable the HUD instruments */
+    void enableHUDInstruments(bool enabled);
+    /** @brief Enable Video */
+    void enableVideo(bool enabled);
+
 
 protected slots:
     void paintCenterBackground(float roll, float pitch, float yaw);
@@ -119,6 +127,8 @@ protected:
     void showEvent(QShowEvent* event);
     /** @brief Stop updating widget */
     void hideEvent(QHideEvent* event);
+    void contextMenuEvent (QContextMenuEvent* event);
+    void createActions();
 
     static const int updateInterval = 50;
 
@@ -192,6 +202,16 @@ protected:
     double lon;
     double alt;
     float load;
+    QString offlineDirectory;
+    QString nextOfflineImage;
+    bool hudInstrumentsEnabled;
+    bool videoEnabled;
+    float xImageFactor;
+    float yImageFactor;
+    QAction* enableHUDAction;
+    QAction* enableVideoAction;
+    QAction* selectOfflineDirectoryAction;
+    QAction* selectVideoChannelAction;
     void paintEvent(QPaintEvent *event);
 
 };

src/ui/MainWindow.cc

@@ -64,7 +64,7 @@ MainWindow* MainWindow::instance()
 MainWindow::MainWindow(QWidget *parent):
         QMainWindow(parent),
         toolsMenuActions(),
-        currentView(VIEW_ENGINEER),
+        currentView(VIEW_UNCONNECTED),
         aboutToCloseFlag(false),
         changingViewsFlag(false)
 {
@@ -82,47 +82,17 @@ MainWindow::MainWindow(QWidget *parent):
     }
     else
     {
-        if (settings.value("CURRENT_VIEW", VIEW_PILOT) != VIEW_PILOT)
+        // LOAD THE LAST VIEW
+        VIEW_SECTIONS currentViewCandidate = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt();
+        if (currentViewCandidate != VIEW_ENGINEER &&
+            currentViewCandidate != VIEW_OPERATOR &&
+            currentViewCandidate != VIEW_PILOT)
         {
-            currentView = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt();
+            currentView = currentViewCandidate;
         }
     }
 
-    // Check if the settings exist, instantiate defaults if necessary
-
-    // 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);
-    }
-
-    QString listKey = buildMenuKey(SUB_SECTION_CHECKED, MENU_UAS_LIST, currentView);
-    if (!settings.contains(listKey))
-    {
-        settings.setValue(listKey, true);
-    }
+    setDefaultSettingsForAp();
 
     settings.sync();
 
@@ -138,6 +108,7 @@ MainWindow::MainWindow(QWidget *parent):
     perspectives->addAction(ui.actionMavlinkView);
     perspectives->addAction(ui.actionPilotsView);
     perspectives->addAction(ui.actionOperatorsView);
+    perspectives->addAction(ui.actionUnconnectedView);
     perspectives->setExclusive(true);
 
     // Mark the right one as selected
@@ -145,9 +116,13 @@ MainWindow::MainWindow(QWidget *parent):
     if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true);
     if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true);
     if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true);
+    if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true);
 
-    // The pilot view is not available on startup
+    // The pilot, engineer and operator view are not available on startup
+    // since they only make sense with a system connected.
     ui.actionPilotsView->setEnabled(false);
+    ui.actionOperatorsView->setEnabled(false);
+    ui.actionEngineersView->setEnabled(false);
 
     buildCommonWidgets();
 
@@ -217,6 +192,54 @@ MainWindow::~MainWindow()
 
 }
 
+/**
+ * Set default settings for this AP type.
+ */
+void MainWindow::setDefaultSettingsForAp()
+{
+    // Check if the settings exist, instantiate defaults if necessary
+
+    // UNCONNECTED VIEW DEFAULT
+    QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_UNCONNECTED);
+    if (!settings.contains(centralKey))
+    {
+        settings.setValue(centralKey,true);
+    }
+
+    // OPERATOR VIEW DEFAULT
+    centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_OPERATOR);
+    if (!settings.contains(centralKey))
+    {
+        settings.setValue(centralKey,true);
+
+        // ENABLE UAS LIST
+        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,VIEW_OPERATOR), true);
+        // ENABLE HUD TOOL WIDGET
+        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HUD,VIEW_OPERATOR), 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);
+    }
+}
+
 void MainWindow::resizeEvent(QResizeEvent * event)
 {
     Q_UNUSED(event);
@@ -289,7 +312,7 @@ void MainWindow::buildCommonWidgets()
         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);
+        addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea);
     }
 
     if (!listDockWidget)
@@ -297,7 +320,7 @@ void MainWindow::buildCommonWidgets()
         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);
+        addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget(bool)), MENU_UAS_LIST, Qt::RightDockWidgetArea);
     }
 
     if (!waypointsDockWidget)
@@ -305,7 +328,7 @@ void MainWindow::buildCommonWidgets()
         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);
+        addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget(bool)), MENU_WAYPOINTS, Qt::BottomDockWidgetArea);
     }
 
     if (!infoDockWidget)
@@ -313,7 +336,7 @@ void MainWindow::buildCommonWidgets()
         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);
+        addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget(bool)), MENU_STATUS, Qt::RightDockWidgetArea);
     }
 
     if (!debugConsoleDockWidget)
@@ -321,7 +344,7 @@ void MainWindow::buildCommonWidgets()
         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);
+        addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget(bool)), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea);
     }
 
     if (!logPlayerDockWidget)
@@ -329,13 +352,13 @@ void MainWindow::buildCommonWidgets()
         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);
+        addToToolsMenu(logPlayerDockWidget, tr("MAVLink Log Replay"), SLOT(showToolWidget(bool)), MENU_MAVLINK_LOG_PLAYER, Qt::RightDockWidgetArea);
     }
 
     // Center widgets
     if (!mapWidget)
     {
-        mapWidget         = new MapWidget(this);
+        mapWidget = new MapWidget(this);
         addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP);
     }
 
@@ -393,17 +416,17 @@ void MainWindow::buildPxWidgets()
 #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);
-//    }
+    if (!_3DMapWidget)
+    {
+        _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
+        addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
+    }
 #endif
 
 #if (defined _MSC_VER) | (defined Q_OS_MAC)
@@ -422,7 +445,7 @@ void MainWindow::buildPxWidgets()
         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);
+        addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget(bool)), MENU_DETECTION, Qt::RightDockWidgetArea);
     }
 
     if (!parametersDockWidget)
@@ -430,7 +453,7 @@ void MainWindow::buildPxWidgets()
         parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this);
         parametersDockWidget->setWidget( new ParameterInterface(this) );
         parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET");
-        addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget()), MENU_PARAMETERS, Qt::RightDockWidgetArea);
+        addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea);
     }
 
     if (!watchdogControlDockWidget)
@@ -438,7 +461,7 @@ void MainWindow::buildPxWidgets()
         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);
+        addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget(bool)), MENU_WATCHDOG, Qt::BottomDockWidgetArea);
     }
 
     if (!hsiDockWidget)
@@ -446,7 +469,7 @@ void MainWindow::buildPxWidgets()
         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);
+        addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea);
     }
 
     if (!headDown1DockWidget)
@@ -454,7 +477,7 @@ void MainWindow::buildPxWidgets()
         headDown1DockWidget = new QDockWidget(tr("Flight Display"), this);
         headDown1DockWidget->setWidget( new HDDisplay(acceptList, "Flight Display", this) );
         headDown1DockWidget->setObjectName("HEAD_DOWN_DISPLAY_1_DOCK_WIDGET");
-        addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget()), MENU_HDD_1, Qt::RightDockWidgetArea);
+        addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget(bool)), MENU_HDD_1, Qt::RightDockWidgetArea);
     }
 
     if (!headDown2DockWidget)
@@ -462,7 +485,7 @@ void MainWindow::buildPxWidgets()
         headDown2DockWidget = new QDockWidget(tr("Payload Status"), this);
         headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Payload Status", this) );
         headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET");
-        addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget()), MENU_HDD_2, Qt::RightDockWidgetArea);
+        addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea);
     }
 
     if (!rcViewDockWidget)
@@ -470,7 +493,7 @@ void MainWindow::buildPxWidgets()
         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);
+        addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
     }
 
     if (!headUpDockWidget)
@@ -478,7 +501,31 @@ void MainWindow::buildPxWidgets()
         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);
+        addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::RightDockWidgetArea);
+    }
+
+    if (!video1DockWidget)
+    {
+        video1DockWidget = new QDockWidget(tr("Video Stream 1"), this);
+        HUD* video1 =  new HUD(160, 120, this);
+        video1->enableHUDInstruments(false);
+        video1->enableVideo(true);
+        // FIXME select video stream as well
+        video1DockWidget->setWidget(video1);
+        video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET");
+        addToToolsMenu (video1DockWidget, tr("Video Stream 1"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_1, Qt::LeftDockWidgetArea);
+    }
+
+    if (!video2DockWidget)
+    {
+        video2DockWidget = new QDockWidget(tr("Video Stream 2"), this);
+        HUD* video2 =  new HUD(160, 120, this);
+        video2->enableHUDInstruments(false);
+        video2->enableVideo(true);
+        // FIXME select video stream as well
+        video2DockWidget->setWidget(video2);
+        video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET");
+        addToToolsMenu (video2DockWidget, tr("Video Stream 2"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_2, Qt::LeftDockWidgetArea);
     }
 
     // Dialogue widgets
@@ -499,7 +546,7 @@ void MainWindow::buildSlugsWidgets()
         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);
+        addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea);
     }
 
     if (!rcViewDockWidget)
@@ -507,7 +554,7 @@ void MainWindow::buildSlugsWidgets()
         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);
+        addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
     }
 
 //    if (!slugsDataWidget)
@@ -553,15 +600,17 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget,
     QAction* tempAction;
 
 
-    // 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);
-    }
+// Not needed any more - separate menu now available
+
+//    // 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->addAction(title);
+    tempAction = ui.menuMain->addAction(title);
 
     tempAction->setCheckable(true);
     tempAction->setData(centralWidget);
@@ -583,13 +632,17 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget,
     }
 
     // connect the action
-    connect(tempAction,SIGNAL(triggered()),this, slotName);
+    connect(tempAction,SIGNAL(triggered(bool)),this, slotName);
 }
 
 
 void MainWindow::showCentralWidget()
 {
     QAction* senderAction = qobject_cast<QAction *>(sender());
+
+    // Block sender action while manipulating state
+    senderAction->blockSignals(true);
+
     int tool = senderAction->data().toInt();
     QString chKey;
 
@@ -597,7 +650,6 @@ void MainWindow::showCentralWidget()
 
     if (senderAction && dockWidgets[tool])
     {
-
         // uncheck all central widget actions
         QHashIterator<int, QAction*> i(toolsMenuActions);
         while (i.hasNext())
@@ -606,7 +658,11 @@ void MainWindow::showCentralWidget()
             //qDebug() << "shCW" << i.key() << "read";
             if (i.value() && i.value()->data().toInt() > 255)
             {
+                // Block signals and uncheck action
+                // firing would be unneccesary
+                i.value()->blockSignals(true);
                 i.value()->setChecked(false);
+                i.value()->blockSignals(false);
 
                 // update the settings
                 chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.value()->data().toInt()), currentView);
@@ -625,6 +681,9 @@ void MainWindow::showCentralWidget()
         chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(tool), currentView);
         settings.setValue(chKey,true);
 
+        // Unblock sender action
+        senderAction->blockSignals(false);
+
         presentView();
     }
 }
@@ -684,39 +743,97 @@ void MainWindow::addToToolsMenu ( QWidget* widget,
     }
     else
     {
-        tempAction->setChecked(settings.value(chKey).toBool());
+        tempAction->setChecked(settings.value(chKey, false).toBool());
         widget->setVisible(settings.value(chKey, false).toBool());
     }
 
     // connect the action
-    connect(tempAction,SIGNAL(triggered()),this, slotName);
+    connect(tempAction,SIGNAL(toggled(bool)),this, slotName);
+
+    connect(qobject_cast <QDockWidget *>(dockWidgets[tool]),
+            SIGNAL(visibilityChanged(bool)), this, SLOT(showToolWidget(bool)));
 
-//  connect(qobject_cast <QDockWidget *>(dockWidgets[tool]),
-//          SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool)));
+    //  connect(qobject_cast <QDockWidget *>(dockWidgets[tool]),
+    //          SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool)));
 
     connect(qobject_cast <QDockWidget *>(dockWidgets[tool]),
             SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea)));
-
 }
 
-void MainWindow::showToolWidget()
+void MainWindow::showToolWidget(bool visible)
 {
-    QAction* temp = qobject_cast<QAction *>(sender());
-    int tool = temp->data().toInt();
-
-    if (temp && dockWidgets[tool])
+    if (!aboutToCloseFlag && !changingViewsFlag)
     {
-        if (temp->isChecked())
+        QAction* action = qobject_cast<QAction *>(sender());
+
+        // Prevent this to fire if undocked
+        if (action)
         {
-            addDockWidget(dockWidgetLocations[tool], qobject_cast<QDockWidget *> (dockWidgets[tool]));
-            qobject_cast<QDockWidget *>(dockWidgets[tool])->show();
+            int tool = action->data().toInt();
+
+            QDockWidget* dockWidget = qobject_cast<QDockWidget *> (dockWidgets[tool]);
+
+            qDebug() << "DATA:" << tool << "FLOATING" << dockWidget->isFloating() << "checked" << action->isChecked() << "visible" << dockWidget->isVisible() << "action vis:" << action->isVisible();
+            if (dockWidget && dockWidget->isVisible() != visible)
+            {
+                if (visible)
+                {
+                    qDebug() << "DOCK WIDGET ADDED";
+                    addDockWidget(dockWidgetLocations[tool], dockWidget);
+                    dockWidget->show();
+                }
+                else
+                {
+                    qDebug() << "DOCK WIDGET REMOVED";
+                    removeDockWidget(dockWidget);
+                    //dockWidget->hide();
+                }
+
+                QHashIterator<int, QWidget*> i(dockWidgets);
+                while (i.hasNext())
+                {
+                    i.next();
+                    if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == dockWidget)
+                    {
+                        QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.key()), currentView);
+                        settings.setValue(chKey,visible);
+                        qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible;
+                        break;
+                    }
+                }
+            }
         }
-        else
+
+        QDockWidget* dockWidget = qobject_cast<QDockWidget*>(QObject::sender());
+
+        qDebug() << "Trying to cast dockwidget" << dockWidget << "isvisible" << visible;
+
+        if (dockWidget)
         {
-            removeDockWidget(qobject_cast<QDockWidget *>(dockWidgets[tool]));
+            // Get action
+            int tool = dockWidgets.key(dockWidget);
+
+            qDebug() << "Updating widget setting" << tool << "to" << visible;
+
+            QAction* action = toolsMenuActions[tool];
+            action->blockSignals(true);
+            action->setChecked(visible);
+            action->blockSignals(false);
+
+            QHashIterator<int, QWidget*> i(dockWidgets);
+            while (i.hasNext())
+            {
+                i.next();
+                if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == dockWidget)
+                {
+                    QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.key()), currentView);
+                    settings.setValue(chKey,visible);
+                    qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible;
+                    break;
+                }
+            }
         }
     }
-
 }
 
 
@@ -728,6 +845,8 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view)
 
     tempVisible =  settings.value(buildMenuKey(SUB_SECTION_CHECKED,widget,view), false).toBool();
 
+     qDebug() << "showTheWidget(): Set key" << buildMenuKey(SUB_SECTION_CHECKED,widget,view) << "to" << tempVisible;
+
     if (tempWidget)
     {
         toolsMenuActions[widget]->setChecked(tempVisible);
@@ -746,12 +865,14 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view)
     //        }
     //    }
 
-    if ((tempWidget != NULL) && tempVisible)
+    if (tempWidget != NULL)
     {
-        addDockWidget(tempLocation, tempWidget);
-        tempWidget->show();
+        if (tempVisible)
+        {
+            addDockWidget(tempLocation, tempWidget);
+            tempWidget->show();
+        }
     }
-
 }
 
 QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view)
@@ -763,11 +884,11 @@ QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES t
              UASManager::instance()->getActiveUAS()->getAutopilotType():
              -1;
 
-    return (QString::number(apType) + "/" +
-            QString::number(SECTION_MENU) + "/" +
-            QString::number(view) + "/" +
-            QString::number(tool) + "/" +
-            QString::number(section) + "/" );
+    return (QString::number(apType) + "_" +
+            QString::number(SECTION_MENU) + "_" +
+            QString::number(view) + "_" +
+            QString::number(tool) + "_" +
+            QString::number(section) + "_" );
 }
 
 void MainWindow::closeEvent(QCloseEvent *event)
@@ -785,6 +906,29 @@ void MainWindow::closeEvent(QCloseEvent *event)
     QMainWindow::closeEvent(event);
 }
 
+void MainWindow::showDockWidget (bool vis)
+{
+    if (!aboutToCloseFlag && !changingViewsFlag)
+    {
+        QDockWidget* temp = qobject_cast<QDockWidget *>(sender());
+
+        if (temp)
+        {
+            QHashIterator<int, QWidget*> i(dockWidgets);
+            while (i.hasNext())
+            {
+                i.next();
+                if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == temp)
+                {
+                    QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.key()), currentView);
+                    settings.setValue(chKey,vis);
+                    toolsMenuActions[i.key()]->setChecked(vis);
+                    break;
+                }
+            }
+        }
+    }
+}
 
 void MainWindow::updateVisibilitySettings (bool vis)
 {
@@ -795,9 +939,11 @@ void MainWindow::updateVisibilitySettings (bool vis)
         if (temp)
         {
             QHashIterator<int, QWidget*> i(dockWidgets);
-        while (i.hasNext()) {
+            while (i.hasNext())
+            {
                 i.next();
-          if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == temp) {
+                if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == temp)
+                {
                     QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.key()), currentView);
                     settings.setValue(chKey,vis);
                     toolsMenuActions[i.key()]->setChecked(vis);
@@ -838,13 +984,7 @@ void MainWindow::connectCommonWidgets()
 
     if (mapWidget && waypointsDockWidget->widget())
     {
-        // clear path create on the map
-        connect(waypointsDockWidget->widget(), SIGNAL(clearPathclicked()), mapWidget, SLOT(clearWaypoints()));
-        // add Waypoint widget in the WaypointList widget when mouse clicked
-        connect(mapWidget, SIGNAL(captureMapCoordinateClick(QPointF)), waypointsDockWidget->widget(), SLOT(addWaypointMouse(QPointF)));
 
-        // it notifies that a waypoint global goes to do create and a map graphic too
-        connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF)));
     }
 
     //TODO temporaly debug
@@ -1130,6 +1270,7 @@ void MainWindow::connectCommonActions()
     connect(ui.actionPilotsView, SIGNAL(triggered()), this, SLOT(loadPilotView()));
     connect(ui.actionEngineersView, SIGNAL(triggered()), this, SLOT(loadEngineerView()));
     connect(ui.actionOperatorsView, SIGNAL(triggered()), this, SLOT(loadOperatorView()));
+    connect(ui.actionUnconnectedView, SIGNAL(triggered()), this, SLOT(loadUnconnectedView()));
 
     connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView()));
     connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet()));
@@ -1276,8 +1417,13 @@ void MainWindow::UASCreated(UASInterface* uas)
 
     if (uas != NULL)
     {
-        // The pilot view was not available on startup, enable it now
+        // Set default settings
+        setDefaultSettingsForAp();
+
+        // The pilot, operator and engineer views were not available on startup, enable them now
         ui.actionPilotsView->setEnabled(true);
+        ui.actionOperatorsView->setEnabled(true);
+        ui.actionEngineersView->setEnabled(true);
 
         QIcon icon;
         // Set matching icon
@@ -1371,6 +1517,7 @@ void MainWindow::UASCreated(UASInterface* uas)
                 //                }
                 //              }
                 //            }
+
             }
             break;
         default:
@@ -1391,8 +1538,6 @@ void MainWindow::UASCreated(UASInterface* uas)
                 connectPxActions();
             }
             break;
-
-            loadOperatorView();
         }
 
         // Change the view only if this is the first UAS
@@ -1406,19 +1551,31 @@ void MainWindow::UASCreated(UASInterface* uas)
             // Load last view if setting is present
             if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED"))
             {
+                clearView();
                 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()))
+                switch (view)
                 {
-                    restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion());
+                case VIEW_ENGINEER:
+                    loadEngineerView();
+                    break;
+                case VIEW_MAVLINK:
+                    loadMAVLinkView();
+                    break;
+                case VIEW_PILOT:
+                    loadPilotView();
+                    break;
+                case VIEW_UNCONNECTED:
+                    loadUnconnectedView();
+                    break;
+                case VIEW_OPERATOR:
+                default:
+                    loadOperatorView();
+                    break;
                 }
             }
             else
             {
-                loadEngineerView();
+                loadOperatorView();
             }
         }
 
@@ -1436,7 +1593,7 @@ void MainWindow::UASCreated(UASInterface* uas)
 void MainWindow::clearView()
 {
     // Save current state
-    if (UASManager::instance()->getActiveUAS()) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion()));
+    if (UASManager::instance()->getUASList().count() > 0) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion()));
     settings.setValue(getWindowGeometryKey(), saveGeometry());
 
     QAction* temp;
@@ -1449,12 +1606,12 @@ void MainWindow::clearView()
 
         if (temp)
         {
-            //qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked();
+            qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked();
             settings.setValue(chKey,temp->isChecked());
         }
         else
         {
-            //qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED";
+            qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED";
             settings.setValue(chKey,false);
         }
     }
@@ -1471,11 +1628,8 @@ void MainWindow::clearView()
         if (dockWidget)
         {
             // Remove dock widget from main window
-            //this->removeDockWidget(dockWidget);
-            dockWidget->setVisible(false);
-            // Deletion of dockWidget would also delete all child
-            // widgets of dockWidget
-            // Is there a way to unset a widget from QDockWidget?
+            removeDockWidget(dockWidget);
+            //dockWidget->setVisible(false);
         }
     }
     changingViewsFlag = false;
@@ -1485,11 +1639,10 @@ void MainWindow::loadEngineerView()
 {
     if (currentView != VIEW_ENGINEER)
     {
-    clearView();
-
-    currentView = VIEW_ENGINEER;
-
-    presentView();
+        clearView();
+        currentView = VIEW_ENGINEER;
+        ui.actionEngineersView->setChecked(true);
+        presentView();
     }
 }
 
@@ -1497,11 +1650,21 @@ void MainWindow::loadOperatorView()
 {
     if (currentView != VIEW_OPERATOR)
     {
-    clearView();
-
-    currentView = VIEW_OPERATOR;
+        clearView();
+        currentView = VIEW_OPERATOR;
+        ui.actionOperatorsView->setChecked(true);
+        presentView();
+    }
+}
 
-    presentView();
+void MainWindow::loadUnconnectedView()
+{
+    if (currentView != VIEW_UNCONNECTED)
+    {
+        clearView();
+        currentView = VIEW_UNCONNECTED;
+        ui.actionUnconnectedView->setChecked(true);
+        presentView();
     }
 }
 
@@ -1509,11 +1672,10 @@ void MainWindow::loadPilotView()
 {
     if (currentView != VIEW_PILOT)
     {
-    clearView();
-
-    currentView = VIEW_PILOT;
-
-    presentView();
+        clearView();
+        currentView = VIEW_PILOT;
+        ui.actionPilotsView->setChecked(true);
+        presentView();
     }
 }
 
@@ -1521,11 +1683,10 @@ void MainWindow::loadMAVLinkView()
 {
     if (currentView != VIEW_MAVLINK)
     {
-    clearView();
-
-    currentView = VIEW_MAVLINK;
-
-    presentView();
+        clearView();
+        currentView = VIEW_MAVLINK;
+        ui.actionMavlinkView->setChecked(true);
+        presentView();
     }
 }
 
@@ -1556,7 +1717,6 @@ void MainWindow::presentView()
     showTheCentralWidget(CENTRAL_DATA_PLOT, currentView);
 
 
-
     // Show docked widgets based on current view and autopilot type
 
     // UAS CONTROL
@@ -1631,8 +1791,29 @@ void MainWindow::presentView()
     // MAVLINK LOG PLAYER
     showTheWidget(MENU_MAVLINK_LOG_PLAYER, currentView);
 
+    // VIDEO 1
+    showTheWidget(MENU_VIDEO_STREAM_1, currentView);
+
+    // VIDEO 2
+    showTheWidget(MENU_VIDEO_STREAM_2, currentView);
+
     this->show();
 
+    // Restore window state
+    if (UASManager::instance()->getUASList().count() > 0)
+    {
+        // Restore the mainwindow size
+        if (settings.contains(getWindowGeometryKey()))
+        {
+            restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray());
+        }
+
+        // Restore the widget positions and size
+        if (settings.contains(getWindowStateKey()))
+        {
+            restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion());
+        }
+    }
 }
 
 void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view)
@@ -1640,15 +1821,17 @@ void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SE
     bool tempVisible;
     QWidget* tempWidget = dockWidgets[centralWidget];
 
-    tempVisible =  settings.value(buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view), false).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])
     {
+        qDebug() << "SETTING TO:" << tempVisible;
         toolsMenuActions[centralWidget]->setChecked(tempVisible);
     }
 
     if (centerStack && tempWidget && tempVisible)
     {
+        qDebug() << "ACTIVATING MAIN WIDGET";
         centerStack->setCurrentWidget(tempWidget);
     }
 }

src/ui/MainWindow.h

@@ -112,6 +112,8 @@ public slots:
     void stopVideoCapture();
     void saveScreen();
 
+    /** @brief Load default view when no MAV is connected */
+    void loadUnconnectedView();
     /** @brief Load view for pilot */
     void loadPilotView();
     /** @brief Load view for engineer */
@@ -166,7 +168,7 @@ public slots:
      * It shows the QDockedWidget based on the action sender
      *
      */
-    void showToolWidget();
+    void showToolWidget(bool visible);
 
     /**
      * @brief Shows a Widget from the center stack based on the action sender
@@ -177,8 +179,10 @@ public slots:
      */
     void showCentralWidget();
 
+    /** @brief Change actively a QDockWidgets visibility by an action */
+    void showDockWidget(bool vis);
     /** @brief Updates a QDockWidget's checked status based on its visibility */
-    void updateVisibilitySettings (bool vis);
+    void updateVisibilitySettings(bool vis);
 
     /** @brief Updates a QDockWidget's location */
     void updateLocationSettings (Qt::DockWidgetArea location);
@@ -187,6 +191,9 @@ protected:
 
     MainWindow(QWidget *parent = 0);
 
+    /** @brief Set default window settings for the current autopilot type */
+    void setDefaultSettingsForAp();
+
     // These defines are used to save the settings when selecting with
     // which widgets populate the views
     // FIXME: DO NOT PUT CUSTOM VALUES IN THIS ENUM since it is iterated over
@@ -212,6 +219,8 @@ protected:
       MENU_SLUGS_HIL,
       MENU_SLUGS_CAMERA,
       MENU_MAVLINK_LOG_PLAYER,
+      MENU_VIDEO_STREAM_1,
+      MENU_VIDEO_STREAM_2,
       CENTRAL_SEPARATOR= 255, // do not change
       CENTRAL_LINECHART,
       CENTRAL_PROTOCOL,
@@ -238,6 +247,7 @@ protected:
       VIEW_OPERATOR,
       VIEW_PILOT,
       VIEW_MAVLINK,
+      VIEW_UNCONNECTED,    ///< View in unconnected mode, when no UAS is available
     } VIEW_SECTIONS;
 
 
@@ -368,6 +378,8 @@ protected:
     QPointer<QDockWidget> watchdogControlDockWidget;
 
     QPointer<QDockWidget> headUpDockWidget;
+    QPointer<QDockWidget> video1DockWidget;
+    QPointer<QDockWidget> video2DockWidget;
     QPointer<QDockWidget> logPlayerDockWidget;
 
     QPointer<QDockWidget> hsiDockWidget;

src/ui/MainWindow.ui

@@ -100,7 +100,7 @@
    </widget>
    <widget class="QMenu" name="menuTools">
     <property name="title">
-     <string>Tools</string>
+     <string>Widgets</string>
     </property>
     <addaction name="actionNewCustomWidget"/>
    </widget>
@@ -121,11 +121,18 @@
     <addaction name="actionPilotsView"/>
     <addaction name="separator"/>
     <addaction name="actionMavlinkView"/>
+    <addaction name="actionUnconnectedView"/>
+   </widget>
+   <widget class="QMenu" name="menuMain">
+    <property name="title">
+     <string>Main</string>
+    </property>
    </widget>
    <addaction name="menuMGround"/>
    <addaction name="menuNetwork"/>
    <addaction name="menuConnected_Systems"/>
    <addaction name="menuUnmanned_System"/>
+   <addaction name="menuMain"/>
    <addaction name="menuTools"/>
    <addaction name="menuPerspectives"/>
    <addaction name="menuHelp"/>
@@ -296,6 +303,9 @@
    <property name="text">
     <string>Operator</string>
    </property>
+   <property name="shortcut">
+    <string>Meta+O</string>
+   </property>
   </action>
   <action name="actionEngineersView">
    <property name="checkable">
@@ -308,6 +318,9 @@
    <property name="text">
     <string>Engineer</string>
    </property>
+   <property name="shortcut">
+    <string>Meta+E</string>
+   </property>
   </action>
   <action name="actionMavlinkView">
    <property name="checkable">
@@ -320,6 +333,9 @@
    <property name="text">
     <string>Mavlink</string>
    </property>
+   <property name="shortcut">
+    <string>Meta+M</string>
+   </property>
   </action>
   <action name="actionReloadStyle">
    <property name="icon">
@@ -341,6 +357,9 @@
    <property name="text">
     <string>Pilot</string>
    </property>
+   <property name="shortcut">
+    <string>Meta+P</string>
+   </property>
   </action>
   <action name="actionNewCustomWidget">
    <property name="icon">
@@ -379,6 +398,21 @@
     <string>QGroundControl global settings</string>
    </property>
   </action>
+  <action name="actionUnconnectedView">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="icon">
+    <iconset resource="../../mavground.qrc">
+     <normaloff>:/images/devices/network-wireless.svg</normaloff>:/images/devices/network-wireless.svg</iconset>
+   </property>
+   <property name="text">
+    <string>Unconnected</string>
+   </property>
+   <property name="shortcut">
+    <string>Meta+U</string>
+   </property>
+  </action>
  </widget>
  <layoutdefault spacing="6" margin="11"/>
  <resources>

src/ui/MapWidget.cc

@@ -21,6 +21,7 @@
 #include "UASManager.h"
 #include "MAV2DIcon.h"
 #include "Waypoint2DIcon.h"
+#include "UASWaypointManager.h"
 
 #include "MG.h"
 
@@ -38,7 +39,7 @@ MapWidget::MapWidget(QWidget *parent) :
     mc = new qmapcontrol::MapControl(QSize(320, 240));
 
     //   VISUAL MAP STYLE
-    QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)}");
+    QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }");
     mc->setPen(QGC::colorCyan.darker(400));
 
 
@@ -93,7 +94,7 @@ MapWidget::MapWidget(QWidget *parent) :
 
     // SET INITIAL POSITION AND ZOOM
     // Set default zoom level
-    mc->setZoom(16);
+    mc->setZoom(17);
     // Zurich, ETH
     mc->setView(QPointF(8.548056,47.376389));
 
@@ -371,7 +372,7 @@ void MapWidget::createPathButtonClicked(bool checked)
 
 
         // emit signal start to create a Waypoint global
-        emit createGlobalWP(true, mc->currentCoordinate());
+        //emit createGlobalWP(true, mc->currentCoordinate());
 
         //        // Clear the previous WP track
         //        // TODO: Move this to an actual clear track button and add a warning dialog
@@ -400,50 +401,104 @@ void MapWidget::createPathButtonClicked(bool checked)
 
 void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate)
 {
-    //qDebug() << mc->mouseMode();
     if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked())
     {
         // Create waypoint name
         QString str;
 
-        str = QString("%1").arg(waypointPath->numberOfPoints());
-
         // create the WP and set everything in the LineString to display the path
         Waypoint2DIcon* tempCirclePoint;
 
         if (mav)
         {
-            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, new QPen(mav->getColor()));
+            mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y()));
         }
         else
         {
+            str = QString("%1").arg(waypointPath->numberOfPoints());
             tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
-        }
-        mc->layer("Waypoints")->addGeometry(tempCirclePoint);
 
-        qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str);
-        wps.append(tempPoint);
-        waypointPath->addPoint(tempPoint);
+            mc->layer("Waypoints")->addGeometry(tempCirclePoint);
 
-        wpIndex.insert(str,tempPoint);
+            qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str);
+            wps.append(tempPoint);
+            waypointPath->addPoint(tempPoint);
 
-        // Refresh the screen
-        mc->updateRequest(tempPoint->boundingBox().toRect());
+            // Refresh the screen
+            mc->updateRequest(tempPoint->boundingBox().toRect());
+        }
 
         // emit signal mouse was clicked
-        emit captureMapCoordinateClick(coordinate);
+        //emit captureMapCoordinateClick(coordinate);
     }
 }
 
-void MapWidget::createWaypointGraphAtMap(const QPointF coordinate)
+void MapWidget::updateWaypoint(int uas, Waypoint* wp)
 {
-    if (!wpExists(coordinate))
+    //qDebug() << "UPDATING WP" << wp->getId() << __FILE__ << __LINE__;
+    if (uas == this->mav->getUASID())
     {
+        int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp);
         // Create waypoint name
-        QString str;
+        QString str = QString("%1").arg(wpindex);
+        // Check if wp exists yet
+        if (!(wps.count() > wpindex))
+        {
+            QPointF coordinate;
+            coordinate.setX(wp->getX());
+            coordinate.setY(wp->getY());
+            createWaypointGraphAtMap(wpindex, coordinate);
 
+            qDebug() << "Waypoint Index did not contain" << str;
+        }
+        else
+        {
+            // Waypoint exists, update it
+            if(!waypointIsDrag)
+            {
+                qDebug() <<"indice WP= "<< wpindex <<"\n";
+
+                QPointF coordinate;
+                coordinate.setX(wp->getX());
+                coordinate.setY(wp->getY());
+
+                Point* waypoint;
+                waypoint = wps.at(wpindex);//wpIndex[str];
+                if (waypoint)
+                {
+                    // First set waypoint coordinate
+                    waypoint->setCoordinate(coordinate);
+                    // Now update icon position
+                    //mc->layer("Waypoints")->removeGeometry(wpIcons.at(wpindex));
+                    wpIcons.at(wpindex)->setCoordinate(coordinate);
+                    //mc->layer("Waypoints")->addGeometry(wpIcons.at(wpindex));
+                    // Then waypoint line coordinate
+                    Point* linesegment = NULL;
+                    if (waypointPath->points().size() > wpindex)
+                    {
+                        linesegment = waypointPath->points().at(wpindex);
+                    }
+
+                    if (linesegment)
+                    {
+                        linesegment->setCoordinate(coordinate);
+                    }
+
+                    //point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(wpindex));
+                    //point2Find->setCoordinate(coordinate);
+                    mc->updateRequest(waypoint->boundingBox().toRect());
+                }
+            }
+        }
+    }
+}
 
-        str = QString("%1").arg(waypointPath->numberOfPoints());
+void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate)
+{
+    if (!wpExists(coordinate))
+    {
+        // Create waypoint name
+        QString str;
 
         // create the WP and set everything in the LineString to display the path
         //CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str);
@@ -451,35 +506,42 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate)
 
         if (mav)
         {
-            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, new QPen(mav->getColor()));
+            int uas = mav->getUASID();
+            str = QString("%1").arg(id);
+            qDebug() << "Waypoint list count:" << str;
+            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, mavPens.value(uas));
         }
         else
         {
+            str = QString("%1").arg(id);
             tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
         }
 
 
         mc->layer("Waypoints")->addGeometry(tempCirclePoint);
+        wpIcons.append(tempCirclePoint);
 
         Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str);
         wps.append(tempPoint);
         waypointPath->addPoint(tempPoint);
 
-        wpIndex.insert(str,tempPoint);
+        //wpIndex.insert(str,tempPoint);
         qDebug()<<"Funcion createWaypointGraphAtMap WP= "<<str<<" -> x= "<<tempPoint->latitude()<<" y= "<<tempPoint->longitude();
 
         // Refresh the screen
-        mc->updateRequestNew();//(tempPoint->boundingBox().toRect());
+        mc->updateRequest(tempPoint->boundingBox().toRect());
     }
 
     ////    // emit signal mouse was clicked
     //    emit captureMapCoordinateClick(coordinate);
 }
 
-int MapWidget::wpExists(const QPointF coordinate){
+int MapWidget::wpExists(const QPointF coordinate)
+{
     for (int i = 0; i < wps.size(); i++){
         if (wps.at(i)->latitude() == coordinate.y() &&
-            wps.at(i)->longitude()== coordinate.x()){
+            wps.at(i)->longitude()== coordinate.x())
+        {
             return 1;
         }
     }
@@ -493,21 +555,23 @@ void MapWidget::captureGeometryClick(Geometry* geom, QPoint point)
     Q_UNUSED(point);
 
     mc->setMouseMode(qmapcontrol::MapControl::None);
-
 }
 
 void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
 {
-
-
     waypointIsDrag = true;
 
     // Refresh the screen
     mc->updateRequest(geom->boundingBox().toRect());
 
     int temp = 0;
+
+    // Get waypoint index in list
+    bool wpIndexOk;
+    int index = geom->name().toInt(&wpIndexOk);
+
     qmapcontrol::Point* point2Find;
-    point2Find = wpIndex[geom->name()];
+    point2Find = wps.at(geom->name().toInt());
 
     if (point2Find)
     {
@@ -518,6 +582,21 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
         {
             point2Find->setCoordinate(coordinate);
 
+            if (wpIndexOk)
+            {
+                mc->updateRequest(point2Find->boundingBox().toRect());
+                if (mav)
+                {
+                        QVector<Waypoint*> wps = mav->getWaypointManager()->getWaypointList();
+
+                    if (wps.size() > index)
+                    {
+                        wps.at(index)->setX(coordinate.x());
+                        wps.at(index)->setY(coordinate.y());
+                        mav->getWaypointManager()->notifyOfChange(wps.at(index));
+                    }
+                }
+            }
             // qDebug() << geom->name();
             temp = geom->get_myIndex();
             //qDebug() << temp;
@@ -553,18 +632,102 @@ MapWidget::~MapWidget()
 void MapWidget::addUAS(UASInterface* uas)
 {
     connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
+    //connect(uas->getWaypointManager(), SIGNAL(waypointListChanged()), this, SLOT(redoWaypoints()));
+}
+
+void MapWidget::updateWaypointList(int uas)
+{
+    // Get already existing waypoints
+    UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
+    if (uasInstance)
+    {
+        QVector<Waypoint*> wpList = uasInstance->getWaypointManager()->getWaypointList();
+
+        // Clear if necessary
+        if (wpList.count() == 0)
+        {
+            clearWaypoints(uas);
+            return;
+        }
+
+        // Load all existing waypoints into map view
+        foreach (Waypoint* wp, wpList)
+        {
+            updateWaypoint(mav->getUASID(), wp);
+        }
+
+        // Delete now unused wps
+        if (wps.count() > wpList.count())
+        {
+            mc->layer("Waypoints")->removeGeometry(waypointPath);
+            for (int i = wpList.count(); i < wps.count(); ++i)
+            {
+                QRect updateRect = wps.at(i)->boundingBox().toRect();
+                wps.removeAt(i);
+                mc->layer("Waypoints")->removeGeometry(wpIcons.at(i));
+                waypointPath->points().removeAt(i);
+                //Point* linesegment = waypointPath->points().at(mav->getWaypointManager()->getWaypointList().indexOf(wp));
+
+                mc->updateRequest(updateRect);
+            }
+            mc->layer("Waypoints")->addGeometry(waypointPath);
+        }
+
+        // Clear and rebuild linestring
+
+    }
+}
+
+void MapWidget::redoWaypoints(int uas)
+{
+    //    QObject* sender = QObject::sender();
+    //    UASWaypointManager* manager = dynamic_cast<UASWaypointManager*>(sender);
+    //    if (sender)
+    //    {
+    // Get waypoint list for this MAV
+
+    // Clear all waypoints
+    clearWaypoints();
+    // Re-add the updated waypoints
+
+    //    }
+
+    updateWaypointList(uas);
 }
 
 void MapWidget::activeUASSet(UASInterface* uas)
 {
+    // Disconnect old MAV
+    if (mav)
+    {
+        // clear path create on the map
+        disconnect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
+        // add Waypoint widget in the WaypointList widget when mouse clicked
+        disconnect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
+
+
+        // it notifies that a waypoint global goes to do create and a map graphic too
+        //connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF)));
+    }
+
     if (uas)
     {
         mav = uas;
-        QColor color = mav->getColor();
+        QColor color = mav->getColor().lighter(100);
         color.setAlphaF(0.6);
         QPen* pen = new QPen(color);
-        pen->setWidth(3.0);
-        // FIXME Load waypoints of this system
+        pen->setWidth(2.0);
+        mavPens.insert(mav->getUASID(), pen);
+        // FIXME Remove after refactoring
+        waypointPath->setPen(pen);
+
+        // Delete all waypoints and start fresh
+        redoWaypoints();
+
+        // clear path create on the map
+        connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
+        // add Waypoint widget in the WaypointList widget when mouse clicked
+        connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
     }
 }
 
@@ -589,7 +752,10 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
     //pointpen->setWidth(3);
     //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen));
 
-    MAV2DIcon* p;
+    qmapcontrol::Point* p;
+    QPointF coordinate;
+    coordinate.setX(lat);
+    coordinate.setY(lon);
 
     if (!uasIcons.contains(uas->getUASID()))
     {
@@ -597,40 +763,42 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
         QColor uasColor = uas->getColor();
 
         // Icon
-        QPen* pointpen = new QPen(uasColor);
-        qDebug() << uas->getUASName();
-        p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, pointpen);
+        //QPen* pointpen = new QPen(uasColor);
+        qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__;
+        //p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, mavPens.value(uas->getUASID()));
+        p = new Waypoint2DIcon(lat, lon, 20, QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
         uasIcons.insert(uas->getUASID(), p);
-        tracks->addGeometry(p);
+        mc->layer("Waypoints")->addGeometry(p);
 
         // Line
         // A QPen also can use transparency
 
         QList<qmapcontrol::Point*> points;
-        points.append(new qmapcontrol::Point(lat, lon, ""));
+        points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
         QPen* linepen = new QPen(uasColor.darker());
         linepen->setWidth(2);
 
         // Create tracking line string
-        qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, uas->getUASName(), linepen);
+        qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen);
         uasTrails.insert(uas->getUASID(), ls);
 
         // Add the LineString to the layer
-        mc->layer("Tracking")->addGeometry(ls);
+        mc->layer("Waypoints")->addGeometry(ls);
     }
     else
     {
-        p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
-        if (p)
-        {
+//        p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
+//        if (p)
+//        {
+         p = uasIcons.value(uas->getUASID());
             p->setCoordinate(QPointF(lat, lon));
-            p->setYaw(uas->getYaw());
-        }
+            //p->setYaw(uas->getYaw());
+//        }
         // Extend trail
-        uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, ""));
+        uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
     }
 
-    mc->updateRequest(p->boundingBox().toRect());
+mc->updateRequest(p->boundingBox().toRect());
 
     //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
 
@@ -738,24 +906,28 @@ void MapWidget::changeEvent(QEvent *e)
     }
 }
 
-void MapWidget::clearWaypoints()
+void MapWidget::clearWaypoints(int uas)
 {
     // Clear the previous WP track
 
-    mc->layer("Waypoints")->clearGeometries();
+    //mc->layer("Waypoints")->clearGeometries();
     wps.clear();
-    waypointPath->setPoints(wps);
+    waypointPath->points().clear();
+    //delete waypointPath;
+    //waypointPath = new
     mc->layer("Waypoints")->addGeometry(waypointPath);
-    wpIndex.clear();
+    //wpIndex.clear();
     mc->updateRequestNew();//(waypointPath->boundingBox().toRect());
 
     if(createPath->isChecked())
     {
         createPath->click();
     }
+
+    qDebug() << "CLEARING WAYPOINTS";
 }
 
-void MapWidget::clearPath()
+void MapWidget::clearPath(int uas)
 {
     mc->layer("Tracking")->clearGeometries();
     foreach (qmapcontrol::LineString* ls, uasTrails)
@@ -769,30 +941,6 @@ void MapWidget::clearPath()
     mc->updateRequestNew();//(QRect(0, 0, width(), height()));
 }
 
-void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon)
-{
-    if(!waypointIsDrag)
-    {
-        qDebug() <<"indice WP= "<<index <<"\n";
-
-        QPointF coordinate;
-        coordinate.setX(lon);
-        coordinate.setY(lat);
-
-        Point* point2Find;
-        point2Find = wpIndex[QString::number(index)];
-        point2Find->setCoordinate(coordinate);
-
-        point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(index));
-        point2Find->setCoordinate(coordinate);
-
-        // Refresh the screen
-        mc->updateRequestNew();//(point2Find->boundingBox().toRect());
-    }
-
-
-}
-
 void MapWidget::updateCameraPosition(double radio, double bearing, QString dir)
 {
     // FIXME Mariano

src/ui/MapWidget.h

@@ -44,6 +44,8 @@ This file is part of the QGROUNDCONTROL project
 
 
 class QMenu;
+class Waypoint;
+class Waypoint2DIcon;
 
 namespace Ui {
     class MapWidget;
@@ -72,10 +74,18 @@ public slots:
     QPointF getPointxBearing_Range(double lat1, double lon1, double bearing, double distance);
 
     /** @brief Clear the waypoints overlay layer */
-    void clearWaypoints();
+    void clearWaypoints(int uas=0);
     /** @brief Clear the UAV tracks on the map */
-    void clearPath();
-    void changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon);
+    void clearPath(int uas=0);
+
+    /** @brief Update waypoint list */
+    void updateWaypointList(int uas);
+    /** @brief Clear all waypoints and re-add them from updated list */
+    void redoWaypoints(int uas=0);
+
+    /** @brief Update waypoint */
+    void updateWaypoint(int uas, Waypoint* wp);
+
     void drawBorderCamAtMap(bool status);
     /** @brief Bring up dialog to go to a specific location */
     void goTo();
@@ -105,12 +115,12 @@ protected:
     QMenu* mapMenu;
     QPushButton* mapButton;
 
-    qmapcontrol::MapControl* mc;                   ///< QMapControl widget
-    qmapcontrol::MapAdapter* mapadapter;           ///< Adapter to load the map data
-    qmapcontrol::Layer* l;            ///< Current map layer (background)
-    qmapcontrol::Layer* overlay;      ///< Street overlay (foreground)
-    qmapcontrol::Layer* tracks;      ///< Layer for UAV tracks
-    qmapcontrol::GeometryLayer* geomLayer; ///< Layer for waypoints
+    qmapcontrol::MapControl* mc;            ///< QMapControl widget
+    qmapcontrol::MapAdapter* mapadapter;    ///< Adapter to load the map data
+    qmapcontrol::Layer* l;                  ///< Current map layer (background)
+    qmapcontrol::Layer* overlay;            ///< Street overlay (foreground)
+    qmapcontrol::Layer* tracks;             ///< Layer for UAV tracks
+    qmapcontrol::GeometryLayer* geomLayer;  ///< Layer for waypoints
 
     //only for experiment
     qmapcontrol::GeometryLayer* camLayer; ///< Layer for camera indicator
@@ -124,6 +134,9 @@ protected:
 
     QMap<int, qmapcontrol::Point*> uasIcons;
     QMap<int, qmapcontrol::LineString*> uasTrails;
+    QMap<int, QPen*> mavPens;
+    //QMap<int, QList<qmapcontrol::Point*> > mavWps;
+    //QMap<int, qmapcontrol::LineString*> waypointPaths;
     UASInterface* mav;
     quint64 lastUpdate;
 
@@ -135,21 +148,24 @@ protected:
 
     void createPathButtonClicked(bool checked);
 
-    void createWaypointGraphAtMap (const QPointF coordinate);
+    /** @brief Create the graphic representation of the waypoint */
+    void createWaypointGraphAtMap(int id, const QPointF coordinate);
     void mapproviderSelected(QAction* action);
 
   signals:
     //void movePoint(QPointF newCoord);
-    void captureMapCoordinateClick(const QPointF coordinate); //ROCA
-    void createGlobalWP(bool value, QPointF centerCoordinate);
+    //void captureMapCoordinateClick(const QPointF coordinate); //ROCA
+    //void createGlobalWP(bool value, QPointF centerCoordinate);
+    void waypointCreated(Waypoint* wp);
     void sendGeometryEndDrag(const QPointF coordinate, const int index);
 
 
 private:
     Ui::MapWidget *m_ui;
     QList<qmapcontrol::Point*> wps;
+    QList<Waypoint2DIcon*>wpIcons;
     qmapcontrol::LineString* waypointPath;
-    QHash <QString, qmapcontrol::Point*> wpIndex;
+    //QHash <QString, qmapcontrol::Point*> wpIndex;
     QPen* pointPen;
     int wpExists(const QPointF coordinate);
     bool waypointIsDrag;

src/ui/QGCMAVLinkLogPlayer.cc

@@ -319,7 +319,7 @@ void QGCMAVLinkLogPlayer::logLoop()
 
         //qDebug() << "nextExecutionTime:" << nextExecutionTime << "QGC START TIME:" << currentStartTime << "LOG START TIME:" << startTime;
 
-        if (nextExecutionTime < 5)
+        if (nextExecutionTime < 2)
         {
             logLoop();
         }

src/ui/WaypointList.cc

@@ -127,11 +127,11 @@ void WaypointList::setUAS(UASInterface* uas)
         connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)),  this, SLOT(updatePosition(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)),       this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
 
-        connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),        this, SLOT(updateStatusLabel(const QString &)));
-        connect(&uas->getWaypointManager(), SIGNAL(waypointListChanged(void)),                  this, SLOT(waypointListChanged(void)));
-        connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)),            this, SLOT(currentWaypointChanged(quint16)));
-        connect(&uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP()));
-        connect(&uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool)));
+        connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),        this, SLOT(updateStatusLabel(const QString &)));
+        connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(void)),                  this, SLOT(waypointListChanged(void)));
+        connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)),            this, SLOT(currentWaypointChanged(quint16)));
+        connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP()));
+        connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool)));
 
     }
 }
@@ -141,7 +141,7 @@ void WaypointList::saveWaypoints()
     if (uas)
     {
         QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
-        uas->getWaypointManager().saveWaypoints(fileName);
+        uas->getWaypointManager()->saveWaypoints(fileName);
     }
 }
 
@@ -152,7 +152,7 @@ void WaypointList::loadWaypoints()
 
 
         QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
-        uas->getWaypointManager().loadWaypoints(fileName);
+        uas->getWaypointManager()->loadWaypoints(fileName);
      }
 }
 
@@ -160,7 +160,7 @@ void WaypointList::transmit()
 {
     if (uas)
     {
-        uas->getWaypointManager().writeWaypoints();
+        uas->getWaypointManager()->writeWaypoints();
     }
 }
 
@@ -168,7 +168,7 @@ void WaypointList::read()
 {
     if (uas)
     {
-        uas->getWaypointManager().readWaypoints();
+        uas->getWaypointManager()->readWaypoints();
     }
 }
 
@@ -178,17 +178,17 @@ void WaypointList::add()
     {
 //        if(isGlobalWP)
 //        {
-//            const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+//            const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
 //            if (waypoints.size() > 0)
 //            {
 //                Waypoint *last = waypoints.at(waypoints.size()-1);
 //                Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
-//                uas->getWaypointManager().addWaypoint(wp);
+//                uas->getWaypointManager()->addWaypoint(wp);
 //            }
 //            else
 //            {
 //                Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
-//                uas->getWaypointManager().addWaypoint(wp);
+//                uas->getWaypointManager()->addWaypoint(wp);
 //            }
 //
 //            emit createWaypointAtMap(centerMapCoordinate);
@@ -196,7 +196,7 @@ void WaypointList::add()
 //        else
         {
 
-            const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+            const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
             Waypoint *wp;
 
 
@@ -206,13 +206,13 @@ void WaypointList::add()
                 Waypoint *last = waypoints.at(waypoints.size()-1);
                 wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(),
                                             last->getHoldTime(), last->getFrame(), last->getAction());
-                uas->getWaypointManager().addWaypoint(wp);
+                uas->getWaypointManager()->addWaypoint(wp);
             }
             else
             {
                     // Create global frame waypoint per default
                     wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE);
-                    uas->getWaypointManager().addWaypoint(wp);
+                    uas->getWaypointManager()->addWaypoint(wp);
             }
             if (wp->getFrame() == MAV_FRAME_GLOBAL)
             {
@@ -234,17 +234,17 @@ void WaypointList::addCurrentPositonWaypoint()
         //}
         //else
         {
-            const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+            const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
             if (waypoints.size() > 0)
             {
                 Waypoint *last = waypoints.at(waypoints.size()-1);
                 Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
-                uas->getWaypointManager().addWaypoint(wp);
+                uas->getWaypointManager()->addWaypoint(wp);
             }
             else
             {
                 Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000);
-                uas->getWaypointManager().addWaypoint(wp);
+                uas->getWaypointManager()->addWaypoint(wp);
             }
 
              //isLocalWP = true;
@@ -264,7 +264,7 @@ void WaypointList::changeCurrentWaypoint(quint16 seq)
 {
     if (uas)
     {
-        uas->getWaypointManager().setCurrentWaypoint(seq);
+        uas->getWaypointManager()->setCurrentWaypoint(seq);
     }
 }
 
@@ -272,7 +272,7 @@ void WaypointList::currentWaypointChanged(quint16 seq)
 {
     if (uas)
     {
-        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
 
         if (seq < waypoints.size())
         {
@@ -297,124 +297,125 @@ void WaypointList::waypointListChanged()
 {
     if (uas)
     {
-        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
-
-            // For Global Waypoints
-            //if(isGlobalWP)
-            //{
-                //isLocalWP = false;
-                //// first remove all views of non existing waypoints
-                //if (!wpGlobalViews.empty())
-                //{
-                    //QMapIterator<Waypoint*,WaypointGlobalView*> viewIt(wpGlobalViews);
-                    //viewIt.toFront();
-                    //while(viewIt.hasNext())
-                    //{
-                        //viewIt.next();
-                        //Waypoint *cur = viewIt.key();
-                        //int i;
-                        //for (i = 0; i < waypoints.size(); i++)
-                        //{
-                            //if (waypoints[i] == cur)
-                            //{
-                                //break;
-                            //}
-                        //}
-                        //if (i == waypoints.size())
-                        //{
-                            //WaypointGlobalView* widget = wpGlobalViews.find(cur).value();
-                            //widget->hide();
-                            //listLayout->removeWidget(widget);
-                            //wpGlobalViews.remove(cur);
-                        //}
-                    //}
-                //}
-
-                //// then add/update the views for each waypoint in the list
-                //for(int i = 0; i < waypoints.size(); i++)
-                //{
-                    //Waypoint *wp = waypoints[i];
-                    //if (!wpGlobalViews.contains(wp))
-                    //{
-                        //WaypointGlobalView* wpview = new WaypointGlobalView(wp, this);
-                        //wpGlobalViews.insert(wp, wpview);
-                        //connect(wpview, SIGNAL(removeWaypoint(Waypoint*)),      this, SLOT(removeWaypoint(Waypoint*)));
-                        //connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(changeWPPositionBySpinBox(Waypoint*)));
-////                        connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)),    this, SLOT(moveDown(Waypoint*)));
-////                        connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)),      this, SLOT(moveUp(Waypoint*)));
-                       //// connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(waypointGlobalPositionChanged(Waypoint*)));
-////                        connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
-////                        connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
-                    //}
-                //WaypointGlobalView *wpgv = wpGlobalViews.value(wp);
-                //wpgv->updateValues();
-                //listLayout->addWidget(wpgv);
-            //}
-
-            //}
-            //else
+        // Prevent updates to prevent visual flicker
+        this->setUpdatesEnabled(false);
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
+
+        // For Global Waypoints
+        //if(isGlobalWP)
+        //{
+        //isLocalWP = false;
+        //// first remove all views of non existing waypoints
+        //if (!wpGlobalViews.empty())
+        //{
+        //QMapIterator<Waypoint*,WaypointGlobalView*> viewIt(wpGlobalViews);
+        //viewIt.toFront();
+        //while(viewIt.hasNext())
+        //{
+        //viewIt.next();
+        //Waypoint *cur = viewIt.key();
+        //int i;
+        //for (i = 0; i < waypoints.size(); i++)
+        //{
+        //if (waypoints[i] == cur)
+        //{
+        //break;
+        //}
+        //}
+        //if (i == waypoints.size())
+        //{
+        //WaypointGlobalView* widget = wpGlobalViews.find(cur).value();
+        //widget->hide();
+        //listLayout->removeWidget(widget);
+        //wpGlobalViews.remove(cur);
+        //}
+        //}
+        //}
+
+        //// then add/update the views for each waypoint in the list
+        //for(int i = 0; i < waypoints.size(); i++)
+        //{
+        //Waypoint *wp = waypoints[i];
+        //if (!wpGlobalViews.contains(wp))
+        //{
+        //WaypointGlobalView* wpview = new WaypointGlobalView(wp, this);
+        //wpGlobalViews.insert(wp, wpview);
+        //connect(wpview, SIGNAL(removeWaypoint(Waypoint*)),      this, SLOT(removeWaypoint(Waypoint*)));
+        //connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(changeWPPositionBySpinBox(Waypoint*)));
+        ////                        connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)),    this, SLOT(moveDown(Waypoint*)));
+        ////                        connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)),      this, SLOT(moveUp(Waypoint*)));
+        //// connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(waypointGlobalPositionChanged(Waypoint*)));
+        ////                        connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
+        ////                        connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
+        //}
+        //WaypointGlobalView *wpgv = wpGlobalViews.value(wp);
+        //wpgv->updateValues();
+        //listLayout->addWidget(wpgv);
+        //}
+
+        //}
+        //else
+        //{
+            // for local Waypoints
+            // first remove all views of non existing waypoints
+            if (!wpViews.empty())
             {
-                // for local Waypoints
-                // first remove all views of non existing waypoints
-                if (!wpViews.empty())
+                QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews);
+                viewIt.toFront();
+                while(viewIt.hasNext())
                 {
-                    QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews);
-                    viewIt.toFront();
-                    while(viewIt.hasNext())
+                    viewIt.next();
+                    Waypoint *cur = viewIt.key();
+                    int i;
+                    for (i = 0; i < waypoints.size(); i++)
                     {
-                        viewIt.next();
-                        Waypoint *cur = viewIt.key();
-                        int i;
-                        for (i = 0; i < waypoints.size(); i++)
+                        if (waypoints[i] == cur)
                         {
-                            if (waypoints[i] == cur)
-                            {
-                                break;
-                            }
-                        }
-                        if (i == waypoints.size())
-                        {
-                            WaypointView* widget = wpViews.find(cur).value();
-                            widget->hide();
-                            listLayout->removeWidget(widget);
-                            wpViews.remove(cur);
+                            break;
                         }
                     }
+                    if (i == waypoints.size())
+                    {
+                        WaypointView* widget = wpViews.find(cur).value();
+                        widget->hide();
+                        listLayout->removeWidget(widget);
+                        wpViews.remove(cur);
+                    }
                 }
+            }
 
-                // then add/update the views for each waypoint in the list
-                for(int i = 0; i < waypoints.size(); i++)
+            // then add/update the views for each waypoint in the list
+            for(int i = 0; i < waypoints.size(); i++)
+            {
+                Waypoint *wp = waypoints[i];
+                if (!wpViews.contains(wp))
                 {
-                    Waypoint *wp = waypoints[i];
-                    if (!wpViews.contains(wp))
-                    {
-                        WaypointView* wpview = new WaypointView(wp, this);
-                        wpViews.insert(wp, wpview);
-                        connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)),    this, SLOT(moveDown(Waypoint*)));
-                        connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)),      this, SLOT(moveUp(Waypoint*)));
-                        connect(wpview, SIGNAL(removeWaypoint(Waypoint*)),      this, SLOT(removeWaypoint(Waypoint*)));
-                        connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
-                        connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
-                    }
+                    WaypointView* wpview = new WaypointView(wp, this);
+                    wpViews.insert(wp, wpview);
+                    connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)),    this, SLOT(moveDown(Waypoint*)));
+                    connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)),      this, SLOT(moveUp(Waypoint*)));
+                    connect(wpview, SIGNAL(removeWaypoint(Waypoint*)),      this, SLOT(removeWaypoint(Waypoint*)));
+                    connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
+                    connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
+                }
                 WaypointView *wpv = wpViews.value(wp);
                 wpv->updateValues();    // update the values of the ui elements in the view
                 listLayout->addWidget(wpv);
 
             }
-            }
-
-
-            loadFileGlobalWP = false;
+                    this->setUpdatesEnabled(true);
         }
 
 
+        loadFileGlobalWP = false;
+    //}
 }
 
 void WaypointList::moveUp(Waypoint* wp)
 {
     if (uas)
     {
-        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
 
         //get the current position of wp in the local storage
         int i;
@@ -427,7 +428,7 @@ void WaypointList::moveUp(Waypoint* wp)
         // if wp was found and its not the first entry, move it
         if (i < waypoints.size() && i > 0)
         {
-            uas->getWaypointManager().moveWaypoint(i, i-1);
+            uas->getWaypointManager()->moveWaypoint(i, i-1);
         }
     }
 }
@@ -436,7 +437,7 @@ void WaypointList::moveDown(Waypoint* wp)
 {
     if (uas)
     {
-        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
 
         //get the current position of wp in the local storage
         int i;
@@ -449,7 +450,7 @@ void WaypointList::moveDown(Waypoint* wp)
         // if wp was found and its not the last entry, move it
         if (i < waypoints.size()-1)
         {
-            uas->getWaypointManager().moveWaypoint(i, i+1);
+            uas->getWaypointManager()->moveWaypoint(i, i+1);
         }
     }
 }
@@ -458,7 +459,7 @@ void WaypointList::removeWaypoint(Waypoint* wp)
 {
     if (uas)
     {
-        uas->getWaypointManager().removeWaypoint(wp->getId());
+        uas->getWaypointManager()->removeWaypoint(wp->getId());
     }
 }
 
@@ -482,7 +483,7 @@ void WaypointList::on_clearWPListButton_clicked()
     if (uas)
     {
            emit clearPathclicked();
-          const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+          const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
             while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
             {
                 WaypointView* widget = wpViews.find(waypoints[0]).value();
@@ -503,17 +504,17 @@ void WaypointList::addWaypointMouse(QPointF coordinate)
 {
     if (uas)
     {
-        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
         if (waypoints.size() > 0)
         {
             Waypoint *last = waypoints.at(waypoints.size()-1);
             Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
-            uas->getWaypointManager().addWaypoint(wp);
+            uas->getWaypointManager()->addWaypoint(wp);
         }
         else
         {
             Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
-            uas->getWaypointManager().addWaypoint(wp);
+            uas->getWaypointManager()->addWaypoint(wp);
         }
 
 
@@ -527,7 +528,7 @@ void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP)
 {
     if (uas)
     {
-        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
         if (waypoints.size() > 0)
         {
             Waypoint *temp = waypoints.at(indexWP);
@@ -560,7 +561,7 @@ void WaypointList::clearWPWidget()
 {
   if (uas)
   {
-    const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+    const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
       while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
       {
           WaypointView* widget = wpViews.find(waypoints[0]).value();

src/ui/WaypointView.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1389</width>
-    <height>33</height>
+    <height>39</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -375,7 +375,7 @@ QProgressBar::chunk#thrustBar {
          <string>m</string>
         </property>
         <property name="decimals">
-         <number>7</number>
+         <number>2</number>
         </property>
         <property name="minimum">
          <double>-100000.000000000000000</double>

src/ui/linechart/LinechartWidget.cc

@@ -483,7 +483,7 @@ void LinechartWidget::startLogging()
         fileName.append(".csv");
     }
 
-    while (!(fileName.endsWith(".txt") || fileName.endsWith(".csv")) && !abort)
+    while (!(fileName.endsWith(".txt") || fileName.endsWith(".csv")) && !abort && fileName != "")
     {
         QMessageBox msgBox;
         msgBox.setIcon(QMessageBox::Critical);

src/ui/map/MAV2DIcon.cc

@@ -46,50 +46,50 @@ void MAV2DIcon::setYaw(float yaw)
 void MAV2DIcon::drawIcon(QPen* pen)
 {
 
-//    mypixmap = new QPixmap(radius+1, radius+1);
-//    mypixmap->fill(Qt::transparent);
-//    QPainter painter(mypixmap);
+    mypixmap = new QPixmap(radius+1, radius+1);
+    mypixmap->fill(Qt::transparent);
+    QPainter painter(mypixmap);
 
-//    // DRAW WAYPOINT
-//    QPointF p(radius/2, radius/2);
+    // DRAW WAYPOINT
+    QPointF p(radius/2, radius/2);
 
-//    float waypointSize = radius;
-//    QPolygonF poly(4);
-//    // Top point
-//    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
-//    // Right point
-//    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
-//    // Bottom point
-//    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
-//    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
+    float waypointSize = radius;
+    QPolygonF poly(4);
+    // Top point
+    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
+    // Right point
+    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
+    // Bottom point
+    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
+    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
 
-////    // Select color based on if this is the current waypoint
-////    if (list.at(i)->getCurrent())
-////    {
-////        color = QGC::colorCyan;//uas->getColor();
-////        pen.setWidthF(refLineWidthToPen(0.8f));
-////    }
-////    else
-////    {
-////        color = uas->getColor();
-////        pen.setWidthF(refLineWidthToPen(0.4f));
-////    }
-
-//    //pen.setColor(color);
-//    if (pen)
+//    // Select color based on if this is the current waypoint
+//    if (list.at(i)->getCurrent())
 //    {
-//        pen->setWidthF(2);
-//        painter.setPen(*pen);
+//        color = QGC::colorCyan;//uas->getColor();
+//        pen.setWidthF(refLineWidthToPen(0.8f));
 //    }
 //    else
 //    {
-//        QPen pen2(Qt::red);
-//        pen2.setWidth(2);
-//        painter.setPen(pen2);
+//        color = uas->getColor();
+//        pen.setWidthF(refLineWidthToPen(0.4f));
 //    }
-//    painter.setBrush(Qt::NoBrush);
 
-//    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
-//    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
-//    painter.drawPolygon(poly);
+    //pen.setColor(color);
+    if (pen)
+    {
+        pen->setWidthF(2);
+        painter.setPen(*pen);
+    }
+    else
+    {
+        QPen pen2(Qt::red);
+        pen2.setWidth(2);
+        painter.setPen(pen2);
+    }
+    painter.setBrush(Qt::NoBrush);
+
+    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
+    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
+    painter.drawPolygon(poly);
 }

src/ui/map3D/Pixhawk3DWidget.cc

@@ -308,7 +308,7 @@ Pixhawk3DWidget::insertWaypoint(void)
         if (wp)
         {
             wp->setFrame(frame);
-            uas->getWaypointManager().addWaypoint(wp);
+            uas->getWaypointManager()->addWaypoint(wp);
         }
     }
 }
@@ -325,7 +325,7 @@ Pixhawk3DWidget::setWaypoint(void)
     if (uas)
     {
         const QVector<Waypoint *> waypoints =
-                uas->getWaypointManager().getWaypointList();
+                uas->getWaypointManager()->getWaypointList();
         Waypoint* waypoint = waypoints.at(selectedWpIndex);
 
         if (frame == MAV_FRAME_GLOBAL)
@@ -366,7 +366,7 @@ Pixhawk3DWidget::deleteWaypoint(void)
 {
     if (uas)
     {
-        uas->getWaypointManager().removeWaypoint(selectedWpIndex);
+        uas->getWaypointManager()->removeWaypoint(selectedWpIndex);
     }
 }
 
@@ -377,7 +377,7 @@ Pixhawk3DWidget::setWaypointAltitude(void)
     {
         bool ok;
         const QVector<Waypoint *> waypoints =
-                uas->getWaypointManager().getWaypointList();
+                uas->getWaypointManager()->getWaypointList();
         Waypoint* waypoint = waypoints.at(selectedWpIndex);
 
         double altitude = waypoint->getZ();
@@ -409,10 +409,10 @@ Pixhawk3DWidget::clearAllWaypoints(void)
     if (uas)
     {
         const QVector<Waypoint *> waypoints =
-                uas->getWaypointManager().getWaypointList();
+                uas->getWaypointManager()->getWaypointList();
         for (int i = waypoints.size() - 1; i >= 0; --i)
         {
-            uas->getWaypointManager().removeWaypoint(i);
+            uas->getWaypointManager()->removeWaypoint(i);
         }
     }
 }

src/ui/map3D/WaypointGroupNode.cc

@@ -78,7 +78,7 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
             removeChild(0, getNumChildren());
         }
 
-        const QVector<Waypoint *>& list = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointList();
 
         for (int i = 0; i < list.size(); i++)
         {

src/ui/uas/UASView.cc

@@ -83,7 +83,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
     connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double)));
     connect(uas, SIGNAL(heartbeatTimeout()), this, SLOT(heartbeatTimeout()));
     connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int)));
-    connect(&(uas->getWaypointManager()), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16)));
+    connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16)));
     connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
     connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool)));