UASWaypointManager now implements the protocol described in...

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

src/uas/UASWaypointManager.cc

@@ -33,10 +33,12 @@ This file is part of the PIXHAWK project
 #include "UAS.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
 #define PROTOCOL_MAX_RETRIES 3      ///< maximum number of send retries (after timeout)
 
 UASWaypointManager::UASWaypointManager(UAS &_uas)
         : uas(_uas),
+        current_retries(0),
         current_wp_id(0),
         current_count(0),
         current_state(WP_IDLE),
@@ -49,6 +51,38 @@ UASWaypointManager::UASWaypointManager(UAS &_uas)
 
 void UASWaypointManager::timeout()
 {
+    if (current_retries > 0)
+    {
+        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+        current_retries--;
+        qDebug() << "Timeout, retrying (retries left:" << current_retries << ")";
+        if (current_state == WP_GETLIST)
+        {
+            sendWaypointRequestList();
+        }
+        else if (current_state == WP_GETLIST_GETWPS)
+        {
+            sendWaypointRequest(current_wp_id);
+        }
+        else if (current_state == WP_SENDLIST)
+        {
+            sendWaypointCount();
+        }
+        else if (current_state == WP_SENDLIST_SENDWPS)
+        {
+            sendWaypoint(current_wp_id);
+        }
+        else if (current_state == WP_CLEARLIST)
+        {
+            sendWaypointClearAll();
+        }
+        else if (current_state == WP_SETCURRENT)
+        {
+            sendWaypointSetCurrent(current_wp_id);
+        }
+    }
+    else
+    {
         protocol_timer.stop();
 
         qDebug() << "Waypoint transaction (state=" << current_state << ") timed out going to state WP_IDLE";
@@ -60,14 +94,16 @@ void UASWaypointManager::timeout()
         current_wp_id = 0;
         current_partner_systemid = 0;
         current_partner_compid = 0;
+    }
 }
 
 void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count)
 {
-    protocol_timer.start(PROTOCOL_TIMEOUT_MS);
-
     if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid)
     {
+        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+        current_retries = PROTOCOL_MAX_RETRIES;
+
         qDebug() << "got waypoint count (" << count << ") from ID " << systemId;
 
         if (count > 0)
@@ -88,10 +124,11 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
 
 void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp)
 {
-    protocol_timer.start(PROTOCOL_TIMEOUT_MS);
-
     if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id)
     {
+        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+        current_retries = PROTOCOL_MAX_RETRIES;
+
         if(wp->seq == current_wp_id)
         {
             //update the UI FIXME
@@ -130,30 +167,32 @@ 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 (systemId == current_partner_systemid && compId == current_partner_compid)
     {
-        if(current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0)
+        if((current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS) && (current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0))
         {
             //all waypoints sent and ack received
-            current_state = WP_IDLE;
-
             protocol_timer.stop();
+            current_state = WP_IDLE;
             emit updateStatusString("done.");
-
             qDebug() << "sent all waypoints to ID " << systemId;
         }
+        else if(current_state == WP_CLEARLIST)
+        {
+            protocol_timer.stop();
+            current_state = WP_IDLE;
+            emit updateStatusString("done.");
+            qDebug() << "cleared waypoint list of 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))))
     {
-        qDebug() << "handleWaypointRequest";
+        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+        current_retries = PROTOCOL_MAX_RETRIES;
 
         if (wpr->seq < waypoint_buffer.count())
         {
@@ -180,6 +219,12 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
 {
     if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER)
     {
+        if (current_state == WP_SETCURRENT)
+        {
+            protocol_timer.stop();
+            current_state = WP_IDLE;
+            emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq));
+        }
         qDebug() << "new current waypoint" << wpc->seq;
         emit currentWaypointChanged(wpc->seq);
     }
@@ -190,60 +235,57 @@ 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_retries = PROTOCOL_MAX_RETRIES;
 
         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);
+        sendWaypointClearAll();
     }
 }
 
-void UASWaypointManager::requestWaypoints()
+void UASWaypointManager::setCurrent(quint16 seq)
 {
     if(current_state == WP_IDLE)
     {
         protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+        current_retries = PROTOCOL_MAX_RETRIES;
 
-        mavlink_message_t message;
-        mavlink_waypoint_request_list_t wprl;
+        current_state = WP_SETCURRENT;
+        current_wp_id = seq;
+        current_partner_systemid = uas.getUASID();
+        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
 
-        wprl.target_system = uas.getUASID();
-        wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
+        sendWaypointSetCurrent(current_wp_id);
+    }
+}
+
+void UASWaypointManager::readWaypoints()
+{
+    if(current_state == WP_IDLE)
+    {
+        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+        current_retries = PROTOCOL_MAX_RETRIES;
 
         current_state = WP_GETLIST;
         current_wp_id = 0;
         current_partner_systemid = uas.getUASID();
         current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
 
-        const QString str = QString("requesting waypoint list...");
-        emit updateStatusString(str);
-
-        mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
-        uas.sendMessage(message);
-
-        qDebug() << "sent waypoint list request to ID " << wprl.target_system;
+        sendWaypointRequestList();
     }
 }
 
-void UASWaypointManager::sendWaypoints()
+void UASWaypointManager::writeWaypoints()
 {
     if (current_state == WP_IDLE)
     {
         if (waypoints.count() > 0)
         {
             protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+            current_retries = PROTOCOL_MAX_RETRIES;
 
             current_count = waypoints.count();
             current_state = WP_SENDLIST;
@@ -281,6 +323,55 @@ void UASWaypointManager::sendWaypoints()
             }
 
             //send the waypoint count to UAS (this starts the send transaction)
+            sendWaypointCount();
+        }
+    }
+    else
+    {
+        //we're in another transaction, ignore command
+        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
+    }
+}
+
+void UASWaypointManager::sendWaypointClearAll()
+{
+    mavlink_message_t message;
+    mavlink_waypoint_clear_all_t wpca;
+
+    wpca.target_system = uas.getUASID();
+    wpca.target_component = 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);
+    usleep(PROTOCOL_DELAY_MS * 1000);
+
+    qDebug() << "sent waypoint clear all to ID " << wpca.target_system;
+}
+
+void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
+{
+    mavlink_message_t message;
+    mavlink_waypoint_set_current_t wpsc;
+
+    wpsc.target_system = uas.getUASID();
+    wpsc.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
+    wpsc.seq = seq;
+
+    const QString str = QString("Updating target waypoint...");
+    emit updateStatusString(str);
+
+    mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc);
+    uas.sendMessage(message);
+    usleep(PROTOCOL_DELAY_MS * 1000);
+
+    qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system;
+}
+
+void UASWaypointManager::sendWaypointCount()
+{
     mavlink_message_t message;
     mavlink_waypoint_count_t wpc;
 
@@ -293,15 +384,27 @@ void UASWaypointManager::sendWaypoints()
 
     mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
     uas.sendMessage(message);
+    usleep(PROTOCOL_DELAY_MS * 1000);
 
     qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
-        }
-    }
-    else
-    {
-        //we're in another transaction, ignore command
-        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
-    }
+}
+
+void UASWaypointManager::sendWaypointRequestList()
+{
+    mavlink_message_t message;
+    mavlink_waypoint_request_list_t wprl;
+
+    wprl.target_system = uas.getUASID();
+    wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
+
+    const QString str = QString("requesting waypoint list...");
+    emit updateStatusString(str);
+
+    mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
+    uas.sendMessage(message);
+    usleep(PROTOCOL_DELAY_MS * 1000);
+
+    qDebug() << "sent waypoint list request to ID " << wprl.target_system;
 }
 
 void UASWaypointManager::sendWaypointRequest(quint16 seq)
@@ -318,6 +421,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq)
 
     mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
     uas.sendMessage(message);
+    usleep(PROTOCOL_DELAY_MS * 1000);
 
     qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system;
 }
@@ -339,6 +443,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
 
         mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
         uas.sendMessage(message);
+        usleep(PROTOCOL_DELAY_MS * 1000);
 
         qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system;
     }
@@ -355,6 +460,7 @@ void UASWaypointManager::sendWaypointAck(quint8 type)
 
     mavlink_msg_waypoint_ack_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpa);
     uas.sendMessage(message);
+    usleep(PROTOCOL_DELAY_MS * 1000);
 
     qDebug() << "sent waypoint ack (" << wpa.type << ") to ID " << wpa.target_system;
 }

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);
 };