waypoint changes

src/uas/UASInterface.h

@@ -149,9 +149,9 @@ public slots:
     /** @brief Launches the system/Liftof **/
     virtual void launch() = 0;
     /** @brief Set a new waypoint **/
-    virtual void setWaypoint(Waypoint* wp) = 0;
+    //virtual void setWaypoint(Waypoint* wp) = 0;
     /** @brief Set this waypoint as next waypoint to fly to */
-    virtual void setWaypointActive(int wp) = 0;
+    //virtual void setWaypointActive(int wp) = 0;
     /** @brief Order the robot to return home / to land on the runway **/
     virtual void home() = 0;
     /** @brief Halt the system */
@@ -171,9 +171,9 @@ public slots:
      */
     virtual void shutdown() = 0;
     /** @brief Request the list of stored waypoints from the robot */
-    virtual void requestWaypoints() = 0;
+    //virtual void requestWaypoints() = 0;
     /** @brief Clear all existing waypoints on the robot */
-    virtual void clearWaypointList() = 0;
+    //virtual void clearWaypointList() = 0;
     /** @brief Request all onboard parameters of all components */
     virtual void requestParameters() = 0;
     /** @brief Write parameter to permanent storage */

src/uas/UASWaypointManager.cc

@@ -2,7 +2,12 @@
 #include "UAS.h"
 
 UASWaypointManager::UASWaypointManager(UAS &_uas)
-        : uas(_uas)
+        : uas(_uas),
+        current_wp_id(0),
+        current_count(0),
+        current_state(WP_IDLE),
+        current_partner_systemid(0),
+        current_partner_compid(0)
 {
 }
 
@@ -22,17 +27,19 @@ 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_system = MAV_COMP_ID_WAYPOINTPLANNER;
+        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);
     }
 }
@@ -41,21 +48,67 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
 {
     if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid)
     {
+        qDebug() << "handleWaypointCount";
+
         current_count = count;
 
         mavlink_message_t message;
         mavlink_waypoint_request_t wpr;
 
         wpr.target_system = uas.getUASID();
-        wpr.target_system = MAV_COMP_ID_WAYPOINTPLANNER;
-        wpr.seq = current_wp_id;
+        wpr.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
+        wpr.seq = 0;
 
+        current_wp_id = 0;
         current_state = WP_GETLIST_GETWPS;
 
+        mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
         uas.sendMessage(message);
     }
 }
 
+void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp)
+{
+    if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id)
+    {
+        qDebug() << "handleWaypoint";
+
+        if(wp->seq == current_wp_id)
+        {
+            //update the UI FIXME
+            emit waypointUpdated(uas.getUASID(), wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current);
+
+            //get next waypoint
+            current_wp_id++;
+
+            if(current_wp_id < current_count)
+            {
+                mavlink_message_t message;
+                mavlink_waypoint_request_t wpr;
+
+                wpr.target_system = uas.getUASID();
+                wpr.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
+                wpr.seq = current_wp_id;
+
+                mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
+                uas.sendMessage(message);
+            }
+            else
+            {
+                current_state = WP_IDLE;
+                current_count = 0;
+                current_wp_id = 0;
+                current_partner_systemid = 0;
+                current_partner_compid = 0;
+            }
+        }
+        else
+        {
+            //FIXME error handling
+        }
+    }
+}
+
 void UASWaypointManager::getWaypoint(quint16 seq)
 {
 

src/uas/UASWaypointManager.h

@@ -3,6 +3,7 @@
 
 #include <QObject>
 #include "Waypoint.h"
+#include <mavlink.h>
 class UAS;
 
 class UASWaypointManager : public QObject
@@ -21,6 +22,7 @@ public:
     UASWaypointManager(UAS&);
 
     void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);
+    void handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp);
 
 private:
     void getWaypoint(quint16 seq);
@@ -32,6 +34,9 @@ public slots:
     void requestWaypoints();
     void clearWaypointList();
 
+signals:
+    void waypointUpdated(int,int,double,double,double,double,bool,bool);
+
 private:
     UAS &uas;
     quint16 current_wp_id;                 ///< The last used waypoint ID