New item uses altitude from last item

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;
+}

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;