diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h
index b839410fc5..78ebbc2461 100644
--- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h
+++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h
@@ -47,6 +47,7 @@ static constexpr int kMaxNodes = 100;
 static constexpr int kCircleApproxVertices = 8;
 
 struct PlannedPath {
+	// does not include the start and the end, just intermediate points
 	static constexpr int kMaxPathPoints = 32;
 	matrix::Vector2<double> points[kMaxPathPoints];
 	int num_points{0};
@@ -64,19 +65,20 @@ struct PlannedPath {
 
 	}
 
-	matrix::Vector2<double> getNextPoint()
+	matrix::Vector2<double> getAndPopCurrentPoint()
 	{
 		if (current_index < num_points) {
 			return points[current_index++];
 
 		} else {
-			return points[num_points - 1];
+			return matrix::Vector2d{NAN, NAN};
 		}
 
 	}
 
 	matrix::Vector2d getCurrentPoint()
 	{
+		// returns the current points but does not increase the index
 		if (current_index < num_points) {
 			return points[current_index];
 		}
diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp
index 116b585a7f..72a77f1ff2 100644
--- a/src/modules/navigator/rtl_direct.cpp
+++ b/src/modules/navigator/rtl_direct.cpp
@@ -250,7 +250,14 @@ void RtlDirect::set_rtl_item()
 
 	case RTLState::MOVE_TO_INTERMEDIATE_POINT: {
 
-			const matrix::Vector2d point = _geofence_aware_return_path.getNextPoint();
+			matrix::Vector2d point = _geofence_aware_return_path.getAndPopCurrentPoint();
+
+			if (point.isAllNan()) {
+				// should never happen
+				point(0) = _land_approach.lat;
+				point(1) = _land_approach.lon;
+			}
+
 			PositionYawSetpoint pos_yaw_sp {
 				.lat = point(0),
 				.lon = point(1),