moved the local waypoint list into waypoint manager

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,12 +70,12 @@ 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 currentWaypointChanged(quint16);                                       ///< emits the new current waypoint sequence number
-    void updateStatusString(const QString &);                                   ///< emits the current status string
+    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
 
 private:
     UAS &uas;                                       ///< Reference to the corresponding UAS
@@ -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)
 {
-    m_ui->statusLabel->setText(string);
+    if (this->uas)
+    {
+        m_ui->statusLabel->setText(string);
+    }
 }
 
 void WaypointList::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
@@ -120,71 +123,81 @@ 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(currentWaypointChanged(quint16)),                                    this, SLOT(currentWaypointChanged(quint16)));
+        connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),                                          this, SLOT(updateStatusLabel(const QString &)));
+        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(requestWaypoints()),                           &uas->getWaypointManager(), SLOT(requestWaypoints()));
-        connect(this, SIGNAL(clearWaypointList()),                          &uas->getWaypointManager(), SLOT(clearWaypointList()));
+        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;
-
-    updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId));
+    if (this->uas)
+    {
+        updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId));
+    }
 }
 
 void WaypointList::currentWaypointChanged(quint16 seq)
 {
-    if (seq < waypoints.size())
+    if (this->uas)
     {
-        for(int i = 0; i < waypoints.size(); i++)
-        {
-            WaypointView* widget = wpViews.find(waypoints[i]).value();
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
 
-            if (waypoints[i]->getId() == seq)
-            {
-                waypoints[i]->setCurrent(true);
-                widget->setCurrent(true);
-            }
-            else
+        if (seq < waypoints.size())
+        {
+            for(int i = 0; i < waypoints.size(); i++)
             {
-                waypoints[i]->setCurrent(false);
-                widget->setCurrent(false);
+                WaypointView* widget = wpViews.find(waypoints[i]).value();
+
+                if (waypoints[i]->getId() == seq)
+                {
+                    waypoints[i]->setCurrent(true);
+                    widget->setCurrent(true);
+                }
+                else
+                {
+                    waypoints[i]->setCurrent(false);
+                    widget->setCurrent(false);
+                }
             }
+            redrawList();
         }
-        redrawList();
     }
 }
 
 void WaypointList::read()
 {
-    while(waypoints.size()>0)
+    if (uas)
     {
-        removeWaypoint(waypoints[0]);
-    }
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
 
-    emit requestWaypoints();
+        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,104 +241,127 @@ void WaypointList::addCurrentPositonWaypoint()
 
 void WaypointList::addWaypoint(Waypoint* wp)
 {
-    waypoints.push_back(wp);
-    if (!wpViews.contains(wp))
+    if (uas)
     {
-        WaypointView* wpview = new WaypointView(wp, this);
-        wpViews.insert(wp, wpview);
-        listLayout->addWidget(wpViews.value(wp));
-        connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*)));
-        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)));
+        uas->getWaypointManager().getWaypointList().push_back(wp);
+        if (!wpViews.contains(wp))
+        {
+            WaypointView* wpview = new WaypointView(wp, this);
+            wpViews.insert(wp, wpview);
+            listLayout->addWidget(wpViews.value(wp));
+            connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*)));
+            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)));
+        }
     }
 }
 
 void WaypointList::redrawList()
 {
-    // Clear list layout
-    if (!wpViews.empty())
+    if (uas)
     {
-        QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews);
-        viewIt.toFront();
-        while(viewIt.hasNext())
-        {
-            viewIt.next();
-            listLayout->removeWidget(viewIt.value());
-        }
-        // Re-add waypoints
-        for(int i = 0; i < waypoints.size(); i++)
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
+        // Clear list layout
+        if (!wpViews.empty())
         {
-            listLayout->addWidget(wpViews.value(waypoints[i]));
+            QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews);
+            viewIt.toFront();
+            while(viewIt.hasNext())
+            {
+                viewIt.next();
+                listLayout->removeWidget(viewIt.value());
+            }
+            // Re-add waypoints
+            for(int i = 0; i < waypoints.size(); i++)
+            {
+                listLayout->addWidget(wpViews.value(waypoints[i]));
+            }
         }
     }
 }
 
 void WaypointList::moveUp(Waypoint* wp)
 {
-    int id = wp->getId();
-    if (waypoints.size() > 1 && waypoints.size() > id)
+    if (uas)
     {
-        Waypoint* temp = waypoints[id];
-        if (id > 0)
-        {
-            waypoints[id] = waypoints[id-1];
-            waypoints[id-1] = temp;
-            waypoints[id-1]->setId(id-1);
-            waypoints[id]->setId(id);
-        }
-        else
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
+        int id = wp->getId();
+        if (waypoints.size() > 1 && waypoints.size() > id)
         {
-            waypoints[id] = waypoints[waypoints.size()-1];
-            waypoints[waypoints.size()-1] = temp;
-            waypoints[waypoints.size()-1]->setId(waypoints.size()-1);
-            waypoints[id]->setId(id);
+            Waypoint* temp = waypoints[id];
+            if (id > 0)
+            {
+                waypoints[id] = waypoints[id-1];
+                waypoints[id-1] = temp;
+                waypoints[id-1]->setId(id-1);
+                waypoints[id]->setId(id);
+            }
+            else
+            {
+                waypoints[id] = waypoints[waypoints.size()-1];
+                waypoints[waypoints.size()-1] = temp;
+                waypoints[waypoints.size()-1]->setId(waypoints.size()-1);
+                waypoints[id]->setId(id);
+            }
+            redrawList();
         }
-        redrawList();
     }
 }
 
 void WaypointList::moveDown(Waypoint* wp)
 {
-    int id = wp->getId();
-    if (waypoints.size() > 1 && waypoints.size() > id)
+    if (uas)
     {
-        Waypoint* temp = waypoints[id];
-        if (id != waypoints.size()-1)
-        {
-            waypoints[id] = waypoints[id+1];
-            waypoints[id+1] = temp;
-            waypoints[id+1]->setId(id+1);
-            waypoints[id]->setId(id);
-        }
-        else
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
+        int id = wp->getId();
+        if (waypoints.size() > 1 && waypoints.size() > id)
         {
-            waypoints[id] = waypoints[0];
-            waypoints[0] = temp;
-            waypoints[0]->setId(0);
-            waypoints[id]->setId(id);
+            Waypoint* temp = waypoints[id];
+            if (id != waypoints.size()-1)
+            {
+                waypoints[id] = waypoints[id+1];
+                waypoints[id+1] = temp;
+                waypoints[id+1]->setId(id+1);
+                waypoints[id]->setId(id);
+            }
+            else
+            {
+                waypoints[id] = waypoints[0];
+                waypoints[0] = temp;
+                waypoints[0]->setId(0);
+                waypoints[id]->setId(id);
+            }
+            redrawList();
         }
-        redrawList();
     }
 }
 
 void WaypointList::removeWaypoint(Waypoint* wp)
 {
-    // Delete from list
-    if (wp != NULL)
+    if (uas)
     {
-        waypoints.remove(wp->getId());
-        for(int i = wp->getId(); i < waypoints.size(); i++)
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
+        // Delete from list
+        if (wp != NULL)
         {
-            waypoints[i]->setId(i);
-        }
+            waypoints.remove(wp->getId());
+            for(int i = wp->getId(); i < waypoints.size(); i++)
+            {
+                waypoints[i]->setId(i);
+            }
 
-        // Remove from view
-        WaypointView* widget = wpViews.find(wp).value();
-        wpViews.remove(wp);
-        widget->hide();
-        listLayout->removeWidget(widget);
-        delete wp;
+            // Remove from view
+            WaypointView* widget = wpViews.find(wp).value();
+            wpViews.remove(wp);
+            widget->hide();
+            listLayout->removeWidget(widget);
+            delete wp;
+        }
     }
 }
 
@@ -338,40 +378,50 @@ void WaypointList::changeEvent(QEvent *e)
 
 void WaypointList::saveWaypoints()
 {
-    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;
-
-    QTextStream in(&file);
-    for (int i = 0; i < waypoints.size(); i++)
+    if (uas)
     {
-        Waypoint* wp = waypoints[i];
-        in << "\t" << wp->getId() << "\t" << wp->getX() << "\t" << wp->getY()  << "\t" << wp->getZ()  << "\t" << wp->getYaw()  << "\t" << wp->getAutoContinue() << "\t" << wp->getCurrent() << "\t" << wp->getOrbit() << "\t" << wp->getHoldTime() << "\n";
-        in.flush();
+        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++)
+        {
+            Waypoint* wp = waypoints[i];
+            in << "\t" << wp->getId() << "\t" << wp->getX() << "\t" << wp->getY()  << "\t" << wp->getZ()  << "\t" << wp->getYaw()  << "\t" << wp->getAutoContinue() << "\t" << wp->getCurrent() << "\t" << wp->getOrbit() << "\t" << wp->getHoldTime() << "\n";
+            in.flush();
+        }
+        file.close();
     }
-    file.close();
 }
 
 void WaypointList::loadWaypoints()
 {
-    QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
-    QFile file(fileName);
-    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
-        return;
-
-    while(waypoints.size()>0)
+    if (uas)
     {
-        removeWaypoint(waypoints[0]);
-    }
+        QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
+        QFile file(fileName);
+        if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
+            return;
 
-    QTextStream in(&file);
-    while (!in.atEnd())
-    {
-        QStringList wpParams = in.readLine().split("\t");
-        if (wpParams.size() == 10)
-            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()));
+        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
+        while(waypoints.size()>0)
+        {
+            removeWaypoint(waypoints[0]);
+        }
+
+        QTextStream in(&file);
+        while (!in.atEnd())
+        {
+            QStringList wpParams = in.readLine().split("\t");
+            if (wpParams.size() == 10)
+                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();
     }
-    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;