diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc
index ba6f388375d0ef58de35b3431e4a830d46cee88b..7c7291d4c77a6dce1661d0d8df55aa7d621db21b 100644
--- a/src/MissionManager/MissionController.cc
+++ b/src/MissionManager/MissionController.cc
@@ -391,24 +391,38 @@ int MissionController::insertSimpleMissionItem(const MissionItem &missionItem, i
     int sequenceNumber = _nextSequenceNumber();
     SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, missionItem, this);
     newItem->setSequenceNumber(sequenceNumber);
-    newItem->setCommand(MAV_CMD_NAV_WAYPOINT);
     _initVisualItem(newItem);
     MAV_CMD takeoffCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF;
     if (newItem->command() ==  takeoffCmd) {
         if (!_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) {
+            newItem->setCommand(MAV_CMD_NAV_WAYPOINT);
             return -1; // can not add this takeoff command for this vehicle
         }
     }
-    if (newItem->specifiesAltitude()) {
-        double  prevAltitude;
-        int     prevAltitudeMode;
+    if (newItem->specifiesAltitude()) {        
+        newItem->altitude()->setRawValue(missionItem.relativeAltitude());
+        newItem->setAltitudeMode(QGroundControlQmlGlobal::AltitudeMode::AltitudeModeRelative);
+    }
+    newItem->setMissionFlightStatus(_missionFlightStatus);
+    _visualItems->insert(i, newItem);
 
-        if (_findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) {
-            newItem->altitude()->setRawValue(prevAltitude);
-            newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
+    return newItem->sequenceNumber();
+}
+
+int MissionController::insertSimpleMissionItem(SimpleMissionItem &missionItem, int i)
+{
+    int sequenceNumber = _nextSequenceNumber();
+    SimpleMissionItem * newItem = new SimpleMissionItem(missionItem, _flyView, this);
+    newItem->setSequenceNumber(sequenceNumber);
+    _initVisualItem(newItem);
+    MAV_CMD takeoffCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF;
+    if (newItem->command() ==  takeoffCmd) {
+        if (!_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) {
+            newItem->setCommand(MAV_CMD_NAV_WAYPOINT);
+            return -1; // can not add this takeoff command for this vehicle
         }
     }
-    newItem->setMissionFlightStatus(_missionFlightStatus);
+    //newItem->setMissionFlightStatus(_missionFlightStatus);
     _visualItems->insert(i, newItem);
 
     return newItem->sequenceNumber();
diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h
index 6e56a7f4272cdf0f082a88fc096f2fba307956ce..461bfaf595eff38f154684ee8f47894d30f364dc 100644
--- a/src/MissionManager/MissionController.h
+++ b/src/MissionManager/MissionController.h
@@ -113,6 +113,11 @@ public:
     /// @return Sequence number for new item
                 int insertSimpleMissionItem(const MissionItem &missionItem, int i);
 
+    /// Add a new simple mission item to the list
+    ///     @param i: index to insert at
+    /// @return Sequence number for new item
+                int insertSimpleMissionItem(SimpleMissionItem &missionItem, int i);
+
     /// Add a new ROI mission item to the list
     ///     @param i: index to insert at
     /// @return Sequence number for new item
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index a5bae57afe07de94a57716d6128769f6181b171e..044f6964f874272572d087c8c004a06fc2e2edf3 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -165,6 +165,56 @@ void WimaController::resetPhase()
 
 void WimaController::uploadToVehicle()
 {
+    if (_currentMissionItems.count() < 1)
+        return;
+
+    _missionController->removeAll();
+    // set homeposition of settingsItem
+    QmlObjectListModel* visuals = _missionController->visualItems();
+    MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
+    if (settingsItem == nullptr) {
+        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
+        return;
+    }
+    settingsItem->setCoordinate(_takeoffLandPostion);
+
+    // copy mission items from _currentMissionItems to _missionController
+    for (int i = 0; i < _currentMissionItems.count(); i++){
+        SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
+        _missionController->insertSimpleMissionItem(*item, visuals->count());
+    }
+
+//    // set land command for last mission item
+//    SimpleMissionItem *landItem = visuals->value<SimpleMissionItem*>(visuals->count()-1);
+//    if (landItem == nullptr) {
+//        qWarning("WimaController::uploadToVehicle(): nullptr");
+//        _missionController->removeAll();
+//        return;
+//    }
+
+//    // check vehicle type, before setting land command
+//    Vehicle* controllerVehicle = _masterController->controllerVehicle();
+//    MAV_CMD landCmd = controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
+//    if (controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
+//        landItem->setCommand(landCmd);
+//    } else {
+//        _missionController->removeAll();
+//        return;
+//    }
+
+    for (int i = 1; i < visuals->count(); i++) {
+        SimpleMissionItem* item = visuals->value<SimpleMissionItem *>(i);
+        qWarning() << item->coordinate();
+        qWarning() << item->command();
+        qWarning() << item->missionItem().param1();
+        qWarning() << item->missionItem().param2();
+        qWarning() << item->missionItem().param3();
+        qWarning() << item->missionItem().param4();
+        qWarning() << item->missionItem().param5();
+        qWarning() << item->missionItem().param6();
+        qWarning() << item->missionItem().param7();
+        qWarning(" ");
+    }
     _masterController->sendToVehicle();
 }
 
@@ -542,6 +592,8 @@ void WimaController::calcNextPhase()
         _currentMissionItems.append(visualItemCopy);
     }
 
+    _missionController->removeAll(); // remove items from _missionController, will be added on upload
+
     updateCurrentPath();
     emit currentMissionItemsChanged();
 }