Moved all system commands to the new world

21 files changed, 2493 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/module.mk b/src/systemcmds/eeprom/module.mk
index 3b4fc0479..07f3945be 100644
--- a/src/systemcmds/eeprom/module.mk
+++ b/src/systemcmds/eeprom/module.mk
@@ -1,3 +1,36 @@
+############################################################################
+#
+#   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
 #
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..56f5317e3
--- /dev/null
+++ b/src/systemcmds/param/param.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 param.c
+ *
+ * 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("old: %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("float values are not yet supported.");
+			// printf("old: %4.4f", (double)f);
+
+			// /* convert string */
+			// char* end;
+			// f = strtof(val,&end);
+			// param_set(param, &f);
+			// printf(" -> new: %4.4f\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..de7a53199
--- /dev/null
+++ b/src/systemcmds/pwm/pwm.c
@@ -0,0 +1,233 @@
+/****************************************************************************
+ *
+ *   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>] [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"
+		"\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;
+
+	if (argc < 2)
+		usage(NULL);
+
+	while ((ch = getopt(argc, argv, "c:d:u:v")) != 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 '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)");
+	}
+
+	/* 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..47d3cd948
--- /dev/null
+++ b/src/systemcmds/reboot/reboot.c
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   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[])
+{
+	up_systemreset();
+}
+
+
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;
+}