diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
index 568f5c5b8..ebb489a13 100644
--- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
@@ -2538,7 +2538,7 @@ void OPMapGadgetWidget::doVisualizationChanged(QList<PathCompiler::waypoint> way
     foreach (PathCompiler::waypoint waypoint, waypoints) {
         internals::PointLatLng position(waypoint.latitude, waypoint.longitude);
 
-        WayPointItem * wayPointItem = m_map->WPCreate(position, 0, QString(index));
+        WayPointItem * wayPointItem = m_map->WPCreate(position, waypoint.altitude, QString(index));
         Q_ASSERT(wayPointItem);
         if(wayPointItem) {
             wayPointItem->SetNumber(index);
diff --git a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp
index 59ac2e200..a7c9425a9 100644
--- a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp
+++ b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp
@@ -335,6 +335,7 @@ struct PathCompiler::waypoint PathCompiler::UavoToInternal(Waypoint::DataFields
 
     internalWaypoint.latitude = LLA[0];
     internalWaypoint.longitude = LLA[1];
+    internalWaypoint.altitude = LLA[2];
     return internalWaypoint;
 }
 
@@ -363,7 +364,7 @@ Waypoint::DataFields PathCompiler::InternalToUavo(waypoint internal)
     // TODO: Give the point a concept of altitude
     LLA[0] = internal.latitude;
     LLA[1] = internal.longitude;
-    LLA[2] = -50;
+    LLA[2] = internal.altitude;
 
     Utils::CoordinateConversions().GetNED(homeLLA, LLA, NED);
 
diff --git a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h
index 2fb615195..ed721794c 100644
--- a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h
+++ b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h
@@ -76,6 +76,7 @@ public:
 
         double latitude;
         double longitude;
+        double altitude;
     };
 
 private: