diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c
index c15b024e7..d2b956d1b 100644
--- a/flight/modules/PathPlanner/pathplanner.c
+++ b/flight/modules/PathPlanner/pathplanner.c
@@ -80,7 +80,7 @@ static uint8_t conditionPythonScript();
 static uint8_t conditionImmediate();
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
 static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
-static void planner_setup_pathdesired(PathDesiredData *pathDesired);
+static void planner_setup_pathdesired(PathDesiredData *pathDesired, uint8_t);
 
 
 // Private variables
@@ -323,20 +323,22 @@ void updatePathDesired()
         return;
     }
 
-    PathDesiredData pathDesired;
-
     // find out current waypoint
     WaypointActiveGet(&waypointActive);
-
     WaypointInstGet(waypointActive.Index, &waypoint);
+
+    // Capture if current mode is takeoff
+    uint8_t autotakeoff = (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF);
+
     PathActionInstGet(waypoint.Action, &pathAction);
 
+    PathDesiredData pathDesired;
     switch (pathAction.Mode) {
     case PATHACTION_MODE_AUTOTAKEOFF:
         planner_setup_pathdesired_takeoff(&pathDesired);
         break;
     default:
-        planner_setup_pathdesired(&pathDesired);
+        planner_setup_pathdesired(&pathDesired, autotakeoff);
         break;
     }
 
@@ -429,7 +431,7 @@ void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
 }
 
 // Standard setup of a pathDesired command from the waypoint path plan
-static void planner_setup_pathdesired(PathDesiredData *pathDesired)
+static void planner_setup_pathdesired(PathDesiredData *pathDesired, uint8_t overwrite_start_position)
 {
     pathDesired->End.North = waypoint.Position.North;
     pathDesired->End.East  = waypoint.Position.East;
@@ -443,7 +445,7 @@ static void planner_setup_pathdesired(PathDesiredData *pathDesired)
     pathDesired->UID = waypointActive.Index;
 
 
-    if (waypointActive.Index == 0) {
+    if (waypointActive.Index == 0 || overwrite_start_position) {
         PositionStateData positionState;
         PositionStateGet(&positionState);
         // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)