diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index b6403c05fd..24ada09fd7 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1032,7 +1032,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
 		const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
+	if (pos_sp_triplet.current.valid && !_l1_control.circle_mode()) {
 
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 0573fd837c..9e0963f7fe 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -169,6 +169,38 @@ MissionBlock::is_mission_item_reached()
 					_navigator->get_global_position()->alt,
 					&dist_xy, &dist_z);
 
+		/* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */
+		if (!_navigator->get_vstatus()->is_rotary_wing &&
+			(_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) {
+
+			struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
+			/* close to waypoint, but altitude error greater than twice acceptance */
+			if ((dist >= 0.0f)
+				&& (dist_z > 2 * _navigator->get_altitude_acceptance_radius())
+				&& (dist_xy < 2 * _navigator->get_loiter_radius())) {
+
+				/* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */
+				if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+					curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+					curr_sp->loiter_radius = _navigator->get_loiter_radius();
+					curr_sp->loiter_direction = 1;
+					_navigator->set_position_setpoint_triplet_updated();
+				}
+			} else {
+				/* restore SETPOINT_TYPE_POSITION */
+				if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+					/* loiter acceptance criteria required to revert back to SETPOINT_TYPE_POSITION */
+					if ((dist >= 0.0f)
+						&& (dist_z < _navigator->get_loiter_radius())
+						&& (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) {
+
+						curr_sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+						_navigator->set_position_setpoint_triplet_updated();
+					}
+				}
+			}
+		}
+
 		if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
 			&& _navigator->get_vstatus()->is_rotary_wing) {