Fixed a few waypoint and persistence issues

src/uas/UAS.cc

@@ -76,7 +76,9 @@ airframe(0),
 attitudeKnown(false),
 paramManager(NULL),
 attitudeStamped(false),
-lastAttitude(0)
+lastAttitude(0),
+isLocalPositionKnown(false),
+isGlobalPositionKnown(false)
 {
     color = UASInterface::getNextColor();
     setBattery(LIPOLY, 3);
@@ -545,6 +547,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     GAudioOutput::instance()->notifyPositive();
                 }
                 positionLock = true;
+                isLocalPositionKnown = true;
             }
             break;
         case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
@@ -573,6 +576,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     GAudioOutput::instance()->notifyPositive();
                 }
                 positionLock = true;
+                isGlobalPositionKnown = true;
                 //TODO fix this hack for forwarding of global position for patch antenna tracking
                 forwardMessage(message);
             }

src/uas/UAS.h

@@ -105,6 +105,14 @@ public:
     double getAltitude() const {
         return altitude;
     }
+    virtual bool localPositionKnown() const
+    {
+        return isLocalPositionKnown;
+    }
+    virtual bool globalPositionKnown() const
+    {
+        return isGlobalPositionKnown;
+    }
 
     double getRoll() const {
         return roll;
@@ -198,14 +206,16 @@ protected: //COMMENTS FOR TEST UNIT
     quint64 imageStart;
 
     QMap<int, QMap<QString, float>* > parameters; ///< All parameters
-    bool paramsOnceRequested;   ///< If the parameter list has been read at least once
+    bool paramsOnceRequested;     ///< If the parameter list has been read at least once
-    int airframe;               ///< The airframe type
+    int airframe;                 ///< The airframe type
-    bool attitudeKnown;         ///< True if attitude was received, false else
+    bool attitudeKnown;           ///< True if attitude was received, false else
     QGCUASParamManager* paramManager; ///< Parameter manager class
-    QString shortStateText;     ///< Short textual state description
+    QString shortStateText;       ///< Short textual state description
-    QString shortModeText;      ///< Short textual mode description
+    QString shortModeText;        ///< Short textual mode description
-    bool attitudeStamped;       ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV
+    bool attitudeStamped;         ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV
-    quint64 lastAttitude;       ///< Timestamp of last attitude measurement
+    quint64 lastAttitude;         ///< Timestamp of last attitude measurement
+    bool isLocalPositionKnown;    ///< If the local position has been received for this MAV
+    bool isGlobalPositionKnown;   ///< If the global position has been received for this MAV
 
 public:
     /** @brief Set the current battery type */

src/uas/UASInterface.h

@@ -74,10 +74,12 @@ public:
     virtual double getLocalX() const = 0;
     virtual double getLocalY() const = 0;
     virtual double getLocalZ() const = 0;
+    virtual bool localPositionKnown() const = 0;
 
     virtual double getLatitude() const = 0;
     virtual double getLongitude() const = 0;
     virtual double getAltitude() const = 0;
+    virtual bool globalPositionKnown() const = 0;
 
     virtual double getRoll() const = 0;
     virtual double getPitch() const = 0;

src/ui/WaypointList.cc

@@ -166,54 +166,72 @@ void WaypointList::read()
 
 void WaypointList::add()
 {
-    if (uas) {
+    if (uas)
+    {
         const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
         Waypoint *wp;
-
+        if (waypoints.size() > 0)
-
+        {
-        if (waypoints.size() > 0) {
             // 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());
             uas->getWaypointManager()->addWaypoint(wp);
-        } else {
+        }
-            // Create global frame waypoint per default
+        else
-            wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0.0, 0.0, 0.0, 0.0, true, true, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT);
+        {
-            uas->getWaypointManager()->addWaypoint(wp);
+            // Create first waypoint at current MAV position
+            addCurrentPositonWaypoint();
         }
     }
 }
 
 void WaypointList::addCurrentPositonWaypoint()
 {
-    /* TODO: implement with new waypoint structure
     if (uas)
     {
-        // For Global Waypoints
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
-        //if(isGlobalWP)
+        Waypoint *wp;
-        //{
+        Waypoint *last = 0;
-            //isLocalWP = false;
+        if (waypoints.size() > 0)
-        //}
-        //else
         {
-            const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
+            last = waypoints.at(waypoints.size()-1);
-            if (waypoints.size() > 0)
+        }
+
+        if (uas->globalPositionKnown())
+        {
+            float acceptanceRadiusGlobal = 10.0f;
+            float holdTime = 0.0f;
+            float yawGlobal = 0.0f;
+            if (last)
             {
-                Waypoint *last = waypoints.at(waypoints.size()-1);
+                acceptanceRadiusGlobal = last->getAcceptanceRadius();
-                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());
+                holdTime = last->getHoldTime();
-                uas->getWaypointManager()->addWaypoint(wp);
+                yawGlobal = last->getYaw();
             }
-            else
+            // Create global frame waypoint per default
+            wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), yawGlobal, acceptanceRadiusGlobal, holdTime, 0.0, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
+            uas->getWaypointManager()->addWaypoint(wp);
+        }
+        else if (uas->localPositionKnown())
+        {
+            float acceptanceRadiusLocal = 0.2f;
+            float holdTime = 0.5f;
+            if (last)
             {
-                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);
+                acceptanceRadiusLocal = last->getAcceptanceRadius();
-                uas->getWaypointManager()->addWaypoint(wp);
+                holdTime = last->getHoldTime();
             }
-
+            // Create local frame waypoint as second option
-             //isLocalWP = true;
+            wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, true, MAV_FRAME_LOCAL, MAV_CMD_NAV_WAYPOINT);
+            uas->getWaypointManager()->addWaypoint(wp);
+        }
+        else
+        {
+            // Do nothing
+            updateStatusLabel(tr("Not adding waypoint, no position known of MAV known yet."));
         }
     }
-    */
 }
 
 void WaypointList::updateStatusLabel(const QString &string)
@@ -223,23 +241,30 @@ void WaypointList::updateStatusLabel(const QString &string)
 
 void WaypointList::changeCurrentWaypoint(quint16 seq)
 {
-    if (uas) {
+    if (uas)
+    {
         uas->getWaypointManager()->setCurrentWaypoint(seq);
     }
 }
 
 void WaypointList::currentWaypointChanged(quint16 seq)
 {
-    if (uas) {
+    if (uas)
+    {
         const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
 
-        if (seq < waypoints.size()) {
+        if (seq < waypoints.size())
-            for(int i = 0; i < waypoints.size(); i++) {
+        {
+            for(int i = 0; i < waypoints.size(); i++)
+            {
                 WaypointView* widget = wpViews.find(waypoints[i]).value();
 
-                if (waypoints[i]->getId() == seq) {
+                if (waypoints[i]->getId() == seq)
+                {
                     widget->setCurrent(true);
-                } else {
+                }
+                else
+                {
                     widget->setCurrent(false);
                 }
             }