Merge branch 'master' of pixhawk.ethz.ch:qgroundcontrol

src/uas/UASWaypointManager.cc

@@ -94,7 +94,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
         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, wp->orbit, wp->hold_time);
+            emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->orbit, wp->hold_time);
 
             //get next waypoint
             current_wp_id++;
@@ -206,15 +206,15 @@ void UASWaypointManager::requestWaypoints()
     }
 }
 
-void UASWaypointManager::sendWaypoints(const QVector<Waypoint*> &list)
+void UASWaypointManager::sendWaypoints()
 {
     if (current_state == WP_IDLE)
     {
-        if (list.count() > 0)
+        if (waypoints.count() > 0)
         {
             protocol_timer.start(PROTOCOL_TIMEOUT_MS);
 
-            current_count = list.count();
+            current_count = waypoints.count();
             current_state = WP_SENDLIST;
             current_wp_id = 0;
             current_partner_systemid = uas.getUASID();
@@ -233,7 +233,7 @@ void UASWaypointManager::sendWaypoints(const QVector<Waypoint*> &list)
                 waypoint_buffer.push_back(new mavlink_waypoint_t);
                 mavlink_waypoint_t *cur_d = waypoint_buffer.back();
                 memset(cur_d, 0, sizeof(mavlink_waypoint_t));   //initialize with zeros
-                const Waypoint *cur_s = list.at(i);
+                const Waypoint *cur_s = waypoints.at(i);
 
                 cur_d->autocontinue = cur_s->getAutoContinue();
                 cur_d->current = cur_s->getCurrent();

src/uas/UASWaypointManager.h

@@ -60,6 +60,8 @@ public:
     void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr);
     void handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr);
 
+    QVector<Waypoint *> &getWaypointList(void) { return waypoints; }
+
 private:
     void sendWaypointRequest(quint16 seq);
     void sendWaypoint(quint16 seq);
@@ -68,10 +70,10 @@ public slots:
     void timeout();
     void clearWaypointList();
     void requestWaypoints();
-    void sendWaypoints(const QVector<Waypoint *> &list);
+    void sendWaypoints();
 
 signals:
-    void waypointUpdated(int,quint16,double,double,double,double,bool,bool,double,int);    ///< Adds a waypoint to the waypoint list widget
+    void waypointUpdated(quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget
     void currentWaypointChanged(quint16);                                           ///< emits the new current waypoint sequence number
     void updateStatusString(const QString &);                                       ///< emits the current status string
 
@@ -83,6 +85,7 @@ private:
     quint8 current_partner_systemid;                ///< The current protocol communication target system
     quint8 current_partner_compid;                  ///< The current protocol communication target component
 
+    QVector<Waypoint *> waypoints;                  ///< local waypoint list
     QVector<mavlink_waypoint_t *> waypoint_buffer;  ///< communication buffer for waypoints
     QTimer protocol_timer;                          ///< Timer to catch timeouts
 };

src/ui/WaypointList.cc

@@ -79,11 +79,11 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
 
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
 
-    // STATUS LABEL
-    updateStatusLabel("");
-
     // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED
     setUAS(uas);
+
+    // STATUS LABEL
+    updateStatusLabel("");
 }
 
 WaypointList::~WaypointList()
@@ -93,7 +93,10 @@ WaypointList::~WaypointList()
 
 void WaypointList::updateStatusLabel(const QString &string)
 {
+    if (this->uas)
+    {
         m_ui->statusLabel->setText(string);
+    }
 }
 
 void WaypointList::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
@@ -121,36 +124,40 @@ void WaypointList::setUAS(UASInterface* uas)
         this->uas = uas;
 
         connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),                                          this, SLOT(updateStatusLabel(const QString &)));
-        connect(&uas->getWaypointManager(), SIGNAL(waypointUpdated(int,quint16,double,double,double,double,bool,bool,double,int)), this, SLOT(setWaypoint(int,quint16,double,double,double,double,bool,bool,double,int)));
+        connect(&uas->getWaypointManager(), SIGNAL(waypointUpdated(quint16,double,double,double,double,bool,bool,double,int)),    this, SLOT(setWaypoint(quint16,double,double,double,double,bool,bool,double,int)));
         connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)),                                              this, SLOT(currentWaypointChanged(quint16)));
         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(const QVector<Waypoint*> &)),    &uas->getWaypointManager(), SLOT(sendWaypoints(const QVector<Waypoint*> &)));
+        connect(this, SIGNAL(sendWaypoints()),       &uas->getWaypointManager(), SLOT(sendWaypoints()));
         connect(this, SIGNAL(requestWaypoints()),   &uas->getWaypointManager(), SLOT(requestWaypoints()));
         connect(this, SIGNAL(clearWaypointList()),  &uas->getWaypointManager(), SLOT(clearWaypointList()));
     }
 }
 
-void WaypointList::setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime)
+void WaypointList::setWaypoint(quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime)
 {
-    if (uasId == this->uas->getUASID())
+    if (this->uas)
     {
         Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime);
         addWaypoint(wp);
     }
 }
 
-void WaypointList::waypointReached(UASInterface* uas, quint16 waypointId)
+void WaypointList::waypointReached(quint16 waypointId)
 {
-    Q_UNUSED(uas);
-    qDebug() << "Waypoint reached: " << waypointId;
-
+    if (this->uas)
+    {
         updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId));
+    }
 }
 
 void WaypointList::currentWaypointChanged(quint16 seq)
 {
+    if (this->uas)
+    {
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         if (seq < waypoints.size())
         {
             for(int i = 0; i < waypoints.size(); i++)
@@ -170,21 +177,27 @@ void WaypointList::currentWaypointChanged(quint16 seq)
             }
             redrawList();
         }
+    }
 }
 
 void WaypointList::read()
 {
+    if (uas)
+    {
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         while(waypoints.size()>0)
         {
             removeWaypoint(waypoints[0]);
         }
 
         emit requestWaypoints();
+    }
 }
 
 void WaypointList::transmit()
 {
-    emit sendWaypoints(waypoints);
+    emit sendWaypoints();
     //emit requestWaypoints(); FIXME
 }
 
@@ -193,6 +206,8 @@ void WaypointList::add()
     // Only add waypoints if UAS is present
     if (uas)
     {
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         if (waypoints.size() > 0)
         {
             Waypoint *last = waypoints.at(waypoints.size()-1);
@@ -209,6 +224,8 @@ void WaypointList::addCurrentPositonWaypoint()
 {
     if (uas)
     {
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         if (waypoints.size() > 0)
         {
             Waypoint *last = waypoints.at(waypoints.size()-1);
@@ -224,7 +241,9 @@ void WaypointList::addCurrentPositonWaypoint()
 
 void WaypointList::addWaypoint(Waypoint* wp)
 {
-    waypoints.push_back(wp);
+    if (uas)
+    {
+        uas->getWaypointManager().getWaypointList().push_back(wp);
         if (!wpViews.contains(wp))
         {
             WaypointView* wpview = new WaypointView(wp, this);
@@ -235,10 +254,15 @@ void WaypointList::addWaypoint(Waypoint* wp)
             connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*)));
             connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
         }
+    }
 }
 
 void WaypointList::redrawList()
 {
+    if (uas)
+    {
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         // Clear list layout
         if (!wpViews.empty())
         {
@@ -255,10 +279,15 @@ void WaypointList::redrawList()
                 listLayout->addWidget(wpViews.value(waypoints[i]));
             }
         }
+    }
 }
 
 void WaypointList::moveUp(Waypoint* wp)
 {
+    if (uas)
+    {
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         int id = wp->getId();
         if (waypoints.size() > 1 && waypoints.size() > id)
         {
@@ -279,10 +308,15 @@ void WaypointList::moveUp(Waypoint* wp)
             }
             redrawList();
         }
+    }
 }
 
 void WaypointList::moveDown(Waypoint* wp)
 {
+    if (uas)
+    {
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         int id = wp->getId();
         if (waypoints.size() > 1 && waypoints.size() > id)
         {
@@ -303,10 +337,15 @@ void WaypointList::moveDown(Waypoint* wp)
             }
             redrawList();
         }
+    }
 }
 
 void WaypointList::removeWaypoint(Waypoint* wp)
 {
+    if (uas)
+    {
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         // Delete from list
         if (wp != NULL)
         {
@@ -323,6 +362,7 @@ void WaypointList::removeWaypoint(Waypoint* wp)
             listLayout->removeWidget(widget);
             delete wp;
         }
+    }
 }
 
 void WaypointList::changeEvent(QEvent *e)
@@ -338,11 +378,15 @@ void WaypointList::changeEvent(QEvent *e)
 
 void WaypointList::saveWaypoints()
 {
+    if (uas)
+    {
         QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
         QFile file(fileName);
         if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
             return;
 
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         QTextStream in(&file);
         for (int i = 0; i < waypoints.size(); i++)
         {
@@ -351,15 +395,20 @@ void WaypointList::saveWaypoints()
             in.flush();
         }
         file.close();
+    }
 }
 
 void WaypointList::loadWaypoints()
 {
+    if (uas)
+    {
         QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
         QFile file(fileName);
         if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
             return;
 
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
         while(waypoints.size()>0)
         {
             removeWaypoint(waypoints[0]);
@@ -373,5 +422,6 @@ void WaypointList::loadWaypoints()
                 addWaypoint(new Waypoint(wpParams[1].toInt(), wpParams[2].toDouble(), wpParams[3].toDouble(), wpParams[4].toDouble(), wpParams[5].toDouble(), (wpParams[6].toInt() == 1 ? true : false), (wpParams[7].toInt() == 1 ? true : false), wpParams[8].toDouble(), wpParams[9].toInt()));
         }
         file.close();
+    }
 }
 

src/ui/WaypointList.h

@@ -73,24 +73,21 @@ public slots:
     void updateStatusLabel(const QString &string);
 
     void currentWaypointChanged(quint16 seq);
-
-    //To be moved to UASWaypointManager (?)
-    void setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime);
+    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);
-    void waypointReached(UASInterface* uas, quint16 waypointId);
+    void waypointReached(quint16 waypointId);
     void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
     void updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
 
 signals:
-    void sendWaypoints(const QVector<Waypoint*> &);
+    void sendWaypoints();
     void requestWaypoints();
     void clearWaypointList();
 
 protected:
     virtual void changeEvent(QEvent *e);
 
-    QVector<Waypoint*> waypoints;
     QMap<Waypoint*, WaypointView*> wpViews;
     QVBoxLayout* listLayout;
     UASInterface* uas;