Fixed 2D map regarding waypoints (almost done), added option to replay image logs in HUD view

images/earth.html

@@ -192,20 +192,20 @@ 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
+    // Create a style and set width and color of line
-trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
+    trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
-lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
+    lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
-lineStyle.setWidth(5);
+    lineStyle.setWidth(5);
-lineStyle.getColor().set(trailColors[id]);  // aabbggrr format
+    lineStyle.getColor().set(trailColors[id]);  // aabbggrr format
-//lineStyle.getColor().set(color);  // aabbggrr format
+    //lineStyle.getColor().set(color);  // aabbggrr format
 
-// Add the feature to Earth
+    // Add the feature to Earth
-if (trailsVisible[id] == true) ge.getFeatures().replaceChild(trailPlacemarks[id], trailPlacemarks[id]);
+    if (trailsVisible[id] == true) ge.getFeatures().replaceChild(trailPlacemarks[id], trailPlacemarks[id]);
 }
 
 function initCallback(object)
@@ -224,8 +224,6 @@ function initCallback(object)
     ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, 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/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,7 @@ void UASWaypointManager::addWaypoint(Waypoint *wp)
         waypoints.insert(waypoints.size(), wp);
 
         emit waypointListChanged();
+        emit waypointListChanged(uas.getUASID());
     }
 }
 
@@ -329,6 +337,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
             waypoints[i]->setId(i);
         }
         emit waypointListChanged();
+        emit waypointListChanged(uas.getUASID());
         return 0;
     }
     return -1;
@@ -359,6 +368,7 @@ void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
         //waypoints[new_seq]->setId(new_seq);
 
         emit waypointListChanged();
+        emit waypointListChanged(uas.getUASID());
     }
 }
 
@@ -425,6 +435,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
 
     emit loadWPFile();
     emit waypointListChanged();
+    emit waypointListChanged(uas.getUASID());
 }
 
 

src/uas/UASWaypointManager.h

@@ -86,11 +86,11 @@ 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
@@ -99,6 +99,8 @@ public:
 
     /*@}*/
 
+    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.
@@ -120,9 +122,12 @@ 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
 

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,36 @@ void HUD::hideEvent(QHideEvent* event)
     refreshTimer->stop();
 }
 
+void HUD::contextMenuEvent (QContextMenuEvent* event)
+{
+    QMenu menu(this);
+    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,8 +275,17 @@ 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)));
+
+        // Try to disconnect the image link
+        UAS* u = dynamic_cast<UAS*>(uas);
+        if (u)
+        {
+            disconnect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
+        }
     }
 
+    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)));
@@ -244,8 +298,16 @@ void HUD::setActiveUAS(UASInterface* uas)
         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;
+    }
 }
 
 //void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec)
@@ -609,40 +671,40 @@ 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 (videoEnabled)
-        if (QFileInfo(imagePath).exists())
         {
-            //qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
+            if (nextOfflineImage != "" && QFileInfo(nextOfflineImage).exists())
-            QImage fill = QImage(imagePath);
+            {
+                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;
+                glImage = QGLWidget::convertToGLFormat(fill);
-            float yFactor;
 
-            xFactor = width() / (float)fill.width();
+                // Reset to save load efforts
-            yFactor = height() / (float)fill.height();
+                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);
 
@@ -775,8 +837,17 @@ void HUD::paintHUD()
 
             paintPitchLines(pitchLP, &painter);
             painter.end();
+        }
+        else
+        {
+            QPainter painter;
+            painter.begin(this);
+            painter.end();
+        }
         //glDisable(GL_MULTISAMPLE);
 
+
+
         //glFlush();
     }
 }
@@ -1517,6 +1588,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

@@ -408,17 +408,17 @@ void MainWindow::buildPxWidgets()
 #ifdef QGC_OSG_ENABLED
     if (!_3DWidget)
     {
-//        _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
+        _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
-//        addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
+        addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
     }
 #endif
 
 #ifdef QGC_OSGEARTH_ENABLED
-//    if (!_3DMapWidget)
+    if (!_3DMapWidget)
-//    {
+    {
-//        _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
+        _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
-//        addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
+        addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
-//    }
+    }
 #endif
 
 #if (defined _MSC_VER) | (defined Q_OS_MAC)
@@ -853,13 +853,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

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));
 
 
@@ -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
@@ -406,32 +407,92 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina
         // 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()));
+            //            QVector<Waypoint*> mavWps = mav->getWaypointManager()->getWaypointList();
+            //            str = QString("%1").arg(mavWps.count());
+
+            //            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, new QPen(mav->getColor()));
+            //            //mav->getWaypointManager()->addWaypoint(new Waypoint(mavWps.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);
+            //        if (mav)
+            //        {
+            //            QPen* pathPen = new QPen(mav->getColor());
+            //            pathPen->setWidth(2);
+            //            waypointPath->setPen(pathPen);
+            //        }
 
             wpIndex.insert(str,tempPoint);
 
             // Refresh the screen
             mc->updateRequest(tempPoint->boundingBox().toRect());
+        }
 
         // emit signal mouse was clicked
-        emit captureMapCoordinateClick(coordinate);
+        //emit captureMapCoordinateClick(coordinate);
+    }
+}
+
+void MapWidget::updateWaypoint(int uas, Waypoint* wp)
+{
+    if (uas == this->mav->getUASID())
+    {
+        // Create waypoint name
+        QString str = QString("%1").arg(wp->getId());
+        // Check if wp exists yet
+        if (!wpIndex.contains(str))
+        {
+            QPointF coordinate;
+            coordinate.setX(wp->getX());
+            coordinate.setY(wp->getY());
+            createWaypointGraphAtMap(coordinate);
+            qDebug() << "Waypoint Index did not contain" << str;
+        }
+        else
+        {
+            // Waypoint exists, update it
+            if(!waypointIsDrag)
+            {
+                qDebug() <<"indice WP= "<< wp->getId() <<"\n";
+
+                QPointF coordinate;
+                coordinate.setX(wp->getX());
+                coordinate.setY(wp->getY());
+
+                Point* waypoint;
+                waypoint = wpIndex[str];
+                if (waypoint)
+                {
+                    // First set waypoint coordinate
+                    waypoint->setCoordinate(coordinate);
+                    // Then waypoint line coordinate
+                    Point* linesegment = waypointPath->points().at(wp->getId());
+
+                    if (linesegment)
+                    {
+                        linesegment->setCoordinate(coordinate);
+                    }
+
+                    //point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(wp->getId()));
+                    //point2Find->setCoordinate(coordinate);
+                    mc->updateRequest(waypoint->boundingBox().toRect());
+                }
+            }
+        }
     }
 }
 
@@ -443,7 +504,7 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate)
         QString str;
 
 
-        str = QString("%1").arg(waypointPath->numberOfPoints());
+
 
         // create the WP and set everything in the LineString to display the path
         //CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str);
@@ -451,10 +512,14 @@ 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(mav->getWaypointManager()->getWaypointList().count());
+            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(waypointPath->numberOfPoints());
             tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
         }
 
@@ -469,17 +534,19 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate)
         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,19 +560,21 @@ 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()];
 
@@ -518,6 +587,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 +637,95 @@ 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 (wpIndex.count() > wpList.count())
+        {
+            for (int i = wpList.count(); i < wpIndex.count(); ++i)
+            {
+                wpIndex.remove(QString("%i").arg(i));
+                QRect updateRect = wps.at(i)->boundingBox().toRect();
+                wps.removeAt(i);
+                waypointPath->points().removeAt(i);
+                mc->updateRequest(updateRect);
+            }
+        }
+    }
+}
+
+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);
+        pen->setWidth(2.0);
-        // FIXME Load waypoints of this system
+        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*)));
     }
 }
 
@@ -738,13 +899,13 @@ void MapWidget::changeEvent(QEvent *e)
     }
 }
 
-void MapWidget::clearWaypoints()
+void MapWidget::clearWaypoints(int uas)
 {
     // Clear the previous WP track
 
     mc->layer("Waypoints")->clearGeometries();
     wps.clear();
-    waypointPath->setPoints(wps);
+    waypointPath->points().clear();
     mc->layer("Waypoints")->addGeometry(waypointPath);
     wpIndex.clear();
     mc->updateRequestNew();//(waypointPath->boundingBox().toRect());
@@ -753,9 +914,11 @@ void MapWidget::clearWaypoints()
     {
         createPath->click();
     }
+
+    qDebug() << "CLEARING WAYPOINTS";
 }
 
-void MapWidget::clearPath()
+void MapWidget::clearPath(int uas)
 {
     mc->layer("Tracking")->clearGeometries();
     foreach (qmapcontrol::LineString* ls, uasTrails)
@@ -769,30 +932,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,7 @@ This file is part of the QGROUNDCONTROL project
 
 
 class QMenu;
+class Waypoint;
 
 namespace Ui {
     class MapWidget;
@@ -72,10 +73,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 clearPath(int uas=0);
-    void changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon);
+
+    /** @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();
@@ -124,6 +133,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,13 +147,14 @@ protected:
 
     void createPathButtonClicked(bool checked);
 
-    void createWaypointGraphAtMap (const QPointF coordinate);
+    void createWaypointGraphAtMap(const QPointF coordinate);
     void mapproviderSelected(QAction* action);
 
   signals:
     //void movePoint(QPointF newCoord);
-    void captureMapCoordinateClick(const QPointF coordinate); //ROCA
+    //void captureMapCoordinateClick(const QPointF coordinate); //ROCA
-    void createGlobalWP(bool value, QPointF centerCoordinate);
+    //void createGlobalWP(bool value, QPointF centerCoordinate);
+    void waypointCreated(Waypoint* wp);
     void sendGeometryEndDrag(const QPointF coordinate, const int index);
 
 

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/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);