diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index fdfb0b08a9..6e2b4a978c 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -100,11 +100,22 @@ Loiter::set_loiter_position()
 
 	_loiter_pos_set = true;
 
-	// set current mission item to loiter
-	set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	if (_navigator->get_land_detected()->landed) {
+		_mission_item.nav_cmd = NAV_CMD_IDLE;
+
+	} else {
+		if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+
+		} else {
+			setLoiterItemFromCurrentPosition(&_mission_item);
+		}
+
+	}
 
 	// convert mission item to current setpoint
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->current.velocity_valid = false;
 	pos_sp_triplet->previous.valid = false;
 	mission_apply_limitation(_mission_item);
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 389aa795a2..5c73004497 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -683,11 +683,22 @@ Mission::set_mission_items()
 
 		_mission_type = MISSION_TYPE_NONE;
 
-		/* set loiter mission item and ensure that there is a minimum clearance from home */
-		set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+		if (_navigator->get_land_detected()->landed) {
+			_mission_item.nav_cmd = NAV_CMD_IDLE;
+
+		} else {
+			if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+				setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+
+			} else {
+				setLoiterItemFromCurrentPosition(&_mission_item);
+			}
+
+		}
 
 		/* update position setpoint triplet  */
-		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->previous.valid = false;
 		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index fd556e0bbd..6d6ba8da38 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -644,42 +644,40 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 }
 
 void
-MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
+MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item)
 {
-	if (_navigator->get_land_detected()->landed) {
-		/* landed, don't takeoff, but switch to IDLE mode */
-		item->nav_cmd = NAV_CMD_IDLE;
+	setLoiterItemCommonFields(item);
 
-	} else {
-		item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+	const position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	item->lat = pos_sp_triplet->current.lat;
+	item->lon = pos_sp_triplet->current.lon;
+	item->altitude = pos_sp_triplet->current.alt;
+	item->loiter_radius = pos_sp_triplet->current.loiter_radius;
+}
 
-		if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
-			/* use current position setpoint */
-			item->lat = pos_sp_triplet->current.lat;
-			item->lon = pos_sp_triplet->current.lon;
-			item->altitude = pos_sp_triplet->current.alt;
+void
+MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
+{
+	setLoiterItemCommonFields(item);
 
-		} else {
-			/* use current position and use return altitude as clearance */
-			item->lat = _navigator->get_global_position()->lat;
-			item->lon = _navigator->get_global_position()->lon;
-			item->altitude = _navigator->get_global_position()->alt;
+	item->lat = _navigator->get_global_position()->lat;
+	item->lon = _navigator->get_global_position()->lon;
+	item->altitude = _navigator->get_global_position()->alt;
+	item->loiter_radius = _navigator->get_loiter_radius();
+}
 
-			if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
-				item->altitude = _navigator->get_home_position()->alt + min_clearance;
-			}
-		}
+void
+MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
+{
+	item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
 
-		item->altitude_is_relative = false;
-		item->yaw = NAN;
-		item->loiter_radius = _navigator->get_loiter_radius();
-		item->acceptance_radius = _navigator->get_acceptance_radius();
-		item->time_inside = 0.0f;
-		item->autocontinue = false;
-		item->origin = ORIGIN_ONBOARD;
-	}
+	item->altitude_is_relative = false;
+	item->acceptance_radius = _navigator->get_acceptance_radius();
+	item->yaw = NAN;
+	item->time_inside = 0.0f;
+	item->autocontinue = false;
+	item->origin = ORIGIN_ONBOARD;
 }
 
 void
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 2de76f1f8f..e888058e6b 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -108,10 +108,11 @@ protected:
 	 */
 	bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);
 
-	/**
-	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
-	 */
-	void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
+	void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item);
+
+	void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
+
+	void setLoiterItemCommonFields(struct mission_item_s *item);
 
 	/**
 	 * Set a takeoff mission item
diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp
index 06dc1e2b59..1112f61595 100644
--- a/src/modules/navigator/takeoff.cpp
+++ b/src/modules/navigator/takeoff.cpp
@@ -66,11 +66,25 @@ Takeoff::on_active()
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
 
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
 		// set loiter item so position controllers stop doing takeoff logic
-		set_loiter_item(&_mission_item);
-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+		if (_navigator->get_land_detected()->landed) {
+			_mission_item.nav_cmd = NAV_CMD_IDLE;
+
+		} else {
+			if (pos_sp_triplet->current.valid) {
+				setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+
+			} else {
+				setLoiterItemFromCurrentPosition(&_mission_item);
+			}
+		}
+
 		mission_apply_limitation(_mission_item);
+
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 }