diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index bc6723e5f5..91fc2a7512 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -32,6 +32,7 @@ MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/ver
+MODULES		+= systemcmds/top
 
 #
 # Testing modules
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index cfea12f044..c4de996bb9 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -641,9 +641,13 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
 				if (ret != OK) {
 					delete node;
 					free((void *)objname);
+					free((void *)devpath);
 				}
 
-			} while (ret != OK && (group_tries++ < max_group_tries));
+				/* try with next larger index */
+				group_tries++;
+
+			} while (ret != OK && (group_tries < max_group_tries));
 
 			if (group_tries >= max_group_tries) {
 				ret = -ENOMEM;