Major refactoring of the waypoint code. The new design fits the...

Major refactoring of the waypoint code. The new design fits the model-view-controller pattern better.

src/Waypoint.cc

@@ -31,6 +31,7 @@ This file is part of the PIXHAWK project
  */
 
 #include "Waypoint.h"
+#include <QStringList>
 
 Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime)
 : id(_id),
@@ -50,6 +51,30 @@ Waypoint::~Waypoint()
 
 }
 
+void Waypoint::save(QTextStream &saveStream)
+{
+    saveStream << "\t" << this->getId() << "\t" << this->getX() << "\t" << this->getY()  << "\t" << this->getZ()  << "\t" << this->getYaw()  << "\t" << this->getAutoContinue() << "\t" << this->getCurrent() << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\n";
+}
+
+bool Waypoint::load(QTextStream &loadStream)
+{
+    const QStringList &wpParams = loadStream.readLine().split("\t");
+    if (wpParams.size() == 10)
+    {
+        this->id = wpParams[1].toInt();
+        this->x = wpParams[2].toDouble();
+        this->y = wpParams[3].toDouble();
+        this->z = wpParams[4].toDouble();
+        this->yaw = wpParams[5].toDouble();
+        this->autocontinue = (wpParams[6].toInt() == 1 ? true : false);
+        this->current = (wpParams[7].toInt() == 1 ? true : false);
+        this->orbit = wpParams[8].toDouble();
+        this->holdTime = wpParams[9].toInt();
+        return true;
+    }
+    return false;
+}
+
 void Waypoint::setId(quint16 id)
 {
     this->id = id;

src/Waypoint.h

@@ -35,6 +35,7 @@ This file is part of the PIXHAWK project
 
 #include <QObject>
 #include <QString>
+#include <QTextStream>
 
 class Waypoint : public QObject
 {
@@ -54,6 +55,10 @@ public:
     float getOrbit() const { return orbit; }
     float getHoldTime() const { return holdTime; }
 
+    void save(QTextStream &saveStream);
+    bool load(QTextStream &loadStream);
+
+
 private:
     quint16 id;
     float x;

src/uas/UASWaypointManager.cc

@@ -131,8 +131,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
 
         if(wp->seq == current_wp_id)
         {
-            //update the UI FIXME
-            emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2);
+            Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2);
+            localAddWaypoint(lwp);
 
             //get next waypoint
             current_wp_id++;
@@ -223,42 +223,182 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
         {
             protocol_timer.stop();
             current_state = WP_IDLE;
+
+            // update the local main storage
+            if (wpc->seq < waypoints.size())
+            {
+                for(int i = 0; i < waypoints.size(); i++)
+                {
+                    if (waypoints[i]->getId() == wpc->seq)
+                    {
+                        waypoints[i]->setCurrent(true);
+                    }
+                    else
+                    {
+                        waypoints[i]->setCurrent(false);
+                    }
+                }
+            }
+
             emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq));
+            //emit update to UI widgets
+            emit currentWaypointChanged(wpc->seq);
         }
         qDebug() << "new current waypoint" << wpc->seq;
-        emit currentWaypointChanged(wpc->seq);
     }
 }
 
-void UASWaypointManager::clearWaypointList()
+int UASWaypointManager::setCurrentWaypoint(quint16 seq)
 {
-    if(current_state == WP_IDLE)
+    if (seq < waypoints.size())
     {
-        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
-        current_retries = PROTOCOL_MAX_RETRIES;
+        if(current_state == WP_IDLE)
+        {
+            //update local main storage
+            for(int i = 0; i < waypoints.size(); i++)
+            {
+                if (waypoints[i]->getId() == seq)
+                {
+                    waypoints[i]->setCurrent(true);
+                }
+                else
+                {
+                    waypoints[i]->setCurrent(false);
+                }
+            }
 
-        current_state = WP_CLEARLIST;
-        current_wp_id = 0;
-        current_partner_systemid = uas.getUASID();
-        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
+            //TODO: signal changed waypoint list
 
-        sendWaypointClearAll();
+            //send change to UAS - important to note: if the transmission fails, we have inconsistencies
+            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+            current_retries = PROTOCOL_MAX_RETRIES;
+
+            current_state = WP_SETCURRENT;
+            current_wp_id = seq;
+            current_partner_systemid = uas.getUASID();
+            current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
+
+            sendWaypointSetCurrent(current_wp_id);
+
+            //emit waypointListChanged();
+
+            return 0;
+        }
+    }
+    return -1;
+}
+
+void UASWaypointManager::localAddWaypoint(Waypoint *wp)
+{
+    if (wp)
+    {
+        wp->setId(waypoints.size());
+        waypoints.insert(waypoints.size(), wp);
+        emit waypointListChanged();
     }
 }
 
-void UASWaypointManager::setCurrent(quint16 seq)
+int UASWaypointManager::localRemoveWaypoint(quint16 seq)
+{
+    if (seq < waypoints.size())
+    {
+        Waypoint *t = waypoints[seq];
+        waypoints.remove(seq);
+        delete t;
+
+        for(int i = seq; i < waypoints.size(); i++)
+        {
+            waypoints[i]->setId(i);
+        }
+        emit waypointListChanged();
+        return 0;
+    }
+    return -1;
+}
+
+void UASWaypointManager::localMoveWaypoint(quint16 cur_seq, quint16 new_seq)
+{
+    if (cur_seq != new_seq && cur_seq < waypoints.size() && new_seq < waypoints.size())
+    {
+        Waypoint *t = waypoints[cur_seq];
+        if (cur_seq < new_seq)
+        {
+            for (int i = cur_seq; i < new_seq; i++)
+            {
+                waypoints[i] = waypoints[i+1];
+                //waypoints[i]->setId(i);       // let the local IDs stay the same so that they can be found more easily by the user
+            }
+        }
+        else
+        {
+            for (int i = cur_seq; i > new_seq; i--)
+            {
+                waypoints[i] = waypoints[i-1];
+               // waypoints[i]->setId(i);
+            }
+        }
+        waypoints[new_seq] = t;
+        //waypoints[new_seq]->setId(new_seq);
+
+        emit waypointListChanged();
+    }
+}
+
+void UASWaypointManager::localSaveWaypoints(const QString &saveFile)
+{
+    QFile file(saveFile);
+    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
+        return;
+
+    QTextStream out(&file);
+    for (int i = 0; i < waypoints.size(); i++)
+    {
+        waypoints[i]->save(out);
+    }
+    file.close();
+}
+
+void UASWaypointManager::localLoadWaypoints(const QString &loadFile)
+{
+    QFile file(loadFile);
+    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
+        return;
+
+    while(waypoints.size()>0)
+    {
+        Waypoint *t = waypoints[0];
+        waypoints.remove(0);
+        delete t;
+    }
+
+    QTextStream in(&file);
+    while (!in.atEnd())
+    {
+        Waypoint *t = new Waypoint();
+        if(t->load(in))
+        {
+            t->setId(waypoints.size());
+            waypoints.insert(waypoints.size(), t);
+        }
+    }
+    file.close();
+
+    emit waypointListChanged();
+}
+
+void UASWaypointManager::clearWaypointList()
 {
     if(current_state == WP_IDLE)
     {
         protocol_timer.start(PROTOCOL_TIMEOUT_MS);
         current_retries = PROTOCOL_MAX_RETRIES;
 
-        current_state = WP_SETCURRENT;
-        current_wp_id = seq;
+        current_state = WP_CLEARLIST;
+        current_wp_id = 0;
         current_partner_systemid = uas.getUASID();
         current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
 
-        sendWaypointSetCurrent(current_wp_id);
+        sendWaypointClearAll();
     }
 }
 
@@ -266,6 +406,13 @@ void UASWaypointManager::readWaypoints()
 {
     if(current_state == WP_IDLE)
     {
+        while(waypoints.size()>0)
+        {
+            Waypoint *t = waypoints.back();
+            delete t;
+            waypoints.pop_back();
+        }
+
         protocol_timer.start(PROTOCOL_TIMEOUT_MS);
         current_retries = PROTOCOL_MAX_RETRIES;
 
@@ -300,6 +447,8 @@ void UASWaypointManager::writeWaypoints()
                 waypoint_buffer.pop_back();
             }
 
+            bool noCurrent = true;
+
             //copy waypoint data to local buffer
             for (int i=0; i < current_count; i++)
             {
@@ -309,17 +458,20 @@ void UASWaypointManager::writeWaypoints()
                 const Waypoint *cur_s = waypoints.at(i);
 
                 cur_d->autocontinue = cur_s->getAutoContinue();
-                cur_d->current = cur_s->getCurrent();
+                cur_d->current = cur_s->getCurrent() & noCurrent;   //make sure only one current waypoint is selected, the first selected will be chosen
                 cur_d->orbit = 0;
                 cur_d->orbit_direction = 0;
                 cur_d->param1 = cur_s->getOrbit();
                 cur_d->param2 = cur_s->getHoldTime();
-                cur_d->type = 1;        //FIXME
-                cur_d->seq = i;
+                cur_d->type = 1;    //FIXME: we only use local waypoints at the moment
+                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
                 cur_d->x = cur_s->getX();
                 cur_d->y = cur_s->getY();
                 cur_d->z = cur_s->getZ();
                 cur_d->yaw = cur_s->getYaw();
+
+                if (cur_s->getCurrent() && noCurrent)
+                    noCurrent = false;
             }
 
             //send the waypoint count to UAS (this starts the send transaction)
@@ -341,8 +493,7 @@ void UASWaypointManager::sendWaypointClearAll()
     wpca.target_system = uas.getUASID();
     wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
 
-    const QString str = QString("clearing waypoint list...");
-    emit updateStatusString(str);
+    emit updateStatusString(QString("clearing waypoint list..."));
 
     mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
     uas.sendMessage(message);
@@ -360,8 +511,7 @@ void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
     wpsc.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
     wpsc.seq = seq;
 
-    const QString str = QString("Updating target waypoint...");
-    emit updateStatusString(str);
+    emit updateStatusString(QString("Updating target waypoint..."));
 
     mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc);
     uas.sendMessage(message);
@@ -379,8 +529,7 @@ void UASWaypointManager::sendWaypointCount()
     wpc.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
     wpc.count = current_count;
 
-    const QString str = QString("start transmitting waypoints...");
-    emit updateStatusString(str);
+    emit updateStatusString(QString("start transmitting waypoints..."));
 
     mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
     uas.sendMessage(message);
@@ -397,8 +546,7 @@ void UASWaypointManager::sendWaypointRequestList()
     wprl.target_system = uas.getUASID();
     wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
 
-    const QString str = QString("requesting waypoint list...");
-    emit updateStatusString(str);
+    emit updateStatusString(QString("requesting waypoint list..."));
 
     mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
     uas.sendMessage(message);
@@ -416,8 +564,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq)
     wpr.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
     wpr.seq = seq;
 
-    const QString str = QString("retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count);
-    emit updateStatusString(str);
+    emit updateStatusString(QString("retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count));
 
     mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
     uas.sendMessage(message);
@@ -438,8 +585,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
         wp->target_system = uas.getUASID();
         wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER;
 
-        const QString str = QString("sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count);
-        emit updateStatusString(str);
+        emit updateStatusString(QString("sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
 
         mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
         uas.sendMessage(message);

src/uas/UASWaypointManager.h

@@ -75,7 +75,23 @@ public:
     void handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_current_t *wpc);        ///< Handles received set current waypoint messages
     /*@}*/
 
-    QVector<Waypoint *> &getWaypointList(void) { return waypoints; }    ///< Returns a reference to the local waypoint list. Gives full access to the internal data structure - Subject to change: Public const access and friend access for the waypoint list widget.
+    /** @name Remote operations */
+    /*@{*/
+    void clearWaypointList();                       ///< Sends the waypoint clear all message to the MAV
+    void readWaypoints();                           ///< Requests the MAV's current waypoint list
+    void writeWaypoints();                          ///< Sends the local waypoint list to the MAV
+    int setCurrentWaypoint(quint16 seq);            ///< Changes the current waypoint and sends the sequence number of the waypoint that should get the new target waypoint to the UAS
+    /*@}*/
+
+    /** @name Local waypoint list operations */
+    /*@{*/
+    const QVector<Waypoint *> &getWaypointList(void) { return waypoints; }  ///< Returns a const reference to the local waypoint list.
+    void localAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
+    int localRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
+    void localMoveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
+    void localSaveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile
+    void localLoadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
+    /*@}*/
 
 private:
     /** @name Message send functions */
@@ -91,15 +107,11 @@ private:
 
 public slots:
     void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
-    void setCurrent(quint16 seq);                   ///< Sends the sequence number of the waypoint that should get the new target waypoint
-    void clearWaypointList();                       ///< Sends the waypoint clear all message to the MAV
-    void readWaypoints();                           ///< Requests the MAV's current waypoint list
-    void writeWaypoints();                          ///< Sends the local waypoint list to the MAV
 
 signals:
-    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
+    void waypointListChanged(void);                 ///< emits signal that the local waypoint list has been changed
+    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

src/ui/HSIDisplay.cc

@@ -632,7 +632,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
 {
     if (uas)
     {
-        QVector<Waypoint*>& list = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint*>& list = uas->getWaypointManager().getWaypointList();
 
         QColor color;
         painter.setBrush(Qt::NoBrush);

src/ui/WaypointList.cc

@@ -55,8 +55,6 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
     listLayout->setAlignment(Qt::AlignTop);
     m_ui->listWidget->setLayout(listLayout);
 
-    wpViews = QMap<Waypoint*, WaypointView*>();
-
     this->uas = NULL;
 
     // ADD WAYPOINT
@@ -120,41 +118,36 @@ 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(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(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()),       &uas->getWaypointManager(), SLOT(writeWaypoints()));
-        connect(this, SIGNAL(requestWaypoints()),   &uas->getWaypointManager(), SLOT(readWaypoints()));
-        connect(this, SIGNAL(clearWaypointList()),  &uas->getWaypointManager(), SLOT(clearWaypointList()));
-        connect(this, SIGNAL(setCurrent(quint16)),  &uas->getWaypointManager(), SLOT(setCurrent(quint16)));
+        connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),        this, SLOT(updateStatusLabel(const QString &)));
+        connect(&uas->getWaypointManager(), SIGNAL(waypointListChanged(void)),                  this, SLOT(waypointListChanged(void)));
+        connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)),            this, SLOT(currentWaypointChanged(quint16)));
     }
 }
 
-void WaypointList::setWaypoint(quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime)
+void WaypointList::waypointReached(quint16 waypointId)
 {
     if (this->uas)
     {
-        Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime);
-        addWaypoint(wp);
+        updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId));
     }
 }
 
-void WaypointList::waypointReached(quint16 waypointId)
+void WaypointList::changeCurrentWaypoint(quint16 seq)
 {
     if (this->uas)
     {
-        updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId));
+        uas->getWaypointManager().setCurrentWaypoint(seq);
     }
 }
 
-void WaypointList::changeCurrentWaypoint(quint16 seq)
+void WaypointList::currentWaypointChanged(quint16 seq)
 {
     if (this->uas)
     {
-        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
 
         if (seq < waypoints.size())
         {
@@ -164,45 +157,67 @@ void WaypointList::changeCurrentWaypoint(quint16 seq)
 
                 if (waypoints[i]->getId() == seq)
                 {
-                    waypoints[i]->setCurrent(true);
                     widget->setCurrent(true);
-                    emit setCurrent(seq);
                 }
                 else
                 {
-                    waypoints[i]->setCurrent(false);
                     widget->setCurrent(false);
                 }
             }
-            redrawList();
         }
     }
 }
 
-void WaypointList::currentWaypointChanged(quint16 seq)
+void WaypointList::waypointListChanged()
 {
     if (this->uas)
     {
-        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
 
-        if (seq < waypoints.size())
+        // first remove all views of non existing waypoints
+        if (!wpViews.empty())
         {
-            for(int i = 0; i < waypoints.size(); i++)
+            QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews);
+            viewIt.toFront();
+            while(viewIt.hasNext())
             {
-                WaypointView* widget = wpViews.find(waypoints[i]).value();
-
-                if (waypoints[i]->getId() == seq)
+                viewIt.next();
+                Waypoint *cur = viewIt.key();
+                int i;
+                for (i = 0; i < waypoints.size(); i++)
                 {
-                    waypoints[i]->setCurrent(true);
-                    widget->setCurrent(true);
+                    if (waypoints[i] == cur)
+                    {
+                        break;
+                    }
                 }
-                else
+                if (i == waypoints.size())
                 {
-                    waypoints[i]->setCurrent(false);
-                    widget->setCurrent(false);
+                    WaypointView* widget = wpViews.find(cur).value();
+                    widget->hide();
+                    listLayout->removeWidget(widget);
+                    wpViews.remove(cur);
                 }
             }
-            redrawList();
+        }
+
+        for(int i = 0; i < waypoints.size(); i++)
+        {
+            Waypoint *wp = waypoints[i];
+            if (!wpViews.contains(wp))
+            {
+                WaypointView* wpview = new WaypointView(wp, this);
+                wpViews.insert(wp, wpview);
+                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)));
+                connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
+            }
+
+            WaypointView *wpv = wpViews.value(wp);
+            wpv->updateValues();    // update the values of the ui elements in the view
+            listLayout->addWidget(wpv);
         }
     }
 }
@@ -211,21 +226,16 @@ void WaypointList::read()
 {
     if (uas)
     {
-        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
-
-        while(waypoints.size()>0)
-        {
-            removeWaypoint(waypoints[0]);
-        }
-
-        emit requestWaypoints();
+        uas->getWaypointManager().readWaypoints();
     }
 }
 
 void WaypointList::transmit()
 {
-    emit sendWaypoints();
-    //emit requestWaypoints(); FIXME
+    if (uas)
+    {
+        uas->getWaypointManager().writeWaypoints();
+    }
 }
 
 void WaypointList::add()
@@ -233,16 +243,17 @@ void WaypointList::add()
     // Only add waypoints if UAS is present
     if (uas)
     {
-        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
-
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
         if (waypoints.size() > 0)
         {
             Waypoint *last = waypoints.at(waypoints.size()-1);
-            addWaypoint(new Waypoint(waypoints.size(), last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()));
+            Waypoint *wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
+            uas->getWaypointManager().localAddWaypoint(wp);
         }
         else
         {
-            addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000));
+            Waypoint *wp = new Waypoint(0, 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000);
+            uas->getWaypointManager().localAddWaypoint(wp);
         }
     }
 }
@@ -251,119 +262,61 @@ void WaypointList::addCurrentPositonWaypoint()
 {
     if (uas)
     {
-        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
-
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
         if (waypoints.size() > 0)
         {
             Waypoint *last = waypoints.at(waypoints.size()-1);
-            addWaypoint(new Waypoint(waypoints.size(), (float)(qRound(mavX*100))/100.f, (float)(qRound(mavY*100))/100.f, (float)(qRound(mavZ*100))/100.f, (float)(qRound(mavYaw*100))/100.f, last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()));
+            Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
+            uas->getWaypointManager().localAddWaypoint(wp);
         }
         else
         {
-            addWaypoint(new Waypoint(waypoints.size(), (float)(qRound(mavX*100))/100.f, (float)(qRound(mavY*100))/100.f, (float)(qRound(mavZ*100))/100.f, (float)(qRound(mavYaw*100))/100.f, true, true, 0.15, 2000));
+            Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000);
+            uas->getWaypointManager().localAddWaypoint(wp);
         }
-
     }
 }
 
-void WaypointList::addWaypoint(Waypoint* wp)
+void WaypointList::moveUp(Waypoint* wp)
 {
     if (uas)
     {
-        uas->getWaypointManager().getWaypointList().push_back(wp);
-        if (!wpViews.contains(wp))
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
+        //get the current position of wp in the local storage
+        int i;
+        for (i = 0; i < waypoints.size(); i++)
         {
-            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)));
-            connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
+            if (waypoints[i] == wp)
+                break;
         }
-    }
-}
-
-void WaypointList::redrawList()
-{
-    if (uas)
-    {
-        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
 
-        // Clear list layout
-        if (!wpViews.empty())
+        // if wp was found and its not the first entry, move it
+        if (i < waypoints.size() && i > 0)
         {
-            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]));
-            }
+            uas->getWaypointManager().localMoveWaypoint(i, i-1);
         }
     }
 }
 
-void WaypointList::moveUp(Waypoint* wp)
+void WaypointList::moveDown(Waypoint* wp)
 {
     if (uas)
     {
-        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
 
-        int id = wp->getId();
-        if (waypoints.size() > 1 && waypoints.size() > id)
+        //get the current position of wp in the local storage
+        int i;
+        for (i = 0; i < waypoints.size(); i++)
         {
-            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();
+            if (waypoints[i] == wp)
+                break;
         }
-    }
-}
 
-void WaypointList::moveDown(Waypoint* wp)
-{
-    if (uas)
-    {
-        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
-
-        int id = wp->getId();
-        if (waypoints.size() > 1 && waypoints.size() > id)
+        // if wp was found and its not the last entry, move it
+        if (i < waypoints.size()-1)
         {
-            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();
+            uas->getWaypointManager().localMoveWaypoint(i, i+1);
         }
     }
 }
@@ -372,24 +325,7 @@ void WaypointList::removeWaypoint(Waypoint* wp)
 {
     if (uas)
     {
-        QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
-
-        // Delete from list
-        if (wp != NULL)
-        {
-            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;
-        }
+        uas->getWaypointManager().localRemoveWaypoint(wp->getId());
     }
 }
 
@@ -409,20 +345,7 @@ 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++)
-        {
-            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();
+        uas->getWaypointManager().localSaveWaypoints(fileName);
     }
 }
 
@@ -431,25 +354,7 @@ 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]);
-        }
-
-        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();
+        uas->getWaypointManager().localLoadWaypoints(fileName);
     }
 }
 

src/ui/WaypointList.h

@@ -55,7 +55,6 @@ class WaypointList : public QWidget {
 
 public slots:
     void setUAS(UASInterface* uas);
-    void redrawList();
 
     //UI Buttons
     void saveWaypoints();
@@ -73,23 +72,18 @@ public slots:
     void updateStatusLabel(const QString &string);
 
     void changeCurrentWaypoint(quint16 seq);    ///< The user wants to change the current waypoint
-    void currentWaypointChanged(quint16 seq);   ///< The waypointplanner changed the current waypoint
-    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 currentWaypointChanged(quint16 seq);   ///< The waypoint planner changed the current waypoint
+    void waypointListChanged(void);             ///< The waypoint manager informs that the waypoint list was changed
+
     void removeWaypoint(Waypoint* wp);
     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();
-    void requestWaypoints();
-    void clearWaypointList();
-    void setCurrent(quint16);
-
 protected:
     virtual void changeEvent(QEvent *e);
 
+protected:
     QMap<Waypoint*, WaypointView*> wpViews;
     QVBoxLayout* listLayout;
     UASInterface* uas;

src/ui/WaypointView.cc

@@ -48,15 +48,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
     this->wp = wp;
 
     // Read values and set user interface
-    m_ui->xSpinBox->setValue(wp->getX());
-    m_ui->ySpinBox->setValue(wp->getY());
-    m_ui->zSpinBox->setValue(wp->getZ());
-    m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.);
-    m_ui->selectedBox->setChecked(wp->getCurrent());
-    m_ui->autoContinue->setChecked(wp->getAutoContinue());
-    m_ui->idLabel->setText(QString("%1").arg(wp->getId()));\
-    m_ui->orbitSpinBox->setValue(wp->getOrbit());
-    m_ui->holdTimeSpinBox->setValue(wp->getHoldTime());
+    updateValues();
 
     connect(m_ui->xSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double)));
     connect(m_ui->ySpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double)));
@@ -110,15 +102,30 @@ void WaypointView::changedCurrent(int state)
 {
     if (state == 0)
     {
-        wp->setCurrent(false);
+        //m_ui->selectedBox->setChecked(true);
+        //m_ui->selectedBox->setCheckState(Qt::Checked);
+        //wp->setCurrent(false);
     }
     else
     {
-        wp->setCurrent(true);
+        //wp->setCurrent(true);
         emit changeCurrentWaypoint(wp->getId());   //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false
     }
 }
 
+void WaypointView::updateValues()
+{
+    m_ui->xSpinBox->setValue(wp->getX());
+    m_ui->ySpinBox->setValue(wp->getY());
+    m_ui->zSpinBox->setValue(wp->getZ());
+    m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.);
+    m_ui->selectedBox->setChecked(wp->getCurrent());
+    m_ui->autoContinue->setChecked(wp->getAutoContinue());
+    m_ui->idLabel->setText(QString("%1").arg(wp->getId()));\
+    m_ui->orbitSpinBox->setValue(wp->getOrbit());
+    m_ui->holdTimeSpinBox->setValue(wp->getHoldTime());
+}
+
 void WaypointView::setCurrent(bool state)
 {
     if (state)

src/ui/WaypointView.h

@@ -57,6 +57,7 @@ public slots:
     void remove();
     void changedAutoContinue(int);
     void changedCurrent(int);
+    void updateValues(void);
 
     void setYaw(int);   //hidden degree to radian conversion