diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index a05e6d48311a52ce6bb2b0d3d5c19d2b37bc61bc..4a2eb82266682ce36f3cf76b00e539b750854252 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -440,7 +440,8 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa
 
     for (int i = 0; i < geoCoordintateList.size(); i++) {
         QGeoCoordinate vertex = geoCoordintateList[i];
-        if (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude()))
+        if (   (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude()))
+            || !vertex.isValid())
             geoCoordintateList.removeAt(i);
     }
 
@@ -691,6 +692,16 @@ bool WimaController::calcNextPhase()
     }
     settingsItem->setCoordinate(_takeoffLandPostion);
 
+//    qDebug("homeposition");
+//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
+//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
+//        if (item != nullptr) {
+//            qDebug() << item->coordinate();
+//        } else {
+//            qDebug("Error");
+//        }
+//    }
+
     // set takeoff position for first mission item (bug)
     missionController()->insertSimpleMissionItem(_takeoffLandPostion, 1);
     SimpleMissionItem *takeoffItem = missionControllerVisuals->value<SimpleMissionItem*>(1);
@@ -700,6 +711,16 @@ bool WimaController::calcNextPhase()
     }
     takeoffItem->setCoordinate(_takeoffLandPostion);
 
+//    qDebug("takeoff");
+//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
+//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
+//        if (item != nullptr) {
+//            qDebug() << item->coordinate();
+//        } else {
+//            qDebug("Error");
+//        }
+//    }
+
     // create change speed item, after take off
     _missionController->insertSimpleMissionItem(_takeoffLandPostion, 2);
     SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
@@ -707,14 +728,34 @@ bool WimaController::calcNextPhase()
         qWarning("WimaController::calcNextPhase(): nullptr");
         return false;
     }
+    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);// set coordinate must be after setCommand (setCommand sets coordinate to zero)
     speedItem->setCoordinate(_takeoffLandPostion);
-    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
     speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
 
+//    qDebug("speed");
+//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
+//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
+//        if (item != nullptr) {
+//            qDebug() << item->coordinate();
+//        } else {
+//            qDebug("Error");
+//        }
+//    }
+
     // insert arrival path
     for (auto coordinate : arrivalPath)
         _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
 
+//    qDebug("arrival");
+//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
+//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
+//        if (item != nullptr) {
+//            qDebug() << item->coordinate();
+//        } else {
+//            qDebug("Error");
+//        }
+//    }
+
     // create change speed item, after arrival path
     int index = missionControllerVisuals->count();
     _missionController->insertSimpleMissionItem(CSWpList.first(), index);
@@ -723,14 +764,34 @@ bool WimaController::calcNextPhase()
         qWarning("WimaController::calcNextPhase(): nullptr");
         return false;
     }
+    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
     speedItem->setCoordinate(CSWpList.first());
-    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
     speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble());
 
+//    qDebug("speed");
+//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
+//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
+//        if (item != nullptr) {
+//            qDebug() << item->coordinate();
+//        } else {
+//            qDebug("Error");
+//        }
+//    }
+
     // insert Circular Survey coordinates
     for (auto coordinate : CSWpList)
         _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
 
+//    qDebug("survey");
+//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
+//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
+//        if (item != nullptr) {
+//            qDebug() << item->coordinate();
+//        } else {
+//            qDebug("Error");
+//        }
+//    }
+
     // create change speed item, after circular survey
     index = missionControllerVisuals->count();
     _missionController->insertSimpleMissionItem(CSWpList.last(), index);
@@ -739,14 +800,34 @@ bool WimaController::calcNextPhase()
         qWarning("WimaController::calcNextPhase(): nullptr");
         return false;
     }
-    speedItem->setCoordinate(_takeoffLandPostion);
-    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
+    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
+    speedItem->setCoordinate(CSWpList.last());
     speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
 
+//    qDebug("speed");
+//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
+//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
+//        if (item != nullptr) {
+//            qDebug() << item->coordinate();
+//        } else {
+//            qDebug("Error");
+//        }
+//    }
+
     // insert return path coordinates
     for (auto coordinate : returnPath)
         _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
 
+//    qDebug("return path");
+//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
+//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
+//        if (item != nullptr) {
+//            qDebug() << item->coordinate();
+//        } else {
+//            qDebug("Error");
+//        }
+//    }
+
     // set land command for last mission item
     index = missionControllerVisuals->count();
     _missionController->insertSimpleMissionItem(_takeoffLandPostion, index);
@@ -757,6 +838,16 @@ bool WimaController::calcNextPhase()
     }
     _missionController->setLandCommand(*landItem);
 
+//    qDebug("land");
+//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
+//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
+//        if (item != nullptr) {
+//            qDebug() << item->coordinate();
+//        } else {
+//            qDebug("Error");
+//        }
+//    }
+
     // copy to _currentMissionItems
     for ( int i = 1; i < missionControllerVisuals->count(); i++) {
         SimpleMissionItem *visualItem     = missionControllerVisuals->value<SimpleMissionItem*>(i);
@@ -880,6 +971,8 @@ void WimaController::checkBatteryLevel()
     WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
     int batteryThreshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
 
+    static long attemptCounter = 0;
+
     if (managerVehicle != nullptr) {
         Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
         Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
@@ -890,10 +983,14 @@ void WimaController::checkBatteryLevel()
             _setVehicleHasLowBattery(true);
             if (!_lowBatteryHandlingTriggered) {
                 QString errorString;
+                attemptCounter++;
+                if (attemptCounter > 3) {
+                    _lowBatteryHandlingTriggered = true;
+                    attemptCounter = 0;
+                }
                 if (_checkSmartRTLPreCondition(errorString) == true) {
                     managerVehicle->pauseVehicle();
-                    if (_calcReturnPath(errorString)) {                        
-                        _lowBatteryHandlingTriggered = true;
+                    if (_calcReturnPath(errorString)) {
                         emit returnBatteryLowConfirmRequired();
                     } else {
                         qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible."));
diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h
index 4657d430fed0d37edbd29ba122fed565597eeaf9..180b3d3fc8c364e450e90231564f51fd47b5e706 100644
--- a/src/Wima/WimaController.h
+++ b/src/Wima/WimaController.h
@@ -240,7 +240,6 @@ private:
     bool                         _lowBatteryHandlingTriggered;
     bool                         _executingSmartRTL;
 
-
 };
 
 /*