Checkpoint: Cleaned up a lot withint the WP management infrastructure

src/uas/UASWaypointManager.cc

@@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project
 #include "UASWaypointManager.h"
 #include "UAS.h"
 #include "mavlink_types.h"
+#include "UASManager.h"
 
 #define PROTOCOL_TIMEOUT_MS 2000    ///< maximum time to wait for pending messages until timeout
 #define PROTOCOL_DELAY_MS 20        ///< minimum delay between sent messages
@@ -136,7 +137,7 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
 
         //Clear the old edit-list before receiving the new one
         if (read_to_edit == true){
-            while(waypointsEditable.size()>0) {
+            while(waypointsEditable.count()>0) {
                 Waypoint *t = waypointsEditable[0];
                 waypointsEditable.remove(0);
                 delete t;
@@ -331,10 +332,10 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq)
 
 int UASWaypointManager::setCurrentEditable(quint16 seq)
 {
-    if (seq < waypointsEditable.size()) {
+    if (seq < waypointsEditable.count()) {
         if(current_state == WP_IDLE) {
             //update local main storage
-            for(int i = 0; i < waypointsEditable.size(); i++) {
+            for(int i = 0; i < waypointsEditable.count(); i++) {
                 if (waypointsEditable[i]->getId() == seq) {
                     waypointsEditable[i]->setCurrent(true);
                 } else {
@@ -370,13 +371,13 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi
 {
     if (wp)
     {
-        wp->setId(waypointsEditable.size());
-        if (enforceFirstActive && waypointsEditable.size() == 0)
+        wp->setId(waypointsEditable.count());
+        if (enforceFirstActive && waypointsEditable.count() == 0)
         {
             wp->setCurrent(true);
             currentWaypointEditable = wp;
         }
-        waypointsEditable.insert(waypointsEditable.size(), wp);
+        waypointsEditable.insert(waypointsEditable.count(), wp);
         connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
 
         emit waypointEditableListChanged();
@@ -390,13 +391,15 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi
 Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive)
 {
     Waypoint* wp = new Waypoint();
-    wp->setId(waypointsEditable.size());
-    if (enforceFirstActive && waypointsEditable.size() == 0)
+    wp->setId(waypointsEditable.count());
+    wp->setFrame((MAV_FRAME)getFrameRecommendation());
+    wp->setAltitude(getAltitudeRecommendation());
+    if (enforceFirstActive && waypointsEditable.count() == 0)
     {
         wp->setCurrent(true);
         currentWaypointEditable = wp;
     }
-    waypointsEditable.insert(waypointsEditable.size(), wp);
+    waypointsEditable.append(wp);
     connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
 
     emit waypointEditableListChanged();
@@ -406,17 +409,17 @@ Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive)
 
 int UASWaypointManager::removeWaypoint(quint16 seq)
 {
-    if (seq < waypointsEditable.size())
+    if (seq < waypointsEditable.count())
     {
         Waypoint *t = waypointsEditable[seq];
 
         if (t->getCurrent() == true) //trying to remove the current waypoint
         {
-            if (seq+1 < waypointsEditable.size()) // setting the next waypoint as current
+            if (seq+1 < waypointsEditable.count()) // setting the next waypoint as current
             {
                 waypointsEditable[seq+1]->setCurrent(true);
             }
-            else if (seq-1 >= 0) //if deleting the last on the list, then setting the previous waypoint as current
+            else if (seq-1 >= 0) // if deleting the last on the list, then setting the previous waypoint as current
             {
                 waypointsEditable[seq-1]->setCurrent(true);
             }
@@ -426,7 +429,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
         delete t;
         t = NULL;
 
-        for(int i = seq; i < waypointsEditable.size(); i++)
+        for(int i = seq; i < waypointsEditable.count(); i++)
         {
             waypointsEditable[i]->setId(i);
         }
@@ -440,7 +443,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
 
 void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
 {
-    if (cur_seq != new_seq && cur_seq < waypointsEditable.size() && new_seq < waypointsEditable.size())
+    if (cur_seq != new_seq && cur_seq < waypointsEditable.count() && new_seq < waypointsEditable.count())
     {
         Waypoint *t = waypointsEditable[cur_seq];
         if (cur_seq < new_seq) {
@@ -477,7 +480,7 @@ void UASWaypointManager::saveWaypoints(const QString &saveFile)
     //write the waypoint list version to the first line for compatibility check
     out << "QGC WPL 120\r\n";
 
-    for (int i = 0; i < waypointsEditable.size(); i++)
+    for (int i = 0; i < waypointsEditable.count(); i++)
     {
         waypointsEditable[i]->setId(i);
         waypointsEditable[i]->save(out);
@@ -491,7 +494,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
     if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
         return;
 
-    while(waypointsEditable.size()>0) {
+    while(waypointsEditable.count()>0) {
         Waypoint *t = waypointsEditable[0];
         waypointsEditable.remove(0);
         delete t;
@@ -512,8 +515,8 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
             Waypoint *t = new Waypoint();
             if(t->load(in))
             {
-                t->setId(waypointsEditable.size());
-                waypointsEditable.insert(waypointsEditable.size(), t);
+                t->setId(waypointsEditable.count());
+                waypointsEditable.insert(waypointsEditable.count(), t);
             }
             else
             {
@@ -532,7 +535,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
 
 void UASWaypointManager::clearWaypointList()
 {
-    if(current_state == WP_IDLE)
+    if (current_state == WP_IDLE)
     {
         protocol_timer.start(PROTOCOL_TIMEOUT_MS);
         current_retries = PROTOCOL_MAX_RETRIES;
@@ -779,7 +782,7 @@ void UASWaypointManager::readWaypoints(bool readToEdit)
         /* THIS PART WAS MOVED TO handleWaypointCount. THE EDIT-LIST SHOULD NOT BE CLEARED UNLESS THERE IS A RESPONSE FROM UAV.
         //Clear the old edit-list before receiving the new one
         if (read_to_edit == true){
-            while(waypointsEditable.size()>0) {
+            while(waypointsEditable.count()>0) {
                 Waypoint *t = waypointsEditable[0];
                 waypointsEditable.remove(0);
                 delete t;
@@ -1023,3 +1026,39 @@ void UASWaypointManager::sendWaypointAck(quint8 type)
     uas->sendMessage(message);
     QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
 }
+
+UAS* UASWaypointManager::getUAS() {
+    return this->uas;    ///< Returns the owning UAS
+}
+
+float UASWaypointManager::getAltitudeRecommendation()
+{
+    if (waypointsEditable.count() > 0) {
+        return waypointsEditable.last()->getAltitude();
+    } else {
+        return UASManager::instance()->getHomeAltitude() + getHomeAltitudeOffsetDefault();
+    }
+}
+
+int UASWaypointManager::getFrameRecommendation()
+{
+    if (waypointsEditable.count() > 0) {
+        return static_cast<int>(waypointsEditable.last()->getFrame());
+    } else {
+        return MAV_FRAME_GLOBAL;
+    }
+}
+
+float UASWaypointManager::getAcceptanceRadiusRecommendation()
+{
+    if (waypointsEditable.count() > 0) {
+        return waypointsEditable.last()->getAcceptanceRadius();
+    } else {
+        return 10.0f;
+    }
+}
+
+float UASWaypointManager::getHomeAltitudeOffsetDefault()
+{
+    return defaultAltitudeHomeOffset;
+}

src/uas/UASWaypointManager.h

@@ -112,9 +112,11 @@ public:
     int getLocalFrameCount();   ///< Get the count of local waypoints in the list
     /*@}*/
 
-    UAS* getUAS() {
-        return this->uas;    ///< Returns the owning UAS
-    }
+    UAS* getUAS();
+    float getAltitudeRecommendation();
+    int getFrameRecommendation();
+    float getAcceptanceRadiusRecommendation();
+    float getHomeAltitudeOffsetDefault();
 
 private:
     /** @name Message send functions */
@@ -176,6 +178,9 @@ private:
     QTimer protocol_timer;                          ///< Timer to catch timeouts
     bool standalone;                                ///< If standalone is set, do not write to UAS
     quint16 uasid;
+
+    // XXX export to settings
+    static const float defaultAltitudeHomeOffset   = 30.0f;    ///< Altitude offset in meters from home for new waypoints
 };
 
 #endif // UASWAYPOINTMANAGER_H

src/ui/WaypointList.cc

@@ -199,8 +199,8 @@ void WaypointList::setUAS(UASInterface* uas)
 void WaypointList::saveWaypoints()
 {
 
-        QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
-        WPM->saveWaypoints(fileName);
+    QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
+    WPM->saveWaypoints(fileName);
 
 }
 
@@ -248,117 +248,180 @@ void WaypointList::refresh()
 
 void WaypointList::addEditable()
 {
+    addEditable(false);
+}
 
-        const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
-        Waypoint *wp;
-        if (waypoints.size() > 0)
+void WaypointList::addEditable(bool onCurrentPosition)
+{
+
+    const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
+    Waypoint *wp;
+    if (waypoints.size() > 0 &&
+            !(onCurrentPosition && uas && (uas->localPositionKnown() || uas->globalPositionKnown())))
+    {
+        // Create waypoint with last frame
+        Waypoint *last = waypoints.at(waypoints.size()-1);
+        wp = WPM->createWaypoint();
+        wp->blockSignals(true);
+        MAV_FRAME frame = (MAV_FRAME)last->getFrame();
+        wp->setFrame(frame);
+        if (frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)
         {
-            // Create waypoint with last frame
-            Waypoint *last = waypoints.at(waypoints.size()-1);
-            wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(),
-                              last->getAutoContinue(), false, last->getFrame(), last->getAction());
-            WPM->addWaypointEditable(wp);
+            wp->setLatitude(last->getLatitude());
+            wp->setLongitude(last->getLongitude());
+            wp->setAltitude(last->getAltitude());
+        } else {
+            wp->setX(last->getX());
+            wp->setY(last->getY());
+            wp->setZ(last->getZ());
         }
-        else
-        {            
-            if (uas)
+        wp->setParam1(last->getParam1());
+        wp->setParam1(last->getParam2());
+        wp->setParam1(last->getParam3());
+        wp->setParam1(last->getParam4());
+        wp->setAutocontinue(last->getAutoContinue());
+
+        wp->blockSignals(false);
+        wp->setAction(last->getAction());
+//        WPM->addWaypointEditable(wp);
+    }
+    else
+    {
+        if (uas)
+        {
+            // Create first waypoint at current MAV position
+            if (uas->globalPositionKnown())
             {
-                // Create first waypoint at current MAV position
-                if (uas->localPositionKnown() || uas->globalPositionKnown())
+                if (onCurrentPosition)
                 {
-                	if(addCurrentPositionWaypoint())
-                	{
-                 	   updateStatusLabel(tr("Added default LOCAL (NED) waypoint."));
-                 	  wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
-                 	  WPM->addWaypointEditable(wp);
-                	}
+
+                    if (WPM->getFrameRecommendation() == MAV_FRAME_GLOBAL) {
+                        updateStatusLabel(tr("Added default GLOBAL (Abs alt.) waypoint."));
+                    } else {
+                        updateStatusLabel(tr("Added default GLOBAL (Relative alt.) waypoint."));
+                    }
+                    wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT);
+                    WPM->addWaypointEditable(wp);
+
+                } else {
+
+                    if (WPM->getFrameRecommendation() == MAV_FRAME_GLOBAL) {
+                        updateStatusLabel(tr("Added default GLOBAL (Abs alt.) waypoint."));
+                    } else {
+                        updateStatusLabel(tr("Added default GLOBAL (Relative alt.) waypoint."));
+                    }
+                    wp = new Waypoint(0, UASManager::instance()->getHomeLatitude(),
+                                      UASManager::instance()->getHomeLongitude(),
+                                      WPM->getAltitudeRecommendation(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT);
+                    WPM->addWaypointEditable(wp);
                 }
-                else
+            }
+            else if (uas->localPositionKnown())
+            {
+                if (onCurrentPosition)
                 {
-                    // MAV connected, but position unknown, add default waypoint
-                   updateStatusLabel(tr("WARNING: No position known. Adding default LOCAL (NED) waypoint"));
-                   wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
-                   WPM->addWaypointEditable(wp);
+                    updateStatusLabel(tr("Added default LOCAL (NED) waypoint."));
+                    wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
+                    WPM->addWaypointEditable(wp);
+                } else {
+                    updateStatusLabel(tr("Added default LOCAL (NED) waypoint."));
+                    wp = new Waypoint(0, 0, 0, -0.50, 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
+                    WPM->addWaypointEditable(wp);
                 }
             }
             else
             {
-                //Since no UAV available, create first default waypoint.
-                 updateStatusLabel(tr("No UAV connected. Adding default LOCAL (NED) waypoint"));
-                wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
+                // MAV connected, but position unknown, add default waypoint
+                updateStatusLabel(tr("WARNING: No position known. Adding default LOCAL (NED) waypoint"));
+                wp = new Waypoint(0, UASManager::instance()->getHomeLatitude(),
+                                  UASManager::instance()->getHomeLongitude(),
+                                  WPM->getAltitudeRecommendation(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT);
                 WPM->addWaypointEditable(wp);
-                //create a popup notifying the user about the limitations of offline editing
-                if (showOfflineWarning == true)
-                {
-                    QMessageBox msgBox;
-                    msgBox.setIcon(QMessageBox::Warning);
-                    msgBox.setText("Offline editor!");
-                    msgBox.setInformativeText("You are using the offline mission editor. Please don't forget to save your mission plan before connecting the UAV, otherwise it will be lost.");
-                    msgBox.setStandardButtons(QMessageBox::Ok);
-                    msgBox.setDefaultButton(QMessageBox::Ok);
-                    msgBox.exec();
-                    showOfflineWarning = false;
-                }
             }
         }
-
-}
-
-
-int WaypointList::addCurrentPositionWaypoint()
-{    
-    if (uas)
-    {
-        const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
-        Waypoint *wp;
-        Waypoint *last = 0;
-        if (waypoints.size() > 0)
-        {
-            last = waypoints.at(waypoints.size()-1);
-        }
-
-        if (uas->globalPositionKnown())
+        else
         {
-            float acceptanceRadiusGlobal = 10.0f;
-            float holdTime = 0.0f;
-            float yawGlobal = 0.0f;
-            if (last)
-            {
-                acceptanceRadiusGlobal = last->getAcceptanceRadius();
-                holdTime = last->getHoldTime();
-                yawGlobal = last->getYaw();
-            }
-            // Create global frame waypoint per default
-            wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
+            //Since no UAV available, create first default waypoint.
+            updateStatusLabel(tr("No UAV connected. Adding default GLOBAL (NED) waypoint"));
+            wp = new Waypoint(0, UASManager::instance()->getHomeLatitude(),
+                              UASManager::instance()->getHomeLongitude(),
+                              WPM->getAltitudeRecommendation(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT);
             WPM->addWaypointEditable(wp);
-            updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint"));
-            return 0;
-        }
-        else if (uas->localPositionKnown())
-        {
-            float acceptanceRadiusLocal = 0.2f;
-            float holdTime = 0.5f;
-            if (last)
+            //create a popup notifying the user about the limitations of offline editing
+            if (showOfflineWarning == true)
             {
-                acceptanceRadiusLocal = last->getAcceptanceRadius();
-                holdTime = last->getHoldTime();
+                QMessageBox msgBox;
+                msgBox.setIcon(QMessageBox::Warning);
+                msgBox.setText("Offline editor!");
+                msgBox.setInformativeText("You are using the offline mission editor. Please don't forget to save your mission plan before connecting the UAV, otherwise it will be lost.");
+                msgBox.setStandardButtons(QMessageBox::Ok);
+                msgBox.setDefaultButton(QMessageBox::Ok);
+                msgBox.exec();
+                showOfflineWarning = false;
             }
-            // Create local frame waypoint as second option
-            wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, false, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
-            WPM->addWaypointEditable(wp);
-            updateStatusLabel(tr("Added LOCAL (NED) waypoint"));
-            return 0;
-        }
-        else
-        {
-            // Do nothing
-            updateStatusLabel(tr("Not adding waypoint, no position of MAV known yet."));
-            return 1;
         }
     }
-    return 1;
 }
 
+void WaypointList::addCurrentPositionWaypoint() {
+    addEditable(true);
+}
+
+
+//int WaypointList::addCurrentPositionWaypoint()
+//{
+//    if (uas)
+//    {
+//        const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
+//        Waypoint *wp;
+//        Waypoint *last = 0;
+//        if (waypoints.size() > 0)
+//        {
+//            last = waypoints.at(waypoints.size()-1);
+//        }
+
+//        if (uas->globalPositionKnown())
+//        {
+//            float acceptanceRadiusGlobal = 10.0f;
+//            float holdTime = 0.0f;
+//            float yawGlobal = 0.0f;
+//            if (last)
+//            {
+//                acceptanceRadiusGlobal = last->getAcceptanceRadius();
+//                holdTime = last->getHoldTime();
+//                yawGlobal = last->getYaw();
+//            }
+//            // Create global frame waypoint per default
+//            wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
+//            WPM->addWaypointEditable(wp);
+//            updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint"));
+//            return 0;
+//        }
+//        else if (uas->localPositionKnown())
+//        {
+//            float acceptanceRadiusLocal = 0.2f;
+//            float holdTime = 0.5f;
+//            if (last)
+//            {
+//                acceptanceRadiusLocal = last->getAcceptanceRadius();
+//                holdTime = last->getHoldTime();
+//            }
+//            // Create local frame waypoint as second option
+//            wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, false, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
+//            WPM->addWaypointEditable(wp);
+//            updateStatusLabel(tr("Added LOCAL (NED) waypoint"));
+//            return 0;
+//        }
+//        else
+//        {
+//            // Do nothing
+//            updateStatusLabel(tr("Not adding waypoint, no position of MAV known yet."));
+//            return 1;
+//        }
+//    }
+//    return 1;
+//}
+
 void WaypointList::updateStatusLabel(const QString &string)
 {
     // Status label in write widget
@@ -646,11 +709,17 @@ void WaypointList::on_clearWPListButton_clicked()
 
 void WaypointList::clearWPWidget()
 {    
+        // Get list
         const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
+
+
+        // XXX delete wps as well
+
+        // Clear UI elements
         while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++)
             WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value();
             widget->remove();
-        }    
+        }
 }
 
 //void WaypointList::setIsLoadFileWP()

src/ui/WaypointList.h

@@ -77,9 +77,11 @@ public slots:
     /** @brief Read the remote waypoint list to "view"-tab only*/
     void refresh();
     /** @brief Add a waypoint to "edit"-tab */
-    void addEditable(); 
+    void addEditable();
+    /** @brief Add a waypoint to "edit"-tab on current MAV position or on generic position (home) */
+    void addEditable(bool onCurrentPosition);
     /** @brief Add a waypoint at the current MAV position */
-    int addCurrentPositionWaypoint();
+    void addCurrentPositionWaypoint();
     /** @brief Add a waypoint by mouse click over the map */
 
     //Update events

src/ui/map/QGCMapWidget.cc

@@ -17,10 +17,10 @@ QGCMapWidget::QGCMapWidget(QWidget *parent) :
     followUAVID(0),
     mapInitialized(false),
     homeAltitude(0),
-    uas(0)
+    uas(NULL)
 {
-    //currWPManager = new UASWaypointManager(NULL); //Create a waypoint manager that is null.
     currWPManager = UASManager::instance()->getActiveUASWaypointManager();
+    waypointLines.insert(0, new QGraphicsItemGroup(map));
     connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));
     connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
     connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
@@ -207,6 +207,18 @@ void QGCMapWidget::loadSettings(bool changePosition)
     trailInterval = settings.value("TRAIL_INTERVAL", trailInterval).toFloat();
     settings.endGroup();
 
+    // SET CORRECT MENU CHECKBOXES
+    // Set the correct trail interval
+    if (trailType == mapcontrol::UAVTrailType::ByDistance)
+    {
+        // XXX
+#warning Settings loading for trail type not implemented
+    }
+    else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed)
+    {
+        // XXX
+    }
+
     // SET TRAIL TYPE
     foreach (mapcontrol::UAVItem* uav, GetUAVS())
     {
@@ -245,25 +257,22 @@ void QGCMapWidget::storeSettings()
 
 void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event)
 {
-
-    // FIXME HACK!
-    //if (currEditMode == EDIT_MODE_WAYPOINTS)
+    // If a waypoint manager is available
+    if (currWPManager)
     {
-        // If a waypoint manager is available
-        if (currWPManager)
-        {
-            // Create new waypoint
-            internals::PointLatLng pos = map->FromLocalToLatLng(event->pos().x(), event->pos().y());
-            Waypoint* wp = currWPManager->createWaypoint();
-            //            wp->blockSignals(true);
-            //            wp->setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT);
-            wp->setLatitude(pos.Lat());
-            wp->setLongitude(pos.Lng());
-            wp->setAltitude(0);
-            //            wp->blockSignals(false);
-            //            currWPManager->notifyOfChangeEditable(wp);
-        }
+        // Create new waypoint
+        internals::PointLatLng pos = map->FromLocalToLatLng(event->pos().x(), event->pos().y());
+        Waypoint* wp = currWPManager->createWaypoint();
+        //            wp->blockSignals(true);
+        //            wp->setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT);
+        wp->setLatitude(pos.Lat());
+        wp->setLongitude(pos.Lng());
+        wp->setFrame((MAV_FRAME)currWPManager->getFrameRecommendation());
+        wp->setAltitude(currWPManager->getAltitudeRecommendation());
+        //            wp->blockSignals(false);
+        //            currWPManager->notifyOfChangeEditable(wp);
     }
+
     OPMapWidget::mouseDoubleClickEvent(event);
 }
 
@@ -276,6 +285,14 @@ void QGCMapWidget::addUAS(UASInterface* uas)
 {
     connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)));
+    if (!waypointLines.value(uas->getUASID(), NULL)) {
+        waypointLines.insert(uas->getUASID(), new QGraphicsItemGroup(map));
+    } else {
+        foreach (QGraphicsItem* item, waypointLines.value(uas->getUASID())->childItems())
+        {
+            delete item;
+        }
+    }
 }
 
 void QGCMapWidget::activeUASSet(UASInterface* uas)
@@ -294,45 +311,17 @@ void QGCMapWidget::activeUASSet(UASInterface* uas)
         disconnect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
     }
 
-    if (uas)
-    {
-        //UASWaypointManager *old = currWPManager;
-
-        currWPManager = uas->getWaypointManager();
-
-        updateSelectedSystem(uas->getUASID());
-        followUAVID = uas->getUASID();
-
-        updateWaypointList(uas->getUASID());
+    currWPManager = uas->getWaypointManager();
 
-        // Connect the waypoint manager / data storage to the UI
-        connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));
-        connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
-        connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
-        connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
+    updateSelectedSystem(uas->getUASID());
+    followUAVID = uas->getUASID();
+    updateWaypointList(uas->getUASID());
 
-        /*if (offlineMode)
-        {
-
-            if (QMessageBox::question(0,"Question","Do you want to append the offline waypoints to the ones currently on the UAV?",QMessageBox::Yes,QMessageBox::No) == QMessageBox::Yes)
-            {
-                //Need to transfer all waypoints from the offline mode WPManager to the online mode.
-                for (int i=0;i<old->getWaypointEditableList().size();i++)
-                {
-                    Waypoint *wp = currWPManager->createWaypoint();
-                    wp->setLatitude(old->getWaypointEditableList()[i]->getLatitude());
-                    wp->setLongitude(old->getWaypointEditableList()[i]->getLongitude());
-                    wp->setAltitude(old->getWaypointEditableList()[i]->getAltitude());
-                }
-            }
-            offlineMode = false;
-            old->deleteLater();
-        }*/
-
-
-        // Delete all waypoints and add waypoint from new system
-
-    }
+    // Connect the waypoint manager / data storage to the UI
+    connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));
+    connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
+    connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
+    connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
 }
 
 /**
@@ -360,9 +349,17 @@ void QGCMapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lo
             newUAV->setParentItem(map);
             UAVS.insert(uas->getUASID(), newUAV);
             uav = GetUAV(uas->getUASID());
-            uav->SetTrailTime(1);
-            uav->SetTrailDistance(5);
-            uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
+            // Set the correct trail type
+            uav->SetTrailType(trailType);
+            // Set the correct trail interval
+            if (trailType == mapcontrol::UAVTrailType::ByDistance)
+            {
+                uav->SetTrailDistance(trailInterval);
+            }
+            else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed)
+            {
+                uav->SetTrailTime(trailInterval);
+            }
         }
 
         // Set new lat/lon position of UAV icon
@@ -551,6 +548,11 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
 {
     // Block circle updates
     Waypoint* wp = iconsToWaypoints.value(waypoint, NULL);
+
+    // Delete UI element if wp doesn't exist
+    if (!wp)
+        WPDelete(waypoint);
+
     // Protect from vicious double update cycle
     if (firingWaypointChange == wp) return;
     // Not in cycle, block now from entering it
@@ -565,8 +567,8 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
     wp->setLatitude(pos.Lat());
     wp->setLongitude(pos.Lng());
     // XXX Magic values
-    wp->setAltitude(homeAltitude + 50.0f);
-    wp->setAcceptanceRadius(10.0f);
+//    wp->setAltitude(homeAltitude + 50.0f);
+//    wp->setAcceptanceRadius(10.0f);
     wp->blockSignals(false);
 
 
@@ -589,13 +591,15 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
  */
 void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
 {
-    qDebug() << "UPDATING WP FUNCTION CALLED";
+    qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED";
     // Source of the event was in this widget, do nothing
-    if (firingWaypointChange == wp) return;
+    if (firingWaypointChange == wp) {
+        return;
+    }
     // Currently only accept waypoint updates from the UAS in focus
     // this has to be changed to accept read-only updates from other systems as well.
     UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
-    if (!uasInstance || uasInstance->getWaypointManager() == currWPManager || uas == -1)
+    if (currWPManager)
     {
         // Only accept waypoints in global coordinate frame
         if (((wp->getFrame() == MAV_FRAME_GLOBAL) || (wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) && wp->isNavigationType())
@@ -607,7 +611,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
             // as we're only handling global waypoints
             int wpindex = currWPManager->getGlobalFrameAndNavTypeIndexOf(wp);
             // If not found, return (this should never happen, but helps safety)
-            if (wpindex == -1) return;
+            if (wpindex < 0) return;
             // Mark this wp as currently edited
             firingWaypointChange = wp;
 
@@ -686,7 +690,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
             // waypoint list. This implies that the coordinate frame of this
             // waypoint was changed and the list containing only global
             // waypoints was shortened. Thus update the whole list
-            if (waypointsToIcons.size() > currWPManager->getGlobalFrameAndNavTypeCount())
+            if (waypointsToIcons.count() > currWPManager->getGlobalFrameAndNavTypeCount())
             {
                 updateWaypointList(uas);
             }
@@ -705,13 +709,23 @@ void QGCMapWidget::updateWaypointList(int uas)
     // Currently only accept waypoint updates from the UAS in focus
     // this has to be changed to accept read-only updates from other systems as well.
     UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
-    if ((uasInstance && (uasInstance->getWaypointManager() == currWPManager)) || uas == -1)
+    if (currWPManager)
     {
         // ORDER MATTERS HERE!
         // TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE
 
         qDebug() << "DELETING NOW OLD WPS";
 
+        // Delete connecting waypoint lines
+        QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
+        if (group)
+        {
+            foreach (QGraphicsItem* item, group->childItems())
+            {
+                delete item;
+            }
+        }
+
         // Delete first all old waypoints
         // this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
         QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
@@ -742,16 +756,6 @@ void QGCMapWidget::updateWaypointList(int uas)
             if (!waypointsToIcons.contains(wp)) updateWaypoint(uas, wp);
         }
 
-        // Delete connecting waypoint lines
-        QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
-        if (group)
-        {
-            foreach (QGraphicsItem* item, group->childItems())
-            {
-                delete item;
-            }
-        }
-
         // Add line element if this is NOT the first waypoint
         mapcontrol::WayPointItem* prevIcon = NULL;
         foreach (Waypoint* wp, wps)