diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml
index 452420ee6b4ac6386565191d743786436786987e..5c714a8f90a1f2280937200d23030f58e45e94cb 100644
--- a/src/FlightDisplay/FlightDisplayViewMap.qml
+++ b/src/FlightDisplay/FlightDisplayViewMap.qml
@@ -223,8 +223,8 @@ FlightMap {
         missionItems:   wimaController.missionItems
         path:           wimaController.waypointPath
         showItems:      _wimaEnabled && _showAllWimaItems
-        zOrderWP:       QGroundControl.zOrderWaypointIndicators-3
-        zOrderLines:    QGroundControl.zOrderWaypointLines-1
+        zOrderWP:       QGroundControl.zOrderWaypointIndicators-2
+        zOrderLines:    QGroundControl.zOrderWaypointLines-2
         color:          "gray"
     }
     // current Items
@@ -235,8 +235,8 @@ FlightMap {
         path:           wimaController.currentWaypointPath
         showItems:      _wimaEnabled && _showCurrentWimaItems
         zOrderWP:       QGroundControl.zOrderWaypointIndicators-1
-        zOrderLines:    QGroundControl.zOrderWaypointIndicators-2
-        color:          "green" // gray with alpha 0.7
+        zOrderLines:    QGroundControl.zOrderWaypointLines-1
+        color:          "green"
     }
 
     // Add trajectory points to the map
diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml
index 32d8aaa4be72a42c09f857698b4b4ce5c63cb95a..8303de75d1f9f3bd4c69f4012d4ef43390ccd243 100644
--- a/src/FlightDisplay/FlightDisplayWimaMenu.qml
+++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml
@@ -310,6 +310,14 @@ Item {
                             Layout.fillWidth: true
                         }
 
+                        QGCButton {
+                            id: buttonRemoveTrajectoryHistory
+                            text:               qsTr("Clear Trajectory")
+                            onClicked:          wimaController.removeVehicleTrajectoryHistory()
+                            Layout.columnSpan:  2
+                            Layout.fillWidth: true
+                        }
+
 
                         // progess bar
                         Rectangle {
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index cba6240ea911692c2e6e05377215bd9268b7e120..1fd95b22862115753bbcc31fe455df869fe9e6bb 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -1051,7 +1051,7 @@ public:
     QGCCameraManager*           dynamicCameras      () { return _cameras; }
     QString                     hobbsMeter          ();
 
-    /// @true: When flying a mission the vehicle is always facing towards the next waypoint
+    /// @true: When flying a mission the vehicle is always facing towards the next setCurrentMissionSequence
     bool vehicleYawsToNextWaypointInMission(void) const;
 
     /// The vehicle is responsible for making the initial request for the Plan.
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index decad5bd779d4d354ae12f27cae7ea8ae9eb0f37..19011d807d7e630ec26bd7665f4f88595d360e0c 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -292,24 +292,20 @@ bool WimaController::forceUploadToVehicle()
         SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
         _missionController->insertSimpleMissionItem(*item, visuals->count());
         if (item->command() == MAV_CMD_DO_CHANGE_SPEED) {
-            qWarning() << item->missionItem().param2();
         }
     }
 
-    qWarning("blah");
     for (int i = 0; i < _missionController->visualItems()->count(); i++){
         SimpleMissionItem *item =  _missionController->visualItems()->value<SimpleMissionItem*>(i);
         if (item == nullptr)
             continue;
         if (item->command() == MAV_CMD_DO_CHANGE_SPEED) {
-            qWarning() << item->missionItem().param2();
         }
     }
     for (int i = 0; i < _missionController->visualItems()->count(); i++){
         SimpleMissionItem *item =  _missionController->visualItems()->value<SimpleMissionItem*>(i);
         if (item == nullptr)
             continue;
-        qWarning() << i << ":" << item->command();
     }
 
     _masterController->sendToVehicle();
@@ -363,9 +359,16 @@ bool WimaController::initSmartRTL()
     bool retValue = calcReturnPath();
     if (!retValue)
         return false;
+    emit uploadAndExecuteConfirmRequired();
     return true;
 }
 
+void WimaController::removeVehicleTrajectoryHistory()
+{
+    Vehicle *managerVehicle = masterController()->managerVehicle();
+    managerVehicle->trajectoryPoints()->clear();
+}
+
 void WimaController::saveToCurrent()
 {
 
@@ -939,20 +942,24 @@ void WimaController::checkBatteryLevel()
             _setVehicleHasLowBattery(true);
             if (!_lowBatteryHandlingTriggered) {
                 QString errorString;
-                attemptCounter++;
-                if (attemptCounter > 3) {
-                    _lowBatteryHandlingTriggered = true;
-                    attemptCounter = 0;
-                }
+                attemptCounter++;                
                 if (_checkSmartRTLPreCondition(errorString) == true) {
                     managerVehicle->pauseVehicle();
                     if (_calcReturnPath(errorString)) {
+                        _lowBatteryHandlingTriggered = true;
+                        attemptCounter = 0;
                         emit returnBatteryLowConfirmRequired();
                     } else {
-                        qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible."));
-                        qgcApp()->showMessage(errorString);
+                        if (attemptCounter > 3) {
+                            qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible."));
+                            qgcApp()->showMessage(errorString);
+                        }
                     }
                 }
+                if (attemptCounter > 3) { // try 3 times, somtimes vehicle is outside joined area
+                    _lowBatteryHandlingTriggered = true;
+                    attemptCounter = 0;
+                }
             }
         }
         else {
@@ -971,6 +978,8 @@ void WimaController::smartRTLCleanUp(bool flying)
             _executingSmartRTL = false;
             _loadCurrentMissionItemsFromBuffer();
             _showAllMissionItems.setRawValue(true);
+            _missionController->removeAllFromVehicle();
+            _missionController->removeAll();
             disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp);
         }
     }
@@ -1132,7 +1141,6 @@ bool WimaController::_calcReturnPath(QString &errorSring)
     _showAllMissionItems.setRawValue(false);
     managerVehicle->trajectoryPoints()->clear();
 
-    emit uploadAndExecuteConfirmRequired();
     return true;
 }
 
@@ -1160,6 +1168,7 @@ void WimaController::_loadCurrentMissionItemsFromBuffer()
 {
     _currentMissionItems.clear();
     int numItems = _missionItemsBuffer.count();
+    qWarning() << "WimaController::_loadCurrentMissionItemsFromBuffer: numItems" << numItems;
     for (int i = 0; i < numItems; i++)
         _currentMissionItems.append(_missionItemsBuffer.removeAt(0));
 
@@ -1170,8 +1179,9 @@ void WimaController::_loadCurrentMissionItemsFromBuffer()
 void WimaController::_saveCurrentMissionItemsToBuffer()
 {
     _missionItemsBuffer.clear();
-    int numCurrentMissionItems = _currentMissionItems.count();
-    for (int i = 0; i < numCurrentMissionItems; i++)
+    int numItems = _currentMissionItems.count();
+    qWarning() << "WimaController::_saveCurrentMissionItemsToBuffer: numItems" << numItems;
+    for (int i = 0; i < numItems; i++)
         _missionItemsBuffer.append(_currentMissionItems.removeAt(0));
 }
 
diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h
index 5c958e44e3618f6cf1b3e25241eb69598a95627e..2c2cb287143ef8fbf2c0846d1c8a079c2bdf8500 100644
--- a/src/Wima/WimaController.h
+++ b/src/Wima/WimaController.h
@@ -111,6 +111,7 @@ public:
     Q_INVOKABLE bool calcReturnPath(); // wrapper for _calcReturnPath(QString &errorSring)#
     Q_INVOKABLE bool executeSmartRTL(); // wrapper for _executeSmartRTL(QString &errorSring)
     Q_INVOKABLE bool initSmartRTL();
+    Q_INVOKABLE void removeVehicleTrajectoryHistory();
 
 
     Q_INVOKABLE void saveToCurrent  ();