diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 634ad6dfde7a490cd42fd9c0d0db3fbe11a8beb9..b3207fb7c7e83dcb45c8163522e2e4a6752372fc 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -213,7 +213,8 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/UASControlParameters.ui \
     src/ui/mission/QGCMissionDoWidget.ui \
     src/ui/mission/QGCMissionConditionWidget.ui \
-    src/ui/map/QGCMapToolbar.ui
+    src/ui/map/QGCMapTool.ui \
+    src/ui/map/QGCMapToolBar.ui
 INCLUDEPATH += src \
     src/ui \
     src/ui/linechart \
@@ -328,7 +329,8 @@ HEADERS += src/MG.h \
     src/ui/map/MAV2DIcon.h \
     src/ui/map/Waypoint2DIcon.h \
     src/ui/mavlink/QGCMAVLinkTextEdit.h \
-    src/ui/map/QGCMapToolbar.h
+    src/ui/map/QGCMapTool.h \
+    src/ui/map/QGCMapToolBar.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|win32-msvc2008::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
@@ -456,7 +458,8 @@ SOURCES += src/main.cc \
     src/ui/map/MAV2DIcon.cc \
     src/ui/map/Waypoint2DIcon.cc \
     src/ui/mavlink/QGCMAVLinkTextEdit.cc \
-    src/ui/map/QGCMapToolbar.cc
+    src/ui/map/QGCMapTool.cc \
+    src/ui/map/QGCMapToolBar.cc
 macx|win32-msvc2008::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc
 contains(DEPENDENCIES_PRESENT, osg) { 
     message("Including sources for OpenSceneGraph")
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index 12cf26e4238c3a221d0c40e151962f2c2aeff804..8eceb5c72f1d36c5734e539c1db7e9bb8a7d211d 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -29,7 +29,7 @@
 #include "QGCToolWidget.h"
 #include "QGCMAVLinkLogPlayer.h"
 #include "QGCSettingsWidget.h"
-#include "QGCMapWidget.h"
+#include "QGCMapTool.h"
 
 #ifdef QGC_OSG_ENABLED
 #include "Q3DWidgetFactory.h"
@@ -352,7 +352,7 @@ void MainWindow::buildCommonWidgets()
     // Center widgets
     if (!mapWidget)
     {
-        mapWidget = new QGCMapWidget(this);
+        mapWidget = new QGCMapTool(this);
         addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP);
     }
 
diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h
index db0c38a30e8a3c5d580152b69a3008f1c4278742..055072c63104062ead72633e76f0f3ca68b40833 100644
--- a/src/ui/MainWindow.h
+++ b/src/ui/MainWindow.h
@@ -76,6 +76,8 @@ This file is part of the QGROUNDCONTROL project
 #include "SlugsPadCameraControl.h"
 #include "UASControlParameters.h"
 
+class QGCMapTool;
+
 /**
  * @brief Main Application Window
  *
@@ -376,7 +378,7 @@ protected:
 
     QPointer<HUD> hudWidget;
 
-    QPointer<mapcontrol::OPMapWidget> mapWidget;
+    QPointer<QGCMapTool> mapWidget;
     QPointer<XMLCommProtocolWidget> protocolWidget;
     QPointer<QGCDataPlot2D> dataplotWidget;
 #ifdef QGC_OSG_ENABLED
diff --git a/src/ui/map/QGCMapTool.cc b/src/ui/map/QGCMapTool.cc
new file mode 100644
index 0000000000000000000000000000000000000000..1ba9ee0604b177040c30a5295686e870c2f13556
--- /dev/null
+++ b/src/ui/map/QGCMapTool.cc
@@ -0,0 +1,17 @@
+#include "QGCMapTool.h"
+#include "ui_QGCMapTool.h"
+
+QGCMapTool::QGCMapTool(QWidget *parent) :
+    QWidget(parent),
+    ui(new Ui::QGCMapTool)
+{
+    ui->setupUi(this);
+
+    // Connect map and toolbar
+    ui->toolBar->setMap(ui->map);
+}
+
+QGCMapTool::~QGCMapTool()
+{
+    delete ui;
+}
diff --git a/src/ui/map/QGCMapTool.h b/src/ui/map/QGCMapTool.h
new file mode 100644
index 0000000000000000000000000000000000000000..75a0a73990bacb1886c27a850a08781b4cad017e
--- /dev/null
+++ b/src/ui/map/QGCMapTool.h
@@ -0,0 +1,22 @@
+#ifndef QGCMAPTOOL_H
+#define QGCMAPTOOL_H
+
+#include <QWidget>
+
+namespace Ui {
+    class QGCMapTool;
+}
+
+class QGCMapTool : public QWidget
+{
+    Q_OBJECT
+
+public:
+    explicit QGCMapTool(QWidget *parent = 0);
+    ~QGCMapTool();
+
+private:
+    Ui::QGCMapTool *ui;
+};
+
+#endif // QGCMAPTOOL_H
diff --git a/src/ui/map/QGCMapTool.ui b/src/ui/map/QGCMapTool.ui
new file mode 100644
index 0000000000000000000000000000000000000000..b3b9062ac97d31fed065c3bd371ea88d2cef2b5c
--- /dev/null
+++ b/src/ui/map/QGCMapTool.ui
@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>QGCMapTool</class>
+ <widget class="QWidget" name="QGCMapTool">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout" stretch="100,1">
+   <property name="spacing">
+    <number>0</number>
+   </property>
+   <property name="margin">
+    <number>0</number>
+   </property>
+   <item>
+    <widget class="QGCMapWidget" name="map" native="true"/>
+   </item>
+   <item>
+    <widget class="QGCMapToolBar" name="toolBar" native="true"/>
+   </item>
+  </layout>
+ </widget>
+ <customwidgets>
+  <customwidget>
+   <class>QGCMapToolBar</class>
+   <extends>QWidget</extends>
+   <header>QGCMapToolBar.h</header>
+   <container>1</container>
+  </customwidget>
+  <customwidget>
+   <class>QGCMapWidget</class>
+   <extends>QWidget</extends>
+   <header>QGCMapWidget.h</header>
+   <container>1</container>
+  </customwidget>
+ </customwidgets>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/src/ui/map/QGCMapToolBar.cc b/src/ui/map/QGCMapToolBar.cc
new file mode 100644
index 0000000000000000000000000000000000000000..2d70fd316572cf1c86400aa838062d124cb1a8da
--- /dev/null
+++ b/src/ui/map/QGCMapToolBar.cc
@@ -0,0 +1,27 @@
+#include "QGCMapToolBar.h"
+#include "QGCMapWidget.h"
+#include "ui_QGCMapToolBar.h"
+
+QGCMapToolBar::QGCMapToolBar(QWidget *parent) :
+    QWidget(parent),
+    map(NULL),
+    ui(new Ui::QGCMapToolBar)
+{
+    ui->setupUi(this);
+}
+
+void QGCMapToolBar::setMap(QGCMapWidget* map)
+{
+    this->map = map;
+
+    if (map)
+    {
+        connect(ui->goToButton, SIGNAL(clicked()), map, SLOT(showGoToDialog()));
+        connect(ui->goHomeButton, SIGNAL(clicked()), map, SLOT(goHome()));
+    }
+}
+
+QGCMapToolBar::~QGCMapToolBar()
+{
+    delete ui;
+}
diff --git a/src/ui/map/QGCMapToolbar.cc b/src/ui/map/QGCMapToolbar.cc
deleted file mode 100644
index 4b51027e91a5d5a368f832d8c63658bbc3a96824..0000000000000000000000000000000000000000
--- a/src/ui/map/QGCMapToolbar.cc
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "QGCMapToolbar.h"
-#include "ui_QGCMapToolbar.h"
-
-QGCMapToolbar::QGCMapToolbar(QWidget *parent) :
-    QWidget(parent),
-    ui(new Ui::QGCMapToolbar)
-{
-    ui->setupUi(this);
-}
-
-QGCMapToolbar::~QGCMapToolbar()
-{
-    delete ui;
-}
diff --git a/src/ui/map/QGCMapToolbar.h b/src/ui/map/QGCMapToolbar.h
index ecd580bce4dd67a1b72f1c1d5b8cafa48051f8e6..ea773f0305f2f38c9fb02d92ee9c99933d31045e 100644
--- a/src/ui/map/QGCMapToolbar.h
+++ b/src/ui/map/QGCMapToolbar.h
@@ -3,20 +3,27 @@
 
 #include <QWidget>
 
+class QGCMapWidget;
+
 namespace Ui {
-    class QGCMapToolbar;
+    class QGCMapToolBar;
 }
 
-class QGCMapToolbar : public QWidget
+class QGCMapToolBar : public QWidget
 {
     Q_OBJECT
 
 public:
-    explicit QGCMapToolbar(QWidget *parent = 0);
-    ~QGCMapToolbar();
+    explicit QGCMapToolBar(QWidget *parent = 0);
+    ~QGCMapToolBar();
+
+    void setMap(QGCMapWidget* map);
+
+protected:
+    QGCMapWidget* map;
 
 private:
-    Ui::QGCMapToolbar *ui;
+    Ui::QGCMapToolBar *ui;
 };
 
 #endif // QGCMAPTOOLBAR_H
diff --git a/src/ui/map/QGCMapToolbar.ui b/src/ui/map/QGCMapToolbar.ui
index 7e779eeb8915fe7934226ebf9d5750e0cbbd8b2e..f11c923e7498d1f794eb354430eaf57b924f2daa 100644
--- a/src/ui/map/QGCMapToolbar.ui
+++ b/src/ui/map/QGCMapToolbar.ui
@@ -1,13 +1,13 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <ui version="4.0">
- <class>QGCMapToolbar</class>
- <widget class="QWidget" name="QGCMapToolbar">
+ <class>QGCMapToolBar</class>
+ <widget class="QWidget" name="QGCMapToolBar">
   <property name="geometry">
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>617</width>
-    <height>28</height>
+    <width>669</width>
+    <height>35</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -18,19 +18,19 @@
     <number>12</number>
    </property>
    <property name="margin">
-    <number>2</number>
+    <number>4</number>
    </property>
    <item>
-    <widget class="QPushButton" name="goToLatLonButton">
+    <widget class="QLabel" name="posLabel">
      <property name="text">
-      <string>Lat/Lon</string>
+      <string>00.00 00.00</string>
      </property>
     </widget>
    </item>
    <item>
-    <widget class="QPushButton" name="goToHomeButton">
+    <widget class="QPushButton" name="goToButton">
      <property name="text">
-      <string>Home</string>
+      <string>Lat/Lon</string>
      </property>
     </widget>
    </item>
@@ -39,25 +39,35 @@
      <property name="text">
       <string>Edit</string>
      </property>
-     <property name="checkable">
-      <bool>true</bool>
-     </property>
     </widget>
    </item>
    <item>
-    <widget class="QPushButton" name="setHomeButton">
+    <widget class="QPushButton" name="goHomeButton">
      <property name="text">
-      <string>Set Home</string>
+      <string>Go Home</string>
      </property>
     </widget>
    </item>
    <item>
-    <widget class="QLabel" name="latLonLabel">
+    <widget class="QCheckBox" name="followCheckBox">
      <property name="text">
-      <string>Mouse Coordinate</string>
+      <string>Follow</string>
      </property>
     </widget>
    </item>
+   <item>
+    <spacer name="horizontalSpacer">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>242</width>
+       <height>20</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
   </layout>
  </widget>
  <resources/>
diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc
index 547696b453a7fedaf88ec2d81d594a27b85c93b1..ded9b140c9a3afaffc6b61644ae76e6eaf56443e 100644
--- a/src/ui/map/QGCMapWidget.cc
+++ b/src/ui/map/QGCMapWidget.cc
@@ -342,6 +342,11 @@ void QGCMapWidget::updateHomePosition(double latitude, double longitude, double
     SetShowHome(true);                      // display the HOME position on the map
 }
 
+void QGCMapWidget::goHome()
+{
+    SetCurrentPosition(Home->Coord());
+}
+
 /**
  * Limits the update rate on the specified interval. Set to zero (0) to run at maximum
  * telemetry speed. Recommended rate is 2 s.
@@ -406,7 +411,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
         // this has to be changed to accept read-only updates from other systems as well.
         if (UASManager::instance()->getUASForId(uas)->getWaypointManager() == currWPManager) {
             // Only accept waypoints in global coordinate frame
-            if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->isNavigationType()) {
+            if (((wp->getFrame() == MAV_FRAME_GLOBAL) || (wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) && wp->isNavigationType()) {
                 // We're good, this is a global waypoint
 
                 // Get the index of this waypoint
@@ -481,89 +486,45 @@ void QGCMapWidget::updateWaypointList(int uas)
     // Currently only accept waypoint updates from the UAS in focus
     // this has to be changed to accept read-only updates from other systems as well.
     if (UASManager::instance()->getUASForId(uas)->getWaypointManager() == currWPManager) {
-    qDebug() << "UPDATING WP LIST";
-    // Get current WP list
-    // compare to local WP maps and
-    // update / remove all WPs
+        qDebug() << "UPDATING WP LIST";
+        // Get current WP list
+        // compare to local WP maps and
+        // update / remove all WPs
 
-//    int localCount = waypointsToIcons.count();
+        //    int localCount = waypointsToIcons.count();
 
-    // ORDER MATTERS HERE!
-    // TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE
+        // ORDER MATTERS HERE!
+        // TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE
 
-    // Delete first all old waypoints
-    // this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
-    QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
-    foreach (Waypoint* wp, waypointsToIcons.keys())
-    {
-        // Get icon to work on
-        mapcontrol::WayPointItem* icon = waypointsToIcons.value(wp);
-        if (!wps.contains(wp))
+        // Delete first all old waypoints
+        // this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
+        QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
+        foreach (Waypoint* wp, waypointsToIcons.keys())
         {
-            waypointsToIcons.remove(wp);
-            iconsToWaypoints.remove(icon);
-            WPDelete(icon);
+            // Get icon to work on
+            mapcontrol::WayPointItem* icon = waypointsToIcons.value(wp);
+            if (!wps.contains(wp))
+            {
+                waypointsToIcons.remove(wp);
+                iconsToWaypoints.remove(icon);
+                WPDelete(icon);
+            }
         }
-    }
 
-    // Update all existing waypoints
-    foreach (Waypoint* wp, waypointsToIcons.keys())
-    {
-        // Update remaining waypoints
-        updateWaypoint(uas, wp);
-    }
+        // Update all existing waypoints
+        foreach (Waypoint* wp, waypointsToIcons.keys())
+        {
+            // Update remaining waypoints
+            updateWaypoint(uas, wp);
+        }
 
-    // Update all potentially new waypoints
-    foreach (Waypoint* wp, wps)
-    {
-        // Update / add only if new
-        if (!waypointsToIcons.contains(wp)) updateWaypoint(uas, wp);
+        // Update all potentially new waypoints
+        foreach (Waypoint* wp, wps)
+        {
+            // Update / add only if new
+            if (!waypointsToIcons.contains(wp)) updateWaypoint(uas, wp);
+        }
     }
-
-//    int globalCount = uasInstance->getWaypointManager()->getGlobalFrameAndNavTypeCount();
-
-//        // Get already existing waypoints
-//        UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
-//        if (uasInstance) {
-//            // Get update rect of old content, this is what will be redrawn
-//            // in the last step
-//            QRect updateRect = waypointPath->boundingBox().toRect();
-
-//            // Get all waypoints, including non-global waypoints
-//            QVector<Waypoint*> wpList = uasInstance->getWaypointManager()->getWaypointList();
-
-//            // Clear if necessary
-//            if (wpList.count() == 0) {
-//                clearWaypoints(uas);
-//                return;
-//            }
-
-//            // Trim internal list to number of global waypoints in the waypoint manager list
-//            int overSize = waypointPath->points().count() - uasInstance->getWaypointManager()->getGlobalFrameAndNavTypeCount();
-//            if (overSize > 0) {
-//                // Remove n waypoints at the end of the list
-//                // the remaining waypoints will be updated
-//                // in the next step
-//                for (int i = 0; i < overSize; ++i) {
-//                    wps.removeLast();
-//                    mc->layer("Waypoints")->removeGeometry(wpIcons.last());
-//                    wpIcons.removeLast();
-//                    waypointPath->points().removeLast();
-//                }
-//            }
-
-//            // Load all existing waypoints into map view
-//            foreach (Waypoint* wp, wpList) {
-//                // Block map draw updates, since we update everything in the next step
-//                // but update internal data structures.
-//                // Please note that updateWaypoint() ignores non-global waypoints
-//                updateWaypoint(mav->getUASID(), wp, false);
-//            }
-
-//            // Update view
-//            if (isVisible()) mc->updateRequest(updateRect);
-//        }
-                        }
 }
 
 
diff --git a/src/ui/map/QGCMapWidget.h b/src/ui/map/QGCMapWidget.h
index 7111aded207abd7f7cefb8ea143ac63ceb675ee7..a9fab564e19dbf5865efc1b1a0f19a376ddb1895 100644
--- a/src/ui/map/QGCMapWidget.h
+++ b/src/ui/map/QGCMapWidget.h
@@ -43,6 +43,8 @@ public slots:
     void activeUASSet(UASInterface* uas);
     /** @brief Show a dialog to jump to given GPS coordinates */
     void showGoToDialog();
+    /** @brief Jump to the home position on the map */
+    void goHome();
     /** @brief Update this waypoint for this UAS */
     void updateWaypoint(int uas, Waypoint* wp);
     /** @brief Update the whole waypoint */
diff --git a/src/ui/map/Waypoint2DIcon.cc b/src/ui/map/Waypoint2DIcon.cc
index ac7dfade7c05f639dea0696ab7e004cbb5a80985..f4b30a6d2aa646f57dd7f69e80930b29bc047c5f 100644
--- a/src/ui/map/Waypoint2DIcon.cc
+++ b/src/ui/map/Waypoint2DIcon.cc
@@ -82,8 +82,8 @@ void Waypoint2DIcon::updateWaypoint()
             int oldWidth = oldSize.width() + 20;
             int oldHeight = oldSize.height() + 20;
             map->update(this->x()-10, this->y()-10, oldWidth, oldHeight);
-            qDebug() << "UPDATING DUE TO SMALLER SIZE";
-            qDebug() << "X:" << this->x()-1 << "Y:" << this->y()-1 << "WIDTH:" << oldWidth << "HEIGHT:" << oldHeight;
+            //qDebug() << "UPDATING DUE TO SMALLER SIZE";
+            //qDebug() << "X:" << this->x()-1 << "Y:" << this->y()-1 << "WIDTH:" << oldWidth << "HEIGHT:" << oldHeight;
         }
         else
         {
@@ -91,7 +91,6 @@ void Waypoint2DIcon::updateWaypoint()
             this->update();
         }
         oldSize = boundingRect();
-        qDebug() << "UPDATING WP";
     }
 }
 
@@ -133,44 +132,30 @@ void Waypoint2DIcon::drawIcon()
 {
     picture.fill(Qt::transparent);
     QPainter painter(&picture);
+    painter.setRenderHint(QPainter::TextAntialiasing, true);
+    painter.setRenderHint(QPainter::Antialiasing, true);
+    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
+
+    QPen pen1(Qt::black);
+    pen1.setWidth(4);
+    QPen pen2(color);
+    pen2.setWidth(2);
+    painter.setBrush(Qt::NoBrush);
 
     // DRAW WAYPOINT
     QPointF p(picture.width()/2, picture.height()/2);
 
     QPolygonF poly(4);
     // Top point
-    poly.replace(0, QPointF(p.x(), p.y()-picture.height()/2.0f));
+    poly.replace(0, QPointF(p.x(), p.y()-picture.height()/2.0f+8*painter.pen().width()));
     // Right point
-    poly.replace(1, QPointF(p.x()+picture.width()/2.0f, p.y()));
+    poly.replace(1, QPointF(p.x()+picture.width()/2.0f-8*painter.pen().width(), p.y()));
     // Bottom point
-    poly.replace(2, QPointF(p.x(), p.y() + picture.height()/2.0f));
-    poly.replace(3, QPointF(p.x() - picture.width()/2.0f, p.y()));
-
-//    // 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(color);
-        pen2.setWidth(2);
-        painter.setPen(pen2);
-//    }
-    painter.setBrush(Qt::NoBrush);
+    poly.replace(2, QPointF(p.x(), p.y() + picture.height()/2.0f-8*painter.pen().width()));
+    poly.replace(3, QPointF(p.x() - picture.width()/2.0f+8*painter.pen().width(), p.y()));
 
     int waypointSize = qMin(picture.width(), picture.height());
-    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
+    float rad = (waypointSize/2.0f) * 0.7f * (1/sqrt(2.0f));
 
     // If this is not a waypoint (only the default representation)
     // or it is a waypoint, but not one where direction has no meaning
@@ -180,40 +165,62 @@ void Waypoint2DIcon::drawIcon()
             (waypoint->getAction() != (int)MAV_CMD_NAV_LAND) &&
             (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_UNLIM) &&
             (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_TIME) &&
-            (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_TURNS)
+            (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_TURNS) &&
+            (waypoint->getAction() != (int)MAV_CMD_NAV_RETURN_TO_LAUNCH)
             )))
     {
-        painter.drawLine(p.x(), p.y(), p.x()+sin(Heading()) * rad, p.y()-cos(Heading()) * rad);
+        painter.setPen(pen1);
+        painter.drawLine(p.x(), p.y(), p.x()+sin(Heading()/180.0f*M_PI) * rad, p.y()-cos(Heading()/180.0f*M_PI) * rad);
+        painter.setPen(pen2);
+        painter.drawLine(p.x(), p.y(), p.x()+sin(Heading()/180.0f*M_PI) * rad, p.y()-cos(Heading()/180.0f*M_PI) * rad);
     }
 
     if ((waypoint != NULL) && (waypoint->getAction() == (int)MAV_CMD_NAV_TAKEOFF))
     {
         // Takeoff waypoint
-        int width = (picture.width()-1);
-        int height = (picture.height()-1);
-        painter.drawRect(0, 0, width, height);
-        painter.drawRect(width*0.2, height*0.2f, width*0.6f, height*0.6f);
+        int width = picture.width()-2*painter.pen().width();
+        int height = picture.height()-2*-painter.pen().width();
+        painter.drawRect(painter.pen().width()/2, painter.pen().width()/2, width, height);
+        painter.drawRect(width*0.2+painter.pen().width()/2, height*0.2f+painter.pen().width()/2, width*0.6f, height*0.6f);
     }
     else if ((waypoint != NULL) && (waypoint->getAction() == (int)MAV_CMD_NAV_LAND))
     {
         // Landing waypoint
-        int width = (picture.width()-1)/2;
-        int height = (picture.height()-1)/2;
+        int width = (picture.width())/2-painter.pen().width();
+        int height = (picture.height())/2-painter.pen().width();
+        painter.setPen(pen1);
+        painter.drawEllipse(p, width, height);
+        painter.drawLine(p.x()-width/2, p.y()-height/2, width, height);
+        painter.setPen(pen2);
         painter.drawEllipse(p, width, height);
-        painter.drawEllipse(p, width*0.6f, height*0.6f);
-        painter.drawLine(0, 0, picture.width(), picture.height());
-        painter.drawLine(picture.width(), 0, 0, picture.height());
+        painter.drawLine(p.x()-width/2, p.y()-height/2, width, height);
     }
     else if ((waypoint != NULL) && ((waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TURNS)))
     {
         // Loiter waypoint
-        int width = (picture.width()-1)/2;
-        int height = (picture.height()-1)/2;
+        int width = (picture.width())/2-9*painter.pen().width();
+        int height = (picture.height())/2-9*painter.pen().width();
+        painter.setPen(pen1);
+        painter.drawEllipse(p, width, height);
+        painter.drawPoint(p);
+        painter.setPen(pen2);
         painter.drawEllipse(p, width, height);
+        painter.drawPoint(p);
+    }
+    else if ((waypoint != NULL) && (waypoint->getAction() == (int)MAV_CMD_NAV_RETURN_TO_LAUNCH))
+    {
+        // Return to launch waypoint
+        int width = picture.width()-2*painter.pen().width();
+        int height = picture.height()-2*-painter.pen().width();
+        painter.drawRect(painter.pen().width()/2, painter.pen().width()/2, width, height);
+        painter.drawText(width/10, width/10, width/10*8, width/10*8, 0, QString("R"));
     }
     else
     {
         // Navigation waypoint
+        painter.setPen(pen1);
+        painter.drawPolygon(poly);
+        painter.setPen(pen2);
         painter.drawPolygon(poly);
     }
 }
@@ -222,28 +229,42 @@ void Waypoint2DIcon::paint(QPainter *painter, const QStyleOptionGraphicsItem *op
 {
     Q_UNUSED(option);
     Q_UNUSED(widget);
+    QPen pen = painter->pen();
+    pen.setWidth(2);
     painter->drawPixmap(-picture.width()/2,-picture.height()/2,picture);
     if (this->isSelected())
     {
+        pen.setColor(Qt::yellow);
         painter->drawRect(QRectF(-picture.width()/2,-picture.height()/2,picture.width()-1,picture.height()-1));
     }
 
-    QPen pen = painter->pen();
+    QPen penBlack(Qt::black);
+    penBlack.setWidth(4);
     pen.setColor(color);
 
     if ((waypoint) && (waypoint->getAction() == (int)MAV_CMD_NAV_WAYPOINT) && showAcceptanceRadius)
     {
         QPen redPen = QPen(pen);
-        redPen.setColor(Qt::red);
+        redPen.setColor(Qt::yellow);
         painter->setPen(redPen);
         const int acceptance = map->metersToPixels(waypoint->getAcceptanceRadius(), Coord());
-        if (waypoint) painter->drawEllipse(QPointF(0, 0), acceptance, acceptance);
+        painter->setPen(penBlack);
+        painter->drawEllipse(QPointF(0, 0), acceptance, acceptance);
+        painter->setPen(redPen);
+        painter->drawEllipse(QPointF(0, 0), acceptance, acceptance);
     }
     if ((waypoint) && ((waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TURNS)))
     {
-        painter->setPen(pen);
-        pen.setWidth(2);
+        QPen penDash(color);
+        penDash.setWidth(1);
+        //penDash.setStyle(Qt::DotLine);
         const int loiter = map->metersToPixels(waypoint->getLoiterOrbit(), Coord());
-        if (waypoint) painter->drawEllipse(QPointF(0, 0), loiter, loiter);
+        if (loiter > picture.width()/2)
+        {
+            painter->setPen(penBlack);
+            painter->drawEllipse(QPointF(0, 0), loiter, loiter);
+            painter->setPen(penDash);
+            painter->drawEllipse(QPointF(0, 0), loiter, loiter);
+        }
     }
 }
diff --git a/src/ui/map/Waypoint2DIcon.h b/src/ui/map/Waypoint2DIcon.h
index 3fd920b5bcf3833b098bdde7e01db7a4f5a0cd95..72af64123f5fb70bb25e6d8b152d538f58120e7d 100644
--- a/src/ui/map/Waypoint2DIcon.h
+++ b/src/ui/map/Waypoint2DIcon.h
@@ -15,14 +15,14 @@ public:
      * @param longitude
      * @param name name of the circle point
      */
-    Waypoint2DIcon(mapcontrol::MapGraphicItem* map, mapcontrol::OPMapWidget* parent, qreal latitude, qreal longitude, qreal altitude, int listindex, QString name = QString(), QString description = QString(), int radius=24);
+    Waypoint2DIcon(mapcontrol::MapGraphicItem* map, mapcontrol::OPMapWidget* parent, qreal latitude, qreal longitude, qreal altitude, int listindex, QString name = QString(), QString description = QString(), int radius=30);
 
     /**
      *
      * @param wp Waypoint
      * @param radius the radius of the circle
      */
-    Waypoint2DIcon(mapcontrol::MapGraphicItem* map, mapcontrol::OPMapWidget* parent, Waypoint* wp, const QColor& color, int listindex, int radius = 24);
+    Waypoint2DIcon(mapcontrol::MapGraphicItem* map, mapcontrol::OPMapWidget* parent, Waypoint* wp, const QColor& color, int listindex, int radius = 31);
 
     virtual ~Waypoint2DIcon();