working on waypointplanner interface

qgroundcontrol.pro

@@ -123,7 +123,8 @@ HEADERS += src/MG.h \
     src/comm/MAVLinkSyntaxHighlighter.h \
     src/ui/watchdog/WatchdogControl.h \
     src/ui/watchdog/WatchdogProcessView.h \
-    src/ui/watchdog/WatchdogView.h
+    src/ui/watchdog/WatchdogView.h \
+    src/uas/UASWaypointManager.h
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -176,5 +177,6 @@ SOURCES += src/main.cc \
     src/comm/MAVLinkSyntaxHighlighter.cc \
     src/ui/watchdog/WatchdogControl.cc \
     src/ui/watchdog/WatchdogProcessView.cc \
-    src/ui/watchdog/WatchdogView.cc
+    src/ui/watchdog/WatchdogView.cc \
+    src/uas/UASWaypointManager.cc
 RESOURCES = mavground.qrc

src/uas/UAS.cc

@@ -51,6 +51,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) :
         links(new QList<LinkInterface*>()),
         unknownPackets(),
         mavlink(protocol),
+        waypointManager(*this),
         thrustSum(0),
         thrustMax(10),
         startVoltage(0),
@@ -394,8 +395,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             break;
         case MAVLINK_MSG_ID_WAYPOINT:
             {
-//                mavlink_waypoint_t wp;
-//                mavlink_msg_waypoint_decode(&message, &wp);
+                mavlink_waypoint_t wp;
+                mavlink_msg_waypoint_decode(&message, &wp);
 //                emit waypointUpdated(uasId, wp.id, wp.x, wp.y, wp.z, wp.yaw, wp.autocontinue, wp.active);
             }
             break;
@@ -945,7 +946,7 @@ void UAS::receiveButton(int buttonIndex)
 }
 
 
-void UAS::requestWaypoints()
+/*void UAS::requestWaypoints()
 {
 //    mavlink_message_t msg;
 //    mavlink_msg_waypoint_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 25);
@@ -1001,7 +1002,7 @@ void UAS::clearWaypointList()
 //    mavlink_msg_waypoint_clear_list_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &clist);
 //    sendMessage(msg);
 //    qDebug() << "UAS clears Waypoints!";
-}
+}*/
 
 
 void UAS::halt()

src/uas/UAS.h

@@ -59,7 +59,7 @@ public:
         LIPOLY = 3,
         LIFE = 4,
         AGZN = 5
-           }; ///< The type of battery used
+    }; ///< The type of battery used
 
     static const int lipoFull = 4.2f;  ///< 100% charged voltage
     static const int lipoEmpty = 3.5f; ///< Discharged voltage
@@ -91,6 +91,8 @@ protected:
     BatteryType batteryType;      ///< The battery type
     int cells;                    ///< Number of cells
 
+    UASWaypointManager waypointManager;
+
     QList<double> actuatorValues;
     QList<QString> actuatorNames;
 
@@ -137,13 +139,16 @@ protected:
     /** @brief Check if vehicle is in autonomous mode */
     bool isAuto();
 
+public:
+    UASWaypointManager &getWaypointManager(void) { return waypointManager; }
+
 public slots:
     /** @brief Launches the system **/
     void launch();
     /** @brief Write this waypoint to the list of waypoints */
-    void setWaypoint(Waypoint* wp);
+    //void setWaypoint(Waypoint* wp); FIXME tbd
     /** @brief Set currently active waypoint */
-    void setWaypointActive(int id);
+    //void setWaypointActive(int id); FIXME tbd
     /** @brief Order the robot to return home / to land on the runway **/
     void home();
     void halt();
@@ -160,9 +165,9 @@ public slots:
     void startLowBattAlarm();
     void stopLowBattAlarm();
 
-    void requestWaypoints();
+    //void requestWaypoints();  FIXME tbd
+    //void clearWaypointList();   FIXME tbd
     void requestParameters();
-    void clearWaypointList();
     /** @brief Enable the motors */
     void enable_motors();
     /** @brief Disable the motors */

src/uas/UASInterface.h

@@ -39,7 +39,7 @@ This file is part of the PIXHAWK project
 
 #include "LinkInterface.h"
 #include "ProtocolInterface.h"
-#include "Waypoint.h"
+#include "UASWaypointManager.h"
 
 /**
  * @brief Interface for all robots.
@@ -68,6 +68,9 @@ public:
     /** @brief Get the status flag for the communication **/
     virtual int getCommunicationStatus() const = 0;
 
+    /** @brief Get reference to the waypoint manager **/
+    virtual UASWaypointManager &getWaypointManager(void) = 0;
+
     /* COMMUNICATION FLAGS */
 
     enum CommStatus {

src/uas/UASWaypointManager.cc

@@ -0,0 +1,66 @@
+#include "UASWaypointManager.h"
+#include "UAS.h"
+
+UASWaypointManager::UASWaypointManager(UAS &_uas)
+        : uas(_uas)
+{
+}
+
+void UASWaypointManager::waypointChanged(Waypoint*)
+{
+}
+
+void UASWaypointManager::currentWaypointChanged(int)
+{
+}
+
+void UASWaypointManager::removeWaypointId(int)
+{
+}
+
+void UASWaypointManager::requestWaypoints()
+{
+    if(current_state == WP_IDLE)
+    {
+        mavlink_message_t message;
+        mavlink_waypoint_request_list_t wprl;
+
+        wprl.target_system = uas.getUASID();
+        wprl.target_system = MAV_COMP_ID_WAYPOINTPLANNER;
+
+        current_state = WP_GETLIST;
+        current_wp_id = 0;
+        current_partner_systemid = uas.getUASID();
+        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
+
+        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)
+    {
+        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;
+
+        current_state = WP_GETLIST_GETWPS;
+
+        uas.sendMessage(message);
+    }
+}
+
+void UASWaypointManager::getWaypoint(quint16 seq)
+{
+
+}
+
+void UASWaypointManager::clearWaypointList()
+{
+}

src/uas/UASWaypointManager.h

@@ -0,0 +1,44 @@
+#ifndef UASWAYPOINTMANAGER_H
+#define UASWAYPOINTMANAGER_H
+
+#include <QObject>
+#include "Waypoint.h"
+class UAS;
+
+class UASWaypointManager : public QObject
+{
+Q_OBJECT
+private:
+    enum WaypointState {
+        WP_IDLE = 0,
+        WP_SENDLIST,
+        WP_SENDLIST_SENDWPS,
+        WP_GETLIST,
+        WP_GETLIST_GETWPS
+    }; ///< The possible states for the waypoint protocol
+
+public:
+    UASWaypointManager(UAS&);
+
+    void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);
+
+private:
+    void getWaypoint(quint16 seq);
+
+public slots:
+    void waypointChanged(Waypoint*);
+    void currentWaypointChanged(int);
+    void removeWaypointId(int);
+    void requestWaypoints();
+    void clearWaypointList();
+
+private:
+    UAS &uas;
+    quint16 current_wp_id;                 ///< The last used waypoint ID
+    quint16 current_count;
+    WaypointState current_state;        ///< The current state
+    quint8 current_partner_systemid;
+    quint8 current_partner_compid;
+};
+
+#endif // UASWAYPOINTMANAGER_H

src/ui/WaypointList.cc

@@ -87,11 +87,11 @@ void WaypointList::setUAS(UASInterface* uas)
         this->uas = uas;
         connect(uas, SIGNAL(waypointUpdated(int,int,double,double,double,double,bool,bool)), this, SLOT(setWaypoint(int,int,double,double,double,double,bool,bool)));
         connect(uas, SIGNAL(waypointReached(UASInterface*,int)), this, SLOT(waypointReached(UASInterface*,int)));
-        connect(this, SIGNAL(waypointChanged(Waypoint*)), uas, SLOT(setWaypoint(Waypoint*)));
-        connect(this, SIGNAL(currentWaypointChanged(int)), uas, SLOT(setWaypointActive(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, SLOT(requestWaypoints()));
-        connect(this, SIGNAL(clearWaypointList()), uas, SLOT(clearWaypointList()));
+        connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(requestWaypoints()));
+        connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList()));
         qDebug() << "Requesting waypoints";
         emit requestWaypoints();
     }