UASWaypointManager now implements the protocol described in...

UASWaypointManager now implements the protocol described in http://qgroundcontrol.org/waypoint_protocol

src/uas/UASWaypointManager.h

@@ -58,7 +58,8 @@ private:
         WP_SENDLIST_SENDWPS,///< Sending waypoints
         WP_GETLIST,         ///< Initial state for retrieving wayppoints from the MAV
         WP_GETLIST_GETWPS,  ///< Receiving waypoints
-        WP_CLEARLIST        ///< Clearing waypoint list on the MAV
+        WP_CLEARLIST,       ///< Clearing waypoint list on the MAV
+        WP_SETCURRENT       ///< Setting new current waypoint on the MAV
     }; ///< The possible states for the waypoint protocol
 
 public:
@@ -77,15 +78,20 @@ public:
     QVector<Waypoint *> &getWaypointList(void) { return waypoints; }    ///< Returns a reference to the local waypoint list. Gives full access to the internal data structure - Subject to change: Public const access and friend access for the waypoint list widget.
 
 private:
+    void sendWaypointClearAll();
+    void sendWaypointSetCurrent(quint16 seq);
+    void sendWaypointCount();
+    void sendWaypointRequestList();
     void sendWaypointRequest(quint16 seq);          ///< Requests a waypoint with sequence number seq
     void sendWaypoint(quint16 seq);                 ///< Sends a waypoint with sequence number seq
     void sendWaypointAck(quint8 type);              ///< Sends a waypoint ack
 
 public slots:
     void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
+    void setCurrent(quint16 seq);                   ///< Sends the sequence number of the waypoint that should get the new target waypoint
     void clearWaypointList();                       ///< Sends the waypoint clear all message to the MAV
-    void requestWaypoints();                        ///< Requests the MAV's current waypoint list
-    void sendWaypoints();                           ///< Sends the local waypoint list to the MAV
+    void readWaypoints();                           ///< Requests the MAV's current waypoint list
+    void writeWaypoints();                          ///< Sends the local waypoint list to the MAV
 
 signals:
     void waypointUpdated(quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget
@@ -94,6 +100,7 @@ signals:
 
 private:
     UAS &uas;                                       ///< Reference to the corresponding UAS
+    quint32 current_retries;                        ///< The current number of retries left
     quint16 current_wp_id;                          ///< The last used waypoint ID in the current protocol transaction
     quint16 current_count;                          ///< The number of waypoints in the current protocol transaction
     WaypointState current_state;                    ///< The current protocol state

src/ui/WaypointList.cc

@@ -126,9 +126,10 @@ void WaypointList::setUAS(UASInterface* uas)
         connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
 
-        connect(this, SIGNAL(sendWaypoints()),       &uas->getWaypointManager(), SLOT(sendWaypoints()));
-        connect(this, SIGNAL(requestWaypoints()),   &uas->getWaypointManager(), SLOT(requestWaypoints()));
+        connect(this, SIGNAL(sendWaypoints()),       &uas->getWaypointManager(), SLOT(writeWaypoints()));
+        connect(this, SIGNAL(requestWaypoints()),   &uas->getWaypointManager(), SLOT(readWaypoints()));
         connect(this, SIGNAL(clearWaypointList()),  &uas->getWaypointManager(), SLOT(clearWaypointList()));
+        connect(this, SIGNAL(setCurrent(quint16)),  &uas->getWaypointManager(), SLOT(setCurrent(quint16)));
     }
 }
 
@@ -149,6 +150,35 @@ void WaypointList::waypointReached(quint16 waypointId)
     }
 }
 
+void WaypointList::changeCurrentWaypoint(quint16 seq)
+{
+    if (this->uas)
+    {
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
+        if (seq < waypoints.size())
+        {
+            for(int i = 0; i < waypoints.size(); i++)
+            {
+                WaypointView* widget = wpViews.find(waypoints[i]).value();
+
+                if (waypoints[i]->getId() == seq)
+                {
+                    waypoints[i]->setCurrent(true);
+                    widget->setCurrent(true);
+                    emit setCurrent(seq);
+                }
+                else
+                {
+                    waypoints[i]->setCurrent(false);
+                    widget->setCurrent(false);
+                }
+            }
+            redrawList();
+        }
+    }
+}
+
 void WaypointList::currentWaypointChanged(quint16 seq)
 {
     if (this->uas)
@@ -250,6 +280,7 @@ void WaypointList::addWaypoint(Waypoint* wp)
             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)));
         }
     }
 }

src/ui/WaypointList.h

@@ -72,7 +72,8 @@ public slots:
     /** @brief sets statusLabel string */
     void updateStatusLabel(const QString &string);
 
-    void currentWaypointChanged(quint16 seq);
+    void changeCurrentWaypoint(quint16 seq);    ///< The user wants to change the current waypoint
+    void currentWaypointChanged(quint16 seq);   ///< The waypointplanner changed the current waypoint
     void setWaypoint(quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime);
     void addWaypoint(Waypoint* wp);
     void removeWaypoint(Waypoint* wp);
@@ -84,6 +85,7 @@ signals:
     void sendWaypoints();
     void requestWaypoints();
     void clearWaypointList();
+    void setCurrent(quint16);
 
 protected:
     virtual void changeEvent(QEvent *e);

src/ui/WaypointView.cc

@@ -115,7 +115,7 @@ void WaypointView::changedCurrent(int state)
     else
     {
         wp->setCurrent(true);
-        emit currentWaypointChanged(wp->getId());   //the slot currentWayppointChanged() in WaypointList sets all other current flags to false
+        emit changeCurrentWaypoint(wp->getId());   //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false
     }
 }
 

src/ui/WaypointView.h

@@ -72,6 +72,7 @@ signals:
     void moveDownWaypoint(Waypoint*);
     void removeWaypoint(Waypoint*);
     void currentWaypointChanged(quint16);
+    void changeCurrentWaypoint(quint16);
     void setYaw(double);
 };