Move the 'tests' app to the new world.

23 files changed, 3624 insertions, 0 deletions

diff --git a/src/systemcmds/tests/.context b/src/systemcmds/tests/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/src/systemcmds/tests/.context
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
new file mode 100644
index 000000000..754d3a0da
--- /dev/null
+++ b/src/systemcmds/tests/module.mk
@@ -0,0 +1,28 @@
+#
+# Assorted tests and the like
+#
+
+MODULE_COMMAND		 = tests
+MODULE_STACKSIZE	 = 12000
+MAXOPTIMIZATION		 = -Os
+
+SRCS			 = test_adc.c \
+			   test_bson.c \
+			   test_float.c \
+			   test_gpio.c \
+			   test_hott_telemetry.c \
+			   test_hrt.c \
+			   test_int.c \
+			   test_jig_voltages.c \
+			   test_led.c \
+			   test_sensors.c \
+			   test_servo.c \
+			   test_sleep.c \
+			   test_time.c \
+			   test_uart_baudchange.c \
+			   test_uart_console.c \
+			   test_uart_loopback.c \
+			   test_uart_send.c \
+			   tests_file.c \
+			   tests_main.c \
+			   tests_param.c
diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c
new file mode 100644
index 000000000..030ac6e23
--- /dev/null
+++ b/src/systemcmds/tests/test_adc.c
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_adc.c
+ * Test for the analog to digital converter.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <nuttx/analog/adc.h>
+#include <drivers/drv_adc.h>
+#include <systemlib/err.h>
+
+int test_adc(int argc, char *argv[])
+{
+	int fd = open(ADC_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		warnx("ERROR: can't open ADC device");
+		return 1;
+	}
+
+	for (unsigned i = 0; i < 5; i++) {
+		/* make space for a maximum of eight channels */
+		struct adc_msg_s data[8];
+		/* read all channels available */
+		ssize_t count = read(fd, data, sizeof(data));
+
+		if (count < 0)
+			goto errout_with_dev;
+
+		unsigned channels = count / sizeof(data[0]);
+
+		for (unsigned j = 0; j < channels; j++) {
+			printf("%d: %u  ", data[j].am_channel, data[j].am_data);
+		}
+
+		printf("\n");
+		usleep(150000);
+	}
+
+	warnx("\t ADC test successful.\n");
+
+errout_with_dev:
+
+	if (fd != 0) close(fd);
+
+	return OK;
+}
diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c
new file mode 100644
index 000000000..6130fe763
--- /dev/null
+++ b/src/systemcmds/tests/test_bson.c
@@ -0,0 +1,244 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_bson.c
+ *
+ * Tests for the bson en/decoder
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <systemlib/err.h>
+#include <systemlib/bson/tinybson.h>
+
+#include "tests.h"
+
+static const bool sample_bool = true;
+static const int32_t sample_small_int = 123;
+static const int64_t sample_big_int = (int64_t)INT_MAX + 123LL;
+static const double sample_double = 2.5f;
+static const char *sample_string = "this is a test";
+static const uint8_t sample_data[256] = {0};
+//static const char *sample_filename = "/fs/microsd/bson.test";
+
+static int
+encode(bson_encoder_t encoder)
+{
+	if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0)
+		warnx("FAIL: encoder: append bool failed");
+	if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0)
+		warnx("FAIL: encoder: append int failed");
+	if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0)
+		warnx("FAIL: encoder: append int failed");
+	if (bson_encoder_append_double(encoder, "double1", sample_double) != 0)
+		warnx("FAIL: encoder: append double failed");
+	if (bson_encoder_append_string(encoder, "string1", sample_string) != 0)
+		warnx("FAIL: encoder: append string failed");
+	if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0)
+		warnx("FAIL: encoder: append data failed");
+
+	bson_encoder_fini(encoder);
+
+	return 0;
+}
+
+static int
+decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	unsigned len;
+
+	if (!strcmp(node->name, "bool1")) {
+		if (node->type != BSON_BOOL) {
+			warnx("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL);
+			return 1;
+		}
+		if (node->b != sample_bool) {
+			warnx("FAIL: decoder: bool1 value %s, expected %s", 
+				(node->b ? "true" : "false"),
+				(sample_bool ? "true" : "false"));
+			return 1;
+		}
+		warnx("PASS: decoder: bool1");
+		return 1;
+	}
+	if (!strcmp(node->name, "int1")) {
+		if (node->type != BSON_INT32) {
+			warnx("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32);
+			return 1;
+		}
+		if (node->i != sample_small_int) {
+			warnx("FAIL: decoder: int1 value %lld, expected %d", node->i, sample_small_int);
+			return 1;
+		}
+		warnx("PASS: decoder: int1");
+		return 1;
+	}
+	if (!strcmp(node->name, "int2")) {
+		if (node->type != BSON_INT64) {
+			warnx("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64);
+			return 1;
+		}
+		if (node->i != sample_big_int) {
+			warnx("FAIL: decoder: int2 value %lld, expected %lld", node->i, sample_big_int);
+			return 1;
+		}
+		warnx("PASS: decoder: int2");
+		return 1;
+	}
+	if (!strcmp(node->name, "double1")) {
+		if (node->type != BSON_DOUBLE) {
+			warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE);
+			return 1;
+		}
+		if (node->d != sample_double) {
+			warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double);
+			return 1;
+		}
+		warnx("PASS: decoder: double1");
+		return 1;
+	}
+	if (!strcmp(node->name, "string1")) {
+		if (node->type != BSON_STRING) {
+			warnx("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING);
+			return 1;
+		}
+
+		len = bson_decoder_data_pending(decoder);
+
+		if (len != strlen(sample_string) + 1) {
+			warnx("FAIL: decoder: string1 length %d wrong, expected %d", len, strlen(sample_string) + 1);
+			return 1;
+		}
+
+		char sbuf[len];
+
+		if (bson_decoder_copy_data(decoder, sbuf)) {
+			warnx("FAIL: decoder: string1 copy failed");
+			return 1;
+		}
+		if (bson_decoder_data_pending(decoder) != 0) {
+			warnx("FAIL: decoder: string1 copy did not exhaust all data");
+			return 1;
+		}
+		if (sbuf[len - 1] != '\0') {
+			warnx("FAIL: decoder: string1 not 0-terminated");
+			return 1;
+		}
+		if (strcmp(sbuf, sample_string)) {
+			warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string);
+			return 1;
+		}
+		warnx("PASS: decoder: string1");
+		return 1;
+	}
+	if (!strcmp(node->name, "data1")) {
+		if (node->type != BSON_BINDATA) {
+			warnx("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA);
+			return 1;
+		}
+
+		len = bson_decoder_data_pending(decoder);
+
+		if (len != sizeof(sample_data)) {
+			warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(sample_data));
+			return 1;
+		}
+		
+		if (node->subtype != BSON_BIN_BINARY) {
+			warnx("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY);
+			return 1;
+		}
+
+		uint8_t dbuf[len];
+
+		if (bson_decoder_copy_data(decoder, dbuf)) {
+			warnx("FAIL: decoder: data1 copy failed");
+			return 1;
+		}
+		if (bson_decoder_data_pending(decoder) != 0) {
+			warnx("FAIL: decoder: data1 copy did not exhaust all data");
+			return 1;
+		}
+		if (memcmp(sample_data, dbuf, len)) {
+			warnx("FAIL: decoder: data1 compare fail");
+			return 1;
+		}
+		warnx("PASS: decoder: data1");
+		return 1;
+	}
+
+	if (node->type != BSON_EOO)
+		warnx("FAIL: decoder: unexpected node name '%s'", node->name);
+	return 1;
+}
+
+static void
+decode(bson_decoder_t decoder)
+{
+	int result;
+
+	do {
+		result = bson_decoder_next(decoder);
+	} while (result > 0);
+}
+
+int
+test_bson(int argc, char *argv[])
+{
+	struct bson_encoder_s encoder;
+	struct bson_decoder_s decoder;
+	void *buf;
+	int len;
+
+	/* encode data to a memory buffer */
+	if (bson_encoder_init_buf(&encoder, NULL, 0))
+		errx(1, "FAIL: bson_encoder_init_buf");
+	encode(&encoder);
+	len = bson_encoder_buf_size(&encoder);
+	if (len <= 0)
+		errx(1, "FAIL: bson_encoder_buf_len");
+	buf = bson_encoder_buf_data(&encoder);
+	if (buf == NULL)
+		errx(1, "FAIL: bson_encoder_buf_data");
+
+	/* now test-decode it */
+	if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL))
+		errx(1, "FAIL: bson_decoder_init_buf");
+	decode(&decoder);
+	free(buf);
+
+	return OK;
+}
\ No newline at end of file
diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c
new file mode 100644
index 000000000..4921c9bbb
--- /dev/null
+++ b/src/systemcmds/tests/test_float.c
@@ -0,0 +1,283 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_float.c
+ * Floating point tests
+ */
+
+#include <nuttx/config.h>
+#include <sys/types.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include "tests.h"
+#include <math.h>
+#include <float.h>
+
+typedef union {
+	float f;
+	double d;
+	uint8_t b[8];
+} test_float_double_t;
+
+int test_float(int argc, char *argv[])
+{
+	int ret = 0;
+
+	printf("\n--- SINGLE PRECISION TESTS ---\n");
+	printf("The single precision test involves calls to fabsf(),\nif test fails check this function as well.\n\n");
+
+	float f1 = 1.55f;
+
+	float sinf_zero = sinf(0.0f);
+	float sinf_one = sinf(1.0f);
+	float sqrt_two = sqrt(2.0f);
+
+	if (sinf_zero == 0.0f) {
+		printf("\t success: sinf(0.0f) == 0.0f\n");
+
+	} else {
+		printf("\t FAIL: sinf(0.0f) != 0.0f, result: %8.4f\n", (double)sinf_zero);
+		ret = -4;
+	}
+
+	fflush(stdout);
+
+	if (fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
+		printf("\t success: sinf(1.0f) == 0.84147f\n");
+
+	} else {
+		printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %8.4f\n", (double)sinf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	float asinf_one = asinf(1.0f);
+
+	if (fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
+		printf("\t success: asinf(1.0f) == 1.57079f\n");
+
+	} else {
+		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	float cosf_one = cosf(1.0f);
+
+	if (fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
+		printf("\t success: cosf(1.0f) == 0.54030f\n");
+
+	} else {
+		printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %8.4f\n", (double)cosf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+	float acosf_one = acosf(1.0f);
+
+	if (fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
+		printf("\t success: acosf(1.0f) == 0.0f\n");
+
+	} else {
+		printf("\t FAIL: acosf(1.0f) != 0.0f, result: %8.4f\n", (double)acosf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+	float sinf_zero_one = sinf(0.1f);
+
+	if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
+		printf("\t success: sinf(0.1f) == 0.09983f\n");
+
+	} else {
+		printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %8.4f\n", (double)sinf_zero_one);
+		ret = -2;
+	}
+
+	if (sqrt_two == 1.41421356f) {
+		printf("\t success: sqrt(2.0f) == 1.41421f\n");
+
+	} else {
+		printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %8.4f\n", (double)sinf_zero_one);
+		ret = -3;
+	}
+
+	float atan2f_ones = atan2(1.0f, 1.0f);
+
+	if (fabsf(atan2f_ones - 0.785398163397448278999490867136f) < FLT_EPSILON) {
+		printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n");
+
+	} else {
+		printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %8.4f\n", (double)atan2f_ones);
+		ret = -4;
+	}
+
+	char sbuf[30];
+	sprintf(sbuf, "%8.4f", 0.553415f);
+
+	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
+		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
+		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
+		printf("\t success: printf(\"%%8.4f\", 0.553415f) == %8.4f\n", (double)0.553415f);
+	} else {
+		printf("\t FAIL: printf(\"%%8.4f\", 0.553415f) != \"  0.5534\", result: %s\n", sbuf);
+		ret = -5;
+	}
+
+	sprintf(sbuf, "%8.4f", -0.553415f);
+
+	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
+		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
+		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
+		printf("\t success: printf(\"%%8.4f\", -0.553415f) == %8.4f\n", (double)-0.553415f);
+	} else {
+		printf("\t FAIL: printf(\"%%8.4f\", -0.553415f) != \" -0.5534\", result: %s\n", sbuf);
+		ret = -6;
+	}
+
+
+
+
+
+	printf("\n--- DOUBLE PRECISION TESTS ---\n");
+
+	double d1 = 1.0111;
+	double d2 = 2.0;
+
+	double d1d2 = d1 * d2;
+
+	if (d1d2 == 2.022200000000000219557705349871) {
+		printf("\t success: 1.0111 * 2.0 == 2.0222\n");
+
+	} else {
+		printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %8.4f\n", d1d2);
+		ret = -7;
+	}
+
+	fflush(stdout);
+
+	// Assign value of f1 to d1
+	d1 = f1;
+
+	if (f1 == (float)d1) {
+		printf("\t success: (float) 1.55f == 1.55 (double)\n");
+
+	} else {
+		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1);
+		ret = -8;
+	}
+
+	fflush(stdout);
+
+
+	double sin_zero = sin(0.0);
+	double sin_one = sin(1.0);
+	double atan2_ones = atan2(1.0, 1.0);
+
+	if (sin_zero == 0.0) {
+		printf("\t success: sin(0.0) == 0.0\n");
+
+	} else {
+		printf("\t FAIL: sin(0.0) != 0.0, result: %8.4f\n", sin_zero);
+		ret = -9;
+	}
+
+	if (sin_one == 0.841470984807896504875657228695) {
+		printf("\t success: sin(1.0) == 0.84147098480\n");
+
+	} else {
+		printf("\t FAIL: sin(1.0) != 1.0, result: %8.4f\n", sin_one);
+		ret = -10;
+	}
+
+	if (atan2_ones != 0.785398) {
+		printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
+
+	} else {
+		printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %8.4f\n", atan2_ones);
+		ret = -11;
+	}
+
+	printf("\t testing pow() with magic value\n");
+	printf("\t   (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
+	fflush(stdout);
+	usleep(20000);
+	double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));
+	printf("\t success: result: %8.4f\n", (double)powres);
+
+	sprintf(sbuf, "%8.4f", 0.553415);
+
+	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
+		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
+		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
+		printf("\t success: printf(\"%%8.4f\", 0.553415) == %8.4f\n", 0.553415);
+	} else {
+		printf("\t FAIL: printf(\"%%8.4f\", 0.553415) != \"  0.5534\", result: %s\n", sbuf);
+		ret = -12;
+	}
+
+	sprintf(sbuf, "%8.4f", -0.553415);
+
+	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
+		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
+		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
+		printf("\t success: printf(\"%%8.4f\", -0.553415) == %8.4f\n", -0.553415);
+	} else {
+		printf("\t FAIL: printf(\"%%8.4f\", -0.553415) != \" -0.5534\", result: %s\n", sbuf);
+		ret = -13;
+	}
+
+
+	if (ret == 0) {
+		printf("\n SUCCESS: All float and double tests passed.\n");
+
+	} else {
+		printf("\n FAIL: One or more tests failed.\n");
+	}
+
+	printf("\n");
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c
new file mode 100644
index 000000000..ab536d956
--- /dev/null
+++ b/src/systemcmds/tests/test_gpio.c
@@ -0,0 +1,115 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/analog/adc.h>
+
+#include "tests.h"
+
+#include <drivers/drv_gpio.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_gpio
+ ****************************************************************************/
+
+int test_gpio(int argc, char *argv[])
+{
+	int		fd;
+	int		ret = 0;
+
+	fd = open(GPIO_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		printf("GPIO: open fail\n");
+		return ERROR;
+	}
+
+	/* set all GPIOs to default state */
+	ioctl(fd, GPIO_RESET, ~0);
+
+
+	/* XXX need to add some GPIO waving stuff here */
+
+
+	/* Go back to default */
+	ioctl(fd, GPIO_RESET, ~0);
+
+	printf("\t GPIO test successful.\n");
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c
new file mode 100644
index 000000000..270dc3857
--- /dev/null
+++ b/src/systemcmds/tests/test_hott_telemetry.c
@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_hott_telemetry.c
+ *
+ * Tests the Graupner HoTT telemetry support.
+ *
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <drivers/drv_gpio.h>
+#include <nuttx/config.h>
+#include <sys/types.h>
+#include <systemlib/err.h>
+
+#include <debug.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include "tests.h"
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+static int open_uart(const char *device)
+{
+	/* baud rate */
+	int speed = B19200;
+
+	/* open uart */
+	int uart = open(device, O_RDWR | O_NOCTTY);
+
+	if (uart < 0) {
+		errx(1, "FAIL: Error opening port");
+		return ERROR;
+	}
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(uart, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		errx(1, "FAIL: Error setting baudrate / termios config for cfsetispeed, cfsetospeed");
+		return ERROR;
+	}
+
+	if (tcsetattr(uart, TCSANOW, &uart_config) < 0) {
+		errx(1, "FAIL: Error setting baudrate / termios config for tcsetattr");
+		return ERROR;
+	}
+
+	return uart;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_hott_telemetry
+ ****************************************************************************/
+
+int test_hott_telemetry(int argc, char *argv[])
+{
+	warnx("HoTT Telemetry Test Requirements:");
+	warnx("- Radio on and Electric Air. Mod on (telemetry -> sensor select).");
+	warnx("- Receiver telemetry port must be in telemetry mode.");
+	warnx("- Connect telemetry wire to /dev/ttyS1 (USART2).");
+	warnx("Testing...");
+
+	const char device[] = "/dev/ttyS1";
+	int fd = open_uart(device);
+
+	if (fd < 0) {
+		close(fd);
+		return ERROR;
+	}
+
+	/* Activate single wire mode */
+	ioctl(fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+
+	char send = 'a';
+	write(fd, &send, 1);
+
+	/* Since TX and RX are now connected we should be able to read in what we wrote */
+	const int timeout = 1000;
+	struct pollfd fds[] = { { .fd = fd, .events = POLLIN } };
+
+	if (poll(fds, 1, timeout) == 0) {
+		errx(1, "FAIL: Could not read sent data.");
+	}
+
+	char receive;
+	read(fd, &receive, 1);
+	warnx("PASS: Single wire enabled. Sent %x and received %x", send, receive);
+
+
+	/* Attempt to read HoTT poll messages from the HoTT receiver */
+	int received_count = 0;
+	int valid_count = 0;
+	const int max_polls = 5;
+	uint8_t byte;
+
+	for (; received_count < 5; received_count++) {
+		if (poll(fds, 1, timeout) == 0) {
+			errx(1, "FAIL: Could not read sent data. Is your HoTT receiver plugged in on %s?", device);
+			break;
+
+		} else {
+			read(fd, &byte, 1);
+
+			if (byte == 0x80) {
+				valid_count++;
+			}
+
+			/* Read the device ID being polled */
+			read(fd, &byte, 1);
+		}
+	}
+
+	if (received_count > 0 && valid_count > 0) {
+		if (received_count == max_polls && valid_count == max_polls) {
+			warnx("PASS: Received %d out of %d valid byte pairs from the HoTT receiver device.", received_count, max_polls);
+
+		} else {
+			warnx("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, max_polls, valid_count);
+		}
+
+	} else {
+		/* Let's work out what went wrong */
+		if (received_count == 0) {
+			errx(1, "FAIL: Could not read any polls from HoTT receiver device.");
+		}
+
+		if (valid_count == 0) {
+			errx(1, "FAIL: Received unexpected values from the HoTT receiver device.");
+		}
+	}
+
+
+	/* Attempt to send a HoTT response messages */
+	uint8_t response[] = {0x7c, 0x8e, 0x00, 0xe0, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, \
+			      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, \
+			      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf4, 0x01, 0x00, 0x00, \
+			      0x19, 0x00, 0x00, 0x00, 0x30, 0x75, 0x78, 0x00, 0x00, 0x00, \
+			      0x00, 0x00, 0x00, 0x7d, 0x12
+			     };
+
+	usleep(5000);
+
+	for (unsigned int i = 0; i < sizeof(response); i++) {
+		write(fd, &response[i], 1);
+		usleep(1000);
+	}
+
+	warnx("PASS: Response sent to the HoTT receiver device. Voltage should now show 2.5V.");
+
+
+	/* Disable single wire */
+	ioctl(fd, TIOCSSINGLEWIRE, ~SER_SINGLEWIRE_ENABLED);
+
+	write(fd, &send, 1);
+
+	/* We should timeout as there will be nothing to read (TX and RX no longer connected) */
+	if (poll(fds, 1, timeout) == 0) {
+		errx(1, "FAIL: timeout expected.");
+	}
+
+	warnx("PASS: Single wire disabled.");
+
+	close(fd);
+	exit(0);
+}
diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c
new file mode 100644
index 000000000..f21dd115b
--- /dev/null
+++ b/src/systemcmds/tests/test_hrt.c
@@ -0,0 +1,218 @@
+/****************************************************************************
+ * px4/sensors/test_hrt.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <sys/time.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+extern uint16_t ppm_buffer[];
+extern unsigned ppm_decoded_channels;
+extern uint16_t ppm_edge_history[];
+extern uint16_t ppm_pulse_history[];
+
+int test_ppm(int argc, char *argv[])
+{
+#ifdef CONFIG_HRT_PPM
+	unsigned i;
+
+	printf("channels: %u\n", ppm_decoded_channels);
+
+	for (i = 0; i < ppm_decoded_channels; i++)
+		printf("  %u\n", ppm_buffer[i]);
+
+	printf("edges\n");
+
+	for (i = 0; i < 32; i++)
+		printf("  %u\n", ppm_edge_history[i]);
+
+	printf("pulses\n");
+
+	for (i = 0; i < 32; i++)
+		printf("  %u\n", ppm_pulse_history[i]);
+
+	fflush(stdout);
+#else
+	printf("PPM not configured\n");
+#endif
+	return 0;
+}
+
+int test_tone(int argc, char *argv[])
+{
+	int fd, result;
+	unsigned long tone;
+
+	fd = open("/dev/tone_alarm", O_WRONLY);
+
+	if (fd < 0) {
+		printf("failed opening /dev/tone_alarm\n");
+		goto out;
+	}
+
+	tone = 1;
+
+	if (argc == 2)
+		tone = atoi(argv[1]);
+
+	if (tone  == 0) {
+		result = ioctl(fd, TONE_SET_ALARM, 0);
+
+		if (result < 0) {
+			printf("failed clearing alarms\n");
+			goto out;
+
+		} else {
+			printf("Alarm stopped.\n");
+		}
+
+	} else {
+		result = ioctl(fd, TONE_SET_ALARM, 0);
+
+		if (result < 0) {
+			printf("failed clearing alarms\n");
+			goto out;
+		}
+
+		result = ioctl(fd, TONE_SET_ALARM, tone);
+
+		if (result < 0) {
+			printf("failed setting alarm %lu\n", tone);
+
+		} else {
+			printf("Alarm %lu (disable with: tests tone 0)\n", tone);
+		}
+	}
+
+out:
+
+	if (fd >= 0)
+		close(fd);
+
+	return 0;
+}
+
+/****************************************************************************
+ * Name: test_hrt
+ ****************************************************************************/
+
+int test_hrt(int argc, char *argv[])
+{
+	struct hrt_call call;
+	hrt_abstime prev, now;
+	int i;
+	struct timeval tv1, tv2;
+
+	printf("start-time (hrt, sec/usec), end-time (hrt, sec/usec), microseconds per half second\n");
+
+	for (i = 0; i < 10; i++) {
+		prev = hrt_absolute_time();
+		gettimeofday(&tv1, NULL);
+		usleep(500000);
+		now = hrt_absolute_time();
+		gettimeofday(&tv2, NULL);
+		printf("%lu (%lu/%lu), %lu (%lu/%lu), %lu\n",
+		       (unsigned long)prev, (unsigned long)tv1.tv_sec, (unsigned long)tv1.tv_usec,
+		       (unsigned long)now, (unsigned long)tv2.tv_sec, (unsigned long)tv2.tv_usec,
+		       (unsigned long)(hrt_absolute_time() - prev));
+		fflush(stdout);
+	}
+
+	usleep(1000000);
+
+	printf("one-second ticks\n");
+
+	for (i = 0; i < 10; i++) {
+		hrt_call_after(&call, 1000000, NULL, NULL);
+
+		while (!hrt_called(&call)) {
+			usleep(1000);
+		}
+
+		printf("tick\n");
+		fflush(stdout);
+	}
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c
new file mode 100644
index 000000000..c59cee7b7
--- /dev/null
+++ b/src/systemcmds/tests/test_int.c
@@ -0,0 +1,149 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+typedef union {
+	int32_t i;
+	int64_t l;
+	uint8_t b[8];
+} test_32_64_t;
+
+int test_int(int argc, char *argv[])
+{
+	int ret = 0;
+
+	printf("\n--- 64 BIT MATH TESTS ---\n");
+
+	int64_t large = 354156329598;
+
+	int64_t calc = large * 5;
+
+	if (calc == 1770781647990) {
+		printf("\t success: 354156329598 * 5 == %lld\n", calc);
+
+	} else {
+		printf("\t FAIL: 354156329598 * 5 != %lld\n", calc);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+
+
+
+	printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n");
+
+
+	int32_t small = 50;
+	int32_t large_int = 2147483647; // MAX INT value
+
+	uint64_t small_times_large = large_int * (uint64_t)small;
+
+	if (small_times_large == 107374182350) {
+		printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %lld\n", small_times_large);
+
+	} else {
+		printf("\t FAIL: 50 * 2147483647 != %lld, 64bit cast might fail\n", small_times_large);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	if (ret == 0) {
+		printf("\n SUCCESS: All float and double tests passed.\n");
+
+	} else {
+		printf("\n FAIL: One or more tests failed.\n");
+	}
+
+	printf("\n");
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c
new file mode 100644
index 000000000..10c93b264
--- /dev/null
+++ b/src/systemcmds/tests/test_jig_voltages.c
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <nuttx/analog/adc.h>
+#include <drivers/drv_adc.h>
+#include <systemlib/err.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_gpio
+ ****************************************************************************/
+
+int test_jig_voltages(int argc, char *argv[])
+{
+	int fd = open(ADC_DEVICE_PATH, O_RDONLY);
+	int ret = OK;
+
+	if (fd < 0) {
+		warnx("can't open ADC device");
+		return 1;
+	}
+
+	/* make space for a maximum of eight channels */
+	struct adc_msg_s data[8];
+	/* read all channels available */
+	ssize_t count = read(fd, data, sizeof(data));
+
+	if (count < 0) {
+		close(fd);
+		warnx("can't read from ADC driver. Forgot 'adc start' command?");
+		return 1;
+	}
+
+	unsigned channels = count / sizeof(data[0]);
+
+	for (unsigned j = 0; j < channels; j++) {
+		printf("%d: %u  ", data[j].am_channel, data[j].am_data);
+	}
+
+	printf("\n");
+
+	warnx("\t ADC operational.\n");
+
+	/* Expected values */
+	int16_t expected_min[] = {2800, 2800, 1800,  800};
+	int16_t expected_max[] = {3100, 3100, 2100, 1100};
+	char *check_res[channels];
+
+	if (channels < 4) {
+		close(fd);
+		warnx("not all four test channels available, aborting.");
+		return 1;
+
+	} else {
+		/* Check values */
+		check_res[0] = (expected_min[0] < data[0].am_data && expected_max[0] > data[0].am_data) ? "OK" : "FAIL";
+		check_res[1] = (expected_min[1] < data[1].am_data && expected_max[1] > data[1].am_data) ? "OK" : "FAIL";
+		check_res[2] = (expected_min[2] < data[2].am_data && expected_max[2] > data[2].am_data) ? "OK" : "FAIL";
+		check_res[3] = (expected_min[3] < data[3].am_data && expected_max[3] > data[3].am_data) ? "OK" : "FAIL";
+
+		/* Accumulate result */
+		ret += (expected_min[0] > data[0].am_data || expected_max[0] < data[0].am_data) ? 1 : 0;
+		ret += (expected_min[1] > data[1].am_data || expected_max[1] < data[1].am_data) ? 1 : 0;
+		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]);
+
+		if (ret != OK) {
+			printf("\t JIG voltages test FAILED. Some channels where out of allowed range. Check supply voltages.\n");
+			goto errout_with_dev;
+		}
+	}
+
+	printf("\t JIG voltages test successful.\n");
+
+errout_with_dev:
+
+	if (fd != 0) close(fd);
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_led.c b/src/systemcmds/tests/test_led.c
new file mode 100644
index 000000000..6e3efc668
--- /dev/null
+++ b/src/systemcmds/tests/test_led.c
@@ -0,0 +1,134 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_led.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_led(int argc, char *argv[])
+{
+	int		fd;
+	int		ret = 0;
+
+	fd = open(LED_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		printf("\tLED: open fail\n");
+		return ERROR;
+	}
+
+	if (ioctl(fd, LED_ON, LED_BLUE) ||
+	    ioctl(fd, LED_ON, LED_AMBER)) {
+
+		printf("\tLED: ioctl fail\n");
+		return ERROR;
+	}
+
+	/* let them blink for fun */
+
+	int i;
+	uint8_t ledon = 1;
+
+	for (i = 0; i < 10; i++) {
+		if (ledon) {
+			ioctl(fd, LED_ON, LED_BLUE);
+			ioctl(fd, LED_OFF, LED_AMBER);
+
+		} else {
+			ioctl(fd, LED_OFF, LED_BLUE);
+			ioctl(fd, LED_ON, LED_AMBER);
+		}
+
+		ledon = !ledon;
+		usleep(60000);
+	}
+
+	/* Go back to default */
+	ioctl(fd, LED_ON, LED_BLUE);
+	ioctl(fd, LED_OFF, LED_AMBER);
+
+	printf("\t LED test completed, no errors.\n");
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
new file mode 100644
index 000000000..14503276c
--- /dev/null
+++ b/src/systemcmds/tests/test_sensors.c
@@ -0,0 +1,297 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_sensors.c
+ * Tests the onboard sensors.
+ * 
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int accel(int argc, char *argv[]);
+static int gyro(int argc, char *argv[]);
+static int mag(int argc, char *argv[]);
+static int baro(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+struct {
+	const char	*name;
+	const char	*path;
+	int	(* test)(int argc, char *argv[]);
+} sensors[] = {
+	{"accel",	"/dev/accel",	accel},
+	{"gyro",	"/dev/gyro",	gyro},
+	{"mag",		"/dev/mag",	mag},
+	{"baro",	"/dev/baro",	baro},
+	{NULL, NULL, NULL}
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int
+accel(int argc, char *argv[])
+{
+	printf("\tACCEL: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct accel_report buf;
+	int		ret;
+
+	fd = open("/dev/accel", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tACCEL: open fail, run <mpu6000 start> or <lsm303 start> or <bma180 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 100ms, sensor should have data after no more than 20ms */
+	usleep(100000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tACCEL: read1 fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	// /* wait at least 10ms, sensor should have data after no more than 2ms */
+	// usleep(100000);
+
+	// ret = read(fd, buf, sizeof(buf));
+
+	// if (ret != sizeof(buf)) {
+	// 	printf("\tMPU-6000: read2 fail (%d)\n", ret);
+	// 	return ERROR;
+
+	// } else {
+	// 	printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+	// }
+
+	/* XXX more tests here */
+
+	/* Let user know everything is ok */
+	printf("\tOK: ACCEL passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+gyro(int argc, char *argv[])
+{
+	printf("\tGYRO: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct gyro_report buf;
+	int		ret;
+
+	fd = open("/dev/gyro", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tGYRO: read fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: GYRO passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+mag(int argc, char *argv[])
+{
+	printf("\tMAG: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct mag_report buf;
+	int		ret;
+
+	fd = open("/dev/mag", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tMAG: open fail, run <hmc5883 start> or <lsm303 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tMAG: read fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: MAG passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+baro(int argc, char *argv[])
+{
+	printf("\tBARO: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct baro_report buf;
+	int		ret;
+
+	fd = open("/dev/baro", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tBARO: open fail, run <ms5611 start> or <lps331 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tBARO: read fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: BARO passed all tests successfully\n");
+
+	return OK;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_sensors
+ ****************************************************************************/
+
+int test_sensors(int argc, char *argv[])
+{
+	unsigned	i;
+
+	printf("Running sensors tests:\n\n");
+	fflush(stdout);
+
+	int ret = OK;
+
+	for (i = 0; sensors[i].name; i++) {
+		printf("  sensor: %s\n", sensors[i].name);
+
+		/* Flush and leave enough time for the flush to become effective */
+		fflush(stdout);
+		usleep(50000);
+		/* Test the sensor - if the tests crash at this point, the right sensor name has been printed */
+
+		ret += sensors[i].test(argc, argv);
+	}
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
new file mode 100644
index 000000000..f95760ca8
--- /dev/null
+++ b/src/systemcmds/tests/test_servo.c
@@ -0,0 +1,132 @@
+/****************************************************************************
+ * px4/sensors/test_hrt.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <time.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_servo
+ ****************************************************************************/
+
+int test_servo(int argc, char *argv[])
+{
+	int fd, result;
+	servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
+	servo_position_t pos;
+
+	fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0) {
+		printf("failed opening /dev/pwm_servo\n");
+		goto out;
+	}
+
+	result = read(fd, &data, sizeof(data));
+
+	if (result != sizeof(data)) {
+		printf("failed bulk-reading channel values\n");
+		goto out;
+	}
+
+	printf("Servo readback, pairs of values should match defaults\n");
+
+	for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
+		result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos);
+
+		if (result < 0) {
+			printf("failed reading channel %u\n", i);
+			goto out;
+		}
+
+		printf("%u: %u %u\n", i, pos, data[i]);
+
+	}
+
+	printf("Servos arming at default values\n");
+	result = ioctl(fd, PWM_SERVO_ARM, 0);
+	usleep(5000000);
+	printf("Advancing channel 0 to 1500\n");
+	result = ioctl(fd, PWM_SERVO_SET(0), 1500);
+out:
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_sleep.c b/src/systemcmds/tests/test_sleep.c
new file mode 100644
index 000000000..ae682b542
--- /dev/null
+++ b/src/systemcmds/tests/test_sleep.c
@@ -0,0 +1,100 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_sleep
+ ****************************************************************************/
+
+int test_sleep(int argc, char *argv[])
+{
+	unsigned int nsleeps = 100;
+	printf("\t %d 100ms sleeps\n", nsleeps);
+	fflush(stdout);
+
+	for (unsigned int i = 0; i < nsleeps; i++) {
+		usleep(100000);
+	}
+
+	printf("\t Sleep test successful.\n");
+
+	return OK;
+}
diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c
new file mode 100644
index 000000000..8a164f3fc
--- /dev/null
+++ b/src/systemcmds/tests/test_time.c
@@ -0,0 +1,160 @@
+/****************************************************************************
+ * px4/tests/test_time.c
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <drivers/drv_hrt.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/* emulate hrt_absolute_time using the cycle counter */
+static hrt_abstime
+cycletime(void)
+{
+	static uint64_t basetime;
+	static uint32_t lasttime;
+	uint32_t cycles;
+
+	cycles = *(unsigned long *)0xe0001004;
+
+	if (cycles < lasttime)
+		basetime += 0x100000000ULL;
+
+	lasttime = cycles;
+
+	return (basetime + cycles) / 168;	/* XXX magic number */
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_time(int argc, char *argv[])
+{
+	hrt_abstime h, c;
+	int64_t lowdelta, maxdelta = 0;
+	int64_t delta, deltadelta;
+
+	/* enable the cycle counter */
+	(*(unsigned long *)0xe000edfc) |= (1 << 24);    /* DEMCR |= DEMCR_TRCENA */
+	(*(unsigned long *)0xe0001000) |= 1;    	/* DWT_CTRL |= DWT_CYCCNT_ENA */
+
+	/* get an average delta between the two clocks - this should stay roughly the same */
+	delta = 0;
+
+	for (unsigned i = 0; i < 100; i++) {
+		uint32_t flags = irqsave();
+
+		h = hrt_absolute_time();
+		c = cycletime();
+
+		irqrestore(flags);
+
+		delta += h - c;
+	}
+
+	lowdelta = abs(delta / 100);
+
+	/* loop checking the time */
+	for (unsigned i = 0; i < 100; i++) {
+
+		usleep(rand());
+
+		uint32_t flags = irqsave();
+
+		c = cycletime();
+		h = hrt_absolute_time();
+
+		irqrestore(flags);
+
+		delta = abs(h - c);
+		deltadelta = abs(delta - lowdelta);
+
+		if (deltadelta > maxdelta)
+			maxdelta = deltadelta;
+
+		if (deltadelta > 1000)
+			fprintf(stderr, "h %llu  c %llu  d %lld\n", h, c, delta - lowdelta);
+	}
+
+	printf("Maximum jitter %lldus\n", maxdelta);
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c
new file mode 100644
index 000000000..609a65c62
--- /dev/null
+++ b/src/systemcmds/tests/test_uart_baudchange.c
@@ -0,0 +1,158 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *            Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_uart_baudchange(int argc, char *argv[])
+{
+	int uart2_nwrite = 0;
+
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+
+	if (uart2 < 0) {
+		printf("ERROR opening UART2, aborting..\n");
+		return uart2;
+	}
+
+	struct termios uart2_config;
+
+	struct termios uart2_config_original;
+
+	int termios_state = 0;
+
+#define UART_BAUDRATE_RUNTIME_CONF
+#ifdef UART_BAUDRATE_RUNTIME_CONF
+
+	if ((termios_state = tcgetattr(uart2, &uart2_config)) < 0) {
+		printf("ERROR getting termios config for UART2: %d\n", termios_state);
+		exit(termios_state);
+	}
+
+	memcpy(&uart2_config_original, &uart2_config, sizeof(struct termios));
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart2_config, B9600) < 0 || cfsetospeed(&uart2_config, B9600) < 0) {
+		printf("ERROR setting termios config for UART2: %d\n", termios_state);
+		exit(ERROR);
+	}
+
+	if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config)) < 0) {
+		printf("ERROR setting termios config for UART2\n");
+		exit(termios_state);
+	}
+
+	/* Set back to original settings */
+	if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config_original)) < 0) {
+		printf("ERROR setting termios config for UART2\n");
+		exit(termios_state);
+	}
+
+#endif
+
+	uint8_t sample_uart2[] = {'U', 'A', 'R', 'T', '2', ' ', '#', 0, '\n'};
+
+	int i, r;
+
+	for (i = 0; i < 100; i++) {
+		/* uart2 -> */
+		r = write(uart2, sample_uart2, sizeof(sample_uart2));
+
+		if (r > 0)
+			uart2_nwrite += r;
+	}
+
+	close(uart2);
+
+	printf("uart2_nwrite %d\n", uart2_nwrite);
+
+	return OK;
+}
diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c
new file mode 100644
index 000000000..f8582b52f
--- /dev/null
+++ b/src/systemcmds/tests/test_uart_console.c
@@ -0,0 +1,175 @@
+/****************************************************************************
+ * px4/sensors/test_uart_console.c
+ *
+ *   Copyright (C) 2012 Lorenz Meier. All rights reserved.
+ *   Authors: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <drivers/drv_hrt.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+static void *receive_loop(void *arg)
+{
+	int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY);
+
+	while (1) {
+		char c;
+		read(uart_usb, &c, 1);
+		printf("%c", c);
+		fflush(stdout);
+	}
+
+	return NULL;
+}
+
+int test_uart_console(int argc, char *argv[])
+{
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	//
+	int uart_usb = open("/dev/ttyACM0", O_WRONLY | O_NOCTTY);
+
+	if (uart_usb < 0) {
+		printf("ERROR opening /dev/ttyACM0. Do you need to run sercon first? Aborting..\n");
+		return uart_usb;
+	}
+
+	uint8_t sample_uart_usb[] = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'};
+
+	pthread_t receive_thread;
+
+	pthread_create(&receive_thread, NULL, receive_loop, NULL);
+
+	//wait for threads to complete:
+	pthread_join(receive_thread, NULL);
+
+	for (int i = 0; i < 30; i++) {
+		write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb));
+		printf(".");
+		fflush(stdout);
+		sleep(1);
+	}
+
+//	uint64_t start_time = hrt_absolute_time();
+//
+////	while (true)
+//	for (int i = 0; i < 1000; i++)
+//	{
+//		//write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb));
+//		int nread = 0;
+//		char c;
+//		do {
+//			nread = read(uart_usb, &c, 1);
+//			if (nread > 0)
+//			{
+//				printf("%c", c);
+//			}
+//		} while (nread > 0);
+//
+//		do {
+//			nread = read(uart_console, &c, 1);
+//			if (nread > 0)
+//			{
+//				if (c == 0x03)
+//				{
+//					close(uart_usb);
+//					close(uart_console);
+//					exit(OK);
+//				}
+//				else
+//				{
+//					write(uart_usb, &c, 1);
+//				}
+//			}
+//		} while (nread > 0);
+//		usleep(10000);
+//	}
+//
+//	int interval = hrt_absolute_time() - start_time;
+
+	close(uart_usb);
+//	close(uart_console);
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c
new file mode 100644
index 000000000..3be152004
--- /dev/null
+++ b/src/systemcmds/tests/test_uart_loopback.c
@@ -0,0 +1,195 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *            Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_uart_loopback(int argc, char *argv[])
+{
+
+	int uart5_nread = 0;
+	int uart2_nread = 0;
+	int uart5_nwrite = 0;
+	int uart2_nwrite = 0;
+
+	int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); //
+
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+	int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+
+	if (uart2 < 0) {
+		printf("ERROR opening UART2, aborting..\n");
+		return uart2;
+	}
+
+	if (uart5 < 0) {
+		printf("ERROR opening UART5, aborting..\n");
+		exit(uart5);
+	}
+
+	uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
+	uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
+	uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
+
+	int i, r;
+
+	for (i = 0; i < 1000; i++) {
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		/* uart2 -> uart5 */
+		r = write(uart2, sample_uart2, sizeof(sample_uart2));
+
+		if (r > 0)
+			uart2_nwrite += r;
+
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		/* uart2 -> uart5 */
+		r = write(uart5, sample_uart5, sizeof(sample_uart5));
+
+		if (r > 0)
+			uart5_nwrite += r;
+
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		/* try to read back values */
+		do {
+			r = read(uart5, sample_uart2, sizeof(sample_uart2));
+
+			if (r > 0)
+				uart5_nread += r;
+		} while (r > 0);
+
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		do {
+			r = read(uart2, sample_uart5, sizeof(sample_uart5));
+
+			if (r > 0)
+				uart2_nread += r;
+		} while (r > 0);
+
+//		printf("TEST #%d\n",i);
+//		write(uart1, sample_uart1, sizeof(sample_uart5));
+	}
+
+	for (i = 0; i < 200000; i++) {
+
+		/* try to read back values */
+		r = read(uart5, sample_uart2, sizeof(sample_uart2));
+
+		if (r > 0)
+			uart5_nread += r;
+
+		r = read(uart2, sample_uart5, sizeof(sample_uart5));
+
+		if (r > 0)
+			uart2_nread += r;
+
+		if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite))
+			break;
+	}
+
+
+	close(uart1);
+	close(uart2);
+	close(uart5);
+
+	printf("uart2_nwrite %d\n", uart2_nwrite);
+	printf("uart5_nwrite %d\n", uart5_nwrite);
+	printf("uart2_nread  %d\n", uart2_nread);
+	printf("uart5_nread  %d\n", uart5_nread);
+
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c
new file mode 100644
index 000000000..7e1e8d307
--- /dev/null
+++ b/src/systemcmds/tests/test_uart_send.c
@@ -0,0 +1,131 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *            Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <drivers/drv_hrt.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_uart_send(int argc, char *argv[])
+{
+	/* input handling */
+	char *uart_name = "/dev/ttyS3";
+
+	if (argc > 1) uart_name = argv[1];
+
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); //
+
+	if (test_uart < 0) {
+		printf("ERROR opening UART %s, aborting..\n", uart_name);
+		return test_uart;
+
+	} else {
+		printf("Writing to UART %s\n", uart_name);
+	}
+
+	char sample_test_uart[25];// = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'};
+
+	int i, n;
+
+	uint64_t start_time = hrt_absolute_time();
+
+	for (i = 0; i < 30000; i++) {
+		n = sprintf(sample_test_uart, "SAMPLE #%d\n", i);
+		write(test_uart, sample_test_uart, n);
+	}
+
+	int interval = hrt_absolute_time() - start_time;
+
+	int bytes = i * sizeof(sample_test_uart);
+
+	printf("Wrote %d bytes in %d ms on UART %s\n", bytes, interval / 1000, uart_name);
+
+	close(test_uart);
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
new file mode 100644
index 000000000..c02ea6808
--- /dev/null
+++ b/src/systemcmds/tests/tests.h
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef __APPS_PX4_TESTS_H
+#define __APPS_PX4_TESTS_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * 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
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+extern int	test_sensors(int argc, char *argv[]);
+extern int	test_gpio(int argc, char *argv[]);
+extern int	test_hrt(int argc, char *argv[]);
+extern int	test_tone(int argc, char *argv[]);
+extern int	test_led(int argc, char *argv[]);
+extern int	test_adc(int argc, char *argv[]);
+extern int	test_int(int argc, char *argv[]);
+extern int	test_float(int argc, char *argv[]);
+extern int	test_ppm(int argc, char *argv[]);
+extern int	test_servo(int argc, char *argv[]);
+extern int	test_uart_loopback(int argc, char *argv[]);
+extern int	test_uart_baudchange(int argc, char *argv[]);
+extern int	test_cpuload(int argc, char *argv[]);
+extern int	test_uart_send(int argc, char *argv[]);
+extern int	test_sleep(int argc, char *argv[]);
+extern int	test_time(int argc, char *argv[]);
+extern int	test_uart_console(int argc, char *argv[]);
+extern int	test_hott_telemetry(int argc, char *argv[]);
+extern int	test_jig_voltages(int argc, char *argv[]);
+extern int	test_param(int argc, char *argv[]);
+extern int	test_bson(int argc, char *argv[]);
+extern int	test_file(int argc, char *argv[]);
+
+#endif /* __APPS_PX4_TESTS_H */
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c
new file mode 100644
index 000000000..6f75b9812
--- /dev/null
+++ b/src/systemcmds/tests/tests_file.c
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_file.c
+ *
+ * File write test.
+ */
+
+#include <sys/stat.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+int
+test_file(int argc, char *argv[])
+{
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+	uint8_t buf[512];
+	hrt_abstime start, end;
+	perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes");
+
+	int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+	memset(buf, 0, sizeof(buf));
+
+	start = hrt_absolute_time();
+	for (unsigned i = 0; i < 1024; i++) {
+		perf_begin(wperf);
+		write(fd, buf, sizeof(buf));
+		perf_end(wperf);
+	}
+	end = hrt_absolute_time();
+
+	close(fd);
+
+	warnx("512KiB in %llu microseconds", end - start);
+	perf_print_counter(wperf);
+	perf_free(wperf);
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
new file mode 100644
index 000000000..9f8c5c9ea
--- /dev/null
+++ b/src/systemcmds/tests/tests_main.c
@@ -0,0 +1,339 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_main.c
+ * Tests main file, loads individual tests.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/spi.h>
+
+#include <systemlib/perf_counter.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int test_help(int argc, char *argv[]);
+static int test_all(int argc, char *argv[]);
+static int test_perf(int argc, char *argv[]);
+static int test_jig(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+const struct {
+	const char 	*name;
+	int	(* fn)(int argc, char *argv[]);
+	unsigned	options;
+#define OPT_NOHELP	(1<<0)
+#define OPT_NOALLTEST	(1<<1)
+#define OPT_NOJIGTEST	(1<<2)
+} tests[] = {
+	{"led",			test_led,	0},
+	{"int",			test_int,	0},
+	{"float",		test_float,	0},
+	{"sensors",		test_sensors,	0},
+	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"adc",			test_adc,	OPT_NOJIGTEST},
+	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
+	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"hott_telemetry",	test_hott_telemetry,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"tone",		test_tone,	0},
+	{"sleep",		test_sleep,	OPT_NOJIGTEST},
+	{"time",		test_time,	OPT_NOJIGTEST},
+	{"perf",		test_perf,	OPT_NOJIGTEST},
+	{"all",			test_all,	OPT_NOALLTEST | OPT_NOJIGTEST},
+	{"jig",			test_jig,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"param",		test_param,	0},
+	{"bson",		test_bson,	0},
+	{"file",		test_file,	0},
+	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
+	{NULL,			NULL, 		0}
+};
+
+#define NTESTS (sizeof(tests) / sizeof(tests[0]))
+
+static int
+test_help(int argc, char *argv[])
+{
+	unsigned	i;
+
+	printf("Available tests:\n");
+
+	for (i = 0; tests[i].name; i++)
+		printf("  %s\n", tests[i].name);
+
+	return 0;
+}
+
+static int
+test_all(int argc, char *argv[])
+{
+	unsigned	i;
+	char		*args[2] = {"all", NULL};
+	unsigned int failcount = 0;
+	unsigned int testcount = 0;
+	bool		passed[NTESTS];
+
+	printf("\nRunning all tests...\n\n");
+
+	for (i = 0; tests[i].name; i++) {
+		/* Only run tests that are not excluded */
+		if (!(tests[i].options & OPT_NOALLTEST)) {
+			printf("  [%s] \t\t\tSTARTING TEST\n", tests[i].name);
+			fflush(stdout);
+
+			/* Execute test */
+			if (tests[i].fn(1, args) != 0) {
+				fprintf(stderr, "  [%s] \t\t\tFAIL\n", tests[i].name);
+				fflush(stderr);
+				failcount++;
+				passed[i] = false;
+			} else {
+				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				fflush(stdout);
+				passed[i] = true;
+			}
+			testcount++;
+		}
+	}
+
+	/* Print summary */
+	printf("\n");
+	int j;
+
+	for (j = 0; j < 40; j++) {
+		printf("-");
+	}
+
+	printf("\n\n");
+
+	printf("     T E S T    S U M M A R Y\n\n");
+
+	if (failcount == 0) {
+		printf("  ______     __         __            ______     __  __    \n");
+		printf(" /\\  __ \\   /\\ \\       /\\ \\          /\\  __ \\   /\\ \\/ /    \n");
+		printf(" \\ \\  __ \\  \\ \\ \\____  \\ \\ \\____     \\ \\ \\/\\ \\  \\ \\  _\"-.  \n");
+		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
+		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
+		printf("\n");
+		printf(" All tests passed (%d of %d)\n", testcount, testcount);
+
+	} else {
+		printf("  ______   ______     __     __ \n");
+		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
+		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
+		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
+		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
+		printf("\n");
+		printf(" Some tests failed (%d of %d)\n", failcount, testcount);
+	}
+
+	printf("\n");
+
+	/* Print failed tests */
+	if (failcount > 0) printf(" Failed tests:\n\n");
+
+	unsigned int k;
+
+	for (k = 0; k < i; k++) {
+		if (!passed[k] && !(tests[k].options & OPT_NOALLTEST)) {
+			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
+		}
+	}
+
+	fflush(stdout);
+
+	return 0;
+}
+
+static int
+test_perf(int argc, char *argv[])
+{
+	perf_counter_t	cc, ec;
+
+	cc = perf_alloc(PC_COUNT, "test_count");
+	ec = perf_alloc(PC_ELAPSED, "test_elapsed");
+
+	if ((cc == NULL) || (ec == NULL)) {
+		printf("perf: counter alloc failed\n");
+		return 1;
+	}
+
+	perf_begin(ec);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	printf("perf: expect count of 4\n");
+	perf_print_counter(cc);
+	perf_end(ec);
+	printf("perf: expect count of 1\n");
+	perf_print_counter(ec);
+	printf("perf: expect at least two counters\n");
+	perf_print_all();
+
+	perf_free(cc);
+	perf_free(ec);
+
+	return OK;
+}
+
+int test_jig(int argc, char *argv[])
+{
+	unsigned	i;
+	char		*args[2] = {"jig", NULL};
+	unsigned int failcount = 0;
+	unsigned int testcount = 0;
+	bool		passed[NTESTS];
+
+	printf("\nRunning all tests...\n\n");
+	for (i = 0; tests[i].name; i++) {
+		/* Only run tests that are not excluded */
+		if (!(tests[i].options & OPT_NOJIGTEST)) {
+			printf("  [%s] \t\t\tSTARTING TEST\n", tests[i].name);
+			fflush(stdout);
+			/* Execute test */
+			if (tests[i].fn(1, args) != 0) {
+				fprintf(stderr, "  [%s] \t\t\tFAIL\n", tests[i].name);
+				fflush(stderr);
+				failcount++;
+				passed[i] = false;
+			} else {
+				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				fflush(stdout);
+				passed[i] = true;
+			}
+			testcount++;
+		}
+	}
+
+	/* Print summary */
+	printf("\n");
+	int j;
+	for (j = 0; j < 40; j++)
+	{
+		printf("-");
+	}
+	printf("\n\n");
+
+	printf("     T E S T    S U M M A R Y\n\n");
+	if (failcount == 0) {
+		printf("  ______     __         __            ______     __  __    \n");
+		printf(" /\\  __ \\   /\\ \\       /\\ \\          /\\  __ \\   /\\ \\/ /    \n");
+		printf(" \\ \\  __ \\  \\ \\ \\____  \\ \\ \\____     \\ \\ \\/\\ \\  \\ \\  _\"-.  \n");
+		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
+		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
+		printf("\n");
+		printf(" All tests passed (%d of %d)\n", testcount, testcount);
+	} else {
+		printf("  ______   ______     __     __ \n");
+		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
+		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
+		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
+		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
+		printf("\n");
+		printf(" Some tests failed (%d of %d)\n", failcount, testcount);
+	}
+	printf("\n");
+
+	/* Print failed tests */
+	if (failcount > 0) printf(" Failed tests:\n\n");
+	unsigned int k;
+	for (k = 0; k < i; k++)
+	{
+		if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST))
+		{
+			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
+		}
+	}
+	fflush(stdout);
+
+	return 0;
+}
+
+__EXPORT int tests_main(int argc, char *argv[]);
+
+/**
+ * Executes system tests.
+ */
+int tests_main(int argc, char *argv[])
+{
+	unsigned	i;
+
+	if (argc < 2) {
+		printf("tests: missing test name - 'tests help' for a list of tests\n");
+		return 1;
+	}
+
+	for (i = 0; tests[i].name; i++) {
+		if (!strcmp(tests[i].name, argv[1]))
+			return tests[i].fn(argc - 1, argv + 1);
+	}
+
+	printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]);
+	return ERROR;
+}
diff --git a/src/systemcmds/tests/tests_param.c b/src/systemcmds/tests/tests_param.c
new file mode 100644
index 000000000..13f17bc43
--- /dev/null
+++ b/src/systemcmds/tests/tests_param.c
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_param.c
+ *
+ * Tests related to the parameter system.
+ */
+
+#include <stdio.h>
+#include "systemlib/err.h"
+
+#include "systemlib/param/param.h"
+#include "tests.h"
+
+PARAM_DEFINE_INT32(test, 0x12345678);
+
+int
+test_param(int argc, char *argv[])
+{
+	param_t		p;
+
+	p = param_find("test");
+	if (p == PARAM_INVALID)
+		errx(1, "test parameter not found");
+
+	param_type_t t = param_type(p);
+	if (t != PARAM_TYPE_INT32)
+		errx(1, "test parameter type mismatch (got %u)", (unsigned)t);
+
+	int32_t	val;
+	if (param_get(p, &val) != 0)
+		errx(1, "failed to read test parameter");
+	if (val != 0x12345678)
+		errx(1, "parameter value mismatch");
+
+	val = 0xa5a5a5a5;
+	if (param_set(p, &val) != 0)
+		errx(1, "failed to write test parameter");
+	if (param_get(p, &val) != 0)
+		errx(1, "failed to re-read test parameter");
+	if ((uint32_t)val != 0xa5a5a5a5)
+		errx(1, "parameter value mismatch after write");
+
+	warnx("parameter test PASS");
+
+	return 0;
+}