merged

src/uas/UAS.cc

@@ -379,6 +379,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
             }
             break;
+
+        case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
+            {
+                mavlink_waypoint_request_t wpr;
+                mavlink_msg_waypoint_request_decode(&message, &wpr);
+                waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
+            }
+            break;
+
         case MAVLINK_MSG_ID_WAYPOINT_REACHED:
             {
 //                mavlink_waypoint_reached_t wp;

src/uas/UASWaypointManager.cc

@@ -11,39 +11,6 @@ UASWaypointManager::UASWaypointManager(UAS &_uas)
 {
 }
 
-void UASWaypointManager::waypointChanged(Waypoint*)
-{
-}
-
-void UASWaypointManager::currentWaypointChanged(int)
-{
-}
-
-void UASWaypointManager::removeWaypointId(int)
-{
-}
-
-void UASWaypointManager::requestWaypoints()
-{
-    if(current_state == WP_IDLE)
-    {
-        qDebug() << "handleWaypointCount";
-        mavlink_message_t message;
-        mavlink_waypoint_request_list_t wprl;
-
-        wprl.target_system = uas.getUASID();
-        wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
-
-        current_state = WP_GETLIST;
-        current_wp_id = 0;
-        current_partner_systemid = uas.getUASID();
-        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
-
-        mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
-        uas.sendMessage(message);
-    }
-}
-
 void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count)
 {
     if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid)
@@ -95,6 +62,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
             }
             else
             {
+                // all waypoints retrieved, change state to idle
                 current_state = WP_IDLE;
                 current_count = 0;
                 current_wp_id = 0;
@@ -104,16 +72,75 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
         }
         else
         {
-            //FIXME error handling
+            //TODO: error handling
         }
     }
 }
 
-void UASWaypointManager::getWaypoint(quint16 seq)
+void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr)
 {
+    if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_SENDLIST && wpr->seq == current_wp_id)
+    {
+        qDebug() << "handleWaypointRequest";
 
+        if (wpr->seq < 0)
+        {
+            //TODO: send waypoint
+        }
+        else
+        {
+            //TODO: Error message or something
+        }
+    }
 }
 
 void UASWaypointManager::clearWaypointList()
 {
+
+}
+
+void UASWaypointManager::currentWaypointChanged(int)
+{
+
+}
+
+void UASWaypointManager::removeWaypointId(int)
+{
+
+}
+
+void UASWaypointManager::requestWaypoints()
+{
+    if(current_state == WP_IDLE)
+    {
+        qDebug() << "requestWaypoints";
+        mavlink_message_t message;
+        mavlink_waypoint_request_list_t wprl;
+
+        wprl.target_system = uas.getUASID();
+        wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
+
+        current_state = WP_GETLIST;
+        current_wp_id = 0;
+        current_partner_systemid = uas.getUASID();
+        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
+
+        mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
+        uas.sendMessage(message);
+    }
+}
+
+void UASWaypointManager::sendWaypoints(void)
+{
+
+}
+
+void UASWaypointManager::getWaypoint(quint16 seq)
+{
+
+}
+
+void UASWaypointManager::waypointChanged(Waypoint*)
+{
+
 }

src/uas/UASWaypointManager.h

@@ -23,16 +23,18 @@ public:
 
     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);
 
 private:
     void getWaypoint(quint16 seq);
 
 public slots:
-    void waypointChanged(Waypoint*);
+    void clearWaypointList();
     void currentWaypointChanged(int);
     void removeWaypointId(int);
     void requestWaypoints();
-    void clearWaypointList();
+    void sendWaypoints(void);
+    void waypointChanged(Waypoint*);
 
 signals:
     void waypointUpdated(int,int,double,double,double,double,bool,bool);

src/ui/WaypointList.cc

@@ -89,9 +89,11 @@ void WaypointList::setUAS(UASInterface* uas)
         connect(&uas->getWaypointManager(), SIGNAL(waypointReached(UASInterface*,int)), this, SLOT(waypointReached(UASInterface*,int)));
         connect(this, SIGNAL(waypointChanged(Waypoint*)),   &uas->getWaypointManager(), SLOT(setWaypoint(Waypoint*)));
         connect(this, SIGNAL(currentWaypointChanged(int)),  &uas->getWaypointManager(), SLOT(setWaypointActive(int)));
-        // This slot is not implemented in UAS: connect(this, SIGNAL(removeWaypointId(int)), uas, SLOT(removeWaypoint(Waypoint*)));
         connect(this, SIGNAL(requestWaypoints()),           &uas->getWaypointManager(), SLOT(requestWaypoints()));
         connect(this, SIGNAL(clearWaypointList()),          &uas->getWaypointManager(), SLOT(clearWaypointList()));
+
+        // This slot is not implemented in UAS: connect(this, SIGNAL(removeWaypointId(int)), uas, SLOT(removeWaypoint(Waypoint*)));
+
         qDebug() << "Requesting waypoints";
         emit requestWaypoints();
     }

src/ui/WaypointList.h

@@ -53,30 +53,35 @@ class WaypointList : public QWidget {
     virtual ~WaypointList();
 
 public slots:
-    void transmit();
-    void add();
-    void setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current);
     void setUAS(UASInterface* uas);
-    void addWaypoint(Waypoint* wp);
-    void removeWaypointAndName(Waypoint* wp);
     void redrawList();
-    void moveUp(Waypoint* wp);
-    void moveDown(Waypoint* wp);
-    void setCurrentWaypoint(Waypoint* wp);
     void reenableTransmit();
+
+    //UI Buttons
     void saveWaypoints();
     void loadWaypoints();    
+    void transmit();
+    void add();
+    void moveUp(Waypoint* wp);
+    void moveDown(Waypoint* wp);
+    void setCurrentWaypoint(Waypoint* wp);
+
+    //To be moved to UASWaypointManager (?)
+    void setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current);
+    void addWaypoint(Waypoint* wp);
+    void removeWaypointAndName(Waypoint* wp);
     void waypointReached(UASInterface* uas, int waypointId);
 
 protected:
     virtual void changeEvent(QEvent *e);
+    void debugOutputWaypoints();
+
     QVector<Waypoint*> waypoints;
     QMap<int, QString> waypointNames;
     QMap<Waypoint*, WaypointView*> wpViews;
     QVBoxLayout* listLayout;
     QTimer* transmitDelay;
     UASInterface* uas;
-    void debugOutputWaypoints();
 
 private:
     Ui::WaypointList *m_ui;