diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc
index 45fe505a366edb356ee33a2b1a8f262dcd3331cb..b95e7547086c6b559e69d3a1b6e09550063ed407 100644
--- a/src/ui/MapWidget.cc
+++ b/src/ui/MapWidget.cc
@@ -593,21 +593,12 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
             uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, QString("lat: %1 lon: %2").arg(lat, lon)));
         }
 
-        //    points.append(new CirclePoint(8.275145, 50.016992, 15, "Wiesbaden-Mainz-Kastel, Johannes-Goßner-Straße", Point::Middle, pointpen));
-        //    points.append(new CirclePoint(8.270476, 50.021426, 15, "Wiesbaden-Mainz-Kastel, Ruthof", Point::Middle, pointpen));
-        //    // "Blind" Points
-        //    points.append(new Point(8.266445, 50.025913, "Wiesbaden-Mainz-Kastel, Mudra Kaserne"));
-        //    points.append(new Point(8.260378, 50.030345, "Wiesbaden-Mainz-Amoneburg, Dyckerhoffstraße"));
 
         // Connect click events of the layer to this object
         // connect(osmLayer, SIGNAL(geometryClicked(Geometry*, QPoint)),
         //                  this, SLOT(geometryClicked(Geometry*, QPoint)));
 
         // Sets the view to the interesting area
-        //QList<QPointF> view;
-        //view.append(QPointF(8.24764, 50.0319));
-        //view.append(QPointF(8.28412, 49.9998));
-        // mc->setView(view);
         updatePosition(0, lat, lon);
     }
 }
diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc
index 573149a853e851695af11d316bc059bd36f49ab8..a87962c052174e0c91b751a8b5f48ddd53a37b43 100644
--- a/src/ui/map/MAV2DIcon.cc
+++ b/src/ui/map/MAV2DIcon.cc
@@ -14,7 +14,10 @@ MAV2DIcon::MAV2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen*
 {
     int radius = 10;
     size = QSize(radius, radius);
-    drawIcon(pen);
+    if (pen)
+    {
+        drawIcon(pen);
+    }
 }
 
 MAV2DIcon::~MAV2DIcon()
@@ -24,8 +27,11 @@ MAV2DIcon::~MAV2DIcon()
 
 void MAV2DIcon::setPen(QPen* pen)
 {
-    mypen = pen;
-    drawIcon(pen);
+    if (pen)
+    {
+        mypen = pen;
+        drawIcon(pen);
+    }
 }
 
 /**
@@ -38,46 +44,49 @@ void MAV2DIcon::setYaw(float yaw)
 
 void MAV2DIcon::drawIcon(QPen* pen)
 {
-    mypixmap = new QPixmap(radius+1, radius+1);
-    mypixmap->fill(Qt::transparent);
-    QPainter painter(mypixmap);
+    if (pen)
+    {
+        mypixmap = new QPixmap(radius+1, radius+1);
+        mypixmap->fill(Qt::transparent);
+        QPainter painter(mypixmap);
 
-    // DRAW MICRO AIR VEHICLE
-    QPointF p(radius/2, radius/2);
+        // DRAW MICRO AIR VEHICLE
+        QPointF p(radius/2, radius/2);
 
-    float waypointSize = radius;
-    QPolygonF poly(3);
-    // 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()-waypointSize/2.0f));
-    // Left point
-    poly.replace(2, QPointF(p.x()+waypointSize/2.0f, p.y() + waypointSize/2.0f));
+        float waypointSize = radius;
+        QPolygonF poly(3);
+        // 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()-waypointSize/2.0f));
+        // Left point
+        poly.replace(2, QPointF(p.x()+waypointSize/2.0f, p.y() + waypointSize/2.0f));
 
-//    // 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));
-//    }
+        //    // 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)
-    {
-        pen->setWidthF(2);
-        painter.setPen(*pen);
-    }
-    else
-    {
-        QPen pen2(Qt::yellow);
-        pen2.setWidth(2);
-        painter.setPen(pen2);
+        //pen.setColor(color);
+        if (pen)
+        {
+            pen->setWidthF(2);
+            painter.setPen(*pen);
+        }
+        else
+        {
+            QPen pen2(Qt::yellow);
+            pen2.setWidth(2);
+            painter.setPen(pen2);
+        }
+        painter.setBrush(Qt::NoBrush);
+        painter.drawPolygon(poly);
     }
-    painter.setBrush(Qt::NoBrush);
-    painter.drawPolygon(poly);
 }