Fresh import of the PX4 firmware sources.

18 files changed, 1755 insertions, 0 deletions

diff --git a/apps/systemcmds/boardinfo/.context b/apps/systemcmds/boardinfo/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/systemcmds/boardinfo/.context
diff --git a/apps/systemcmds/boardinfo/Makefile b/apps/systemcmds/boardinfo/Makefile
new file mode 100644
index 000000000..753a6843e
--- /dev/null
+++ b/apps/systemcmds/boardinfo/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build the boardinfo tool.
+#
+
+APPNAME		 = boardinfo
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c
new file mode 100644
index 000000000..61a6ea221
--- /dev/null
+++ b/apps/systemcmds/boardinfo/boardinfo.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 autopilot and carrier board information app */
+
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "systemlib/systemlib.h"
+
+__EXPORT int boardinfo_main(int argc, char *argv[]);
+
+/*
+ * Reads out the board information
+ *
+ * @param argc the number of string arguments (including the executable name)
+ * @param argv the argument strings
+ *
+ * @return 0 on success, 1 on error
+ */
+int boardinfo_main(int argc, char *argv[])
+{
+	const char *commandline_usage = "\tusage: boardinfo [-c|-f] [-t id] [-w \"<info>\"]\n";
+
+	bool carrier_mode = false;
+	bool fmu_mode = false;
+	bool write_mode = false;
+	char *write_string = 0;
+	bool silent = false;
+	bool test_enabled = false;
+	int test_boardid = -1;
+	int ch;
+
+	while ((ch = getopt(argc, argv, "cft:w:v")) != -1) {
+		switch (ch) {
+		case 'c':
+			carrier_mode = true;
+			break;
+
+		case 'f':
+			fmu_mode = true;
+			break;
+
+		case 't':
+			test_enabled = true;
+			test_boardid = strtol(optarg, NULL, 10);
+			silent = true;
+			break;
+
+		case 'w':
+			write_mode = true;
+			write_string = optarg;
+			break;
+
+		default:
+			printf(commandline_usage);
+			exit(0);
+		}
+	}
+
+	/* Check if write is required - only one mode is allowed then */
+	if (write_mode && fmu_mode && carrier_mode) {
+		fprintf(stderr, "[boardinfo] Please choose only one mode for writing: --carrier or --fmu\n");
+		printf(commandline_usage);
+		return ERROR;
+	}
+
+	/* Write FMU information
+	if (fmu_mode && write_mode) {
+		struct fmu_board_info_s info;
+		int ret = fmu_store_board_info(&info);
+
+
+		if (ret == OK) {
+			printf("[boardinfo] Successfully wrote FMU board info\n");
+		} else {
+			fprintf(stderr, "[boardinfo] ERROR writing board info to FMU EEPROM, aborting\n");
+			return ERROR;
+		}
+	}*/
+
+	/* write carrier board info */
+	if (carrier_mode && write_mode) {
+
+		struct carrier_board_info_s info;
+		bool parse_ok = true;
+		unsigned parse_idx = 0;
+		//int maxlen = strlen(write_string);
+		char *curr_char;
+
+		/* Parse board info string */
+		if (write_string[0] != 'P' || write_string[1] != 'X' || write_string[2] != '4') {
+			fprintf(stderr, "[boardinfo] header must start with 'PX4'\n");
+			parse_ok = false;
+		}
+
+		info.header[0] = 'P'; info.header[1] = 'X'; info.header[2] = '4';
+		parse_idx = 3;
+		/* Copy board name */
+
+		int i = 0;
+
+		while (write_string[parse_idx] != 0x20 && (parse_idx < sizeof(info.board_name) + sizeof(info.header))) {
+			info.board_name[i] = write_string[parse_idx];
+			i++; parse_idx++;
+		}
+
+		/* Enforce null termination */
+		info.board_name[sizeof(info.board_name) - 1] = '\0';
+
+		curr_char = write_string + parse_idx;
+
+		/* Index is now on next field */
+		info.board_id = strtol(curr_char, &curr_char, 10);//atoi(write_string + parse_index);
+		info.board_version = strtol(curr_char, &curr_char, 10);
+
+		/* Read in multi ports */
+		for (i = 0; i < MULT_COUNT; i++) {
+			info.multi_port_config[i] = strtol(curr_char, &curr_char, 10);
+		}
+
+		/* Read in production data */
+		info.production_year = strtol(curr_char, &curr_char, 10);
+
+		if (info.production_year < 2012 || info.production_year > 3000) {
+			fprintf(stderr, "[boardinfo] production year is invalid: %d\n", info.production_year);
+			parse_ok = false;
+		}
+
+		info.production_month = strtol(curr_char, &curr_char, 10);
+
+		if (info.production_month < 1 || info.production_month > 12) {
+			fprintf(stderr, "[boardinfo] production month is invalid: %d\n", info.production_month);
+			parse_ok = false;
+		}
+
+		info.production_day = strtol(curr_char, &curr_char, 10);
+
+		if (info.production_day < 1 || info.production_day > 31) {
+			fprintf(stderr, "[boardinfo] production day is invalid: %d\n", info.production_day);
+			parse_ok = false;
+		}
+
+		info.production_fab = strtol(curr_char, &curr_char, 10);
+		info.production_tester = strtol(curr_char, &curr_char, 10);
+
+		if (!parse_ok) {
+			/* Parsing failed */
+			fprintf(stderr, "[boardinfo] failed parsing info string:\n\t%s\naborting\n", write_string);
+			return ERROR;
+
+		} else {
+			int ret = carrier_store_board_info(&info);
+
+			/* Display result */
+			if (ret == sizeof(info)) {
+				printf("[boardinfo] Successfully wrote carrier board info\n");
+
+			} else {
+				fprintf(stderr, "[boardinfo] ERROR writing board info to carrier EEPROM (forgot to pull the WRITE_ENABLE line high?), aborting\n");
+				return ERROR;
+			}
+		}
+	}
+
+	/* Print FMU information */
+	if (fmu_mode && !silent) {
+		struct fmu_board_info_s info;
+		int ret = fmu_get_board_info(&info);
+
+		/* Print human readable name */
+		if (ret == sizeof(info)) {
+			printf("[boardinfo] Autopilot:\n\t%s\n", info.header);
+
+		} else {
+			fprintf(stderr, "[boardinfo] ERROR loading board info from FMU, aborting\n");
+			return ERROR;
+		}
+	}
+
+	/* print carrier information */
+	if (carrier_mode && !silent) {
+
+		struct carrier_board_info_s info;
+		int ret = carrier_get_board_info(&info);
+
+		/* Print human readable name */
+		if (ret == sizeof(info)) {
+			printf("[boardinfo] Carrier board:\n\t%s\n", info.header);
+			printf("\tboard id:\t\t%d\n", info.board_id);
+			printf("\tversion:\t\t%d\n", info.board_version);
+
+			for (unsigned i = 0; i < MULT_COUNT; i++) {
+				printf("\tmulti port #%d:\t\t%s: function #%d\n", i, multiport_info.port_names[i], info.multi_port_config[i]);
+			}
+
+			printf("\tproduction date:\t%d-%d-%d (fab #%d / tester #%d)\n", info.production_year, info.production_month, info.production_day, info.production_fab, info.production_tester);
+
+		} else {
+			fprintf(stderr, "[boardinfo] ERROR loading board info from carrier EEPROM (errno #%d), aborting\n", -ret);
+			return ERROR;
+		}
+	}
+
+	/* test for a specific carrier */
+	if (test_enabled) {
+
+		struct carrier_board_info_s info;
+		int ret = carrier_get_board_info(&info);
+
+		if (ret != sizeof(info)) {
+			fprintf(stderr, "[boardinfo] ERROR loading board info from EEPROM (errno #%d), aborting\n", -ret);
+			exit(1);
+		}
+
+		if (info.board_id == test_boardid) {
+			printf("[boardinfo] Found carrier board with ID %d, test succeeded\n", info.board_id);
+			exit(0);
+
+		} else {
+			/* exit silently with an error so we can test for multiple boards quietly */
+			exit(1);
+		}
+	}
+
+	return 0;
+}
+
+
diff --git a/apps/systemcmds/calibration/Makefile b/apps/systemcmds/calibration/Makefile
new file mode 100644
index 000000000..aa1aa7761
--- /dev/null
+++ b/apps/systemcmds/calibration/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the calibration tool
+#
+
+APPNAME		 = calibration
+PRIORITY	 = SCHED_PRIORITY_MAX - 1
+STACKSIZE	 = 4096
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/calibration/calibration.c b/apps/systemcmds/calibration/calibration.c
new file mode 100755
index 000000000..7f8b9240f
--- /dev/null
+++ b/apps/systemcmds/calibration/calibration.c
@@ -0,0 +1,147 @@
+/****************************************************************************
+ * calibration.c
+ *
+ *   Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
+ *   Authors: Nils Wenzler <wenzlern@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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "calibration.h"
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int calibrate_help(int argc, char *argv[]);
+static int calibrate_all(int argc, char *argv[]);
+
+__EXPORT int calibration_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+struct {
+	const char 	*name;
+	int	(* fn)(int argc, char *argv[]);
+	unsigned	options;
+#define OPT_NOHELP	(1<<0)
+#define OPT_NOALLTEST	(1<<1)
+} calibrates[] = {
+	{"range",	          range_cal,	0},
+	{"servo",	          servo_cal,	0},
+	{"all",	          calibrate_all,	OPT_NOALLTEST},
+	{"help",          calibrate_help,	OPT_NOALLTEST | OPT_NOHELP},
+	{NULL,		NULL}
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int
+calibrate_help(int argc, char *argv[])
+{
+	unsigned	i;
+
+	printf("Available calibration routines:\n");
+
+	for (i = 0; calibrates[i].name; i++)
+		printf("  %s\n", calibrates[i].name);
+
+	return 0;
+}
+
+static int
+calibrate_all(int argc, char *argv[])
+{
+	unsigned	i;
+	char		*args[2] = {"all", NULL};
+
+	printf("Running all calibration routines...\n\n");
+
+	for (i = 0; calibrates[i].name; i++) {
+		printf("  %s:\n", calibrates[i].name);
+
+		if (calibrates[i].fn(1, args)) {
+			printf("  FAIL\n");
+
+		} else {
+			printf("  DONE\n");
+		}
+	}
+
+	return 0;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+void press_enter(void)
+{
+	int c;
+	printf("Press CTRL+ENTER to continue... \n");
+	fflush(stdout);
+
+	do c = getchar(); while ((c != '\n') && (c != EOF));
+}
+
+/****************************************************************************
+ * Name: calibrate_main
+ ****************************************************************************/
+
+int calibration_main(int argc, char *argv[])
+{
+	unsigned	i;
+
+	if (argc < 2) {
+		printf("calibration: missing name - 'calibration help' for a list of routines\n");
+		return 1;
+	}
+
+	for (i = 0; calibrates[i].name; i++) {
+		if (!strcmp(calibrates[i].name, argv[1]))
+			return calibrates[i].fn(argc - 1, argv + 1);
+	}
+
+	printf("calibrate: no routines called '%s' - 'calibration help' for a list of routines\n", argv[1]);
+	return 1;
+}
diff --git a/apps/systemcmds/calibration/calibration.h b/apps/systemcmds/calibration/calibration.h
new file mode 100644
index 000000000..028853ec8
--- /dev/null
+++ b/apps/systemcmds/calibration/calibration.h
@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   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 _CALIBRATION_H
+#define _CALIBRATION_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+extern int range_cal(int argc, char *argv[]);
+extern int servo_cal(int argc, char *argv[]);
+
+#endif
diff --git a/apps/systemcmds/calibration/channels_cal.c b/apps/systemcmds/calibration/channels_cal.c
new file mode 100755
index 000000000..575138b12
--- /dev/null
+++ b/apps/systemcmds/calibration/channels_cal.c
@@ -0,0 +1,196 @@
+/****************************************************************************
+ * channels_cal.c
+ *
+ *   Copyright (C) 2012 Nils Wenzler. All rights reserved.
+ *   Authors: Nils Wenzler <wenzlern@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 <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include "calibration.h"
+
+/****************************************************************************
+ * Defines
+ ****************************************************************************/
+#define ABS(a)    (((a) < 0) ? -(a) : (a))
+#define MIN(a,b)  (((a)<(b))?(a):(b))
+#define MAX(a,b) (((a)>(b))?(a):(b))
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+//Store the values here before writing them to global_data_rc_channels
+uint16_t old_values[NR_CHANNELS];
+uint16_t cur_values[NR_CHANNELS];
+uint8_t  function_map[NR_CHANNELS];
+char names[12][9];
+
+
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+void press_enter_2(void)
+{
+	int c;
+	printf("Press CTRL+ENTER to continue... \n");
+	fflush(stdout);
+
+	do c = getchar(); while ((c != '\n') && (c != EOF));
+}
+
+/**This gets all current values and writes them to min or max
+ */
+uint8_t get_val(uint16_t *buffer)
+{
+	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
+		uint8_t i;
+
+		for (i = 0; i < NR_CHANNELS; i++) {
+			printf("Channel: %i\t RAW Value: %i: \n", i, global_data_rc_channels->chan[i].raw);
+			buffer[i] = global_data_rc_channels->chan[i].raw;
+		}
+
+		global_data_unlock(&global_data_rc_channels->access_conf);
+		return 0;
+
+	} else
+		return -1;
+}
+
+void set_channel(void)
+{
+	static uint8_t j = 0;
+	uint8_t i;
+
+	for (i = 0; i < NR_CHANNELS; i++) {
+		if (ABS(old_values - cur_values) > 50) {
+			function_map[j] = i;
+			strcpy(names[i], global_data_rc_channels->function_names[j]);
+			j++;
+		}
+	}
+}
+
+void write_dat(void)
+{
+	global_data_lock(&global_data_rc_channels->access_conf);
+	uint8_t i;
+
+	for (i = 0; i < NR_CHANNELS; i++) {
+		global_data_rc_channels->function[i] = function_map[i];
+		strcpy(global_data_rc_channels->chan[i].name, names[i]);
+
+		printf("Channel %i\t Function %s\n", i,
+		       global_data_rc_channels->chan[i].name);
+	}
+
+	global_data_unlock(&global_data_rc_channels->access_conf);
+}
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+int channels_cal(int argc, char *argv[])
+{
+
+	printf("This routine maps the input functions on the remote control\n");
+	printf("to the corresponding function (Throttle, Roll,..)\n");
+	printf("Always move the stick all the way\n");
+	press_enter_2();
+
+	printf("Pull the THROTTLE stick down\n");
+	press_enter_2();
+
+	while (get_val(old_values));
+
+	printf("Move the THROTTLE stick up\n ");
+	press_enter_2();
+
+	while (get_val(cur_values));
+
+	set_channel();
+
+	printf("Pull the PITCH stick down\n");
+	press_enter_2();
+
+	while (get_val(old_values));
+
+	printf("Move the PITCH stick up\n ");
+	press_enter_2();
+
+	while (get_val(cur_values));
+
+	set_channel();
+
+	printf("Put the ROLL stick to the left\n");
+	press_enter_2();
+
+	while (get_val(old_values));
+
+	printf("Put the ROLL stick to the right\n ");
+	press_enter_2();
+
+	while (get_val(cur_values));
+
+	set_channel();
+
+	printf("Put the YAW stick to the left\n");
+	press_enter_2();
+
+	while (get_val(old_values));
+
+	printf("Put the YAW stick to the right\n ");
+	press_enter_2();
+
+	while (get_val(cur_values));
+
+	set_channel();
+
+	uint8_t k;
+
+	for (k = 5; k < NR_CHANNELS; k++) {
+		function_map[k] = k;
+		strcpy(names[k], global_data_rc_channels->function_names[k]);
+	}
+
+//write the values to global_data_rc_channels
+	write_dat();
+
+	return 0;
+
+}
+
diff --git a/apps/systemcmds/calibration/range_cal.c b/apps/systemcmds/calibration/range_cal.c
new file mode 100755
index 000000000..159a0d06b
--- /dev/null
+++ b/apps/systemcmds/calibration/range_cal.c
@@ -0,0 +1,224 @@
+/****************************************************************************
+ * range_cal.c
+ *
+ *   Copyright (C) 2012 Nils Wenzler. All rights reserved.
+ *   Authors: Nils Wenzler <wenzlern@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 <stdio.h>
+#include <stdlib.h>
+#include "calibration.h"
+
+/****************************************************************************
+ * Defines
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+//Store the values here before writing them to global_data_rc_channels
+uint16_t max_values[NR_CHANNELS];
+uint16_t min_values[NR_CHANNELS];
+uint16_t mid_values[NR_CHANNELS];
+
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/**This sets the middle values
+ */
+uint8_t set_mid(void)
+{
+	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
+		uint8_t i;
+
+		for (i = 0; i < NR_CHANNELS; i++) {
+			if (i == global_data_rc_channels->function[ROLL] ||
+			    i == global_data_rc_channels->function[YAW] ||
+			    i == global_data_rc_channels->function[PITCH]) {
+
+				mid_values[i] = global_data_rc_channels->chan[i].raw;
+
+			} else {
+				mid_values[i] = (max_values[i] + min_values[i]) / 2;
+			}
+
+		}
+
+		global_data_unlock(&global_data_rc_channels->access_conf);
+		return 0;
+
+	} else
+		return -1;
+}
+
+/**This gets all current values and writes them to min or max
+ */
+uint8_t get_value(void)
+{
+	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
+		uint8_t i;
+
+		for (i = 0; i < NR_CHANNELS; i++) {
+			//see if the value is bigger or smaller than 1500 (roughly neutral)
+			//and write to the appropriate array
+			if (global_data_rc_channels->chan[i].raw != 0 &&
+			    global_data_rc_channels->chan[i].raw < 1500) {
+				min_values[i] = global_data_rc_channels->chan[i].raw;
+
+			} else if (global_data_rc_channels->chan[i].raw != 0 &&
+				   global_data_rc_channels->chan[i].raw > 1500) {
+				max_values[i] = global_data_rc_channels->chan[i].raw;
+
+			} else {
+				max_values[i] = 0;
+				min_values[i] = 0;
+			}
+		}
+
+		global_data_unlock(&global_data_rc_channels->access_conf);
+		return 0;
+
+	} else
+		return -1;
+}
+
+
+void write_data(void)
+{
+	//  global_data_lock(&global_data_rc_channels->access_conf);
+	//  uint8_t i;
+	//  for(i=0; i < NR_CHANNELS; i++){
+	//    //Write the data to global_data_rc_channels (if not 0)
+	//    if(mid_values[i]!=0 && min_values[i]!=0 && max_values[i]!=0){
+	//      global_data_rc_channels->chan[i].scaling_factor =
+	//        10000/((max_values[i] - min_values[i])/2);
+	//
+	//      global_data_rc_channels->chan[i].mid = mid_values[i];
+	//    }
+	//
+	//    printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
+	//        i,
+	//        global_data_rc_channels->function_name[global_data_rc_channels->function[i]],
+	//        min_values[i], max_values[i],
+	//        global_data_rc_channels->chan[i].scaling_factor,
+	//        global_data_rc_channels->chan[i].mid);
+	//  }
+	//  global_data_unlock(&global_data_rc_channels->access_conf);
+
+	//Write to the Parameter storage
+
+	global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN] = min_values[0];
+	global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN] = min_values[1];
+	global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN] = min_values[2];
+	global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN] = min_values[3];
+	global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN] = min_values[4];
+	global_data_parameter_storage->pm.param_values[PARAM_RC6_MIN] = min_values[5];
+	global_data_parameter_storage->pm.param_values[PARAM_RC7_MIN] = min_values[6];
+	global_data_parameter_storage->pm.param_values[PARAM_RC8_MIN] = min_values[7];
+
+
+	global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] = max_values[0];
+	global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] = max_values[1];
+	global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] = max_values[2];
+	global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] = max_values[3];
+	global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] = max_values[4];
+	global_data_parameter_storage->pm.param_values[PARAM_RC6_MAX] = max_values[5];
+	global_data_parameter_storage->pm.param_values[PARAM_RC7_MAX] = max_values[6];
+	global_data_parameter_storage->pm.param_values[PARAM_RC8_MAX] = max_values[7];
+
+
+	global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM] = mid_values[0];
+	global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM] = mid_values[1];
+	global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM] = mid_values[2];
+	global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM] = mid_values[3];
+	global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM] = mid_values[4];
+	global_data_parameter_storage->pm.param_values[PARAM_RC6_TRIM] = mid_values[5];
+	global_data_parameter_storage->pm.param_values[PARAM_RC7_TRIM] = mid_values[6];
+	global_data_parameter_storage->pm.param_values[PARAM_RC8_TRIM] = mid_values[7];
+
+	usleep(3e6);
+	uint8_t i;
+
+	for (i = 0; i < NR_CHANNELS; i++) {
+		printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
+		       i,
+		       min_values[i], max_values[i],
+		       global_data_rc_channels->chan[i].scaling_factor,
+		       global_data_rc_channels->chan[i].mid);
+	}
+
+
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+int range_cal(int argc, char *argv[])
+{
+
+	printf("The range calibration routine assumes you already did the channel\n");
+	printf("assignment\n");
+	printf("This routine chooses the minimum, maximum and middle\n");
+	printf("value for each channel separatly. The ROLL, PITCH and YAW function\n");
+	printf("get their middle value from the RC direct, for the rest it is\n");
+	printf("calculated out of the min and max values.\n");
+	press_enter();
+
+	printf("Hold both sticks in lower left corner and continue\n ");
+	press_enter();
+	usleep(500000);
+
+	while (get_value());
+
+	printf("Hold both sticks in upper right corner and continue\n");
+	press_enter();
+	usleep(500000);
+
+	while (get_value());
+
+	printf("Set the trim to 0 and leave the sticks in the neutral position\n");
+	press_enter();
+
+	//Loop until successfull
+	while (set_mid());
+
+	//write the values to global_data_rc_channels
+	write_data();
+
+	return 0;
+
+}
+
diff --git a/apps/systemcmds/calibration/servo_cal.c b/apps/systemcmds/calibration/servo_cal.c
new file mode 100644
index 000000000..96b3045a9
--- /dev/null
+++ b/apps/systemcmds/calibration/servo_cal.c
@@ -0,0 +1,264 @@
+/****************************************************************************
+ * servo_cal.c
+ *
+ *   Copyright (C) 2012 Nils Wenzler. All rights reserved.
+ *   Authors: Nils Wenzler <wenzlern@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 <stdio.h>
+#include <stdlib.h>
+#include <arch/board/drv_pwm_servo.h>
+#include <fcntl.h>
+#include "calibration.h"
+
+/****************************************************************************
+ * Defines
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+//Store the values here before writing them to global_data_rc_channels
+uint16_t max_values_servo[PWM_SERVO_MAX_CHANNELS];
+uint16_t min_values_servo[PWM_SERVO_MAX_CHANNELS];
+uint16_t mid_values_servo[PWM_SERVO_MAX_CHANNELS];
+
+// Servo loop thread
+
+pthread_t servo_calib_thread;
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/**This sets the middle values
+ */
+uint8_t set_mid_s(void)
+{
+	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
+		uint8_t i;
+
+		for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+			if (i == global_data_rc_channels->function[ROLL] ||
+			    i == global_data_rc_channels->function[YAW] ||
+			    i == global_data_rc_channels->function[PITCH]) {
+
+				mid_values_servo[i] = global_data_rc_channels->chan[i].raw;
+
+			} else {
+				mid_values_servo[i] = (max_values_servo[i] + min_values_servo[i]) / 2;
+			}
+
+		}
+
+		global_data_unlock(&global_data_rc_channels->access_conf);
+		return 0;
+
+	} else
+		return -1;
+}
+
+/**This gets all current values and writes them to min or max
+ */
+uint8_t get_value_s(void)
+{
+	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
+		uint8_t i;
+
+		for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+			//see if the value is bigger or smaller than 1500 (roughly neutral)
+			//and write to the appropriate array
+			if (global_data_rc_channels->chan[i].raw != 0 &&
+			    global_data_rc_channels->chan[i].raw < 1500) {
+				min_values_servo[i] = global_data_rc_channels->chan[i].raw;
+
+			} else if (global_data_rc_channels->chan[i].raw != 0 &&
+				   global_data_rc_channels->chan[i].raw > 1500) {
+				max_values_servo[i] = global_data_rc_channels->chan[i].raw;
+
+			} else {
+				max_values_servo[i] = 0;
+				min_values_servo[i] = 0;
+			}
+		}
+
+		global_data_unlock(&global_data_rc_channels->access_conf);
+		return 0;
+
+	} else
+		return -1;
+}
+
+
+void write_data_s(void)
+{
+	//  global_data_lock(&global_data_rc_channels->access_conf);
+	//  uint8_t i;
+	//  for(i=0; i < NR_CHANNELS; i++){
+	//    //Write the data to global_data_rc_channels (if not 0)
+	//    if(mid_values_servo[i]!=0 && min_values_servo[i]!=0 && max_values_servo[i]!=0){
+	//      global_data_rc_channels->chan[i].scaling_factor =
+	//        10000/((max_values_servo[i] - min_values_servo[i])/2);
+	//
+	//      global_data_rc_channels->chan[i].mid = mid_values_servo[i];
+	//    }
+	//
+	//    printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
+	//        i,
+	//        global_data_rc_channels->function_name[global_data_rc_channels->function[i]],
+	//        min_values_servo[i], max_values_servo[i],
+	//        global_data_rc_channels->chan[i].scaling_factor,
+	//        global_data_rc_channels->chan[i].mid);
+	//  }
+	//  global_data_unlock(&global_data_rc_channels->access_conf);
+
+	//Write to the Parameter storage
+
+
+
+	global_data_lock(&global_data_parameter_storage->access_conf);
+
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MIN] = min_values_servo[0];
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MIN] = min_values_servo[1];
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MIN] = min_values_servo[2];
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MIN] = min_values_servo[3];
+
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MAX] = max_values_servo[0];
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MAX] = max_values_servo[1];
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MAX] = max_values_servo[2];
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MAX] = max_values_servo[3];
+
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM] = mid_values_servo[0];
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM] = mid_values_servo[1];
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM] = mid_values_servo[2];
+	global_data_parameter_storage->pm.param_values[PARAM_SERVO4_TRIM] = mid_values_servo[3];
+
+	global_data_unlock(&global_data_parameter_storage->access_conf);
+
+	usleep(3e6);
+	uint8_t i;
+
+	for (i = 0; i < NR_CHANNELS; i++) {
+		printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
+		       i,
+		       min_values_servo[i], max_values_servo[i],
+		       global_data_rc_channels->chan[i].scaling_factor,
+		       global_data_rc_channels->chan[i].mid);
+	}
+
+}
+
+static void *servo_loop(void *arg)
+{
+
+	// Set thread name
+	prctl(1, "calibration servo", getpid());
+
+	// initialize servos
+	int fd;
+	servo_position_t data[PWM_SERVO_MAX_CHANNELS];
+
+	fd = open("/dev/pwm_servo", O_RDWR);
+
+	if (fd < 0) {
+		printf("failed opening /dev/pwm_servo\n");
+	}
+
+	ioctl(fd, PWM_SERVO_ARM, 0);
+
+	while (1) {
+		int i;
+
+		for (i = 0; i < 4; i++) {
+			data[i] = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].raw;
+		}
+
+		int result = write(fd, &data, sizeof(data));
+
+		if (result != sizeof(data)) {
+			printf("failed bulk-reading channel values\n");
+		}
+
+		// 5Hz loop
+		usleep(200000);
+	}
+
+	return NULL;
+}
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+int servo_cal(int argc, char *argv[])
+{
+	//	pthread_attr_t servo_loop_attr;
+	//	pthread_attr_init(&servo_loop_attr);
+	//	pthread_attr_setstacksize(&servo_loop_attr, 1024);
+	pthread_create(&servo_calib_thread, NULL, servo_loop, NULL);
+	pthread_join(servo_calib_thread, NULL);
+
+	printf("The servo calibration routine assumes you already did the channel\n");
+	printf("assignment with 'calibration channels'\n");
+	printf("This routine chooses the minimum, maximum and middle\n");
+	printf("value for each channel separately. The ROLL, PITCH and YAW function\n");
+	printf("get their middle value from the RC direct, for the rest it is\n");
+	printf("calculated out of the min and max values.\n");
+	press_enter();
+
+	printf("Hold both sticks in lower left corner and continue\n ");
+	press_enter();
+	usleep(500000);
+
+	while (get_value_s());
+
+	printf("Hold both sticks in upper right corner and continue\n");
+	press_enter();
+	usleep(500000);
+
+	while (get_value_s());
+
+	printf("Set the trim to 0 and leave the sticks in the neutral position\n");
+	press_enter();
+
+	//Loop until successfull
+	while (set_mid_s());
+
+	//write the values to global_data_rc_channels
+	write_data_s();
+
+	return 0;
+
+}
+
diff --git a/apps/systemcmds/perf/.context b/apps/systemcmds/perf/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/systemcmds/perf/.context
diff --git a/apps/systemcmds/perf/Makefile b/apps/systemcmds/perf/Makefile
new file mode 100644
index 000000000..0134c9948
--- /dev/null
+++ b/apps/systemcmds/perf/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# perf_counter reporting tool
+#
+
+APPNAME		 = perf
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/perf/perf.c b/apps/systemcmds/perf/perf.c
new file mode 100644
index 000000000..72406e9c7
--- /dev/null
+++ b/apps/systemcmds/perf/perf.c
@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 Lorenz Meier. 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 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 <unistd.h>
+#include <stdio.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[])
+{
+	perf_print_all();
+	fflush(stdout);
+	return 0;
+}
+
+
diff --git a/apps/systemcmds/reboot/.context b/apps/systemcmds/reboot/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/systemcmds/reboot/.context
diff --git a/apps/systemcmds/reboot/Makefile b/apps/systemcmds/reboot/Makefile
new file mode 100644
index 000000000..9609a24fd
--- /dev/null
+++ b/apps/systemcmds/reboot/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Reboot command.
+#
+
+APPNAME		 = reboot
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/reboot/reboot.c b/apps/systemcmds/reboot/reboot.c
new file mode 100644
index 000000000..2a4cb18d0
--- /dev/null
+++ b/apps/systemcmds/reboot/reboot.c
@@ -0,0 +1,72 @@
+/****************************************************************************
+ * apps/reboot.c
+ *
+ *   Copyright (C) 2012 Lorenz Meier. 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 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 <unistd.h>
+#include <stdio.h>
+
+#include "systemlib/systemlib.h"
+
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT int reboot_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * user_start
+ ****************************************************************************/
+
+int reboot_main(int argc, char *argv[])
+{
+	reboot();
+	return 0;
+}
+
+
diff --git a/apps/systemcmds/top/.context b/apps/systemcmds/top/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/systemcmds/top/.context
diff --git a/apps/systemcmds/top/Makefile b/apps/systemcmds/top/Makefile
new file mode 100644
index 000000000..c45775f4b
--- /dev/null
+++ b/apps/systemcmds/top/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# realtime system performance display
+#
+
+APPNAME		 = top
+PRIORITY	 = SCHED_PRIORITY_DEFAULT - 10
+STACKSIZE	 = 3000
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c
new file mode 100644
index 000000000..af1bb23d9
--- /dev/null
+++ b/apps/systemcmds/top/top.c
@@ -0,0 +1,245 @@
+/****************************************************************************
+ * apps/reboot.c
+ *
+ *   Copyright (C) 2012 Lorenz Meier. 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 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 <stdio.h>
+#include <fcntl.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <string.h>
+#include <poll.h>
+
+#include <arch/board/up_cpuload.h>
+#include <arch/board/up_hrt.h>
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT int top_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * user_start
+ ****************************************************************************/
+
+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 \tMIN STACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
+#else
+					printf("\033[KPID\tCOMMAND%s    CPU TOTAL \t%%CPU CURR \tMIN STACK 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 = "";
+					}
+
+					printf("\033[K % 2d\t%s%s\t% 8lld ms%s  \t % 2d.%03d \t % 6d B", (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), (uint32_t)system_load.tasks[i].tcb->adj_stack_ptr - (uint32_t)system_load.tasks[i].tcb->xcp.regs[REG_R13]);
+					/* 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;
+}