diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 71347c2c0f..ec40efff9b 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -88,7 +88,7 @@ set(config_module_list
 	#modules/mavlink/mavlink_tests #TODO: fix mavlink_tests
 	modules/unit_test
 	modules/uORB/uORB_tests
-	#systemcmds/tests
+	systemcmds/tests
 
 	)
 
diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c
index ef7f217fdf..1ffb08d65d 100644
--- a/src/systemcmds/tests/test_adc.c
+++ b/src/systemcmds/tests/test_adc.c
@@ -49,7 +49,7 @@
 #include <fcntl.h>
 #include <errno.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <drivers/drv_adc.h>
 
diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c
index 7a168cac5a..57cb016b99 100644
--- a/src/systemcmds/tests/test_bson.c
+++ b/src/systemcmds/tests/test_bson.c
@@ -49,7 +49,7 @@
 #include <systemlib/err.h>
 #include <systemlib/bson/tinybson.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 static const bool sample_bool = true;
 static const int32_t sample_small_int = 123;
diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp
index c26455eda8..f6cfaab073 100644
--- a/src/systemcmds/tests/test_conv.cpp
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -48,12 +48,11 @@
 #include <fcntl.h>
 #include <errno.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <math.h>
 #include <float.h>
 
-#include <systemlib/err.h>
 #include <unit_test/unit_test.h>
 #include <px4iofirmware/protocol.h>
 
diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c
index fb081e27b3..06329a06d4 100644
--- a/src/systemcmds/tests/test_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -53,7 +53,7 @@
 
 #include <drivers/drv_hrt.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 static int check_user_abort(int fd);
 
diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c
index 56d9f0d83a..700803f9b5 100644
--- a/src/systemcmds/tests/test_file2.c
+++ b/src/systemcmds/tests/test_file2.c
@@ -49,7 +49,7 @@
 #include <stdlib.h>
 #include <px4_getopt.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #define FLAG_FSYNC 1
 #define FLAG_LSEEK 2
diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c
index 3d27d903f0..b8021a3ac8 100644
--- a/src/systemcmds/tests/test_gpio.c
+++ b/src/systemcmds/tests/test_gpio.c
@@ -49,7 +49,7 @@
 
 #include <px4_adc.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <drivers/drv_gpio.h>
 
@@ -96,7 +96,7 @@ int test_gpio(int argc, char *argv[])
 	int fd = px4_open(PX4IO_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		printf("GPIO: open fail\n");
+		PX4_ERR("GPIO: open fail");
 		return ERROR;
 	}
 
@@ -111,7 +111,7 @@ int test_gpio(int argc, char *argv[])
 	px4_ioctl(fd, GPIO_RESET, ~0);
 
 	px4_close(fd);
-	printf("\t GPIO test successful.\n");
+	PX4_INFO("GPIO test successful.");
 
 #endif
 
diff --git a/src/systemcmds/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c
index d1e9281191..adb76265ae 100644
--- a/src/systemcmds/tests/test_hott_telemetry.c
+++ b/src/systemcmds/tests/test_hott_telemetry.c
@@ -58,7 +58,7 @@
 #include <termios.h>
 #include <unistd.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 
 /****************************************************************************
diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c
index c88603e745..a899c47c0d 100644
--- a/src/systemcmds/tests/test_hrt.c
+++ b/src/systemcmds/tests/test_hrt.c
@@ -53,9 +53,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
 
-//#include <nuttx/spi.h>
-
-#include "tests.h"
+#include "tests_main.h"
 
 /****************************************************************************
  * Pre-processor Definitions
diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c
index f94caa87b3..e5f636fb4d 100644
--- a/src/systemcmds/tests/test_jig_voltages.c
+++ b/src/systemcmds/tests/test_jig_voltages.c
@@ -46,9 +46,7 @@
 #include <fcntl.h>
 #include <errno.h>
 
-//#include <nuttx/spi.h>
-
-#include "tests.h"
+#include "tests_main.h"
 
 #include <px4_adc.h>
 #include <drivers/drv_adc.h>
@@ -93,7 +91,7 @@ int test_jig_voltages(int argc, char *argv[])
 	int ret = OK;
 
 	if (fd < 0) {
-		warnx("can't open ADC device");
+		PX4_ERR("can't open ADC device");
 		return 1;
 	}
 
@@ -104,7 +102,7 @@ int test_jig_voltages(int argc, char *argv[])
 
 	if (count < 0) {
 		close(fd);
-		warnx("can't read from ADC driver. Forgot 'adc start' command?");
+		PX4_ERR("can't read from ADC driver. Forgot 'adc start' command?");
 		return 1;
 	}
 
@@ -116,7 +114,7 @@ int test_jig_voltages(int argc, char *argv[])
 
 	printf("\n");
 
-	warnx("\t ADC operational.\n");
+	PX4_INFO("\t ADC operational.\n");
 
 	/* Expected values */
 	int16_t expected_min[] = {2800, 2800, 1800,  800};
@@ -125,7 +123,7 @@ int test_jig_voltages(int argc, char *argv[])
 
 	if (channels < 4) {
 		close(fd);
-		warnx("not all four test channels available, aborting.");
+		PX4_ERR("not all four test channels available, aborting.");
 		return 1;
 
 	} else {
@@ -141,23 +139,23 @@ int test_jig_voltages(int argc, char *argv[])
 		ret += (expected_min[2] > data[2].am_data || expected_max[2] < data[2].am_data) ? 1 : 0;
 		ret += (expected_min[3] > data[3].am_data || expected_max[3] < data[3].am_data) ? 1 : 0;
 
-		message("Sample:");
-		message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
-			data[0].am_channel, (int)(data[0].am_data), expected_min[0], expected_max[0], check_res[0]);
-		message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
-			data[1].am_channel, (int)(data[1].am_data), expected_min[1], expected_max[1], check_res[1]);
-		message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
-			data[2].am_channel, (int)(data[2].am_data), expected_min[2], expected_max[2], check_res[2]);
-		message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
-			data[3].am_channel, (int)(data[3].am_data), expected_min[3], expected_max[3], check_res[3]);
+		PX4_INFO("Sample:");
+		PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
+			 data[0].am_channel, (int)(data[0].am_data), expected_min[0], expected_max[0], check_res[0]);
+		PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
+			 data[1].am_channel, (int)(data[1].am_data), expected_min[1], expected_max[1], check_res[1]);
+		PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
+			 data[2].am_channel, (int)(data[2].am_data), expected_min[2], expected_max[2], check_res[2]);
+		PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
+			 data[3].am_channel, (int)(data[3].am_data), expected_min[3], expected_max[3], check_res[3]);
 
 		if (ret != OK) {
-			printf("\t JIG voltages test FAILED. Some channels where out of allowed range. Check supply voltages.\n");
+			PX4_ERR("\t JIG voltages test FAILED. Some channels where out of allowed range. Check supply voltages.");
 			goto errout_with_dev;
 		}
 	}
 
-	printf("\t JIG voltages test successful.\n");
+	PX4_INFO("JIG voltages test successful.");
 
 errout_with_dev:
 
diff --git a/src/systemcmds/tests/test_led.c b/src/systemcmds/tests/test_led.c
index f045640453..db308fc2cf 100644
--- a/src/systemcmds/tests/test_led.c
+++ b/src/systemcmds/tests/test_led.c
@@ -51,7 +51,7 @@
 
 #include <drivers/drv_led.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 /****************************************************************************
  * Pre-processor Definitions
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
index 9de1f06686..013734f0d4 100644
--- a/src/systemcmds/tests/test_mathlib.cpp
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -54,6 +54,8 @@
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 
+#include "tests_main.h"
+
 class MathlibTest : public UnitTest
 {
 public:
@@ -73,8 +75,6 @@ private:
 	bool testQuaternionRotate();
 };
 
-#include "tests.h"
-
 #define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); }
 
 using namespace math;
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index ac07dd6a70..40436c9bd3 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -58,7 +58,7 @@
 
 #include <uORB/topics/actuator_controls.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 static int	mixer_callback(uintptr_t handle,
 			       uint8_t control_group,
diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c
index 364536b973..080b458cdf 100644
--- a/src/systemcmds/tests/test_mount.c
+++ b/src/systemcmds/tests/test_mount.c
@@ -41,6 +41,7 @@
 
 #include <px4_config.h>
 #include <px4_posix.h>
+#include <px4_tasks.h>
 
 #include <sys/stat.h>
 #include <dirent.h>
@@ -53,7 +54,7 @@
 
 #include <drivers/drv_hrt.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 const int fsync_tries = 1;
 const int abort_tries = 10;
diff --git a/src/systemcmds/tests/test_params.c b/src/systemcmds/tests/test_params.c
index 228d88fb9e..8439bf998a 100644
--- a/src/systemcmds/tests/test_params.c
+++ b/src/systemcmds/tests/test_params.c
@@ -41,7 +41,7 @@
 #include <stdio.h>
 #include "systemlib/err.h"
 #include "systemlib/param/param.h"
-#include "tests.h"
+#include "tests_main.h"
 
 #define PARAM_MAGIC1 12345678
 #define PARAM_MAGIC2 0xa5a5a5a5
diff --git a/src/systemcmds/tests/test_perf.c b/src/systemcmds/tests/test_perf.c
index 89be5000b2..e82bde9cce 100644
--- a/src/systemcmds/tests/test_perf.c
+++ b/src/systemcmds/tests/test_perf.c
@@ -36,7 +36,7 @@
 
 #include <systemlib/perf_counter.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 int
 test_perf(int argc, char *argv[])
diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c
index a889488eab..dbfd93c4eb 100644
--- a/src/systemcmds/tests/test_ppm_loopback.c
+++ b/src/systemcmds/tests/test_ppm_loopback.c
@@ -53,7 +53,7 @@
 #include <uORB/topics/rc_channels.h>
 #include <systemlib/err.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <math.h>
 #include <float.h>
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
index 1f150816d4..a159b63beb 100644
--- a/src/systemcmds/tests/test_rc.c
+++ b/src/systemcmds/tests/test_rc.c
@@ -54,7 +54,7 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <math.h>
 #include <float.h>
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index d663a7c3f0..ee0d8d2579 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -53,9 +53,7 @@
 
 #include <arch/board/board.h>
 
-//#include <nuttx/spi.h>
-
-#include "tests.h"
+#include "tests_main.h"
 
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_accel.h>
diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
index a862d41d1a..bc399315fb 100644
--- a/src/systemcmds/tests/test_servo.c
+++ b/src/systemcmds/tests/test_servo.c
@@ -52,9 +52,7 @@
 #include <drivers/drv_pwm_output.h>
 #include <systemlib/err.h>
 
-//#include <nuttx/spi.h>
-
-#include "tests.h"
+#include "tests_main.h"
 
 int test_servo(int argc, char *argv[])
 {
diff --git a/src/systemcmds/tests/test_sleep.c b/src/systemcmds/tests/test_sleep.c
index 09f2bd2dca..02de5df653 100644
--- a/src/systemcmds/tests/test_sleep.c
+++ b/src/systemcmds/tests/test_sleep.c
@@ -49,7 +49,7 @@
 
 #include <arch/board/board.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 int test_sleep(int argc, char *argv[])
 {
diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c
index bc41fe78bc..c253ade44b 100644
--- a/src/systemcmds/tests/test_time.c
+++ b/src/systemcmds/tests/test_time.c
@@ -49,7 +49,7 @@
 
 #include <arch/board/board.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <math.h>
 #include <float.h>
diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c
index 7156543021..de575f9ae1 100644
--- a/src/systemcmds/tests/test_uart_baudchange.c
+++ b/src/systemcmds/tests/test_uart_baudchange.c
@@ -52,7 +52,7 @@
 
 #include <arch/board/board.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <math.h>
 #include <float.h>
diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c
index dc1f59c57d..2c6adc7947 100644
--- a/src/systemcmds/tests/test_uart_console.c
+++ b/src/systemcmds/tests/test_uart_console.c
@@ -50,7 +50,7 @@
 
 #include <arch/board/board.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <math.h>
 #include <float.h>
diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c
index a45254bf93..6c81f2fd79 100644
--- a/src/systemcmds/tests/test_uart_loopback.c
+++ b/src/systemcmds/tests/test_uart_loopback.c
@@ -50,7 +50,7 @@
 
 #include <arch/board/board.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <math.h>
 #include <float.h>
diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c
index bc4a0bddad..44cd613c45 100644
--- a/src/systemcmds/tests/test_uart_send.c
+++ b/src/systemcmds/tests/test_uart_send.c
@@ -49,7 +49,7 @@
 
 #include <arch/board/board.h>
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <math.h>
 #include <float.h>
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 127ac208c8..a7b4759d38 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -38,12 +38,10 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include "tests.h"
+#include "tests_main.h"
 
 #include <px4_config.h>
 
-#include <sys/types.h>
-
 #include <stdio.h>
 #include <string.h>
 #include <fcntl.h>
@@ -52,7 +50,7 @@
 // Not using Eigen at the moment
 #define TESTS_EIGEN_DISABLE
 
-#include "tests.h"
+#include "tests_main.h"
 
 /****************************************************************************
  * Private Function Prototypes
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests_main.h
similarity index 89%
rename from src/systemcmds/tests/tests.h
rename to src/systemcmds/tests/tests_main.h
index 80e5d27d6d..c9cf30ab17 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests_main.h
@@ -35,7 +35,7 @@
 #define __APPS_PX4_TESTS_H
 
 /**
- * @file tests.h
+ * @file tests_main.h
  * Tests declaration file.
  *
  */
@@ -50,26 +50,6 @@
  * Definitions
  ****************************************************************************/
 
-/* Debug ********************************************************************/
-
-#ifdef CONFIG_CPP_HAVE_VARARGS
-#  ifdef CONFIG_DEBUG
-#    define message(...) lowsyslog(__VA_ARGS__)
-#    define msgflush()
-#  else
-#    define message(...) printf(__VA_ARGS__)
-#    define msgflush() fflush(stdout)
-#  endif
-#else
-#  ifdef CONFIG_DEBUG
-#    define message lowsyslog
-#    define msgflush()
-#  else
-#    define message printf
-#    define msgflush() fflush(stdout)
-#  endif
-#endif
-
 __BEGIN_DECLS
 
 extern int	test_adc(int argc, char *argv[]);