diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index da2990b35bfd716a576079d480aabb3da73177f7..cc07dd40737badf834094c8aa266687c49997e9d 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -228,7 +228,7 @@
         <file alias="QGroundControl/Controls/WimaJoinedAreaMapVisual.qml">src/WimaView/WimaJoinedAreaMapVisual.qml</file>
         <file alias="QGroundControl/Controls/WimaCorridorEditor.qml">src/WimaView/WimaCorridorEditor.qml</file>
         <file alias="QGroundControl/FlightMap/WimaPlanMapItems.qml">src/FlightMap/MapItems/WimaPlanMapItems.qml</file>
-        <file alias="QGroundControl/Controls/WimaMissionItemMapVisual.qml">src/PlanView/WimaMissionItemMapVisual.qml</file>
+        <file alias="QGroundControl/FlightMap/WimaMissionItemMapVisual.qml">src/PlanView/WimaMissionItemMapVisual.qml</file>
     </qresource>
     <qresource prefix="/json">
         <file alias="APMMavlinkStreamRate.SettingsGroup.json">src/Settings/APMMavlinkStreamRate.SettingsGroup.json</file>
diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml
index 852af22d07f6749d5c3d69f826671948238e124b..b7e97f785aee5ef7c6db2904dd4f83e47e13689a 100644
--- a/src/FlightDisplay/FlightDisplayViewMap.qml
+++ b/src/FlightDisplay/FlightDisplayViewMap.qml
@@ -214,8 +214,8 @@ FlightMap {
         map:                flightMap
         largeMapView:       _mainIsMap
         wimaController:     flightMap.wimaController
-        z:          QGroundControl.zOrderTrajectoryLines-1
-        color: "gray"
+        z:                  QGroundControl.zOrderTrajectoryLines-1
+        color: "#B4808080" // gray with alpha #AARRGGBB
     }
 
     // Add trajectory points to the map
diff --git a/src/FlightMap/MapItems/MissionItemIndicator.qml b/src/FlightMap/MapItems/MissionItemIndicator.qml
index 71cbaf99a5744392001d1778995cd22695c4ea2d..d41b286975584e5ee397ff34264dfecf1231b9a3 100644
--- a/src/FlightMap/MapItems/MissionItemIndicator.qml
+++ b/src/FlightMap/MapItems/MissionItemIndicator.qml
@@ -38,7 +38,8 @@ MapQuickItem {
             vehicleYaw:     missionItem.missionVehicleYaw
             showGimbalYaw:  !isNaN(missionItem.missionGimbalYaw)
             onClicked:      _item.clicked()
-            color:          _item.color
+            color:          _item.color ? _item.color
+                          : checked ? "green" : (child ? qgcPal.mapIndicatorChild : qgcPal.mapIndicator)
 
             property bool _isCurrentItem:   missionItem ? missionItem.isCurrentItem : false
         }
diff --git a/src/FlightMap/MapItems/WimaPlanMapItems.qml b/src/FlightMap/MapItems/WimaPlanMapItems.qml
index 15cb9c434ac8296d6e4b54e644c620ce7c654218..bfbe89cc57754b16f7e7582f901e7ee16d0a5db7 100644
--- a/src/FlightMap/MapItems/WimaPlanMapItems.qml
+++ b/src/FlightMap/MapItems/WimaPlanMapItems.qml
@@ -23,6 +23,7 @@ Item {
     property bool   largeMapView        ///< true: map takes up entire view, false: map is in small window
     property var    wimaController
     property var    color
+    property var    lineColor: color
 
     property var    _map: map
     property var    _missionLineViewComponent
@@ -68,7 +69,7 @@ Item {
 
         MapPolyline {
             line.width: 3
-            line.color: "gray"                           // Hack, can't get palette to work in here
+            line.color: lineColor                       // Hack, can't get palette to work in here
             z:          QGroundControl.zOrderWaypointLines
             path:       wimaController.waypointPath
         }
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index d8b6bd0c06f17ff21c4f40597284f822be6601d4..f9fbd676b94803f83669bd2a5971e997be4d475f 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -12,6 +12,7 @@ WimaController::WimaController(QObject *parent)
     , _serviceArea          (this)
     , _corridor             (this)
     , _localPlanDataValid   (false)
+    , _firstWaypointIndex   (2) // starts with 2, 0 is home position, 1 is takeoff
 {
 
 }
@@ -141,6 +142,74 @@ QJsonDocument WimaController::saveToJson(FileType fileType)
     return QJsonDocument();
 }
 
+bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList<QGeoCoordinate> &path)
+{
+    using namespace GeoUtilities;
+    using namespace PolygonCalculus;
+    QList<QPointF> path2D;
+    bool retVal = PolygonCalculus::shortestPath(
+                                   toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/ start)),
+                                   /*start point*/ QPointF(0,0),
+                                   /*destination*/ toCartesian2D(destination, start),
+                                   /*shortest path*/ path2D);
+    path.append(toGeo(path2D, /*origin*/ start));
+
+    return  retVal;
+}
+
+bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItems, QList<QGeoCoordinate> &coordinateList)
+{
+    return extractCoordinateList(missionItems, coordinateList, 0, missionItems.count()-1);
+}
+
+bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItems, QList<QGeoCoordinate> &coordinateList, int startIndex, int endIndex)
+{
+    if (   startIndex >= 0
+        && startIndex < missionItems.count()
+        && endIndex >= 0
+        && endIndex < missionItems.count()) {
+        if (startIndex > endIndex) {
+            if (!extractCoordinateList(missionItems, coordinateList, startIndex, missionItems.count()-1))
+                return false;
+            if (!extractCoordinateList(missionItems, coordinateList, 0, endIndex))
+                return false;
+        } else {
+            for (int i = startIndex; i <= endIndex; i++) {
+                const MissionItem *mItem = qobject_cast<MissionItem*>(missionItems[i]);
+
+                if (mItem == nullptr) {
+                    coordinateList.clear();
+                    return false;
+                }
+                coordinateList.append(mItem->coordinate());
+            }
+        }
+    } else
+        return false;
+
+    return true;
+}
+
+bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItems, QVariantList &coordinateList)
+{
+    return extractCoordinateList(missionItems, coordinateList, 0 , missionItems.count()-1);
+}
+
+bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex)
+{
+    QList<QGeoCoordinate> geoCoordintateList;
+
+    bool retValue = extractCoordinateList(missionItems, geoCoordintateList, startIndex, endIndex);
+
+    if (!retValue)
+        return false;
+
+    for (auto coordinate : geoCoordintateList)
+        coordinateList.append(QVariant::fromValue(coordinate));
+
+    return true;
+}
+
 /*!
  * \fn void WimaController::containerDataValidChanged(bool valid)
  * Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true).
@@ -254,25 +323,35 @@ void WimaController::containerDataValidChanged(bool valid)
 #endif
 }
 
-void WimaController::updateWaypointPath()
+void WimaController::updateCurrentMissionItems()
 {
-    _waypointPath.clear();
+    int numberWaypoints = 30; // the number of waypoints currentMissionItems must not exceed
 
-    for ( int i = 1; i < _missionItems.count(); i++) {
-        SimpleMissionItem *item = qobject_cast<SimpleMissionItem *>(_missionItems[i]);
-        if (item == nullptr) {
-            qWarning("WimaController::updateWaypointPath(): nullptr");
-            return;
-        }
-        _waypointPath.append(QVariant::fromValue(item->coordinate()));
+    QList<QGeoCoordinate> geoCoordinateList; // list with potential waypoints (from _missionItems), for _currentMissionItems
+    if (!extractCoordinateList(_missionItems, geoCoordinateList, _firstWaypointIndex,
+                               std::min(_firstWaypointIndex + numberWaypoints, _missionItems.count()-2))) // -2 -> last item is land item
+    {
+        qWarning("WimaController::updateCurrentMissionItems(): error on waypoint extraction.");
+        return;
     }
 
-    emit waypointPathChanged();
-}
 
+}
 
+void WimaController::updateWaypointPath()
+{
+    _waypointPath.clear();
+    extractCoordinateList(_missionItems, _waypointPath, 1, _missionItems.count()-1);
 
+    emit waypointPathChanged();
+}
+void WimaController::updateCurrentPath()
+{
+    _currentWaypointPath.clear();
+    extractCoordinateList(_currentMissionItems, _currentWaypointPath, 1, _currentMissionItems.count()-1);
 
+    emit currentWaypointPathChanged();
+}