Fresh import of the PX4 firmware sources.

20 files changed, 3700 insertions, 0 deletions

diff --git a/apps/px4/tests/.context b/apps/px4/tests/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/px4/tests/.context
diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile
new file mode 100644
index 000000000..41979f85a
--- /dev/null
+++ b/apps/px4/tests/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Makefile to build assorted test cases
+#
+
+APPNAME		 = tests
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 4096
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c
new file mode 100644
index 000000000..7795f07ac
--- /dev/null
+++ b/apps/px4/tests/test_adc.c
@@ -0,0 +1,211 @@
+/****************************************************************************
+ * 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 <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>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_gpio
+ ****************************************************************************/
+
+int test_adc(int argc, char *argv[])
+{
+	int		fd0;
+	int		ret = 0;
+	//struct adc_msg_s sample[4],sample2[4],sample3[4],sample4[4],sample5[4],sample6[4],sample7[4],sample8[4],sample9[4];
+
+	struct adc_msg4_s {
+		uint8_t      am_channel1;               /* The 8-bit ADC Channel */
+		int32_t      am_data1;                  /* ADC convert result (4 bytes) */
+		uint8_t      am_channel2;               /* The 8-bit ADC Channel */
+		int32_t      am_data2;                  /* ADC convert result (4 bytes) */
+		uint8_t      am_channel3;               /* The 8-bit ADC Channel */
+		int32_t      am_data3;                  /* ADC convert result (4 bytes) */
+		uint8_t      am_channel4;               /* The 8-bit ADC Channel */
+		int32_t      am_data4;                  /* ADC convert result (4 bytes) */
+	} __attribute__((__packed__));;
+
+	struct adc_msg4_s sample1[4], sample2[4];
+
+	size_t readsize;
+	ssize_t nbytes, nbytes2;
+	int i;
+	int j;
+	int errval;
+
+	for (j = 0; j < 1; j++) {
+		char name[11];
+		sprintf(name, "/dev/adc%d", j);
+		fd0 = open(name, O_RDONLY | O_NONBLOCK);
+
+		if (fd0 < 0) {
+			printf("ADC: %s open fail\n", name);
+			return ERROR;
+
+		} else {
+			printf("Opened %s successfully\n", name);
+		}
+
+		/* first adc read round */
+		readsize = 4 * sizeof(struct adc_msg_s);
+		up_udelay(10000);//microseconds
+		nbytes = read(fd0, sample1, readsize);
+		up_udelay(10000);//microseconds
+		nbytes2 = read(fd0, sample2, readsize);
+//	nbytes2 = read(fd0, sample3, readsize);
+//	nbytes2 = read(fd0, sample4, readsize);
+//	nbytes2 = read(fd0, sample5, readsize);
+//	nbytes2 = read(fd0, sample6, readsize);
+//	nbytes2 = read(fd0, sample7, readsize);
+//	nbytes2 = read(fd0, sample8, readsize);
+		//nbytes2 = read(fd0, sample9, readsize);
+
+		/* Handle unexpected return values */
+
+		if (nbytes < 0) {
+			errval = errno;
+
+			if (errval != EINTR) {
+				message("read %s failed: %d\n",
+					name, errval);
+				errval = 3;
+				goto errout_with_dev;
+			}
+
+			message("\tInterrupted read...\n");
+
+		} else if (nbytes == 0) {
+			message("\tNo data read, Ignoring\n");
+		}
+
+		/* Print the sample data on successful return */
+
+		else {
+			int nsamples = nbytes / sizeof(struct adc_msg_s);
+
+			if (nsamples * sizeof(struct adc_msg_s) != nbytes) {
+				message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n",
+					nbytes, sizeof(struct adc_msg_s));
+
+			} else {
+				message("Sample:");
+
+				for (i = 0; i < 1 ; i++) {
+					message("%d: channel: %d value: %d\n",
+						i, sample1[i].am_channel1, sample1[i].am_data1);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i, sample1[i].am_channel2, sample1[i].am_data2);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i, sample1[i].am_channel3, sample1[i].am_data3);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i, sample1[i].am_channel4, sample1[i].am_data4);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i + 1, sample2[i].am_channel1, sample2[i].am_data1);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i + 1, sample2[i].am_channel2, sample2[i].am_data2);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i + 1, sample2[i].am_channel3, sample2[i].am_data3);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i + 1, sample2[i].am_channel4, sample2[i].am_data4);
+//    			message("%d: channel: %d value: %d\n",
+//						i, sample9[i].am_channel, sample9[i].am_data);
+				}
+			}
+		}
+	}
+
+	printf("\t ADC test successful.\n");
+
+errout_with_dev:
+
+	if (fd0 != 0) close(fd0);
+
+	return ret;
+}
diff --git a/apps/px4/tests/test_eeproms.c b/apps/px4/tests/test_eeproms.c
new file mode 100644
index 000000000..29ca8267f
--- /dev/null
+++ b/apps/px4/tests/test_eeproms.c
@@ -0,0 +1,328 @@
+/****************************************************************************
+ * px4/eeproms/test_eeproms.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 <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <arch/board/drv_eeprom.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int  onboard_eeprom(int argc, char *argv[]);
+static int	baseboard_eeprom(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+struct {
+	const char	*name;
+	const char	*path;
+	int	(* test)(int argc, char *argv[]);
+} eeproms[] = {
+	{"onboard_eeprom",	"/dev/eeprom",	onboard_eeprom},
+	{"baseboard_eeprom",	"/dev/baseboard_eeprom",	baseboard_eeprom},
+	{NULL, NULL, NULL}
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int
+onboard_eeprom(int argc, char *argv[])
+{
+	printf("\tonboard_eeprom: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	uint8_t	buf1[210] = {' ', 'P', 'X', '4', ' ', 'E', 'E', 'P', 'R', 'O', 'M', ' ', 'T', 'E', 'S', 'T', ' '};
+	int		ret;
+	bool force_write = false;
+	if (strcmp(argv[0], "jig") == 0) force_write = true;
+
+	/* fill with spaces */
+	//memset(buf1+16, 'x', sizeof(buf1-16));
+
+	/* fill in some magic values at magic positions */
+	buf1[63] = 'E';
+	buf1[64] = 'S';
+	buf1[127] = 'F';
+	buf1[128] = 'T';
+
+	/* terminate string */
+	buf1[sizeof(buf1) - 1] = '\0';
+
+	fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK);
+
+	if (fd < 0) {
+		printf("onboard eeprom: open fail\n");
+		return ERROR;
+	}
+
+	/* read data */
+	ret = read(fd, buf1, 1);
+
+	if (ret != 1) {
+		printf("\tonboard eeprom: ERROR: reading first byte fail: %d\n", ret);
+
+		switch (-ret) {
+		case EPERM:
+			printf("\treason: %s\n", EPERM_STR);
+			break;
+
+		case ENOENT:
+			printf("\treason: %s\n", ENOENT_STR);
+			break;
+
+		case ESRCH:
+			printf("\treason: %s\n", ESRCH_STR);
+			break;
+
+		case EINTR:
+			printf("\treason: %s\n", EINTR_STR);
+			break;
+
+		}
+	}
+
+	printf("\tonboard eeprom: first byte: %d\n", buf1[0]);
+	if (!force_write) {
+		printf("\tonboard eeprom: WARNING: FURTHER TEST STEPS WILL DESTROY YOUR FLIGHT PARAMETER CONFIGURATION. PROCEED? (y/N)\n");
+
+		printf("Input: ");
+		char c = getchar();
+		printf("%c\n", c);
+		if (c != 'y' && c != 'Y') {
+			/* not yes, abort */
+			close(fd);
+
+			/* Let user know everything is ok */
+			printf("\tOK: onboard eeprom test aborted by user, read test successful\r\n");
+			return OK;
+		}
+	}
+
+	printf("\tonboard eeprom: proceeding with write test\r\n");
+
+	/* increment counter */
+	buf1[0] = buf1[0] + 1;
+
+	/* rewind to the start of the file */
+	lseek(fd, 0, SEEK_SET);
+
+	/* write data */
+	ret = write(fd, buf1, sizeof(buf1));
+
+	if (ret != sizeof(buf1)) {
+		printf("\tonboard eeprom: ERROR: write fail: %d\n", (char)ret);
+
+		switch (-ret) {
+		case EPERM:
+			printf("\treason: %s\n", EPERM_STR);
+			break;
+
+		case ENOENT:
+			printf("\treason: %s\n", ENOENT_STR);
+			break;
+
+		case ESRCH:
+			printf("\treason: %s\n", ESRCH_STR);
+			break;
+
+		case EINTR:
+			printf("\treason: %s\n", EINTR_STR);
+			break;
+
+		}
+
+		//return ERROR;
+	}
+
+	/* rewind to the start of the file */
+	lseek(fd, 0, SEEK_SET);
+
+	/* read data */
+	ret = read(fd, buf1, sizeof(buf1));
+
+	if (ret != sizeof(buf1)) {
+		printf("\tonboard eeprom: ERROR: read fail: %d\n", ret);
+
+		switch (-ret) {
+		case EPERM:
+			printf("\treason: %s\n", EPERM_STR);
+			break;
+
+		case ENOENT:
+			printf("\treason: %s\n", ENOENT_STR);
+			break;
+
+		case ESRCH:
+			printf("\treason: %s\n", ESRCH_STR);
+			break;
+
+		case EINTR:
+			printf("\treason: %s\n", EINTR_STR);
+			break;
+
+		}
+
+		return ERROR;
+
+	} else {
+		/* enforce null termination and print as string */
+		if (buf1[sizeof(buf1) - 1] != 0) {
+			printf("\tWARNING: Null termination in file not present as expected, enforcing it now..\r\n");
+			buf1[sizeof(buf1) - 1] = '\0';
+		}
+
+		/* read out counter and replace val */
+		int counter = buf1[0];
+		printf("\tonboard eeprom: count: #%d, read values: %s\n", counter, (char *)buf1 + 1);
+		printf("\tAll %d bytes:\n\n\t", sizeof(buf1));
+
+		for (int i = 0; i < sizeof(buf1); i++) {
+			printf("0x%02x ", buf1[i]);
+
+			if (i % 8 == 7) printf("\n\t");
+
+			if (i % 64 == 63) printf("\n\t");
+		}
+
+		/* end any open line */
+		printf("\n\n");
+	}
+
+	close(fd);
+
+	/* Let user know everything is ok */
+	printf("\tOK: onboard eeprom passed all tests successfully\n");
+	return ret;
+}
+
+static int
+baseboard_eeprom(int argc, char *argv[])
+{
+	printf("\tbaseboard eeprom: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	uint8_t	buf[128] = {'R', 'E', 'A', 'D', ' ', 'F', 'A', 'I', 'L', 'E', 'D', '\0'};
+	int		ret;
+
+	fd = open("/dev/baseboard_eeprom", O_RDONLY | O_NONBLOCK);
+
+	if (fd < 0) {
+		printf("\tbaseboard eeprom: open fail\n");
+		return ERROR;
+	}
+
+	/* read data */
+	ret = read(fd, buf, sizeof(buf));
+	/* set last char to string termination */
+	buf[127] = '\0';
+
+	if (ret != sizeof(buf)) {
+		printf("\tbaseboard eeprom: ERROR: read fail\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tbaseboard eeprom: string: %s\n", (char *)buf);
+	}
+
+	close(fd);
+
+	/* XXX more tests here */
+
+	/* Let user know everything is ok */
+	printf("\tOK: baseboard eeprom passed all tests successfully\n");
+	return ret;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_eeproms
+ ****************************************************************************/
+
+int test_eeproms(int argc, char *argv[])
+{
+	unsigned	i;
+
+	printf("Running EEPROMs tests:\n\n");
+	fflush(stdout);
+
+	for (i = 0; eeproms[i].name; i++) {
+		printf("  eeprom: %s\n", eeproms[i].name);
+		eeproms[i].test(argc, argv);
+		fflush(stdout);
+		/* wait 100 ms to make sure buffer is emptied */
+		usleep(100000);
+	}
+
+	return 0;
+}
diff --git a/apps/px4/tests/test_float.c b/apps/px4/tests/test_float.c
new file mode 100644
index 000000000..02e748e70
--- /dev/null
+++ b/apps/px4/tests/test_float.c
@@ -0,0 +1,282 @@
+/****************************************************************************
+ * 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 <arch/board/drv_led.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 {
+	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 fabs(),\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: %f\n", sinf_zero);
+		ret = -4;
+	}
+
+	fflush(stdout);
+
+	if (fabs((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: %f\n", sinf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	float asinf_one = asinf(1.0f);
+
+	if (fabs((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 (fabs((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: %f\n", cosf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+	float acosf_one = acosf(1.0f);
+
+	if (fabs((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: %f\n", 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: %f\n", 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: %f\n", sinf_zero_one);
+		ret = -3;
+	}
+
+	float atan2f_ones = atan2(1.0f, 1.0f);
+
+	if (fabs(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: %f\n", atan2f_ones);
+		ret = -4;
+	}
+
+	printf("\t testing printing: printf(0.553415f): %f\n", 0.553415f);
+
+
+
+
+
+	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: %f\n", d1d2);
+	}
+
+	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: %f\n", f1);
+		ret = -4;
+	}
+
+	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: %f\n", sin_zero);
+		ret = -5;
+	}
+
+	if (sin_one == 0.841470984807896504875657228695) {
+		printf("\t success: sin(1.0) == 0.84147098480\n");
+
+	} else {
+		printf("\t FAIL: sin(1.0) != 1.0, result: %f\n", sin_one);
+		ret = -6;
+	}
+
+	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: %f\n", atan2_ones);
+		ret = -7;
+	}
+
+	printf("\t testing printing: printf(0.553415): %f\n", 0.553415);
+
+	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: %f\n", (float)powres);
+
+
+	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/apps/px4/tests/test_gpio.c b/apps/px4/tests/test_gpio.c
new file mode 100644
index 000000000..ab536d956
--- /dev/null
+++ b/apps/px4/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/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c
new file mode 100644
index 000000000..41f207b7e
--- /dev/null
+++ b/apps/px4/tests/test_hrt.c
@@ -0,0 +1,219 @@
+/****************************************************************************
+ * 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 <unistd.h>
+
+#include <arch/board/board.h>
+#include <arch/board/up_hrt.h>
+#include <arch/board/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[])
+{
+#ifdef CONFIG_TONE_ALARM
+	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);
+
+#endif
+	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/apps/px4/tests/test_int.c b/apps/px4/tests/test_int.c
new file mode 100644
index 000000000..40e030641
--- /dev/null
+++ b/apps/px4/tests/test_int.c
@@ -0,0 +1,151 @@
+/****************************************************************************
+ * 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 <arch/board/drv_led.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/apps/px4/tests/test_jig_voltages.c b/apps/px4/tests/test_jig_voltages.c
new file mode 100644
index 000000000..51f9b9a5b
--- /dev/null
+++ b/apps/px4/tests/test_jig_voltages.c
@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ *   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>
+
+
+/****************************************************************************
+ * 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		fd0 = 0;
+	int		ret = OK;
+	const int nchannels = 4;
+
+	struct adc_msg4_s
+	{
+	  uint8_t      am_channel1;               /* The 8-bit ADC Channel */
+	  int32_t      am_data1;                  /* ADC convert result (4 bytes) */
+	  uint8_t      am_channel2;               /* The 8-bit ADC Channel */
+	  int32_t      am_data2;                  /* ADC convert result (4 bytes) */
+	  uint8_t      am_channel3;               /* The 8-bit ADC Channel */
+	  int32_t      am_data3;                  /* ADC convert result (4 bytes) */
+	  uint8_t      am_channel4;               /* The 8-bit ADC Channel */
+	  int32_t      am_data4;                  /* ADC convert result (4 bytes) */
+	}__attribute__((__packed__));;
+
+	struct adc_msg4_s sample1[4];
+
+	size_t readsize;
+	ssize_t nbytes;
+	int i = 0;
+	int j = 0;
+	int errval;
+
+	char name[11];
+	sprintf(name, "/dev/adc%d", j);
+	fd0 = open(name, O_RDONLY | O_NONBLOCK);
+	if (fd0 < 0)
+	{
+		printf("ADC: %s open fail\n", name);
+		return ERROR;
+	} else {
+		printf("Opened %s successfully\n", name);
+	}
+
+
+	/* Expected values */
+	int16_t expected_min[] = {2700, 2700, 2200, 2000};
+	int16_t expected_max[] = {3000, 3000, 2500, 2200};
+	char* check_res[nchannels];
+
+	/* first adc read round */
+	readsize = 4 * sizeof(struct adc_msg_s);
+
+	/* Empty all buffers */
+	do {
+		nbytes = read(fd0, sample1, readsize);
+	}
+	while (nbytes > 0);
+
+	up_udelay(20000);//microseconds
+	/* Take measurements */
+	nbytes = read(fd0, sample1, readsize);
+
+    /* Handle unexpected return values */
+
+    if (nbytes <= 0)
+      {
+        errval = errno;
+        if (errval != EINTR)
+          {
+            message("read %s failed: %d\n",
+                    name, errval);
+            errval = 3;
+            goto errout_with_dev;
+          }
+
+        message("\tInterrupted read...\n");
+      }
+    else if (nbytes == 0)
+      {
+        message("\tNo data read, Ignoring\n");
+      }
+
+    /* Print the sample data on successful return */
+
+    else
+    {
+    	int nsamples = nbytes / sizeof(struct adc_msg_s);
+    	if (nsamples * sizeof(struct adc_msg_s) != nbytes)
+    	{
+    		message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n",
+    				nbytes, sizeof(struct adc_msg_s));
+    	}
+    	else
+    	{
+    		/* Check values */
+    		check_res[0] = (expected_min[0] < sample1[i].am_data1 && expected_max[0] > sample1[i].am_data1) ? "OK" : "FAIL";
+    		check_res[1] = (expected_min[1] < sample1[i].am_data2 && expected_max[1] > sample1[i].am_data2) ? "OK" : "FAIL";
+    		check_res[2] = (expected_min[2] < sample1[i].am_data3 && expected_max[2] > sample1[i].am_data3) ? "OK" : "FAIL";
+    		check_res[3] = (expected_min[3] < sample1[i].am_data4 && expected_max[3] > sample1[i].am_data4) ? "OK" : "FAIL";
+
+    		/* Accumulate result */
+    		ret += (expected_min[0] > sample1[i].am_data1 || expected_max[0] < sample1[i].am_data1) ? 1 : 0;
+    		// XXX Chan 11 not connected on test setup
+    		//ret += (expected_min[1] > sample1[i].am_data2 || expected_max[1] < sample1[i].am_data2) ? 1 : 0;
+    		ret += (expected_min[2] > sample1[i].am_data3 || expected_max[2] < sample1[i].am_data3) ? 1 : 0;
+    		ret += (expected_min[3] > sample1[i].am_data4 || expected_max[3] < sample1[i].am_data4) ? 1 : 0;
+
+    		message("Sample:");
+    		message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+    					i, sample1[i].am_channel1, sample1[i].am_data1, expected_min[0], expected_max[0], check_res[0]);
+        	message("Sample:");
+    		message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+						i, sample1[i].am_channel2, sample1[i].am_data2, expected_min[1], expected_max[1], check_res[1]);
+        	message("Sample:");
+    		message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+						i, sample1[i].am_channel3, sample1[i].am_data3, expected_min[2], expected_max[2], check_res[2]);
+        	message("Sample:");
+    		message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+						i, sample1[i].am_channel4, sample1[i].am_data4, expected_min[3], expected_max[3], check_res[3]);
+
+    		if (ret != OK) {
+    			printf("\t ADC test FAILED. Some channels where out of allowed range. Check supply voltages.\n");
+    		 	goto errout_with_dev;
+    		}
+    	}
+    }
+
+	printf("\t ADC test successful.\n");
+
+	errout_with_dev:
+	  if (fd0 != 0) close(fd0);
+
+	return ret;
+}
diff --git a/apps/px4/tests/test_led.c b/apps/px4/tests/test_led.c
new file mode 100644
index 000000000..53615ccd8
--- /dev/null
+++ b/apps/px4/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 <arch/board/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("/dev/led", O_RDONLY | O_NONBLOCK);
+
+	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/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c
new file mode 100644
index 000000000..bdb68f88b
--- /dev/null
+++ b/apps/px4/tests/test_sensors.c
@@ -0,0 +1,501 @@
+/****************************************************************************
+ * px4/sensors/test_sensors.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 <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <arch/board/drv_lis331.h>
+#include <arch/board/drv_bma180.h>
+#include <arch/board/drv_l3gd20.h>
+#include <arch/board/drv_hmc5883l.h>
+#include <arch/board/drv_mpu6000.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+//static int	lis331(int argc, char *argv[]);
+static int l3gd20(int argc, char *argv[]);
+static int bma180(int argc, char *argv[]);
+static int hmc5883l(int argc, char *argv[]);
+static int ms5611(int argc, char *argv[]);
+static int mpu6000(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+struct {
+	const char	*name;
+	const char	*path;
+	int	(* test)(int argc, char *argv[]);
+} sensors[] = {
+	{"l3gd20",	"/dev/l3gd20",	l3gd20},
+	{"bma180",	"/dev/bma180",	bma180},
+	{"hmc5883l",	"/dev/hmc5883l",	hmc5883l},
+	{"ms5611",	"/dev/ms5611",	ms5611},
+	{"mpu6000",	"/dev/mpu6000",	mpu6000},
+//    {"lis331",	"/dev/lis331",	lis331},
+	{NULL, NULL, NULL}
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+//static int
+//lis331(int argc, char *argv[])
+//{
+//	int		fd;
+//	int16_t	buf[3];
+//	int		ret;
+//
+//	fd = open("/dev/lis331", O_RDONLY);
+//	if (fd < 0) {
+//		printf("\tlis331: not present on PX4FMU v1.5 and later\n");
+//		return ERROR;
+//	}
+//
+//	if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) ||
+//	    ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) {
+//
+//		printf("LIS331: ioctl fail\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("LIS331: read1 fail (%d)\n", ret);
+//		return ERROR;
+//	}
+//
+//	/* read data - expect no samples (should not be ready again yet) */
+//	ret = read(fd, buf, sizeof(buf));
+//	if (ret != 0) {
+//		printf("LIS331: read2 fail (%d)\n", ret);
+//		return ERROR;
+//	}
+//
+//	/* XXX more tests here */
+//
+//	return 0;
+//}
+
+static int
+l3gd20(int argc, char *argv[])
+{
+	printf("\tL3GD20: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	int16_t	buf[3] = {0, 0, 0};
+	int		ret;
+
+	fd = open("/dev/l3gd20", O_RDONLY | O_NONBLOCK);
+
+	if (fd < 0) {
+		printf("L3GD20: open fail\n");
+		return ERROR;
+	}
+
+//	if (ioctl(fd, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_50HZ) ||
+//	    ioctl(fd, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
+//
+//		printf("L3GD20: ioctl fail\n");
+//		return ERROR;
+//	} else {
+//		printf("\tconfigured..\n");
+//	}
+//
+//	/* wait at least 100ms, sensor should have data after no more than 2ms */
+//	usleep(100000);
+
+
+
+	/* read data - expect samples */
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tL3GD20: read1 fail (%d should have been %d)\n", ret, sizeof(buf));
+		//return ERROR;
+
+	} else {
+		printf("\tL3GD20 values #1: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+	}
+
+	/* wait at least 2 ms, sensor should have data after no more than 1.5ms */
+	usleep(2000);
+
+	/* read data - expect no samples (should not be ready again yet) */
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tL3GD20: read2 fail (%d)\n", ret);
+		close(fd);
+		return ERROR;
+
+	} else {
+		printf("\tL3GD20 values #2: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+	}
+
+	/* empty sensor buffer */
+	ret = 0;
+
+	while (ret != sizeof(buf)) {
+		// Keep reading until successful
+		ret = read(fd, buf, sizeof(buf));
+	}
+
+	/* test if FIFO is operational */
+	usleep(14800); // Expecting 10 measurements
+
+	ret = 0;
+	int count = 0;
+	bool dataready = true;
+
+	while (dataready) {
+		// Keep reading until successful
+		ret = read(fd, buf, sizeof(buf));
+
+		if (ret != sizeof(buf)) {
+			dataready = false;
+
+		} else {
+			count++;
+		}
+	}
+
+	printf("\tL3GD20: Drained FIFO with %d values (expected 8-12)\n", count);
+
+	/* read data - expect no samples (should not be ready again yet) */
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != 0) {
+		printf("\tL3GD20: Note: read3 got data - there should not have been data ready\n", ret);
+//		return ERROR;
+	}
+
+	close(fd);
+
+	/* Let user know everything is ok */
+	printf("\tOK: L3GD20 passed all tests successfully\n");
+	return OK;
+}
+
+static int
+bma180(int argc, char *argv[])
+{
+	printf("\tBMA180: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	int16_t	buf[3] = {0, 0, 0};
+	int		ret;
+
+	fd = open("/dev/bma180", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tBMA180: open fail\n");
+		return ERROR;
+	}
+
+//	if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) ||
+//	    ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) {
+//
+//		printf("BMA180: ioctl fail\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("\tBMA180: read1 fail (%d)\n", ret);
+		close(fd);
+		return ERROR;
+
+	} else {
+		printf("\tBMA180 values: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+	}
+
+	/* 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("\tBMA180: read2 fail (%d)\n", ret);
+		close(fd);
+		return ERROR;
+
+	} else {
+		printf("\tBMA180: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+	}
+
+	/* empty sensor buffer */
+	ret = 0;
+
+	while (ret != sizeof(buf)) {
+		// Keep reading until successful
+		ret = read(fd, buf, sizeof(buf));
+	}
+
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != 0) {
+		printf("\tBMA180: Note: read3 got data - there should not have been data ready\n", ret);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: BMA180 passed all tests successfully\n");
+	close(fd);
+
+	return OK;
+}
+
+static int
+mpu6000(int argc, char *argv[])
+{
+	printf("\tMPU-6000: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	int16_t	buf[5] = { -1, 0, -1, 0, -1, 0};
+	int		ret;
+
+	fd = open("/dev/mpu6000", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tMPU-6000: open fail\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("\tMPU-6000: read1 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]);
+	// }
+
+	// /* 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: MPU-6000 passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+ms5611(int argc, char *argv[])
+{
+	printf("\tMS5611: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	float	buf[3] = {0.0f, 0.0f, 0.0f};
+	int		ret;
+
+	fd = open("/dev/ms5611", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tMS5611: open fail\n");
+		return ERROR;
+	}
+
+	for (int i = 0; i < 5; i++) {
+		/* read data - expect samples */
+		ret = read(fd, buf, sizeof(buf));
+
+		if (ret != sizeof(buf)) {
+			if ((uint8_t)ret == -EAGAIN || (uint8_t)ret == -EINPROGRESS || i < 3) {
+				/* waiting for device to become ready, this is not an error */
+			} else {
+				printf("\tMS5611: read fail (%d)\n", ret);
+				close(fd);
+				return ERROR;
+			}
+
+		} else {
+
+			/* hack for float printing */
+			int32_t pressure_int = buf[0];
+			int32_t altitude_int = buf[1];
+			int32_t temperature_int = buf[2];
+
+			printf("\tMS5611: pressure:%d.%03d mbar - altitude: %d.%02d meters - temp:%d.%02d deg celcius\n", pressure_int, (int)(buf[0] * 1000 - pressure_int * 1000), altitude_int, (int)(buf[1] * 100 - altitude_int * 100), temperature_int, (int)(buf[2] * 100 - temperature_int * 100));
+		}
+
+		/* wait at least 10ms, sensor should have data after no more than 6.5ms */
+		usleep(10000);
+	}
+
+	close(fd);
+
+	/* Let user know everything is ok */
+	printf("\tOK: MS5611 passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+hmc5883l(int argc, char *argv[])
+{
+	printf("\tHMC5883L: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	int16_t	buf[3] = {0, 0, 0};
+	int		ret;
+
+	fd = open("/dev/hmc5883l", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tHMC5883L: open fail\n");
+		return ERROR;
+	}
+
+	int i;
+
+	for (i = 0; i < 5; i++) {
+		/* wait at least 7ms, sensor should have data after no more than 6.5ms */
+		usleep(7000);
+
+		/* read data - expect samples */
+		ret = read(fd, buf, sizeof(buf));
+
+		if (ret != sizeof(buf)) {
+			printf("\tHMC5883L: read1 fail (%d) values: x:%d\ty:%d\tz:%d\n", ret, buf[0], buf[1], buf[2]);
+			close(fd);
+			return ERROR;
+
+		} else {
+			printf("\tHMC5883L: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+		}
+	}
+
+	close(fd);
+
+	/* Let user know everything is ok */
+	printf("\tOK: HMC5883L 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/apps/px4/tests/test_servo.c b/apps/px4/tests/test_servo.c
new file mode 100644
index 000000000..f95760ca8
--- /dev/null
+++ b/apps/px4/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/apps/px4/tests/test_sleep.c b/apps/px4/tests/test_sleep.c
new file mode 100644
index 000000000..911a9c2e1
--- /dev/null
+++ b/apps/px4/tests/test_sleep.c
@@ -0,0 +1,103 @@
+/****************************************************************************
+ * 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 <arch/board/drv_led.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 (int i = 0; i < nsleeps; i++) {
+		usleep(100000);
+		//printf("\ttick\n");
+	}
+
+	printf("\t Sleep test successful.\n");
+
+	return OK;
+}
diff --git a/apps/px4/tests/test_time.c b/apps/px4/tests/test_time.c
new file mode 100644
index 000000000..c128c73a3
--- /dev/null
+++ b/apps/px4/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 <arch/board/up_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;
+}
+
+/****************************************************************************
+ * 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 < 100000; i++) {
+
+		usleep(rand() * 10);
+
+		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 %lld\n", maxdelta);
+
+	return 0;
+}
diff --git a/apps/px4/tests/test_uart_baudchange.c b/apps/px4/tests/test_uart_baudchange.c
new file mode 100644
index 000000000..06965bd3d
--- /dev/null
+++ b/apps/px4/tests/test_uart_baudchange.c
@@ -0,0 +1,160 @@
+/****************************************************************************
+ * 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 <arch/board/drv_led.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/apps/px4/tests/test_uart_console.c b/apps/px4/tests/test_uart_console.c
new file mode 100644
index 000000000..de1249b8c
--- /dev/null
+++ b/apps/px4/tests/test_uart_console.c
@@ -0,0 +1,177 @@
+/****************************************************************************
+ * 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 <arch/board/drv_led.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <arch/board/up_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/apps/px4/tests/test_uart_loopback.c b/apps/px4/tests/test_uart_loopback.c
new file mode 100644
index 000000000..b2e07df1c
--- /dev/null
+++ b/apps/px4/tests/test_uart_loopback.c
@@ -0,0 +1,197 @@
+/****************************************************************************
+ * 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 <arch/board/drv_led.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/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c
new file mode 100644
index 000000000..83d205440
--- /dev/null
+++ b/apps/px4/tests/test_uart_send.c
@@ -0,0 +1,133 @@
+/****************************************************************************
+ * 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 <arch/board/drv_led.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <arch/board/up_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, r, 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/apps/px4/tests/tests.h b/apps/px4/tests/tests.h
new file mode 100644
index 000000000..aa6dae1e0
--- /dev/null
+++ b/apps/px4/tests/tests.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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(...) lib_rawprintf(__VA_ARGS__)
+#    define msgflush()
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#    define msgflush() fflush(stdout)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lib_rawprintf
+#    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_eeproms(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_jig_voltages(int argc, char *argv[]);
+
+#endif /* __APPS_PX4_TESTS_H */
diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c
new file mode 100644
index 000000000..704b5a237
--- /dev/null
+++ b/apps/px4/tests/tests_main.c
@@ -0,0 +1,339 @@
+/****************************************************************************
+ * px4/sensors/tests_main.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 <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
+ ****************************************************************************/
+
+struct {
+	const char 	*name;
+	int	(* fn)(int argc, char *argv[]);
+	unsigned	options;
+	int			passed;
+#define OPT_NOHELP	(1<<0)
+#define OPT_NOALLTEST	(1<<1)
+#define OPT_NOJIGTEST	(1<<2)
+} tests[] = {
+	{"led",			test_led,	0, 0},
+	{"int",			test_int,	0, 0},
+	{"float",		test_float,	0, 0},
+	{"sensors",		test_sensors,	0, 0},
+	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"adc",			test_adc,	OPT_NOJIGTEST, 0},
+	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST, 0},
+	{"eeproms",		test_eeproms,	0, 0},
+	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"tone",		test_tone,	0, 0},
+	{"sleep",		test_sleep,	OPT_NOJIGTEST, 0},
+	{"time",		test_time,	OPT_NOJIGTEST, 0},
+	{"perf",		test_perf,	OPT_NOJIGTEST, 0},
+	{"all",			test_all,	OPT_NOALLTEST | OPT_NOJIGTEST, 0},
+	{"jig",			test_jig,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST, 0},
+	{NULL,			NULL, 		0, 0}
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+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;
+
+	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++;
+
+			} else {
+				tests[i].passed = 1;
+				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				fflush(stdout);
+			}
+		}
+	}
+
+	/* 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", i, i);
+
+	} else {
+		printf("  ______   ______     __     __ \n");
+		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
+		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
+		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
+		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
+		printf("\n");
+		printf(" Some tests failed (%d of %d)\n", failcount, i);
+	}
+
+	printf("\n");
+
+	/* Print failed tests */
+	if (failcount > 0) printf(" Failed tests:\n\n");
+
+	int k;
+
+	for (k = 0; k < i; k++) {
+		if ((tests[k].passed == 0) && !(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);
+}
+
+test_jig(int argc, char *argv[])
+{
+	unsigned	i;
+	char		*args[2] = {"jig", NULL};
+	unsigned int failcount = 0;
+
+	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++;
+			} else {
+				tests[i].passed = 1;
+				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				fflush(stdout);
+			}
+		}
+	}
+
+	/* 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", i, i);
+	} else {
+		printf("  ______   ______     __     __ \n");
+		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
+		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
+		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
+		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
+		printf("\n");
+		printf(" Some tests failed (%d of %d)\n", failcount, i);
+	}
+	printf("\n");
+
+	/* Print failed tests */
+	if (failcount > 0) printf(" Failed tests:\n\n");
+	int k;
+	for (k = 0; k < i; k++)
+	{
+		if ((tests[k].passed == 0) && !(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;
+}
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT int tests_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * Name: tests_main
+ ****************************************************************************/
+
+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;
+}