Merged waypoint fixes

src/Waypoint.cc

@@ -55,23 +55,27 @@ Waypoint::~Waypoint()
 
 void Waypoint::save(QTextStream &saveStream)
 {
-    saveStream << "\t" << this->getId() << "\t" << this->getX() << "\t" << this->getY()  << "\t" << this->getZ()  << "\t" << this->getYaw()  << "\t" << this->getAutoContinue() << "\t" << this->getCurrent() << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\n";
+    saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t"  << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << this->getX() << "\t" << this->getY()  << "\t" << this->getZ()  << "\t" << this->getYaw()  << "\t" << this->getAutoContinue() << "\r\n";
 }
 
 bool Waypoint::load(QTextStream &loadStream)
 {
     const QStringList &wpParams = loadStream.readLine().split("\t");
-    if (wpParams.size() == 10)
+    if (wpParams.size() == 13)
     {
-        this->id = wpParams[1].toInt();
-        this->x = wpParams[2].toDouble();
-        this->y = wpParams[3].toDouble();
-        this->z = wpParams[4].toDouble();
-        this->yaw = wpParams[5].toDouble();
-        this->autocontinue = (wpParams[6].toInt() == 1 ? true : false);
+        this->id = wpParams[0].toInt();
+        this->frame = (MAV_FRAME) wpParams[1].toInt();
+        this->action = (MAV_ACTION) wpParams[2].toInt();
+        this->orbit = wpParams[3].toDouble();
+        //TODO: orbit direction
+        //TODO: param1
+        this->holdTime = wpParams[6].toInt();
         this->current = (wpParams[7].toInt() == 1 ? true : false);
-        this->orbit = wpParams[8].toDouble();
-        this->holdTime = wpParams[9].toInt();
+        this->x = wpParams[8].toDouble();
+        this->y = wpParams[9].toDouble();
+        this->z = wpParams[10].toDouble();
+        this->yaw = wpParams[11].toDouble();
+        this->autocontinue = (wpParams[12].toInt() == 1 ? true : false);
         return true;
     }
     return false;

src/Waypoint.h

@@ -44,7 +44,7 @@ class Waypoint : public QObject
 
 public:
     Waypoint(quint16 id = 0, float x = 0.0f, float y = 0.0f, float z = 0.0f, float yaw = 0.0f, bool autocontinue = false,
-             bool current = false, float orbit = 0.1f, int holdTime = 2000,
+             bool current = false, float orbit = 0.15f, int holdTime = 0,
              MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_ACTION action=MAV_ACTION_NAVIGATE);
     ~Waypoint();
 

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 "MainWindow.h"
 
 #define PROTOCOL_TIMEOUT_MS 2000    ///< maximum time to wait for pending messages until timeout
 #define PROTOCOL_DELAY_MS 40        ///< minimum delay between sent messages
@@ -164,7 +165,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
         }
         else
         {
-            //TODO: error handling
+            //MainWindow::instance()->showStatusMessage(tr("Waypoint ID mismatch, rejecting waypoint"));
         }
     }
 }
@@ -356,6 +357,10 @@ void UASWaypointManager::saveWaypoints(const QString &saveFile)
         return;
 
     QTextStream out(&file);
+
+    //write the waypoint list version to the first line for compatibility check
+    out << "QGC WPL 100\r\n";
+
     for (int i = 0; i < waypoints.size(); i++)
     {
         waypoints[i]->save(out);
@@ -377,20 +382,35 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
     }
 
     QTextStream in(&file);
-    while (!in.atEnd())
+
+    const QStringList &version = in.readLine().split(" ");
+
+    if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "100"))
     {
-        Waypoint *t = new Waypoint();
-        if(t->load(in))
+        MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),tr("The waypoint file is not compatible with the current version of QGroundControl."));
+    }
+    else
+    {
+        while (!in.atEnd())
         {
-            t->setId(waypoints.size());
-            waypoints.insert(waypoints.size(), t);
+            Waypoint *t = new Waypoint();
+            if(t->load(in))
+            {
+                t->setId(waypoints.size());
+                waypoints.insert(waypoints.size(), t);
+            }
+            else
+            {
+                MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),tr("The waypoint file is corrupted. Load operation only partly succesful."));
+                break;
+            }
         }
     }
+
     file.close();
 
     emit loadWPFile();
     emit waypointListChanged();
-
 }
 
 
@@ -430,8 +450,7 @@ void UASWaypointManager::readWaypoints()
     {
         while(waypoints.size()>0)
         {
-            Waypoint *t = waypoints.back();
-            delete t;
+            delete waypoints.back();
             waypoints.pop_back();
 
         }
@@ -465,7 +484,7 @@ void UASWaypointManager::writeWaypoints()
             current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
 
             //clear local buffer
-            //TODO: Why not replace with waypoint_buffer.clear() ?
+            //TODO: Why not replace with waypoint_buffer.clear() ? - because this will lead to memory leaks, the waypoint-structs have to be deleted, clear() would only delete the pointers.
             while(!waypoint_buffer.empty())
             {
                 delete waypoint_buffer.back();
@@ -504,6 +523,10 @@ void UASWaypointManager::writeWaypoints()
             sendWaypointCount();
         }
     }
+    else if (waypoints.count() == 0)
+    {
+        sendWaypointClearAll();
+    }
     else
     {
         //we're in another transaction, ignore command

src/ui/WaypointList.cc

@@ -211,8 +211,7 @@ void WaypointList::add()
             else
             {
                     // Create global frame waypoint per default
-                    wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(),
-                                                0.0, true, true, 0.15, 2000);
+                    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);
             }
             if (wp->getFrame() == MAV_FRAME_GLOBAL)