Merge remote-tracking branch 'upstream/master' into new_state_machine

Conflicts:
	src/drivers/px4io/px4io.cpp
	src/modules/commander/commander.c
	src/modules/commander/state_machine_helper.c

46 files changed, 6977 insertions, 0 deletions

diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c
new file mode 100644
index 000000000..0569d89f5
--- /dev/null
+++ b/src/systemcmds/bl_update/bl_update.c
@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bl_update.c
+ *
+ * STM32F4 bootloader update tool.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+
+__EXPORT int bl_update_main(int argc, char *argv[]);
+
+static void setopt(void);
+
+int
+bl_update_main(int argc, char *argv[])
+{
+	if (argc != 2)
+		errx(1, "missing firmware filename or command");
+
+	if (!strcmp(argv[1], "setopt"))
+		setopt();
+
+	int fd = open(argv[1], O_RDONLY);
+
+	if (fd < 0)
+		err(1, "open %s", argv[1]);
+
+	struct stat s;
+
+	if (stat(argv[1], &s) < 0)
+		err(1, "stat %s", argv[1]);
+
+	/* sanity-check file size */
+	if (s.st_size > 16384)
+		errx(1, "%s: file too large", argv[1]);
+
+	uint8_t *buf = malloc(s.st_size);
+
+	if (buf == NULL)
+		errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size);
+
+	if (read(fd, buf, s.st_size) != s.st_size)
+		err(1, "firmware read error");
+
+	close(fd);
+
+	uint32_t *hdr = (uint32_t *)buf;
+
+	if ((hdr[0] < 0x20000000) ||			/* stack not below RAM */
+	    (hdr[0] > (0x20000000 + (128 * 1024))) ||	/* stack not above RAM */
+	    (hdr[1] < 0x08000000) ||			/* entrypoint not below flash */
+	    ((hdr[1] - 0x08000000) > 16384)) {		/* entrypoint not outside bootloader */
+		free(buf);
+		errx(1, "not a bootloader image");
+	}
+
+	warnx("image validated, erasing bootloader...");
+	usleep(10000);
+
+	/* prevent other tasks from running while we do this */
+	sched_lock();
+
+	/* unlock the control register */
+	volatile uint32_t *keyr = (volatile uint32_t *)0x40023c04;
+	*keyr = 0x45670123U;
+	*keyr = 0xcdef89abU;
+
+	volatile uint32_t *sr = (volatile uint32_t *)0x40023c0c;
+	volatile uint32_t *cr = (volatile uint32_t *)0x40023c10;
+	volatile uint8_t *base = (volatile uint8_t *)0x08000000;
+
+	/* check the control register */
+	if (*cr & 0x80000000) {
+		warnx("WARNING: flash unlock failed, flash aborted");
+		goto flash_end;
+	}
+
+	/* erase the bootloader sector */
+	*cr = 0x2;
+	*cr = 0x10002;
+
+	/* wait for the operation to complete */
+	while (*sr & 0x1000) {
+	}
+
+	if (*sr & 0xf2) {
+		warnx("WARNING: erase error 0x%02x", *sr);
+		goto flash_end;
+	}
+
+	/* verify the erase */
+	for (int i = 0; i < s.st_size; i++) {
+		if (base[i] != 0xff) {
+			warnx("WARNING: erase failed at %d - retry update, DO NOT reboot", i);
+			goto flash_end;
+		}
+	}
+
+	warnx("flashing...");
+
+	/* now program the bootloader - speed is not critical so use x8 mode */
+	for (int i = 0; i < s.st_size; i++) {
+
+		/* program a byte */
+		*cr = 1;
+		base[i] = buf[i];
+
+		/* wait for the operation to complete */
+		while (*sr & 0x1000) {
+		}
+
+		if (*sr & 0xf2) {
+			warnx("WARNING: program error 0x%02x", *sr);
+			goto flash_end;
+		}
+	}
+
+	/* re-lock the flash control register */
+	*cr = 0x80000000;
+
+	warnx("verifying...");
+
+	/* now run a verify pass */
+	for (int i = 0; i < s.st_size; i++) {
+		if (base[i] != buf[i]) {
+			warnx("WARNING: verify failed at %u - retry update, DO NOT reboot", i);
+			goto flash_end;
+		}
+	}
+
+	warnx("bootloader update complete");
+
+flash_end:
+	/* unlock the scheduler */
+	sched_unlock();
+
+	free(buf);
+	exit(0);
+}
+
+static void
+setopt(void)
+{
+	volatile uint32_t *optcr = (volatile uint32_t *)0x40023c14;
+
+	const uint16_t opt_mask = (3 << 2);		/* BOR_LEV bitmask */
+	const uint16_t opt_bits = (0 << 2);		/* BOR = 0, setting for 2.7-3.6V operation */
+
+	if ((*optcr & opt_mask) == opt_bits)
+		errx(0, "option bits are already set as required");
+
+	/* unlock the control register */
+	volatile uint32_t *optkeyr = (volatile uint32_t *)0x40023c08;
+	*optkeyr = 0x08192a3bU;
+	*optkeyr = 0x4c5d6e7fU;
+
+	if (*optcr & 1)
+		errx(1, "option control register unlock failed");
+
+	/* program the new option value */
+	*optcr = (*optcr & ~opt_mask) | opt_bits | (1 << 1);
+
+	usleep(1000);
+
+	if ((*optcr & opt_mask) == opt_bits)
+		errx(0, "option bits set");
+
+	errx(1, "option bits setting failed; readback 0x%04x", *optcr);
+
+}
\ No newline at end of file
diff --git a/src/systemcmds/bl_update/module.mk b/src/systemcmds/bl_update/module.mk
new file mode 100644
index 000000000..e8eea045e
--- /dev/null
+++ b/src/systemcmds/bl_update/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Bootloader updater (flashes the FMU USB bootloader software)
+#
+
+MODULE_COMMAND	 = bl_update
+SRCS		 = bl_update.c
+
+MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/boardinfo/boardinfo.c b/src/systemcmds/boardinfo/boardinfo.c
new file mode 100644
index 000000000..3ff5a066c
--- /dev/null
+++ b/src/systemcmds/boardinfo/boardinfo.c
@@ -0,0 +1,409 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file boardinfo.c
+ * autopilot and carrier board information app
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <nuttx/i2c.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/bson/tinybson.h>
+
+__EXPORT int boardinfo_main(int argc, char *argv[]);
+
+struct eeprom_info_s
+{
+	unsigned	bus;
+	unsigned	address;
+	unsigned	page_size;
+	unsigned	page_count;
+	unsigned	page_write_delay;
+};
+
+/* XXX currently code below only supports 8-bit addressing */
+const struct eeprom_info_s eeprom_info[] = {
+	{3, 0x57, 8, 16, 3300},
+};
+
+struct board_parameter_s {
+	const char	*name;
+	bson_type_t	type;
+};
+
+const struct board_parameter_s board_parameters[] = {
+	{"name",	BSON_STRING},	/* ascii board name */
+	{"vers",	BSON_INT32},	/* board version (major << 8) | minor */
+	{"date",	BSON_INT32},	/* manufacture date */
+	{"build",	BSON_INT32}	/* build code (fab << 8) | tester */
+};
+
+const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]);
+
+static int
+eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size)
+{
+	int result = -1;
+
+	struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus);
+	if (dev == NULL) {
+		warnx("failed to init bus %d for EEPROM", eeprom->bus);
+		goto out;
+	}
+	I2C_SETFREQUENCY(dev, 400000);
+
+	/* loop until all data has been transferred */
+	for (unsigned address = 0; address < size; ) {
+
+		uint8_t pagebuf[eeprom->page_size + 1];
+
+		/* how many bytes available to transfer? */
+		unsigned count = size - address;
+
+		/* constrain writes to the page size */
+		if (count > eeprom->page_size)
+			count = eeprom->page_size;
+
+		pagebuf[0] = address & 0xff;
+		memcpy(pagebuf + 1, buf + address, count);
+
+		struct i2c_msg_s msgv[1] = {
+			{
+				.addr = eeprom->address,
+				.flags = 0,
+				.buffer = pagebuf,
+				.length = count + 1
+			}
+		};
+
+		result = I2C_TRANSFER(dev, msgv, 1);
+		if (result != OK) {
+			warnx("EEPROM write failed: %d", result);
+			goto out;
+		}
+		usleep(eeprom->page_write_delay);
+		address += count;
+	}
+
+out:
+	if (dev != NULL)
+		up_i2cuninitialize(dev);
+	return result;
+}
+
+static int
+eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size)
+{
+	int result = -1;
+
+	struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus);
+	if (dev == NULL) {
+		warnx("failed to init bus %d for EEPROM", eeprom->bus);
+		goto out;
+	}
+	I2C_SETFREQUENCY(dev, 400000);
+
+	/* loop until all data has been transferred */
+	for (unsigned address = 0; address < size; ) {
+
+		/* how many bytes available to transfer? */
+		unsigned count = size - address;
+
+		/* constrain transfers to the page size (bus anti-hog) */
+		if (count > eeprom->page_size)
+			count = eeprom->page_size;
+
+		uint8_t addr = address;
+		struct i2c_msg_s msgv[2] = {
+			{
+				.addr = eeprom->address,
+				.flags = 0,
+				.buffer = &addr,
+				.length = 1
+			},
+			{
+				.addr = eeprom->address,
+				.flags = I2C_M_READ,
+				.buffer = buf + address,
+				.length = count
+			}
+		};
+
+		result = I2C_TRANSFER(dev, msgv, 2);
+		if (result != OK) {
+			warnx("EEPROM read failed: %d", result);
+			goto out;
+		}
+		address += count;
+	}
+
+out:
+	if (dev != NULL)
+		up_i2cuninitialize(dev);
+	return result;
+}
+
+static void *
+idrom_read(const struct eeprom_info_s *eeprom)
+{
+	uint32_t size = 0xffffffff;
+	int result;
+	void *buf = NULL;
+
+	result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size));
+	if (result != 0) {
+		warnx("failed reading ID ROM length");
+		goto fail;
+	}
+	if (size > (eeprom->page_size * eeprom->page_count)) {
+		warnx("ID ROM not programmed");
+		goto fail;
+	}
+
+	buf = malloc(size);
+	if (buf == NULL) {
+		warnx("could not allocate %d bytes for ID ROM", size);
+		goto fail;
+	}
+	result = eeprom_read(eeprom, buf, size);
+	if (result != 0) {
+		warnx("failed reading ID ROM");
+		goto fail;
+	}
+	return buf;
+
+fail:
+	if (buf != NULL)
+		free(buf);
+	return NULL;
+}
+
+static void
+boardinfo_set(const struct eeprom_info_s *eeprom, char *spec)
+{
+	struct bson_encoder_s encoder;
+	int result = 1;
+	char *state, *token;
+	unsigned i;
+
+	/* create the encoder and make a writable copy of the spec */
+	bson_encoder_init_buf(&encoder, NULL, 0);
+
+	for (i = 0, token = strtok_r(spec, ",", &state);
+	     token && (i < num_parameters);
+	     i++, token = strtok_r(NULL, ",", &state)) {
+
+		switch (board_parameters[i].type) {
+		case BSON_STRING:
+			result = bson_encoder_append_string(&encoder, board_parameters[i].name, token);
+			break;
+		case BSON_INT32:
+			result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0));
+			break;
+		default:
+			result = 1;
+		}
+		if (result) {
+			warnx("bson append failed for %s<%s>", board_parameters[i].name, token);
+			goto out;
+		}
+	}
+	bson_encoder_fini(&encoder);
+	if (i != num_parameters) {
+		warnx("incorrect parameter list, expected: \"<name>,<version><date>,<buildcode>\"");
+		result = 1;
+		goto out;
+	}
+	if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) {
+		warnx("data too large for EEPROM");
+		result = 1;
+		goto out;
+	}
+	if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) {
+		warnx("buffer length mismatch");
+		result = 1;
+		goto out;
+	}
+	warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder));
+
+	result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder));
+	if (result < 0) {
+		warnc(-result, "error writing EEPROM");
+		result = 1;
+	} else {
+		result = 0;
+	}
+
+out:
+	free(bson_encoder_buf_data(&encoder));
+
+	exit(result);
+}
+
+static int
+boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	switch (node->type) {
+	case BSON_INT32:
+		printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i);
+		break;
+	case BSON_STRING: {
+		char buf[bson_decoder_data_pending(decoder)];
+		bson_decoder_copy_data(decoder, buf);
+		printf("%s: %s\n", node->name, buf);
+		break;
+	}
+	case BSON_EOO:
+		break;
+	default:
+		warnx("unexpected node type %d", node->type);
+		break;
+	}
+	return 1;
+}
+
+static void
+boardinfo_show(const struct eeprom_info_s *eeprom)
+{
+	struct bson_decoder_s decoder;
+	void *buf;
+
+	buf = idrom_read(eeprom);
+	if (buf == NULL)
+		errx(1, "ID ROM read failed");
+
+	if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) {
+		while (bson_decoder_next(&decoder) > 0)
+			;
+	} else {
+		warnx("failed to init decoder");
+	}
+	free(buf);
+	exit(0);
+}
+
+struct {
+	const char *property;
+	const char *value;
+} test_args;
+
+static int
+boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	/* reject nodes with non-matching names */
+	if (strcmp(node->name, test_args.property))
+		return 1;
+
+	/* compare node values to check for a match */
+	switch (node->type) {
+	case BSON_STRING: {
+		char buf[bson_decoder_data_pending(decoder)];
+		bson_decoder_copy_data(decoder, buf);
+
+		/* check for a match */
+		if (!strcmp(test_args.value, buf)) {
+			return 2;
+		}
+		break;
+	}
+
+	case BSON_INT32: {
+		int32_t val = strtol(test_args.value, NULL, 0);
+
+		/* check for a match */
+		if (node->i == val) {
+			return 2;
+		}
+		break;
+	}
+
+	default:
+		break;
+	}
+
+	return 1;
+}
+
+static void
+boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value)
+{
+	struct bson_decoder_s decoder;
+	void *buf;
+	int result = -1;
+
+	if ((property == NULL) || (strlen(property) == 0) ||
+	    (value == NULL) || (strlen(value) == 0))
+		errx(1, "missing property name or value");
+
+	test_args.property = property;
+	test_args.value = value;
+
+	buf = idrom_read(eeprom);
+	if (buf == NULL)
+		errx(1, "ID ROM read failed");
+
+	if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) {
+		do {
+			result = bson_decoder_next(&decoder);
+		} while (result == 1);
+	} else {
+		warnx("failed to init decoder");
+	}
+	free(buf);
+
+	/* if we matched, we exit with zero success */	
+	exit((result == 2) ? 0 : 1);
+}
+
+int
+boardinfo_main(int argc, char *argv[])
+{
+	if (!strcmp(argv[1], "set"))
+		boardinfo_set(&eeprom_info[0], argv[2]);
+
+	if (!strcmp(argv[1], "show"))
+		boardinfo_show(&eeprom_info[0]);
+
+	if (!strcmp(argv[1], "test"))
+		boardinfo_test(&eeprom_info[0], argv[2], argv[3]);
+
+	errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'");
+}
diff --git a/src/systemcmds/boardinfo/module.mk b/src/systemcmds/boardinfo/module.mk
new file mode 100644
index 000000000..6851d229b
--- /dev/null
+++ b/src/systemcmds/boardinfo/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Information about FMU and IO boards connected
+#
+
+MODULE_COMMAND	 = boardinfo
+SRCS		 = boardinfo.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/eeprom/24xxxx_mtd.c
new file mode 100644
index 000000000..e34be44e3
--- /dev/null
+++ b/src/systemcmds/eeprom/24xxxx_mtd.c
@@ -0,0 +1,571 @@
+/************************************************************************************
+ * Driver for 24xxxx-style I2C EEPROMs.
+ *
+ * Adapted from:
+ *
+ * drivers/mtd/at24xx.c
+ * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256)
+ *
+ *   Copyright (C) 2011 Li Zhuoyi. All rights reserved.
+ *   Author: Li Zhuoyi <lzyy.cn@gmail.com>
+ *   History: 0.1 2011-08-20 initial version
+ *
+ *   2011-11-1 Added support for larger MTD block sizes: Hal Glenn <hglenn@2g-eng.com>
+ *
+ * Derived from drivers/mtd/m25px.c
+ *
+ *   Copyright (C) 2009-2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/kmalloc.h>
+#include <nuttx/fs/ioctl.h>
+#include <nuttx/i2c.h>
+#include <nuttx/mtd.h>
+
+#include "systemlib/perf_counter.h"
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+
+/*
+ * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be
+ * omitted in order to avoid building the AT24XX driver as well.
+ */
+
+/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */
+
+#ifndef CONFIG_AT24XX_SIZE
+#  warning "Assuming AT24 size 64"
+#  define CONFIG_AT24XX_SIZE 64
+#endif
+#ifndef CONFIG_AT24XX_ADDR
+#  warning "Assuming AT24 address of 0x50"
+#  define CONFIG_AT24XX_ADDR 0x50
+#endif
+
+/* Get the part configuration based on the size configuration */
+
+#if CONFIG_AT24XX_SIZE == 32
+#  define AT24XX_NPAGES     128
+#  define AT24XX_PAGESIZE   32
+#elif CONFIG_AT24XX_SIZE == 48
+#  define AT24XX_NPAGES     192
+#  define AT24XX_PAGESIZE   32
+#elif CONFIG_AT24XX_SIZE == 64
+#  define AT24XX_NPAGES     256
+#  define AT24XX_PAGESIZE   32
+#elif CONFIG_AT24XX_SIZE == 128
+#  define AT24XX_NPAGES     256
+#  define AT24XX_PAGESIZE   64
+#elif CONFIG_AT24XX_SIZE == 256
+#  define AT24XX_NPAGES     512
+#  define AT24XX_PAGESIZE   64
+#endif
+
+/* For applications where a file system is used on the AT24, the tiny page sizes
+ * will result in very inefficient FLASH usage.  In such cases, it is better if
+ * blocks are comprised of "clusters" of pages so that the file system block
+ * size is, say, 256 or 512 bytes.  In any event, the block size *must* be an
+ * even multiple of the pages.
+ */
+
+#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE
+#  warning "Assuming driver block size is the same as the FLASH page size"
+#  define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE
+#endif
+
+/* The AT24 does not respond on the bus during write cycles, so we depend on a long
+ * timeout to detect problems.  The max program time is typically ~5ms.
+ */
+#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS
+#  define CONFIG_AT24XX_WRITE_TIMEOUT_MS  20
+#endif
+
+/************************************************************************************
+ * Private Types
+ ************************************************************************************/
+
+/* This type represents the state of the MTD device.  The struct mtd_dev_s
+ * must appear at the beginning of the definition so that you can freely
+ * cast between pointers to struct mtd_dev_s and struct at24c_dev_s.
+ */
+
+struct at24c_dev_s {
+	struct mtd_dev_s      mtd;      /* MTD interface */
+	FAR struct i2c_dev_s *dev;      /* Saved I2C interface instance */
+	uint8_t               addr;     /* I2C address */
+	uint16_t              pagesize; /* 32, 63 */
+	uint16_t              npages;   /* 128, 256, 512, 1024 */
+
+	perf_counter_t        perf_reads;
+	perf_counter_t        perf_writes;
+	perf_counter_t        perf_resets;
+	perf_counter_t        perf_read_retries;
+	perf_counter_t        perf_read_errors;
+	perf_counter_t        perf_write_errors;
+};
+
+/************************************************************************************
+ * Private Function Prototypes
+ ************************************************************************************/
+
+/* MTD driver methods */
+
+static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks);
+static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
+			   size_t nblocks, FAR uint8_t *buf);
+static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock,
+			    size_t nblocks, FAR const uint8_t *buf);
+static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg);
+
+void at24c_test(void);
+
+/************************************************************************************
+ * Private Data
+ ************************************************************************************/
+
+/* At present, only a single AT24 part is supported.  In this case, a statically
+ * allocated state structure may be used.
+ */
+
+static struct at24c_dev_s g_at24c;
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+static int at24c_eraseall(FAR struct at24c_dev_s *priv)
+{
+	int startblock = 0;
+	uint8_t buf[AT24XX_PAGESIZE + 2];
+
+	struct i2c_msg_s msgv[1] = {
+		{
+			.addr = priv->addr,
+			.flags = 0,
+			.buffer = &buf[0],
+			.length = sizeof(buf),
+		}
+	};
+
+	memset(&buf[2], 0xff, priv->pagesize);
+
+	for (startblock = 0; startblock < priv->npages; startblock++) {
+		uint16_t offset = startblock * priv->pagesize;
+		buf[1] = offset & 0xff;
+		buf[0] = (offset >> 8) & 0xff;
+
+		while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) {
+			fvdbg("erase stall\n");
+			usleep(10000);
+		}
+	}
+
+	return OK;
+}
+
+/************************************************************************************
+ * Name: at24c_erase
+ ************************************************************************************/
+
+static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks)
+{
+	/* EEprom need not erase */
+
+	return (int)nblocks;
+}
+
+/************************************************************************************
+ * Name: at24c_test
+ ************************************************************************************/
+
+void at24c_test(void)
+{
+	uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE];
+	unsigned count = 0;
+	unsigned errors = 0;
+
+	for (count = 0; count < 10000; count++) {
+		ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf);
+		if (result == ERROR) {
+			if (errors++ > 2) {
+				vdbg("too many errors\n");
+				return;
+			}
+		} else if (result != 1) {
+			vdbg("unexpected %u\n", result);
+		}
+		if ((count % 100) == 0)
+			vdbg("test %u errors %u\n", count, errors);
+	}
+}
+
+/************************************************************************************
+ * Name: at24c_bread
+ ************************************************************************************/
+
+static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
+			   size_t nblocks, FAR uint8_t *buffer)
+{
+	FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
+	size_t blocksleft;
+	uint8_t addr[2];
+	int ret;
+
+	struct i2c_msg_s msgv[2] = {
+		{
+			.addr = priv->addr,
+			.flags = 0,
+			.buffer = &addr[0],
+			.length = sizeof(addr),
+		},
+		{
+			.addr = priv->addr,
+			.flags = I2C_M_READ,
+			.buffer = 0,
+			.length = priv->pagesize,
+		}
+	};
+
+#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
+	startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+	nblocks    *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+#endif
+	blocksleft  = nblocks;
+
+	fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
+
+	if (startblock >= priv->npages) {
+		return 0;
+	}
+
+	if (startblock + nblocks > priv->npages) {
+		nblocks = priv->npages - startblock;
+	}
+
+	while (blocksleft-- > 0) {
+		uint16_t offset = startblock * priv->pagesize;
+		unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
+
+		addr[1] = offset & 0xff;
+		addr[0] = (offset >> 8) & 0xff;
+		msgv[1].buffer = buffer;
+
+		for (;;) {
+
+			perf_begin(priv->perf_reads);
+			ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
+			perf_end(priv->perf_reads);
+
+			if (ret >= 0)
+				break;
+
+			fvdbg("read stall");
+			usleep(1000);
+
+			/* We should normally only be here on the first read after
+			 * a write.
+			 *
+			 * XXX maybe do special first-read handling with optional
+			 * bus reset as well?
+			 */
+			perf_count(priv->perf_read_retries);
+
+			if (--tries == 0) {
+				perf_count(priv->perf_read_errors);
+				return ERROR;
+			}
+		}
+
+		startblock++;
+		buffer += priv->pagesize;
+	}
+
+#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
+	return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+#else
+	return nblocks;
+#endif
+}
+
+/************************************************************************************
+ * Name: at24c_bwrite
+ *
+ * Operates on MTD block's and translates to FLASH pages
+ *
+ ************************************************************************************/
+
+static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks,
+			    FAR const uint8_t *buffer)
+{
+	FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
+	size_t blocksleft;
+	uint8_t buf[AT24XX_PAGESIZE + 2];
+	int ret;
+
+	struct i2c_msg_s msgv[1] = {
+		{
+			.addr = priv->addr,
+			.flags = 0,
+			.buffer = &buf[0],
+			.length = sizeof(buf),
+		}
+	};
+
+#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
+	startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+	nblocks    *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+#endif
+	blocksleft  = nblocks;
+
+	if (startblock >= priv->npages) {
+		return 0;
+	}
+
+	if (startblock + nblocks > priv->npages) {
+		nblocks = priv->npages - startblock;
+	}
+
+	fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
+
+	while (blocksleft-- > 0) {
+		uint16_t offset = startblock * priv->pagesize;
+		unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
+
+		buf[1] = offset & 0xff;
+		buf[0] = (offset >> 8) & 0xff;
+		memcpy(&buf[2], buffer, priv->pagesize);
+
+		for (;;) {
+
+			perf_begin(priv->perf_writes);
+			ret = I2C_TRANSFER(priv->dev, &msgv[0], 1);
+			perf_end(priv->perf_writes);
+
+			if (ret >= 0)
+				break;
+
+			fvdbg("write stall");
+			usleep(1000);
+
+			/* We expect to see a number of retries per write cycle as we
+			 * poll for write completion.
+			 */
+			if (--tries == 0) {
+				perf_count(priv->perf_write_errors);
+				return ERROR;
+			}
+		}
+
+		startblock++;
+		buffer += priv->pagesize;
+	}
+
+#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
+	return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+#else
+	return nblocks;
+#endif
+}
+
+/************************************************************************************
+ * Name: at24c_ioctl
+ ************************************************************************************/
+
+static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
+{
+	FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
+	int ret = -EINVAL; /* Assume good command with bad parameters */
+
+	fvdbg("cmd: %d \n", cmd);
+
+	switch (cmd) {
+	case MTDIOC_GEOMETRY: {
+			FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg);
+
+			if (geo) {
+				/* Populate the geometry structure with information need to know
+				 * the capacity and how to access the device.
+				 *
+				 * NOTE: that the device is treated as though it where just an array
+				 * of fixed size blocks.  That is most likely not true, but the client
+				 * will expect the device logic to do whatever is necessary to make it
+				 * appear so.
+				 *
+				 * blocksize:
+				 *   May be user defined. The block size for the at24XX devices may be
+				 *   larger than the page size in order to better support file systems.
+				 *   The read and write functions translate BLOCKS to pages for the
+				 *   small flash devices
+				 * erasesize:
+				 *   It has to be at least as big as the blocksize, bigger serves no
+				 *   purpose.
+				 * neraseblocks
+				 *   Note that the device size is in kilobits and must be scaled by
+				 *   1024 / 8
+				 */
+
+#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
+				geo->blocksize    = CONFIG_AT24XX_MTD_BLOCKSIZE;
+				geo->erasesize    = CONFIG_AT24XX_MTD_BLOCKSIZE;
+				geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE;
+#else
+				geo->blocksize    = priv->pagesize;
+				geo->erasesize    = priv->pagesize;
+				geo->neraseblocks = priv->npages;
+#endif
+				ret               = OK;
+
+				fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n",
+				      geo->blocksize, geo->erasesize, geo->neraseblocks);
+			}
+		}
+		break;
+
+	case MTDIOC_BULKERASE:
+		ret = at24c_eraseall(priv);
+		break;
+
+	case MTDIOC_XIPBASE:
+	default:
+		ret = -ENOTTY; /* Bad command */
+		break;
+	}
+
+	return ret;
+}
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: at24c_initialize
+ *
+ * Description:
+ *   Create an initialize MTD device instance.  MTD devices are not registered
+ *   in the file system, but are created as instances that can be bound to
+ *   other functions (such as a block or character driver front end).
+ *
+ ************************************************************************************/
+
+FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
+	FAR struct at24c_dev_s *priv;
+
+	fvdbg("dev: %p\n", dev);
+
+	/* Allocate a state structure (we allocate the structure instead of using
+	 * a fixed, static allocation so that we can handle multiple FLASH devices.
+	 * The current implementation would handle only one FLASH part per I2C
+	 * device (only because of the SPIDEV_FLASH definition) and so would have
+	 * to be extended to handle multiple FLASH parts on the same I2C bus.
+	 */
+
+	priv = &g_at24c;
+
+	if (priv) {
+		/* Initialize the allocated structure */
+
+		priv->addr       = CONFIG_AT24XX_ADDR;
+		priv->pagesize   = AT24XX_PAGESIZE;
+		priv->npages     = AT24XX_NPAGES;
+
+		priv->mtd.erase  = at24c_erase;
+		priv->mtd.bread  = at24c_bread;
+		priv->mtd.bwrite = at24c_bwrite;
+		priv->mtd.ioctl  = at24c_ioctl;
+		priv->dev        = dev;
+
+		priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read");
+		priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write");
+		priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset");
+		priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries");
+		priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors");
+		priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors");
+	}
+
+	/* attempt to read to validate device is present */
+	unsigned char buf[5];
+	uint8_t addrbuf[2] = {0, 0};
+
+	struct i2c_msg_s msgv[2] = {
+		{
+			.addr = priv->addr,
+			.flags = 0,
+			.buffer = &addrbuf[0],
+			.length = sizeof(addrbuf),
+		},
+		{
+			.addr = priv->addr,
+			.flags = I2C_M_READ,
+			.buffer = &buf[0],
+			.length = sizeof(buf),
+		}
+	};
+
+	perf_begin(priv->perf_reads);
+	int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
+	perf_end(priv->perf_reads);
+
+	if (ret < 0) {
+		return NULL;
+	}
+
+	/* Return the implementation-specific state structure as the MTD device */
+
+	fvdbg("Return %p\n", priv);
+	return (FAR struct mtd_dev_s *)priv;
+}
+
+/*
+ * XXX: debug hackery
+ */
+int at24c_nuke(void)
+{
+	return at24c_eraseall(&g_at24c);
+}
diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c
new file mode 100644
index 000000000..49da51358
--- /dev/null
+++ b/src/systemcmds/eeprom/eeprom.c
@@ -0,0 +1,265 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file eeprom.c
+ *
+ * EEPROM service and utility app.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/param/param.h"
+#include "systemlib/err.h"
+
+#ifndef PX4_I2C_BUS_ONBOARD
+#  error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
+#endif
+
+__EXPORT int eeprom_main(int argc, char *argv[]);
+
+static void	eeprom_attach(void);
+static void	eeprom_start(void);
+static void	eeprom_erase(void);
+static void	eeprom_ioctl(unsigned operation);
+static void	eeprom_save(const char *name);
+static void	eeprom_load(const char *name);
+static void	eeprom_test(void);
+
+static bool attached = false;
+static bool started = false;
+static struct mtd_dev_s *eeprom_mtd;
+
+int eeprom_main(int argc, char *argv[])
+{
+	if (argc >= 2) {
+		if (!strcmp(argv[1], "start"))
+			eeprom_start();
+
+		if (!strcmp(argv[1], "save_param"))
+			eeprom_save(argv[2]);
+
+		if (!strcmp(argv[1], "load_param"))
+			eeprom_load(argv[2]);
+
+		if (!strcmp(argv[1], "erase"))
+			eeprom_erase();
+
+		if (!strcmp(argv[1], "test"))
+			eeprom_test();
+
+		if (0) {	/* these actually require a file on the filesystem... */
+
+			if (!strcmp(argv[1], "reformat"))
+				eeprom_ioctl(FIOC_REFORMAT);
+
+			if (!strcmp(argv[1], "repack"))
+				eeprom_ioctl(FIOC_OPTIMIZE);
+		}
+	}
+
+	errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
+}
+
+
+static void
+eeprom_attach(void)
+{
+	/* find the right I2C */
+	struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
+	/* this resets the I2C bus, set correct bus speed again */
+	I2C_SETFREQUENCY(i2c, 400000);
+
+	if (i2c == NULL)
+		errx(1, "failed to locate I2C bus");
+
+	/* start the MTD driver, attempt 5 times */
+	for (int i = 0; i < 5; i++) {
+		eeprom_mtd = at24c_initialize(i2c);
+		if (eeprom_mtd) {
+			/* abort on first valid result */
+			if (i > 0) {
+				warnx("warning: EEPROM needed %d attempts to attach", i+1);
+			}
+			break;
+		}
+	}
+
+	/* if last attempt is still unsuccessful, abort */
+	if (eeprom_mtd == NULL)
+		errx(1, "failed to initialize EEPROM driver");
+
+	attached = true;
+}
+
+static void
+eeprom_start(void)
+{
+	int ret;
+
+	if (started)
+		errx(1, "EEPROM already mounted");
+
+	if (!attached)
+		eeprom_attach();
+
+	/* start NXFFS */
+	ret = nxffs_initialize(eeprom_mtd);
+
+	if (ret < 0)
+		errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
+
+	/* mount the EEPROM */
+	ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
+
+	if (ret < 0)
+		errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
+
+	started = true;
+	warnx("mounted EEPROM at /eeprom");
+	exit(0);
+}
+
+extern int at24c_nuke(void);
+
+static void
+eeprom_erase(void)
+{
+	if (!attached)
+		eeprom_attach();
+
+	if (at24c_nuke())
+		errx(1, "erase failed");
+
+	errx(0, "erase done, reboot now");
+}
+
+static void
+eeprom_ioctl(unsigned operation)
+{
+	int fd;
+
+	fd = open("/eeprom/.", 0);
+
+	if (fd < 0)
+		err(1, "open /eeprom");
+
+	if (ioctl(fd, operation, 0) < 0)
+		err(1, "ioctl");
+
+	exit(0);
+}
+
+static void
+eeprom_save(const char *name)
+{
+	if (!started)
+		errx(1, "must be started first");
+
+	if (!name)
+		err(1, "missing argument for device name, try '/eeprom/parameters'");
+
+	warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
+
+	/* delete the file in case it exists */
+	unlink(name);
+
+	/* create the file */
+	int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
+
+	if (fd < 0)
+		err(1, "opening '%s' failed", name);
+
+	int result = param_export(fd, false);
+	close(fd);
+
+	if (result < 0) {
+		unlink(name);
+		errx(1, "error exporting to '%s'", name);
+	}
+
+	exit(0);
+}
+
+static void
+eeprom_load(const char *name)
+{
+	if (!started)
+		errx(1, "must be started first");
+
+	if (!name)
+		err(1, "missing argument for device name, try '/eeprom/parameters'");
+
+	warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
+
+	int fd = open(name, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "open '%s'", name);
+
+	int result = param_load(fd);
+	close(fd);
+
+	if (result < 0)
+		errx(1, "error importing from '%s'", name);
+
+	exit(0);
+}
+
+extern void at24c_test(void);
+
+static void
+eeprom_test(void)
+{
+	at24c_test();
+	exit(0);
+}
diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk
new file mode 100644
index 000000000..07f3945be
--- /dev/null
+++ b/src/systemcmds/eeprom/module.mk
@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# EEPROM file system driver
+#
+
+MODULE_COMMAND	 = eeprom
+SRCS		 = 24xxxx_mtd.c eeprom.c
diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c
new file mode 100644
index 000000000..4da238039
--- /dev/null
+++ b/src/systemcmds/i2c/i2c.c
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file i2c.c
+ *
+ * i2c hacking tool
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+
+#ifndef PX4_I2C_BUS_ONBOARD
+#  error PX4_I2C_BUS_ONBOARD not defined, no device interface
+#endif
+#ifndef PX4_I2C_OBDEV_PX4IO
+#  error PX4_I2C_OBDEV_PX4IO not defined
+#endif
+
+__EXPORT int i2c_main(int argc, char *argv[]);
+
+static int transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+
+static struct i2c_dev_s *i2c;
+
+int i2c_main(int argc, char *argv[])
+{
+	/* find the right I2C */
+	i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
+
+	if (i2c == NULL)
+		errx(1, "failed to locate I2C bus");
+
+	usleep(100000);
+
+	uint8_t buf[] = { 0, 4};
+	int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0);
+
+	if (ret)
+		errx(1, "send failed - %d", ret);
+
+	uint32_t val;
+	ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val));
+	if (ret)
+		errx(1, "recive failed - %d", ret);
+
+	errx(0, "got 0x%08x", val);
+}
+
+static int
+transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+	struct i2c_msg_s msgv[2];
+	unsigned msgs;
+	int ret;
+
+	//	debug("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
+
+	msgs = 0;
+
+	if (send_len > 0) {
+		msgv[msgs].addr = address;
+		msgv[msgs].flags = 0;
+		msgv[msgs].buffer = send;
+		msgv[msgs].length = send_len;
+		msgs++;
+	}
+
+	if (recv_len > 0) {
+		msgv[msgs].addr = address;
+		msgv[msgs].flags = I2C_M_READ;
+		msgv[msgs].buffer = recv;
+		msgv[msgs].length = recv_len;
+		msgs++;
+	}
+
+	if (msgs == 0)
+		return -1;
+
+	/*
+	 * I2C architecture means there is an unavoidable race here
+	 * if there are any devices on the bus with a different frequency
+	 * preference.  Really, this is pointless.
+	 */
+	I2C_SETFREQUENCY(i2c, 400000);
+	ret = I2C_TRANSFER(i2c, &msgv[0], msgs);
+
+	// reset the I2C bus to unwedge on error
+	if (ret != OK)
+		up_i2creset(i2c);
+
+	return ret;
+}
diff --git a/src/systemcmds/i2c/module.mk b/src/systemcmds/i2c/module.mk
new file mode 100644
index 000000000..ab2357c7d
--- /dev/null
+++ b/src/systemcmds/i2c/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Build the i2c test tool.
+#
+
+MODULE_COMMAND	 = i2c
+SRCS		 = i2c.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.c
new file mode 100644
index 000000000..55c4f0836
--- /dev/null
+++ b/src/systemcmds/mixer/mixer.c
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer.c
+ *
+ * Mixer utility.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <ctype.h>
+#include <nuttx/compiler.h>
+
+#include <systemlib/err.h>
+#include <drivers/drv_mixer.h>
+#include <uORB/topics/actuator_controls.h>
+
+__EXPORT int mixer_main(int argc, char *argv[]);
+
+static void	usage(const char *reason) noreturn_function;
+static void	load(const char *devname, const char *fname) noreturn_function;
+
+int
+mixer_main(int argc, char *argv[])
+{
+	if (argc < 2)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "load")) {
+		if (argc < 4)
+			usage("missing device or filename");
+
+		load(argv[2], argv[3]);
+	}
+
+	usage("unrecognised command");
+}
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage:\n");
+	fprintf(stderr, "  mixer load <device> <filename>\n");
+	/* XXX other useful commands? */
+	exit(1);
+}
+
+static void
+load(const char *devname, const char *fname)
+{
+	int		dev;
+	FILE		*fp;
+	char		line[80];
+	char		buf[512];
+
+	/* open the device */
+	if ((dev = open(devname, 0)) < 0)
+		err(1, "can't open %s\n", devname);
+
+	/* reset mixers on the device */
+	if (ioctl(dev, MIXERIOCRESET, 0))
+		err(1, "can't reset mixers on %s", devname);
+
+	/* open the mixer definition file */
+	fp = fopen(fname, "r");
+	if (fp == NULL)
+		err(1, "can't open %s", fname);
+
+	/* read valid lines from the file into a buffer */
+	buf[0] = '\0';
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		line[0] = '\0';
+		if (fgets(line, sizeof(line), fp) == NULL)
+			break;
+
+		/* if the line doesn't look like a mixer definition line, skip it */
+		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
+			continue;
+
+		/* compact whitespace in the buffer */
+		char *t, *f;
+		for (f = buf; *f != '\0'; f++) {
+			/* scan for space characters */
+			if (*f == ' ') {
+				/* look for additional spaces */
+				t = f + 1;
+				while (*t == ' ')
+					t++;
+				if (*t == '\0') {
+					/* strip trailing whitespace */
+					*f = '\0';
+				} else if (t > (f + 1)) {
+					memmove(f + 1, t, strlen(t) + 1);
+				}
+			}
+		}
+
+		/* if the line is too long to fit in the buffer, bail */
+		if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
+			break;
+
+		/* add the line to the buffer */
+		strcat(buf, line);
+	}
+
+	/* XXX pass the buffer to the device */
+	int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
+
+	if (ret < 0)
+		err(1, "error loading mixers from %s", fname);
+	exit(0);
+}
diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk
new file mode 100644
index 000000000..d5a3f9f77
--- /dev/null
+++ b/src/systemcmds/mixer/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Build the mixer tool.
+#
+
+MODULE_COMMAND	 = mixer
+SRCS		 = mixer.c
+
+MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/param/module.mk b/src/systemcmds/param/module.mk
new file mode 100644
index 000000000..63f15ad28
--- /dev/null
+++ b/src/systemcmds/param/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Build the parameters tool.
+#
+
+MODULE_COMMAND	 = param
+SRCS		 = param.c
+
+MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION	 = -Os
+
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
new file mode 100644
index 000000000..60e61d07b
--- /dev/null
+++ b/src/systemcmds/param/param.c
@@ -0,0 +1,297 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file param.c
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Parameter tool.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/param/param.h"
+#include "systemlib/err.h"
+
+__EXPORT int param_main(int argc, char *argv[]);
+
+static void	do_save(const char* param_file_name);
+static void	do_load(const char* param_file_name);
+static void	do_import(const char* param_file_name);
+static void	do_show(const char* search_string);
+static void	do_show_print(void *arg, param_t param);
+static void	do_set(const char* name, const char* val);
+
+int
+param_main(int argc, char *argv[])
+{
+	if (argc >= 2) {
+		if (!strcmp(argv[1], "save")) {
+			if (argc >= 3) {
+				do_save(argv[2]);
+			} else {
+				do_save(param_get_default_file());
+			}
+		}
+
+		if (!strcmp(argv[1], "load")) {
+			if (argc >= 3) {
+				do_load(argv[2]);
+			} else {
+				do_load(param_get_default_file());
+			}
+		}
+
+		if (!strcmp(argv[1], "import")) {
+			if (argc >= 3) {
+				do_import(argv[2]);
+			} else {
+				do_import(param_get_default_file());
+			}
+		}
+
+		if (!strcmp(argv[1], "select")) {
+			if (argc >= 3) {
+				param_set_default_file(argv[2]);
+			} else {
+				param_set_default_file(NULL);
+			}
+			warnx("selected parameter default file %s", param_get_default_file());
+			exit(0);
+		}
+
+		if (!strcmp(argv[1], "show")) {
+			if (argc >= 3) {
+				do_show(argv[2]);
+			} else {
+				do_show(NULL);
+			}
+		}
+
+		if (!strcmp(argv[1], "set")) {
+			if (argc >= 4) {
+				do_set(argv[2], argv[3]);
+			} else {
+				errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'");
+			}
+		}
+	}
+	
+	errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'");
+}
+
+static void
+do_save(const char* param_file_name)
+{
+	/* delete the parameter file in case it exists */
+	unlink(param_file_name);
+
+	/* create the file */
+	int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL);
+
+	if (fd < 0)
+		err(1, "opening '%s' failed", param_file_name);
+
+	int result = param_export(fd, false);
+	close(fd);
+
+	if (result < 0) {
+		unlink(param_file_name);
+		errx(1, "error exporting to '%s'", param_file_name);
+	}
+
+	exit(0);
+}
+
+static void
+do_load(const char* param_file_name)
+{
+	int fd = open(param_file_name, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "open '%s'", param_file_name);
+
+	int result = param_load(fd);
+	close(fd);
+
+	if (result < 0) {
+		errx(1, "error importing from '%s'", param_file_name);
+	}
+
+	exit(0);
+}
+
+static void
+do_import(const char* param_file_name)
+{
+	int fd = open(param_file_name, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "open '%s'", param_file_name);
+
+	int result = param_import(fd);
+	close(fd);
+
+	if (result < 0)
+		errx(1, "error importing from '%s'", param_file_name);
+
+	exit(0);
+}
+
+static void
+do_show(const char* search_string)
+{
+	printf(" + = saved, * = unsaved\n");
+	param_foreach(do_show_print, search_string, false);
+
+	exit(0);
+}
+
+static void
+do_show_print(void *arg, param_t param)
+{
+	int32_t i;
+	float f;
+	const char *search_string = (const char*)arg;
+
+	/* print nothing if search string is invalid and not matching */
+	if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) {
+		/* param not found */
+		return;
+	}
+
+	printf("%c %s: ",
+	       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
+	       param_name(param));
+
+	/*
+	 * This case can be expanded to handle printing common structure types.
+	 */
+
+	switch (param_type(param)) {
+	case PARAM_TYPE_INT32:
+		if (!param_get(param, &i)) {
+			printf("%d\n", i);
+			return;
+		}
+
+		break;
+
+	case PARAM_TYPE_FLOAT:
+		if (!param_get(param, &f)) {
+			printf("%4.4f\n", (double)f);
+			return;
+		}
+
+		break;
+
+	case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+		printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
+		return;
+
+	default:
+		printf("<unknown type %d>\n", 0 + param_type(param));
+		return;
+	}
+
+	printf("<error fetching parameter %d>\n", param);
+}
+
+static void
+do_set(const char* name, const char* val)
+{
+	int32_t i;
+	float f;
+	param_t param = param_find(name);
+
+	/* set nothing if parameter cannot be found */
+	if (param == PARAM_INVALID) {
+		/* param not found */
+		errx(1, "Error: Parameter %s not found.", name);
+	}
+
+	printf("%c %s: ",
+	       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
+	       param_name(param));
+
+	/*
+	 * Set parameter if type is known and conversion from string to value turns out fine
+	 */
+
+	switch (param_type(param)) {
+	case PARAM_TYPE_INT32:
+		if (!param_get(param, &i)) {
+			printf("curr: %d", i);
+
+			/* convert string */
+			char* end;
+			i = strtol(val,&end,10);
+			param_set(param, &i);
+			printf(" -> new: %d\n", i);
+
+		}
+
+		break;
+
+	case PARAM_TYPE_FLOAT:
+		if (!param_get(param, &f)) {
+			printf("curr: %4.4f", (double)f);
+
+			/* convert string */
+			char* end;
+			f = strtod(val,&end);
+			param_set(param, &f);
+			printf(" -> new: %f\n", f);
+
+		}
+
+		break;
+
+	default:
+		errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
+	}
+
+	exit(0);
+}
diff --git a/src/systemcmds/perf/module.mk b/src/systemcmds/perf/module.mk
new file mode 100644
index 000000000..77952842b
--- /dev/null
+++ b/src/systemcmds/perf/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# perf_counter reporting tool
+#
+
+MODULE_COMMAND	 = perf
+SRCS		 = perf.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c
new file mode 100644
index 000000000..b69ea597b
--- /dev/null
+++ b/src/systemcmds/perf/perf.c
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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 <unistd.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "systemlib/perf_counter.h"
+
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT int perf_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * user_start
+ ****************************************************************************/
+
+int perf_main(int argc, char *argv[])
+{
+	if (argc > 1) {
+		if (strcmp(argv[1], "reset") == 0) {
+			perf_reset_all();
+			return 0;
+		}
+		printf("Usage: perf <reset>\n");
+		return -1;
+	}
+
+	perf_print_all();
+	fflush(stdout);
+	return 0;
+}
+
+
diff --git a/src/systemcmds/preflight_check/module.mk b/src/systemcmds/preflight_check/module.mk
new file mode 100644
index 000000000..7c3c88783
--- /dev/null
+++ b/src/systemcmds/preflight_check/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Pre-flight check. Locks down system for a few systems with blinking leds
+# and buzzer if the sensors do not report an OK status.
+#
+
+MODULE_COMMAND	 = preflight_check
+SRCS		 = preflight_check.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
new file mode 100644
index 000000000..42256be61
--- /dev/null
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -0,0 +1,206 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file reboot.c
+ * Tool similar to UNIX reboot command
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include <systemlib/err.h>
+
+#include <drivers/drv_led.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_baro.h>
+
+#include <mavlink/mavlink_log.h>
+
+__EXPORT int preflight_check_main(int argc, char *argv[]);
+static int led_toggle(int leds, int led);
+static int led_on(int leds, int led);
+static int led_off(int leds, int led);
+
+int preflight_check_main(int argc, char *argv[])
+{
+	bool fail_on_error = false;
+
+	if (argc > 1 && !strcmp(argv[1], "--help")) {
+		warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error");
+		exit(1);
+	}
+
+	if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) {
+		fail_on_error = true;
+	}
+
+	bool system_ok = true;
+
+	int fd;
+	/* open text message output path */
+	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	int ret;
+
+	/* give the system some time to sample the sensors in the background */
+	usleep(150000);
+
+
+	/* ---- MAG ---- */
+	close(fd);
+	fd = open(MAG_DEVICE_PATH, 0);
+	if (fd < 0) {
+		warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
+		system_ok = false;
+		goto system_eval;
+	}
+	ret = ioctl(fd, MAGIOCSELFTEST, 0);
+	
+	if (ret != OK) {
+		warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CALIBRATION");
+		system_ok = false;
+		goto system_eval;
+	}
+
+	/* ---- ACCEL ---- */
+
+	close(fd);
+	fd = open(ACCEL_DEVICE_PATH, 0);
+	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+	
+	if (ret != OK) {
+		warnx("accel self test failed");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK");
+		system_ok = false;
+		goto system_eval;
+	}
+
+	/* ---- GYRO ---- */
+
+	close(fd);
+	fd = open(GYRO_DEVICE_PATH, 0);
+	ret = ioctl(fd, GYROIOCSELFTEST, 0);
+	
+	if (ret != OK) {
+		warnx("gyro self test failed");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK");
+		system_ok = false;
+		goto system_eval;
+	}
+
+	/* ---- BARO ---- */
+
+	close(fd);
+	fd = open(BARO_DEVICE_PATH, 0);
+
+		
+
+system_eval:
+	
+	if (system_ok) {
+		/* all good, exit silently */
+		exit(0);
+	} else {
+		fflush(stdout);
+
+		int buzzer = open("/dev/tone_alarm", O_WRONLY);
+		int leds = open(LED_DEVICE_PATH, 0);
+
+		/* flip blue led into alternating amber */
+		led_off(leds, LED_BLUE);
+		led_off(leds, LED_AMBER);
+		led_toggle(leds, LED_BLUE);
+
+		/* display and sound error */
+		for (int i = 0; i < 150; i++)
+		{
+			led_toggle(leds, LED_BLUE);
+			led_toggle(leds, LED_AMBER);
+
+			if (i % 10 == 0) {
+				ioctl(buzzer, TONE_SET_ALARM, 4);
+			} else if (i % 5 == 0) {
+				ioctl(buzzer, TONE_SET_ALARM, 2);
+			}
+			usleep(100000);
+		}
+
+		/* stop alarm */
+		ioctl(buzzer, TONE_SET_ALARM, 0);
+
+		/* switch on leds */
+		led_on(leds, LED_BLUE);
+		led_on(leds, LED_AMBER);
+
+		if (fail_on_error) {
+			/* exit with error message */
+			exit(1);
+		} else {
+			/* do not emit an error code to make sure system still boots */
+			exit(0);
+		}
+	}
+}
+
+static int led_toggle(int leds, int led)
+{
+	static int last_blue = LED_ON;
+	static int last_amber = LED_ON;
+
+	if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
+
+	if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
+
+	return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
+}
+
+static int led_off(int leds, int led)
+{
+	return ioctl(leds, LED_OFF, led);
+}
+
+static int led_on(int leds, int led)
+{
+	return ioctl(leds, LED_ON, led);
+}
\ No newline at end of file
diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk
new file mode 100644
index 000000000..4a23bba90
--- /dev/null
+++ b/src/systemcmds/pwm/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Build the pwm tool.
+#
+
+MODULE_COMMAND	 = pwm
+SRCS		 = pwm.c
+
+MODULE_STACKSIZE = 4096
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
new file mode 100644
index 000000000..ff733df52
--- /dev/null
+++ b/src/systemcmds/pwm/pwm.c
@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pwm.c
+ *
+ * PWM servo output configuration and monitoring tool.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+#include "drivers/drv_pwm_output.h"
+
+static void	usage(const char *reason);
+__EXPORT int	pwm_main(int argc, char *argv[]);
+
+
+static void
+usage(const char *reason)
+{
+	if (reason != NULL)
+		warnx("%s", reason);
+	errx(1, 
+		"usage:\n"
+		"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n"
+		"  -v                 Print information about the PWM device\n"
+		"  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"  <alt_rate>         PWM update rate for channels in <alt_channel_mask>\n"
+		"  <channel_group>    Channel group that should update at the alternate rate (may be specified more than once)\n"
+		"  arm | disarm       Arm or disarm the ouptut\n"
+		"  <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
+		"  <chanmask>         Directly supply alt rate channel mask (debug use only)\n"
+		"\n"
+		"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
+		);
+
+}
+
+int
+pwm_main(int argc, char *argv[])
+{
+	const char *dev = PWM_OUTPUT_DEVICE_PATH;
+	unsigned alt_rate = 0;
+	uint32_t alt_channel_groups = 0;
+	bool alt_channels_set = false;
+	bool print_info = false;
+	int ch;
+	int ret;
+	char *ep;
+	unsigned group;
+	int32_t set_mask = -1;
+
+	if (argc < 2)
+		usage(NULL);
+
+	while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
+		switch (ch) {
+		case 'c':
+			group = strtoul(optarg, &ep, 0);
+			if ((*ep != '\0') || (group >= 32))
+				usage("bad channel_group value");
+			alt_channel_groups |= (1 << group);
+			alt_channels_set = true;
+			break;
+
+		case 'd':
+			dev = optarg;
+			break;
+
+		case 'u':
+			alt_rate = strtol(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad alt_rate value");
+			break;
+
+		case 'm':
+			set_mask = strtol(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad set_mask value");
+			break;
+
+		case 'v':
+			print_info = true;
+			break;
+
+		default:
+			usage(NULL);
+		}
+	}
+	argc -= optind;
+	argv += optind;
+
+	/* open for ioctl only */
+	int fd = open(dev, 0);
+	if (fd < 0)
+		err(1, "can't open %s", dev);
+
+	/* change alternate PWM rate */
+	if (alt_rate > 0) {
+		ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
+		if (ret != OK)
+			err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
+	}
+
+	/* directly supplied channel mask */
+	if (set_mask != -1) {
+		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
+		if (ret != OK)
+			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
+	}
+
+	/* assign alternate rate to channel groups */
+	if (alt_channels_set) {
+		uint32_t mask = 0;
+
+		for (unsigned group = 0; group < 32; group++) {
+			if ((1 << group) & alt_channel_groups) {
+				uint32_t group_mask;
+
+				ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
+				if (ret != OK)
+					err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
+
+				mask |= group_mask;
+			}
+		}
+
+		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
+		if (ret != OK)
+			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
+	}
+
+	/* iterate remaining arguments */
+	unsigned channel = 0;
+	while (argc--) {
+		const char *arg = argv[0];
+		argv++;
+		if (!strcmp(arg, "arm")) {
+			ret = ioctl(fd, PWM_SERVO_ARM, 0);
+			if (ret != OK)
+				err(1, "PWM_SERVO_ARM");
+			continue;
+		}
+		if (!strcmp(arg, "disarm")) {
+			ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+			if (ret != OK)
+				err(1, "PWM_SERVO_DISARM");
+			continue;
+		}
+		unsigned pwm_value = strtol(arg, &ep, 0);
+		if (*ep == '\0') {
+			ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET(%d)", channel);
+			channel++;
+			continue;
+		}
+		usage("unrecognised option");
+	}
+
+	/* print verbose info */
+	if (print_info) {
+		/* get the number of servo channels */
+		unsigned count;
+		ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
+		if (ret != OK)
+			err(1, "PWM_SERVO_GET_COUNT");
+
+		/* print current servo values */
+		for (unsigned i = 0; i < count; i++) {
+			servo_position_t spos;
+
+			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
+			if (ret == OK) {
+				printf("channel %u: %uus\n", i, spos);
+			} else {
+				printf("%u: ERROR\n", i);
+			}
+		}
+
+		/* print rate groups */
+		for (unsigned i = 0; i < count; i++) {
+			uint32_t group_mask;
+
+			ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
+			if (ret != OK)
+				break;
+			if (group_mask != 0) {
+				printf("channel group %u: channels", i);
+				for (unsigned j = 0; j < 32; j++)
+					if (group_mask & (1 << j))
+						printf(" %u", j);
+				printf("\n");
+			}
+		}
+		fflush(stdout);
+	}
+	exit(0);
+}
\ No newline at end of file
diff --git a/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk
new file mode 100644
index 000000000..19f64af54
--- /dev/null
+++ b/src/systemcmds/reboot/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# reboot command.
+#
+
+MODULE_COMMAND	 = reboot
+SRCS		 = reboot.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c
new file mode 100644
index 000000000..cc380f94b
--- /dev/null
+++ b/src/systemcmds/reboot/reboot.c
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file reboot.c
+ * Tool similar to UNIX reboot command
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+
+#include <systemlib/systemlib.h>
+
+__EXPORT int reboot_main(int argc, char *argv[]);
+
+int reboot_main(int argc, char *argv[])
+{
+	printf("Rebooting now...\n");
+	fflush(stdout);
+	usleep(5000);
+	up_systemreset();
+}
+
+
diff --git a/src/systemcmds/tests/.context b/src/systemcmds/tests/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/src/systemcmds/tests/.context
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
new file mode 100644
index 000000000..754d3a0da
--- /dev/null
+++ b/src/systemcmds/tests/module.mk
@@ -0,0 +1,28 @@
+#
+# Assorted tests and the like
+#
+
+MODULE_COMMAND		 = tests
+MODULE_STACKSIZE	 = 12000
+MAXOPTIMIZATION		 = -Os
+
+SRCS			 = test_adc.c \
+			   test_bson.c \
+			   test_float.c \
+			   test_gpio.c \
+			   test_hott_telemetry.c \
+			   test_hrt.c \
+			   test_int.c \
+			   test_jig_voltages.c \
+			   test_led.c \
+			   test_sensors.c \
+			   test_servo.c \
+			   test_sleep.c \
+			   test_time.c \
+			   test_uart_baudchange.c \
+			   test_uart_console.c \
+			   test_uart_loopback.c \
+			   test_uart_send.c \
+			   tests_file.c \
+			   tests_main.c \
+			   tests_param.c
diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c
new file mode 100644
index 000000000..030ac6e23
--- /dev/null
+++ b/src/systemcmds/tests/test_adc.c
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_adc.c
+ * Test for the analog to digital converter.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <nuttx/analog/adc.h>
+#include <drivers/drv_adc.h>
+#include <systemlib/err.h>
+
+int test_adc(int argc, char *argv[])
+{
+	int fd = open(ADC_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		warnx("ERROR: can't open ADC device");
+		return 1;
+	}
+
+	for (unsigned i = 0; i < 5; i++) {
+		/* make space for a maximum of eight channels */
+		struct adc_msg_s data[8];
+		/* read all channels available */
+		ssize_t count = read(fd, data, sizeof(data));
+
+		if (count < 0)
+			goto errout_with_dev;
+
+		unsigned channels = count / sizeof(data[0]);
+
+		for (unsigned j = 0; j < channels; j++) {
+			printf("%d: %u  ", data[j].am_channel, data[j].am_data);
+		}
+
+		printf("\n");
+		usleep(150000);
+	}
+
+	warnx("\t ADC test successful.\n");
+
+errout_with_dev:
+
+	if (fd != 0) close(fd);
+
+	return OK;
+}
diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c
new file mode 100644
index 000000000..6130fe763
--- /dev/null
+++ b/src/systemcmds/tests/test_bson.c
@@ -0,0 +1,244 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_bson.c
+ *
+ * Tests for the bson en/decoder
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <systemlib/err.h>
+#include <systemlib/bson/tinybson.h>
+
+#include "tests.h"
+
+static const bool sample_bool = true;
+static const int32_t sample_small_int = 123;
+static const int64_t sample_big_int = (int64_t)INT_MAX + 123LL;
+static const double sample_double = 2.5f;
+static const char *sample_string = "this is a test";
+static const uint8_t sample_data[256] = {0};
+//static const char *sample_filename = "/fs/microsd/bson.test";
+
+static int
+encode(bson_encoder_t encoder)
+{
+	if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0)
+		warnx("FAIL: encoder: append bool failed");
+	if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0)
+		warnx("FAIL: encoder: append int failed");
+	if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0)
+		warnx("FAIL: encoder: append int failed");
+	if (bson_encoder_append_double(encoder, "double1", sample_double) != 0)
+		warnx("FAIL: encoder: append double failed");
+	if (bson_encoder_append_string(encoder, "string1", sample_string) != 0)
+		warnx("FAIL: encoder: append string failed");
+	if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0)
+		warnx("FAIL: encoder: append data failed");
+
+	bson_encoder_fini(encoder);
+
+	return 0;
+}
+
+static int
+decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	unsigned len;
+
+	if (!strcmp(node->name, "bool1")) {
+		if (node->type != BSON_BOOL) {
+			warnx("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL);
+			return 1;
+		}
+		if (node->b != sample_bool) {
+			warnx("FAIL: decoder: bool1 value %s, expected %s", 
+				(node->b ? "true" : "false"),
+				(sample_bool ? "true" : "false"));
+			return 1;
+		}
+		warnx("PASS: decoder: bool1");
+		return 1;
+	}
+	if (!strcmp(node->name, "int1")) {
+		if (node->type != BSON_INT32) {
+			warnx("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32);
+			return 1;
+		}
+		if (node->i != sample_small_int) {
+			warnx("FAIL: decoder: int1 value %lld, expected %d", node->i, sample_small_int);
+			return 1;
+		}
+		warnx("PASS: decoder: int1");
+		return 1;
+	}
+	if (!strcmp(node->name, "int2")) {
+		if (node->type != BSON_INT64) {
+			warnx("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64);
+			return 1;
+		}
+		if (node->i != sample_big_int) {
+			warnx("FAIL: decoder: int2 value %lld, expected %lld", node->i, sample_big_int);
+			return 1;
+		}
+		warnx("PASS: decoder: int2");
+		return 1;
+	}
+	if (!strcmp(node->name, "double1")) {
+		if (node->type != BSON_DOUBLE) {
+			warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE);
+			return 1;
+		}
+		if (node->d != sample_double) {
+			warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double);
+			return 1;
+		}
+		warnx("PASS: decoder: double1");
+		return 1;
+	}
+	if (!strcmp(node->name, "string1")) {
+		if (node->type != BSON_STRING) {
+			warnx("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING);
+			return 1;
+		}
+
+		len = bson_decoder_data_pending(decoder);
+
+		if (len != strlen(sample_string) + 1) {
+			warnx("FAIL: decoder: string1 length %d wrong, expected %d", len, strlen(sample_string) + 1);
+			return 1;
+		}
+
+		char sbuf[len];
+
+		if (bson_decoder_copy_data(decoder, sbuf)) {
+			warnx("FAIL: decoder: string1 copy failed");
+			return 1;
+		}
+		if (bson_decoder_data_pending(decoder) != 0) {
+			warnx("FAIL: decoder: string1 copy did not exhaust all data");
+			return 1;
+		}
+		if (sbuf[len - 1] != '\0') {
+			warnx("FAIL: decoder: string1 not 0-terminated");
+			return 1;
+		}
+		if (strcmp(sbuf, sample_string)) {
+			warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string);
+			return 1;
+		}
+		warnx("PASS: decoder: string1");
+		return 1;
+	}
+	if (!strcmp(node->name, "data1")) {
+		if (node->type != BSON_BINDATA) {
+			warnx("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA);
+			return 1;
+		}
+
+		len = bson_decoder_data_pending(decoder);
+
+		if (len != sizeof(sample_data)) {
+			warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(sample_data));
+			return 1;
+		}
+		
+		if (node->subtype != BSON_BIN_BINARY) {
+			warnx("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY);
+			return 1;
+		}
+
+		uint8_t dbuf[len];
+
+		if (bson_decoder_copy_data(decoder, dbuf)) {
+			warnx("FAIL: decoder: data1 copy failed");
+			return 1;
+		}
+		if (bson_decoder_data_pending(decoder) != 0) {
+			warnx("FAIL: decoder: data1 copy did not exhaust all data");
+			return 1;
+		}
+		if (memcmp(sample_data, dbuf, len)) {
+			warnx("FAIL: decoder: data1 compare fail");
+			return 1;
+		}
+		warnx("PASS: decoder: data1");
+		return 1;
+	}
+
+	if (node->type != BSON_EOO)
+		warnx("FAIL: decoder: unexpected node name '%s'", node->name);
+	return 1;
+}
+
+static void
+decode(bson_decoder_t decoder)
+{
+	int result;
+
+	do {
+		result = bson_decoder_next(decoder);
+	} while (result > 0);
+}
+
+int
+test_bson(int argc, char *argv[])
+{
+	struct bson_encoder_s encoder;
+	struct bson_decoder_s decoder;
+	void *buf;
+	int len;
+
+	/* encode data to a memory buffer */
+	if (bson_encoder_init_buf(&encoder, NULL, 0))
+		errx(1, "FAIL: bson_encoder_init_buf");
+	encode(&encoder);
+	len = bson_encoder_buf_size(&encoder);
+	if (len <= 0)
+		errx(1, "FAIL: bson_encoder_buf_len");
+	buf = bson_encoder_buf_data(&encoder);
+	if (buf == NULL)
+		errx(1, "FAIL: bson_encoder_buf_data");
+
+	/* now test-decode it */
+	if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL))
+		errx(1, "FAIL: bson_decoder_init_buf");
+	decode(&decoder);
+	free(buf);
+
+	return OK;
+}
\ No newline at end of file
diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c
new file mode 100644
index 000000000..4921c9bbb
--- /dev/null
+++ b/src/systemcmds/tests/test_float.c
@@ -0,0 +1,283 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_float.c
+ * Floating point tests
+ */
+
+#include <nuttx/config.h>
+#include <sys/types.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include "tests.h"
+#include <math.h>
+#include <float.h>
+
+typedef union {
+	float f;
+	double d;
+	uint8_t b[8];
+} test_float_double_t;
+
+int test_float(int argc, char *argv[])
+{
+	int ret = 0;
+
+	printf("\n--- SINGLE PRECISION TESTS ---\n");
+	printf("The single precision test involves calls to fabsf(),\nif test fails check this function as well.\n\n");
+
+	float f1 = 1.55f;
+
+	float sinf_zero = sinf(0.0f);
+	float sinf_one = sinf(1.0f);
+	float sqrt_two = sqrt(2.0f);
+
+	if (sinf_zero == 0.0f) {
+		printf("\t success: sinf(0.0f) == 0.0f\n");
+
+	} else {
+		printf("\t FAIL: sinf(0.0f) != 0.0f, result: %8.4f\n", (double)sinf_zero);
+		ret = -4;
+	}
+
+	fflush(stdout);
+
+	if (fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
+		printf("\t success: sinf(1.0f) == 0.84147f\n");
+
+	} else {
+		printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %8.4f\n", (double)sinf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	float asinf_one = asinf(1.0f);
+
+	if (fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
+		printf("\t success: asinf(1.0f) == 1.57079f\n");
+
+	} else {
+		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	float cosf_one = cosf(1.0f);
+
+	if (fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
+		printf("\t success: cosf(1.0f) == 0.54030f\n");
+
+	} else {
+		printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %8.4f\n", (double)cosf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+	float acosf_one = acosf(1.0f);
+
+	if (fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
+		printf("\t success: acosf(1.0f) == 0.0f\n");
+
+	} else {
+		printf("\t FAIL: acosf(1.0f) != 0.0f, result: %8.4f\n", (double)acosf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+	float sinf_zero_one = sinf(0.1f);
+
+	if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
+		printf("\t success: sinf(0.1f) == 0.09983f\n");
+
+	} else {
+		printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %8.4f\n", (double)sinf_zero_one);
+		ret = -2;
+	}
+
+	if (sqrt_two == 1.41421356f) {
+		printf("\t success: sqrt(2.0f) == 1.41421f\n");
+
+	} else {
+		printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %8.4f\n", (double)sinf_zero_one);
+		ret = -3;
+	}
+
+	float atan2f_ones = atan2(1.0f, 1.0f);
+
+	if (fabsf(atan2f_ones - 0.785398163397448278999490867136f) < FLT_EPSILON) {
+		printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n");
+
+	} else {
+		printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %8.4f\n", (double)atan2f_ones);
+		ret = -4;
+	}
+
+	char sbuf[30];
+	sprintf(sbuf, "%8.4f", 0.553415f);
+
+	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
+		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
+		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
+		printf("\t success: printf(\"%%8.4f\", 0.553415f) == %8.4f\n", (double)0.553415f);
+	} else {
+		printf("\t FAIL: printf(\"%%8.4f\", 0.553415f) != \"  0.5534\", result: %s\n", sbuf);
+		ret = -5;
+	}
+
+	sprintf(sbuf, "%8.4f", -0.553415f);
+
+	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
+		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
+		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
+		printf("\t success: printf(\"%%8.4f\", -0.553415f) == %8.4f\n", (double)-0.553415f);
+	} else {
+		printf("\t FAIL: printf(\"%%8.4f\", -0.553415f) != \" -0.5534\", result: %s\n", sbuf);
+		ret = -6;
+	}
+
+
+
+
+
+	printf("\n--- DOUBLE PRECISION TESTS ---\n");
+
+	double d1 = 1.0111;
+	double d2 = 2.0;
+
+	double d1d2 = d1 * d2;
+
+	if (d1d2 == 2.022200000000000219557705349871) {
+		printf("\t success: 1.0111 * 2.0 == 2.0222\n");
+
+	} else {
+		printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %8.4f\n", d1d2);
+		ret = -7;
+	}
+
+	fflush(stdout);
+
+	// Assign value of f1 to d1
+	d1 = f1;
+
+	if (f1 == (float)d1) {
+		printf("\t success: (float) 1.55f == 1.55 (double)\n");
+
+	} else {
+		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1);
+		ret = -8;
+	}
+
+	fflush(stdout);
+
+
+	double sin_zero = sin(0.0);
+	double sin_one = sin(1.0);
+	double atan2_ones = atan2(1.0, 1.0);
+
+	if (sin_zero == 0.0) {
+		printf("\t success: sin(0.0) == 0.0\n");
+
+	} else {
+		printf("\t FAIL: sin(0.0) != 0.0, result: %8.4f\n", sin_zero);
+		ret = -9;
+	}
+
+	if (sin_one == 0.841470984807896504875657228695) {
+		printf("\t success: sin(1.0) == 0.84147098480\n");
+
+	} else {
+		printf("\t FAIL: sin(1.0) != 1.0, result: %8.4f\n", sin_one);
+		ret = -10;
+	}
+
+	if (atan2_ones != 0.785398) {
+		printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
+
+	} else {
+		printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %8.4f\n", atan2_ones);
+		ret = -11;
+	}
+
+	printf("\t testing pow() with magic value\n");
+	printf("\t   (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
+	fflush(stdout);
+	usleep(20000);
+	double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));
+	printf("\t success: result: %8.4f\n", (double)powres);
+
+	sprintf(sbuf, "%8.4f", 0.553415);
+
+	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
+		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
+		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
+		printf("\t success: printf(\"%%8.4f\", 0.553415) == %8.4f\n", 0.553415);
+	} else {
+		printf("\t FAIL: printf(\"%%8.4f\", 0.553415) != \"  0.5534\", result: %s\n", sbuf);
+		ret = -12;
+	}
+
+	sprintf(sbuf, "%8.4f", -0.553415);
+
+	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
+		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
+		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
+		printf("\t success: printf(\"%%8.4f\", -0.553415) == %8.4f\n", -0.553415);
+	} else {
+		printf("\t FAIL: printf(\"%%8.4f\", -0.553415) != \" -0.5534\", result: %s\n", sbuf);
+		ret = -13;
+	}
+
+
+	if (ret == 0) {
+		printf("\n SUCCESS: All float and double tests passed.\n");
+
+	} else {
+		printf("\n FAIL: One or more tests failed.\n");
+	}
+
+	printf("\n");
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c
new file mode 100644
index 000000000..ab536d956
--- /dev/null
+++ b/src/systemcmds/tests/test_gpio.c
@@ -0,0 +1,115 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/analog/adc.h>
+
+#include "tests.h"
+
+#include <drivers/drv_gpio.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_gpio
+ ****************************************************************************/
+
+int test_gpio(int argc, char *argv[])
+{
+	int		fd;
+	int		ret = 0;
+
+	fd = open(GPIO_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		printf("GPIO: open fail\n");
+		return ERROR;
+	}
+
+	/* set all GPIOs to default state */
+	ioctl(fd, GPIO_RESET, ~0);
+
+
+	/* XXX need to add some GPIO waving stuff here */
+
+
+	/* Go back to default */
+	ioctl(fd, GPIO_RESET, ~0);
+
+	printf("\t GPIO test successful.\n");
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c
new file mode 100644
index 000000000..270dc3857
--- /dev/null
+++ b/src/systemcmds/tests/test_hott_telemetry.c
@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_hott_telemetry.c
+ *
+ * Tests the Graupner HoTT telemetry support.
+ *
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <drivers/drv_gpio.h>
+#include <nuttx/config.h>
+#include <sys/types.h>
+#include <systemlib/err.h>
+
+#include <debug.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include "tests.h"
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+static int open_uart(const char *device)
+{
+	/* baud rate */
+	int speed = B19200;
+
+	/* open uart */
+	int uart = open(device, O_RDWR | O_NOCTTY);
+
+	if (uart < 0) {
+		errx(1, "FAIL: Error opening port");
+		return ERROR;
+	}
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(uart, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		errx(1, "FAIL: Error setting baudrate / termios config for cfsetispeed, cfsetospeed");
+		return ERROR;
+	}
+
+	if (tcsetattr(uart, TCSANOW, &uart_config) < 0) {
+		errx(1, "FAIL: Error setting baudrate / termios config for tcsetattr");
+		return ERROR;
+	}
+
+	return uart;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_hott_telemetry
+ ****************************************************************************/
+
+int test_hott_telemetry(int argc, char *argv[])
+{
+	warnx("HoTT Telemetry Test Requirements:");
+	warnx("- Radio on and Electric Air. Mod on (telemetry -> sensor select).");
+	warnx("- Receiver telemetry port must be in telemetry mode.");
+	warnx("- Connect telemetry wire to /dev/ttyS1 (USART2).");
+	warnx("Testing...");
+
+	const char device[] = "/dev/ttyS1";
+	int fd = open_uart(device);
+
+	if (fd < 0) {
+		close(fd);
+		return ERROR;
+	}
+
+	/* Activate single wire mode */
+	ioctl(fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+
+	char send = 'a';
+	write(fd, &send, 1);
+
+	/* Since TX and RX are now connected we should be able to read in what we wrote */
+	const int timeout = 1000;
+	struct pollfd fds[] = { { .fd = fd, .events = POLLIN } };
+
+	if (poll(fds, 1, timeout) == 0) {
+		errx(1, "FAIL: Could not read sent data.");
+	}
+
+	char receive;
+	read(fd, &receive, 1);
+	warnx("PASS: Single wire enabled. Sent %x and received %x", send, receive);
+
+
+	/* Attempt to read HoTT poll messages from the HoTT receiver */
+	int received_count = 0;
+	int valid_count = 0;
+	const int max_polls = 5;
+	uint8_t byte;
+
+	for (; received_count < 5; received_count++) {
+		if (poll(fds, 1, timeout) == 0) {
+			errx(1, "FAIL: Could not read sent data. Is your HoTT receiver plugged in on %s?", device);
+			break;
+
+		} else {
+			read(fd, &byte, 1);
+
+			if (byte == 0x80) {
+				valid_count++;
+			}
+
+			/* Read the device ID being polled */
+			read(fd, &byte, 1);
+		}
+	}
+
+	if (received_count > 0 && valid_count > 0) {
+		if (received_count == max_polls && valid_count == max_polls) {
+			warnx("PASS: Received %d out of %d valid byte pairs from the HoTT receiver device.", received_count, max_polls);
+
+		} else {
+			warnx("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, max_polls, valid_count);
+		}
+
+	} else {
+		/* Let's work out what went wrong */
+		if (received_count == 0) {
+			errx(1, "FAIL: Could not read any polls from HoTT receiver device.");
+		}
+
+		if (valid_count == 0) {
+			errx(1, "FAIL: Received unexpected values from the HoTT receiver device.");
+		}
+	}
+
+
+	/* Attempt to send a HoTT response messages */
+	uint8_t response[] = {0x7c, 0x8e, 0x00, 0xe0, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, \
+			      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, \
+			      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf4, 0x01, 0x00, 0x00, \
+			      0x19, 0x00, 0x00, 0x00, 0x30, 0x75, 0x78, 0x00, 0x00, 0x00, \
+			      0x00, 0x00, 0x00, 0x7d, 0x12
+			     };
+
+	usleep(5000);
+
+	for (unsigned int i = 0; i < sizeof(response); i++) {
+		write(fd, &response[i], 1);
+		usleep(1000);
+	}
+
+	warnx("PASS: Response sent to the HoTT receiver device. Voltage should now show 2.5V.");
+
+
+	/* Disable single wire */
+	ioctl(fd, TIOCSSINGLEWIRE, ~SER_SINGLEWIRE_ENABLED);
+
+	write(fd, &send, 1);
+
+	/* We should timeout as there will be nothing to read (TX and RX no longer connected) */
+	if (poll(fds, 1, timeout) == 0) {
+		errx(1, "FAIL: timeout expected.");
+	}
+
+	warnx("PASS: Single wire disabled.");
+
+	close(fd);
+	exit(0);
+}
diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c
new file mode 100644
index 000000000..f21dd115b
--- /dev/null
+++ b/src/systemcmds/tests/test_hrt.c
@@ -0,0 +1,218 @@
+/****************************************************************************
+ * px4/sensors/test_hrt.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <sys/time.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+extern uint16_t ppm_buffer[];
+extern unsigned ppm_decoded_channels;
+extern uint16_t ppm_edge_history[];
+extern uint16_t ppm_pulse_history[];
+
+int test_ppm(int argc, char *argv[])
+{
+#ifdef CONFIG_HRT_PPM
+	unsigned i;
+
+	printf("channels: %u\n", ppm_decoded_channels);
+
+	for (i = 0; i < ppm_decoded_channels; i++)
+		printf("  %u\n", ppm_buffer[i]);
+
+	printf("edges\n");
+
+	for (i = 0; i < 32; i++)
+		printf("  %u\n", ppm_edge_history[i]);
+
+	printf("pulses\n");
+
+	for (i = 0; i < 32; i++)
+		printf("  %u\n", ppm_pulse_history[i]);
+
+	fflush(stdout);
+#else
+	printf("PPM not configured\n");
+#endif
+	return 0;
+}
+
+int test_tone(int argc, char *argv[])
+{
+	int fd, result;
+	unsigned long tone;
+
+	fd = open("/dev/tone_alarm", O_WRONLY);
+
+	if (fd < 0) {
+		printf("failed opening /dev/tone_alarm\n");
+		goto out;
+	}
+
+	tone = 1;
+
+	if (argc == 2)
+		tone = atoi(argv[1]);
+
+	if (tone  == 0) {
+		result = ioctl(fd, TONE_SET_ALARM, 0);
+
+		if (result < 0) {
+			printf("failed clearing alarms\n");
+			goto out;
+
+		} else {
+			printf("Alarm stopped.\n");
+		}
+
+	} else {
+		result = ioctl(fd, TONE_SET_ALARM, 0);
+
+		if (result < 0) {
+			printf("failed clearing alarms\n");
+			goto out;
+		}
+
+		result = ioctl(fd, TONE_SET_ALARM, tone);
+
+		if (result < 0) {
+			printf("failed setting alarm %lu\n", tone);
+
+		} else {
+			printf("Alarm %lu (disable with: tests tone 0)\n", tone);
+		}
+	}
+
+out:
+
+	if (fd >= 0)
+		close(fd);
+
+	return 0;
+}
+
+/****************************************************************************
+ * Name: test_hrt
+ ****************************************************************************/
+
+int test_hrt(int argc, char *argv[])
+{
+	struct hrt_call call;
+	hrt_abstime prev, now;
+	int i;
+	struct timeval tv1, tv2;
+
+	printf("start-time (hrt, sec/usec), end-time (hrt, sec/usec), microseconds per half second\n");
+
+	for (i = 0; i < 10; i++) {
+		prev = hrt_absolute_time();
+		gettimeofday(&tv1, NULL);
+		usleep(500000);
+		now = hrt_absolute_time();
+		gettimeofday(&tv2, NULL);
+		printf("%lu (%lu/%lu), %lu (%lu/%lu), %lu\n",
+		       (unsigned long)prev, (unsigned long)tv1.tv_sec, (unsigned long)tv1.tv_usec,
+		       (unsigned long)now, (unsigned long)tv2.tv_sec, (unsigned long)tv2.tv_usec,
+		       (unsigned long)(hrt_absolute_time() - prev));
+		fflush(stdout);
+	}
+
+	usleep(1000000);
+
+	printf("one-second ticks\n");
+
+	for (i = 0; i < 10; i++) {
+		hrt_call_after(&call, 1000000, NULL, NULL);
+
+		while (!hrt_called(&call)) {
+			usleep(1000);
+		}
+
+		printf("tick\n");
+		fflush(stdout);
+	}
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c
new file mode 100644
index 000000000..c59cee7b7
--- /dev/null
+++ b/src/systemcmds/tests/test_int.c
@@ -0,0 +1,149 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+typedef union {
+	int32_t i;
+	int64_t l;
+	uint8_t b[8];
+} test_32_64_t;
+
+int test_int(int argc, char *argv[])
+{
+	int ret = 0;
+
+	printf("\n--- 64 BIT MATH TESTS ---\n");
+
+	int64_t large = 354156329598;
+
+	int64_t calc = large * 5;
+
+	if (calc == 1770781647990) {
+		printf("\t success: 354156329598 * 5 == %lld\n", calc);
+
+	} else {
+		printf("\t FAIL: 354156329598 * 5 != %lld\n", calc);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+
+
+
+	printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n");
+
+
+	int32_t small = 50;
+	int32_t large_int = 2147483647; // MAX INT value
+
+	uint64_t small_times_large = large_int * (uint64_t)small;
+
+	if (small_times_large == 107374182350) {
+		printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %lld\n", small_times_large);
+
+	} else {
+		printf("\t FAIL: 50 * 2147483647 != %lld, 64bit cast might fail\n", small_times_large);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	if (ret == 0) {
+		printf("\n SUCCESS: All float and double tests passed.\n");
+
+	} else {
+		printf("\n FAIL: One or more tests failed.\n");
+	}
+
+	printf("\n");
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c
new file mode 100644
index 000000000..10c93b264
--- /dev/null
+++ b/src/systemcmds/tests/test_jig_voltages.c
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <nuttx/analog/adc.h>
+#include <drivers/drv_adc.h>
+#include <systemlib/err.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_gpio
+ ****************************************************************************/
+
+int test_jig_voltages(int argc, char *argv[])
+{
+	int fd = open(ADC_DEVICE_PATH, O_RDONLY);
+	int ret = OK;
+
+	if (fd < 0) {
+		warnx("can't open ADC device");
+		return 1;
+	}
+
+	/* make space for a maximum of eight channels */
+	struct adc_msg_s data[8];
+	/* read all channels available */
+	ssize_t count = read(fd, data, sizeof(data));
+
+	if (count < 0) {
+		close(fd);
+		warnx("can't read from ADC driver. Forgot 'adc start' command?");
+		return 1;
+	}
+
+	unsigned channels = count / sizeof(data[0]);
+
+	for (unsigned j = 0; j < channels; j++) {
+		printf("%d: %u  ", data[j].am_channel, data[j].am_data);
+	}
+
+	printf("\n");
+
+	warnx("\t ADC operational.\n");
+
+	/* Expected values */
+	int16_t expected_min[] = {2800, 2800, 1800,  800};
+	int16_t expected_max[] = {3100, 3100, 2100, 1100};
+	char *check_res[channels];
+
+	if (channels < 4) {
+		close(fd);
+		warnx("not all four test channels available, aborting.");
+		return 1;
+
+	} else {
+		/* Check values */
+		check_res[0] = (expected_min[0] < data[0].am_data && expected_max[0] > data[0].am_data) ? "OK" : "FAIL";
+		check_res[1] = (expected_min[1] < data[1].am_data && expected_max[1] > data[1].am_data) ? "OK" : "FAIL";
+		check_res[2] = (expected_min[2] < data[2].am_data && expected_max[2] > data[2].am_data) ? "OK" : "FAIL";
+		check_res[3] = (expected_min[3] < data[3].am_data && expected_max[3] > data[3].am_data) ? "OK" : "FAIL";
+
+		/* Accumulate result */
+		ret += (expected_min[0] > data[0].am_data || expected_max[0] < data[0].am_data) ? 1 : 0;
+		ret += (expected_min[1] > data[1].am_data || expected_max[1] < data[1].am_data) ? 1 : 0;
+		ret += (expected_min[2] > data[2].am_data || expected_max[2] < data[2].am_data) ? 1 : 0;
+		ret += (expected_min[3] > data[3].am_data || expected_max[3] < data[3].am_data) ? 1 : 0;
+
+		message("Sample:");
+		message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+			data[0].am_channel, (int)(data[0].am_data), expected_min[0], expected_max[0], check_res[0]);
+		message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+			data[1].am_channel, (int)(data[1].am_data), expected_min[1], expected_max[1], check_res[1]);
+		message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+			data[2].am_channel, (int)(data[2].am_data), expected_min[2], expected_max[2], check_res[2]);
+		message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+			data[3].am_channel, (int)(data[3].am_data), expected_min[3], expected_max[3], check_res[3]);
+
+		if (ret != OK) {
+			printf("\t JIG voltages test FAILED. Some channels where out of allowed range. Check supply voltages.\n");
+			goto errout_with_dev;
+		}
+	}
+
+	printf("\t JIG voltages test successful.\n");
+
+errout_with_dev:
+
+	if (fd != 0) close(fd);
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_led.c b/src/systemcmds/tests/test_led.c
new file mode 100644
index 000000000..6e3efc668
--- /dev/null
+++ b/src/systemcmds/tests/test_led.c
@@ -0,0 +1,134 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_led.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_led(int argc, char *argv[])
+{
+	int		fd;
+	int		ret = 0;
+
+	fd = open(LED_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		printf("\tLED: open fail\n");
+		return ERROR;
+	}
+
+	if (ioctl(fd, LED_ON, LED_BLUE) ||
+	    ioctl(fd, LED_ON, LED_AMBER)) {
+
+		printf("\tLED: ioctl fail\n");
+		return ERROR;
+	}
+
+	/* let them blink for fun */
+
+	int i;
+	uint8_t ledon = 1;
+
+	for (i = 0; i < 10; i++) {
+		if (ledon) {
+			ioctl(fd, LED_ON, LED_BLUE);
+			ioctl(fd, LED_OFF, LED_AMBER);
+
+		} else {
+			ioctl(fd, LED_OFF, LED_BLUE);
+			ioctl(fd, LED_ON, LED_AMBER);
+		}
+
+		ledon = !ledon;
+		usleep(60000);
+	}
+
+	/* Go back to default */
+	ioctl(fd, LED_ON, LED_BLUE);
+	ioctl(fd, LED_OFF, LED_AMBER);
+
+	printf("\t LED test completed, no errors.\n");
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
new file mode 100644
index 000000000..14503276c
--- /dev/null
+++ b/src/systemcmds/tests/test_sensors.c
@@ -0,0 +1,297 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_sensors.c
+ * Tests the onboard sensors.
+ * 
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int accel(int argc, char *argv[]);
+static int gyro(int argc, char *argv[]);
+static int mag(int argc, char *argv[]);
+static int baro(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+struct {
+	const char	*name;
+	const char	*path;
+	int	(* test)(int argc, char *argv[]);
+} sensors[] = {
+	{"accel",	"/dev/accel",	accel},
+	{"gyro",	"/dev/gyro",	gyro},
+	{"mag",		"/dev/mag",	mag},
+	{"baro",	"/dev/baro",	baro},
+	{NULL, NULL, NULL}
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int
+accel(int argc, char *argv[])
+{
+	printf("\tACCEL: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct accel_report buf;
+	int		ret;
+
+	fd = open("/dev/accel", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tACCEL: open fail, run <mpu6000 start> or <lsm303 start> or <bma180 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 100ms, sensor should have data after no more than 20ms */
+	usleep(100000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tACCEL: read1 fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	// /* wait at least 10ms, sensor should have data after no more than 2ms */
+	// usleep(100000);
+
+	// ret = read(fd, buf, sizeof(buf));
+
+	// if (ret != sizeof(buf)) {
+	// 	printf("\tMPU-6000: read2 fail (%d)\n", ret);
+	// 	return ERROR;
+
+	// } else {
+	// 	printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+	// }
+
+	/* XXX more tests here */
+
+	/* Let user know everything is ok */
+	printf("\tOK: ACCEL passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+gyro(int argc, char *argv[])
+{
+	printf("\tGYRO: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct gyro_report buf;
+	int		ret;
+
+	fd = open("/dev/gyro", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tGYRO: read fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: GYRO passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+mag(int argc, char *argv[])
+{
+	printf("\tMAG: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct mag_report buf;
+	int		ret;
+
+	fd = open("/dev/mag", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tMAG: open fail, run <hmc5883 start> or <lsm303 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tMAG: read fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: MAG passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+baro(int argc, char *argv[])
+{
+	printf("\tBARO: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct baro_report buf;
+	int		ret;
+
+	fd = open("/dev/baro", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tBARO: open fail, run <ms5611 start> or <lps331 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tBARO: read fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: BARO passed all tests successfully\n");
+
+	return OK;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_sensors
+ ****************************************************************************/
+
+int test_sensors(int argc, char *argv[])
+{
+	unsigned	i;
+
+	printf("Running sensors tests:\n\n");
+	fflush(stdout);
+
+	int ret = OK;
+
+	for (i = 0; sensors[i].name; i++) {
+		printf("  sensor: %s\n", sensors[i].name);
+
+		/* Flush and leave enough time for the flush to become effective */
+		fflush(stdout);
+		usleep(50000);
+		/* Test the sensor - if the tests crash at this point, the right sensor name has been printed */
+
+		ret += sensors[i].test(argc, argv);
+	}
+
+	return ret;
+}
diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
new file mode 100644
index 000000000..f95760ca8
--- /dev/null
+++ b/src/systemcmds/tests/test_servo.c
@@ -0,0 +1,132 @@
+/****************************************************************************
+ * px4/sensors/test_hrt.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <time.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_servo
+ ****************************************************************************/
+
+int test_servo(int argc, char *argv[])
+{
+	int fd, result;
+	servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
+	servo_position_t pos;
+
+	fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0) {
+		printf("failed opening /dev/pwm_servo\n");
+		goto out;
+	}
+
+	result = read(fd, &data, sizeof(data));
+
+	if (result != sizeof(data)) {
+		printf("failed bulk-reading channel values\n");
+		goto out;
+	}
+
+	printf("Servo readback, pairs of values should match defaults\n");
+
+	for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
+		result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos);
+
+		if (result < 0) {
+			printf("failed reading channel %u\n", i);
+			goto out;
+		}
+
+		printf("%u: %u %u\n", i, pos, data[i]);
+
+	}
+
+	printf("Servos arming at default values\n");
+	result = ioctl(fd, PWM_SERVO_ARM, 0);
+	usleep(5000000);
+	printf("Advancing channel 0 to 1500\n");
+	result = ioctl(fd, PWM_SERVO_SET(0), 1500);
+out:
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_sleep.c b/src/systemcmds/tests/test_sleep.c
new file mode 100644
index 000000000..ae682b542
--- /dev/null
+++ b/src/systemcmds/tests/test_sleep.c
@@ -0,0 +1,100 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_sleep
+ ****************************************************************************/
+
+int test_sleep(int argc, char *argv[])
+{
+	unsigned int nsleeps = 100;
+	printf("\t %d 100ms sleeps\n", nsleeps);
+	fflush(stdout);
+
+	for (unsigned int i = 0; i < nsleeps; i++) {
+		usleep(100000);
+	}
+
+	printf("\t Sleep test successful.\n");
+
+	return OK;
+}
diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c
new file mode 100644
index 000000000..8a164f3fc
--- /dev/null
+++ b/src/systemcmds/tests/test_time.c
@@ -0,0 +1,160 @@
+/****************************************************************************
+ * px4/tests/test_time.c
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <drivers/drv_hrt.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/* emulate hrt_absolute_time using the cycle counter */
+static hrt_abstime
+cycletime(void)
+{
+	static uint64_t basetime;
+	static uint32_t lasttime;
+	uint32_t cycles;
+
+	cycles = *(unsigned long *)0xe0001004;
+
+	if (cycles < lasttime)
+		basetime += 0x100000000ULL;
+
+	lasttime = cycles;
+
+	return (basetime + cycles) / 168;	/* XXX magic number */
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_time(int argc, char *argv[])
+{
+	hrt_abstime h, c;
+	int64_t lowdelta, maxdelta = 0;
+	int64_t delta, deltadelta;
+
+	/* enable the cycle counter */
+	(*(unsigned long *)0xe000edfc) |= (1 << 24);    /* DEMCR |= DEMCR_TRCENA */
+	(*(unsigned long *)0xe0001000) |= 1;    	/* DWT_CTRL |= DWT_CYCCNT_ENA */
+
+	/* get an average delta between the two clocks - this should stay roughly the same */
+	delta = 0;
+
+	for (unsigned i = 0; i < 100; i++) {
+		uint32_t flags = irqsave();
+
+		h = hrt_absolute_time();
+		c = cycletime();
+
+		irqrestore(flags);
+
+		delta += h - c;
+	}
+
+	lowdelta = abs(delta / 100);
+
+	/* loop checking the time */
+	for (unsigned i = 0; i < 100; i++) {
+
+		usleep(rand());
+
+		uint32_t flags = irqsave();
+
+		c = cycletime();
+		h = hrt_absolute_time();
+
+		irqrestore(flags);
+
+		delta = abs(h - c);
+		deltadelta = abs(delta - lowdelta);
+
+		if (deltadelta > maxdelta)
+			maxdelta = deltadelta;
+
+		if (deltadelta > 1000)
+			fprintf(stderr, "h %llu  c %llu  d %lld\n", h, c, delta - lowdelta);
+	}
+
+	printf("Maximum jitter %lldus\n", maxdelta);
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c
new file mode 100644
index 000000000..609a65c62
--- /dev/null
+++ b/src/systemcmds/tests/test_uart_baudchange.c
@@ -0,0 +1,158 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *            Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_uart_baudchange(int argc, char *argv[])
+{
+	int uart2_nwrite = 0;
+
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+
+	if (uart2 < 0) {
+		printf("ERROR opening UART2, aborting..\n");
+		return uart2;
+	}
+
+	struct termios uart2_config;
+
+	struct termios uart2_config_original;
+
+	int termios_state = 0;
+
+#define UART_BAUDRATE_RUNTIME_CONF
+#ifdef UART_BAUDRATE_RUNTIME_CONF
+
+	if ((termios_state = tcgetattr(uart2, &uart2_config)) < 0) {
+		printf("ERROR getting termios config for UART2: %d\n", termios_state);
+		exit(termios_state);
+	}
+
+	memcpy(&uart2_config_original, &uart2_config, sizeof(struct termios));
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart2_config, B9600) < 0 || cfsetospeed(&uart2_config, B9600) < 0) {
+		printf("ERROR setting termios config for UART2: %d\n", termios_state);
+		exit(ERROR);
+	}
+
+	if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config)) < 0) {
+		printf("ERROR setting termios config for UART2\n");
+		exit(termios_state);
+	}
+
+	/* Set back to original settings */
+	if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config_original)) < 0) {
+		printf("ERROR setting termios config for UART2\n");
+		exit(termios_state);
+	}
+
+#endif
+
+	uint8_t sample_uart2[] = {'U', 'A', 'R', 'T', '2', ' ', '#', 0, '\n'};
+
+	int i, r;
+
+	for (i = 0; i < 100; i++) {
+		/* uart2 -> */
+		r = write(uart2, sample_uart2, sizeof(sample_uart2));
+
+		if (r > 0)
+			uart2_nwrite += r;
+	}
+
+	close(uart2);
+
+	printf("uart2_nwrite %d\n", uart2_nwrite);
+
+	return OK;
+}
diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c
new file mode 100644
index 000000000..f8582b52f
--- /dev/null
+++ b/src/systemcmds/tests/test_uart_console.c
@@ -0,0 +1,175 @@
+/****************************************************************************
+ * px4/sensors/test_uart_console.c
+ *
+ *   Copyright (C) 2012 Lorenz Meier. All rights reserved.
+ *   Authors: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <drivers/drv_hrt.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+static void *receive_loop(void *arg)
+{
+	int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY);
+
+	while (1) {
+		char c;
+		read(uart_usb, &c, 1);
+		printf("%c", c);
+		fflush(stdout);
+	}
+
+	return NULL;
+}
+
+int test_uart_console(int argc, char *argv[])
+{
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	//
+	int uart_usb = open("/dev/ttyACM0", O_WRONLY | O_NOCTTY);
+
+	if (uart_usb < 0) {
+		printf("ERROR opening /dev/ttyACM0. Do you need to run sercon first? Aborting..\n");
+		return uart_usb;
+	}
+
+	uint8_t sample_uart_usb[] = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'};
+
+	pthread_t receive_thread;
+
+	pthread_create(&receive_thread, NULL, receive_loop, NULL);
+
+	//wait for threads to complete:
+	pthread_join(receive_thread, NULL);
+
+	for (int i = 0; i < 30; i++) {
+		write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb));
+		printf(".");
+		fflush(stdout);
+		sleep(1);
+	}
+
+//	uint64_t start_time = hrt_absolute_time();
+//
+////	while (true)
+//	for (int i = 0; i < 1000; i++)
+//	{
+//		//write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb));
+//		int nread = 0;
+//		char c;
+//		do {
+//			nread = read(uart_usb, &c, 1);
+//			if (nread > 0)
+//			{
+//				printf("%c", c);
+//			}
+//		} while (nread > 0);
+//
+//		do {
+//			nread = read(uart_console, &c, 1);
+//			if (nread > 0)
+//			{
+//				if (c == 0x03)
+//				{
+//					close(uart_usb);
+//					close(uart_console);
+//					exit(OK);
+//				}
+//				else
+//				{
+//					write(uart_usb, &c, 1);
+//				}
+//			}
+//		} while (nread > 0);
+//		usleep(10000);
+//	}
+//
+//	int interval = hrt_absolute_time() - start_time;
+
+	close(uart_usb);
+//	close(uart_console);
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c
new file mode 100644
index 000000000..3be152004
--- /dev/null
+++ b/src/systemcmds/tests/test_uart_loopback.c
@@ -0,0 +1,195 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *            Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_uart_loopback(int argc, char *argv[])
+{
+
+	int uart5_nread = 0;
+	int uart2_nread = 0;
+	int uart5_nwrite = 0;
+	int uart2_nwrite = 0;
+
+	int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); //
+
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+	int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+
+	if (uart2 < 0) {
+		printf("ERROR opening UART2, aborting..\n");
+		return uart2;
+	}
+
+	if (uart5 < 0) {
+		printf("ERROR opening UART5, aborting..\n");
+		exit(uart5);
+	}
+
+	uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
+	uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
+	uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
+
+	int i, r;
+
+	for (i = 0; i < 1000; i++) {
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		/* uart2 -> uart5 */
+		r = write(uart2, sample_uart2, sizeof(sample_uart2));
+
+		if (r > 0)
+			uart2_nwrite += r;
+
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		/* uart2 -> uart5 */
+		r = write(uart5, sample_uart5, sizeof(sample_uart5));
+
+		if (r > 0)
+			uart5_nwrite += r;
+
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		/* try to read back values */
+		do {
+			r = read(uart5, sample_uart2, sizeof(sample_uart2));
+
+			if (r > 0)
+				uart5_nread += r;
+		} while (r > 0);
+
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		do {
+			r = read(uart2, sample_uart5, sizeof(sample_uart5));
+
+			if (r > 0)
+				uart2_nread += r;
+		} while (r > 0);
+
+//		printf("TEST #%d\n",i);
+//		write(uart1, sample_uart1, sizeof(sample_uart5));
+	}
+
+	for (i = 0; i < 200000; i++) {
+
+		/* try to read back values */
+		r = read(uart5, sample_uart2, sizeof(sample_uart2));
+
+		if (r > 0)
+			uart5_nread += r;
+
+		r = read(uart2, sample_uart5, sizeof(sample_uart5));
+
+		if (r > 0)
+			uart2_nread += r;
+
+		if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite))
+			break;
+	}
+
+
+	close(uart1);
+	close(uart2);
+	close(uart5);
+
+	printf("uart2_nwrite %d\n", uart2_nwrite);
+	printf("uart5_nwrite %d\n", uart5_nwrite);
+	printf("uart2_nread  %d\n", uart2_nread);
+	printf("uart5_nread  %d\n", uart5_nread);
+
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c
new file mode 100644
index 000000000..7e1e8d307
--- /dev/null
+++ b/src/systemcmds/tests/test_uart_send.c
@@ -0,0 +1,131 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *            Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <drivers/drv_hrt.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_uart_send(int argc, char *argv[])
+{
+	/* input handling */
+	char *uart_name = "/dev/ttyS3";
+
+	if (argc > 1) uart_name = argv[1];
+
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); //
+
+	if (test_uart < 0) {
+		printf("ERROR opening UART %s, aborting..\n", uart_name);
+		return test_uart;
+
+	} else {
+		printf("Writing to UART %s\n", uart_name);
+	}
+
+	char sample_test_uart[25];// = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'};
+
+	int i, n;
+
+	uint64_t start_time = hrt_absolute_time();
+
+	for (i = 0; i < 30000; i++) {
+		n = sprintf(sample_test_uart, "SAMPLE #%d\n", i);
+		write(test_uart, sample_test_uart, n);
+	}
+
+	int interval = hrt_absolute_time() - start_time;
+
+	int bytes = i * sizeof(sample_test_uart);
+
+	printf("Wrote %d bytes in %d ms on UART %s\n", bytes, interval / 1000, uart_name);
+
+	close(test_uart);
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
new file mode 100644
index 000000000..c02ea6808
--- /dev/null
+++ b/src/systemcmds/tests/tests.h
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef __APPS_PX4_TESTS_H
+#define __APPS_PX4_TESTS_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#    define msgflush()
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#    define msgflush() fflush(stdout)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#    define msgflush()
+#  else
+#    define message printf
+#    define msgflush() fflush(stdout)
+#  endif
+#endif
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+extern int	test_sensors(int argc, char *argv[]);
+extern int	test_gpio(int argc, char *argv[]);
+extern int	test_hrt(int argc, char *argv[]);
+extern int	test_tone(int argc, char *argv[]);
+extern int	test_led(int argc, char *argv[]);
+extern int	test_adc(int argc, char *argv[]);
+extern int	test_int(int argc, char *argv[]);
+extern int	test_float(int argc, char *argv[]);
+extern int	test_ppm(int argc, char *argv[]);
+extern int	test_servo(int argc, char *argv[]);
+extern int	test_uart_loopback(int argc, char *argv[]);
+extern int	test_uart_baudchange(int argc, char *argv[]);
+extern int	test_cpuload(int argc, char *argv[]);
+extern int	test_uart_send(int argc, char *argv[]);
+extern int	test_sleep(int argc, char *argv[]);
+extern int	test_time(int argc, char *argv[]);
+extern int	test_uart_console(int argc, char *argv[]);
+extern int	test_hott_telemetry(int argc, char *argv[]);
+extern int	test_jig_voltages(int argc, char *argv[]);
+extern int	test_param(int argc, char *argv[]);
+extern int	test_bson(int argc, char *argv[]);
+extern int	test_file(int argc, char *argv[]);
+
+#endif /* __APPS_PX4_TESTS_H */
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c
new file mode 100644
index 000000000..6f75b9812
--- /dev/null
+++ b/src/systemcmds/tests/tests_file.c
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_file.c
+ *
+ * File write test.
+ */
+
+#include <sys/stat.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+int
+test_file(int argc, char *argv[])
+{
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+	uint8_t buf[512];
+	hrt_abstime start, end;
+	perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes");
+
+	int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+	memset(buf, 0, sizeof(buf));
+
+	start = hrt_absolute_time();
+	for (unsigned i = 0; i < 1024; i++) {
+		perf_begin(wperf);
+		write(fd, buf, sizeof(buf));
+		perf_end(wperf);
+	}
+	end = hrt_absolute_time();
+
+	close(fd);
+
+	warnx("512KiB in %llu microseconds", end - start);
+	perf_print_counter(wperf);
+	perf_free(wperf);
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
new file mode 100644
index 000000000..9f8c5c9ea
--- /dev/null
+++ b/src/systemcmds/tests/tests_main.c
@@ -0,0 +1,339 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_main.c
+ * Tests main file, loads individual tests.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/spi.h>
+
+#include <systemlib/perf_counter.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int test_help(int argc, char *argv[]);
+static int test_all(int argc, char *argv[]);
+static int test_perf(int argc, char *argv[]);
+static int test_jig(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+const struct {
+	const char 	*name;
+	int	(* fn)(int argc, char *argv[]);
+	unsigned	options;
+#define OPT_NOHELP	(1<<0)
+#define OPT_NOALLTEST	(1<<1)
+#define OPT_NOJIGTEST	(1<<2)
+} tests[] = {
+	{"led",			test_led,	0},
+	{"int",			test_int,	0},
+	{"float",		test_float,	0},
+	{"sensors",		test_sensors,	0},
+	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"adc",			test_adc,	OPT_NOJIGTEST},
+	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
+	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"hott_telemetry",	test_hott_telemetry,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"tone",		test_tone,	0},
+	{"sleep",		test_sleep,	OPT_NOJIGTEST},
+	{"time",		test_time,	OPT_NOJIGTEST},
+	{"perf",		test_perf,	OPT_NOJIGTEST},
+	{"all",			test_all,	OPT_NOALLTEST | OPT_NOJIGTEST},
+	{"jig",			test_jig,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"param",		test_param,	0},
+	{"bson",		test_bson,	0},
+	{"file",		test_file,	0},
+	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
+	{NULL,			NULL, 		0}
+};
+
+#define NTESTS (sizeof(tests) / sizeof(tests[0]))
+
+static int
+test_help(int argc, char *argv[])
+{
+	unsigned	i;
+
+	printf("Available tests:\n");
+
+	for (i = 0; tests[i].name; i++)
+		printf("  %s\n", tests[i].name);
+
+	return 0;
+}
+
+static int
+test_all(int argc, char *argv[])
+{
+	unsigned	i;
+	char		*args[2] = {"all", NULL};
+	unsigned int failcount = 0;
+	unsigned int testcount = 0;
+	bool		passed[NTESTS];
+
+	printf("\nRunning all tests...\n\n");
+
+	for (i = 0; tests[i].name; i++) {
+		/* Only run tests that are not excluded */
+		if (!(tests[i].options & OPT_NOALLTEST)) {
+			printf("  [%s] \t\t\tSTARTING TEST\n", tests[i].name);
+			fflush(stdout);
+
+			/* Execute test */
+			if (tests[i].fn(1, args) != 0) {
+				fprintf(stderr, "  [%s] \t\t\tFAIL\n", tests[i].name);
+				fflush(stderr);
+				failcount++;
+				passed[i] = false;
+			} else {
+				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				fflush(stdout);
+				passed[i] = true;
+			}
+			testcount++;
+		}
+	}
+
+	/* Print summary */
+	printf("\n");
+	int j;
+
+	for (j = 0; j < 40; j++) {
+		printf("-");
+	}
+
+	printf("\n\n");
+
+	printf("     T E S T    S U M M A R Y\n\n");
+
+	if (failcount == 0) {
+		printf("  ______     __         __            ______     __  __    \n");
+		printf(" /\\  __ \\   /\\ \\       /\\ \\          /\\  __ \\   /\\ \\/ /    \n");
+		printf(" \\ \\  __ \\  \\ \\ \\____  \\ \\ \\____     \\ \\ \\/\\ \\  \\ \\  _\"-.  \n");
+		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
+		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
+		printf("\n");
+		printf(" All tests passed (%d of %d)\n", testcount, testcount);
+
+	} else {
+		printf("  ______   ______     __     __ \n");
+		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
+		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
+		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
+		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
+		printf("\n");
+		printf(" Some tests failed (%d of %d)\n", failcount, testcount);
+	}
+
+	printf("\n");
+
+	/* Print failed tests */
+	if (failcount > 0) printf(" Failed tests:\n\n");
+
+	unsigned int k;
+
+	for (k = 0; k < i; k++) {
+		if (!passed[k] && !(tests[k].options & OPT_NOALLTEST)) {
+			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
+		}
+	}
+
+	fflush(stdout);
+
+	return 0;
+}
+
+static int
+test_perf(int argc, char *argv[])
+{
+	perf_counter_t	cc, ec;
+
+	cc = perf_alloc(PC_COUNT, "test_count");
+	ec = perf_alloc(PC_ELAPSED, "test_elapsed");
+
+	if ((cc == NULL) || (ec == NULL)) {
+		printf("perf: counter alloc failed\n");
+		return 1;
+	}
+
+	perf_begin(ec);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	printf("perf: expect count of 4\n");
+	perf_print_counter(cc);
+	perf_end(ec);
+	printf("perf: expect count of 1\n");
+	perf_print_counter(ec);
+	printf("perf: expect at least two counters\n");
+	perf_print_all();
+
+	perf_free(cc);
+	perf_free(ec);
+
+	return OK;
+}
+
+int test_jig(int argc, char *argv[])
+{
+	unsigned	i;
+	char		*args[2] = {"jig", NULL};
+	unsigned int failcount = 0;
+	unsigned int testcount = 0;
+	bool		passed[NTESTS];
+
+	printf("\nRunning all tests...\n\n");
+	for (i = 0; tests[i].name; i++) {
+		/* Only run tests that are not excluded */
+		if (!(tests[i].options & OPT_NOJIGTEST)) {
+			printf("  [%s] \t\t\tSTARTING TEST\n", tests[i].name);
+			fflush(stdout);
+			/* Execute test */
+			if (tests[i].fn(1, args) != 0) {
+				fprintf(stderr, "  [%s] \t\t\tFAIL\n", tests[i].name);
+				fflush(stderr);
+				failcount++;
+				passed[i] = false;
+			} else {
+				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				fflush(stdout);
+				passed[i] = true;
+			}
+			testcount++;
+		}
+	}
+
+	/* Print summary */
+	printf("\n");
+	int j;
+	for (j = 0; j < 40; j++)
+	{
+		printf("-");
+	}
+	printf("\n\n");
+
+	printf("     T E S T    S U M M A R Y\n\n");
+	if (failcount == 0) {
+		printf("  ______     __         __            ______     __  __    \n");
+		printf(" /\\  __ \\   /\\ \\       /\\ \\          /\\  __ \\   /\\ \\/ /    \n");
+		printf(" \\ \\  __ \\  \\ \\ \\____  \\ \\ \\____     \\ \\ \\/\\ \\  \\ \\  _\"-.  \n");
+		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
+		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
+		printf("\n");
+		printf(" All tests passed (%d of %d)\n", testcount, testcount);
+	} else {
+		printf("  ______   ______     __     __ \n");
+		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
+		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
+		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
+		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
+		printf("\n");
+		printf(" Some tests failed (%d of %d)\n", failcount, testcount);
+	}
+	printf("\n");
+
+	/* Print failed tests */
+	if (failcount > 0) printf(" Failed tests:\n\n");
+	unsigned int k;
+	for (k = 0; k < i; k++)
+	{
+		if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST))
+		{
+			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
+		}
+	}
+	fflush(stdout);
+
+	return 0;
+}
+
+__EXPORT int tests_main(int argc, char *argv[]);
+
+/**
+ * Executes system tests.
+ */
+int tests_main(int argc, char *argv[])
+{
+	unsigned	i;
+
+	if (argc < 2) {
+		printf("tests: missing test name - 'tests help' for a list of tests\n");
+		return 1;
+	}
+
+	for (i = 0; tests[i].name; i++) {
+		if (!strcmp(tests[i].name, argv[1]))
+			return tests[i].fn(argc - 1, argv + 1);
+	}
+
+	printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]);
+	return ERROR;
+}
diff --git a/src/systemcmds/tests/tests_param.c b/src/systemcmds/tests/tests_param.c
new file mode 100644
index 000000000..13f17bc43
--- /dev/null
+++ b/src/systemcmds/tests/tests_param.c
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_param.c
+ *
+ * Tests related to the parameter system.
+ */
+
+#include <stdio.h>
+#include "systemlib/err.h"
+
+#include "systemlib/param/param.h"
+#include "tests.h"
+
+PARAM_DEFINE_INT32(test, 0x12345678);
+
+int
+test_param(int argc, char *argv[])
+{
+	param_t		p;
+
+	p = param_find("test");
+	if (p == PARAM_INVALID)
+		errx(1, "test parameter not found");
+
+	param_type_t t = param_type(p);
+	if (t != PARAM_TYPE_INT32)
+		errx(1, "test parameter type mismatch (got %u)", (unsigned)t);
+
+	int32_t	val;
+	if (param_get(p, &val) != 0)
+		errx(1, "failed to read test parameter");
+	if (val != 0x12345678)
+		errx(1, "parameter value mismatch");
+
+	val = 0xa5a5a5a5;
+	if (param_set(p, &val) != 0)
+		errx(1, "failed to write test parameter");
+	if (param_get(p, &val) != 0)
+		errx(1, "failed to re-read test parameter");
+	if ((uint32_t)val != 0xa5a5a5a5)
+		errx(1, "parameter value mismatch after write");
+
+	warnx("parameter test PASS");
+
+	return 0;
+}
diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk
new file mode 100644
index 000000000..9239aafc3
--- /dev/null
+++ b/src/systemcmds/top/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Build top memory / cpu status tool.
+#
+
+MODULE_COMMAND	 = top
+SRCS		 = top.c
+
+MODULE_STACKSIZE = 3000
+
+MAXOPTIMIZATION	 = -Os
+
diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
new file mode 100644
index 000000000..59d2bc8f1
--- /dev/null
+++ b/src/systemcmds/top/top.c
@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file top.c
+ * Tool similar to UNIX top command
+ * @see http://en.wikipedia.org/wiki/Top_unix
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <string.h>
+#include <poll.h>
+
+#include <systemlib/cpuload.h>
+#include <drivers/drv_hrt.h>
+
+/**
+ * Start the top application.
+ */
+__EXPORT int top_main(int argc, char *argv[]);
+
+extern struct system_load_s system_load;
+
+bool top_sigusr1_rcvd = false;
+
+int top_main(int argc, char *argv[])
+{
+	int t;
+
+	uint64_t total_user_time = 0;
+
+	int running_count = 0;
+	int blocked_count = 0;
+
+	uint64_t new_time = hrt_absolute_time();
+	uint64_t interval_start_time = new_time;
+
+	uint64_t last_times[CONFIG_MAX_TASKS];
+	float curr_loads[CONFIG_MAX_TASKS];
+
+	for (t = 0; t < CONFIG_MAX_TASKS; t++)
+		last_times[t] = 0;
+
+	float interval_time_ms_inv = 0.f;
+
+	/* Open console directly to grab CTRL-C signal */
+	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+
+	while (true)
+//	for (t = 0; t < 10; t++)
+	{
+		int i;
+
+		uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU);
+		unsigned int curr_time_s = curr_time_ms / 1000LLU;
+
+		uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU);
+		unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU;
+
+		if (new_time > interval_start_time)
+			interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000));
+
+		running_count = 0;
+		blocked_count = 0;
+		total_user_time = 0;
+
+		for (i = 0; i < CONFIG_MAX_TASKS; i++) {
+			uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0;
+
+			last_times[i] = system_load.tasks[i].total_runtime;
+
+			if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
+				curr_loads[i] = interval_runtime * interval_time_ms_inv;
+
+				if (i > 0)
+					total_user_time += interval_runtime;
+
+			} else
+				curr_loads[i] = 0;
+		}
+
+		for (i = 0; i < CONFIG_MAX_TASKS; i++) {
+			if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
+				if (system_load.tasks[i].tcb->pid == 0) {
+					float idle = curr_loads[0];
+					float task_load = (float)(total_user_time) * interval_time_ms_inv;
+
+					if (task_load > (1.f - idle)) task_load = (1.f - idle);	/* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */
+
+					float sched_load = 1.f - idle - task_load;
+
+					/* print system information */
+					printf("\033[H"); /* cursor home */
+					printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count);
+					printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100));
+					printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000));
+
+					/* 34 chars command name length (32 chars plus two spaces) */
+					char header_spaces[CONFIG_TASK_NAME_SIZE + 1];
+					memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
+					header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
+#if CONFIG_RR_INTERVAL > 0
+					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
+#else
+					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces);
+#endif
+
+				} else {
+					enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state;
+
+					if (task_state == TSTATE_TASK_PENDING ||
+					    task_state == TSTATE_TASK_READYTORUN ||
+					    task_state == TSTATE_TASK_RUNNING) {
+						running_count++;
+					}
+
+					if (task_state == TSTATE_TASK_INACTIVE ||      /* BLOCKED      - Initialized but not yet activated */
+					    task_state == TSTATE_WAIT_SEM           /* BLOCKED      - Waiting for a semaphore */
+#ifndef CONFIG_DISABLE_SIGNALS
+					    || task_state == TSTATE_WAIT_SIG           /* BLOCKED      - Waiting for a signal */
+#endif
+#ifndef CONFIG_DISABLE_MQUEUE
+					    || task_state == TSTATE_WAIT_MQNOTEMPTY     /* BLOCKED      - Waiting for a MQ to become not empty. */
+					    || task_state == TSTATE_WAIT_MQNOTFULL      /* BLOCKED      - Waiting for a MQ to become not full. */
+#endif
+#ifdef CONFIG_PAGING
+					    || task_state == TSTATE_WAIT_PAGEFILL       /* BLOCKED      - Waiting for page fill */
+#endif
+					   ) {
+						blocked_count++;
+					}
+
+					char spaces[CONFIG_TASK_NAME_SIZE + 2];
+
+					/* count name len */
+					int namelen = 0;
+
+					while (namelen < CONFIG_TASK_NAME_SIZE) {
+						if (system_load.tasks[i].tcb->name[namelen] == '\0') break;
+
+						namelen++;
+					}
+
+					int s = 0;
+
+					for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) {
+						spaces[s] = ' ';
+					}
+
+					spaces[s] = '\0';
+
+					char *runtime_spaces = "  ";
+
+					if ((system_load.tasks[i].total_runtime / 1000) < 99) {
+						runtime_spaces = "";
+					}
+
+					unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
+							      (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
+					unsigned stack_free = 0;
+					uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
+
+					while (stack_free < stack_size) {
+						if (*stack_sweeper++ != 0xff)
+							break;
+
+						stack_free++;
+					}
+
+					printf("\033[K % 2d\t%s%s % 8lld ms%s  \t % 2d.%03d \t % 4u / % 4u",
+					       (int)system_load.tasks[i].tcb->pid,
+					       system_load.tasks[i].tcb->name,
+					       spaces,
+					       (system_load.tasks[i].total_runtime / 1000),
+					       runtime_spaces,
+					       (int)(curr_loads[i] * 100),
+					       (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
+					       stack_size - stack_free,
+					       stack_size);
+					/* Print scheduling info with RR time slice */
+#if CONFIG_RR_INTERVAL > 0
+					printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);
+#else
+					/* Print scheduling info without time slice*/
+					printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority);
+#endif
+				}
+			}
+		}
+
+		printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J");
+		fflush(stdout);
+
+		interval_start_time = new_time;
+
+		char c;
+
+		/* Sleep 200 ms waiting for user input four times */
+		/* XXX use poll ... */
+		for (int k = 0; k < 4; k++) {
+			if (read(console, &c, 1) == 1) {
+				if (c == 0x03 || c == 0x63) {
+					printf("Abort\n");
+					close(console);
+					return OK;
+				}
+			}
+
+			usleep(200000);
+		}
+
+		new_time = hrt_absolute_time();
+	}
+
+	close(console);
+
+	return OK;
+}