diff --git a/.gitignore b/.gitignore
index 76ea62d30ca7f1e222fba341bfed0d52bc87050c..7f6a4445f300111a6fc92b9950e0d63ef57b4264 100644
--- a/.gitignore
+++ b/.gitignore
@@ -84,3 +84,5 @@ cmake-build-*/
 # gcov code coverage
 coverage-html/
 coverage.info
+
+posix-configs/SITL/init/test/*_generated
diff --git a/Makefile b/Makefile
index 8fdfcf2bce3d165ec39a0ff1b09a5a1f3fb020e6..0db6bc966b4047a8769842c22e7a15ae2849db09 100755
--- a/Makefile
+++ b/Makefile
@@ -275,7 +275,7 @@ unittest: posix_sitl_default
 	@(cd build_unittest && ctest -j2 --output-on-failure)
 
 run_tests_posix: posix_sitl_default
-	@(cd build_posix_sitl_default/ && ctest -V)
+	@(cd build_posix_sitl_default/ && ctest --output-on-failure)
 
 tests: unittest run_tests_posix
 
diff --git a/posix-configs/SITL/init/test/iris b/posix-configs/SITL/init/test/test
similarity index 100%
rename from posix-configs/SITL/init/test/iris
rename to posix-configs/SITL/init/test/test
diff --git a/posix-configs/SITL/init/test/test_template.in b/posix-configs/SITL/init/test/test_template.in
new file mode 100644
index 0000000000000000000000000000000000000000..5bc09bf26a40e4aac705c5bf4ccc9598219e60e1
--- /dev/null
+++ b/posix-configs/SITL/init/test/test_template.in
@@ -0,0 +1,22 @@
+uorb start
+
+param load
+param set SYS_RESTART_TYPE 0
+
+dataman start
+
+rgbledsim start
+tone_alarm start
+
+#simulator start -s
+#gyrosim start
+#accelsim start
+#barosim start
+
+#adcsim start
+#gpssim start
+#pwm_out_sim mode_pwm
+
+tests @test_name@
+
+shutdown
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 707d9b027b5fb2a30b6c914e90033abc1debeffd..2530c160fa8823e09524f8dead81e5211a2fbb3d 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -141,17 +141,50 @@ install(DIRECTORY ${PROJECT_SOURCE_DIR}/posix-configs DESTINATION ${CMAKE_INSTAL
 # tests
 #
 
-add_test(NAME rcS_tests
-	COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
-		$<TARGET_FILE:px4>
-		posix-configs/SITL/init/test
-		none
-		none
-		none
-		${PX4_SOURCE_DIR}
-		${PX4_BINARY_DIR}
-		WORKING_DIRECTORY ${SITL_WORKING_DIR})
-set_tests_properties(rcS_tests PROPERTIES
-	PASS_REGULAR_EXPRESSION "All tests passed")
+file(GLOB test_src_files 
+	RELATIVE
+	${PX4_SOURCE_DIR}/src/systemcmds/tests/
+	${PX4_SOURCE_DIR}/src/systemcmds/tests/test_*.c*)
+
+#TODO: find a way to keep this in sync with tests_main
+set(tests_exclude
+	adc
+	dataman
+	file
+	hott_telemetry
+	jig_voltages
+	led
+	mount
+	ppm_loopback
+	sensors
+	time
+	uart_baudchange
+	uart_break
+	uart_console
+	uart_loopback
+	)
+
+foreach(test_name ${test_src_files})
+	string(REPLACE "test_" "" test_name ${test_name})
+	string(REPLACE ".cpp" "" test_name ${test_name})
+	string(REPLACE ".c" "" test_name ${test_name})
+	configure_file(${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_template.in ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/${test_name}_generated)
+	
+	list(FIND tests_exclude ${test_name} _index)
+	if (${_index} EQUAL -1)
+		add_test(NAME ${test_name}
+			COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
+				$<TARGET_FILE:px4>
+				posix-configs/SITL/init/test/
+				none
+				none
+				${test_name}_generated
+				${PX4_SOURCE_DIR}
+				${PX4_BINARY_DIR}
+				WORKING_DIRECTORY ${SITL_WORKING_DIR})
+
+		set_tests_properties(${test_name} PROPERTIES PASS_REGULAR_EXPRESSION "${test_name} PASSED")
+	endif()
+endforeach()
 
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index ae91edcc22bbdb4945faf98b4e46201685e893b2..b86d12ff873cc3fe4078b1e4d61a0f5c6766e6e7 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -51,7 +51,7 @@ set(srcs
 	test_matrix.cpp
 	test_mixer.cpp
 	test_mount.c
-	test_params.c
+	test_param.c
 	test_perf.c
 	test_ppm_loopback.c
 	test_rc.c
diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c
index 0d7a95dac97a51e88c13e0ab6eeb1c515de88593..9b41cfaaa30565e1bc0c9931f3787a612993db27 100644
--- a/src/systemcmds/tests/test_dataman.c
+++ b/src/systemcmds/tests/test_dataman.c
@@ -46,7 +46,6 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <errno.h>
-#include <debug.h>
 
 #include <arch/board/board.h>
 
@@ -55,8 +54,7 @@
 #include <drivers/drv_hrt.h>
 #include <semaphore.h>
 
-
-#include "tests.h"
+#include "tests_main.h"
 
 #include "dataman/dataman.h"
 
diff --git a/src/systemcmds/tests/test_params.c b/src/systemcmds/tests/test_param.c
similarity index 100%
rename from src/systemcmds/tests/test_params.c
rename to src/systemcmds/tests/test_param.c
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 46e98449cf8d91a868f176ab58baf03fbf23dfa3..32de342541a6d574de57df4299d62f1baf0cc349 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -79,11 +79,13 @@ const struct {
 	{"jig",			test_jig,	OPT_NOJIGTEST | OPT_NOALLTEST},
 #ifdef __PX4_NUTTX
 	{"adc",			test_adc,	OPT_NOJIGTEST},
+	{"file",		test_file,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"led",			test_led,	0},
 	{"sensors",		test_sensors,	0},
 	{"time",		test_time,	OPT_NOJIGTEST},
 	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST},
 	{"uart_break",		test_uart_break,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST},
 #else
 	{"rc",			rc_tests_main,	0},
 #endif /* __PX4_NUTTX */
@@ -102,8 +104,7 @@ const struct {
 	{"autodeclination",	test_autodeclination,	0},
 	{"bson",		test_bson,	0},
 	{"conv",		test_conv, 0},
-	//{"dataman",		test_dataman, 0}, // Enable for by hand testing
-	{"file",		test_file,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	//{"dataman",		test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, // Enable for by hand testing
 	{"file2",		test_file2,	OPT_NOJIGTEST},
 	{"float",		test_float,	0},
 	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST},
@@ -122,7 +123,6 @@ const struct {
 	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"sleep",		test_sleep,	OPT_NOJIGTEST},
 	{"tone",		test_tone,	0},
-	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{NULL,			NULL, 		0}
@@ -264,7 +264,14 @@ int tests_main(int argc, char *argv[])
 
 	for (unsigned i = 0; tests[i].name; i++) {
 		if (!strcmp(tests[i].name, argv[1])) {
-			return tests[i].fn(argc - 1, argv + 1);
+			if (tests[i].fn(argc - 1, argv + 1) == 0) {
+				printf("%s PASSED\n", tests[i].name);
+				return 0;
+
+			} else {
+				printf("%s FAILED\n", tests[i].name);
+				return -1;
+			}
 		}
 	}