diff --git a/images/earth.html b/images/earth.html
index 066105d..44d3fb2 100644
--- a/images/earth.html
+++ b/images/earth.html
@@ -250,9 +250,9 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
     }
         
                 
-    //planeLoc.setLatitude(lat);
-    //planeLoc.setLongitude(lon);
-    //planeLoc.setAltitude(alt);
+    planeLoc.setLatitude(lat);
+    planeLoc.setLongitude(lon);
+    planeLoc.setAltitude(alt);
 	
 }
 
@@ -297,13 +297,7 @@ function updateFollowAircraft()
 
 	currView.setRange(currViewRange);
 	currView.setTilt(currFollowTilt);
-	currFollowHeading = 0;// 0.9*currFollowHeading+0.1*((Math.atan2(lastLat-currLat, lastLon-currLon)/M_PI)+1.0)*360.0;
-	currView.setHeading(currFollowHeading-0.0);
-	
-	// FIXME Shouldn't update aircraft in here, but works for now
-	planeLoc.setLatitude(lastLat);
-    planeLoc.setLongitude(lastLon);
-    planeLoc.setAltitude(lastAlt);
+	currView.setHeading(currFollowHeading-90.0);
 	
 	ge.getView().setAbstractView(currView);
 }
diff --git a/src/Core.cc b/src/Core.cc
index b9ac4d6..d584698 100644
--- a/src/Core.cc
+++ b/src/Core.cc
@@ -189,6 +189,15 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
             QTimer::singleShot(200, mainWindow, SLOT(close()));
         }
     }
+
+//    forever
+//    {
+//        QGC::SLEEP::msleep(5000);
+//    }
+
+//    mainWindow->close();
+//    mainWindow->deleteLater();
+//    QGC::SLEEP::msleep(200);
 }
 
 /**
diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc
index affe46b..00b1f42 100644
--- a/src/comm/LinkManager.cc
+++ b/src/comm/LinkManager.cc
@@ -143,11 +143,9 @@ bool LinkManager::disconnectLink(LinkInterface* link)
 void LinkManager::removeLink(QObject* link)
 {
     LinkInterface* linkInterface = dynamic_cast<LinkInterface*>(link);
-    // Add link to link list
-    if (links.contains(linkInterface))
+    if (linkInterface)
     {
-        int linkIndex = links.indexOf(linkInterface);
-        links.removeAt(linkIndex);
+        removeLink(linkInterface);
     }
 }
 
@@ -162,6 +160,12 @@ bool LinkManager::removeLink(LinkInterface* link)
                     links.removeAt(i); //remove from link list
                 }
             }
+            // Remove link from protocol map
+            QList<ProtocolInterface* > protocols = protocolLinks.keys(link);
+            foreach (ProtocolInterface* proto, protocols)
+            {
+                protocolLinks.remove(proto, link);
+            }
             return true;
         }
         return false;
diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h
index 36ef64c..6f2f919 100644
--- a/src/comm/LinkManager.h
+++ b/src/comm/LinkManager.h
@@ -34,7 +34,7 @@ This file is part of the PIXHAWK project
 
 #include <QThread>
 #include <QList>
-#include <QMap>
+#include <QMultiMap>
 #include <LinkInterface.h>
 #include <ProtocolInterface.h>
 
@@ -79,7 +79,7 @@ public slots:
 protected:
     LinkManager();
     QList<LinkInterface*> links;
-    QMap<ProtocolInterface*,LinkInterface*> protocolLinks;
+    QMultiMap<ProtocolInterface*,LinkInterface*> protocolLinks;
 
 private:
     static LinkManager* _instance;
diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index aa483e7..350cba4 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -97,6 +97,7 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     maxTimeNoise = 0;
     this->id = getNextLinkId();
     LinkManager::instance()->add(this);
+    QObject::connect(this, SIGNAL(destroyed(QObject*)), LinkManager::instance(), SLOT(removeLink(QObject*)));
 }
 
 MAVLinkSimulationLink::~MAVLinkSimulationLink()
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 8eed5f5..6aa9b81 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -956,18 +956,22 @@ void UAS::forwardMessage(mavlink_message_t message)
 {
     // Emit message on all links that are currently connected
     QList<LinkInterface*>link_list = LinkManager::instance()->getLinksForProtocol(mavlink);
+
     foreach(LinkInterface* link, link_list)
     {
-        SerialLink* serial = dynamic_cast<SerialLink*>(link);
-        if(serial != 0)
+        if (link)
         {
-
-            for(int i=0;i<links->size();i++)
+            SerialLink* serial = dynamic_cast<SerialLink*>(link);
+            if(serial != 0)
             {
-                if(serial != links->at(i))
+
+                for(int i=0;i<links->size();i++)
                 {
-                    qDebug()<<"Forwarding Over link: "<<serial->getName()<<" "<<serial;
-                    sendMessage(serial, message);
+                    if(serial != links->at(i))
+                    {
+                        qDebug()<<"Forwarding Over link: "<<serial->getName()<<" "<<serial;
+                        sendMessage(serial, message);
+                    }
                 }
             }
         }
@@ -1671,11 +1675,21 @@ void UAS::addLink(LinkInterface* link)
     if (!links->contains(link))
     {
         links->append(link);
+        connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
     }
     //links->append(link);
     //qDebug() << link<<" ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK";
 }
 
+void UAS::removeLink(QObject* object)
+{
+    LinkInterface* link = dynamic_cast<LinkInterface*>(object);
+    if (link)
+    {
+        links->removeAt(links->indexOf(link));
+    }
+}
+
 /**
  * @brief Get the links associated with this robot
  *
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 74de151..e260644 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -223,6 +223,8 @@ public slots:
 
     /** @brief Add a link associated with this robot */
     void addLink(LinkInterface* link);
+    /** @brief Remove a link associated with this robot */
+    void removeLink(QObject* object);
 
     /** @brief Receive a message from one of the communication links. */
     virtual void receiveMessage(LinkInterface* link, mavlink_message_t message);
diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc
index 1fb7c7e..235e898 100644
--- a/src/ui/HUD.cc
+++ b/src/ui/HUD.cc
@@ -142,9 +142,9 @@ HUD::HUD(int width, int height, QWidget* parent)
     fill.setColor(2, qRgb(0, 0, 0));
     fill.fill(0);
 
-    //QString imagePath = MG::DIR::getIconDirectory() + "hud-template.png";
-    //qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
-    //fill = QImage(imagePath);
+    QString imagePath = "/Users/user/Desktop/frame0000.png";
+    qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
+    fill = QImage(imagePath);
 
     glImage = QGLWidget::convertToGLFormat(fill);
 
diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc
index 2167425..57d4985 100644
--- a/src/ui/JoystickWidget.cc
+++ b/src/ui/JoystickWidget.cc
@@ -136,5 +136,5 @@ void JoystickWidget::pressKey(int key)
 
 void JoystickWidget::updateStatus(const QString& status)
 {
-
+    m_ui->statusLabel->setText(status);
 }
diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc
index 7624ce2..e27f851 100644
--- a/src/ui/QGCMAVLinkLogPlayer.cc
+++ b/src/ui/QGCMAVLinkLogPlayer.cc
@@ -23,6 +23,9 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare
     ui->setupUi(this);
     ui->gridLayout->setAlignment(Qt::AlignTop);
 
+    // Connect protocol
+    connect(this, SIGNAL(bytesReady(LinkInterface*,QByteArray)), mavlink, SLOT(receiveBytes(LinkInterface*,QByteArray)));
+
     // Setup timer
     connect(&loopTimer, SIGNAL(timeout()), this, SLOT(logLoop()));
 
@@ -31,9 +34,12 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare
     connect(ui->pauseButton, SIGNAL(clicked()), this, SLOT(pause()));
     connect(ui->playButton, SIGNAL(clicked()), this, SLOT(play()));
     connect(ui->speedSlider, SIGNAL(valueChanged(int)), this, SLOT(setAccelerationFactorInt(int)));
+    connect(ui->positionSlider, SIGNAL(valueChanged(int)), this, SLOT(jumpToSliderVal(int)));
+    connect(ui->positionSlider, SIGNAL(sliderPressed()), this, SLOT(pause()));
 
     setAccelerationFactorInt(49);
     ui->speedSlider->setValue(49);
+    ui->positionSlider->setValue(ui->positionSlider->minimum());
 }
 
 QGCMAVLinkLogPlayer::~QGCMAVLinkLogPlayer()
@@ -47,8 +53,10 @@ void QGCMAVLinkLogPlayer::play()
     {
         ui->pauseButton->setChecked(false);
         ui->selectFileButton->setEnabled(false);
-        if (logLink != NULL)
+        if (logLink)
         {
+            logLink->disconnect();
+            LinkManager::instance()->removeLink(logLink);
             delete logLink;
         }
         logLink = new MAVLinkSimulationLink("");
@@ -74,18 +82,44 @@ void QGCMAVLinkLogPlayer::pause()
     loopTimer.stop();
     ui->playButton->setChecked(false);
     ui->selectFileButton->setEnabled(true);
-    delete logLink;
-    logLink = NULL;
+    if (logLink)
+    {
+        logLink->disconnect();
+        LinkManager::instance()->removeLink(logLink);
+        delete logLink;
+        logLink = NULL;
+    }
 }
 
-void QGCMAVLinkLogPlayer::reset()
+bool QGCMAVLinkLogPlayer::reset(int packetIndex)
 {
-    pause();
-    loopCounter = 0;
-    logFile.reset();
-    ui->pauseButton->setChecked(true);
-    ui->progressBar->setValue(0);
-    startTime = 0;
+    // Reset only for valid values
+    if (packetIndex >= 0 && packetIndex <= logFile.size() - (packetLen+timeLen))
+    {
+        bool result = true;
+        pause();
+        loopCounter = 0;
+        logFile.reset();
+        if (!logFile.seek(packetIndex*(timeLen + packetLen)))
+        {
+            // Fallback: Start from scratch
+            logFile.reset();
+            ui->logStatsLabel->setText(tr("Changing packet index failed, back to start."));
+            result = false;
+        }
+
+        ui->pauseButton->setChecked(true);
+        ui->positionSlider->blockSignals(true);
+        int sliderVal = (packetIndex / (double)(logFile.size()/(packetLen+timeLen))) * (ui->positionSlider->maximum() - ui->positionSlider->minimum());
+        ui->positionSlider->setValue(sliderVal);
+        ui->positionSlider->blockSignals(false);
+        startTime = 0;
+        return result;
+    }
+    else
+    {
+        return false;
+    }
 }
 
 void QGCMAVLinkLogPlayer::selectLogFile()
@@ -129,8 +163,7 @@ void QGCMAVLinkLogPlayer::loadLogFile(const QString& file)
     // Ensure that the playback process is stopped
     if (logFile.isOpen())
     {
-        loopTimer.stop();
-        logFile.reset();
+        pause();
         logFile.close();
     }
     logFile.setFileName(file);
@@ -146,15 +179,52 @@ void QGCMAVLinkLogPlayer::loadLogFile(const QString& file)
         logFile.reset();
         startTime = 0;
         ui->logFileNameLabel->setText(tr("%1").arg(logFileInfo.baseName()));
-        ui->logStatsLabel->setText(tr("Log: %2 MB, %3 packets").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))));
+
+        // Get the time interval from the logfile
+        QByteArray timestamp = logFile.read(timeLen);
+
+        // First timestamp
+        quint64 starttime = *((quint64*)(timestamp.constData()));
+
+        // Last timestamp
+        logFile.seek(logFile.size()-packetLen-timeLen);
+        QByteArray timestamp2 = logFile.read(timeLen);
+        quint64 endtime = *((quint64*)(timestamp2.constData()));
+        // Reset everything
+        logFile.reset();
+
+        qDebug() << "Starttime:" << starttime << "End:" << endtime;
+
+        // WARNING: Order matters in this computation
+        int seconds = (endtime - starttime)/1000000;
+        int minutes = seconds / 60;
+        int hours = minutes / 60;
+        seconds -= 60*minutes;
+        minutes -= 60*hours;
+
+        QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
+        ui->logStatsLabel->setText(tr("%2 MB, %3 packets, %4").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))).arg(timelabel));
     }
 }
 
-union log64
+/**
+ * Jumps to the current percentage of the position slider
+ */
+void QGCMAVLinkLogPlayer::jumpToSliderVal(int slidervalue)
 {
-    quint64 q;
-    const char* b;
-};
+    loopTimer.stop();
+    // Set the logfile to the correct percentage and
+    // align to the timestamp values
+    int packetCount = logFile.size() / (packetLen + timeLen);
+    int packetIndex = (packetCount - 1) * (slidervalue / (double)(ui->positionSlider->maximum() - ui->positionSlider->minimum()));
+
+    // Do only accept valid jumps
+    if (reset(packetIndex))
+    {
+        ui->logStatsLabel->setText(tr("Jumped to packet %1").arg(packetIndex));
+        //qDebug() << "SET POSITION TO PACKET:" << packetIndex;
+    }
+}
 
 /**
  * This function is the "mainloop" of the log player, reading one line
@@ -166,8 +236,6 @@ union log64
  */
 void QGCMAVLinkLogPlayer::logLoop()
 {
-    const int packetLen = MAVLINK_MAX_PACKET_LEN;
-    const int timeLen = sizeof(quint64);
     bool ok;
 
     // First check initialization
@@ -207,7 +275,7 @@ void QGCMAVLinkLogPlayer::logLoop()
     QByteArray packet = chunk.mid(0, packetLen);
 
     // Emit this packet
-    mavlink->receiveBytes(logLink, packet);
+    emit bytesReady(logLink, packet);
 
     // Check if reached end of file before reading next timestamp
     if (chunk.length() < timeLen+packetLen || logFile.atEnd())
@@ -217,7 +285,6 @@ void QGCMAVLinkLogPlayer::logLoop()
 
         QString status = tr("Reached end of MAVLink log file.");
         ui->logStatsLabel->setText(status);
-        ui->progressBar->setValue(100);
         MainWindow::instance()->showStatusMessage(status);
         return;
     }
@@ -265,9 +332,11 @@ void QGCMAVLinkLogPlayer::logLoop()
         if (loopCounter % 40 == 0)
         {
             QFileInfo logFileInfo(logFile);
-            int progress = 100.0f*(loopCounter/(float)(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))));
+            int progress = (ui->positionSlider->maximum()-ui->positionSlider->minimum())*(logFile.pos()/static_cast<float>(logFileInfo.size()));
             //qDebug() << "Progress:" << progress;
-            ui->progressBar->setValue(progress);
+            ui->positionSlider->blockSignals(true);
+            ui->positionSlider->setValue(progress);
+            ui->positionSlider->blockSignals(false);
         }
         loopCounter++;
         // Ui update: Only every 20 messages
diff --git a/src/ui/QGCMAVLinkLogPlayer.h b/src/ui/QGCMAVLinkLogPlayer.h
index eb0ea53..1ec0603 100644
--- a/src/ui/QGCMAVLinkLogPlayer.h
+++ b/src/ui/QGCMAVLinkLogPlayer.h
@@ -5,6 +5,7 @@
 #include <QFile>
 
 #include "MAVLinkProtocol.h"
+#include "LinkInterface.h"
 #include "MAVLinkSimulationLink.h"
 
 namespace Ui {
@@ -32,16 +33,22 @@ public slots:
     /** @brief Pause the logfile */
     void pause();
     /** @brief Reset the logfile */
-    void reset();
+    bool reset(int packetIndex=0);
     /** @brief Select logfile */
     void selectLogFile();
     /** @brief Load log file */
     void loadLogFile(const QString& file);
+    /** @brief Jump to a position in the logfile */
+    void jumpToSliderVal(int slidervalue);
     /** @brief The logging mainloop */
     void logLoop();
     /** @brief Set acceleration factor in percent */
     void setAccelerationFactorInt(int factor);
 
+signals:
+    /** @brief Send ready bytes */
+    void bytesReady(LinkInterface* link, const QByteArray& bytes);
+
 protected:
     int lineCounter;
     int totalLines;
@@ -54,6 +61,8 @@ protected:
     QFile logFile;
     QTimer loopTimer;
     int loopCounter;
+    static const int packetLen = MAVLINK_MAX_PACKET_LEN;
+    static const int timeLen = sizeof(quint64);
     void changeEvent(QEvent *e);
 
 private:
diff --git a/src/ui/QGCMAVLinkLogPlayer.ui b/src/ui/QGCMAVLinkLogPlayer.ui
index 780c3ba..3dd76b8 100644
--- a/src/ui/QGCMAVLinkLogPlayer.ui
+++ b/src/ui/QGCMAVLinkLogPlayer.ui
@@ -7,13 +7,13 @@
     <x>0</x>
     <y>0</y>
     <width>407</width>
-    <height>144</height>
+    <height>152</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout" columnstretch="1,10,10,1,1,0">
+  <layout class="QGridLayout" name="gridLayout" columnstretch="1,0,0,0,0,0">
    <property name="horizontalSpacing">
     <number>12</number>
    </property>
@@ -101,22 +101,6 @@
      </property>
     </widget>
    </item>
-   <item row="4" column="0" colspan="6">
-    <widget class="QProgressBar" name="progressBar">
-     <property name="toolTip">
-      <string>Percent of the logfile replayed</string>
-     </property>
-     <property name="statusTip">
-      <string>Percent of the logfile replayed</string>
-     </property>
-     <property name="whatsThis">
-      <string>Percent of the logfile replayed</string>
-     </property>
-     <property name="value">
-      <number>0</number>
-     </property>
-    </widget>
-   </item>
    <item row="1" column="0">
     <widget class="QLabel" name="speedLabel">
      <property name="toolTip">
@@ -156,6 +140,22 @@
      </property>
     </widget>
    </item>
+   <item row="4" column="0" colspan="6">
+    <widget class="QSlider" name="positionSlider">
+     <property name="maximum">
+      <number>10000</number>
+     </property>
+     <property name="pageStep">
+      <number>100</number>
+     </property>
+     <property name="tracking">
+      <bool>false</bool>
+     </property>
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources>
diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc
index 8920f76..dc0e187 100644
--- a/src/ui/map/MAV2DIcon.cc
+++ b/src/ui/map/MAV2DIcon.cc
@@ -46,50 +46,50 @@ void MAV2DIcon::setYaw(float yaw)
 void MAV2DIcon::drawIcon(QPen* pen)
 {
 
-    mypixmap = new QPixmap(radius+1, radius+1);
-    mypixmap->fill(Qt::transparent);
-    QPainter painter(mypixmap);
+//    mypixmap = new QPixmap(radius+1, radius+1);
+//    mypixmap->fill(Qt::transparent);
+//    QPainter painter(mypixmap);
 
-    // DRAW WAYPOINT
-    QPointF p(radius/2, radius/2);
+//    // DRAW WAYPOINT
+//    QPointF p(radius/2, radius/2);
 
-    float waypointSize = radius;
-    QPolygonF poly(4);
-    // Top point
-    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
-    // Right point
-    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
-    // Bottom point
-    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
-    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
+//    float waypointSize = radius;
+//    QPolygonF poly(4);
+//    // Top point
+//    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
+//    // Right point
+//    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
+//    // Bottom point
+//    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
+//    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
 
-//    // Select color based on if this is the current waypoint
-//    if (list.at(i)->getCurrent())
+////    // Select color based on if this is the current waypoint
+////    if (list.at(i)->getCurrent())
+////    {
+////        color = QGC::colorCyan;//uas->getColor();
+////        pen.setWidthF(refLineWidthToPen(0.8f));
+////    }
+////    else
+////    {
+////        color = uas->getColor();
+////        pen.setWidthF(refLineWidthToPen(0.4f));
+////    }
+
+//    //pen.setColor(color);
+//    if (pen)
 //    {
-//        color = QGC::colorCyan;//uas->getColor();
-//        pen.setWidthF(refLineWidthToPen(0.8f));
+//        pen->setWidthF(2);
+//        painter.setPen(*pen);
 //    }
 //    else
 //    {
-//        color = uas->getColor();
-//        pen.setWidthF(refLineWidthToPen(0.4f));
+//        QPen pen2(Qt::red);
+//        pen2.setWidth(2);
+//        painter.setPen(pen2);
 //    }
+//    painter.setBrush(Qt::NoBrush);
 
-    //pen.setColor(color);
-    if (pen)
-    {
-        pen->setWidthF(2);
-        painter.setPen(*pen);
-    }
-    else
-    {
-        QPen pen2(Qt::red);
-        pen2.setWidth(2);
-        painter.setPen(pen2);
-    }
-    painter.setBrush(Qt::NoBrush);
-
-    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
-    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
-    painter.drawPolygon(poly);
+//    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
+//    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
+//    painter.drawPolygon(poly);
 }