diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc
index 7bb30c6c9cd4825087e35b32c343c4870b6f7f30..a78c60e4f66403999e65a3a0618026e5e2523efa 100644
--- a/src/MissionManager/MissionController.cc
+++ b/src/MissionManager/MissionController.cc
@@ -168,6 +168,9 @@ int MissionController::addMissionItem(QGeoCoordinate coordinate)
         newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF);
     }
     newItem->setDefaultsForCommand();
+    if (newItem->command() == MAV_CMD_NAV_WAYPOINT) {
+        newItem->setParam7(_findLastAltitude());
+    }
     _missionItems->append(newItem);
 
     _recalcAll();
@@ -592,3 +595,18 @@ QmlObjectListModel* MissionController::missionItems(void)
 {
     return _missionItems;
 }
+
+double MissionController::_findLastAltitude(void)
+{
+    double lastAltitude = MissionItem::defaultAltitude;
+
+    for (int i=0; i<_missionItems->count(); i++) {
+        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
+
+        if (item->specifiesCoordinate()) {
+            lastAltitude = item->param7();
+        }
+    }
+
+    return lastAltitude;
+}
diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h
index dd11398d036bad9179982650bb344962f21969a0..6c4d1e3291706dbc7135aba51b0e72f0878328ce 100644
--- a/src/MissionManager/MissionController.h
+++ b/src/MissionManager/MissionController.h
@@ -95,6 +95,7 @@ private:
     void _setupMissionItems(bool loadFromVehicle, bool forceLoad);
     void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
     double _calcDistance(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2);
+    double _findLastAltitude(void);
 
 private:
     bool                _editMode;