diff --git a/src/ui/QGCDataPlot2D.cc b/src/ui/QGCDataPlot2D.cc
index 659313cae6398aedef5f9ff961d6cad388f0afbc..14a4e02f1d8d569b46db2ba7ad757907e74748d2 100644
--- a/src/ui/QGCDataPlot2D.cc
+++ b/src/ui/QGCDataPlot2D.cc
@@ -405,8 +405,6 @@ void QGCDataPlot2D::loadCsvLog(QString file, QString xAxisName, QString yAxisFil
 
     int curveNameIndex = 0;
 
-    //int xValueIndex = curveNames.indexOf(xAxisName);
-
     QString xAxisFilter;
     if (xAxisName == "") {
         xAxisFilter = curveNames.first();
@@ -414,6 +412,36 @@ void QGCDataPlot2D::loadCsvLog(QString file, QString xAxisName, QString yAxisFil
         xAxisFilter = xAxisName;
     }
 
+    // Fill y-axis renaming lookup table
+    // Allow the user to rename data dimensions in the plot
+    QMap<QString, QString> renaming;
+
+    QStringList yCurves = yAxisFilter.split("|", QString::SkipEmptyParts);
+    qDebug() << "YFILTER" << yAxisFilter;
+
+    // Figure out the correct renaming
+    for (int i = 0; i < yCurves.count(); ++i)
+    {
+        qDebug() << "CURVE:" << yCurves.at(i);
+        if (yCurves.at(i).contains(":"))
+        {
+            QStringList parts = yCurves.at(i).split(":", QString::SkipEmptyParts);
+            if (parts.count() > 1)
+            {
+                // Insert renaming map
+                renaming.insert(parts.first(), parts.last());
+                // Replace curve value with first part only
+                yCurves.replace(i, parts.first());
+            }
+        }
+//        else
+//        {
+//            // Insert same value, not renaming anything
+//            renaming.insert(yCurves.at(i), yCurves.at(i));
+//        }
+    }
+
+
     foreach(curveName, curveNames) {
         // Add to plot x axis selection
         ui->xAxis->addItem(curveName);
@@ -421,14 +449,19 @@ void QGCDataPlot2D::loadCsvLog(QString file, QString xAxisName, QString yAxisFil
         ui->xRegressionComboBox->addItem(curveName);
         ui->yRegressionComboBox->addItem(curveName);
         if (curveName != xAxisFilter) {
-            if ((yAxisFilter == "") || yAxisFilter.contains(curveName)) {
+            if ((yAxisFilter == "") || yCurves.contains(curveName)) {
                 yValues.insert(curveName, new QVector<double>());
                 xValues.insert(curveName, new QVector<double>());
                 // Add separator starting with second item
                 if (curveNameIndex > 0 && curveNameIndex < curveNames.count()) {
                     ui->yAxis->setText(ui->yAxis->text()+"|");
                 }
-                ui->yAxis->setText(ui->yAxis->text()+curveName);
+                // If this curve was renamed, re-add the renaming to the text field
+                QString renamingText = "";
+                if (renaming.contains(curveName)) renamingText = QString(":%1").arg(renaming.value(curveName));
+                ui->yAxis->setText(ui->yAxis->text()+curveName+renamingText);
+                // Insert same value, not renaming anything
+                if (!renaming.contains(curveName)) renaming.insert(curveName, curveName);
                 curveNameIndex++;
             }
         }
@@ -478,7 +511,8 @@ void QGCDataPlot2D::loadCsvLog(QString file, QString xAxisName, QString yAxisFil
             foreach(curveName, curveNames)
             {
                 // Y  AXIS HANDLING
-                if(curveName != xAxisFilter && (yAxisFilter == "" || yAxisFilter.contains(curveName)))
+                // Only plot non-x curver and those selected in the yAxisFilter (or all if the filter is not set)
+                if(curveName != xAxisFilter && (yAxisFilter == "" || yCurves.contains(curveName)))
                 {
                     bool oky;
                     int curveNameIndex = curveNames.indexOf(curveName);
@@ -504,7 +538,14 @@ void QGCDataPlot2D::loadCsvLog(QString file, QString xAxisName, QString yAxisFil
     // Add data array of each curve to the plot at once (fast)
     // Iterates through all x-y curve combinations
     for (int i = 0; i < yValues.count(); i++) {
-        plot->appendData(yValues.keys().at(i), xValues.values().at(i)->data(), yValues.values().at(i)->data(), xValues.values().at(i)->count());
+        if (renaming.contains(yValues.keys().at(i)))
+        {
+            plot->appendData(renaming.value(yValues.keys().at(i)), xValues.values().at(i)->data(), yValues.values().at(i)->data(), xValues.values().at(i)->count());
+        }
+        else
+        {
+            plot->appendData(yValues.keys().at(i), xValues.values().at(i)->data(), yValues.values().at(i)->data(), xValues.values().at(i)->count());
+        }
     }
     plot->updateScale();
     plot->setStyleText(ui->style->currentText());
diff --git a/src/ui/QGCDataPlot2D.ui b/src/ui/QGCDataPlot2D.ui
index 04861aebb8bf5716b31f66b2171458b2250d5b86..1cd5288e490948ffd0efc5e01aa3c60132396741 100644
--- a/src/ui/QGCDataPlot2D.ui
+++ b/src/ui/QGCDataPlot2D.ui
@@ -6,14 +6,14 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1073</width>
-    <height>308</height>
+    <width>1463</width>
+    <height>311</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout">
+  <layout class="QGridLayout" name="gridLayout" columnstretch="1,1,1,1,100,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1">
    <item row="0" column="0">
     <widget class="QLabel" name="label_2">
      <property name="text">
@@ -303,7 +303,7 @@
      </property>
      <property name="sizeHint" stdset="0">
       <size>
-       <width>40</width>
+       <width>5</width>
        <height>20</height>
       </size>
      </property>
diff --git a/src/ui/map/QGCMapTool.cc b/src/ui/map/QGCMapTool.cc
index f7ed1442e89abcf17024d694e9f31c79dd4abad1..ef159c69d578d0301b3173c01dc9e5e711e89489 100644
--- a/src/ui/map/QGCMapTool.cc
+++ b/src/ui/map/QGCMapTool.cc
@@ -1,5 +1,7 @@
 #include "QGCMapTool.h"
 #include "ui_QGCMapTool.h"
+#include <QAction>
+#include <QMenu>
 
 QGCMapTool::QGCMapTool(QWidget *parent) :
     QWidget(parent),
diff --git a/src/ui/map/QGCMapTool.h b/src/ui/map/QGCMapTool.h
index d7d4f4c4a68b4ceba6747e7aa7ce0771a7c375b7..784bf786252a8e9b61a6a730eb169e7d1df55b5a 100644
--- a/src/ui/map/QGCMapTool.h
+++ b/src/ui/map/QGCMapTool.h
@@ -2,6 +2,7 @@
 #define QGCMAPTOOL_H
 
 #include <QWidget>
+#include <QMenu>
 
 namespace Ui {
     class QGCMapTool;
diff --git a/src/ui/map/QGCMapToolBar.cc b/src/ui/map/QGCMapToolBar.cc
index 490a2cd74559712c8ac9ef30b707c4396c5b2ec0..f57aa2ae1dc9e5d49c0517d4bfe3af1a64e51d36 100644
--- a/src/ui/map/QGCMapToolBar.cc
+++ b/src/ui/map/QGCMapToolBar.cc
@@ -29,9 +29,42 @@ void QGCMapToolBar::setMap(QGCMapWidget* map)
 
         // Edit mode handling
         ui->editButton->hide();
+
+        const int uavTrailTimeList[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};                      // seconds
+        const int uavTrailTimeCount = 10;
+
+        const int uavTrailDistanceList[] = {1, 2, 5, 10, 20, 50, 100, 200, 500};             // meters
+        const int uavTrailDistanceCount = 9;
+
+        optionsMenu.setParent(this);
+
+
+        // Build up menu
+        //trailPlotMenu(tr("Add trail dot every.."), this);
+        for (int i = 0; i < uavTrailTimeCount; ++i)
+        {
+            trailPlotMenu.addAction(QString("%1 second%2").arg(uavTrailTimeList[i]).arg((uavTrailTimeList[i] > 1) ? "s" : ""), this, SLOT(setUAVTrailTime()));
+        }
+        for (int i = 0; i < uavTrailDistanceCount; ++i)
+        {
+            trailPlotMenu.addAction(QString("%1 meter%2").arg(uavTrailDistanceList[i]).arg((uavTrailDistanceList[i] > 1) ? "s" : ""), this, SLOT(setUAVTrailDistance()));
+        }
+        optionsMenu.addMenu(&trailPlotMenu);
+
+        ui->optionsButton->setMenu(&optionsMenu);
     }
 }
 
+void QGCMapToolBar::setUAVTrailTime()
+{
+
+}
+
+void QGCMapToolBar::setUAVTrailDistance()
+{
+
+}
+
 void QGCMapToolBar::tileLoadStart()
 {
     ui->posLabel->setText(QString("Starting to load tiles.."));
diff --git a/src/ui/map/QGCMapToolBar.h b/src/ui/map/QGCMapToolBar.h
index 8b61d776b46a2427d9c60990ef6bf824e1d7484b..d7d4284bd43319f19db850e9cfcf06bf69af4d0f 100644
--- a/src/ui/map/QGCMapToolBar.h
+++ b/src/ui/map/QGCMapToolBar.h
@@ -2,6 +2,7 @@
 #define QGCMAPTOOLBAR_H
 
 #include <QWidget>
+#include <QMenu>
 
 class QGCMapWidget;
 
@@ -23,9 +24,13 @@ public slots:
     void tileLoadStart();
     void tileLoadEnd();
     void tileLoadProgress(int progress);
+    void setUAVTrailTime();
+    void setUAVTrailDistance();
 
 protected:
     QGCMapWidget* map;
+    QMenu optionsMenu;
+    QMenu trailPlotMenu;
 
 private:
     Ui::QGCMapToolBar *ui;
diff --git a/src/ui/map/QGCMapToolBar.ui b/src/ui/map/QGCMapToolBar.ui
index 38f0151fe3c78a2f4f91bb0f75b7fd699f0c0ac1..84d7a8a615bab543a75713fb86cbae7d96356600 100644
--- a/src/ui/map/QGCMapToolBar.ui
+++ b/src/ui/map/QGCMapToolBar.ui
@@ -69,6 +69,13 @@
      </property>
     </widget>
    </item>
+   <item>
+    <widget class="QPushButton" name="optionsButton">
+     <property name="text">
+      <string>Options</string>
+     </property>
+    </widget>
+   </item>
    <item>
     <spacer name="horizontalSpacer">
      <property name="orientation">
diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc
index cfdccc53ff433dd2ba3b4773ecf2cbe4c5af004b..744a71fecd6704dd1cd1264b29d9e27b318f75a9 100644
--- a/src/ui/map/QGCMapWidget.cc
+++ b/src/ui/map/QGCMapWidget.cc
@@ -60,12 +60,6 @@ void QGCMapWidget::showEvent(QShowEvent* event)
     //        magic_waypoint.time_seconds = 0;
     //        magic_waypoint.hold_time_seconds = 0;
 
-    const int safe_area_radius_list[] = {5, 10, 20, 50, 100, 200, 500, 1000, 2000, 5000};   // meters
-
-    const int uav_trail_time_list[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};                      // seconds
-
-    const int uav_trail_distance_list[] = {1, 2, 5, 10, 20, 50, 100, 200, 500};             // meters
-
     SetMouseWheelZoomType(internals::MouseWheelZoomType::MousePositionWithoutCenter);	    // set how the mouse wheel zoom functions
     SetFollowMouse(true);				    // we want a contiuous mouse position reading
 
@@ -74,7 +68,7 @@ void QGCMapWidget::showEvent(QShowEvent* event)
     //SetShowDiagnostics(true); // Not needed in flight / production mode
 
 
-    Home->SetSafeArea(safe_area_radius_list[0]);                         // set radius (meters)
+    Home->SetSafeArea(30);                         // set radius (meters)
     Home->SetShowSafeArea(true);                                         // show the safe area
 
 ////    UAV->SetTrailTime(uav_trail_time_list[0]);                           // seconds