adapted UASWaypointManager to waypoint message changes

src/uas/UAS.cc

@@ -437,6 +437,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
             break;
 
+        case MAVLINK_MSG_ID_WAYPOINT_ACK:
+            {
+                mavlink_waypoint_ack_t wpa;
+                mavlink_msg_waypoint_ack_decode(&message, &wpa);
+                if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId())
+                {
+                    waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
+                }
+            }
+            break;
+
         case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
             {
                 mavlink_waypoint_request_t wpr;

src/uas/UASWaypointManager.cc

@@ -32,7 +32,8 @@ This file is part of the PIXHAWK project
 #include "UASWaypointManager.h"
 #include "UAS.h"
 
-#define PROTOCOL_TIMEOUT_MS 2000
+#define PROTOCOL_TIMEOUT_MS 2000    ///< maximum time to wait for pending messages until timeout
+#define PROTOCOL_MAX_RETRIES 3      ///< maximum number of send retries (after timeout)
 
 UASWaypointManager::UASWaypointManager(UAS &_uas)
         : uas(_uas),
@@ -94,7 +95,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
         if(wp->seq == current_wp_id)
         {
             //update the UI FIXME
-            emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->orbit, wp->hold_time);
+            emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2);
 
             //get next waypoint
             current_wp_id++;
@@ -105,6 +106,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
             }
             else
             {
+                sendWaypointAck(0);
+
                 // all waypoints retrieved, change state to idle
                 current_state = WP_IDLE;
                 current_count = 0;
@@ -125,11 +128,30 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
     }
 }
 
+void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa)
+{
+    protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+
+    if (systemId == current_partner_systemid && compId == current_partner_compid && (current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS))
+    {
+        if(current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0)
+        {
+            //all waypoints sent and ack received
+            current_state = WP_IDLE;
+
+            protocol_timer.stop();
+            emit updateStatusString("done.");
+
+            qDebug() << "sent all waypoints to ID " << systemId;
+        }
+    }
+}
+
 void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr)
 {
     protocol_timer.start(PROTOCOL_TIMEOUT_MS);
 
-    if (systemId == current_partner_systemid && compId == current_partner_compid && ((current_state == WP_SENDLIST && wpr->seq == 0) || (current_state == WP_SENDLIST_SENDWPS && (wpr->seq == current_wp_id || wpr->seq == current_wp_id + 1)) || (current_state == WP_IDLE && wpr->seq == current_count-1)))
+    if (systemId == current_partner_systemid && compId == current_partner_compid && ((current_state == WP_SENDLIST && wpr->seq == 0) || (current_state == WP_SENDLIST_SENDWPS && (wpr->seq == current_wp_id || wpr->seq == current_wp_id + 1))))
     {
         qDebug() << "handleWaypointRequest";
 
@@ -138,17 +160,6 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m
             current_state = WP_SENDLIST_SENDWPS;
             current_wp_id = wpr->seq;
             sendWaypoint(current_wp_id);
-
-            if(current_wp_id == waypoint_buffer.count()-1)
-            {
-                //all waypoints sent, but we still have to wait for a possible rerequest of the last waypoint
-                current_state = WP_IDLE;
-
-                protocol_timer.stop();
-                emit updateStatusString("done.");
-
-                qDebug() << "send all waypoints to ID " << systemId;
-            }
         }
         else
         {
@@ -176,7 +187,27 @@ void UASWaypointManager::handleWaypointSetCurrent(quint8 systemId, quint8 compId
 
 void UASWaypointManager::clearWaypointList()
 {
+    if(current_state == WP_IDLE)
+    {
+        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+
+        mavlink_message_t message;
+        mavlink_waypoint_clear_all_t wpca;
+
+        wpca.target_system = uas.getUASID();
+        wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
+
+        current_state = WP_CLEARLIST;
+        current_wp_id = 0;
+        current_partner_systemid = uas.getUASID();
+        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
+
+        const QString str = QString("clearing waypoint list...");
+        emit updateStatusString(str);
 
+        mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
+        uas.sendMessage(message);
+    }
 }
 
 void UASWaypointManager::requestWaypoints()
@@ -237,8 +268,9 @@ void UASWaypointManager::sendWaypoints()
 
                 cur_d->autocontinue = cur_s->getAutoContinue();
                 cur_d->current = cur_s->getCurrent();
-                cur_d->orbit = cur_s->getOrbit();
-                cur_d->hold_time = cur_s->getHoldTime();
+                cur_d->orbit = 0.f;     //FIXME
+                cur_d->param1 = cur_s->getOrbit();
+                cur_d->param2 = cur_s->getHoldTime();
                 cur_d->type = 1;        //FIXME
                 cur_d->seq = i;
                 cur_d->x = cur_s->getX();
@@ -310,3 +342,18 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
         qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system;
     }
 }
+
+void UASWaypointManager::sendWaypointAck(quint8 type)
+{
+    mavlink_message_t message;
+    mavlink_waypoint_ack_t wpa;
+
+    wpa.target_system = uas.getUASID();
+    wpa.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
+    wpa.type = type;
+
+    mavlink_msg_waypoint_ack_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpa);
+    uas.sendMessage(message);
+
+    qDebug() << "sent waypoint ack (" << wpa.type << ") to ID " << wpa.target_system;
+}

src/uas/UASWaypointManager.h

@@ -39,38 +39,53 @@ This file is part of the PIXHAWK project
 #include <mavlink.h>
 class UAS;
 
+/**
+ * @brief Implementation of the MAVLINK waypoint protocol
+ *
+ * This class handles the communication with a waypoint manager on the MAV.
+ * All waypoints are stored in the QVector waypoints, modifications can be done with the WaypointList widget.
+ * Notice that currently the access to the internal waypoint storage is not guarded nor thread-safe. This works as long as no other widget alters the data.
+ *
+ * See http://qgroundcontrol.org/waypoint_protocol for more information about the protocol and the states.
+ */
 class UASWaypointManager : public QObject
 {
 Q_OBJECT
 private:
     enum WaypointState {
-        WP_IDLE = 0,
-        WP_SENDLIST,
-        WP_SENDLIST_SENDWPS,
-        WP_GETLIST,
-        WP_GETLIST_GETWPS
+        WP_IDLE = 0,        ///< Waiting for commands
+        WP_SENDLIST,        ///< Initial state for sending waypoints to the MAV
+        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
     }; ///< The possible states for the waypoint protocol
 
 public:
-    UASWaypointManager(UAS&);
+    UASWaypointManager(UAS&);   ///< Standard constructor.
 
-    void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);
-    void handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp);
-    void handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr);
-    void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr);
-    void handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr);
+    /** @name Protocol handlers */
+    /*@{*/
+    void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);                            ///< Handles received waypoint count messages
+    void handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp);                        ///< Handles received waypoint messages
+    void handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa);                ///< Handles received waypoint ack messages
+    void handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr);        ///< Handles received waypoint request messages
+    void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr);        ///< Handles received waypoint reached messages
+    void handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr); ///< Handles received set current waypoint messages
+    /*@}*/
 
-    QVector<Waypoint *> &getWaypointList(void) { return waypoints; }
+    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 sendWaypointRequest(quint16 seq);
-    void sendWaypoint(quint16 seq);
+    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();
-    void clearWaypointList();
-    void requestWaypoints();
-    void sendWaypoints();
+    void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
+    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
 
 signals:
     void waypointUpdated(quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget