Merge branch 'experimental' of git@github.com:pixhawk/qgroundcontrol into experimental

lib/QMapControl/src/imagemanager.cpp

@@ -153,7 +153,7 @@ namespace qmapcontrol
         QBuffer buffer(&bytes);
         buffer.open(QIODevice::WriteOnly);
         tileData.save(&buffer, "PNG");
-
+        // FIXME This is weird - why first write in buffer and then in file?
         file.write(bytes);
         file.close();
         return true;

lib/QMapControl/src/layer.cpp

@@ -39,10 +39,10 @@ namespace qmapcontrol
         delete mapAdapter;
     }
 
-    void Layer::setSize(QSize size)
+    void Layer::setSize(QSize size, QPoint screenmiddle)
     {
         this->size = size;
-        screenmiddle = QPoint(size.width()/2, size.height()/2);
+        this->screenmiddle = screenmiddle;// QPoint(size.width()/2, size.height()/2);
         //QMatrix mat;
         //mat.translate(480/2, 640/2);
         //mat.rotate(45);
@@ -292,10 +292,12 @@ namespace qmapcontrol
 
 
         // PREFETCHING
-        int upper = mapmiddle_tile_y-tiles_above-1;
-        int right = mapmiddle_tile_x+tiles_right+1;
-        int left = mapmiddle_tile_x-tiles_right-1;
-        int lower = mapmiddle_tile_y+tiles_bottom+1;
+
+        // FIXME Make prefetching a parameter
+        int upper = mapmiddle_tile_y-tiles_above-2;
+        int right = mapmiddle_tile_x+tiles_right+2;
+        int left = mapmiddle_tile_x-tiles_right-2;
+        int lower = mapmiddle_tile_y+tiles_bottom+2;
 
         int j = upper;
         for (int i=left; i<=right; i++)

lib/QMapControl/src/layer.h

@@ -139,7 +139,7 @@ namespace qmapcontrol
         void moveWidgets(const QPoint mapmiddle_px) const;
         void drawYourImage(QPainter* painter, const QPoint mapmiddle_px) const;
         void drawYourGeometries(QPainter* painter, const QPoint mapmiddle_px, QRect viewport) const;
-        void setSize(QSize size);
+        void setSize(QSize size, QPoint screenmiddle);
         QRect offscreenViewport() const;
         bool takesMouseEvents() const;
         void mouseEvent(const QMouseEvent*, const QPoint mapmiddle_px);

lib/QMapControl/src/layermanager.cpp

@@ -47,6 +47,7 @@ namespace qmapcontrol
         offFactor = factor;
         composedOffscreenImage = QPixmap(offSize);
         composedOffscreenImage2 = QPixmap(offSize);
+        resize(size);
     }
 
     float LayerManager::offscreenImageFactor()
@@ -219,7 +220,7 @@ namespace qmapcontrol
     {
         mylayers.append(layer);
 
-        layer->setSize(size);
+        layer->setSize(size, screenmiddle);
 
         connect(layer, SIGNAL(updateRequest(QRectF)),
                 this, SLOT(updateRequest(QRectF)));
@@ -438,6 +439,20 @@ namespace qmapcontrol
         forceRedraw();
     }
 
+    void LayerManager::drawGeoms()
+    {
+        QPainter painter(&composedOffscreenImage);
+        QListIterator<Layer*> it(mylayers);
+        while (it.hasNext())
+        {
+            Layer* l = it.next();
+            if (l->layertype() == Layer::GeometryLayer && l->isVisible())
+            {
+                l->drawYourGeometries(&painter, mapmiddle_px, layer()->offscreenViewport());
+            }
+        }
+    }
+
     void LayerManager::drawGeoms(QPainter* painter)
     {
         QListIterator<Layer*> it(mylayers);
@@ -450,6 +465,8 @@ namespace qmapcontrol
             }
         }
     }
+
+
     void LayerManager::drawImage(QPainter* painter)
     {
         painter->drawPixmap(-scroll.x()-screenmiddle.x(),
@@ -477,7 +494,7 @@ namespace qmapcontrol
         while (it.hasNext())
         {
             Layer* l = it.next();
-            l->setSize(newSize);
+            l->setSize(newSize, screenmiddle);
         }
 
         newOffscreenImage();

lib/QMapControl/src/layermanager.h

@@ -171,6 +171,8 @@ namespace qmapcontrol
          */
         int currentZoom() const;
 
+
+        void drawGeoms();
         void drawGeoms(QPainter* painter);
         void drawImage(QPainter* painter);
 

lib/QMapControl/src/mapcontrol.cpp

@@ -350,6 +350,12 @@ namespace qmapcontrol
     {
         update(rect);
     }
+
+    void MapControl::drawGeometries()
+    {
+        layermanager->drawGeoms();
+    }
+
     void MapControl::updateRequestNew()
     {
         // qDebug() << "MapControl::updateRequestNew()";

lib/QMapControl/src/mapcontrol.h

@@ -345,6 +345,8 @@ namespace qmapcontrol
          */
         void updateRequest ( QRect rect );
 
+        void drawGeometries();
+
         //! updates the hole map by creating a new offscreen image
         /*!
          *

qgroundcontrol.pro

@@ -261,7 +261,9 @@ HEADERS += src/MG.h \
     src/ui/designer/QGCParamSlider.h \
     src/ui/designer/QGCActionButton.h \
     src/ui/designer/QGCToolWidgetItem.h \
-    src/ui/QGCMAVLinkLogPlayer.h
+    src/ui/QGCMAVLinkLogPlayer.h \
+    src/comm/MAVLinkSimulationWaypointPlanner.h \
+    src/comm/MAVLinkSimulationMAV.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|win32-msvc2008: {
@@ -383,7 +385,9 @@ SOURCES += src/main.cc \
     src/ui/designer/QGCParamSlider.cc \
     src/ui/designer/QGCActionButton.cc \
     src/ui/designer/QGCToolWidgetItem.cc \
-    src/ui/QGCMAVLinkLogPlayer.cc
+    src/ui/QGCMAVLinkLogPlayer.cc \
+    src/comm/MAVLinkSimulationWaypointPlanner.cc \
+    src/comm/MAVLinkSimulationMAV.cc
 
 macx|win32-msvc2008: {
     SOURCES += src/ui/map3D/QGCGoogleEarthView.cc

src/comm/LinkManager.cc

@@ -66,10 +66,13 @@ LinkManager::~LinkManager()
 
 void LinkManager::add(LinkInterface* link)
 {
+    if (!links.contains(link))
+    {
         if(!link) return;
         connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
         links.append(link);
         emit newLink(link);
+    }
 }
 
 void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol)
@@ -77,9 +80,18 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol)
     // Connect link to protocol
     // the protocol will receive new bytes from the link
     if(!link || !protocol) return;
+
+    QList<LinkInterface*> linkList = protocolLinks.values(protocol);
+
+    // If protocol has not been added before (list length == 0)
+    // OR if link has not been added to protocol, add
+    if ((linkList.length() > 0 && !linkList.contains(link)) || linkList.length() == 0)
+    {
+        // Protocol is new, add
         connect(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)), protocol, SLOT(receiveBytes(LinkInterface*, QByteArray)));
         // Store the connection information in the protocol links map
         protocolLinks.insertMulti(protocol, link);
+    }
     //qDebug() << __FILE__ << __LINE__ << "ADDED LINK TO PROTOCOL" << link->getName() << protocol->getName() << "NEW SIZE OF LINK LIST:" << protocolLinks.size();
 }
 

src/comm/MAVLinkSimulationLink.cc

@@ -40,9 +40,9 @@ This file is part of the QGROUNDCONTROL project
 #include "LinkManager.h"
 #include "MAVLinkProtocol.h"
 #include "MAVLinkSimulationLink.h"
-// MAVLINK includes
-#include <QGCMAVLink.h>
+#include "QGCMAVLink.h"
 #include "QGC.h"
+#include "MAVLinkSimulationMAV.h"
 
 /**
  * Create a simulated link. This link is connected to an input and output file.
@@ -90,15 +90,13 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
         this->name = "MAVLink simulation link";
     }
 
+
+
     // Initialize the pseudo-random number generator
     srand(QTime::currentTime().msec());
     maxTimeNoise = 0;
     this->id = getNextLinkId();
     LinkManager::instance()->add(this);
-
-    // Open packet log
-    mavlinkLogFile = new QFile();
-    //mavlinkLogFile->open(QIODevice::ReadOnly);
 }
 
 MAVLinkSimulationLink::~MAVLinkSimulationLink()
@@ -118,6 +116,8 @@ void MAVLinkSimulationLink::run()
     status.motor_block = 1;
     status.packet_drop = 0;
 
+
+
     forever
     {
 
@@ -146,6 +146,23 @@ void MAVLinkSimulationLink::run()
     }
 }
 
+void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg)
+{
+    // Allocate buffer with packet data
+    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+    unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);
+
+    // Pack to link buffer
+    readyBufferMutex.lock();
+    for (unsigned int i = 0; i < bufferlength; i++)
+    {
+        readyBuffer.enqueue(*(buf + i));
+    }
+    readyBufferMutex.unlock();
+
+    //qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid;
+}
+
 void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg)
 {
     // Allocate buffer with packet data
@@ -162,10 +179,7 @@ void MAVLinkSimulationLink::mainloop()
     // Test for encoding / decoding packets
 
     // Test data stream
-    const int streamlength = 4096;
-    int streampointer = 0;
-    //const int testoffset = 0;
-    uint8_t stream[streamlength] = {};
+    streampointer = 0;
 
     // Fake system values
 
@@ -673,7 +687,7 @@ void MAVLinkSimulationLink::mainloop()
     }*/
 
     readyBufferMutex.lock();
-    for (int i = 0; i < streampointer; i++)
+    for (unsigned int i = 0; i < streampointer; i++)
     {
         readyBuffer.enqueue(*(stream + i));
     }
@@ -717,6 +731,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
         if (mavlink_parse_char(this->id, data[i], &msg, &comm))
         {
             // MESSAGE RECEIVED!
+            qDebug() << "SIMULATION LINK RECEIVED MESSAGE!";
+            emit messageReceived(msg);
 
             switch (msg.msgid)
             {
@@ -937,6 +953,8 @@ bool MAVLinkSimulationLink::connect()
     emit connected(true);
 
     start(LowPriority);
+    MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1);
+    Q_UNUSED(mav1);
     //    timer->start(rate);
     return true;
 }

src/comm/MAVLinkSimulationLink.h

@@ -89,6 +89,7 @@ public slots:
     virtual void mainloop();
     bool connectLink(bool connect);
     void connectLink();
+    void sendMAVLinkMessage(const mavlink_message_t* msg);
 
 
 protected:
@@ -106,7 +107,7 @@ protected:
     QString simulationHeader;
     /** File where the commands sent by the groundstation are stored **/
     QFile* receiveFile;
-    QTextStream stream;
+    QTextStream textStream;
     QTextStream* fileStream;
     QTextStream* outStream;
     /** Buffer which can be read from connected protocols through readBytes(). **/
@@ -115,6 +116,10 @@ protected:
     quint64 rate;
     int maxTimeNoise;
     quint64 lastSent;
+    static const int streamlength = 4096;
+    unsigned int streampointer;
+    //const int testoffset = 0;
+    uint8_t stream[streamlength];
 
     int readyBytes;
     QQueue<uint8_t> readyBuffer;
@@ -133,6 +138,7 @@ protected:
 
 signals:
     void valueChanged(int uasId, QString curve, double value, quint64 usec);
+    void messageReceived(const mavlink_message_t& message);
 
 };
 

src/comm/MAVLinkSimulationMAV.cc

+#include <QDebug>
+
+#include "MAVLinkSimulationMAV.h"
+
+MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) :
+    QObject(parent),
+    link(parent),
+    planner(parent, systemid),
+    systemid(systemid)
+{
+    connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop()));
+    mainloopTimer.start(1000);
+    connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
+    mainloop();
+}
+
+void MAVLinkSimulationMAV::mainloop()
+{
+    mavlink_message_t msg;
+    mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_SLUGS);
+    link->sendMAVLinkMessage(&msg);
+}
+
+void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
+{
+    qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid;
+
+    switch(msg.msgid)
+    {
+    case MAVLINK_MSG_ID_ATTITUDE:
+        break;
+    }
+}

src/comm/MAVLinkSimulationMAV.h

+#ifndef MAVLINKSIMULATIONMAV_H
+#define MAVLINKSIMULATIONMAV_H
+
+#include <QObject>
+#include <QTimer>
+
+#include "MAVLinkSimulationLink.h"
+#include "MAVLinkSimulationWaypointPlanner.h"
+
+class MAVLinkSimulationMAV : public QObject
+{
+    Q_OBJECT
+public:
+    explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid);
+
+signals:
+
+public slots:
+    void mainloop();
+    void handleMessage(const mavlink_message_t& msg);
+
+protected:
+    MAVLinkSimulationLink* link;
+    MAVLinkSimulationWaypointPlanner planner;
+    int systemid;
+    QTimer mainloopTimer;
+
+
+};
+
+#endif // MAVLINKSIMULATIONMAV_H

src/comm/MAVLinkSimulationWaypointPlanner.cc

+/*======================================================================
+
+PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
+
+(c) 2009-2011 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
+
+This file is part of the PIXHAWK project
+
+    PIXHAWK is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    PIXHAWK is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
+
+========================================================================*/
+
+/**
+*   @file
+*   @brief a program to manage waypoints and exchange them with the ground station
+*
+*   @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+*   @author Benjamin Knecht <bknecht@student.ethz.ch>
+*   @author Christian Schluchter <schluchc@ee.ethz.ch>
+*/
+
+#include <cmath>
+
+#include "MAVLinkSimulationWaypointPlanner.h"
+#include "QGC.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+class PxMatrix3x3;
+
+
+/**
+ * @brief Pixhawk 3D vector class, can be cast to a local OpenCV CvMat.
+ *
+ */
+class PxVector3
+{
+public:
+        /** @brief standard constructor */
+        PxVector3(void) {}
+        /** @brief copy constructor */
+        PxVector3(const PxVector3 &v) { for (int i=0; i < 3; i++) { m_vec[i] = v.m_vec[i]; } }
+        /** @brief x,y,z constructor */
+        PxVector3(const float _x, const float _y, const float _z) { m_vec[0] = _x; m_vec[1] = _y; m_vec[2] = _z; }
+        /** @brief broadcast constructor */
+        PxVector3(const float _f) { for (int i=0; i < 3; i++) { m_vec[i] = _f; } }
+
+private:
+        /** @brief private constructor (not used here, for SSE compatibility) */
+        PxVector3(const float (&_vec)[3]) { for (int i=0; i < 3; i++) { m_vec[i] = _vec[i]; } }
+
+public:
+        /** @brief assignment operator */
+        void operator= (const PxVector3 &r) { for (int i=0; i < 3; i++) { m_vec[i] = r.m_vec[i]; } }
+        /** @brief const element access */
+        float operator[] (const int i) const { return m_vec[i]; }
+        /** @brief element access */
+        float &operator[] (const int i) { return m_vec[i]; }
+
+        // === arithmetic operators ===
+        /** @brief element-wise negation */
+        friend PxVector3 operator- (const PxVector3 &v) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = -v.m_vec[i]; } return ret; }
+        friend PxVector3 operator+ (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } return ret; }
+        friend PxVector3 operator- (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } return ret; }
+        friend PxVector3 operator* (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } return ret; }
+        friend PxVector3 operator/ (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } return ret; }
+
+        friend void operator+= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } }
+        friend void operator-= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } }
+        friend void operator*= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } }
+        friend void operator/= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } }
+
+        friend PxVector3 operator+ (const PxVector3 &l, float f) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + f; } return ret; }
+        friend PxVector3 operator- (const PxVector3 &l, float f) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - f; } return ret; }
+        friend PxVector3 operator* (const PxVector3 &l, float f) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * f; } return ret; }
+        friend PxVector3 operator/ (const PxVector3 &l, float f) { PxVector3 ret; float inv = 1.f/f; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * inv; } return ret; }
+
+        friend void operator+= (PxVector3 &l, float f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + f; } }
+        friend void operator-= (PxVector3 &l, float f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - f; } }
+        friend void operator*= (PxVector3 &l, float f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * f; } }
+        friend void operator/= (PxVector3 &l, float f) { float inv = 1.f/f; for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * inv; } }
+
+        // === vector operators ===
+        /** @brief dot product */
+        float	dot(const PxVector3 &v) const { return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; }
+        /** @brief length squared of the vector */
+        float	lengthSquared(void) const { return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; }
+        /** @brief length of the vector */
+        float	length(void) const { return sqrt(lengthSquared()); }
+        /** @brief cross product */
+        PxVector3 cross(const PxVector3 &v) const { return PxVector3(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); }
+        /** @brief normalizes the vector */
+        PxVector3 &normalize(void) { const float l = 1.f / length(); for (int i=0; i < 3; i++) { m_vec[i] *= l; } return *this; }
+
+friend class PxMatrix3x3;
+protected:
+        float m_vec[3];
+};
+
+/**
+ * @brief Pixhawk 3D vector class in double precision, can be cast to a local OpenCV CvMat.
+ *
+ */
+class PxVector3Double
+{
+public:
+        /** @brief standard constructor */
+        PxVector3Double(void) {}
+        /** @brief copy constructor */
+        PxVector3Double(const PxVector3Double &v) { for (int i=0; i < 3; i++) { m_vec[i] = v.m_vec[i]; } }
+        /** @brief x,y,z constructor */
+        PxVector3Double(const double _x, const double _y, const double _z) { m_vec[0] = _x; m_vec[1] = _y; m_vec[2] = _z; }
+        /** @brief broadcast constructor */
+        PxVector3Double(const double _f) { for (int i=0; i < 3; i++) { m_vec[i] = _f; } }
+
+private:
+        /** @brief private constructor (not used here, for SSE compatibility) */
+        PxVector3Double(const double (&_vec)[3]) { for (int i=0; i < 3; i++) { m_vec[i] = _vec[i]; } }
+
+public:
+        /** @brief assignment operator */
+        void operator= (const PxVector3Double &r) { for (int i=0; i < 3; i++) { m_vec[i] = r.m_vec[i]; } }
+        /** @brief const element access */
+        double operator[] (const int i) const { return m_vec[i]; }
+        /** @brief element access */
+        double &operator[] (const int i) { return m_vec[i]; }
+
+        // === arithmetic operators ===
+        /** @brief element-wise negation */
+        friend PxVector3Double operator- (const PxVector3Double &v) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = -v.m_vec[i]; } return ret; }
+        friend PxVector3Double operator+ (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } return ret; }
+        friend PxVector3Double operator- (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } return ret; }
+        friend PxVector3Double operator* (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } return ret; }
+        friend PxVector3Double operator/ (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } return ret; }
+
+        friend void operator+= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } }
+        friend void operator-= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } }
+        friend void operator*= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } }
+        friend void operator/= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } }
+
+        friend PxVector3Double operator+ (const PxVector3Double &l, double f) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + f; } return ret; }
+        friend PxVector3Double operator- (const PxVector3Double &l, double f) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - f; } return ret; }
+        friend PxVector3Double operator* (const PxVector3Double &l, double f) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * f; } return ret; }
+        friend PxVector3Double operator/ (const PxVector3Double &l, double f) { PxVector3Double ret; double inv = 1.f/f; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * inv; } return ret; }
+
+        friend void operator+= (PxVector3Double &l, double f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + f; } }
+        friend void operator-= (PxVector3Double &l, double f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - f; } }
+        friend void operator*= (PxVector3Double &l, double f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * f; } }
+        friend void operator/= (PxVector3Double &l, double f) { double inv = 1.f/f; for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * inv; } }
+
+        // === vector operators ===
+        /** @brief dot product */
+        double	dot(const PxVector3Double &v) const { return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; }
+        /** @brief length squared of the vector */
+        double	lengthSquared(void) const { return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; }
+        /** @brief length of the vector */
+        double	length(void) const { return sqrt(lengthSquared()); }
+        /** @brief cross product */
+        PxVector3Double cross(const PxVector3Double &v) const { return PxVector3Double(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); }
+        /** @brief normalizes the vector */
+        PxVector3Double &normalize(void) { const double l = 1.f / length(); for (int i=0; i < 3; i++) { m_vec[i] *= l; } return *this; }
+
+friend class PxMatrix3x3;
+protected:
+        double m_vec[3];
+};
+
+MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int sysid) :
+    QObject(parent),
+    link(parent),
+    idle(false),
+    current_active_wp_id(-1),
+    timestamp_lastoutside_orbit(0),
+    timestamp_firstinside_orbit(0),
+    waypoints(&waypoints1),
+    waypoints_receive_buffer(&waypoints2),
+    current_state(PX_WPP_IDLE),
+    protocol_current_wp_id(0),
+    protocol_current_count(0),
+    protocol_current_partner_systemid(0),
+    protocol_current_partner_compid(0),
+    protocol_timestamp_lastaction(0),
+    protocol_timeout(1000),
+    timestamp_last_send_setpoint(0),
+    systemid(sysid),
+    compid(MAV_COMP_ID_WAYPOINTPLANNER),
+    setpointDelay(10),
+    yawTolerance(0.4f),
+    verbose(true),
+    debug(false),
+    silent(false)
+{
+    connect(parent, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
+}
+
+
+
+/*
+*  @brief Sends an waypoint ack message
+*/
+void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type)
+{
+        mavlink_message_t msg;
+        mavlink_waypoint_ack_t wpa;
+
+        wpa.target_system = target_systemid;
+        wpa.target_component = target_compid;
+        wpa.type = type;
+
+        mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa);
+        link->sendMAVLinkMessage(&msg);
+
+
+
+        if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
+}
+
+/*
+*  @brief Broadcasts the new target waypoint and directs the MAV to fly there
+*
+*  This function broadcasts its new active waypoint sequence number and
+*  sends a message to the controller, advising it to fly to the coordinates
+*  of the waypoint with a given orientation
+*
+*  @param seq The waypoint sequence number the MAV should fly to.
+*/
+void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq)
+{
+        if(seq < waypoints->size())
+        {
+                mavlink_waypoint_t *cur = waypoints->at(seq);
+
+                mavlink_message_t msg;
+                mavlink_waypoint_current_t wpc;
+
+                wpc.seq = cur->seq;
+
+                mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc);
+                link->sendMAVLinkMessage(&msg);
+
+
+
+                if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq);
+        }
+}
+
+/*
+*  @brief Directs the MAV to fly to a position
+*
+*  Sends a message to the controller, advising it to fly to the coordinates
+*  of the waypoint with a given orientation
+*
+*  @param seq The waypoint sequence number the MAV should fly to.
+*/
+void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq)
+{
+        if(seq < waypoints->size())
+        {
+                mavlink_waypoint_t *cur = waypoints->at(seq);
+
+                mavlink_message_t msg;
+                mavlink_local_position_setpoint_set_t PControlSetPoint;
+
+                // send new set point to local IMU
+                if (cur->frame == 1)
+                {
+                        PControlSetPoint.target_system = systemid;
+                        PControlSetPoint.target_component = MAV_COMP_ID_IMU;
+                        PControlSetPoint.x = cur->x;
+                        PControlSetPoint.y = cur->y;
+                        PControlSetPoint.z = cur->z;
+                        PControlSetPoint.yaw = cur->yaw;
+
+                        mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint);
+                        link->sendMAVLinkMessage(&msg);
+
+
+                }
+                else
+                {
+                        if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
+                }
+
+                uint64_t now = QGC::groundTimeUsecs();
+                timestamp_last_send_setpoint = now;
+        }
+}
+
+void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count)
+{
+        mavlink_message_t msg;
+        mavlink_waypoint_count_t wpc;
+
+        wpc.target_system = target_systemid;
+        wpc.target_component = target_compid;
+        wpc.count = count;
+
+        mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc);
+        link->sendMAVLinkMessage(&msg);
+
+        if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
+
+
+}
+
+void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq)
+{
+        if (seq < waypoints->size())
+        {
+                mavlink_message_t msg;
+                mavlink_waypoint_t *wp = waypoints->at(seq);
+                wp->target_system = target_systemid;
+                wp->target_component = target_compid;
+
+                if (verbose) printf("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue);
+
+                mavlink_msg_waypoint_encode(systemid, compid, &msg, wp);
+                link->sendMAVLinkMessage(&msg);
+                if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
+
+
+        }
+        else
+        {
+                if (verbose) printf("ERROR: index out of bounds\n");
+        }
+}
+
+void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq)
+{
+        mavlink_message_t msg;
+        mavlink_waypoint_request_t wpr;
+        wpr.target_system = target_systemid;
+        wpr.target_component = target_compid;
+        wpr.seq = seq;
+        mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr);
+        link->sendMAVLinkMessage(&msg);
+        if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
+
+
+}
+
+/*
+*  @brief emits a message that a waypoint reached
+*
+*  This function broadcasts a message that a waypoint is reached.
+*
+*  @param seq The waypoint sequence number the MAV has reached.
+*/
+void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq)
+{
+        mavlink_message_t msg;
+        mavlink_waypoint_reached_t wp_reached;
+
+        wp_reached.seq = seq;
+
+        mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached);
+        link->sendMAVLinkMessage(&msg);
+
+        if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq);
+
+
+}
+
+float MAVLinkSimulationWaypointPlanner::distanceToSegment(uint16_t seq, float x, float y, float z)
+{
+        if (seq < waypoints->size())
+        {
+                mavlink_waypoint_t *cur = waypoints->at(seq);
+
+                const PxVector3 A(cur->x, cur->y, cur->z);
+                const PxVector3 C(x, y, z);
+
+                // seq not the second last waypoint
+                if ((uint16_t)(seq+1) < waypoints->size())
+                {
+                        mavlink_waypoint_t *next = waypoints->at(seq+1);
+                        const PxVector3 B(next->x, next->y, next->z);
+                        const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
+                        if (r >= 0 && r <= 1)
+                        {
+                                const PxVector3 P(A + r*(B-A));
+                                return (P-C).length();
+                        }
+                        else if (r < 0.f)
+                        {
+                                return (C-A).length();
+                        }
+                        else
+                        {
+                                return (C-B).length();
+                        }
+                }
+                else
+                {
+                        return (C-A).length();
+                }
+        }
+        return -1.f;
+}
+
+float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y, float z)
+{
+        if (seq < waypoints->size())
+        {
+                mavlink_waypoint_t *cur = waypoints->at(seq);
+
+                const PxVector3 A(cur->x, cur->y, cur->z);
+                const PxVector3 C(x, y, z);
+
+                return (C-A).length();
+        }
+        return -1.f;
+}
+
+void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg)
+{
+    mavlink_handler(&msg);
+}
+
+void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* msg)
+{
+        // Handle param messages
+//        paramClient->handleMAVLinkPacket(msg);
+
+        //check for timed-out operations
+
+        printf("MAV: %d WAYPOINTPLANNER GOT MESSAGE\n", systemid);
+
+        uint64_t now = QGC::groundTimeUsecs();
+        if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE)
+        {
+                if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state);
+                current_state = PX_WPP_IDLE;
+                protocol_current_count = 0;
+                protocol_current_partner_systemid = 0;
+                protocol_current_partner_compid = 0;
+                protocol_current_wp_id = -1;
+
+                if(waypoints->size() == 0)
+                {
+                        current_active_wp_id = -1;
+                }
+        }
+
+        if(now-timestamp_last_send_setpoint > setpointDelay)
+        {
+                send_setpoint(current_active_wp_id);
+        }
+
+        switch(msg->msgid)
+        {
+                case MAVLINK_MSG_ID_ATTITUDE:
+                {
+                        if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
+                        {
+                                mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
+                                if(wp->frame == 1)
+                                {
+                                        mavlink_attitude_t att;
+                                        mavlink_msg_attitude_decode(msg, &att);
+                                        float yaw_tolerance = yawTolerance;
+                                        //compare current yaw
+                                        if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
+                                        {
+                                                if (att.yaw - yaw_tolerance <= wp->yaw && att.yaw + yaw_tolerance >= wp->yaw)
+                                                        yawReached = true;
+                                        }
+                                        else if(att.yaw - yaw_tolerance < 0.0f)
+                                        {
+                                                float lowerBound = 360.0f + att.yaw - yaw_tolerance;
+                                                if (lowerBound < wp->yaw || wp->yaw < att.yaw + yaw_tolerance)
+                                                        yawReached = true;
+                                        }
+                                        else
+                                        {
+                                                float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
+                                                if (att.yaw - yaw_tolerance < wp->yaw || wp->yaw < upperBound)
+                                                        yawReached = true;
+                                        }
+                                }
+                        }
+                        break;
+                }
+
+                case MAVLINK_MSG_ID_LOCAL_POSITION:
+                {
+                        if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
+                        {
+                                mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
+
+                                if(wp->frame == 1)
+                                {
+                                        mavlink_local_position_t pos;
+                                        mavlink_msg_local_position_decode(msg, &pos);
+                                        if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
+
+                                        posReached = false;
+
+                                        // compare current position (given in message) with current waypoint
+                                        float orbit = wp->param1;
+
+                                        float dist;
+                                        if (wp->param2 == 0)
+                                        {
+                                                dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z);
+                                        }
+                                        else
+                                        {
+                                                dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z);
+                                        }
+
+                                        if (dist >= 0.f && dist <= orbit && yawReached)
+                                        {
+                                                posReached = true;
+                                        }
+                                }
+                        }
+                        break;
+                }
+
+                case MAVLINK_MSG_ID_ACTION: // special action from ground station
+                {
+                        mavlink_action_t action;
+                        mavlink_msg_action_decode(msg, &action);
+                        if(action.target == systemid)
+                        {
+                                if (verbose) printf("Waypoint: received message with action %d\n", action.action);
+                                switch (action.action)
+                                {
+//				case MAV_ACTION_LAUNCH:
+//					if (verbose) std::cerr << "Launch received" << std::endl;
+//					current_active_wp_id = 0;
+//					if (waypoints->size()>0)
+//					{
+//						setActive(waypoints[current_active_wp_id]);
+//					}
+//					else
+//						if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
+//					break;
+
+//				case MAV_ACTION_CONTINUE:
+//					if (verbose) std::c
+//					err << "Continue received" << std::endl;
+//					idle = false;
+//					setActive(waypoints[current_active_wp_id]);
+//					break;
+
+//				case MAV_ACTION_HALT:
+//					if (verbose) std::cerr << "Halt received" << std::endl;
+//					idle = true;
+//					break;
+
+//				default:
+//					if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
+//					break;
+                                }
+                        }
+                        break;
+                }
+
+                case MAVLINK_MSG_ID_WAYPOINT_ACK:
+                {
+                        mavlink_waypoint_ack_t wpa;
+                        mavlink_msg_waypoint_ack_decode(msg, &wpa);
+
+                        if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid))
+                        {
+                                protocol_timestamp_lastaction = now;
+
+                                if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)
+                                {
+                                        if (protocol_current_wp_id == waypoints->size()-1)
+                                        {
+                                                if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n");
+                                                current_state = PX_WPP_IDLE;
+                                                protocol_current_wp_id = 0;
+                                        }
+                                }
+                        }
+                        break;
+                }
+
+                case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
+                {
+                        mavlink_waypoint_set_current_t wpc;
+                        mavlink_msg_waypoint_set_current_decode(msg, &wpc);
+
+                        if(wpc.target_system == systemid && wpc.target_component == compid)
+                        {
+                                protocol_timestamp_lastaction = now;
+
+                                if (current_state == PX_WPP_IDLE)
+                                {
+                                        if (wpc.seq < waypoints->size())
+                                        {
+                                                if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
+                                                current_active_wp_id = wpc.seq;
+                                                uint32_t i;
+                                                for(i = 0; i < waypoints->size(); i++)
+                                                {
+                                                        if (i == current_active_wp_id)
+                                                        {
+                                                                waypoints->at(i)->current = true;
+                                                        }
+                                                        else
+                                                        {
+                                                                waypoints->at(i)->current = false;
+                                                        }
+                                                }
+                                                if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
+                                                yawReached = false;
+                                                posReached = false;
+                                                send_waypoint_current(current_active_wp_id);
+                                                send_setpoint(current_active_wp_id);
+                                                timestamp_firstinside_orbit = 0;
+                                        }
+                                        else
+                                        {
+                                                if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
+                                        }
+                                }
+                        }
+                        break;
+                }
+
+                case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
+                {
+                        mavlink_waypoint_request_list_t wprl;
+                        mavlink_msg_waypoint_request_list_decode(msg, &wprl);
+                        if(wprl.target_system == systemid && wprl.target_component == compid)
+                        {
+                                protocol_timestamp_lastaction = now;
+
+                                if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST)
+                                {
+                                        if (waypoints->size() > 0)
+                                        {
+                                                if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid);
+                                                if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid);
+                                                current_state = PX_WPP_SENDLIST;
+                                                protocol_current_wp_id = 0;
+                                                protocol_current_partner_systemid = msg->sysid;
+                                                protocol_current_partner_compid = msg->compid;
+                                        }
+                                        else
+                                        {
+                                                if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
+                                        }
+                                        protocol_current_count = waypoints->size();
+                                        send_waypoint_count(msg->sysid,msg->compid, protocol_current_count);
+                                }
+                                else
+                                {
+                                        if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state);
+                                }
+                        }
+                        else
+                        {
+                                if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because not my systemid or compid.\n");
+                        }
+                        break;
+                }
+
+                case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
+                {
+                        mavlink_waypoint_request_t wpr;
+                        mavlink_msg_waypoint_request_decode(msg, &wpr);
+                        if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid)
+                        {
+                                protocol_timestamp_lastaction = now;
+
+                                //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
+                                if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size()))
+                                {
+                                        if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+
+                                        current_state = PX_WPP_SENDLIST_SENDWPS;
+                                        protocol_current_wp_id = wpr.seq;
+                                        send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq);
+                                }
+                                else
+                                {
+                                        if (verbose)
+                                        {
+                                                if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; }
+                                                else if (current_state == PX_WPP_SENDLIST)
+                                                {
+                                                        if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
+                                                }
+                                                else if (current_state == PX_WPP_SENDLIST_SENDWPS)
+                                                {
+                                                        if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1);
+                                                        else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
+                                                }
+                                                else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
+                                        }
+                                }
+                        }
+                        else
+                        {
+                                //we we're target but already communicating with someone else
+                                if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid))
+                                {
+                                        if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid);
+                                }
+                        }
+                        break;
+                }
+
+                case MAVLINK_MSG_ID_WAYPOINT_COUNT:
+                {
+                        mavlink_waypoint_count_t wpc;
+                        mavlink_msg_waypoint_count_decode(msg, &wpc);
+                        if(wpc.target_system == systemid && wpc.target_component == compid)
+                        {
+                                protocol_timestamp_lastaction = now;
+
+                                if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0))
+                                {
+                                        if (wpc.count > 0)
+                                        {
+                                                if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid);
+                                                if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
+
+                                                current_state = PX_WPP_GETLIST;
+                                                protocol_current_wp_id = 0;
+                                                protocol_current_partner_systemid = msg->sysid;
+                                                protocol_current_partner_compid = msg->compid;
+                                                protocol_current_count = wpc.count;
+
+                                                printf("clearing receive buffer and readying for receiving waypoints\n");
+                                                while(waypoints_receive_buffer->size() > 0)
+                                                {
+                                                        delete waypoints_receive_buffer->back();
+                                                        waypoints_receive_buffer->pop_back();
+                                                }
+
+                                                send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
+                                        }
+                                        else
+                                        {
+                                                if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
+                                        }
+                                }
+                                else
+                                {
+                                        if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state);
+                                        else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
+                                        else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
+                                }
+                        }
+                        break;
+                }
+
+                case MAVLINK_MSG_ID_WAYPOINT:
+                {
+                        mavlink_waypoint_t wp;
+                        mavlink_msg_waypoint_decode(msg, &wp);
+
+                        if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid))
+                        {
+                                protocol_timestamp_lastaction = now;
+
+                                //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
+                                if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count))
+                                {
+                                        if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
+
+                                        current_state = PX_WPP_GETLIST_GETWPS;
+                                        protocol_current_wp_id = wp.seq + 1;
+                                        mavlink_waypoint_t* newwp = new mavlink_waypoint_t;
+                                        memcpy(newwp, &wp, sizeof(mavlink_waypoint_t));
+                                        waypoints_receive_buffer->push_back(newwp);
+
+                                        if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS)
+                                        {
+                                                if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count);
+
+                                                send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
+
+                                                if (current_active_wp_id > waypoints_receive_buffer->size()-1)
+                                                {
+                                                        current_active_wp_id = waypoints_receive_buffer->size() - 1;
+                                                }
+
+                                                // switch the waypoints list
+                                                std::vector<mavlink_waypoint_t*>* waypoints_temp = waypoints;
+                                                waypoints = waypoints_receive_buffer;
+                                                waypoints_receive_buffer = waypoints_temp;
+
+                                                //get the new current waypoint
+                                                uint32_t i;
+                                                for(i = 0; i < waypoints->size(); i++)
+                                                {
+                                                        if (waypoints->at(i)->current == 1)
+                                                        {
+                                                                current_active_wp_id = i;
+                                                                //if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
+                                                                yawReached = false;
+                                                                posReached = false;
+                                                                send_waypoint_current(current_active_wp_id);
+                                                                send_setpoint(current_active_wp_id);
+                                                                timestamp_firstinside_orbit = 0;
+                                                                break;
+                                                        }
+                                                }
+
+                                                if (i == waypoints->size())
+                                                {
+                                                        current_active_wp_id = -1;
+                                                        yawReached = false;
+                                                        posReached = false;
+                                                        timestamp_firstinside_orbit = 0;
+                                                }
+
+                                                current_state = PX_WPP_IDLE;
+                                        }
+                                        else
+                                        {
+                                                send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
+                                        }
+                                }
+                                else
+                                {
+                                        if (current_state == PX_WPP_IDLE)
+                                        {
+                                                //we're done receiving waypoints, answer with ack.
+                                                send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
+                                                printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n");
+                                        }
+                                        if (verbose)
+                                        {
+                                                if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; }
+                                                else if (current_state == PX_WPP_GETLIST)
+                                                {
+                                                        if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
+                                                        else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
+                                                }
+                                                else if (current_state == PX_WPP_GETLIST_GETWPS)
+                                                {
+                                                        if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id);
+                                                        else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
+                                                        else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
+                                                }
+                                                else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
+                                        }
+                                }
+                        }
+                        else
+                        {
+                                //we we're target but already communicating with someone else
+                                if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE)
+                                {
+                                        if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid);
+                                }
+                                else if(wp.target_system == systemid && wp.target_component == compid)
+                                {
+                                        if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
+                                }
+                        }
+                        break;
+                }
+
+                case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
+                {
+                        mavlink_waypoint_clear_all_t wpca;
+                        mavlink_msg_waypoint_clear_all_decode(msg, &wpca);
+
+                        if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE)
+                        {
+                                protocol_timestamp_lastaction = now;
+
+                                if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
+                                while(waypoints->size() > 0)
+                                {
+                                        delete waypoints->back();
+                                        waypoints->pop_back();
+                                }
+                                current_active_wp_id = -1;
+                        }
+                        else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE)
+                        {
+                                if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state);
+                        }
+                        break;
+                }
+
+                default:
+                {
+                        if (debug) printf("Waypoint: received message of unknown type\n");
+                        break;
+                }
+        }
+
+        //check if the current waypoint was reached
+        if ((posReached && /*yawReached &&*/ !idle))
+        {
+                if (current_active_wp_id < waypoints->size())
+                {
+                        mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id);
+
+                        if (timestamp_firstinside_orbit == 0)
+                        {
+                                // Announce that last waypoint was reached
+                                if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq);
+                                send_waypoint_reached(cur_wp->seq);
+                                timestamp_firstinside_orbit = now;
+                        }
+
+                        // check if the MAV was long enough inside the waypoint orbit
+                        //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
+                        if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000)
+                        {
+                                if (cur_wp->autocontinue)
+                                {
+                                        cur_wp->current = 0;
+                                        if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1)
+                                        {
+                                                //the last waypoint was reached, if auto continue is
+                                                //activated restart the waypoint list from the beginning
+                                                current_active_wp_id = 1;
+                                        }
+                                        else
+                                        {
+                                                current_active_wp_id++;
+                                        }
+
+                                        // Fly to next waypoint
+                                        timestamp_firstinside_orbit = 0;
+                                        send_waypoint_current(current_active_wp_id);
+                                        send_setpoint(current_active_wp_id);
+                                        waypoints->at(current_active_wp_id)->current = true;
+                                        posReached = false;
+                                        //yawReached = false;
+                                        if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id);
+                                }
+                        }
+                }
+        }
+        else
+        {
+                timestamp_lastoutside_orbit = now;
+        }
+}

src/comm/MAVLinkSimulationWaypointPlanner.h

+#ifndef MAVLINKSIMULATIONWAYPOINTPLANNER_H
+#define MAVLINKSIMULATIONWAYPOINTPLANNER_H
+
+#include <QObject>
+#include <vector>
+
+#include "MAVLinkSimulationLink.h"
+#include "QGCMAVLink.h"
+
+enum PX_WAYPOINTPLANNER_STATES
+{
+    PX_WPP_IDLE = 0,
+    PX_WPP_SENDLIST,
+    PX_WPP_SENDLIST_SENDWPS,
+    PX_WPP_GETLIST,
+    PX_WPP_GETLIST_GETWPS,
+    PX_WPP_GETLIST_GOTALL
+};
+
+class MAVLinkSimulationWaypointPlanner : public QObject
+{
+    Q_OBJECT
+public:
+    explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid);
+
+signals:
+
+public slots:
+    void handleMessage(const mavlink_message_t& msg);
+
+protected:
+    MAVLinkSimulationLink* link;
+    bool idle;      				///< indicates if the system is following the waypoints or is waiting
+    uint16_t current_active_wp_id;		///< id of current waypoint
+    bool yawReached;						///< boolean for yaw attitude reached
+    bool posReached;						///< boolean for position reached
+    uint64_t timestamp_lastoutside_orbit;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
+    uint64_t timestamp_firstinside_orbit;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
+
+    std::vector<mavlink_waypoint_t*> waypoints1;	///< vector1 that holds the waypoints
+    std::vector<mavlink_waypoint_t*> waypoints2;	///< vector2 that holds the waypoints
+
+    std::vector<mavlink_waypoint_t*>* waypoints;		///< pointer to the currently active waypoint vector
+    std::vector<mavlink_waypoint_t*>* waypoints_receive_buffer;	///< pointer to the receive buffer waypoint vector
+    PX_WAYPOINTPLANNER_STATES current_state;
+    uint16_t protocol_current_wp_id;
+    uint16_t protocol_current_count;
+    uint8_t protocol_current_partner_systemid;
+    uint8_t protocol_current_partner_compid;
+    uint64_t protocol_timestamp_lastaction;
+    unsigned int protocol_timeout;
+    uint64_t timestamp_last_send_setpoint;
+    uint8_t systemid;
+    uint8_t compid;
+    unsigned int setpointDelay;
+    float yawTolerance;
+    bool verbose;
+    bool debug;
+    bool silent;
+
+    void send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type);
+    void send_waypoint_current(uint16_t seq);
+    void send_setpoint(uint16_t seq);
+    void send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count);
+    void send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq);
+    void send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq);
+    void send_waypoint_reached(uint16_t seq);
+    float distanceToSegment(uint16_t seq, float x, float y, float z);
+    float distanceToPoint(uint16_t seq, float x, float y, float z);
+    void mavlink_handler(const mavlink_message_t* msg);
+
+};
+
+#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H

src/comm/SerialLink.cc

@@ -22,7 +22,8 @@
 #endif
 
 
-SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, ParityType parity, DataBitsType dataBits, StopBitsType stopBits)
+SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, ParityType parity, DataBitsType dataBits, StopBitsType stopBits) :
+        port(NULL)
 {
     // Setup settings
     this->porthandle = portname.trimmed();
@@ -39,7 +40,7 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P
     this->id = getNextLinkId();
 
     // *nix (Linux, MacOS tested) serial port support
-    port = new QextSerialPort(porthandle, QextSerialPort::Polling);
+//    port = new QextSerialPort(porthandle, QextSerialPort::Polling);
     //port = new QextSerialPort(porthandle, QextSerialPort::EventDriven);
 
     this->baudrate = baudrate;
@@ -48,12 +49,12 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P
     this->dataBits = dataBits;
     this->stopBits = stopBits;
     this->timeout = 1; ///< The timeout controls how long the program flow should wait for new serial bytes. As we're polling, we don't want to wait at all.
-    port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time
-    port->setBaudRate(baudrate);
-    port->setFlowControl(flow);
-    port->setParity(parity);
-    port->setDataBits(dataBits);
-    port->setStopBits(stopBits);
+//    port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time
+//    port->setBaudRate(baudrate);
+//    port->setFlowControl(flow);
+//    port->setParity(parity);
+//    port->setDataBits(dataBits);
+//    port->setStopBits(stopBits);
 
     // Set the port name
     if (porthandle == "")
@@ -86,9 +87,6 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P
 #endif
 
     loadSettings();
-
-    // Link is setup, register it with link manager
-    LinkManager::instance()->add(this);
 }
 
 SerialLink::~SerialLink()
@@ -153,7 +151,7 @@ void SerialLink::run()
 void SerialLink::checkForBytes()
 {
     /* Check if bytes are available */
-    if(port->isOpen())
+    if(port && port->isOpen() && port->isWritable())
     {
         dataMutex.lock();
         qint64 available = port->bytesAvailable();
@@ -174,7 +172,7 @@ void SerialLink::checkForBytes()
 
 void SerialLink::writeBytes(const char* data, qint64 size)
 {
-    if(port->isOpen())
+    if(port && port->isOpen())
     {
         int b = port->write(data, size);
         qDebug() << "Serial link " << this->getName() << "transmitted" << b << "bytes:";
@@ -201,7 +199,7 @@ void SerialLink::writeBytes(const char* data, qint64 size)
 void SerialLink::readBytes()
 {
     dataMutex.lock();
-    if(port->isOpen())
+    if(port && port->isOpen())
     {
         const qint64 maxLength = 2048;
         char data[maxLength];
@@ -238,7 +236,14 @@ void SerialLink::readBytes()
  **/
 qint64 SerialLink::bytesAvailable()
 {
+    if (port)
+    {
         return port->bytesAvailable();
+    }
+    else
+    {
+        return 0;
+    }
 }
 
 /**
@@ -248,13 +253,17 @@ qint64 SerialLink::bytesAvailable()
  **/
 bool SerialLink::disconnect()
 {
+    if (port)
+    {
         //#if !defined _WIN32 || !defined _WIN64
         /* Block the thread until it returns from run() */
         //#endif
-    dataMutex.lock();
+//        dataMutex.lock();
         port->flush();
         port->close();
-    dataMutex.unlock();
+        delete port;
+        port = NULL;
+//        dataMutex.unlock();
 
         if(this->isRunning()) this->terminate(); //stop running the thread, restart it upon connect
 
@@ -263,8 +272,14 @@ bool SerialLink::disconnect()
 
         emit disconnected();
         emit connected(false);
-
         return ! closed;
+    }
+    else
+    {
+        // No port, so we're disconnected
+        return true;
+    }
+
 }
 
 /**
@@ -274,21 +289,15 @@ bool SerialLink::disconnect()
  **/
 bool SerialLink::connect()
 {
+    if (!isConnected())
+    {
         qDebug() << "CONNECTING LINK: " << __FILE__ << __LINE__ << "with settings" << porthandle << baudrate << dataBits << parity << stopBits;
         if (!this->isRunning())
         {
             this->start(LowPriority);
         }
-    else
-    {
-        if(isConnected())
-        {
-            disconnect();
-        }
-        hardwareConnect();
     }
-
-    return port->isOpen();
+	return true;
 }
 
 /**
@@ -301,6 +310,12 @@ bool SerialLink::connect()
  **/
 bool SerialLink::hardwareConnect()
 {
+    if(port)
+    {
+        port->close();
+        delete port;
+    }
+    port = new QextSerialPort(porthandle, QextSerialPort::Polling);
     QObject::connect(port, SIGNAL(aboutToClose()), this, SIGNAL(disconnected()));
 
     port->open(QIODevice::ReadWrite);
@@ -308,10 +323,9 @@ bool SerialLink::hardwareConnect()
     port->setParity(this->parity);
     port->setStopBits(this->stopBits);
     port->setDataBits(this->dataBits);
+    port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time
 
-    statisticsMutex.lock();
     connectionStartTime = MG::TIME::getGroundTimeNow();
-    statisticsMutex.unlock();
 
     bool connectionUp = isConnected();
     if(connectionUp)
@@ -580,11 +594,9 @@ bool SerialLink::setPortName(QString portName)
     if(portName.trimmed().length() > 0)
     {
         bool reconnect = false;
-        if(isConnected())
-        {
+        if (isConnected()) reconnect = true;
         disconnect();
-            reconnect = true;
-        }
+
         this->porthandle = portName.trimmed();
         setName(tr("serial port ") + portName.trimmed());
 #ifdef _WIN32
@@ -596,15 +608,7 @@ bool SerialLink::setPortName(QString portName)
             this->porthandle = "\\\\.\\" + this->porthandle;
         }
 #endif
-        if(port) delete port;
-        port = new QextSerialPort(porthandle, QextSerialPort::Polling);
 
-        port->setBaudRate(baudrate);
-        port->setFlowControl(flow);
-        port->setParity(parity);
-        port->setDataBits(dataBits);
-        port->setStopBits(stopBits);
-        port->setTimeout(timeout);
         if(reconnect) connect();
         return true;
     }
@@ -619,10 +623,9 @@ bool SerialLink::setBaudRateType(int rateIndex)
 {
     bool reconnect = false;
     bool accepted = true; // This is changed if none of the data rates matches
-    if(isConnected()) {
+    if(isConnected()) reconnect = true;
     disconnect();
-        reconnect = true;
-    }
+
     switch (rateIndex) {
     case 0:
         baudrate = BAUD50;
@@ -705,9 +708,6 @@ bool SerialLink::setBaudRateType(int rateIndex)
         break;
     }
 
-    dataMutex.lock();
-    port->setBaudRate(this->baudrate);
-    dataMutex.unlock();
     if(reconnect) connect();
     return accepted;
 }
@@ -720,9 +720,9 @@ bool SerialLink::setBaudRate(int rate)
     bool accepted = true; // This is changed if none of the data rates matches
     if(isConnected())
     {
-        disconnect();
         reconnect = true;
     }
+    disconnect();
 
     switch (rate)
     {
@@ -807,27 +807,17 @@ bool SerialLink::setBaudRate(int rate)
         break;
     }
 
-    if (port)
-    {
-        port->setBaudRate(this->baudrate);
     if(reconnect) connect();
     return accepted;
-    }
-    else
-    {
-        return false;
-    }
+
 }
 
 bool SerialLink::setFlowType(int flow)
 {
     bool reconnect = false;
     bool accepted = true;
-    if(isConnected())
-    {
+    if(isConnected()) reconnect = true;
     disconnect();
-        reconnect = true;
-    }
 
     switch (flow)
     {
@@ -845,7 +835,7 @@ bool SerialLink::setFlowType(int flow)
         accepted = false;
         break;
     }
-    port->setFlowControl(this->flow);
+
     if(reconnect) connect();
     return accepted;
 }
@@ -854,27 +844,24 @@ bool SerialLink::setParityType(int parity)
 {
     bool reconnect = false;
     bool accepted = true;
-    if(isConnected())
-    {
+    if (isConnected()) reconnect = true;
     disconnect();
-        reconnect = true;
-    }
 
     switch (parity)
     {
-    case PAR_NONE:
+    case (int)PAR_NONE:
         this->parity = PAR_NONE;
         break;
-    case PAR_ODD:
+    case (int)PAR_ODD:
         this->parity = PAR_ODD;
         break;
-    case PAR_EVEN:
+    case (int)PAR_EVEN:
         this->parity = PAR_EVEN;
         break;
-    case PAR_MARK:
+    case (int)PAR_MARK:
         this->parity = PAR_MARK;
         break;
-    case PAR_SPACE:
+    case (int)PAR_SPACE:
         this->parity = PAR_SPACE;
         break;
     default:
@@ -883,15 +870,17 @@ bool SerialLink::setParityType(int parity)
         break;
     }
 
-    port->setParity(this->parity);
-    if(reconnect) connect();
+    if (reconnect) connect();
     return accepted;
 }
 
 
 bool SerialLink::setDataBits(int dataBits)
 {
+    bool reconnect = false;
+    if (isConnected()) reconnect = true;
     bool accepted = true;
+    disconnect();
 
     switch (dataBits)
     {
@@ -913,11 +902,7 @@ bool SerialLink::setDataBits(int dataBits)
         break;
     }
 
-    port->setDataBits(this->dataBits);
-    if(isConnected()) {
-        disconnect();
-        connect();
-    }
+    if(reconnect) connect();
 
     return accepted;
 }
@@ -926,11 +911,8 @@ bool SerialLink::setStopBits(int stopBits)
 {
     bool reconnect = false;
     bool accepted = true;
-    if(isConnected())
-    {
+    if(isConnected()) reconnect = true;
     disconnect();
-        reconnect = true;
-    }
 
     switch (stopBits)
     {
@@ -946,7 +928,6 @@ bool SerialLink::setStopBits(int stopBits)
         break;
     }
 
-    port->setStopBits(this->stopBits);
     if(reconnect) connect();
     return accepted;
 }
@@ -956,21 +937,14 @@ bool SerialLink::setDataBitsType(int dataBits)
     bool reconnect = false;
     bool accepted = false;
 
-    if (isConnected())
-    {
+    if (isConnected()) reconnect = true;
     disconnect();
-        reconnect = true;
-    }
 
     if (dataBits >= (int)DATA_5 && dataBits <= (int)DATA_8)
     {
-        DataBitsType newBits = (DataBitsType) dataBits;
+        this->dataBits = (DataBitsType) dataBits;
 
-        port->setDataBits(newBits);
-        if(reconnect)
-        {
-            connect();
-        }
+        if(reconnect) connect();
         accepted = true;
     }
 
@@ -981,11 +955,8 @@ bool SerialLink::setStopBitsType(int stopBits)
 {
     bool reconnect = false;
     bool accepted = false;
-    if(isConnected())
-    {
+    if(isConnected()) reconnect = true;
     disconnect();
-        reconnect = true;
-    }
 
     if (stopBits >= (int)STOP_1 && dataBits <= (int)STOP_2)
     {

src/uas/SlugsMAV.h

@@ -36,7 +36,9 @@ class SlugsMAV : public UAS
 public:
     SlugsMAV(MAVLinkProtocol* mavlink, int id = 0);
 
-
+#ifdef MAVLINK_ENABLED_SLUGS
+    mavlink_pwm_commands_t* getPwmCommands();
+#endif
 
 public slots:
     /** @brief Receive a MAVLink message from this MAV */

src/uas/UAS.cc

@@ -271,6 +271,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     GAudioOutput::instance()->stopEmergency();
                     GAudioOutput::instance()->say(audiostring);
                 }
+
+                if (state.status == MAV_STATE_POWEROFF)
+                {
+                    emit systemRemoved(this);
+                    emit systemRemoved();
+                }
             }
             break;
         case MAVLINK_MSG_ID_RAW_IMU:

src/uas/UASInterface.h

@@ -262,6 +262,9 @@ signals:
      * @param description longer textual description. Should be however limited to a short text, e.g. 200 chars.
      */
     void statusChanged(UASInterface* uas, QString status, QString description);
+    /** @brief System has been removed / disconnected / shutdown cleanly, remove */
+    void systemRemoved(UASInterface* uas);
+    void systemRemoved();
     /**
      * @brief Received a plain text message from the robot
      * This signal should NOT be used for standard communication, but rather for VERY IMPORTANT

src/ui/CommConfigurationWindow.cc

@@ -68,6 +68,8 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
     // Create configuration action for this link
     // Connect the current UAS
     action = new QAction(QIcon(":/images/devices/network-wireless.svg"), "", link);
+    LinkManager::instance()->add(link);
+    action->setData(LinkManager::instance()->getLinks().indexOf(link));
     setLinkName(link->getName());
     connect(action, SIGNAL(triggered()), this, SLOT(show()));
 
@@ -209,11 +211,12 @@ void CommConfigurationWindow::remove()
         link->disconnect(); //disconnect port, and also calls terminate() to stop the thread
         if (link->isRunning()) link->terminate(); // terminate() the serial thread just in case it is still running
         link->wait(); // wait() until thread is stoped before deleting
-        delete link;
+        link->deleteLater();
     }
     link=NULL;
 
     this->window()->close();
+    this->deleteLater();
 }
 
 void CommConfigurationWindow::connectionState(bool connect)

src/ui/MainWindow.cc

@@ -402,17 +402,17 @@ void MainWindow::buildPxWidgets()
 #ifdef QGC_OSG_ENABLED
     if (!_3DWidget)
     {
-        _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
-        addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
+//        _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
+//        addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
     }
 #endif
 
 #ifdef QGC_OSGEARTH_ENABLED
-    if (!_3DMapWidget)
-    {
-        _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
-        addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
-    }
+//    if (!_3DMapWidget)
+//    {
+//        _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
+//        addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
+//    }
 #endif
 
 #if (defined _MSC_VER) | (defined Q_OS_MAC)
@@ -1220,18 +1220,27 @@ void MainWindow::addLink()
     SerialLink* link = new SerialLink();
     // TODO This should be only done in the dialog itself
 
+    LinkManager::instance()->add(link);
     LinkManager::instance()->addProtocol(link, mavlink);
 
-    CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this);
-
-    ui.menuNetwork->addAction(commWidget->getAction());
+    // Go fishing for this link's configuration window
+    QList<QAction*> actions = ui.menuNetwork->actions();
 
-    commWidget->show();
+    foreach (QAction* act, actions)
+    {
+        if (act->data().toInt() == LinkManager::instance()->getLinks().indexOf(link))
+        {
+            act->trigger();
+            break;
+        }
+    }
 }
 
 void MainWindow::addLink(LinkInterface *link)
 {
+    LinkManager::instance()->add(link);
     LinkManager::instance()->addProtocol(link, mavlink);
+
     CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this);
     ui.menuNetwork->addAction(commWidget->getAction());
 
@@ -1290,7 +1299,11 @@ void MainWindow::UASCreated(UASInterface* uas)
             break;
         }
 
-        ui.menuConnected_Systems->addAction(icon, tr("Select %1 for control").arg(uas->getUASName()), uas, SLOT(setSelected()));
+        QAction* uasAction = new QAction(icon, tr("Select %1 for control").arg(uas->getUASName()), ui.menuConnected_Systems);
+        connect(uas, SIGNAL(systemRemoved()), uasAction, SLOT(deleteLater()));
+        connect(uasAction, SIGNAL(triggered()), uas, SLOT(setSelected()));
+
+        ui.menuConnected_Systems->addAction(uasAction);
 
         // FIXME Should be not inside the mainwindow
         if (debugConsoleDockWidget)

src/ui/MapWidget.cc

@@ -42,14 +42,6 @@ MapWidget::MapWidget(QWidget *parent) :
     QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)}  QDoubleSpinBox { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)}");
     mc->setPen(QGC::colorCyan.darker(400));
 
-
-
-
-
-
-
-
-
     waypointIsDrag = false;
 
     // Accept focus by clicking or keyboard
@@ -267,6 +259,10 @@ MapWidget::MapWidget(QWidget *parent) :
     drawCamBorder = false;
     radioCamera = 10;
 
+//    mc->setZoom(20);
+//    //mc->resize(QSize(7025, 4968));
+//    mc->resize(QSize(3000, 2000));
+
     //mc->setOffscreenImageFactor(15);
 }
 
@@ -650,6 +646,10 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
         uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, ""));
     }
 
+    mc->drawGeometries();
+
+    //mc->updateRequest(QRect(QPoint(0, 0), QPoint(600, 600)));
+
     //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
 
 

src/ui/SerialConfigurationWindow.cc

@@ -260,9 +260,9 @@ userConfigured(false)
         connect(ui.portName, SIGNAL(currentIndexChanged(QString)), this, SLOT(setPortName(QString)));
         connect(ui.baudRate, SIGNAL(activated(int)), this->link, SLOT(setBaudRateType(int)));
         connect(ui.flowControlCheckBox, SIGNAL(toggled(bool)), this, SLOT(enableFlowControl(bool)));
-        connect(ui.parNone, SIGNAL(toggled(bool)), this, SLOT(setParityNone()));
-        connect(ui.parOdd, SIGNAL(toggled(bool)), this, SLOT(setParityOdd()));
-        connect(ui.parEven, SIGNAL(toggled(bool)), this, SLOT(setParityEven()));
+        connect(ui.parNone, SIGNAL(toggled(bool)), this, SLOT(setParityNone(bool)));
+        connect(ui.parOdd, SIGNAL(toggled(bool)), this, SLOT(setParityOdd(bool)));
+        connect(ui.parEven, SIGNAL(toggled(bool)), this, SLOT(setParityEven(bool)));
         connect(ui.dataBitsSpinBox, SIGNAL(valueChanged(int)), this->link, SLOT(setDataBits(int)));
         connect(ui.stopBitsSpinBox, SIGNAL(valueChanged(int)), this->link, SLOT(setStopBits(int)));
 
@@ -283,7 +283,7 @@ userConfigured(false)
             break;
         default:
             // Enforce default: no parity in link
-            setParityNone();
+            setParityNone(true);
             ui.parNone->setChecked(true);
             break;
         }
@@ -320,7 +320,8 @@ userConfigured(false)
     }
 }
 
-SerialConfigurationWindow::~SerialConfigurationWindow() {
+SerialConfigurationWindow::~SerialConfigurationWindow()
+{
 
 }
 
@@ -472,19 +473,19 @@ void SerialConfigurationWindow::enableFlowControl(bool flow)
     }
 }
 
-void SerialConfigurationWindow::setParityNone()
+void SerialConfigurationWindow::setParityNone(bool accept)
 {
-    link->setParityType(0);
+    if (accept) link->setParityType(0);
 }
 
-void SerialConfigurationWindow::setParityOdd()
+void SerialConfigurationWindow::setParityOdd(bool accept)
 {
-    link->setParityType(1);
+    if (accept) link->setParityType(1);
 }
 
-void SerialConfigurationWindow::setParityEven()
+void SerialConfigurationWindow::setParityEven(bool accept)
 {
-    link->setParityType(2);
+    if (accept) link->setParityType(2);
 }
 
 void SerialConfigurationWindow::setPortName(QString port)

src/ui/SerialConfigurationWindow.h

@@ -55,9 +55,9 @@ public:
 public slots:
     void configureCommunication();
     void enableFlowControl(bool flow);
-    void setParityNone();
-    void setParityOdd();
-    void setParityEven();
+    void setParityNone(bool accept);
+    void setParityOdd(bool accept);
+    void setParityEven(bool accept);
     void setPortName(QString port);
     void setLinkName(QString name);
     void setupPortList();

src/ui/SlugsHilSim.cc

@@ -51,12 +51,14 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) :
 
     linksAvailable.clear();
 
+	#ifdef MAVLINK_ENABLED_SLUGS
     memset(&tmpAirData, 0, sizeof(mavlink_air_data_t));
     memset(&tmpAttitudeData, 0, sizeof(mavlink_attitude_t));
     memset(&tmpGpsData, 0, sizeof(mavlink_gps_raw_t));
     memset(&tmpGpsTime, 0, sizeof(mavlink_gps_date_time_t));
     memset(&tmpLocalPositionData, 0, sizeof(mavlink_sensor_bias_t));
     memset(&tmpRawImuData, 0, sizeof(mavlink_raw_imu_t));
+	#endif
 
     foreach (LinkInterface* link, LinkManager::instance()->getLinks())
     {
@@ -263,6 +265,7 @@ void SlugsHilSim::linkSelected(int cbIndex){
 
 void SlugsHilSim::sendMessageToSlugs()
 {
+	#ifdef MAVLINK_ENABLED_SLUGS
     mavlink_message_t msg;
 
     mavlink_msg_local_position_encode(MG::SYSTEM::ID,
@@ -306,11 +309,13 @@ void SlugsHilSim::sendMessageToSlugs()
                                     &tmpGpsTime);
     activeUas->sendMessage(hilLink, msg);
     memset(&msg, 0, sizeof(mavlink_message_t));
+#endif
 }
 
 
 void SlugsHilSim::commandDatagramToSimulink()
 {
+	#ifdef MAVLINK_ENABLED_SLUGS
     //mavlink_pwm_commands_t* pwdC = (static_cast<SlugsMAV*>(activeUas))->getPwmCommands();
 
     mavlink_pwm_commands_t* pwdC;
@@ -335,6 +340,7 @@ void SlugsHilSim::commandDatagramToSimulink()
     setUInt16ToDatagram(data, &i, 11);//value default
 
     txSocket->writeDatagram(data, QHostAddress::Broadcast, ui->ed_txPort->text().toInt());
+#endif
 }
 
 void SlugsHilSim::setUInt16ToDatagram(QByteArray& datagram, unsigned char* pos, uint16_t value)

src/ui/SlugsHilSim.h

@@ -69,9 +69,13 @@ protected:
     mavlink_local_position_t tmpLocalPositionData;
     mavlink_attitude_t tmpAttitudeData;
     mavlink_raw_imu_t tmpRawImuData;
+#ifdef MAVLINK_ENABLED_SLUGS
     mavlink_air_data_t tmpAirData;
+#endif
     mavlink_gps_raw_t tmpGpsData;
+	#ifdef MAVLINK_ENABLED_SLUGS
     mavlink_gps_date_time_t tmpGpsTime;
+#endif
 
 public slots:
 

src/ui/UASView.ui

@@ -181,7 +181,31 @@ QProgressBar::chunk#speedBar {
 
 QProgressBar::chunk#thrustBar {
 	background-color: orange;
-}</string>
+}
+
+QToolTip {
+        background-color: #090909;
+        border: 1px solid #379AC3;
+	border-radius: 3px;
+    color: #DDDDDF;
+}
+
+QMenu {
+    border: 1px solid #379AC3;
+background-color: #050508;
+color: #DDDDDF;
+background-clip: border;
+font-size: 11px;
+}
+
+QMenu::separator {
+     height: 1px;
+     background: #379AC3;
+     margin-top: 8px;
+     margin-bottom: 4px;
+     margin-left: 5px;
+     margin-right: 5px;
+ }</string>
   </property>
   <layout class="QHBoxLayout" name="horizontalLayout_2">
    <property name="spacing">

src/ui/designer/QGCToolWidget.cc

@@ -28,7 +28,17 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent) :
     if (dock)
     {
         dock->setWindowTitle(title);
+        dock->setObjectName(title+"DOCK");
     }
+
+    // Try with parent
+    dock = dynamic_cast<QDockWidget*>(this->parentWidget());
+    if (dock)
+    {
+        dock->setWindowTitle(title);
+        dock->setObjectName(title+"DOCK");
+    }
+
     this->setWindowTitle(title);
 
     QList<UASInterface*> systems = UASManager::instance()->getUASList();

src/ui/uas/UASView.cc

@@ -31,6 +31,7 @@ This file is part of the PIXHAWK project
 #include <cmath>
 #include <QDateTime>
 #include <QDebug>
+#include <QMenu>
 
 #include "QGC.h"
 #include "MG.h"
@@ -42,6 +43,8 @@ This file is part of the PIXHAWK project
 UASView::UASView(UASInterface* uas, QWidget *parent) :
         QWidget(parent),
         startTime(0),
+        lastHeartbeat(0),
+        iconIsRed(true),
         timeRemaining(0),
         chargeLevel(0),
         uas(uas),
@@ -60,6 +63,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
         alt(0),
         groundDistance(0),
         localFrame(false),
+        removeAction(new QAction("Remove this system", this)),
         m_ui(new Ui::UASView)
 {
     m_ui->setupUi(this);
@@ -93,6 +97,10 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
     connect(m_ui->killButton, SIGNAL(clicked()), uas, SLOT(emergencyKILL()));
     connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
 
+    // Allow to delete this widget
+    connect(removeAction, SIGNAL(triggered()), this, SLOT(deleteLater()));
+    connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater()));
+    
     // Set static values
     
     // Name
@@ -230,12 +238,11 @@ void UASView::hideEvent(QHideEvent* event)
 void UASView::receiveHeartbeat(UASInterface* uas)
 {
     Q_UNUSED(uas);
-    QString colorstyle;
     heartbeatColor = QColor(20, 200, 20);
-    colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}",
-                                    heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue());
-    m_ui->heartbeatIcon->setStyleSheet(colorstyle);
-    m_ui->heartbeatIcon->setAutoFillBackground(true);
+    QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 2px; border: 0px; background-color: %1; }");
+    m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name()));
+    lastHeartbeat = QGC::groundTimeUsecs();
+    //m_ui->heartbeatIcon->setAutoFillBackground(true);
 }
 
 /**
@@ -391,6 +398,16 @@ void UASView::updateLoad(UASInterface* uas, double load)
     }
 }
 
+void UASView::contextMenuEvent (QContextMenuEvent* event)
+{
+    if (QGC::groundTimeUsecs() - lastHeartbeat > 1500000)
+    {
+        QMenu menu(this);
+        menu.addAction(removeAction);
+        menu.exec(event->globalPos());
+    }
+}
+
 void UASView::refresh()
 {
     //setUpdatesEnabled(false);
@@ -494,15 +511,44 @@ void UASView::refresh()
     }
     generalUpdateCount++;
 
+    QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 2px; border: 0px; background-color: %1; }");
+
+    if (QGC::groundTimeUsecs() - lastHeartbeat > 1500000)
+    {
+        // CRITICAL CONDITION, NO HEARTBEAT
+
+        if (iconIsRed)
+        {
+            QColor warnColor(Qt::red);
+            m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name()));
+            QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; background-color: %1; }").arg(warnColor.name());
+            m_ui->uasViewFrame->setStyleSheet(style);
+        }
+        else
+        {
+            QColor warnColor(Qt::black);
+            m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name()));
+            QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; background-color: %1; }").arg(warnColor.name());
+            m_ui->uasViewFrame->setStyleSheet(style);
+        }
+        iconIsRed = !iconIsRed;
+    }
+    else
+    {
+        // Break alert once everything is back to normal
+        if (!iconIsRed)
+        {
+            setBackgroundColor();
+            iconIsRed = true;
+        }
+
     // Fade heartbeat icon
     // Make color darker
     heartbeatColor = heartbeatColor.darker(150);
 
-    QString colorstyle;
-    colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 8px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}",
-                                    heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue());
-    m_ui->heartbeatIcon->setStyleSheet(colorstyle);
-    m_ui->heartbeatIcon->setAutoFillBackground(true);
+    //m_ui->heartbeatIcon->setAutoFillBackground(true);
+    m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name()));
+    }
     //setUpdatesEnabled(true);
 
     //setUpdatesEnabled(false);

src/ui/uas/UASView.h

@@ -82,6 +82,8 @@ protected:
     QTimer* refreshTimer;
     QColor heartbeatColor;
     quint64 startTime;
+    quint64 lastHeartbeat;
+    bool iconIsRed;
     int timeRemaining;
     float chargeLevel;
     UASInterface* uas;
@@ -100,6 +102,7 @@ protected:
     float alt;
     float groundDistance;
     bool localFrame;
+    QAction* removeAction;
     static const int updateInterval = 300;
 
 
@@ -112,6 +115,7 @@ protected:
     void showEvent(QShowEvent* event);
     /** @brief Stop widget updating */
     void hideEvent(QHideEvent* event);
+    void contextMenuEvent(QContextMenuEvent* event);
 
 private:
     Ui::UASView *m_ui;