Merge working changes into export-build branch.

57 files changed, 2 insertions, 11824 deletions

diff --git a/apps/ardrone_interface/Makefile b/apps/ardrone_interface/Makefile
deleted file mode 100644
index fea96082f..000000000
--- a/apps/ardrone_interface/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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 ardrone interface
-#
-
-APPNAME		 = ardrone_interface
-PRIORITY	 = SCHED_PRIORITY_MAX - 15
-STACKSIZE	 = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
deleted file mode 100644
index aeaf830de..000000000
--- a/apps/ardrone_interface/ardrone_interface.c
+++ /dev/null
@@ -1,398 +0,0 @@
-/****************************************************************************
- *
- *   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 ardrone_interface.c
- * Implementation of AR.Drone 1.0 / 2.0 motor control interface.
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <math.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <termios.h>
-#include <time.h>
-#include <systemlib/err.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-
-#include <systemlib/systemlib.h>
-
-#include "ardrone_motor_control.h"
-
-__EXPORT int ardrone_interface_main(int argc, char *argv[]);
-
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int ardrone_interface_task;		/**< Handle of deamon task / thread */
-static int ardrone_write;			/**< UART to write AR.Drone commands to */
-
-/**
- * Mainloop of ardrone_interface.
- */
-int ardrone_interface_thread_main(int argc, char *argv[]);
-
-/**
- * Open the UART connected to the motor controllers
- */
-static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- * 
- * The actual stack size should be set in the call
- * to task_create().
- */
-int ardrone_interface_main(int argc, char *argv[])
-{
-		if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("ardrone_interface already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		ardrone_interface_task = task_spawn("ardrone_interface",
-						    SCHED_DEFAULT,
-						    SCHED_PRIORITY_MAX - 15,
-						    2048,
-						    ardrone_interface_thread_main,
-						    (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			printf("\tardrone_interface is running\n");
-		} else {
-			printf("\tardrone_interface not started\n");
-		}
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original)
-{
-	/* baud rate */
-	int speed = B115200;
-	int uart;
-
-	/* open uart */
-	uart = open(uart_name, O_RDWR | O_NOCTTY);
-
-	/* Try to set baud rate */
-	struct termios uart_config;
-	int termios_state;
-
-	/* Back up the original uart configuration to restore it after exit */
-	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
-		close(uart);
-		return -1;
-	}
-
-	/* Fill the struct for the new configuration */
-	tcgetattr(uart, &uart_config);
-
-	/* Clear ONLCR flag (which appends a CR for every LF) */
-	uart_config.c_oflag &= ~ONLCR;
-
-	/* Set baud rate */
-	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
-		close(uart);
-		return -1;
-	}
-
-
-	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-		close(uart);
-		return -1;
-	}
-
-	return uart;
-}
-
-int ardrone_interface_thread_main(int argc, char *argv[])
-{
-	thread_running = true;
-
-	char *device = "/dev/ttyS1";
-
-	/* welcome user */
-	printf("[ardrone_interface] Control started, taking over motors\n");
-
-	/* File descriptors */
-	int gpios;
-
-	char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
-
-	bool motor_test_mode = false;
-	int test_motor = -1;
-
-	/* read commandline arguments */
-	for (int i = 0; i < argc && argv[i]; i++) {
-		if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
-			motor_test_mode = true;
-		}
-
-		if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
-			if (i+1 < argc) {
-				int motor = atoi(argv[i+1]);
-				if (motor > 0 && motor < 5) {
-					test_motor = motor;
-				} else {
-					thread_running = false;
-					errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
-				}
-			} else {
-				thread_running = false;
-				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
-			}
-		}
-		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
-			if (argc > i + 1) {
-				device = argv[i + 1];
-
-			} else {
-				thread_running = false;
-				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
-			}
-		}
-	}
-
-	struct termios uart_config_original;
-
-	if (motor_test_mode) {
-		printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
-	}
-
-	/* Led animation */
-	int counter = 0;
-	int led_counter = 0;
-
-	/* declare and safely initialize all structs */
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
-	struct actuator_controls_s actuator_controls;
-	memset(&actuator_controls, 0, sizeof(actuator_controls));
-	struct actuator_armed_s armed;
-	armed.armed = false;
-
-	/* subscribe to attitude, motor setpoints and system state */
-	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-
-	printf("[ardrone_interface] Motors initialized - ready.\n");
-	fflush(stdout);
-
-	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	ardrone_write = ardrone_open_uart(device, &uart_config_original);
-
-	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
-	gpios = ar_multiplexing_init();
-
-	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
-		thread_running = false;
-		exit(ERROR);
-	}
-
-	/* initialize motors */
-	if (OK != ar_init_motors(ardrone_write, gpios)) {
-		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
-		thread_running = false;
-		exit(ERROR);
-	}
-
-	ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
-
-
-	// XXX Re-done initialization to make sure it is accepted by the motors
-	// XXX should be removed after more testing, but no harm
-
-	/* close uarts */
-	close(ardrone_write);
-
-	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	ardrone_write = ardrone_open_uart(device, &uart_config_original);
-
-	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
-	gpios = ar_multiplexing_init();
-
-	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
-		thread_running = false;
-		exit(ERROR);
-	}
-
-	/* initialize motors */
-	if (OK != ar_init_motors(ardrone_write, gpios)) {
-		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
-		thread_running = false;
-		exit(ERROR);
-	}
-
-	while (!thread_should_exit) {
-
-		if (motor_test_mode) {
-			/* set motors to idle speed */
-			if (test_motor > 0 && test_motor < 5) {
-				int motors[4] = {0, 0, 0, 0};
-				motors[test_motor - 1] = 10;
-				ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
-			} else {
-				ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
-			}
-
-		} else {
-			/* MAIN OPERATION MODE */
-
-			/* get a local copy of the vehicle state */
-			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-			/* get a local copy of the actuator controls */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
-			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-			
-			/* for now only spin if armed and immediately shut down
-			 * if in failsafe
-			 */
-			if (armed.armed && !armed.lockdown) {
-				ardrone_mixing_and_output(ardrone_write, &actuator_controls);
-
-			} else {
-				/* Silently lock down motor speeds to zero */
-				ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
-			}
-		}
-
-		if (counter % 24 == 0) {
-			if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
-
-			if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
-
-			if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
-
-			if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
-
-			if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
-
-			if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
-
-			if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
-
-			if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
-
-			if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
-
-			if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
-
-			if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
-
-			if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
-
-			led_counter++;
-
-			if (led_counter == 12) led_counter = 0;
-		}
-
-		/* run at approximately 200 Hz */
-		usleep(4500);
-
-		counter++;
-	}
-
-	/* restore old UART config */
-	int termios_state;
-
-	if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
-	}
-
-	printf("[ardrone_interface] Restored original UART config, exiting..\n");
-
-	/* close uarts */
-	close(ardrone_write);
-	ar_multiplexing_deinit(gpios);
-
-	fflush(stdout);
-
-	thread_running = false;
-
-	return OK;
-}
-
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
deleted file mode 100644
index f15c74a22..000000000
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ /dev/null
@@ -1,492 +0,0 @@
-/****************************************************************************
- *
- *   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 ardrone_motor_control.c
- * Implementation of AR.Drone 1.0 / 2.0 motor control interface
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <drivers/drv_gpio.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls_effective.h>
-#include <systemlib/err.h>
-
-#include "ardrone_motor_control.h"
-
-static unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2;
-static unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 };
-
-typedef union {
-	uint16_t motor_value;
-	uint8_t bytes[2];
-} motor_union_t;
-
-#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */
-
-/**
- * @brief Generate the 8-byte motor set packet
- *
- * @return the number of bytes (8)
- */
-void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4)
-{
-	motor_buf[0] = 0x20;
-	motor_buf[1] = 0x00;
-	motor_buf[2] = 0x00;
-	motor_buf[3] = 0x00;
-	motor_buf[4] = 0x00;
-	/*
-	 * {0x20, 0x00, 0x00, 0x00, 0x00};
-	 * 0x20 is start sign / motor command
-	 */
-	motor_union_t curr_motor;
-	uint16_t nineBitMask = 0x1FF;
-
-	/* Set motor 1 */
-	curr_motor.motor_value = (motor1 & nineBitMask) << 4;
-	motor_buf[0] |= curr_motor.bytes[1];
-	motor_buf[1] |= curr_motor.bytes[0];
-
-	/* Set motor 2 */
-	curr_motor.motor_value = (motor2 & nineBitMask) << 3;
-	motor_buf[1] |= curr_motor.bytes[1];
-	motor_buf[2] |= curr_motor.bytes[0];
-
-	/* Set motor 3 */
-	curr_motor.motor_value = (motor3 & nineBitMask) << 2;
-	motor_buf[2] |= curr_motor.bytes[1];
-	motor_buf[3] |= curr_motor.bytes[0];
-
-	/* Set motor 4 */
-	curr_motor.motor_value = (motor4 & nineBitMask) << 1;
-	motor_buf[3] |= curr_motor.bytes[1];
-	motor_buf[4] |= curr_motor.bytes[0];
-}
-
-void ar_enable_broadcast(int fd)
-{
-	ar_select_motor(fd, 0);
-}
-
-int ar_multiplexing_init()
-{
-	int		fd;
-	
-	fd = open(GPIO_DEVICE_PATH, 0);
-
-	if (fd < 0) {
-		warn("GPIO: open fail");
-		return fd;
-	}
-
-	/* deactivate all outputs */
-	if (ioctl(fd, GPIO_SET, motor_gpios)) {
-		warn("GPIO: clearing pins fail");
-		close(fd);
-		return -1;
-	}
-
-	/* configure all motor select GPIOs as outputs */
-	if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
-		warn("GPIO: output set fail");
-		close(fd);
-		return -1;
-	}
-
-	return fd;
-}
-
-int ar_multiplexing_deinit(int fd)
-{
-	if (fd < 0) {
-		printf("GPIO: no valid descriptor\n");
-		return fd;
-	}
-
-	int ret = 0;
-
-	/* deselect motor 1-4 */
-	ret += ioctl(fd, GPIO_SET, motor_gpios);
-
-	if (ret != 0) {
-		printf("GPIO: clear failed %d times\n", ret);
-	}
-
-	if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) {
-		printf("GPIO: input set fail\n");
-		return -1;
-	}
-
-	close(fd);
-
-	return ret;
-}
-
-int ar_select_motor(int fd, uint8_t motor)
-{
-	int ret = 0;
-	/*
-	 *  Four GPIOS:
-	 *		GPIO_EXT1
-	 *		GPIO_EXT2
-	 *		GPIO_UART2_CTS
-	 *		GPIO_UART2_RTS
-	 */
-
-	/* select motor 0 to enable broadcast */
-	if (motor == 0) {
-		/* select motor 1-4 */
-		ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
-
-	} else {
-		/* select reqested motor */	
-		ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
-	}
-
-	return ret;
-}
-
-int ar_deselect_motor(int fd, uint8_t motor)
-{
-	int ret = 0;
-	/*
-	 *  Four GPIOS:
-	 *		GPIO_EXT1
-	 *		GPIO_EXT2
-	 *		GPIO_UART2_CTS
-	 *		GPIO_UART2_RTS
-	 */
-
-	if (motor == 0) {
-		/* deselect motor 1-4 */
-		ret += ioctl(fd, GPIO_SET, motor_gpios);
-
-	} else {
-		/* deselect reqested motor */	
-		ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]);
-	}
-
-	return ret;
-}
-
-int ar_init_motors(int ardrone_uart, int gpios)
-{
-	/* Write ARDrone commands on UART2 */
-	uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
-	uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
-
-	/* deselect all motors */
-	ar_deselect_motor(gpios, 0);
-
-	/* initialize all motors
-	 * - select one motor at a time
-	 * - configure motor
-	 */
-	int i;
-	int errcounter = 0;
-
-
-	/* initial setup run */
-	for (i = 1; i < 5; ++i) {
-		/* Initialize motors 1-4 */
-		errcounter += ar_select_motor(gpios, i);
-		usleep(200);
-
-		/*
-		 * write 0xE0 - request status
-		 * receive one status byte
-		 */
-		write(ardrone_uart, &(initbuf[0]), 1);
-		fsync(ardrone_uart);
-		usleep(UART_TRANSFER_TIME_BYTE_US*1);
-
-		/*
-		 * write 0x91 - request checksum
-		 * receive 120 status bytes
-		 */
-		write(ardrone_uart, &(initbuf[1]), 1);
-		fsync(ardrone_uart);
-		usleep(UART_TRANSFER_TIME_BYTE_US*120);
-
-		/*
-		 * write 0xA1 - set status OK
-		 * receive one status byte - should be A0
-		 * to confirm status is OK
-		 */
-		write(ardrone_uart, &(initbuf[2]), 1);
-		fsync(ardrone_uart);
-		usleep(UART_TRANSFER_TIME_BYTE_US*1);
-
-		/*
-		 * set as motor i, where i = 1..4
-		 * receive nothing
-		 */
-		initbuf[3] = (uint8_t)i;
-		write(ardrone_uart, &(initbuf[3]), 1);
-		fsync(ardrone_uart);
-
-		/*
-		 * write 0x40 - check version
-		 * receive 11 bytes encoding the version
-		 */
-		write(ardrone_uart, &(initbuf[4]), 1);
-		fsync(ardrone_uart);
-		usleep(UART_TRANSFER_TIME_BYTE_US*11);
-
-		ar_deselect_motor(gpios, i);
-		/* sleep 200 ms */
-		usleep(200000);
-	}
-
-	/* start the multicast part */
-	errcounter += ar_select_motor(gpios, 0);
-	usleep(200);
-
-	/*
-	 * first round
-	 * write six times A0 - enable broadcast
-	 * receive nothing
-	 */
-	write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
-	fsync(ardrone_uart);
-	usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
-
-	/*
-	 * second round
-	 * write six times A0 - enable broadcast
-	 * receive nothing
-	 */
-	write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
-	fsync(ardrone_uart);
-	usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
-
-	/* set motors to zero speed (fsync is part of the write command */
-	ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
-
-	if (errcounter != 0) {
-		fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
-		fflush(stdout);
-	}
-	return errcounter;
-}
-
-/**
- * Sets the leds on the motor controllers, 1 turns led on, 0 off.
- */
-void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
-{
-	/*
-	 * 2 bytes are sent. The first 3 bits describe the command: 011 means led control
-	 * the following 4 bits are the red leds for motor 4, 3, 2, 1
-	 * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1
-	 * the last bit is unknown.
-	 *
-	 * The packet is therefore:
-	 * 011 rrrr 0000 gggg 0
-	 */
-	uint8_t leds[2];
-	leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1);
-	leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1);
-	write(ardrone_uart, leds, 2);
-}
-
-int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
-	const unsigned int min_motor_interval = 4900;
-	static uint64_t last_motor_time = 0;
-
-	static struct actuator_outputs_s outputs;
-	outputs.timestamp = hrt_absolute_time();
-	outputs.output[0] = motor1;
-	outputs.output[1] = motor2;
-	outputs.output[2] = motor3;
-	outputs.output[3] = motor4;
-	static orb_advert_t pub = 0;
-	if (pub == 0) {
-		pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
-	}
-
-	if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
-		uint8_t buf[5] = {0};
-		ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
-		int ret;
-		ret = write(ardrone_fd, buf, sizeof(buf));
-		fsync(ardrone_fd);
-
-		/* publish just written values */
-		orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
-
-		if (ret == sizeof(buf)) {
-			return OK;
-		} else {
-			return ret;
-		}
-	} else {
-		return -ERROR;
-	}
-}
-
-void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) {
-
-	float roll_control = actuators->control[0];
-	float pitch_control = actuators->control[1];
-	float yaw_control = actuators->control[2];
-	float motor_thrust = actuators->control[3];
-
-	//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
-
-	const float min_thrust = 0.02f;			/**< 2% minimum thrust */
-	const float max_thrust = 1.0f;			/**< 100% max thrust */
-	const float scaling = 500.0f;			/**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
-	const float min_gas = min_thrust * scaling;	/**< value range sent to motors, minimum */
-	const float max_gas = max_thrust * scaling;	/**< value range sent to motors, maximum */
-
-	/* initialize all fields to zero */
-	uint16_t motor_pwm[4] = {0};
-	float motor_calc[4] = {0};
-
-	float output_band = 0.0f;
-	float band_factor = 0.75f;
-	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
-	float yaw_factor = 1.0f;
-
-	static bool initialized = false;
-	/* publish effective outputs */
-	static struct actuator_controls_effective_s actuator_controls_effective;
-	static orb_advert_t actuator_controls_effective_pub;
-
-	if (motor_thrust <= min_thrust) {
-		motor_thrust = min_thrust;
-		output_band = 0.0f;
-	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
-		output_band = band_factor * (motor_thrust - min_thrust);
-	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
-		output_band = band_factor * startpoint_full_control;
-	} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
-		output_band = band_factor * (max_thrust - motor_thrust);
-	}
-
-	//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
-
-	// FRONT (MOTOR 1)
-	motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
-
-	// RIGHT (MOTOR 2)
-	motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
-
-	// BACK (MOTOR 3)
-	motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
-
-	// LEFT (MOTOR 4)
-	motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
-
-	// if we are not in the output band
-	if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
-	      && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
-	      && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
-	      && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
-
-		yaw_factor = 0.5f;
-		yaw_control *= yaw_factor;
-		// FRONT (MOTOR 1)
-		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
-
-		// RIGHT (MOTOR 2)
-		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
-
-		// BACK (MOTOR 3)
-		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
-
-		// LEFT (MOTOR 4)
-		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
-	}
-
-	for (int i = 0; i < 4; i++) {
-		//check for limits
-		if (motor_calc[i] < motor_thrust - output_band) {
-			motor_calc[i] = motor_thrust - output_band;
-		}
-
-		if (motor_calc[i] > motor_thrust + output_band) {
-			motor_calc[i] = motor_thrust + output_band;
-		}
-	}
-
-	/* publish effective outputs */
-	actuator_controls_effective.control_effective[0] = roll_control;
-	actuator_controls_effective.control_effective[1] = pitch_control;
-	/* yaw output after limiting */
-	actuator_controls_effective.control_effective[2] = yaw_control;
-	/* possible motor thrust limiting */
-	actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
-
-	if (!initialized) {
-		/* advertise and publish */
-		actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
-		initialized = true;
-	} else {
-		/* already initialized, just publishing */
-		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
-	}
-
-	/* set the motor values */
-
-	/* scale up from 0..1 to 10..512) */
-	motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
-	motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
-	motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
-	motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
-
-	/* Keep motors spinning while armed and prevent overflows */
-
-	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
-	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
-	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
-	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
-
-	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
-	motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
-	motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
-	motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
-
-	/* send motors via UART */
-	ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
-}
diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h
deleted file mode 100644
index 78b603b63..000000000
--- a/apps/ardrone_interface/ardrone_motor_control.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/****************************************************************************
- *
- *   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 ardrone_motor_control.h
- * Definition of AR.Drone 1.0 / 2.0 motor control interface
- */
-
-#include <uORB/uORB.h>
-#include <uORB/topics/actuator_controls.h>
-
-/**
- * Generate the 5-byte motor set packet.
- *
- * @return the number of bytes (5)
- */
-void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
-
-/**
- * Select a motor in the multiplexing.
- *
- * @param fd GPIO file descriptor
- * @param motor Motor number, from 1 to 4, 0 selects all
- */
-int ar_select_motor(int fd, uint8_t motor);
-
-/**
- * Deselect a motor in the multiplexing.
- *
- * @param fd GPIO file descriptor
- * @param motor Motor number, from 1 to 4, 0 deselects all
- */
-int ar_deselect_motor(int fd, uint8_t motor);
-
-void ar_enable_broadcast(int fd);
-
-int ar_multiplexing_init(void);
-int ar_multiplexing_deinit(int fd);
-
-/**
- * Write four motor commands to an already initialized port.
- *
- * Writing 0 stops a motor, values from 1-512 encode the full thrust range.
- * on some motor controller firmware revisions a minimum value of 10 is
- * required to spin the motors.
- */
-int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
-
-/**
- * Initialize the motors.
- */
-int ar_init_motors(int ardrone_uart, int gpio);
-
-/**
- * Set LED pattern.
- */
-void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
-
-/**
- * Mix motors and output actuators
- */
-void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators);
diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile
deleted file mode 100755
index 46a54c660..000000000
--- a/apps/attitude_estimator_ekf/Makefile
+++ /dev/null
@@ -1,57 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-APPNAME		 = attitude_estimator_ekf
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-CXXSRCS		 = attitude_estimator_ekf_main.cpp
-
-CSRCS		 = attitude_estimator_ekf_params.c \
-		   codegen/eye.c \
-		   codegen/attitudeKalmanfilter.c \
-		   codegen/mrdivide.c \
-		   codegen/rdivide.c \
-		   codegen/attitudeKalmanfilter_initialize.c \
-		   codegen/attitudeKalmanfilter_terminate.c \
-		   codegen/rt_nonfinite.c \
-		   codegen/rtGetInf.c \
-		   codegen/rtGetNaN.c \
-		   codegen/norm.c \
-		   codegen/cross.c 
-
-
-# XXX this is *horribly* broken
-INCLUDES	+= $(TOPDIR)/../mavlink/include/mavlink
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
deleted file mode 100755
index 1a50dde0f..000000000
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ /dev/null
@@ -1,472 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           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 attitude_estimator_ekf_main.c
- *
- * Extended Kalman Filter for Attitude Estimation.
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <poll.h>
-#include <fcntl.h>
-#include <v1.0/common/mavlink.h>
-#include <float.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <limits.h>
-#include <math.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/debug_key_value.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/parameter_update.h>
-#include <drivers/drv_hrt.h>
-
-#include <systemlib/systemlib.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-#include "codegen/attitudeKalmanfilter_initialize.h"
-#include "codegen/attitudeKalmanfilter.h"
-#include "attitude_estimator_ekf_params.h"
-#ifdef __cplusplus
-}
-#endif
-
-extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int attitude_estimator_ekf_task;				/**< Handle of deamon task / thread */
-
-/**
- * Mainloop of attitude_estimator_ekf.
- */
-int attitude_estimator_ekf_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
-}
-
-/**
- * The attitude_estimator_ekf app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int attitude_estimator_ekf_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("attitude_estimator_ekf already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
-					      SCHED_DEFAULT,
-					      SCHED_PRIORITY_MAX - 5,
-					      12400,
-					      attitude_estimator_ekf_thread_main,
-					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			printf("\tattitude_estimator_ekf app is running\n");
-
-		} else {
-			printf("\tattitude_estimator_ekf app not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-/*
- * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
- */
-
-/*
- * EKF Attitude Estimator main function.
- *
- * Estimates the attitude recursively once started.
- *
- * @param argc number of commandline arguments (plus command name)
- * @param argv strings containing the arguments
- */
-int attitude_estimator_ekf_thread_main(int argc, char *argv[])
-{
-
-const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
-
-	float dt = 0.005f;
-/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
-	float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
-	float x_aposteriori_k[12];		/**< states */
-	float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				     0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				     0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				     0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,
-				     0,   0,   0,   0,  100.f,  0,   0,   0,   0,   0,   0,   0,
-				     0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
-				     0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
-				     0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,
-				     0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
-				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
-				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
-				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
-				    }; /**< init: diagonal matrix with big values */
-
-	float x_aposteriori[12];
-	float P_aposteriori[144];
-
-	/* output euler angles */
-	float euler[3] = {0.0f, 0.0f, 0.0f};
-
-	float Rot_matrix[9] = {1.f,  0,  0,
-			      0,  1.f,  0,
-			      0,  0,  1.f
-			     };		/**< init: identity matrix */
-
-	// print text
-	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
-	fflush(stdout);
-
-	int overloadcounter = 19;
-
-	/* Initialize filter */
-	attitudeKalmanfilter_initialize();
-
-	/* store start time to guard against too slow update rates */
-	uint64_t last_run = hrt_absolute_time();
-
-	struct sensor_combined_s raw;
-	memset(&raw, 0, sizeof(raw));
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
-
-	uint64_t last_data = 0;
-	uint64_t last_measurement = 0;
-
-	/* subscribe to raw data */
-	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate-limit raw data updates to 200Hz */
-	orb_set_interval(sub_raw, 4);
-
-	/* subscribe to param changes */
-	int sub_params = orb_subscribe(ORB_ID(parameter_update));
-
-	/* subscribe to system state*/
-	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
-
-	/* advertise attitude */
-	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
-
-
-	int loopcounter = 0;
-	int printcounter = 0;
-
-	thread_running = true;
-
-	/* advertise debug value */
-	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
-	// orb_advert_t pub_dbg = -1;
-
-	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
-	// XXX write this out to perf regs
-
-	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
-	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
-
-	struct attitude_estimator_ekf_params ekf_params;
-
-	struct attitude_estimator_ekf_param_handles ekf_param_handles;
-
-	/* initialize parameter handles */
-	parameters_init(&ekf_param_handles);
-
-	uint64_t start_time = hrt_absolute_time();
-	bool initialized = false;
-
-	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
-	unsigned offset_count = 0;
-
-	/* register the perf counter */
-	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
-
-	/* Main loop*/
-	while (!thread_should_exit) {
-
-		struct pollfd fds[2];
-		fds[0].fd = sub_raw;
-		fds[0].events = POLLIN;
-		fds[1].fd = sub_params;
-		fds[1].events = POLLIN;
-		int ret = poll(fds, 2, 1000);
-
-		if (ret < 0) {
-			/* XXX this is seriously bad - should be an emergency */
-		} else if (ret == 0) {
-			/* check if we're in HIL - not getting sensor data is fine then */
-			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
-
-			if (!state.flag_hil_enabled) {
-				fprintf(stderr,
-					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
-			}
-
-		} else {
-
-			/* only update parameters if they changed */
-			if (fds[1].revents & POLLIN) {
-				/* read from param to clear updated flag */
-				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), sub_params, &update);
-
-				/* update parameters */
-				parameters_update(&ekf_param_handles, &ekf_params);
-			}
-
-			/* only run filter if sensor values changed */
-			if (fds[0].revents & POLLIN) {
-
-				/* get latest measurements */
-				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
-
-				if (!initialized) {
-
-					gyro_offsets[0] += raw.gyro_rad_s[0];
-					gyro_offsets[1] += raw.gyro_rad_s[1];
-					gyro_offsets[2] += raw.gyro_rad_s[2];
-					offset_count++;
-
-					if (hrt_absolute_time() - start_time > 3000000LL) {
-						initialized = true;
-						gyro_offsets[0] /= offset_count;
-						gyro_offsets[1] /= offset_count;
-						gyro_offsets[2] /= offset_count;
-					}
-
-				} else {
-
-					perf_begin(ekf_loop_perf);
-
-					/* Calculate data time difference in seconds */
-					dt = (raw.timestamp - last_measurement) / 1000000.0f;
-					last_measurement = raw.timestamp;
-					uint8_t update_vect[3] = {0, 0, 0};
-
-					/* Fill in gyro measurements */
-					if (sensor_last_count[0] != raw.gyro_counter) {
-						update_vect[0] = 1;
-						sensor_last_count[0] = raw.gyro_counter;
-						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
-						sensor_last_timestamp[0] = raw.timestamp;
-					}
-
-					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
-					z_k[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
-					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
-
-					/* update accelerometer measurements */
-					if (sensor_last_count[1] != raw.accelerometer_counter) {
-						update_vect[1] = 1;
-						sensor_last_count[1] = raw.accelerometer_counter;
-						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.timestamp;
-					}
-
-					z_k[3] = raw.accelerometer_m_s2[0];
-					z_k[4] = raw.accelerometer_m_s2[1];
-					z_k[5] = raw.accelerometer_m_s2[2];
-
-					/* update magnetometer measurements */
-					if (sensor_last_count[2] != raw.magnetometer_counter) {
-						update_vect[2] = 1;
-						sensor_last_count[2] = raw.magnetometer_counter;
-						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.timestamp;
-					}
-
-					z_k[6] = raw.magnetometer_ga[0];
-					z_k[7] = raw.magnetometer_ga[1];
-					z_k[8] = raw.magnetometer_ga[2];
-
-					uint64_t now = hrt_absolute_time();
-					unsigned int time_elapsed = now - last_run;
-					last_run = now;
-
-					if (time_elapsed > loop_interval_alarm) {
-						//TODO: add warning, cpu overload here
-						// if (overloadcounter == 20) {
-						// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
-						// 	overloadcounter = 0;
-						// }
-
-						overloadcounter++;
-					}
-
-					static bool const_initialized = false;
-
-					/* initialize with good values once we have a reasonable dt estimate */
-					if (!const_initialized && dt < 0.05f && dt > 0.005f) {
-						dt = 0.005f;
-						parameters_update(&ekf_param_handles, &ekf_params);
-
-						x_aposteriori_k[0] = z_k[0];
-						x_aposteriori_k[1] = z_k[1];
-						x_aposteriori_k[2] = z_k[2];
-						x_aposteriori_k[3] = 0.0f;
-						x_aposteriori_k[4] = 0.0f;
-						x_aposteriori_k[5] = 0.0f;
-						x_aposteriori_k[6] = z_k[3];
-						x_aposteriori_k[7] = z_k[4];
-						x_aposteriori_k[8] = z_k[5];
-						x_aposteriori_k[9] = z_k[6];
-						x_aposteriori_k[10] = z_k[7];
-						x_aposteriori_k[11] = z_k[8];
-
-						const_initialized = true;
-					}
-
-					/* do not execute the filter if not initialized */
-					if (!const_initialized) {
-						continue;
-					}
-
-					uint64_t timing_start = hrt_absolute_time();
-
-					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
-							     euler, Rot_matrix, x_aposteriori, P_aposteriori);
-
-					/* swap values for next iteration, check for fatal inputs */
-					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
-						memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
-						memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
-
-					} else {
-						/* due to inputs or numerical failure the output is invalid, skip it */
-						continue;
-					}
-
-					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
-
-					last_data = raw.timestamp;
-
-					/* send out */
-					att.timestamp = raw.timestamp;
-
-					// XXX Apply the same transformation to the rotation matrix
-					att.roll = euler[0] - ekf_params.roll_off;
-					att.pitch = euler[1] - ekf_params.pitch_off;
-					att.yaw = euler[2] - ekf_params.yaw_off;
-
-					att.rollspeed = x_aposteriori[0];
-					att.pitchspeed = x_aposteriori[1];
-					att.yawspeed = x_aposteriori[2];
-					att.rollacc = x_aposteriori[3];
-					att.pitchacc = x_aposteriori[4];
-					att.yawacc = x_aposteriori[5];
-
-					//att.yawspeed =z_k[2] ;
-					/* copy offsets */
-					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
-
-					/* copy rotation matrix */
-					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
-					att.R_valid = true;
-
-					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
-						// Broadcast
-						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
-
-					} else {
-						warnx("NaN in roll/pitch/yaw estimate!");
-					}
-
-					perf_end(ekf_loop_perf);
-				}
-			}
-		}
-
-		loopcounter++;
-	}
-
-	thread_running = false;
-
-	return 0;
-}
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
deleted file mode 100755
index 7d3812abd..000000000
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ /dev/null
@@ -1,105 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           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 attitude_estimator_ekf_params.c
- *
- * Parameters for EKF filter
- */
-
-#include "attitude_estimator_ekf_params.h"
-
-/* Extended Kalman Filter covariances */
-
-/* gyro process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f);
-/* gyro offsets process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f);
-
-/* gyro measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f);
-/* accelerometer measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f);
-
-/* offsets in roll, pitch and yaw of sensor plane and body */
-PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
-
-int parameters_init(struct attitude_estimator_ekf_param_handles *h)
-{
-	/* PID parameters */
-	h->q0 	=	param_find("EKF_ATT_V2_Q0");
-	h->q1 	=	param_find("EKF_ATT_V2_Q1");
-	h->q2 	=	param_find("EKF_ATT_V2_Q2");
-	h->q3 	=	param_find("EKF_ATT_V2_Q3");
-	h->q4 	=	param_find("EKF_ATT_V2_Q4");
-
-	h->r0 	=	param_find("EKF_ATT_V2_R0");
-	h->r1 	=	param_find("EKF_ATT_V2_R1");
-	h->r2 	=	param_find("EKF_ATT_V2_R2");
-	h->r3 	=	param_find("EKF_ATT_V2_R3");
-
-	h->roll_off  =	param_find("ATT_ROLL_OFFS");
-	h->pitch_off =	param_find("ATT_PITCH_OFFS");
-	h->yaw_off   =	param_find("ATT_YAW_OFFS");
-
-	return OK;
-}
-
-int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
-{
-	param_get(h->q0, &(p->q[0]));
-	param_get(h->q1, &(p->q[1]));
-	param_get(h->q2, &(p->q[2]));
-	param_get(h->q3, &(p->q[3]));
-	param_get(h->q4, &(p->q[4]));
-
-	param_get(h->r0, &(p->r[0]));
-	param_get(h->r1, &(p->r[1]));
-	param_get(h->r2, &(p->r[2]));
-	param_get(h->r3, &(p->r[3]));
-
-	param_get(h->roll_off, &(p->roll_off));
-	param_get(h->pitch_off, &(p->pitch_off));
-	param_get(h->yaw_off, &(p->yaw_off));
-
-	return OK;
-}
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
deleted file mode 100755
index 09817d58e..000000000
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           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 attitude_estimator_ekf_params.h
- *
- * Parameters for EKF filter
- */
-
-#include <systemlib/param/param.h>
-
-struct attitude_estimator_ekf_params {
-	float r[9];
-	float q[12];
-	float roll_off;
-	float pitch_off;
-	float yaw_off;
-};
-
-struct attitude_estimator_ekf_param_handles {
-	param_t r0, r1, r2, r3;
-	param_t q0, q1, q2, q3, q4;
-	param_t roll_off, pitch_off, yaw_off;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct attitude_estimator_ekf_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p);
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
deleted file mode 100755
index 9e97ad11a..000000000
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ /dev/null
@@ -1,1148 +0,0 @@
-/*

- * attitudeKalmanfilter.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "rdivide.h"

-#include "norm.h"

-#include "cross.h"

-#include "eye.h"

-#include "mrdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);

-

-/* Function Definitions */

-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)

-{

-  real32_T y;

-  int32_T b_u0;

-  int32_T b_u1;

-  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

-    y = ((real32_T)rtNaN);

-  } else if (rtIsInfF(u0) && rtIsInfF(u1)) {

-    if (u0 > 0.0F) {

-      b_u0 = 1;

-    } else {

-      b_u0 = -1;

-    }

-

-    if (u1 > 0.0F) {

-      b_u1 = 1;

-    } else {

-      b_u1 = -1;

-    }

-

-    y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);

-  } else if (u1 == 0.0F) {

-    if (u0 > 0.0F) {

-      y = RT_PIF / 2.0F;

-    } else if (u0 < 0.0F) {

-      y = -(RT_PIF / 2.0F);

-    } else {

-      y = 0.0F;

-    }

-  } else {

-    y = (real32_T)atan2(u0, u1);

-  }

-

-  return y;

-}

-

-/*

- * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)

- */

-void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const

-  real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T

-  P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T

-  eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T

-  P_aposteriori[144])

-{

-  real32_T wak[3];

-  real32_T O[9];

-  real_T dv0[9];

-  real32_T a[9];

-  int32_T i;

-  real32_T b_a[9];

-  real32_T x_n_b[3];

-  real32_T b_x_aposteriori_k[3];

-  real32_T z_n_b[3];

-  real32_T c_a[3];

-  real32_T d_a[3];

-  int32_T i0;

-  real32_T x_apriori[12];

-  real_T dv1[144];

-  real32_T A_lin[144];

-  static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

-

-  real32_T b_A_lin[144];

-  real32_T b_q[144];

-  real32_T c_A_lin[144];

-  real32_T d_A_lin[144];

-  real32_T e_A_lin[144];

-  int32_T i1;

-  real32_T P_apriori[144];

-  real32_T b_P_apriori[108];

-  static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

-

-  real32_T K_k[108];

-  real32_T fv0[81];

-  static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

-

-  real32_T b_r[81];

-  real32_T fv1[81];

-  real32_T f0;

-  real32_T c_P_apriori[36];

-  static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

-

-  real32_T fv2[36];

-  static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

-

-  real32_T c_r[9];

-  real32_T b_K_k[36];

-  real32_T d_P_apriori[72];

-  static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    1, 0, 0, 0 };

-

-  real32_T c_K_k[72];

-  static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0 };

-

-  real32_T b_z[6];

-  static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 1 };

-

-  static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,

-    0, 0, 0, 1 };

-

-  real32_T fv3[6];

-  real32_T c_z[6];

-

-  /*  Extended Attitude Kalmanfilter */

-  /*  */

-  /*  state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */

-  /*  measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */

-  /*  knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */

-  /*  */

-  /*  [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */

-  /*  */

-  /*  Example.... */

-  /*  */

-  /*  $Author: Tobias Naegeli $    $Date: 2012 $    $Revision: 1 $ */

-  /* coder.varsize('udpIndVect', [9,1], [1,0]) */

-  /* udpIndVect=find(updVect); */

-  /* process and measurement noise covariance matrix */

-  /* Q = diag(q.^2*dt); */

-  /* observation matrix */

-  /* 'attitudeKalmanfilter:33' wx=  x_aposteriori_k(1); */

-  /* 'attitudeKalmanfilter:34' wy=  x_aposteriori_k(2); */

-  /* 'attitudeKalmanfilter:35' wz=  x_aposteriori_k(3); */

-  /* 'attitudeKalmanfilter:37' wax=  x_aposteriori_k(4); */

-  /* 'attitudeKalmanfilter:38' way=  x_aposteriori_k(5); */

-  /* 'attitudeKalmanfilter:39' waz=  x_aposteriori_k(6); */

-  /* 'attitudeKalmanfilter:41' zex=  x_aposteriori_k(7); */

-  /* 'attitudeKalmanfilter:42' zey=  x_aposteriori_k(8); */

-  /* 'attitudeKalmanfilter:43' zez=  x_aposteriori_k(9); */

-  /* 'attitudeKalmanfilter:45' mux=  x_aposteriori_k(10); */

-  /* 'attitudeKalmanfilter:46' muy=  x_aposteriori_k(11); */

-  /* 'attitudeKalmanfilter:47' muz=  x_aposteriori_k(12); */

-  /* % prediction section */

-  /* body angular accelerations */

-  /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */

-  wak[0] = x_aposteriori_k[3];

-  wak[1] = x_aposteriori_k[4];

-  wak[2] = x_aposteriori_k[5];

-

-  /* body angular rates */

-  /* 'attitudeKalmanfilter:54' wk =[wx;  wy; wz] + dt*wak; */

-  /* derivative of the prediction rotation matrix */

-  /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */

-  O[0] = 0.0F;

-  O[1] = -x_aposteriori_k[2];

-  O[2] = x_aposteriori_k[1];

-  O[3] = x_aposteriori_k[2];

-  O[4] = 0.0F;

-  O[5] = -x_aposteriori_k[0];

-  O[6] = -x_aposteriori_k[1];

-  O[7] = x_aposteriori_k[0];

-  O[8] = 0.0F;

-

-  /* prediction of the earth z vector */

-  /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */

-  eye(dv0);

-  for (i = 0; i < 9; i++) {

-    a[i] = (real32_T)dv0[i] + O[i] * dt;

-  }

-

-  /* prediction of the magnetic vector */

-  /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */

-  eye(dv0);

-  for (i = 0; i < 9; i++) {

-    b_a[i] = (real32_T)dv0[i] + O[i] * dt;

-  }

-

-  /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */

-  /* 'attitudeKalmanfilter:66'     -zez,0,zex; */

-  /* 'attitudeKalmanfilter:67'     zey,-zex,0]'; */

-  /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */

-  /* 'attitudeKalmanfilter:69'     -muz,0,mux; */

-  /* 'attitudeKalmanfilter:70'     zey,-mux,0]'; */

-  /* 'attitudeKalmanfilter:74' E=eye(3); */

-  /* 'attitudeKalmanfilter:76' Z=zeros(3); */

-  /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */

-  x_n_b[0] = x_aposteriori_k[0];

-  x_n_b[1] = x_aposteriori_k[1];

-  x_n_b[2] = x_aposteriori_k[2];

-  b_x_aposteriori_k[0] = x_aposteriori_k[6];

-  b_x_aposteriori_k[1] = x_aposteriori_k[7];

-  b_x_aposteriori_k[2] = x_aposteriori_k[8];

-  z_n_b[0] = x_aposteriori_k[9];

-  z_n_b[1] = x_aposteriori_k[10];

-  z_n_b[2] = x_aposteriori_k[11];

-  for (i = 0; i < 3; i++) {

-    c_a[i] = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];

-    }

-

-    d_a[i] = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];

-    }

-

-    x_apriori[i] = x_n_b[i] + dt * wak[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 3] = wak[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 6] = c_a[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 9] = d_a[i];

-  }

-

-  /* 'attitudeKalmanfilter:81' A_lin=[ Z,  E,  Z,  Z */

-  /* 'attitudeKalmanfilter:82'     Z,  Z,  Z,  Z */

-  /* 'attitudeKalmanfilter:83'     EZ, Z,  O,  Z */

-  /* 'attitudeKalmanfilter:84'     MA, Z,  Z,  O]; */

-  /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */

-  b_eye(dv1);

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];

-    }

-

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * i) + 3] = 0.0F;

-    }

-  }

-

-  A_lin[6] = 0.0F;

-  A_lin[7] = x_aposteriori_k[8];

-  A_lin[8] = -x_aposteriori_k[7];

-  A_lin[18] = -x_aposteriori_k[8];

-  A_lin[19] = 0.0F;

-  A_lin[20] = x_aposteriori_k[6];

-  A_lin[30] = x_aposteriori_k[7];

-  A_lin[31] = -x_aposteriori_k[6];

-  A_lin[32] = 0.0F;

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;

-    }

-  }

-

-  A_lin[9] = 0.0F;

-  A_lin[10] = x_aposteriori_k[11];

-  A_lin[11] = -x_aposteriori_k[10];

-  A_lin[21] = -x_aposteriori_k[11];

-  A_lin[22] = 0.0F;

-  A_lin[23] = x_aposteriori_k[9];

-  A_lin[33] = x_aposteriori_k[7];

-  A_lin[34] = -x_aposteriori_k[9];

-  A_lin[35] = 0.0F;

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];

-    }

-  }

-

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *

-        dt;

-    }

-  }

-

-  /* 'attitudeKalmanfilter:88' Qtemp=[ q(1),     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:89'         0,     q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:90'         0,     0,      q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:91'         0,     0,      0,      q(2),   0,      0,     0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:92'         0,     0,      0,      0,      q(2),   0,     0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:93'         0,     0,      0,      0,      0,      q(2),   0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:94'         0,     0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:95'         0,     0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:96'         0,     0,      0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0; */

-  /* 'attitudeKalmanfilter:97'         0,     0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0,      0; */

-  /* 'attitudeKalmanfilter:98'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0; */

-  /* 'attitudeKalmanfilter:99'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4)]; */

-  /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */

-  /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */

-  b_q[0] = q[0];

-  b_q[12] = 0.0F;

-  b_q[24] = 0.0F;

-  b_q[36] = 0.0F;

-  b_q[48] = 0.0F;

-  b_q[60] = 0.0F;

-  b_q[72] = 0.0F;

-  b_q[84] = 0.0F;

-  b_q[96] = 0.0F;

-  b_q[108] = 0.0F;

-  b_q[120] = 0.0F;

-  b_q[132] = 0.0F;

-  b_q[1] = 0.0F;

-  b_q[13] = q[0];

-  b_q[25] = 0.0F;

-  b_q[37] = 0.0F;

-  b_q[49] = 0.0F;

-  b_q[61] = 0.0F;

-  b_q[73] = 0.0F;

-  b_q[85] = 0.0F;

-  b_q[97] = 0.0F;

-  b_q[109] = 0.0F;

-  b_q[121] = 0.0F;

-  b_q[133] = 0.0F;

-  b_q[2] = 0.0F;

-  b_q[14] = 0.0F;

-  b_q[26] = q[0];

-  b_q[38] = 0.0F;

-  b_q[50] = 0.0F;

-  b_q[62] = 0.0F;

-  b_q[74] = 0.0F;

-  b_q[86] = 0.0F;

-  b_q[98] = 0.0F;

-  b_q[110] = 0.0F;

-  b_q[122] = 0.0F;

-  b_q[134] = 0.0F;

-  b_q[3] = 0.0F;

-  b_q[15] = 0.0F;

-  b_q[27] = 0.0F;

-  b_q[39] = q[1];

-  b_q[51] = 0.0F;

-  b_q[63] = 0.0F;

-  b_q[75] = 0.0F;

-  b_q[87] = 0.0F;

-  b_q[99] = 0.0F;

-  b_q[111] = 0.0F;

-  b_q[123] = 0.0F;

-  b_q[135] = 0.0F;

-  b_q[4] = 0.0F;

-  b_q[16] = 0.0F;

-  b_q[28] = 0.0F;

-  b_q[40] = 0.0F;

-  b_q[52] = q[1];

-  b_q[64] = 0.0F;

-  b_q[76] = 0.0F;

-  b_q[88] = 0.0F;

-  b_q[100] = 0.0F;

-  b_q[112] = 0.0F;

-  b_q[124] = 0.0F;

-  b_q[136] = 0.0F;

-  b_q[5] = 0.0F;

-  b_q[17] = 0.0F;

-  b_q[29] = 0.0F;

-  b_q[41] = 0.0F;

-  b_q[53] = 0.0F;

-  b_q[65] = q[1];

-  b_q[77] = 0.0F;

-  b_q[89] = 0.0F;

-  b_q[101] = 0.0F;

-  b_q[113] = 0.0F;

-  b_q[125] = 0.0F;

-  b_q[137] = 0.0F;

-  b_q[6] = 0.0F;

-  b_q[18] = 0.0F;

-  b_q[30] = 0.0F;

-  b_q[42] = 0.0F;

-  b_q[54] = 0.0F;

-  b_q[66] = 0.0F;

-  b_q[78] = q[2];

-  b_q[90] = 0.0F;

-  b_q[102] = 0.0F;

-  b_q[114] = 0.0F;

-  b_q[126] = 0.0F;

-  b_q[138] = 0.0F;

-  b_q[7] = 0.0F;

-  b_q[19] = 0.0F;

-  b_q[31] = 0.0F;

-  b_q[43] = 0.0F;

-  b_q[55] = 0.0F;

-  b_q[67] = 0.0F;

-  b_q[79] = 0.0F;

-  b_q[91] = q[2];

-  b_q[103] = 0.0F;

-  b_q[115] = 0.0F;

-  b_q[127] = 0.0F;

-  b_q[139] = 0.0F;

-  b_q[8] = 0.0F;

-  b_q[20] = 0.0F;

-  b_q[32] = 0.0F;

-  b_q[44] = 0.0F;

-  b_q[56] = 0.0F;

-  b_q[68] = 0.0F;

-  b_q[80] = 0.0F;

-  b_q[92] = 0.0F;

-  b_q[104] = q[2];

-  b_q[116] = 0.0F;

-  b_q[128] = 0.0F;

-  b_q[140] = 0.0F;

-  b_q[9] = 0.0F;

-  b_q[21] = 0.0F;

-  b_q[33] = 0.0F;

-  b_q[45] = 0.0F;

-  b_q[57] = 0.0F;

-  b_q[69] = 0.0F;

-  b_q[81] = 0.0F;

-  b_q[93] = 0.0F;

-  b_q[105] = 0.0F;

-  b_q[117] = q[3];

-  b_q[129] = 0.0F;

-  b_q[141] = 0.0F;

-  b_q[10] = 0.0F;

-  b_q[22] = 0.0F;

-  b_q[34] = 0.0F;

-  b_q[46] = 0.0F;

-  b_q[58] = 0.0F;

-  b_q[70] = 0.0F;

-  b_q[82] = 0.0F;

-  b_q[94] = 0.0F;

-  b_q[106] = 0.0F;

-  b_q[118] = 0.0F;

-  b_q[130] = q[3];

-  b_q[142] = 0.0F;

-  b_q[11] = 0.0F;

-  b_q[23] = 0.0F;

-  b_q[35] = 0.0F;

-  b_q[47] = 0.0F;

-  b_q[59] = 0.0F;

-  b_q[71] = 0.0F;

-  b_q[83] = 0.0F;

-  b_q[95] = 0.0F;

-  b_q[107] = 0.0F;

-  b_q[119] = 0.0F;

-  b_q[131] = 0.0F;

-  b_q[143] = q[3];

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *

-          i0];

-      }

-

-      c_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];

-      }

-    }

-

-    for (i0 = 0; i0 < 12; i0++) {

-      d_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

-      }

-

-      e_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

-      }

-    }

-  }

-

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];

-    }

-  }

-

-  /* % update */

-  /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */

-  if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {

-    /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */

-    if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

-      /* 'attitudeKalmanfilter:112' r(2)=10000; */

-      r[1] = 10000.0F;

-    }

-

-    /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:115'         0,r(1),0,0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:116'         0,0,r(1),0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:117'         0,0,0,r(2),0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:118'         0,0,0,0,r(2),0,0,0,0; */

-    /* 'attitudeKalmanfilter:119'         0,0,0,0,0,r(2),0,0,0; */

-    /* 'attitudeKalmanfilter:120'         0,0,0,0,0,0,r(3),0,0; */

-    /* 'attitudeKalmanfilter:121'         0,0,0,0,0,0,0,r(3),0; */

-    /* 'attitudeKalmanfilter:122'         0,0,0,0,0,0,0,0,r(3)]; */

-    /* observation matrix */

-    /* [zw;ze;zmk]; */

-    /* 'attitudeKalmanfilter:125' H_k=[  E,     Z,      Z,    Z; */

-    /* 'attitudeKalmanfilter:126'         Z,     Z,      E,    Z; */

-    /* 'attitudeKalmanfilter:127'         Z,     Z,      Z,    E]; */

-    /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */

-    /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */

-    /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        b_P_apriori[i + 12 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1

-            + 12 * i0];

-        }

-      }

-    }

-

-    for (i = 0; i < 9; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        K_k[i + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];

-        }

-      }

-

-      for (i0 = 0; i0 < 9; i0++) {

-        fv0[i + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];

-        }

-      }

-    }

-

-    b_r[0] = r[0];

-    b_r[9] = 0.0F;

-    b_r[18] = 0.0F;

-    b_r[27] = 0.0F;

-    b_r[36] = 0.0F;

-    b_r[45] = 0.0F;

-    b_r[54] = 0.0F;

-    b_r[63] = 0.0F;

-    b_r[72] = 0.0F;

-    b_r[1] = 0.0F;

-    b_r[10] = r[0];

-    b_r[19] = 0.0F;

-    b_r[28] = 0.0F;

-    b_r[37] = 0.0F;

-    b_r[46] = 0.0F;

-    b_r[55] = 0.0F;

-    b_r[64] = 0.0F;

-    b_r[73] = 0.0F;

-    b_r[2] = 0.0F;

-    b_r[11] = 0.0F;

-    b_r[20] = r[0];

-    b_r[29] = 0.0F;

-    b_r[38] = 0.0F;

-    b_r[47] = 0.0F;

-    b_r[56] = 0.0F;

-    b_r[65] = 0.0F;

-    b_r[74] = 0.0F;

-    b_r[3] = 0.0F;

-    b_r[12] = 0.0F;

-    b_r[21] = 0.0F;

-    b_r[30] = r[1];

-    b_r[39] = 0.0F;

-    b_r[48] = 0.0F;

-    b_r[57] = 0.0F;

-    b_r[66] = 0.0F;

-    b_r[75] = 0.0F;

-    b_r[4] = 0.0F;

-    b_r[13] = 0.0F;

-    b_r[22] = 0.0F;

-    b_r[31] = 0.0F;

-    b_r[40] = r[1];

-    b_r[49] = 0.0F;

-    b_r[58] = 0.0F;

-    b_r[67] = 0.0F;

-    b_r[76] = 0.0F;

-    b_r[5] = 0.0F;

-    b_r[14] = 0.0F;

-    b_r[23] = 0.0F;

-    b_r[32] = 0.0F;

-    b_r[41] = 0.0F;

-    b_r[50] = r[1];

-    b_r[59] = 0.0F;

-    b_r[68] = 0.0F;

-    b_r[77] = 0.0F;

-    b_r[6] = 0.0F;

-    b_r[15] = 0.0F;

-    b_r[24] = 0.0F;

-    b_r[33] = 0.0F;

-    b_r[42] = 0.0F;

-    b_r[51] = 0.0F;

-    b_r[60] = r[2];

-    b_r[69] = 0.0F;

-    b_r[78] = 0.0F;

-    b_r[7] = 0.0F;

-    b_r[16] = 0.0F;

-    b_r[25] = 0.0F;

-    b_r[34] = 0.0F;

-    b_r[43] = 0.0F;

-    b_r[52] = 0.0F;

-    b_r[61] = 0.0F;

-    b_r[70] = r[2];

-    b_r[79] = 0.0F;

-    b_r[8] = 0.0F;

-    b_r[17] = 0.0F;

-    b_r[26] = 0.0F;

-    b_r[35] = 0.0F;

-    b_r[44] = 0.0F;

-    b_r[53] = 0.0F;

-    b_r[62] = 0.0F;

-    b_r[71] = 0.0F;

-    b_r[80] = r[2];

-    for (i = 0; i < 9; i++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];

-      }

-    }

-

-    mrdivide(b_P_apriori, fv1, K_k);

-

-    /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */

-    for (i = 0; i < 9; i++) {

-      f0 = 0.0F;

-      for (i0 = 0; i0 < 12; i0++) {

-        f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];

-      }

-

-      O[i] = z[i] - f0;

-    }

-

-    for (i = 0; i < 12; i++) {

-      f0 = 0.0F;

-      for (i0 = 0; i0 < 9; i0++) {

-        f0 += K_k[i + 12 * i0] * O[i0];

-      }

-

-      x_aposteriori[i] = x_apriori[i] + f0;

-    }

-

-    /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */

-    b_eye(dv1);

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        f0 = 0.0F;

-        for (i1 = 0; i1 < 9; i1++) {

-          f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];

-        }

-

-        b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-      }

-    }

-

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        P_aposteriori[i + 12 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12

-            * i0];

-        }

-      }

-    }

-  } else {

-    /* 'attitudeKalmanfilter:138' else */

-    /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */

-    if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {

-      /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */

-      /* 'attitudeKalmanfilter:142'             0,r(1),0; */

-      /* 'attitudeKalmanfilter:143'             0,0,r(1)]; */

-      /* observation matrix */

-      /* 'attitudeKalmanfilter:146' H_k=[  E,     Z,      Z,    Z]; */

-      /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */

-      /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */

-      /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 3; i0++) {

-          c_P_apriori[i + 12 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-              iv3[i1 + 12 * i0];

-          }

-        }

-      }

-

-      for (i = 0; i < 3; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          fv2[i + 3 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *

-              i0];

-          }

-        }

-

-        for (i0 = 0; i0 < 3; i0++) {

-          O[i + 3 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];

-          }

-        }

-      }

-

-      c_r[0] = r[0];

-      c_r[3] = 0.0F;

-      c_r[6] = 0.0F;

-      c_r[1] = 0.0F;

-      c_r[4] = r[0];

-      c_r[7] = 0.0F;

-      c_r[2] = 0.0F;

-      c_r[5] = 0.0F;

-      c_r[8] = r[0];

-      for (i = 0; i < 3; i++) {

-        for (i0 = 0; i0 < 3; i0++) {

-          a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];

-        }

-      }

-

-      b_mrdivide(c_P_apriori, a, b_K_k);

-

-      /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */

-      for (i = 0; i < 3; i++) {

-        f0 = 0.0F;

-        for (i0 = 0; i0 < 12; i0++) {

-          f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];

-        }

-

-        x_n_b[i] = z[i] - f0;

-      }

-

-      for (i = 0; i < 12; i++) {

-        f0 = 0.0F;

-        for (i0 = 0; i0 < 3; i0++) {

-          f0 += b_K_k[i + 12 * i0] * x_n_b[i0];

-        }

-

-        x_aposteriori[i] = x_apriori[i] + f0;

-      }

-

-      /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */

-      b_eye(dv1);

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          f0 = 0.0F;

-          for (i1 = 0; i1 < 3; i1++) {

-            f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];

-          }

-

-          b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-        }

-      }

-

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          P_aposteriori[i + 12 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +

-              12 * i0];

-          }

-        }

-      }

-    } else {

-      /* 'attitudeKalmanfilter:156' else */

-      /* 'attitudeKalmanfilter:157' if  updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */

-      if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))

-      {

-        /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */

-        if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

-          /* 'attitudeKalmanfilter:159' r(2)=10000; */

-          r[1] = 10000.0F;

-        }

-

-        /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */

-        /* 'attitudeKalmanfilter:163'                 0,r(1),0,0,0,0; */

-        /* 'attitudeKalmanfilter:164'                 0,0,r(1),0,0,0; */

-        /* 'attitudeKalmanfilter:165'                 0,0,0,r(2),0,0; */

-        /* 'attitudeKalmanfilter:166'                 0,0,0,0,r(2),0; */

-        /* 'attitudeKalmanfilter:167'                 0,0,0,0,0,r(2)]; */

-        /* observation matrix */

-        /* 'attitudeKalmanfilter:170' H_k=[  E,     Z,      Z,    Z; */

-        /* 'attitudeKalmanfilter:171'                 Z,     Z,      E,    Z]; */

-        /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */

-        /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

-        /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 6; i0++) {

-            d_P_apriori[i + 12 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                iv5[i1 + 12 * i0];

-            }

-          }

-        }

-

-        for (i = 0; i < 6; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            c_K_k[i + 6 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12

-                * i0];

-            }

-          }

-

-          for (i0 = 0; i0 < 6; i0++) {

-            fv2[i + 6 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];

-            }

-          }

-        }

-

-        b_K_k[0] = r[0];

-        b_K_k[6] = 0.0F;

-        b_K_k[12] = 0.0F;

-        b_K_k[18] = 0.0F;

-        b_K_k[24] = 0.0F;

-        b_K_k[30] = 0.0F;

-        b_K_k[1] = 0.0F;

-        b_K_k[7] = r[0];

-        b_K_k[13] = 0.0F;

-        b_K_k[19] = 0.0F;

-        b_K_k[25] = 0.0F;

-        b_K_k[31] = 0.0F;

-        b_K_k[2] = 0.0F;

-        b_K_k[8] = 0.0F;

-        b_K_k[14] = r[0];

-        b_K_k[20] = 0.0F;

-        b_K_k[26] = 0.0F;

-        b_K_k[32] = 0.0F;

-        b_K_k[3] = 0.0F;

-        b_K_k[9] = 0.0F;

-        b_K_k[15] = 0.0F;

-        b_K_k[21] = r[1];

-        b_K_k[27] = 0.0F;

-        b_K_k[33] = 0.0F;

-        b_K_k[4] = 0.0F;

-        b_K_k[10] = 0.0F;

-        b_K_k[16] = 0.0F;

-        b_K_k[22] = 0.0F;

-        b_K_k[28] = r[1];

-        b_K_k[34] = 0.0F;

-        b_K_k[5] = 0.0F;

-        b_K_k[11] = 0.0F;

-        b_K_k[17] = 0.0F;

-        b_K_k[23] = 0.0F;

-        b_K_k[29] = 0.0F;

-        b_K_k[35] = r[1];

-        for (i = 0; i < 6; i++) {

-          for (i0 = 0; i0 < 6; i0++) {

-            c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

-          }

-        }

-

-        c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

-

-        /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */

-        for (i = 0; i < 6; i++) {

-          f0 = 0.0F;

-          for (i0 = 0; i0 < 12; i0++) {

-            f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];

-          }

-

-          b_z[i] = z[i] - f0;

-        }

-

-        for (i = 0; i < 12; i++) {

-          f0 = 0.0F;

-          for (i0 = 0; i0 < 6; i0++) {

-            f0 += c_K_k[i + 12 * i0] * b_z[i0];

-          }

-

-          x_aposteriori[i] = x_apriori[i] + f0;

-        }

-

-        /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

-        b_eye(dv1);

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            f0 = 0.0F;

-            for (i1 = 0; i1 < 6; i1++) {

-              f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];

-            }

-

-            b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-          }

-        }

-

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            P_aposteriori[i + 12 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1

-                + 12 * i0];

-            }

-          }

-        }

-      } else {

-        /* 'attitudeKalmanfilter:181' else */

-        /* 'attitudeKalmanfilter:182' if  updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */

-        if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))

-        {

-          /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */

-          /* 'attitudeKalmanfilter:184'                     0,r(1),0,0,0,0; */

-          /* 'attitudeKalmanfilter:185'                     0,0,r(1),0,0,0; */

-          /* 'attitudeKalmanfilter:186'                     0,0,0,r(3),0,0; */

-          /* 'attitudeKalmanfilter:187'                     0,0,0,0,r(3),0; */

-          /* 'attitudeKalmanfilter:188'                     0,0,0,0,0,r(3)]; */

-          /* observation matrix */

-          /* 'attitudeKalmanfilter:191' H_k=[  E,     Z,      Z,    Z; */

-          /* 'attitudeKalmanfilter:192'                     Z,     Z,      Z,    E]; */

-          /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */

-          /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

-          /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 6; i0++) {

-              d_P_apriori[i + 12 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                  iv7[i1 + 12 * i0];

-              }

-            }

-          }

-

-          for (i = 0; i < 6; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              c_K_k[i + 6 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +

-                  12 * i0];

-              }

-            }

-

-            for (i0 = 0; i0 < 6; i0++) {

-              fv2[i + 6 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *

-                  i0];

-              }

-            }

-          }

-

-          b_K_k[0] = r[0];

-          b_K_k[6] = 0.0F;

-          b_K_k[12] = 0.0F;

-          b_K_k[18] = 0.0F;

-          b_K_k[24] = 0.0F;

-          b_K_k[30] = 0.0F;

-          b_K_k[1] = 0.0F;

-          b_K_k[7] = r[0];

-          b_K_k[13] = 0.0F;

-          b_K_k[19] = 0.0F;

-          b_K_k[25] = 0.0F;

-          b_K_k[31] = 0.0F;

-          b_K_k[2] = 0.0F;

-          b_K_k[8] = 0.0F;

-          b_K_k[14] = r[0];

-          b_K_k[20] = 0.0F;

-          b_K_k[26] = 0.0F;

-          b_K_k[32] = 0.0F;

-          b_K_k[3] = 0.0F;

-          b_K_k[9] = 0.0F;

-          b_K_k[15] = 0.0F;

-          b_K_k[21] = r[2];

-          b_K_k[27] = 0.0F;

-          b_K_k[33] = 0.0F;

-          b_K_k[4] = 0.0F;

-          b_K_k[10] = 0.0F;

-          b_K_k[16] = 0.0F;

-          b_K_k[22] = 0.0F;

-          b_K_k[28] = r[2];

-          b_K_k[34] = 0.0F;

-          b_K_k[5] = 0.0F;

-          b_K_k[11] = 0.0F;

-          b_K_k[17] = 0.0F;

-          b_K_k[23] = 0.0F;

-          b_K_k[29] = 0.0F;

-          b_K_k[35] = r[2];

-          for (i = 0; i < 6; i++) {

-            for (i0 = 0; i0 < 6; i0++) {

-              c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

-            }

-          }

-

-          c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

-

-          /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */

-          for (i = 0; i < 3; i++) {

-            b_z[i] = z[i];

-          }

-

-          for (i = 0; i < 3; i++) {

-            b_z[i + 3] = z[i + 6];

-          }

-

-          for (i = 0; i < 6; i++) {

-            fv3[i] = 0.0F;

-            for (i0 = 0; i0 < 12; i0++) {

-              fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];

-            }

-

-            c_z[i] = b_z[i] - fv3[i];

-          }

-

-          for (i = 0; i < 12; i++) {

-            f0 = 0.0F;

-            for (i0 = 0; i0 < 6; i0++) {

-              f0 += c_K_k[i + 12 * i0] * c_z[i0];

-            }

-

-            x_aposteriori[i] = x_apriori[i] + f0;

-          }

-

-          /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

-          b_eye(dv1);

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              f0 = 0.0F;

-              for (i1 = 0; i1 < 6; i1++) {

-                f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];

-              }

-

-              b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-            }

-          }

-

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              P_aposteriori[i + 12 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *

-                  P_apriori[i1 + 12 * i0];

-              }

-            }

-          }

-        } else {

-          /* 'attitudeKalmanfilter:202' else */

-          /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */

-          for (i = 0; i < 12; i++) {

-            x_aposteriori[i] = x_apriori[i];

-          }

-

-          /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */

-          memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));

-        }

-      }

-    }

-  }

-

-  /* % euler anglels extraction */

-  /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = -x_aposteriori[i + 6];

-  }

-

-  rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);

-

-  /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */

-  rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&

-           x_aposteriori[9]), wak);

-

-  /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = wak[i];

-  }

-

-  cross(z_n_b, x_n_b, wak);

-

-  /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = wak[i];

-  }

-

-  rdivide(x_n_b, norm(wak), wak);

-

-  /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */

-  cross(wak, z_n_b, x_n_b);

-

-  /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */

-  for (i = 0; i < 3; i++) {

-    b_x_aposteriori_k[i] = x_n_b[i];

-  }

-

-  rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);

-

-  /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */

-  for (i = 0; i < 3; i++) {

-    Rot_matrix[i] = x_n_b[i];

-    Rot_matrix[3 + i] = wak[i];

-    Rot_matrix[6 + i] = z_n_b[i];

-  }

-

-  /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */

-  /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */

-  /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */

-  /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */

-  eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);

-  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);

-  eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);

-}

-

-/* End of code generation (attitudeKalmanfilter.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
deleted file mode 100755
index afa63c1a9..000000000
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_H__

-#define __ATTITUDEKALMANFILTER_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);

-#endif

-/* End of code generation (attitudeKalmanfilter.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
deleted file mode 100755
index 7d620d7fa..000000000
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * attitudeKalmanfilter_initialize.c

- *

- * Code generation for function 'attitudeKalmanfilter_initialize'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "attitudeKalmanfilter_initialize.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void attitudeKalmanfilter_initialize(void)

-{

-  rt_InitInfAndNaN(8U);

-}

-

-/* End of code generation (attitudeKalmanfilter_initialize.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
deleted file mode 100755
index 8b599be66..000000000
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter_initialize.h

- *

- * Code generation for function 'attitudeKalmanfilter_initialize'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__

-#define __ATTITUDEKALMANFILTER_INITIALIZE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter_initialize(void);

-#endif

-/* End of code generation (attitudeKalmanfilter_initialize.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
deleted file mode 100755
index 7f9727419..000000000
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * attitudeKalmanfilter_terminate.c

- *

- * Code generation for function 'attitudeKalmanfilter_terminate'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "attitudeKalmanfilter_terminate.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void attitudeKalmanfilter_terminate(void)

-{

-  /* (no terminate code required) */

-}

-

-/* End of code generation (attitudeKalmanfilter_terminate.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
deleted file mode 100755
index da84a5024..000000000
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter_terminate.h

- *

- * Code generation for function 'attitudeKalmanfilter_terminate'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__

-#define __ATTITUDEKALMANFILTER_TERMINATE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter_terminate(void);

-#endif

-/* End of code generation (attitudeKalmanfilter_terminate.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
deleted file mode 100755
index 30fd1e75e..000000000
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*

- * attitudeKalmanfilter_types.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_TYPES_H__

-#define __ATTITUDEKALMANFILTER_TYPES_H__

-

-/* Type Definitions */

-

-#endif

-/* End of code generation (attitudeKalmanfilter_types.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c
deleted file mode 100755
index 84ada9002..000000000
--- a/apps/attitude_estimator_ekf/codegen/cross.c
+++ /dev/null
@@ -1,37 +0,0 @@
-/*

- * cross.c

- *

- * Code generation for function 'cross'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "cross.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])

-{

-  c[0] = a[1] * b[2] - a[2] * b[1];

-  c[1] = a[2] * b[0] - a[0] * b[2];

-  c[2] = a[0] * b[1] - a[1] * b[0];

-}

-

-/* End of code generation (cross.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h
deleted file mode 100755
index e727f0684..000000000
--- a/apps/attitude_estimator_ekf/codegen/cross.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * cross.h

- *

- * Code generation for function 'cross'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __CROSS_H__

-#define __CROSS_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);

-#endif

-/* End of code generation (cross.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c
deleted file mode 100755
index b89ab58ef..000000000
--- a/apps/attitude_estimator_ekf/codegen/eye.c
+++ /dev/null
@@ -1,51 +0,0 @@
-/*

- * eye.c

- *

- * Code generation for function 'eye'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "eye.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void b_eye(real_T I[144])

-{

-  int32_T i;

-  memset(&I[0], 0, 144U * sizeof(real_T));

-  for (i = 0; i < 12; i++) {

-    I[i + 12 * i] = 1.0;

-  }

-}

-

-/*

- *

- */

-void eye(real_T I[9])

-{

-  int32_T i;

-  memset(&I[0], 0, 9U * sizeof(real_T));

-  for (i = 0; i < 3; i++) {

-    I[i + 3 * i] = 1.0;

-  }

-}

-

-/* End of code generation (eye.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h
deleted file mode 100755
index 94fbe7671..000000000
--- a/apps/attitude_estimator_ekf/codegen/eye.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*

- * eye.h

- *

- * Code generation for function 'eye'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __EYE_H__

-#define __EYE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void b_eye(real_T I[144]);

-extern void eye(real_T I[9]);

-#endif

-/* End of code generation (eye.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c
deleted file mode 100755
index a810f22e4..000000000
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.c
+++ /dev/null
@@ -1,357 +0,0 @@
-/*

- * mrdivide.c

- *

- * Code generation for function 'mrdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "mrdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])

-{

-  real32_T b_A[9];

-  int32_T rtemp;

-  int32_T k;

-  real32_T b_B[36];

-  int32_T r1;

-  int32_T r2;

-  int32_T r3;

-  real32_T maxval;

-  real32_T a21;

-  real32_T Y[36];

-  for (rtemp = 0; rtemp < 3; rtemp++) {

-    for (k = 0; k < 3; k++) {

-      b_A[k + 3 * rtemp] = B[rtemp + 3 * k];

-    }

-  }

-

-  for (rtemp = 0; rtemp < 12; rtemp++) {

-    for (k = 0; k < 3; k++) {

-      b_B[k + 3 * rtemp] = A[rtemp + 12 * k];

-    }

-  }

-

-  r1 = 0;

-  r2 = 1;

-  r3 = 2;

-  maxval = (real32_T)fabs(b_A[0]);

-  a21 = (real32_T)fabs(b_A[1]);

-  if (a21 > maxval) {

-    maxval = a21;

-    r1 = 1;

-    r2 = 0;

-  }

-

-  if ((real32_T)fabs(b_A[2]) > maxval) {

-    r1 = 2;

-    r2 = 1;

-    r3 = 0;

-  }

-

-  b_A[r2] /= b_A[r1];

-  b_A[r3] /= b_A[r1];

-  b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];

-  b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];

-  b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];

-  b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];

-  if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {

-    rtemp = r2;

-    r2 = r3;

-    r3 = rtemp;

-  }

-

-  b_A[3 + r3] /= b_A[3 + r2];

-  b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];

-  for (k = 0; k < 12; k++) {

-    Y[3 * k] = b_B[r1 + 3 * k];

-    Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];

-    Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3

-      + r3];

-    Y[2 + 3 * k] /= b_A[6 + r3];

-    Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];

-    Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];

-    Y[1 + 3 * k] /= b_A[3 + r2];

-    Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];

-    Y[3 * k] /= b_A[r1];

-  }

-

-  for (rtemp = 0; rtemp < 3; rtemp++) {

-    for (k = 0; k < 12; k++) {

-      y[k + 12 * rtemp] = Y[rtemp + 3 * k];

-    }

-  }

-}

-

-/*

- *

- */

-void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])

-{

-  real32_T b_A[36];

-  int8_T ipiv[6];

-  int32_T i3;

-  int32_T iy;

-  int32_T j;

-  int32_T c;

-  int32_T ix;

-  real32_T temp;

-  int32_T k;

-  real32_T s;

-  int32_T jy;

-  int32_T ijA;

-  real32_T Y[72];

-  for (i3 = 0; i3 < 6; i3++) {

-    for (iy = 0; iy < 6; iy++) {

-      b_A[iy + 6 * i3] = B[i3 + 6 * iy];

-    }

-

-    ipiv[i3] = (int8_T)(1 + i3);

-  }

-

-  for (j = 0; j < 5; j++) {

-    c = j * 7;

-    iy = 0;

-    ix = c;

-    temp = (real32_T)fabs(b_A[c]);

-    for (k = 2; k <= 6 - j; k++) {

-      ix++;

-      s = (real32_T)fabs(b_A[ix]);

-      if (s > temp) {

-        iy = k - 1;

-        temp = s;

-      }

-    }

-

-    if (b_A[c + iy] != 0.0F) {

-      if (iy != 0) {

-        ipiv[j] = (int8_T)((j + iy) + 1);

-        ix = j;

-        iy += j;

-        for (k = 0; k < 6; k++) {

-          temp = b_A[ix];

-          b_A[ix] = b_A[iy];

-          b_A[iy] = temp;

-          ix += 6;

-          iy += 6;

-        }

-      }

-

-      i3 = (c - j) + 6;

-      for (jy = c + 1; jy + 1 <= i3; jy++) {

-        b_A[jy] /= b_A[c];

-      }

-    }

-

-    iy = c;

-    jy = c + 6;

-    for (k = 1; k <= 5 - j; k++) {

-      temp = b_A[jy];

-      if (b_A[jy] != 0.0F) {

-        ix = c + 1;

-        i3 = (iy - j) + 12;

-        for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {

-          b_A[ijA] += b_A[ix] * -temp;

-          ix++;

-        }

-      }

-

-      jy += 6;

-      iy += 6;

-    }

-  }

-

-  for (i3 = 0; i3 < 12; i3++) {

-    for (iy = 0; iy < 6; iy++) {

-      Y[iy + 6 * i3] = A[i3 + 12 * iy];

-    }

-  }

-

-  for (jy = 0; jy < 6; jy++) {

-    if (ipiv[jy] != jy + 1) {

-      for (j = 0; j < 12; j++) {

-        temp = Y[jy + 6 * j];

-        Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];

-        Y[(ipiv[jy] + 6 * j) - 1] = temp;

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 6 * j;

-    for (k = 0; k < 6; k++) {

-      iy = 6 * k;

-      if (Y[k + c] != 0.0F) {

-        for (jy = k + 2; jy < 7; jy++) {

-          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];

-        }

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 6 * j;

-    for (k = 5; k > -1; k += -1) {

-      iy = 6 * k;

-      if (Y[k + c] != 0.0F) {

-        Y[k + c] /= b_A[k + iy];

-        for (jy = 0; jy + 1 <= k; jy++) {

-          Y[jy + c] -= Y[k + c] * b_A[jy + iy];

-        }

-      }

-    }

-  }

-

-  for (i3 = 0; i3 < 6; i3++) {

-    for (iy = 0; iy < 12; iy++) {

-      y[iy + 12 * i3] = Y[i3 + 6 * iy];

-    }

-  }

-}

-

-/*

- *

- */

-void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])

-{

-  real32_T b_A[81];

-  int8_T ipiv[9];

-  int32_T i2;

-  int32_T iy;

-  int32_T j;

-  int32_T c;

-  int32_T ix;

-  real32_T temp;

-  int32_T k;

-  real32_T s;

-  int32_T jy;

-  int32_T ijA;

-  real32_T Y[108];

-  for (i2 = 0; i2 < 9; i2++) {

-    for (iy = 0; iy < 9; iy++) {

-      b_A[iy + 9 * i2] = B[i2 + 9 * iy];

-    }

-

-    ipiv[i2] = (int8_T)(1 + i2);

-  }

-

-  for (j = 0; j < 8; j++) {

-    c = j * 10;

-    iy = 0;

-    ix = c;

-    temp = (real32_T)fabs(b_A[c]);

-    for (k = 2; k <= 9 - j; k++) {

-      ix++;

-      s = (real32_T)fabs(b_A[ix]);

-      if (s > temp) {

-        iy = k - 1;

-        temp = s;

-      }

-    }

-

-    if (b_A[c + iy] != 0.0F) {

-      if (iy != 0) {

-        ipiv[j] = (int8_T)((j + iy) + 1);

-        ix = j;

-        iy += j;

-        for (k = 0; k < 9; k++) {

-          temp = b_A[ix];

-          b_A[ix] = b_A[iy];

-          b_A[iy] = temp;

-          ix += 9;

-          iy += 9;

-        }

-      }

-

-      i2 = (c - j) + 9;

-      for (jy = c + 1; jy + 1 <= i2; jy++) {

-        b_A[jy] /= b_A[c];

-      }

-    }

-

-    iy = c;

-    jy = c + 9;

-    for (k = 1; k <= 8 - j; k++) {

-      temp = b_A[jy];

-      if (b_A[jy] != 0.0F) {

-        ix = c + 1;

-        i2 = (iy - j) + 18;

-        for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {

-          b_A[ijA] += b_A[ix] * -temp;

-          ix++;

-        }

-      }

-

-      jy += 9;

-      iy += 9;

-    }

-  }

-

-  for (i2 = 0; i2 < 12; i2++) {

-    for (iy = 0; iy < 9; iy++) {

-      Y[iy + 9 * i2] = A[i2 + 12 * iy];

-    }

-  }

-

-  for (jy = 0; jy < 9; jy++) {

-    if (ipiv[jy] != jy + 1) {

-      for (j = 0; j < 12; j++) {

-        temp = Y[jy + 9 * j];

-        Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];

-        Y[(ipiv[jy] + 9 * j) - 1] = temp;

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 9 * j;

-    for (k = 0; k < 9; k++) {

-      iy = 9 * k;

-      if (Y[k + c] != 0.0F) {

-        for (jy = k + 2; jy < 10; jy++) {

-          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];

-        }

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 9 * j;

-    for (k = 8; k > -1; k += -1) {

-      iy = 9 * k;

-      if (Y[k + c] != 0.0F) {

-        Y[k + c] /= b_A[k + iy];

-        for (jy = 0; jy + 1 <= k; jy++) {

-          Y[jy + c] -= Y[k + c] * b_A[jy + iy];

-        }

-      }

-    }

-  }

-

-  for (i2 = 0; i2 < 9; i2++) {

-    for (iy = 0; iy < 12; iy++) {

-      y[iy + 12 * i2] = Y[i2 + 9 * iy];

-    }

-  }

-}

-

-/* End of code generation (mrdivide.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h
deleted file mode 100755
index 2d3b0d51f..000000000
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*

- * mrdivide.h

- *

- * Code generation for function 'mrdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __MRDIVIDE_H__

-#define __MRDIVIDE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);

-extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);

-extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);

-#endif

-/* End of code generation (mrdivide.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c
deleted file mode 100755
index 0c418cc7b..000000000
--- a/apps/attitude_estimator_ekf/codegen/norm.c
+++ /dev/null
@@ -1,54 +0,0 @@
-/*

- * norm.c

- *

- * Code generation for function 'norm'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "norm.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-real32_T norm(const real32_T x[3])

-{

-  real32_T y;

-  real32_T scale;

-  int32_T k;

-  real32_T absxk;

-  real32_T t;

-  y = 0.0F;

-  scale = 1.17549435E-38F;

-  for (k = 0; k < 3; k++) {

-    absxk = (real32_T)fabs(x[k]);

-    if (absxk > scale) {

-      t = scale / absxk;

-      y = 1.0F + y * t * t;

-      scale = absxk;

-    } else {

-      t = absxk / scale;

-      y += t * t;

-    }

-  }

-

-  return scale * (real32_T)sqrt(y);

-}

-

-/* End of code generation (norm.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h
deleted file mode 100755
index 60cf77b57..000000000
--- a/apps/attitude_estimator_ekf/codegen/norm.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * norm.h

- *

- * Code generation for function 'norm'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __NORM_H__

-#define __NORM_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern real32_T norm(const real32_T x[3]);

-#endif

-/* End of code generation (norm.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/apps/attitude_estimator_ekf/codegen/rdivide.c
deleted file mode 100755
index d035dae5e..000000000
--- a/apps/attitude_estimator_ekf/codegen/rdivide.c
+++ /dev/null
@@ -1,38 +0,0 @@
-/*

- * rdivide.c

- *

- * Code generation for function 'rdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "rdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void rdivide(const real32_T x[3], real32_T y, real32_T z[3])

-{

-  int32_T i;

-  for (i = 0; i < 3; i++) {

-    z[i] = x[i] / y;

-  }

-}

-

-/* End of code generation (rdivide.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/apps/attitude_estimator_ekf/codegen/rdivide.h
deleted file mode 100755
index 4bbebebe2..000000000
--- a/apps/attitude_estimator_ekf/codegen/rdivide.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * rdivide.h

- *

- * Code generation for function 'rdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RDIVIDE_H__

-#define __RDIVIDE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);

-#endif

-/* End of code generation (rdivide.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c
deleted file mode 100755
index 34164d104..000000000
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/*

- * rtGetInf.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf

- */

-#include "rtGetInf.h"

-#define NumBitsPerChar	8U

-

-/* Function: rtGetInf ==================================================

- * Abstract:

- * Initialize rtInf needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetInf(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T inf = 0.0;

-  if (bitsPerReal == 32U) {

-    inf = rtGetInfF();

-  } else {

-    uint16_T one = 1U;

-    enum {

-      LittleEndian,

-      BigEndian

-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-    switch (machByteOrder) {

-     case LittleEndian:

-      {

-        union {

-          LittleEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        inf = tmpVal.fltVal;

-        break;

-      }

-

-     case BigEndian:

-      {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        inf = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return inf;

-}

-

-/* Function: rtGetInfF ==================================================

- * Abstract:

- * Initialize rtInfF needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetInfF(void)

-{

-  IEEESingle infF;

-  infF.wordL.wordLuint = 0x7F800000U;

-  return infF.wordL.wordLreal;

-}

-

-/* Function: rtGetMinusInf ==================================================

- * Abstract:

- * Initialize rtMinusInf needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetMinusInf(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T minf = 0.0;

-  if (bitsPerReal == 32U) {

-    minf = rtGetMinusInfF();

-  } else {

-    uint16_T one = 1U;

-    enum {

-      LittleEndian,

-      BigEndian

-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-    switch (machByteOrder) {

-     case LittleEndian:

-      {

-        union {

-          LittleEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0xFFF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        minf = tmpVal.fltVal;

-        break;

-      }

-

-     case BigEndian:

-      {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0xFFF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        minf = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return minf;

-}

-

-/* Function: rtGetMinusInfF ==================================================

- * Abstract:

- * Initialize rtMinusInfF needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetMinusInfF(void)

-{

-  IEEESingle minfF;

-  minfF.wordL.wordLuint = 0xFF800000U;

-  return minfF.wordL.wordLreal;

-}

-

-/* End of code generation (rtGetInf.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h
deleted file mode 100755
index 145373cd0..000000000
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*

- * rtGetInf.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTGETINF_H__

-#define __RTGETINF_H__

-

-#include <stddef.h>

-#include "rtwtypes.h"

-#include "rt_nonfinite.h"

-

-extern real_T rtGetInf(void);

-extern real32_T rtGetInfF(void);

-extern real_T rtGetMinusInf(void);

-extern real32_T rtGetMinusInfF(void);

-

-#endif

-/* End of code generation (rtGetInf.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
deleted file mode 100755
index d84ca9573..000000000
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/*

- * rtGetNaN.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *       MATLAB for code generation function to initialize non-finite, NaN

- */

-#include "rtGetNaN.h"

-#define NumBitsPerChar	8U

-

-/* Function: rtGetNaN ==================================================

- * Abstract:

- * Initialize rtNaN needed by the generated code.

- * NaN is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetNaN(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T nan = 0.0;

-  if (bitsPerReal == 32U) {

-    nan = rtGetNaNF();

-  } else {

-  uint16_T one = 1U;

-  enum {

-    LittleEndian,

-    BigEndian

-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-  switch (machByteOrder) {

-   case LittleEndian:

-    {

-      union {

-        LittleEndianIEEEDouble bitVal;

-        real_T fltVal;

-      } tmpVal;

-

-      tmpVal.bitVal.words.wordH = 0xFFF80000U;

-      tmpVal.bitVal.words.wordL = 0x00000000U;

-      nan = tmpVal.fltVal;

-      break;

-    }

-

-   case BigEndian:

-    {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;

-        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;

-        nan = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return nan;

-}

-

-/* Function: rtGetNaNF ==================================================

- * Abstract:

- * Initialize rtNaNF needed by the generated code.

- * NaN is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetNaNF(void)

-{

-  IEEESingle nanF = { { 0 } };

-  uint16_T one = 1U;

-  enum {

-    LittleEndian,

-    BigEndian

-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-  switch (machByteOrder) {

-   case LittleEndian:

-    {

-      nanF.wordL.wordLuint = 0xFFC00000U;

-      break;

-    }

-

-   case BigEndian:

-    {

-      nanF.wordL.wordLuint = 0x7FFFFFFFU;

-      break;

-    }

-  }

-

-  return nanF.wordL.wordLreal;

-}

-

-/* End of code generation (rtGetNaN.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
deleted file mode 100755
index 65fdaa96f..000000000
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*

- * rtGetNaN.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTGETNAN_H__

-#define __RTGETNAN_H__

-

-#include <stddef.h>

-#include "rtwtypes.h"

-#include "rt_nonfinite.h"

-

-extern real_T rtGetNaN(void);

-extern real32_T rtGetNaNF(void);

-

-#endif

-/* End of code generation (rtGetNaN.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h
deleted file mode 100755
index 356498363..000000000
--- a/apps/attitude_estimator_ekf/codegen/rt_defines.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*

- * rt_defines.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RT_DEFINES_H__

-#define __RT_DEFINES_H__

-

-#include <stdlib.h>

-

-#define RT_PI	3.14159265358979323846

-#define RT_PIF	3.1415927F

-#define RT_LN_10	2.30258509299404568402

-#define RT_LN_10F	2.3025851F

-#define RT_LOG10E	0.43429448190325182765

-#define RT_LOG10EF	0.43429449F

-#define RT_E	2.7182818284590452354

-#define RT_EF	2.7182817F

-#endif

-/* End of code generation (rt_defines.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
deleted file mode 100755
index 303d1d9d2..000000000
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/*

- * rt_nonfinite.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *      MATLAB for code generation function to initialize non-finites,

- *      (Inf, NaN and -Inf).

- */

-#include "rt_nonfinite.h"

-#include "rtGetNaN.h"

-#include "rtGetInf.h"

-

-real_T rtInf;

-real_T rtMinusInf;

-real_T rtNaN;

-real32_T rtInfF;

-real32_T rtMinusInfF;

-real32_T rtNaNF;

-

-/* Function: rt_InitInfAndNaN ==================================================

- * Abstract:

- * Initialize the rtInf, rtMinusInf, and rtNaN needed by the

- * generated code. NaN is initialized as non-signaling. Assumes IEEE.

- */

-void rt_InitInfAndNaN(size_t realSize)

-{

-  (void) (realSize);

-  rtNaN = rtGetNaN();

-  rtNaNF = rtGetNaNF();

-  rtInf = rtGetInf();

-  rtInfF = rtGetInfF();

-  rtMinusInf = rtGetMinusInf();

-  rtMinusInfF = rtGetMinusInfF();

-}

-

-/* Function: rtIsInf ==================================================

- * Abstract:

- * Test if value is infinite

- */

-boolean_T rtIsInf(real_T value)

-{

-  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);

-}

-

-/* Function: rtIsInfF =================================================

- * Abstract:

- * Test if single-precision value is infinite

- */

-boolean_T rtIsInfF(real32_T value)

-{

-  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);

-}

-

-/* Function: rtIsNaN ==================================================

- * Abstract:

- * Test if value is not a number

- */

-boolean_T rtIsNaN(real_T value)

-{

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-  return _isnan(value)? TRUE:FALSE;

-#else

-  return (value!=value)? 1U:0U;

-#endif

-}

-

-/* Function: rtIsNaNF =================================================

- * Abstract:

- * Test if single-precision value is not a number

- */

-boolean_T rtIsNaNF(real32_T value)

-{

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-  return _isnan((real_T)value)? true:false;

-#else

-  return (value!=value)? 1U:0U;

-#endif

-}

-

-

-/* End of code generation (rt_nonfinite.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
deleted file mode 100755
index bd56b30d9..000000000
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*

- * rt_nonfinite.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RT_NONFINITE_H__

-#define __RT_NONFINITE_H__

-

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-#include <float.h>

-#endif

-#include <stddef.h>

-#include "rtwtypes.h"

-

-extern real_T rtInf;

-extern real_T rtMinusInf;

-extern real_T rtNaN;

-extern real32_T rtInfF;

-extern real32_T rtMinusInfF;

-extern real32_T rtNaNF;

-extern void rt_InitInfAndNaN(size_t realSize);

-extern boolean_T rtIsInf(real_T value);

-extern boolean_T rtIsInfF(real32_T value);

-extern boolean_T rtIsNaN(real_T value);

-extern boolean_T rtIsNaNF(real32_T value);

-

-typedef struct {

-  struct {

-    uint32_T wordH;

-    uint32_T wordL;

-  } words;

-} BigEndianIEEEDouble;

-

-typedef struct {

-  struct {

-    uint32_T wordL;

-    uint32_T wordH;

-  } words;

-} LittleEndianIEEEDouble;

-

-typedef struct {

-  union {

-    real32_T wordLreal;

-    uint32_T wordLuint;

-  } wordL;

-} IEEESingle;

-

-#endif

-/* End of code generation (rt_nonfinite.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h
deleted file mode 100755
index 9a5c96267..000000000
--- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h
+++ /dev/null
@@ -1,159 +0,0 @@
-/*

- * rtwtypes.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTWTYPES_H__

-#define __RTWTYPES_H__

-#ifndef TRUE

-# define TRUE (1U)

-#endif

-#ifndef FALSE

-# define FALSE (0U)

-#endif

-#ifndef __TMWTYPES__

-#define __TMWTYPES__

-

-#include <limits.h>

-

-/*=======================================================================* 

- * Target hardware information

- *   Device type: Generic->MATLAB Host Computer

- *   Number of bits:     char:   8    short:   16    int:  32

- *                       long:  32      native word size:  32

- *   Byte ordering: LittleEndian

- *   Signed integer division rounds to: Zero

- *   Shift right on a signed integer as arithmetic shift: on

- *=======================================================================*/

-

-/*=======================================================================* 

- * Fixed width word size data types:                                     * 

- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 

- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 

- *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 

- *=======================================================================*/

-

-typedef signed char int8_T;

-typedef unsigned char uint8_T;

-typedef short int16_T;

-typedef unsigned short uint16_T;

-typedef int int32_T;

-typedef unsigned int uint32_T;

-typedef float real32_T;

-typedef double real64_T;

-

-/*===========================================================================* 

- * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 

- *                           ulong_T, char_T and byte_T.                     * 

- *===========================================================================*/

-

-typedef double real_T;

-typedef double time_T;

-typedef unsigned char boolean_T;

-typedef int int_T;

-typedef unsigned int uint_T;

-typedef unsigned long ulong_T;

-typedef char char_T;

-typedef char_T byte_T;

-

-/*===========================================================================* 

- * Complex number type definitions                                           * 

- *===========================================================================*/

-#define CREAL_T	

-   typedef struct {  

-     real32_T re;  

-     real32_T im;  

-   } creal32_T;  

-

-   typedef struct {  

-     real64_T re;  

-     real64_T im;  

-   } creal64_T;  

-

-   typedef struct {  

-     real_T re;  

-     real_T im;  

-   } creal_T;  

-

-   typedef struct {  

-     int8_T re;  

-     int8_T im;  

-   } cint8_T;  

-

-   typedef struct {  

-     uint8_T re;  

-     uint8_T im;  

-   } cuint8_T;  

-

-   typedef struct {  

-     int16_T re;  

-     int16_T im;  

-   } cint16_T;  

-

-   typedef struct {  

-     uint16_T re;  

-     uint16_T im;  

-   } cuint16_T;  

-

-   typedef struct {  

-     int32_T re;  

-     int32_T im;  

-   } cint32_T;  

-

-   typedef struct {  

-     uint32_T re;  

-     uint32_T im;  

-   } cuint32_T;  

-

-

-/*=======================================================================* 

- * Min and Max:                                                          * 

- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 

- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 

- *=======================================================================*/

-

-#define MAX_int8_T  	((int8_T)(127))

-#define MIN_int8_T  	((int8_T)(-128))

-#define MAX_uint8_T 	((uint8_T)(255))

-#define MIN_uint8_T 	((uint8_T)(0))

-#define MAX_int16_T 	((int16_T)(32767))

-#define MIN_int16_T 	((int16_T)(-32768))

-#define MAX_uint16_T	((uint16_T)(65535))

-#define MIN_uint16_T	((uint16_T)(0))

-#define MAX_int32_T 	((int32_T)(2147483647))

-#define MIN_int32_T 	((int32_T)(-2147483647-1))

-#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))

-#define MIN_uint32_T	((uint32_T)(0))

-

-/* Logical type definitions */

-#if !defined(__cplusplus) && !defined(__true_false_are_keywords)

-#  ifndef false

-#   define false (0U)

-#  endif

-#  ifndef true

-#   define true (1U)

-#  endif

-#endif

-

-/*

- * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation

- * for signed integer values.

- */

-#if ((SCHAR_MIN + 1) != -SCHAR_MAX)

-#error "This code must be compiled using a 2's complement representation for signed integer values"

-#endif

-

-/*

- * Maximum length of a MATLAB identifier (function/variable)

- * including the null-termination character. Referenced by

- * rt_logging.c and rt_matrx.c.

- */

-#define TMW_NAME_LENGTH_MAX	64

-

-#endif

-#endif

-/* End of code generation (rtwtypes.h) */

diff --git a/apps/commander/.context b/apps/commander/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/commander/.context
+++ /dev/null
diff --git a/apps/commander/Makefile b/apps/commander/Makefile
deleted file mode 100644
index d12697274..000000000
--- a/apps/commander/Makefile
+++ /dev/null
@@ -1,45 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Commander application
-#
-
-APPNAME		 = commander
-PRIORITY	 = SCHED_PRIORITY_MAX - 30
-STACKSIZE	 = 2048
-
-INCLUDES	= $(TOPDIR)/../mavlink/include/mavlink
-
-include $(APPDIR)/mk/app.mk
-
diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c
deleted file mode 100644
index a26938637..000000000
--- a/apps/commander/calibration_routines.c
+++ /dev/null
@@ -1,219 +0,0 @@
-/****************************************************************************
- *
- *   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 calibration_routines.c
- * Calibration routines implementations.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <math.h>
-
-#include "calibration_routines.h"
-
-
-int sphere_fit_least_squares(const float x[], const float y[], const float z[],
-			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius)
-{
-
-	float x_sumplain = 0.0f;
-	float x_sumsq = 0.0f;
-	float x_sumcube = 0.0f;
-
-	float y_sumplain = 0.0f;
-	float y_sumsq = 0.0f;
-	float y_sumcube = 0.0f;
-
-	float z_sumplain = 0.0f;
-	float z_sumsq = 0.0f;
-	float z_sumcube = 0.0f;
-
-	float xy_sum = 0.0f;
-	float xz_sum = 0.0f;
-	float yz_sum = 0.0f;
-
-	float x2y_sum = 0.0f;
-	float x2z_sum = 0.0f;
-	float y2x_sum = 0.0f;
-	float y2z_sum = 0.0f;
-	float z2x_sum = 0.0f;
-	float z2y_sum = 0.0f;
-
-	for (unsigned int i = 0; i < size; i++) {
-
-		float x2 = x[i] * x[i];
-		float y2 = y[i] * y[i];
-		float z2 = z[i] * z[i];
-
-		x_sumplain += x[i];
-		x_sumsq += x2;
-		x_sumcube += x2 * x[i];
-
-		y_sumplain += y[i];
-		y_sumsq += y2;
-		y_sumcube += y2 * y[i];
-
-		z_sumplain += z[i];
-		z_sumsq += z2;
-		z_sumcube += z2 * z[i];
-
-		xy_sum += x[i] * y[i];
-		xz_sum += x[i] * z[i];
-		yz_sum += y[i] * z[i];
-
-		x2y_sum += x2 * y[i];
-		x2z_sum += x2 * z[i];
-
-		y2x_sum += y2 * x[i];
-		y2z_sum += y2 * z[i];
-
-		z2x_sum += z2 * x[i];
-		z2y_sum += z2 * y[i];
-	}
-
-	//
-	//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
-	//
-	//    P is a structure that has been computed with the data earlier.
-	//    P.npoints is the number of elements; the length of X,Y,Z are identical.
-	//    P's members are logically named.
-	//
-	//    X[n] is the x component of point n
-	//    Y[n] is the y component of point n
-	//    Z[n] is the z component of point n
-	//
-	//    A is the x coordiante of the sphere
-	//    B is the y coordiante of the sphere
-	//    C is the z coordiante of the sphere
-	//    Rsq is the radius squared of the sphere.
-	//
-	//This method should converge; maybe 5-100 iterations or more.
-	//
-	float x_sum = x_sumplain / size;        //sum( X[n] )
-	float x_sum2 = x_sumsq / size;    //sum( X[n]^2 )
-	float x_sum3 = x_sumcube / size;    //sum( X[n]^3 )
-	float y_sum = y_sumplain / size;        //sum( Y[n] )
-	float y_sum2 = y_sumsq / size;    //sum( Y[n]^2 )
-	float y_sum3 = y_sumcube / size;    //sum( Y[n]^3 )
-	float z_sum = z_sumplain / size;        //sum( Z[n] )
-	float z_sum2 = z_sumsq / size;    //sum( Z[n]^2 )
-	float z_sum3 = z_sumcube / size;    //sum( Z[n]^3 )
-
-	float XY = xy_sum / size;        //sum( X[n] * Y[n] )
-	float XZ = xz_sum / size;        //sum( X[n] * Z[n] )
-	float YZ = yz_sum / size;        //sum( Y[n] * Z[n] )
-	float X2Y = x2y_sum / size;    //sum( X[n]^2 * Y[n] )
-	float X2Z = x2z_sum / size;    //sum( X[n]^2 * Z[n] )
-	float Y2X = y2x_sum / size;    //sum( Y[n]^2 * X[n] )
-	float Y2Z = y2z_sum / size;    //sum( Y[n]^2 * Z[n] )
-	float Z2X = z2x_sum / size;    //sum( Z[n]^2 * X[n] )
-	float Z2Y = z2y_sum / size;    //sum( Z[n]^2 * Y[n] )
-
-	//Reduction of multiplications
-	float F0 = x_sum2 + y_sum2 + z_sum2;
-	float F1 =  0.5f * F0;
-	float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
-	float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
-	float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
-
-	//Set initial conditions:
-	float A = x_sum;
-	float B = y_sum;
-	float C = z_sum;
-
-	//First iteration computation:
-	float A2 = A * A;
-	float B2 = B * B;
-	float C2 = C * C;
-	float QS = A2 + B2 + C2;
-	float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
-
-	//Set initial conditions:
-	float Rsq = F0 + QB + QS;
-
-	//First iteration computation:
-	float Q0 = 0.5f * (QS - Rsq);
-	float Q1 = F1 + Q0;
-	float Q2 = 8.0f * (QS - Rsq + QB + F0);
-	float aA, aB, aC, nA, nB, nC, dA, dB, dC;
-
-	//Iterate N times, ignore stop condition.
-	int n = 0;
-
-	while (n < max_iterations) {
-		n++;
-
-		//Compute denominator:
-		aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
-		aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
-		aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
-		aA = (aA == 0.0f) ? 1.0f : aA;
-		aB = (aB == 0.0f) ? 1.0f : aB;
-		aC = (aC == 0.0f) ? 1.0f : aC;
-
-		//Compute next iteration
-		nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
-		nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
-		nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
-
-		//Check for stop condition
-		dA = (nA - A);
-		dB = (nB - B);
-		dC = (nC - C);
-
-		if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
-
-		//Compute next iteration's values
-		A = nA;
-		B = nB;
-		C = nC;
-		A2 = A * A;
-		B2 = B * B;
-		C2 = C * C;
-		QS = A2 + B2 + C2;
-		QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
-		Rsq = F0 + QB + QS;
-		Q0 = 0.5f * (QS - Rsq);
-		Q1 = F1 + Q0;
-		Q2 = 8.0f * (QS - Rsq + QB + F0);
-	}
-
-	*sphere_x = A;
-	*sphere_y = B;
-	*sphere_z = C;
-	*sphere_radius = sqrtf(Rsq);
-
-	return 0;
-}
diff --git a/apps/commander/calibration_routines.h b/apps/commander/calibration_routines.h
deleted file mode 100644
index e3e7fbafd..000000000
--- a/apps/commander/calibration_routines.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/****************************************************************************
- *
- *   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 calibration_routines.h
- * Calibration routines definitions.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/**
- * Least-squares fit of a sphere to a set of points.
- *
- * Fits a sphere to a set of points on the sphere surface.
- *
- * @param x point coordinates on the X axis
- * @param y point coordinates on the Y axis
- * @param z point coordinates on the Z axis
- * @param size number of points
- * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
- * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
- * @param sphere_x coordinate of the sphere center on the X axis
- * @param sphere_y coordinate of the sphere center on the Y axis
- * @param sphere_z coordinate of the sphere center on the Z axis
- * @param sphere_radius sphere radius
- *
- * @return 0 on success, 1 on failure
- */
-int sphere_fit_least_squares(const float x[], const float y[], const float z[],
-			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
\ No newline at end of file
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
deleted file mode 100644
index 7c0a25f86..000000000
--- a/apps/commander/commander.c
+++ /dev/null
@@ -1,2181 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.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 commander.c
- * Main system state machine implementation.
- *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- *
- */
-
-#include "commander.h"
-
-#include <nuttx/config.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <sys/prctl.h>
-#include <string.h>
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-#include "state_machine_helper.h"
-#include "systemlib/systemlib.h"
-#include <math.h>
-#include <poll.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/subsystem_info.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/differential_pressure.h>
-#include <mavlink/mavlink_log.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-
-/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_mag.h>
-#include <drivers/drv_baro.h>
-
-#include "calibration_routines.h"
-
-
-PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
-//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0);	/**< Go into high-level failsafe after 0 ms */
-PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-
-#include <systemlib/cpuload.h>
-extern struct system_load_s system_load;
-
-/* Decouple update interval and hysteris counters, all depends on intervals */
-#define COMMANDER_MONITORING_INTERVAL 50000
-#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define STICK_ON_OFF_LIMIT 0.75f
-#define STICK_THRUST_RANGE 1.0f
-#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
-#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define GPS_FIX_TYPE_2D 2
-#define GPS_FIX_TYPE_3D 3
-#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
-#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-/* File descriptors */
-static int leds;
-static int buzzer;
-static int mavlink_fd;
-static bool commander_initialized = false;
-static struct vehicle_status_s current_status; /**< Main state machine */
-static orb_advert_t stat_pub;
-
-// static uint16_t nofix_counter = 0;
-// static uint16_t gotfix_counter = 0;
-
-static unsigned int failsafe_lowlevel_timeout_ms;
-
-static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
-static int daemon_task;				/**< Handle of daemon task / thread */
-
-/* pthread loops */
-static void *orb_receive_loop(void *arg);
-
-__EXPORT int commander_main(int argc, char *argv[]);
-
-/**
- * Mainloop of commander.
- */
-int commander_thread_main(int argc, char *argv[]);
-
-static int buzzer_init(void);
-static void buzzer_deinit(void);
-static int led_init(void);
-static void led_deinit(void);
-static int led_toggle(int led);
-static int led_on(int led);
-static int led_off(int led);
-static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);
-static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
-
-
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/**
- * Sort calibration values.
- *
- * Sorts the calibration values with bubble sort.
- *
- * @param a 	The array to sort
- * @param n 	The number of entries in the array
- */
-// static void cal_bsort(float a[], int n);
-
-static int buzzer_init()
-{
-	buzzer = open("/dev/tone_alarm", O_WRONLY);
-
-	if (buzzer < 0) {
-		warnx("Buzzer: open fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void buzzer_deinit()
-{
-	close(buzzer);
-}
-
-
-static int led_init()
-{
-	leds = open(LED_DEVICE_PATH, 0);
-
-	if (leds < 0) {
-		warnx("LED: open fail\n");
-		return ERROR;
-	}
-
-	if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
-		warnx("LED: ioctl fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void led_deinit()
-{
-	close(leds);
-}
-
-static int led_toggle(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_on(int led)
-{
-	return ioctl(leds, LED_ON, led);
-}
-
-static int led_off(int led)
-{
-	return ioctl(leds, LED_OFF, led);
-}
-
-enum AUDIO_PATTERN {
-	AUDIO_PATTERN_ERROR = 2,
-	AUDIO_PATTERN_NOTIFY_POSITIVE = 3,
-	AUDIO_PATTERN_NOTIFY_NEUTRAL = 4,
-	AUDIO_PATTERN_NOTIFY_NEGATIVE = 5,
-	AUDIO_PATTERN_NOTIFY_CHARGE = 6
-};
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
-{
-
-	/* Trigger alarm if going into any error state */
-	if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) ||
-	    ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR);
-	}
-
-	/* Trigger neutral on arming / disarming */
-	if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL);
-	}
-
-	/* Trigger Tetris on being bored */
-
-	return 0;
-}
-
-void tune_confirm(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 3);
-}
-
-void tune_error(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 4);
-}
-
-void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	if (current_status.offboard_control_signal_lost) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
-		return;
-	}
-
-	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp;
-	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
-
-	/* set parameters */
-
-	float p = sp.roll;
-	param_set(param_find("TRIM_ROLL"), &p);
-	p = sp.pitch;
-	param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.yaw;
-	param_set(param_find("TRIM_YAW"), &p);
-
-	/* store to permanent storage */
-	/* auto-save to EEPROM */
-	int save_ret = param_save_default();
-
-	if (save_ret != 0) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
-	}
-
-	mavlink_log_info(mavlink_fd, "trim calibration done");
-}
-
-void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
-{
-
-	/* set to mag calibration mode */
-	status->flag_preflight_mag_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
-	struct mag_report mag;
-
-	/* 45 seconds */
-	uint64_t calibration_interval = 45 * 1000 * 1000;
-
-	/* maximum 2000 values */
-	const unsigned int calibration_maxcount = 500;
-	unsigned int calibration_counter = 0;
-
-	/* limit update rate to get equally spaced measurements over time (in ms) */
-	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
-	// XXX old cal
-	//  * FLT_MIN is not the most negative float number,
-	//  * but the smallest number by magnitude float can
-	//  * represent. Use -FLT_MAX to initialize the most
-	//  * negative number
-
-	// float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
-	// float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
-
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	/* erase old calibration */
-	struct mag_scale mscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set scale / offsets for mag");
-		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
-	}
-
-	/* calibrate range */
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
-		warnx("failed to calibrate scale");
-	}
-
-	close(fd);
-
-	/* calibrate offsets */
-
-	// uint64_t calibration_start = hrt_absolute_time();
-
-	uint64_t axis_deadline = hrt_absolute_time();
-	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
-
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = -1;
-
-	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
-
-	if (x == NULL || y == NULL || z == NULL) {
-		warnx("mag cal failed: out of memory");
-		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
-		warnx("x:%p y:%p z:%p\n", x, y, z);
-		return;
-	}
-
-	tune_confirm();
-	sleep(2);
-	tune_confirm();
-
-	while (hrt_absolute_time() < calibration_deadline &&
-	       calibration_counter < calibration_maxcount) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
-
-		/* user guidance */
-		if (hrt_absolute_time() >= axis_deadline &&
-		    axis_index < 3) {
-
-			axis_index++;
-
-			char buf[50];
-			sprintf(buf, "Please rotate around %c", axislabels[axis_index]);
-			mavlink_log_info(mavlink_fd, buf);
-			tune_confirm();
-
-			axis_deadline += calibration_interval / 3;
-		}
-
-		if (!(axis_index < 3)) {
-			break;
-		}
-
-		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
-
-		// if ((axis_left / 1000) == 0 && axis_left > 0) {
-		// 	char buf[50];
-		// 	sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
-		// 	mavlink_log_info(mavlink_fd, buf);
-		// }
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
-
-			x[calibration_counter] = mag.x;
-			y[calibration_counter] = mag.y;
-			z[calibration_counter] = mag.z;
-
-			/* get min/max values */
-
-			// if (mag.x < mag_min[0]) {
-			// 	mag_min[0] = mag.x;
-			// }
-			// else if (mag.x > mag_max[0]) {
-			// 	mag_max[0] = mag.x;
-			// }
-
-			// if (raw.magnetometer_ga[1] < mag_min[1]) {
-			// 	mag_min[1] = raw.magnetometer_ga[1];
-			// }
-			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
-			// 	mag_max[1] = raw.magnetometer_ga[1];
-			// }
-
-			// if (raw.magnetometer_ga[2] < mag_min[2]) {
-			// 	mag_min[2] = raw.magnetometer_ga[2];
-			// }
-			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
-			// 	mag_max[2] = raw.magnetometer_ga[2];
-			// }
-
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
-			break;
-		}
-	}
-
-	float sphere_x;
-	float sphere_y;
-	float sphere_z;
-	float sphere_radius;
-
-	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
-
-	free(x);
-	free(y);
-	free(z);
-
-	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
-
-		fd = open(MAG_DEVICE_PATH, 0);
-
-		struct mag_scale mscale;
-
-		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to get scale / offsets for mag");
-
-		mscale.x_offset = sphere_x;
-		mscale.y_offset = sphere_y;
-		mscale.z_offset = sphere_z;
-
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to set scale / offsets for mag");
-
-		close(fd);
-
-		/* announce and set new offset */
-
-		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
-			warnx("Setting X mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
-			warnx("Setting Y mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
-			warnx("Setting Z mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
-			warnx("Setting X mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
-			warnx("Setting Y mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
-			warnx("Setting Z mag scale failed!\n");
-		}
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
-		}
-
-		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
-		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
-		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
-
-		char buf[52];
-		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
-			(double)mscale.y_offset, (double)mscale.z_offset);
-		mavlink_log_info(mavlink_fd, buf);
-
-		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
-			(double)mscale.y_scale, (double)mscale.z_scale);
-		mavlink_log_info(mavlink_fd, buf);
-
-		mavlink_log_info(mavlink_fd, "mag calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
-	}
-
-	/* disable calibration mode */
-	status->flag_preflight_mag_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(sub_mag);
-}
-
-void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* set to gyro calibration mode */
-	status->flag_preflight_gyro_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 5000;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
-
-	int calibration_counter = 0;
-	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	/* set offsets to zero */
-	int fd = open(GYRO_DEVICE_PATH, 0);
-	struct gyro_scale gscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
-		warn("WARNING: failed to set scale / offsets for gyro");
-
-	close(fd);
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-			gyro_offset[0] += raw.gyro_rad_s[0];
-			gyro_offset[1] += raw.gyro_rad_s[1];
-			gyro_offset[2] += raw.gyro_rad_s[2];
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
-			return;
-		}
-	}
-
-	gyro_offset[0] = gyro_offset[0] / calibration_count;
-	gyro_offset[1] = gyro_offset[1] / calibration_count;
-	gyro_offset[2] = gyro_offset[2] / calibration_count;
-
-	/* exit gyro calibration mode */
-	status->flag_preflight_gyro_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
-
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
-			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
-			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
-		}
-
-		/* set offsets to actual value */
-		fd = open(GYRO_DEVICE_PATH, 0);
-		struct gyro_scale gscale = {
-			gyro_offset[0],
-			1.0f,
-			gyro_offset[1],
-			1.0f,
-			gyro_offset[2],
-			1.0f,
-		};
-
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		// char buf[50];
-		// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
-		// mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
-	}
-
-	close(sub_sensor_combined);
-}
-
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* announce change */
-
-	mavlink_log_info(mavlink_fd, "keep it level and still");
-	/* set to accel calibration mode */
-	status->flag_preflight_accel_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 2500;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
-
-	int calibration_counter = 0;
-	float accel_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	int fd = open(ACCEL_DEVICE_PATH, 0);
-	struct accel_scale ascale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null))
-		warn("WARNING: failed to set scale / offsets for accel");
-
-	close(fd);
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-			accel_offset[0] += raw.accelerometer_m_s2[0];
-			accel_offset[1] += raw.accelerometer_m_s2[1];
-			accel_offset[2] += raw.accelerometer_m_s2[2];
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "acceleration calibration aborted");
-			return;
-		}
-	}
-
-	accel_offset[0] = accel_offset[0] / calibration_count;
-	accel_offset[1] = accel_offset[1] / calibration_count;
-	accel_offset[2] = accel_offset[2] / calibration_count;
-
-	if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) {
-
-		/* add the removed length from x / y to z, since we induce a scaling issue else */
-		float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]);
-
-		/* if length is correct, zero results here */
-		accel_offset[2] = accel_offset[2] + total_len;
-
-		float scale = 9.80665f / total_len;
-
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))
-			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))
-			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))
-			|| param_set(param_find("SENS_ACC_XSCALE"), &(scale))
-			|| param_set(param_find("SENS_ACC_YSCALE"), &(scale))
-			|| param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!");
-		}
-
-		fd = open(ACCEL_DEVICE_PATH, 0);
-		struct accel_scale ascale = {
-			accel_offset[0],
-			scale,
-			accel_offset[1],
-			scale,
-			accel_offset[2],
-			scale,
-		};
-
-		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
-			warn("WARNING: failed to set scale / offsets for accel");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		//char buf[50];
-		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
-		//mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "accel calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)");
-	}
-
-	/* exit accel calibration mode */
-	status->flag_preflight_accel_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(sub_sensor_combined);
-}
-
-void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* announce change */
-
-	mavlink_log_info(mavlink_fd, "keep it still");
-	/* set to accel calibration mode */
-	status->flag_preflight_airspeed_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 2500;
-
-	int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s differential_pressure;
-
-	int calibration_counter = 0;
-	float airspeed_offset = 0.0f;
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
-			airspeed_offset += differential_pressure.voltage;
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
-			return;
-		}
-	}
-
-	airspeed_offset = airspeed_offset / calibration_count;
-
-	if (isfinite(airspeed_offset)) {
-
-		if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
-		}
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		//char buf[50];
-		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
-		//mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "airspeed calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
-	}
-
-	/* exit airspeed calibration mode */
-	status->flag_preflight_airspeed_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(sub_differential_pressure);
-}
-
-
-
-void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
-{
-	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
-	/* announce command handling */
-	tune_confirm();
-
-
-	/* supported command handling start */
-
-	/* request to set different system mode */
-	switch (cmd->command) {
-	case VEHICLE_CMD_DO_SET_MODE: {
-			if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-			} else {
-				result = VEHICLE_CMD_RESULT_DENIED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			/* request to arm */
-			if ((int)cmd->param1 == 1) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				/* request to disarm */
-
-			} else if ((int)cmd->param1 == 0) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-		/* request for an autopilot reboot */
-	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
-			if ((int)cmd->param1 == 1) {
-				if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
-					/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					/* system may return here */
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-//		/* request to land */
-//		case VEHICLE_CMD_NAV_LAND:
-//		 {
-//				//TODO: add check if landing possible
-//				//TODO: add landing maneuver
-//
-//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//		}		}
-//		break;
-//
-//		/* request to takeoff */
-//		case VEHICLE_CMD_NAV_TAKEOFF:
-//		{
-//			//TODO: add check if takeoff possible
-//			//TODO: add takeoff maneuver
-//
-//			if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
-//				result = VEHICLE_CMD_RESULT_ACCEPTED;
-//			}
-//		}
-//		break;
-//
-		/* preflight calibration */
-	case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
-			bool handled = false;
-
-			/* gyro calibration */
-			if ((int)(cmd->param1) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting gyro cal");
-					tune_confirm();
-					do_gyro_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished gyro cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* magnetometer calibration */
-			if ((int)(cmd->param2) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting mag cal");
-					tune_confirm();
-					do_mag_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished mag cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* zero-altitude pressure calibration */
-			if ((int)(cmd->param3) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
-					tune_confirm();
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* trim calibration */
-			if ((int)(cmd->param4) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting trim cal");
-					tune_confirm();
-					do_rc_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished trim cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* accel calibration */
-			if ((int)(cmd->param5) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
-					tune_confirm();
-					do_accel_calibration(status_pub, &current_status);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* airspeed calibration */
-			if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
-					tune_confirm();
-					do_airspeed_calibration(status_pub, &current_status);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* none found */
-			if (!handled) {
-				//warnx("refusing unsupported calibration request\n");
-				mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request");
-				result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_PREFLIGHT_STORAGE: {
-			if (current_status.flag_system_armed &&
-			    ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
-				/* do not perform expensive memory tasks on multirotors in flight */
-				// XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
-				mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed");
-
-			} else {
-
-				// XXX move this to LOW PRIO THREAD of commander app
-				/* Read all parameters from EEPROM to RAM */
-
-				if (((int)(cmd->param1)) == 0)	{
-
-					/* read all parameters from EEPROM to RAM */
-					int read_ret = param_load_default();
-
-					if (read_ret == OK) {
-						//warnx("[mavlink pm] Loaded EEPROM params in RAM\n");
-						mavlink_log_info(mavlink_fd, "OK loading params from");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else if (read_ret == 1) {
-						mavlink_log_info(mavlink_fd, "OK no changes in");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (read_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR loading params from");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR no param file named");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else if (((int)(cmd->param1)) == 1)	{
-
-					/* write all parameters from RAM to EEPROM */
-					int write_ret = param_save_default();
-
-					if (write_ret == OK) {
-						mavlink_log_info(mavlink_fd, "OK saved param file");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (write_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR params file does not exit:");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR writing params to");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else {
-					mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
-					result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-				}
-			}
-		}
-		break;
-
-	default: {
-			mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
-			result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			/* announce command rejection */
-			ioctl(buzzer, TONE_SET_ALARM, 4);
-		}
-		break;
-	}
-
-	/* supported command handling stop */
-	if (result == VEHICLE_CMD_RESULT_FAILED ||
-	    result == VEHICLE_CMD_RESULT_DENIED ||
-	    result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		ioctl(buzzer, TONE_SET_ALARM, 5);
-
-	} else if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-		tune_confirm();
-	}
-
-	/* send any requested ACKs */
-	if (cmd->confirmation > 0) {
-		/* send acknowledge command */
-		// XXX TODO
-	}
-
-}
-
-static void *orb_receive_loop(void *arg)  //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
-{
-	/* Set thread name */
-	prctl(PR_SET_NAME, "commander orb rcv", getpid());
-
-	/* Subscribe to command topic */
-	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
-	struct subsystem_info_s info;
-
-	struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg;
-
-	while (!thread_should_exit) {
-		struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } };
-
-		if (poll(fds, 1, 5000) == 0) {
-			/* timeout, but this is no problem, silently ignore */
-		} else {
-			/* got command */
-			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
-
-			warnx("Subsys changed: %d\n", (int)info.subsystem_type);
-
-			/* mark / unmark as present */
-			if (info.present) {
-				vstatus->onboard_control_sensors_present |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_present &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as enabled */
-			if (info.enabled) {
-				vstatus->onboard_control_sensors_enabled |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as ok */
-			if (info.ok) {
-				vstatus->onboard_control_sensors_health |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_health &= ~info.subsystem_type;
-			}
-		}
-	}
-
-	close(subsys_sub);
-
-	return NULL;
-}
-
-/*
- * Provides a coarse estimate of remaining battery power.
- *
- * The estimate is very basic and based on decharging voltage curves.
- *
- * @return the estimated remaining capacity in 0..1
- */
-float battery_remaining_estimate_voltage(float voltage);
-
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
-
-float battery_remaining_estimate_voltage(float voltage)
-{
-	float ret = 0;
-	static param_t bat_volt_empty;
-	static param_t bat_volt_full;
-	static param_t bat_n_cells;
-	static bool initialized = false;
-	static unsigned int counter = 0;
-	static float ncells = 3;
-	// XXX change cells to int (and param to INT32)
-
-	if (!initialized) {
-		bat_volt_empty = param_find("BAT_V_EMPTY");
-		bat_volt_full = param_find("BAT_V_FULL");
-		bat_n_cells = param_find("BAT_N_CELLS");
-		initialized = true;
-	}
-
-	static float chemistry_voltage_empty = 3.2f;
-	static float chemistry_voltage_full = 4.05f;
-
-	if (counter % 100 == 0) {
-		param_get(bat_volt_empty, &chemistry_voltage_empty);
-		param_get(bat_volt_full, &chemistry_voltage_full);
-		param_get(bat_n_cells, &ncells);
-	}
-
-	counter++;
-
-	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
-
-	/* limit to sane values */
-	ret = (ret < 0) ? 0 : ret;
-	ret = (ret > 1) ? 1 : ret;
-	return ret;
-}
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
-}
-
-/**
- * The daemon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int commander_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("commander already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		daemon_task = task_spawn("commander",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
-					 3000,
-					 commander_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("\tcommander is running\n");
-
-		} else {
-			warnx("\tcommander not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int commander_thread_main(int argc, char *argv[])
-{
-	/* not yet initialized */
-	commander_initialized = false;
-	bool home_position_set = false;
-
-	/* set parameters */
-	failsafe_lowlevel_timeout_ms = 0;
-	param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
-
-	param_t _param_sys_type = param_find("MAV_TYPE");
-	param_t _param_system_id = param_find("MAV_SYS_ID");
-	param_t _param_component_id = param_find("MAV_COMP_ID");
-
-	/* welcome user */
-	warnx("I am in command now!\n");
-
-	/* pthreads for command and subsystem info handling */
-	// pthread_t command_handling_thread;
-	pthread_t subsystem_info_thread;
-
-	/* initialize */
-	if (led_init() != 0) {
-		warnx("ERROR: Failed to initialize leds\n");
-	}
-
-	if (buzzer_init() != 0) {
-		warnx("ERROR: Failed to initialize buzzer\n");
-	}
-
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
-	}
-
-	/* make sure we are in preflight state */
-	memset(&current_status, 0, sizeof(current_status));
-	current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
-	current_status.flag_system_armed = false;
-	/* neither manual nor offboard control commands have been received */
-	current_status.offboard_control_signal_found_once = false;
-	current_status.rc_signal_found_once = false;
-	/* mark all signals lost as long as they haven't been found */
-	current_status.rc_signal_lost = true;
-	current_status.offboard_control_signal_lost = true;
-	/* allow manual override initially */
-	current_status.flag_external_manual_override_ok = true;
-	/* flag position info as bad, do not allow auto mode */
-	current_status.flag_vector_flight_mode_ok = false;
-	/* set battery warning flag */
-	current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
-
-	/* advertise to ORB */
-	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
-	/* publish current state machine */
-	state_machine_publish(stat_pub, &current_status, mavlink_fd);
-
-	/* home position */
-	orb_advert_t home_pub = -1;
-	struct home_position_s home;
-	memset(&home, 0, sizeof(home));
-
-	if (stat_pub < 0) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		exit(ERROR);
-	}
-
-	mavlink_log_info(mavlink_fd, "system is running");
-
-	/* create pthreads */
-	pthread_attr_t subsystem_info_attr;
-	pthread_attr_init(&subsystem_info_attr);
-	pthread_attr_setstacksize(&subsystem_info_attr, 2048);
-	pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, &current_status);
-
-	/* Start monitoring loop */
-	uint16_t counter = 0;
-	uint8_t flight_env;
-
-	/* Initialize to 0.0V */
-	float battery_voltage = 0.0f;
-	bool battery_voltage_valid = false;
-	bool low_battery_voltage_actions_done = false;
-	bool critical_battery_voltage_actions_done = false;
-	uint8_t low_voltage_counter = 0;
-	uint16_t critical_voltage_counter = 0;
-	int16_t mode_switch_rc_value;
-	float bat_remain = 1.0f;
-
-	uint16_t stick_off_counter = 0;
-	uint16_t stick_on_counter = 0;
-
-	/* Subscribe to manual control data */
-	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp_man;
-	memset(&sp_man, 0, sizeof(sp_man));
-
-	/* Subscribe to offboard control data */
-	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s sp_offboard;
-	memset(&sp_offboard, 0, sizeof(sp_offboard));
-
-	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	struct vehicle_global_position_s global_position;
-	memset(&global_position, 0, sizeof(global_position));
-	uint64_t last_global_position_time = 0;
-
-	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	struct vehicle_local_position_s local_position;
-	memset(&local_position, 0, sizeof(local_position));
-	uint64_t last_local_position_time = 0;
-
-	/* 
-	 * The home position is set based on GPS only, to prevent a dependency between
-	 * position estimator and commander. RAW GPS is more than good enough for a
-	 * non-flying vehicle.
-	 */
-	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	struct vehicle_gps_position_s gps_position;
-	memset(&gps_position, 0, sizeof(gps_position));
-
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s sensors;
-	memset(&sensors, 0, sizeof(sensors));
-
-	int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s differential_pressure;
-	memset(&differential_pressure, 0, sizeof(differential_pressure));
-	uint64_t last_differential_pressure_time = 0;
-
-	/* Subscribe to command topic */
-	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	struct vehicle_command_s cmd;
-	memset(&cmd, 0, sizeof(cmd));
-
-	/* Subscribe to parameters changed topic */
-	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
-	struct parameter_update_s param_changed;
-	memset(&param_changed, 0, sizeof(param_changed));
-
-	/* subscribe to battery topic */
-	int battery_sub = orb_subscribe(ORB_ID(battery_status));
-	struct battery_status_s battery;
-	memset(&battery, 0, sizeof(battery));
-	battery.voltage_v = 0.0f;
-
-	// uint8_t vehicle_state_previous = current_status.state_machine;
-	float voltage_previous = 0.0f;
-
-	uint64_t last_idle_time = 0;
-
-	/* now initialized */
-	commander_initialized = true;
-	thread_running = true;
-
-	uint64_t start_time = hrt_absolute_time();
-	uint64_t failsave_ll_start_time = 0;
-
-	bool state_changed = true;
-	bool param_init_forced = true;
-
-	while (!thread_should_exit) {
-
-		/* Get current values */
-		bool new_data;
-		orb_check(sp_man_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
-		}
-
-		orb_check(sp_offboard_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
-		}
-
-		orb_check(sensor_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
-		}
-
-		orb_check(differential_pressure_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
-			last_differential_pressure_time = differential_pressure.timestamp;
-		}
-
-		orb_check(cmd_sub, &new_data);
-
-		if (new_data) {
-			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
-			/* handle it */
-			handle_command(stat_pub, &current_status, &cmd);
-		}
-
-		/* update parameters */
-		orb_check(param_changed_sub, &new_data);
-
-		if (new_data || param_init_forced) {
-			param_init_forced = false;
-			/* parameters changed */
-			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
-
-
-			/* update parameters */
-			if (!current_status.flag_system_armed) {
-				if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
-					warnx("failed setting new system type");
-				}
-
-				/* disable manual override for all systems that rely on electronic stabilization */
-				if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
-					current_status.flag_external_manual_override_ok = false;
-
-				} else {
-					current_status.flag_external_manual_override_ok = true;
-				}
-
-				/* check and update system / component ID */
-				param_get(_param_system_id, &(current_status.system_id));
-				param_get(_param_component_id, &(current_status.component_id));
-
-			}
-		}
-
-		/* update global position estimate */
-		orb_check(global_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
-			last_global_position_time = global_position.timestamp;
-		}
-
-		/* update local position estimate */
-		orb_check(local_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
-			last_local_position_time = local_position.timestamp;
-		}
-
-		/* update battery status */
-		orb_check(battery_sub, &new_data);
-		if (new_data) {
-			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-			battery_voltage = battery.voltage_v;
-			battery_voltage_valid = true;
-
-			/*
-			 * Only update battery voltage estimate if system has
-			 * been running for two and a half seconds.
-			 */
-			if (hrt_absolute_time() - start_time > 2500000) {
-				bat_remain = battery_remaining_estimate_voltage(battery_voltage);
-			}
-		}
-
-		/* Slow but important 8 Hz checks */
-		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
-			/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
-			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
-			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
-			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-				/* armed */
-				led_toggle(LED_BLUE);
-
-			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				/* not armed */
-				led_toggle(LED_BLUE);
-			}
-
-			/* toggle error led at 5 Hz in HIL mode */
-			if (current_status.flag_hil_enabled) {
-				/* hil enabled */
-				led_toggle(LED_AMBER);
-
-			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
-				/* toggle error (red) at 5 Hz on low battery or error */
-				led_toggle(LED_AMBER);
-
-			} else {
-				// /* Constant error indication in standby mode without GPS */
-				// if (!current_status.gps_valid) {
-				// 	led_on(LED_AMBER);
-
-				// } else {
-				// 	led_off(LED_AMBER);
-				// }
-			}
-
-			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				/* compute system load */
-				uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
-
-				if (last_idle_time > 0)
-					current_status.load = 1000 - (interval_runtime / 1000);	//system load is time spent in non-idle
-
-				last_idle_time = system_load.tasks[0].total_runtime;
-			}
-		}
-
-		// // XXX Export patterns and threshold to parameters
-		/* Trigger audio event for low battery */
-		if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) {
-			/* For less than 10%, start be really annoying at 5 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			ioctl(buzzer, TONE_SET_ALARM, 3);
-
-		} else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) {
-			/* For less than 20%, start be slightly annoying at 1 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			tune_confirm();
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-		}
-
-		/* Check battery voltage */
-		/* write to sys_status */
-		if (battery_voltage_valid) {
-			current_status.voltage_battery = battery_voltage;
-
-		} else {
-			current_status.voltage_battery = 0.0f;
-		}
-
-		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
-
-			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
-			}
-
-			low_voltage_counter++;
-		}
-
-		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
-		else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
-			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
-				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
-			}
-
-			critical_voltage_counter++;
-
-		} else {
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
-		}
-
-		/* End battery voltage check */
-
-
-		/*
-		 * Check for valid position information.
-		 *
-		 * If the system has a valid position source from an onboard
-		 * position estimator, it is safe to operate it autonomously.
-		 * The flag_vector_flight_mode_ok flag indicates that a minimum
-		 * set of position measurements is available.
-		 */
-
-		/* store current state to reason later about a state change */
-		bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
-		bool global_pos_valid = current_status.flag_global_position_valid;
-		bool local_pos_valid = current_status.flag_local_position_valid;
-		bool airspeed_valid = current_status.flag_airspeed_valid;
-
-		/* check for global or local position updates, set a timeout of 2s */
-		if (hrt_absolute_time() - last_global_position_time < 2000000) {
-			current_status.flag_global_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_global_position_valid = false;
-		}
-
-		if (hrt_absolute_time() - last_local_position_time < 2000000) {
-			current_status.flag_local_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_local_position_valid = false;
-		}
-
-		/* Check for valid airspeed/differential pressure measurements */
-		if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
-			current_status.flag_airspeed_valid = true;
-
-		} else {
-			current_status.flag_airspeed_valid = false;
-		}
-
-		/*
-		 * Consolidate global position and local position valid flags
-		 * for vector flight mode.
-		 */
-		if (current_status.flag_local_position_valid ||
-		    current_status.flag_global_position_valid) {
-			current_status.flag_vector_flight_mode_ok = true;
-
-		} else {
-			current_status.flag_vector_flight_mode_ok = false;
-		}
-
-		/* consolidate state change, flag as changed if required */
-		if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
-		    global_pos_valid != current_status.flag_global_position_valid ||
-		    local_pos_valid != current_status.flag_local_position_valid ||
-		    airspeed_valid != current_status.flag_airspeed_valid) {
-			state_changed = true;
-		}
-
-		/*
-		 * Mark the position of the first position lock as return to launch (RTL)
-		 * position. The MAV will return here on command or emergency.
-		 *
-		 * Conditions:
-		 *
-		 * 	1) The system aquired position lock just now
-		 *	2) The system has not aquired position lock before
-		 *	3) The system is not armed (on the ground)
-		 */
-		if (!current_status.flag_valid_launch_position &&
-		    !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok &&
-		    !current_status.flag_system_armed) {
-			/* first time a valid position, store it and emit it */
-
-			// XXX implement storage and publication of RTL position
-			current_status.flag_valid_launch_position = true;
-			current_status.flag_auto_flight_mode_ok = true;
-			state_changed = true;
-		}
-
-		if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) {
-
-			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
-
-			/* check for first, long-term and valid GPS lock -> set home position */
-			float hdop_m = gps_position.eph_m;
-			float vdop_m = gps_position.epv_m;
-
-			/* check if gps fix is ok */
-			// XXX magic number
-			float hdop_threshold_m = 4.0f;
-			float vdop_threshold_m = 8.0f;
-
-			/*
-			 * If horizontal dilution of precision (hdop / eph)
-			 * and vertical diluation of precision (vdop / epv)
-			 * are below a certain threshold (e.g. 4 m), AND
-			 * home position is not yet set AND the last GPS
-			 * GPS measurement is not older than two seconds AND
-			 * the system is currently not armed, set home
-			 * position to the current position.
-			 */
-
-			if (gps_position.fix_type == GPS_FIX_TYPE_3D
-				&& (hdop_m < hdop_threshold_m)
-				&& (vdop_m < vdop_threshold_m)
-				&& !home_position_set
-				&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
-				&& !current_status.flag_system_armed) {
-				warnx("setting home position");
-
-				/* copy position data to uORB home message, store it locally as well */
-				home.lat = gps_position.lat;
-				home.lon = gps_position.lon;
-				home.alt = gps_position.alt;
-
-				home.eph_m = gps_position.eph_m;
-				home.epv_m = gps_position.epv_m;
-
-				home.s_variance_m_s = gps_position.s_variance_m_s;
-				home.p_variance_m = gps_position.p_variance_m;
-
-				/* announce new home position */
-				if (home_pub > 0) {
-					orb_publish(ORB_ID(home_position), home_pub, &home);
-				} else {
-					home_pub = orb_advertise(ORB_ID(home_position), &home);
-				}
-
-				/* mark home position as set */
-				home_position_set = true;
-				tune_confirm();
-			}
-		}
-
-		/* ignore RC signals if in offboard control mode */
-		if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
-			/* Start RC state check */
-
-			if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
-
-				// /*
-				//  * Check if manual control modes have to be switched
-				//  */
-				// if (!isfinite(sp_man.manual_mode_switch)) {
-				// 	warnx("man mode sw not finite\n");
-
-				// 	/* this switch is not properly mapped, set default */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, fall back to direct pass-through */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
-
-				// 	/* bottom stick position, set direct mode for vehicles supporting it */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, set to direct pass-through as requested */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
-
-				// 	/* top stick position, set SAS for all vehicle types */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 	current_status.flag_control_attitude_enabled = true;
-				// 	current_status.flag_control_rates_enabled = true;
-
-				// } else {
-				// 	/* center stick position, set rate control */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
-				// 	current_status.flag_control_attitude_enabled = false;
-				// 	current_status.flag_control_rates_enabled = true;
-				// }
-
-				// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
-
-				/*
-				 * Check if manual stability control modes have to be switched
-				 */
-				if (!isfinite(sp_man.manual_sas_switch)) {
-
-					/* this switch is not properly mapped, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
-				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
-
-					/* bottom stick position, set altitude hold */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
-
-				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
-
-					/* top stick position */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
-
-				} else {
-					/* center stick position, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-				}
-
-				/*
-				 * Check if left stick is in lower left position --> switch to standby state.
-				 * Do this only for multirotors, not for fixed wing aircraft.
-				 */
-				if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
-				    ) &&
-				    ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
-				    (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
-					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_off_counter++;
-						stick_on_counter = 0;
-					}
-				}
-
-				/* check if left stick is in lower right position --> arm */
-				if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
-					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_on_counter++;
-						stick_off_counter = 0;
-					}
-				}
-
-				/* check manual override switch - switch to manual or auto mode */
-				if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
-					/* enable manual override */
-					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
-
-				} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
-					// /* check auto mode switch for correct mode */
-					// if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
-					// 	/* enable guided mode */
-					// 	update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
-
-					// } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
-						// XXX hardcode to auto for now
-						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
-
-					// }
-
-				} else {
-					/* center stick position, set SAS for all vehicle types */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-				}
-
-				/* handle the case where RC signal was regained */
-				if (!current_status.rc_signal_found_once) {
-					current_status.rc_signal_found_once = true;
-					mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
-
-				} else {
-					if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
-				}
-
-				current_status.rc_signal_cutting_off = false;
-				current_status.rc_signal_lost = false;
-				current_status.rc_signal_lost_interval = 0;
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					/* only complain if the offboard control is NOT active */
-					current_status.rc_signal_cutting_off = true;
-					mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
-
-				/* if the RC signal is gone for a full second, consider it lost */
-				if (current_status.rc_signal_lost_interval > 1000000) {
-					current_status.rc_signal_lost = true;
-					current_status.failsave_lowlevel = true;
-					state_changed = true;
-				}
-
-				// if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) {
-				// 	publish_armed_status(&current_status);
-				// }
-			}
-		}
-
-
-
-
-		/* End mode switch */
-
-		/* END RC state check */
-
-
-		/* State machine update for offboard control */
-		if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
-			if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
-
-				/* decide about attitude control flag, enable in att/pos/vel */
-				bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
-
-				/* decide about rate control flag, enable it always XXX (for now) */
-				bool rates_ctrl_enabled = true;
-
-				/* set up control mode */
-				if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
-					current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
-					state_changed = true;
-				}
-
-				if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
-					current_status.flag_control_rates_enabled = rates_ctrl_enabled;
-					state_changed = true;
-				}
-
-				/* handle the case where offboard control signal was regained */
-				if (!current_status.offboard_control_signal_found_once) {
-					current_status.offboard_control_signal_found_once = true;
-					/* enable offboard control, disable manual input */
-					current_status.flag_control_manual_enabled = false;
-					current_status.flag_control_offboard_enabled = true;
-					state_changed = true;
-					tune_confirm();
-
-					mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
-
-				} else {
-					if (current_status.offboard_control_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
-						state_changed = true;
-						tune_confirm();
-					}
-				}
-
-				current_status.offboard_control_signal_weak = false;
-				current_status.offboard_control_signal_lost = false;
-				current_status.offboard_control_signal_lost_interval = 0;
-
-				/* arm / disarm on request */
-				if (sp_offboard.armed && !current_status.flag_system_armed) {
-					update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-					/* switch to stabilized mode = takeoff */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-
-				} else if (!sp_offboard.armed && current_status.flag_system_armed) {
-					update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-				}
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					current_status.offboard_control_signal_weak = true;
-					mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
-
-				/* if the signal is gone for 0.1 seconds, consider it lost */
-				if (current_status.offboard_control_signal_lost_interval > 100000) {
-					current_status.offboard_control_signal_lost = true;
-					current_status.failsave_lowlevel_start_time = hrt_absolute_time();
-					tune_confirm();
-
-					/* kill motors after timeout */
-					if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
-						current_status.failsave_lowlevel = true;
-						state_changed = true;
-					}
-				}
-			}
-		}
-
-
-		current_status.counter++;
-		current_status.timestamp = hrt_absolute_time();
-
-
-		/* If full run came back clean, transition to standby */
-		if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
-		    current_status.flag_preflight_gyro_calibration == false &&
-		    current_status.flag_preflight_mag_calibration == false &&
-		    current_status.flag_preflight_accel_calibration == false) {
-			/* All ok, no calibration going on, go to standby */
-			do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-		}
-
-		/* publish at least with 1 Hz */
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
-			publish_armed_status(&current_status);
-			orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
-			state_changed = false;
-		}
-
-		/* Store old modes to detect and act on state transitions */
-		voltage_previous = current_status.voltage_battery;
-
-		fflush(stdout);
-		counter++;
-		usleep(COMMANDER_MONITORING_INTERVAL);
-	}
-
-	/* wait for threads to complete */
-	// pthread_join(command_handling_thread, NULL);
-	pthread_join(subsystem_info_thread, NULL);
-
-	/* close fds */
-	led_deinit();
-	buzzer_deinit();
-	close(sp_man_sub);
-	close(sp_offboard_sub);
-	close(global_position_sub);
-	close(sensor_sub);
-	close(cmd_sub);
-
-	warnx("exiting..\n");
-	fflush(stdout);
-
-	thread_running = false;
-
-	return 0;
-}
diff --git a/apps/commander/commander.h b/apps/commander/commander.h
deleted file mode 100644
index 04b4e72ab..000000000
--- a/apps/commander/commander.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.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 commander.h
- * Main system state machine definition.
- *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- *
- */
-
-#ifndef COMMANDER_H_
-#define COMMANDER_H_
-
-#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
-#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
-
-void tune_confirm(void);
-void tune_error(void);
-
-#endif /* COMMANDER_H_ */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
deleted file mode 100644
index bea388a10..000000000
--- a/apps/commander/state_machine_helper.c
+++ /dev/null
@@ -1,752 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.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 state_machine_helper.c
- * State machine helper functions implementations
- */
-
-#include <stdio.h>
-#include <unistd.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <systemlib/systemlib.h>
-#include <drivers/drv_hrt.h>
-#include <mavlink/mavlink_log.h>
-
-#include "state_machine_helper.h"
-
-static const char *system_state_txt[] = {
-	"SYSTEM_STATE_PREFLIGHT",
-	"SYSTEM_STATE_STANDBY",
-	"SYSTEM_STATE_GROUND_READY",
-	"SYSTEM_STATE_MANUAL",
-	"SYSTEM_STATE_STABILIZED",
-	"SYSTEM_STATE_AUTO",
-	"SYSTEM_STATE_MISSION_ABORT",
-	"SYSTEM_STATE_EMCY_LANDING",
-	"SYSTEM_STATE_EMCY_CUTOFF",
-	"SYSTEM_STATE_GROUND_ERROR",
-	"SYSTEM_STATE_REBOOT",
-
-};
-
-/**
- * Transition from one state to another
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state)
-{
-	int invalid_state = false;
-	int ret = ERROR;
-
-	commander_state_machine_t old_state = current_status->state_machine;
-
-	switch (new_state) {
-	case SYSTEM_STATE_MISSION_ABORT: {
-			/* Indoor or outdoor */
-			// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-			ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
-
-			// } else {
-			// 	ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
-			// }
-		}
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		/* Tell the controller to land */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		warnx("EMERGENCY LANDING!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		/* Tell the controller to cutoff the motors (thrust = 0) */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = false;
-
-		warnx("EMERGENCY MOTOR CUTOFF!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-
-		/* set system flags according to state */
-
-		/* prevent actuators from arming */
-		current_status->flag_system_armed = false;
-
-		warnx("GROUND ERROR, locking down propulsion system\n");
-		mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
-		break;
-
-	case SYSTEM_STATE_PREFLIGHT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-		    || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
-		}
-
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-			|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
-			|| current_status->flag_hil_enabled) {
-			invalid_state = false;
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
-			usleep(500000);
-			up_systemreset();
-			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
-		}
-
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		/* set system flags according to state */
-
-		/* standby enforces disarmed */
-		current_status->flag_system_armed = false;
-
-		mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-
-		/* set system flags according to state */
-
-		/* ground ready has motors / actuators armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
-		break;
-
-	case SYSTEM_STATE_AUTO:
-
-		/* set system flags according to state */
-
-		/* auto is airborne and in auto mode, motors armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
-		break;
-
-	default:
-		invalid_state = true;
-		break;
-	}
-
-	if (invalid_state == false || old_state != new_state) {
-		current_status->state_machine = new_state;
-		state_machine_publish(status_pub, current_status, mavlink_fd);
-		publish_armed_status(current_status);
-		ret = OK;
-	}
-
-	if (invalid_state) {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* publish the new state */
-	current_status->counter++;
-	current_status->timestamp = hrt_absolute_time();
-
-	/* assemble state vector based on flag values */
-	if (current_status->flag_control_rates_enabled) {
-		current_status->onboard_control_sensors_present |= 0x400;
-
-	} else {
-		current_status->onboard_control_sensors_present &= ~0x400;
-	}
-
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
-}
-
-void publish_armed_status(const struct vehicle_status_s *current_status)
-{
-	struct actuator_armed_s armed;
-	armed.armed = current_status->flag_system_armed;
-	/* lock down actuators if required, only in HIL */
-	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
-	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
-	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
-}
-
-
-/*
- * Private functions, update the state machine
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	warnx("EMERGENCY HANDLER\n");
-	/* Depending on the current state go to one of the error states */
-
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
-
-		// DO NOT abort mission
-		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
-
-	} else {
-		warnx("Unknown system state: #%d\n", current_status->state_machine);
-	}
-}
-
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
-{
-	if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
-		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
-
-	} else {
-		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
-	}
-
-}
-
-
-
-// /*
-//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
-//  */
-
-// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
-//  *
-//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
-//  * */
-
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was removed something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was disabled something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//TODO state machine change (recovering)
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		//TODO state machine change
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-
-// }
-
-
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
-// 	current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// //			//TODO: remove this block
-// //			break;
-// //			///////////////////
-// 		//global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
-
-// //				printf("previosly_healthy = %u\n", previosly_healthy);
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency(status_pub, current_status);
-
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-
-
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		state_machine_emergency(status_pub, current_status, mavlink_fd);
-	}
-}
-
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
-		printf("[cmd] arming\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-	}
-}
-
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		printf("[cmd] going standby\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] MISSION ABORT!\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	int old_mode = current_status->flight_mode;
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-
-	current_status->flag_control_manual_enabled = true;
-
-	/* set behaviour based on airframe */
-	if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-		/* assuming a rotary wing, set to SAS */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-
-	} else {
-
-		/* assuming a fixed wing, set to direct pass-through */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-		current_status->flag_control_attitude_enabled = false;
-		current_status->flag_control_rates_enabled = false;
-	}
-
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] manual mode\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-	}
-}
-
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		int old_mode = current_status->flight_mode;
-		int old_manual_control_mode = current_status->manual_control_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		current_status->flag_control_manual_enabled = true;
-
-		if (old_mode != current_status->flight_mode ||
-		    old_manual_control_mode != current_status->manual_control_mode) {
-			printf("[cmd] att stabilized mode\n");
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-		}
-
-	}
-}
-
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
-		tune_error();
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] position guided mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	}
-}
-
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
-		printf("[cmd] auto mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-	}
-}
-
-
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-{
-	uint8_t ret = 1;
-
-	/* Switch on HIL if in standby and not already in HIL mode */
-	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-	    && !current_status->flag_hil_enabled) {
-		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
-			/* Enable HIL on request */
-			current_status->flag_hil_enabled = true;
-			ret = OK;
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-			publish_armed_status(current_status);
-			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-
-		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-			   current_status->flag_system_armed) {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-
-		} else {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
-		}
-	}
-
-	/* switch manual / auto */
-	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
-		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
-		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
-		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
-	}
-
-	/* vehicle is disarmed, mode requests arming */
-	if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only arm in standby state */
-		// XXX REMOVE
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-			ret = OK;
-			printf("[cmd] arming due to command request\n");
-		}
-	}
-
-	/* vehicle is armed, mode requests disarming */
-	if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only disarm in ground ready */
-		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-			ret = OK;
-			printf("[cmd] disarming due to command request\n");
-		}
-	}
-
-	/* NEVER actually switch off HIL without reboot */
-	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
-		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
-{
-	commander_state_machine_t current_system_state = current_status->state_machine;
-
-	uint8_t ret = 1;
-
-	switch (custom_mode) {
-	case SYSTEM_STATE_GROUND_READY:
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		printf("try to reboot\n");
-
-		if (current_system_state == SYSTEM_STATE_STANDBY
-				|| current_system_state == SYSTEM_STATE_PREFLIGHT
-				|| current_status->flag_hil_enabled) {
-			printf("system will reboot\n");
-			mavlink_log_critical(mavlink_fd, "Rebooting..");
-			usleep(200000);
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		printf("try to switch to auto/takeoff\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-			printf("state: auto\n");
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		printf("try to switch to manual\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			printf("state: manual\n");
-			ret = 0;
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-	return ret;
-}
-
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
deleted file mode 100644
index 2f2ccc729..000000000
--- a/apps/commander/state_machine_helper.h
+++ /dev/null
@@ -1,209 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.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 state_machine_helper.h
- * State machine helper functions definitions
- */
-
-#ifndef STATE_MACHINE_HELPER_H_
-#define STATE_MACHINE_HELPER_H_
-
-#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
-#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-
-/**
- * Switch to new state with no checking.
- *
- * do_state_update: this is the functions that all other functions have to call in order to update the state.
- * the function does not question the state change, this must be done before
- * The function performs actions that are connected with the new state (buzzer, reboot, ...)
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- *
- * @return 0 (macro OK) or 1 on error (macro ERROR)
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
-
-/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-
-
-/**
- * Handle state machine if got position fix
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if position fix lost
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if user wants to arm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if user wants to disarm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is manual
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is stabilized
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is guided
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is auto
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish current state information
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-
-/*
- *  Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
- *  If the request is obeyed the functions return 0
- *
- */
-
-/**
- * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
-
-/**
- * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
-
-/**
- * Always switches to critical mode under any circumstances.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Switches to emergency if required.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish the armed state depending on the current system state
- *
- * @param current_status the current system status
- */
-void publish_armed_status(const struct vehicle_status_s *current_status);
-
-
-
-#endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/apps/drivers/boards/px4fmu/Makefile b/apps/drivers/boards/px4fmu/Makefile
deleted file mode 100644
index 6b183d8d2..000000000
--- a/apps/drivers/boards/px4fmu/Makefile
+++ /dev/null
@@ -1,41 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Board-specific startup code for the PX4FMU
-#
-
-INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-LIBNAME		 = brd_px4fmu
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/boards/px4fmu/px4fmu_can.c b/apps/drivers/boards/px4fmu/px4fmu_can.c
deleted file mode 100644
index 0d0b5fcd3..000000000
--- a/apps/drivers/boards/px4fmu/px4fmu_can.c
+++ /dev/null
@@ -1,141 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_can.c
- *
- * Board-specific CAN functions.
- */
-
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <errno.h>
-#include <debug.h>
-
-#include <nuttx/can.h>
-#include <arch/board/board.h>
-
-#include "chip.h"
-#include "up_arch.h"
-
-#include "stm32.h"
-#include "stm32_can.h"
-#include "px4fmu_internal.h"
-
-/************************************************************************************
- * Pre-processor Definitions
- ************************************************************************************/
-/* Configuration ********************************************************************/
-
-#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
-#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
-#  undef CONFIG_STM32_CAN2
-#endif
-
-#ifdef CONFIG_STM32_CAN1
-#  define CAN_PORT 1
-#else
-#  define CAN_PORT 2
-#endif
-
-/* Debug ***************************************************************************/
-/* Non-standard debug that may be enabled just for testing CAN */
-
-#ifdef CONFIG_DEBUG_CAN
-#  define candbg    dbg
-#  define canvdbg   vdbg
-#  define canlldbg  lldbg
-#  define canllvdbg llvdbg
-#else
-#  define candbg(x...)
-#  define canvdbg(x...)
-#  define canlldbg(x...)
-#  define canllvdbg(x...)
-#endif
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: can_devinit
- *
- * Description:
- *   All STM32 architectures must provide the following interface to work with
- *   examples/can.
- *
- ************************************************************************************/
-
-int can_devinit(void)
-{
-	static bool initialized = false;
-	struct can_dev_s *can;
-	int ret;
-
-	/* Check if we have already initialized */
-
-	if (!initialized) {
-		/* Call stm32_caninitialize() to get an instance of the CAN interface */
-
-		can = stm32_caninitialize(CAN_PORT);
-
-		if (can == NULL) {
-			candbg("ERROR:  Failed to get CAN interface\n");
-			return -ENODEV;
-		}
-
-		/* Register the CAN driver at "/dev/can0" */
-
-		ret = can_register("/dev/can0", can);
-
-		if (ret < 0) {
-			candbg("ERROR: can_register failed: %d\n", ret);
-			return ret;
-		}
-
-		/* Now we are initialized */
-
-		initialized = true;
-	}
-
-	return OK;
-}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c
deleted file mode 100644
index 9960c6bbd..000000000
--- a/apps/drivers/boards/px4fmu/px4fmu_init.c
+++ /dev/null
@@ -1,232 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_init.c
- *
- * PX4FMU-specific early startup code.  This file implements the
- * nsh_archinitialize() function that is called early by nsh during startup.
- *
- * Code here is run before the rcS script is invoked; it should start required
- * subsystems and perform board-specific initialisation.
- */
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdbool.h>
-#include <stdio.h>
-#include <debug.h>
-#include <errno.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/spi.h>
-#include <nuttx/i2c.h>
-#include <nuttx/mmcsd.h>
-#include <nuttx/analog/adc.h>
-
-#include "stm32_internal.h"
-#include "px4fmu_internal.h"
-#include "stm32_uart.h"
-
-#include <arch/board/board.h>
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_led.h>
-
-#include <systemlib/cpuload.h>
-
-/****************************************************************************
- * Pre-Processor Definitions
- ****************************************************************************/
-
-/* Configuration ************************************************************/
-
-/* Debug ********************************************************************/
-
-#ifdef CONFIG_CPP_HAVE_VARARGS
-#  ifdef CONFIG_DEBUG
-#    define message(...) lowsyslog(__VA_ARGS__)
-#  else
-#    define message(...) printf(__VA_ARGS__)
-#  endif
-#else
-#  ifdef CONFIG_DEBUG
-#    define message lowsyslog
-#  else
-#    define message printf
-#  endif
-#endif
-
-/****************************************************************************
- * Protected Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/************************************************************************************
- * Name: stm32_boardinitialize
- *
- * Description:
- *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
- *   and mapped but before any devices have been initialized.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_boardinitialize(void)
-{
-	/* configure SPI interfaces */
-	stm32_spiinitialize();
-
-	/* configure LEDs */
-	up_ledinit();
-}
-
-/****************************************************************************
- * Name: nsh_archinitialize
- *
- * Description:
- *   Perform architecture specific initialization
- *
- ****************************************************************************/
-
-static struct spi_dev_s *spi1;
-static struct spi_dev_s *spi3;
-
-#include <math.h>
-
-#ifdef __cplusplus
-__EXPORT int matherr(struct __exception *e)
-{
-	return 1;
-}
-#else
-__EXPORT int matherr(struct exception *e)
-{
-	return 1;
-}
-#endif
-
-__EXPORT int nsh_archinitialize(void)
-{
-	int result;
-
-	/* configure the high-resolution time/callout interface */
-	hrt_init();
-
-	/* configure CPU load estimation */
-#ifdef CONFIG_SCHED_INSTRUMENTATION
-	cpuload_initialize_once();
-#endif
-
-	/* set up the serial DMA polling */
-	static struct hrt_call serial_dma_call;
-	struct timespec ts;
-
-	/*
-	 * Poll at 1ms intervals for received bytes that have not triggered
-	 * a DMA event.
-	 */
-	ts.tv_sec = 0;
-	ts.tv_nsec = 1000000;
-
-	hrt_call_every(&serial_dma_call,
-		       ts_to_abstime(&ts),
-		       ts_to_abstime(&ts),
-		       (hrt_callout)stm32_serial_dma_poll,
-		       NULL);
-
-	// initial LED state
-	drv_led_start();
-	up_ledoff(LED_BLUE);
-	up_ledoff(LED_AMBER);
-	up_ledon(LED_BLUE);
-
-	/* Configure SPI-based devices */
-
-	spi1 = up_spiinitialize(1);
-
-	if (!spi1) {
-		message("[boot] FAILED to initialize SPI port 1\r\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	// Default SPI1 to 1MHz and de-assert the known chip selects.
-	SPI_SETFREQUENCY(spi1, 10000000);
-	SPI_SETBITS(spi1, 8);
-	SPI_SETMODE(spi1, SPIDEV_MODE3);
-	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
-	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
-	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
-	up_udelay(20);
-
-	message("[boot] Successfully initialized SPI port 1\r\n");
-
-	/* Get the SPI port for the microSD slot */
-
-	message("[boot] Initializing SPI port 3\n");
-	spi3 = up_spiinitialize(3);
-
-	if (!spi3) {
-		message("[boot] FAILED to initialize SPI port 3\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	message("[boot] Successfully initialized SPI port 3\n");
-
-	/* Now bind the SPI interface to the MMCSD driver */
-	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
-
-	if (result != OK) {
-		message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
-
-	stm32_configgpio(GPIO_ADC1_IN10);
-	stm32_configgpio(GPIO_ADC1_IN11);
-	stm32_configgpio(GPIO_ADC1_IN12);
-	stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
-
-	return OK;
-}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_internal.h b/apps/drivers/boards/px4fmu/px4fmu_internal.h
deleted file mode 100644
index 6550fdf3d..000000000
--- a/apps/drivers/boards/px4fmu/px4fmu_internal.h
+++ /dev/null
@@ -1,161 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_internal.h
- *
- * PX4FMU internal definitions
- */
-
-#pragma once
-
-/****************************************************************************************************
- * Included Files
- ****************************************************************************************************/
-
-#include <nuttx/config.h>
-#include <nuttx/compiler.h>
-#include <stdint.h>
-
-__BEGIN_DECLS
-
-/* these headers are not C++ safe */
-#include <stm32_internal.h>
-
- 
-/****************************************************************************************************
- * Definitions
- ****************************************************************************************************/
-/* Configuration ************************************************************************************/
-
-#ifdef CONFIG_STM32_SPI2
-#  error "SPI2 is not supported on this board"
-#endif
-
-#if defined(CONFIG_STM32_CAN1)
-#  warning "CAN1 is not supported on this board"
-#endif
-
-/* PX4FMU GPIOs ***********************************************************************************/
-/* LEDs */
-
-#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
-
-/* External interrupts */
-#define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
-// XXX MPU6000 DRDY?
-
-/* SPI chip selects */
-#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
-#define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
-#define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
-
-/* User GPIOs
- *
- * GPIO0-1 are the buffered high-power GPIOs.
- * GPIO2-5 are the USART2 pins.
- * GPIO6-7 are the CAN2 pins.
- */
-#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
-#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
-#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
-#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
-#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13)
-#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2)
-#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
-#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
-#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
-#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
-#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
-#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
-#define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
-
-/* USB OTG FS
- *
- * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
- */
-#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
-
-/* PWM
- *
- * The PX4FMU has five PWM outputs, of which only the output on
- * pin PC8 is fixed assigned to this function. The other four possible
- * pwm sources are the TX, RX, RTS and CTS pins of USART2
- *
- * Alternate function mapping:
- * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
- */
-
-#ifdef CONFIG_PWM
-#  if defined(CONFIG_STM32_TIM3_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#	 define BUZZER_PWMTIMER 3
-#  elif defined(CONFIG_STM32_TIM8_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#    define BUZZER_PWMTIMER 8
-#  endif
-#endif
-
-/****************************************************************************************************
- * Public Types
- ****************************************************************************************************/
-
-/****************************************************************************************************
- * Public data
- ****************************************************************************************************/
-
-#ifndef __ASSEMBLY__
-
-/****************************************************************************************************
- * Public Functions
- ****************************************************************************************************/
-
-/****************************************************************************************************
- * Name: stm32_spiinitialize
- *
- * Description:
- *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
- *
- ****************************************************************************************************/
-
-extern void stm32_spiinitialize(void);
-
-#endif /* __ASSEMBLY__ */
-
-__END_DECLS
diff --git a/apps/drivers/boards/px4fmu/px4fmu_led.c b/apps/drivers/boards/px4fmu/px4fmu_led.c
deleted file mode 100644
index fd1e159aa..000000000
--- a/apps/drivers/boards/px4fmu/px4fmu_led.c
+++ /dev/null
@@ -1,88 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_led.c
- *
- * PX4FMU LED backend.
- */
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <arch/board/board.h>
-
-#include "chip.h"
-#include "up_arch.h"
-#include "up_internal.h"
-#include "stm32_internal.h"
-#include "px4fmu_internal.h"
-
-__EXPORT void up_ledinit()
-{
-	/* Configure LED1-2 GPIOs for output */
-
-	stm32_configgpio(GPIO_LED1);
-	stm32_configgpio(GPIO_LED2);
-}
-
-__EXPORT void up_ledon(int led)
-{
-	if (led == 0)
-	{
-		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED1, false);
-	}
-	if (led == 1)
-	{
-		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED2, false);
-	}
-}
-
-__EXPORT void up_ledoff(int led)
-{
-	if (led == 0)
-	{
-		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED1, true);
-	}
-	if (led == 1)
-	{
-		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED2, true);
-	}
-}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c
deleted file mode 100644
index cb8918306..000000000
--- a/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_pwm_servo.c
- *
- * Configuration data for the stm32 pwm_servo driver.
- *
- * Note that these arrays must always be fully-sized.
- */
-
-#include <stdint.h>
-
-#include <drivers/stm32/drv_pwm_servo.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_pwm_output.h>
-
-#include <stm32_internal.h>
-#include <stm32_gpio.h>
-#include <stm32_tim.h>
-
-__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
-	{
-		.base = STM32_TIM2_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM2EN,
-		.clock_freq = STM32_APB1_TIM2_CLKIN
-	}
-};
-
-__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
-	{
-		.gpio = GPIO_TIM2_CH1OUT,
-		.timer_index = 0,
-		.timer_channel = 1,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH2OUT,
-		.timer_index = 0,
-		.timer_channel = 2,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH3OUT,
-		.timer_index = 0,
-		.timer_channel = 3,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH4OUT,
-		.timer_index = 0,
-		.timer_channel = 4,
-		.default_value = 1000,
-	}
-};
diff --git a/apps/drivers/boards/px4fmu/px4fmu_spi.c b/apps/drivers/boards/px4fmu/px4fmu_spi.c
deleted file mode 100644
index 7a02eaeb7..000000000
--- a/apps/drivers/boards/px4fmu/px4fmu_spi.c
+++ /dev/null
@@ -1,136 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_spi.c
- *
- * Board-specific SPI functions.
- */
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <nuttx/spi.h>
-#include <arch/board/board.h>
-
-#include "up_arch.h"
-#include "chip.h"
-#include "stm32_internal.h"
-#include "px4fmu_internal.h"
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: stm32_spiinitialize
- *
- * Description:
- *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
- *
- ************************************************************************************/
-
-__EXPORT void weak_function stm32_spiinitialize(void)
-{
-	stm32_configgpio(GPIO_SPI_CS_GYRO);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL);
-	stm32_configgpio(GPIO_SPI_CS_MPU);
-	stm32_configgpio(GPIO_SPI_CS_SDCARD);
-
-	/* De-activate all peripherals,
-	 * required for some peripheral
-	 * state machines
-	 */
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
-}
-
-__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
-{
-	/* SPI select is active low, so write !selected to select the device */
-
-	switch (devid) {
-	case PX4_SPIDEV_GYRO:
-		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
-		break;
-
-	case PX4_SPIDEV_ACCEL:
-		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		break;
-
-	case PX4_SPIDEV_MPU:
-		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
-		break;
-
-	default:
-		break;
-
-	}
-}
-
-__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
-{
-	return SPI_STATUS_PRESENT;
-}
-
-
-__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
-{
-	/* there can only be one device on this bus, so always select it */
-	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
-}
-
-__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
-{
-	/* this is actually bogus, but PX4 has no way to sense the presence of an SD card */
-	return SPI_STATUS_PRESENT;
-}
-
diff --git a/apps/drivers/boards/px4fmu/px4fmu_usb.c b/apps/drivers/boards/px4fmu/px4fmu_usb.c
deleted file mode 100644
index b0b669fbe..000000000
--- a/apps/drivers/boards/px4fmu/px4fmu_usb.c
+++ /dev/null
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_usb.c
- *
- * Board-specific USB functions.
- */
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <nuttx/usb/usbdev.h>
-#include <nuttx/usb/usbdev_trace.h>
-
-#include "up_arch.h"
-#include "stm32_internal.h"
-#include "px4fmu_internal.h"
-
-/************************************************************************************
- * Definitions
- ************************************************************************************/
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: stm32_usbinitialize
- *
- * Description:
- *   Called to setup USB-related GPIO pins for the PX4FMU board.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_usbinitialize(void)
-{
-	/* The OTG FS has an internal soft pull-up */
-
-	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
-
-#ifdef CONFIG_STM32_OTGFS
-	stm32_configgpio(GPIO_OTGFS_VBUS);
-	/* XXX We only support device mode
-	stm32_configgpio(GPIO_OTGFS_PWRON);
-	stm32_configgpio(GPIO_OTGFS_OVER);
-	*/
-#endif
-}
-
-/************************************************************************************
- * Name:  stm32_usbsuspend
- *
- * Description:
- *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
- *   used.  This function is called whenever the USB enters or leaves suspend mode.
- *   This is an opportunity for the board logic to shutdown clocks, power, etc.
- *   while the USB is suspended.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
-{
-	ulldbg("resume: %d\n", resume);
-}
-
diff --git a/apps/drivers/l3gd20/Makefile b/apps/drivers/l3gd20/Makefile
deleted file mode 100644
index 98e2d57c5..000000000
--- a/apps/drivers/l3gd20/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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 L3GD20 driver.
-#
-
-APPNAME		 = l3gd20
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp
deleted file mode 100644
index c7f433dd4..000000000
--- a/apps/drivers/l3gd20/l3gd20.cpp
+++ /dev/null
@@ -1,887 +0,0 @@
-/****************************************************************************
- *
- *   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 Driver for the ST L3GD20 MEMS gyro connected via SPI.
- */
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/clock.h>
-
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-
-#include <drivers/device/spi.h>
-#include <drivers/drv_gyro.h>
-
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-/* SPI protocol address bits */
-#define DIR_READ				(1<<7)
-#define DIR_WRITE				(0<<7)
-#define ADDR_INCREMENT				(1<<6)
-
-/* register addresses */
-#define ADDR_WHO_AM_I			0x0F
-#define WHO_I_AM				0xD4
-
-#define ADDR_CTRL_REG1			0x20
-#define REG1_RATE_LP_MASK			0xF0 /* Mask to guard partial register update */
-/* keep lowpass low to avoid noise issues */
-#define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
-#define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
-#define RATE_380HZ_LP_30HZ		((1<<7) | (0<<6) | (1<<5) | (1<<4))
-#define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
-
-#define ADDR_CTRL_REG2			0x21
-#define ADDR_CTRL_REG3			0x22
-#define ADDR_CTRL_REG4			0x23
-#define REG4_RANGE_MASK				0x30 /* Mask to guard partial register update */
-#define RANGE_250DPS				(0<<4)
-#define RANGE_500DPS				(1<<4)
-#define RANGE_2000DPS				(3<<4)
-
-#define ADDR_CTRL_REG5			0x24
-#define ADDR_REFERENCE			0x25
-#define ADDR_OUT_TEMP			0x26
-#define ADDR_STATUS_REG			0x27
-#define ADDR_OUT_X_L			0x28
-#define ADDR_OUT_X_H			0x29
-#define ADDR_OUT_Y_L			0x2A
-#define ADDR_OUT_Y_H			0x2B
-#define ADDR_OUT_Z_L			0x2C
-#define ADDR_OUT_Z_H			0x2D
-#define ADDR_FIFO_CTRL_REG		0x2E
-#define ADDR_FIFO_SRC_REG		0x2F
-#define ADDR_INT1_CFG			0x30
-#define ADDR_INT1_SRC			0x31
-#define ADDR_INT1_TSH_XH		0x32
-#define ADDR_INT1_TSH_XL		0x33
-#define ADDR_INT1_TSH_YH		0x34
-#define ADDR_INT1_TSH_YL		0x35
-#define ADDR_INT1_TSH_ZH		0x36
-#define ADDR_INT1_TSH_ZL		0x37
-#define ADDR_INT1_DURATION		0x38
-
-
-/* Internal configuration values */
-#define REG1_POWER_NORMAL			(1<<3)
-#define REG1_Z_ENABLE				(1<<2)
-#define REG1_Y_ENABLE				(1<<1)
-#define REG1_X_ENABLE				(1<<0)
-
-#define REG4_BDU				(1<<7)
-#define REG4_BLE				(1<<6)
-//#define REG4_SPI_3WIRE			(1<<0)
-
-#define REG5_FIFO_ENABLE			(1<<6)
-#define REG5_REBOOT_MEMORY			(1<<7)
-
-#define STATUS_ZYXOR				(1<<7)
-#define STATUS_ZOR				(1<<6)
-#define STATUS_YOR				(1<<5)
-#define STATUS_XOR				(1<<4)
-#define STATUS_ZYXDA				(1<<3)
-#define STATUS_ZDA				(1<<2)
-#define STATUS_YDA				(1<<1)
-#define STATUS_XDA				(1<<0)
-
-#define FIFO_CTRL_BYPASS_MODE			(0<<5)
-#define FIFO_CTRL_FIFO_MODE			(1<<5)
-#define FIFO_CTRL_STREAM_MODE			(1<<6)
-#define FIFO_CTRL_STREAM_TO_FIFO_MODE		(3<<5)
-#define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
-
-extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
-
-class L3GD20 : public device::SPI
-{
-public:
-	L3GD20(int bus, const char* path, spi_dev_e device);
-	virtual ~L3GD20();
-
-	virtual int		init();
-
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	void			print_info();
-
-protected:
-	virtual int		probe();
-
-private:
-
-	struct hrt_call		_call;
-	unsigned		_call_interval;
-
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct gyro_report	*_reports;
-
-	struct gyro_scale	_gyro_scale;
-	float			_gyro_range_scale;
-	float			_gyro_range_rad_s;
-	orb_advert_t		_gyro_topic;
-
-	unsigned		_current_rate;
-	unsigned		_current_range;
-
-	perf_counter_t		_sample_perf;
-
-	/**
-	 * Start automatic measurement.
-	 */
-	void			start();
-
-	/**
-	 * Stop automatic measurement.
-	 */
-	void			stop();
-
-	/**
-	 * Static trampoline from the hrt_call context; because we don't have a
-	 * generic hrt wrapper yet.
-	 *
-	 * Called by the HRT in interrupt context at the specified rate if
-	 * automatic polling is enabled.
-	 *
-	 * @param arg		Instance pointer for the driver that is polling.
-	 */
-	static void		measure_trampoline(void *arg);
-
-	/**
-	 * Fetch measurements from the sensor and update the report ring.
-	 */
-	void			measure();
-
-	/**
-	 * Read a register from the L3GD20
-	 *
-	 * @param		The register to read.
-	 * @return		The value that was read.
-	 */
-	uint8_t			read_reg(unsigned reg);
-
-	/**
-	 * Write a register in the L3GD20
-	 *
-	 * @param reg		The register to write.
-	 * @param value		The new value to write.
-	 */
-	void			write_reg(unsigned reg, uint8_t value);
-
-	/**
-	 * Modify a register in the L3GD20
-	 *
-	 * Bits are cleared before bits are set.
-	 *
-	 * @param reg		The register to modify.
-	 * @param clearbits	Bits in the register to clear.
-	 * @param setbits	Bits in the register to set.
-	 */
-	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
-
-	/**
-	 * Set the L3GD20 measurement range.
-	 *
-	 * @param max_dps	The measurement range is set to permit reading at least
-	 *			this rate in degrees per second.
-	 *			Zero selects the maximum supported range.
-	 * @return		OK if the value can be supported, -ERANGE otherwise.
-	 */
-	int			set_range(unsigned max_dps);
-
-	/**
-	 * Set the L3GD20 internal sampling frequency.
-	 *
-	 * @param frequency	The internal sampling frequency is set to not less than
-	 *			this value.
-	 *			Zero selects the maximum rate supported.
-	 * @return		OK if the value can be supported.
-	 */
-	int			set_samplerate(unsigned frequency);
-};
-
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-
-L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
-	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
-	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
-	_reports(nullptr),
-	_gyro_range_scale(0.0f),
-	_gyro_range_rad_s(0.0f),
-	_gyro_topic(-1),
-	_current_rate(0),
-	_current_range(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read"))
-{
-	// enable debug() calls
-	_debug_enabled = true;
-
-	// default scale factors
-	_gyro_scale.x_offset = 0;
-	_gyro_scale.x_scale  = 1.0f;
-	_gyro_scale.y_offset = 0;
-	_gyro_scale.y_scale  = 1.0f;
-	_gyro_scale.z_offset = 0;
-	_gyro_scale.z_scale  = 1.0f;
-}
-
-L3GD20::~L3GD20()
-{
-	/* make sure we are truly inactive */
-	stop();
-
-	/* free any existing reports */
-	if (_reports != nullptr)
-		delete[] _reports;
-
-	/* delete the perf counter */
-	perf_free(_sample_perf);
-}
-
-int
-L3GD20::init()
-{
-	int ret = ERROR;
-
-	/* do SPI init (and probe) first */
-	if (SPI::init() != OK)
-		goto out;
-
-	/* allocate basic report buffers */
-	_num_reports = 2;
-	_oldest_report = _next_report = 0;
-	_reports = new struct gyro_report[_num_reports];
-
-	if (_reports == nullptr)
-		goto out;
-
-	/* advertise sensor topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
-
-	/* set default configuration */
-	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
-	write_reg(ADDR_CTRL_REG4, REG4_BDU);
-	write_reg(ADDR_CTRL_REG5, 0);
-
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);	  /* disable wake-on-interrupt */
-	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
-
-	set_range(500);				/* default to 500dps */
-	set_samplerate(0);			/* max sample rate */
-
-	ret = OK;
-out:
-	return ret;
-}
-
-int
-L3GD20::probe()
-{
-	/* read dummy value to void to clear SPI statemachine on sensor */
-	(void)read_reg(ADDR_WHO_AM_I);
-
-	/* verify that the device is attached and functioning */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
-		return OK;
-
-	return -EIO;
-}
-
-ssize_t
-L3GD20::read(struct file *filp, char *buffer, size_t buflen)
-{
-	unsigned count = buflen / sizeof(struct gyro_report);
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (count < 1)
-		return -ENOSPC;
-
-	/* if automatic measurement is enabled */
-	if (_call_interval > 0) {
-
-		/*
-		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the measurement code while we are doing this;
-		 * we are careful to avoid racing with it.
-		 */
-		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
-			}
-		}
-
-		/* if there was no data, warn the caller */
-		return ret ? ret : -EAGAIN;
-	}
-
-	/* manual measurement */
-	_oldest_report = _next_report = 0;
-	measure();
-
-	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _reports, sizeof(*_reports));
-	ret = sizeof(*_reports);
-
-	return ret;
-}
-
-int
-L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_call_interval = 0;
-				return OK;
-
-				/* external signalling not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
-
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT:
-				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
-
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_call_interval == 0);
-
-					/* convert hz to hrt interval via microseconds */
-					unsigned ticks = 1000000 / arg;
-
-					/* check against maximum sane rate */
-					if (ticks < 1000)
-						return -EINVAL;
-
-					/* update interval for next measurement */
-					/* XXX this is a bit shady, but no other way to adjust... */
-					_call.period = _call_interval = ticks;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
-
-					return OK;
-				}
-			}
-		}
-
-	case SENSORIOCGPOLLRATE:
-		if (_call_interval == 0)
-			return SENSOR_POLLRATE_MANUAL;
-
-		return 1000000 / _call_interval;
-
-	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct gyro_report *buf = new struct gyro_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
-		}
-
-	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
-
-	case SENSORIOCRESET:
-		/* XXX implement */
-		return -EINVAL;
-
-	case GYROIOCSSAMPLERATE:
-		return set_samplerate(arg);
-
-	case GYROIOCGSAMPLERATE:
-		return _current_rate;
-
-	case GYROIOCSLOWPASS:
-	case GYROIOCGLOWPASS:
-		/* XXX not implemented due to wacky interaction with samplerate */
-		return -EINVAL;
-
-	case GYROIOCSSCALE:
-		/* copy scale in */
-		memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
-		return OK;
-
-	case GYROIOCGSCALE:
-		/* copy scale out */
-		memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
-		return OK;
-
-	case GYROIOCSRANGE:
-		return set_range(arg);
-
-	case GYROIOCGRANGE:
-		return _current_range;
-
-	default:
-		/* give it to the superclass */
-		return SPI::ioctl(filp, cmd, arg);
-	}
-}
-
-uint8_t
-L3GD20::read_reg(unsigned reg)
-{
-	uint8_t cmd[2];
-
-	cmd[0] = reg | DIR_READ;
-
-	transfer(cmd, cmd, sizeof(cmd));
-
-	return cmd[1];
-}
-
-void
-L3GD20::write_reg(unsigned reg, uint8_t value)
-{
-	uint8_t	cmd[2];
-
-	cmd[0] = reg | DIR_WRITE;
-	cmd[1] = value;
-
-	transfer(cmd, nullptr, sizeof(cmd));
-}
-
-void
-L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
-{
-	uint8_t	val;
-
-	val = read_reg(reg);
-	val &= ~clearbits;
-	val |= setbits;
-	write_reg(reg, val);
-}
-
-int
-L3GD20::set_range(unsigned max_dps)
-{
-	uint8_t bits = REG4_BDU;
-
-	if (max_dps == 0)
-		max_dps = 2000;
-
-	if (max_dps <= 250) {
-		_current_range = 250;
-		bits |= RANGE_250DPS;
-
-	} else if (max_dps <= 500) {
-		_current_range = 500;
-		bits |= RANGE_500DPS;
-
-	} else if (max_dps <= 2000) {
-		_current_range = 2000;
-		bits |= RANGE_2000DPS;
-
-	} else {
-		return -EINVAL;
-	}
-
-	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
-	_gyro_range_scale = _gyro_range_rad_s / 32768.0f;
-	write_reg(ADDR_CTRL_REG4, bits);
-
-	return OK;
-}
-
-int
-L3GD20::set_samplerate(unsigned frequency)
-{
-	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
-
-	if (frequency == 0)
-		frequency = 760;
-
-	if (frequency <= 95) {
-		_current_rate = 95;
-		bits |= RATE_95HZ_LP_25HZ;
-
-	} else if (frequency <= 190) {
-		_current_rate = 190;
-		bits |= RATE_190HZ_LP_25HZ;
-
-	} else if (frequency <= 380) {
-		_current_rate = 380;
-		bits |= RATE_380HZ_LP_30HZ;
-
-	} else if (frequency <= 760) {
-		_current_rate = 760;
-		bits |= RATE_760HZ_LP_30HZ;
-
-	} else {
-		return -EINVAL;
-	}
-
-	write_reg(ADDR_CTRL_REG1, bits);
-
-	return OK;
-}
-
-void
-L3GD20::start()
-{
-	/* make sure we are stopped first */
-	stop();
-
-	/* reset the report ring */
-	_oldest_report = _next_report = 0;
-
-	/* start polling at the specified rate */
-	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
-}
-
-void
-L3GD20::stop()
-{
-	hrt_cancel(&_call);
-}
-
-void
-L3GD20::measure_trampoline(void *arg)
-{
-	L3GD20 *dev = (L3GD20 *)arg;
-
-	/* make another measurement */
-	dev->measure();
-}
-
-void
-L3GD20::measure()
-{
-	/* status register and data as read back from the device */
-#pragma pack(push, 1)
-	struct {
-		uint8_t		cmd;
-		uint8_t		temp;
-		uint8_t		status;
-		int16_t		x;
-		int16_t		y;
-		int16_t		z;
-	} raw_report;
-#pragma pack(pop)
-
-	gyro_report		*report = &_reports[_next_report];
-
-	/* start the performance counter */
-	perf_begin(_sample_perf);
-
-	/* fetch data from the sensor */
-	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
-	transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
-
-	/*
-	 * 1) Scale raw value to SI units using scaling from datasheet.
-	 * 2) Subtract static offset (in SI units)
-	 * 3) Scale the statically calibrated values with a linear
-	 *    dynamically obtained factor
-	 *
-	 * Note: the static sensor offset is the number the sensor outputs
-	 * 	 at a nominally 'zero' input. Therefore the offset has to
-	 * 	 be subtracted.
-	 *
-	 *	 Example: A gyro outputs a value of 74 at zero angular rate
-	 *	 	  the offset is 74 from the origin and subtracting
-	 *		  74 from all measurements centers them around zero.
-	 */
-	report->timestamp = hrt_absolute_time();
-	
-	/* swap x and y and negate y */
-	report->x_raw = raw_report.y;
-	report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
-	report->z_raw = raw_report.z;
-
-	report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-	report->scaling = _gyro_range_scale;
-	report->range_rad_s = _gyro_range_rad_s;
-
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
-
-	/* if we are running up against the oldest report, fix it */
-	if (_next_report == _oldest_report)
-		INCREMENT(_oldest_report, _num_reports);
-
-	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
-
-	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
-
-	/* stop the perf counter */
-	perf_end(_sample_perf);
-}
-
-void
-L3GD20::print_info()
-{
-	perf_print_counter(_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace l3gd20
-{
-
-L3GD20	*g_dev;
-
-void	start();
-void	test();
-void	reset();
-void	info();
-
-/**
- * Start the driver.
- */
-void
-start()
-{
-	int fd;
-
-	if (g_dev != nullptr)
-		errx(1, "already started");
-
-	/* create the driver */
-	g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
-
-	if (g_dev == nullptr)
-		goto fail;
-
-	if (OK != g_dev->init())
-		goto fail;
-
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		goto fail;
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		goto fail;
-
-	exit(0);
-fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	errx(1, "driver start failed");
-}
-
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-void
-test()
-{
-	int fd_gyro = -1;
-	struct gyro_report g_report;
-	ssize_t sz;
-
-	/* get the driver */
-	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
-
-	if (fd_gyro < 0)
-		err(1, "%s open failed", GYRO_DEVICE_PATH);
-
-	/* reset to manual polling */
-	if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
-		err(1, "reset to manual polling");
-
-	/* do a simple demand read */
-	sz = read(fd_gyro, &g_report, sizeof(g_report));
-
-	if (sz != sizeof(g_report))
-		err(1, "immediate gyro read failed");
-
-	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
-	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
-	warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
-	warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
-	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
-	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
-	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
-	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
-
-	/* XXX add poll-rate tests here too */
-
-	reset();
-	errx(0, "PASS");
-}
-
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "failed ");
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
-		err(1, "driver reset failed");
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
-
-	exit(0);
-}
-
-/**
- * Print a little info about the driver.
- */
-void
-info()
-{
-	if (g_dev == nullptr)
-		errx(1, "driver not running\n");
-
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
-
-	exit(0);
-}
-
-
-} // namespace
-
-int
-l3gd20_main(int argc, char *argv[])
-{
-	/*
-	 * Start/load the driver.
-
-	 */
-	if (!strcmp(argv[1], "start"))
-		l3gd20::start();
-
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[1], "test"))
-		l3gd20::test();
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[1], "reset"))
-		l3gd20::reset();
-
-	/*
-	 * Print driver information.
-	 */
-	if (!strcmp(argv[1], "info"))
-		l3gd20::info();
-
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
-}
diff --git a/apps/drivers/px4fmu/Makefile b/apps/drivers/px4fmu/Makefile
deleted file mode 100644
index 7f7c2a732..000000000
--- a/apps/drivers/px4fmu/Makefile
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Interface driver for the PX4FMU board
-#
-
-APPNAME		 = fmu
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
deleted file mode 100644
index e54724536..000000000
--- a/apps/drivers/px4fmu/fmu.cpp
+++ /dev/null
@@ -1,1084 +0,0 @@
-/****************************************************************************
- *
- *   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 fmu.cpp
- *
- * Driver/configurator for the PX4 FMU multi-purpose port.
- */
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <nuttx/arch.h>
-
-#include <drivers/device/device.h>
-#include <drivers/drv_pwm_output.h>
-#include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
-#include <drivers/drv_hrt.h>
-
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-#include <systemlib/mixer/mixer.h>
-#include <drivers/drv_mixer.h>
-#include <drivers/drv_rc_input.h>
-
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
-#include <uORB/topics/actuator_outputs.h>
-
-#include <systemlib/err.h>
-#include <systemlib/ppm_decode.h>
-
-class PX4FMU : public device::CDev
-{
-public:
-	enum Mode {
-		MODE_2PWM,
-		MODE_4PWM,
-		MODE_NONE
-	};
-	PX4FMU();
-	virtual ~PX4FMU();
-
-	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
-	virtual ssize_t	write(file *filp, const char *buffer, size_t len);
-
-	virtual int	init();
-
-	int		set_mode(Mode mode);
-
-	int		set_pwm_alt_rate(unsigned rate);
-	int		set_pwm_alt_channels(uint32_t channels);
-
-private:
-	static const unsigned _max_actuators = 4;
-
-	Mode		_mode;
-	unsigned	_pwm_default_rate;
-	unsigned	_pwm_alt_rate;
-	uint32_t	_pwm_alt_rate_channels;
-	unsigned	_current_update_rate;
-	int		_task;
-	int		_t_actuators;
-	int		_t_armed;
-	orb_advert_t	_t_outputs;
-	orb_advert_t	_t_actuators_effective;
-	unsigned	_num_outputs;
-	bool		_primary_pwm_device;
-
-	volatile bool	_task_should_exit;
-	bool		_armed;
-
-	MixerGroup	*_mixers;
-
-	actuator_controls_s _controls;
-
-	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
-
-	static int	control_callback(uintptr_t handle,
-					 uint8_t control_group,
-					 uint8_t control_index,
-					 float &input);
-
-	int		set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
-	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
-
-	struct GPIOConfig {
-		uint32_t	input;
-		uint32_t	output;
-		uint32_t	alt;
-	};
-
-	static const GPIOConfig	_gpio_tab[];
-	static const unsigned	_ngpio;
-
-	void		gpio_reset(void);
-	void		gpio_set_function(uint32_t gpios, int function);
-	void		gpio_write(uint32_t gpios, int function);
-	uint32_t	gpio_read(void);
-	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
-
-};
-
-const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
-	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
-	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
-	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
-	{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
-	{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
-	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
-	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
-	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
-};
-
-const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
-
-namespace
-{
-
-PX4FMU	*g_fmu;
-
-} // namespace
-
-PX4FMU::PX4FMU() :
-	CDev("fmuservo", "/dev/px4fmu"),
-	_mode(MODE_NONE),
-	_pwm_default_rate(50),
-	_pwm_alt_rate(50),
-	_pwm_alt_rate_channels(0),
-	_current_update_rate(0),
-	_task(-1),
-	_t_actuators(-1),
-	_t_armed(-1),
-	_t_outputs(0),
-	_t_actuators_effective(0),
-	_num_outputs(0),
-	_primary_pwm_device(false),
-	_task_should_exit(false),
-	_armed(false),
-	_mixers(nullptr)
-{
-	_debug_enabled = true;
-}
-
-PX4FMU::~PX4FMU()
-{
-	if (_task != -1) {
-		/* tell the task we want it to go away */
-		_task_should_exit = true;
-
-		unsigned i = 10;
-
-		do {
-			/* wait 50ms - it should wake every 100ms or so worst-case */
-			usleep(50000);
-
-			/* if we have given up, kill it */
-			if (--i == 0) {
-				task_delete(_task);
-				break;
-			}
-
-		} while (_task != -1);
-	}
-
-	/* clean up the alternate device node */
-	if (_primary_pwm_device)
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
-
-	g_fmu = nullptr;
-}
-
-int
-PX4FMU::init()
-{
-	int ret;
-
-	ASSERT(_task == -1);
-
-	/* do regular cdev init */
-	ret = CDev::init();
-
-	if (ret != OK)
-		return ret;
-
-	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
-
-	if (ret == OK) {
-		log("default PWM output device");
-		_primary_pwm_device = true;
-	}
-
-	/* reset GPIOs */
-	gpio_reset();
-
-	/* start the IO interface task */
-	_task = task_spawn("fmuservo",
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_DEFAULT,
-			   2048,
-			   (main_t)&PX4FMU::task_main_trampoline,
-			   nullptr);
-
-	if (_task < 0) {
-		debug("task start failed: %d", errno);
-		return -errno;
-	}
-
-	return OK;
-}
-
-void
-PX4FMU::task_main_trampoline(int argc, char *argv[])
-{
-	g_fmu->task_main();
-}
-
-int
-PX4FMU::set_mode(Mode mode)
-{
-	/*
-	 * Configure for PWM output.
-	 *
-	 * Note that regardless of the configured mode, the task is always
-	 * listening and mixing; the mode just selects which of the channels
-	 * are presented on the output pins.
-	 */
-	switch (mode) {
-	case MODE_2PWM:	// multi-port with flow control lines as PWM
-	case MODE_4PWM: // multi-port as 4 PWM outs
-		debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
-		
-		/* default output rates */
-		_pwm_default_rate = 50;
-		_pwm_alt_rate = 50;
-		_pwm_alt_rate_channels = 0;
-
-		/* XXX magic numbers */
-		up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
-		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
-
-		break;
-
-	case MODE_NONE:
-		debug("MODE_NONE");
-
-		_pwm_default_rate = 10;	/* artificially reduced output rate */
-		_pwm_alt_rate = 10;
-		_pwm_alt_rate_channels = 0;
-
-		/* disable servo outputs - no need to set rates */
-		up_pwm_servo_deinit();
-
-		break;
-
-	default:
-		return -EINVAL;
-	}
-
-	_mode = mode;
-	return OK;
-}
-
-int
-PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
-{
-	debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate);
-
-	for (unsigned pass = 0; pass < 2; pass++) {
-		for (unsigned group = 0; group < _max_actuators; group++) {
-
-			// get the channel mask for this rate group
-			uint32_t mask = up_pwm_servo_get_rate_group(group);
-			if (mask == 0)
-				continue;
-
-			// all channels in the group must be either default or alt-rate
-			uint32_t alt = rate_map & mask;
-
-			if (pass == 0) {
-				// preflight
-				if ((alt != 0) && (alt != mask)) {
-					warn("rate group %u mask %x bad overlap %x", group, mask, alt);
-					// not a legal map, bail
-					return -EINVAL;
-				}
-			} else {
-				// set it - errors here are unexpected
-				if (alt != 0) {
-					if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) {
-						warn("rate group set alt failed");
-						return -EINVAL;
-					}
-				} else {
-					if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
-						warn("rate group set default failed");
-						return -EINVAL;
-					}
-				}
-			}
-		}
-	}
-	_pwm_alt_rate_channels = rate_map;
-	_pwm_default_rate = default_rate;
-	_pwm_alt_rate = alt_rate;
-
-	return OK;
-}
-
-int
-PX4FMU::set_pwm_alt_rate(unsigned rate)
-{
-	return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate);
-}
-
-int
-PX4FMU::set_pwm_alt_channels(uint32_t channels)
-{
-	return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
-}
-
-void
-PX4FMU::task_main()
-{
-	/*
-	 * Subscribe to the appropriate PWM output topic based on whether we are the
-	 * primary PWM output or not.
-	 */
-	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-				     ORB_ID(actuator_controls_1));
-	/* force a reset of the update rate */
-	_current_update_rate = 0;
-
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
-
-	/* advertise the mixed control outputs */
-	actuator_outputs_s outputs;
-	memset(&outputs, 0, sizeof(outputs));
-	/* advertise the mixed control outputs */
-	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
-				   &outputs);
-
-	/* advertise the effective control inputs */
-	actuator_controls_effective_s controls_effective;
-	memset(&controls_effective, 0, sizeof(controls_effective));
-	/* advertise the effective control inputs */
-	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
-				   &controls_effective);
-
-	pollfd fds[2];
-	fds[0].fd = _t_actuators;
-	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
-	fds[1].events = POLLIN;
-
-	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
-
-	// rc input, published to ORB
-	struct rc_input_values rc_in;
-	orb_advert_t to_input_rc = 0;
-
-	memset(&rc_in, 0, sizeof(rc_in));
-	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
-
-	log("starting");
-
-	/* loop until killed */
-	while (!_task_should_exit) {
-
-		/*
-		 * Adjust actuator topic update rate to keep up with
-		 * the highest servo update rate configured.
-		 *
-		 * We always mix at max rate; some channels may update slower.
-		 */
-		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
-		if (_current_update_rate != max_rate) {
-			_current_update_rate = max_rate;
-			int update_rate_in_ms = int(1000 / _current_update_rate);
-
-			/* reject faster than 500 Hz updates */
-			if (update_rate_in_ms < 2) {
-				update_rate_in_ms = 2;
-			}
-			/* reject slower than 10 Hz updates */
-			if (update_rate_in_ms > 100) {
-				update_rate_in_ms = 100;
-			}
-
-			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
-			orb_set_interval(_t_actuators, update_rate_in_ms);
-
-			// set to current max rate, even if we are actually checking slower/faster
-			_current_update_rate = max_rate;
-		}
-
-		/* sleep waiting for data, stopping to check for PPM
-		 * input at 100Hz */
-		int ret = ::poll(&fds[0], 2, 10);
-
-		/* this would be bad... */
-		if (ret < 0) {
-			log("poll error %d", errno);
-			usleep(1000000);
-			continue;
-		}
-
-		/* do we have a control update? */
-		if (fds[0].revents & POLLIN) {
-
-			/* get controls - must always do this to avoid spinning */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
-
-			/* can we mix? */
-			if (_mixers != nullptr) {
-
-				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
-				outputs.timestamp = hrt_absolute_time();
-
-				// XXX output actual limited values
-				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
-
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
-
-				/* iterate actuators */
-				for (unsigned i = 0; i < num_outputs; i++) {
-
-					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i < outputs.noutputs &&
-						isfinite(outputs.output[i]) &&
-						outputs.output[i] >= -1.0f &&
-						outputs.output[i] <= 1.0f) {
-						/* scale for PWM output 900 - 2100us */
-						outputs.output[i] = 1500 + (600 * outputs.output[i]);
-					} else {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = 900;
-					}
-
-					/* output to the servo */
-					up_pwm_servo_set(i, outputs.output[i]);
-				}
-
-				/* and publish for anyone that cares to see */
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
-			}
-		}
-
-		/* how about an arming update? */
-		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
-
-			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
-
-			/* update PWM servo armed status if armed and not locked down */
-			up_pwm_servo_arm(aa.armed && !aa.lockdown);
-		}
-
-		// see if we have new PPM input data
-		if (ppm_last_valid_decode != rc_in.timestamp) {
-			// we have a new PPM frame. Publish it.
-			rc_in.channel_count = ppm_decoded_channels;
-			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
-				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
-			}
-			for (uint8_t i=0; i<rc_in.channel_count; i++) {
-				rc_in.values[i] = ppm_buffer[i];
-			}
-			rc_in.timestamp = ppm_last_valid_decode;
-
-			/* lazily advertise on first publication */
-			if (to_input_rc == 0) {
-				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
-			} else { 
-				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
-			}
-		}
-	}
-
-	::close(_t_actuators);
-	::close(_t_actuators_effective);
-	::close(_t_armed);
-
-	/* make sure servos are off */
-	up_pwm_servo_deinit();
-
-	log("stopping");
-
-	/* note - someone else is responsible for restoring the GPIO config */
-
-	/* tell the dtor that we are exiting */
-	_task = -1;
-	_exit(0);
-}
-
-int
-PX4FMU::control_callback(uintptr_t handle,
-			 uint8_t control_group,
-			 uint8_t control_index,
-			 float &input)
-{
-	const actuator_controls_s *controls = (actuator_controls_s *)handle;
-
-	input = controls->control[control_index];
-	return 0;
-}
-
-int
-PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
-{
-	int ret;
-
-	// XXX disabled, confusing users
-	//debug("ioctl 0x%04x 0x%08x", cmd, arg);
-
-	/* try it as a GPIO ioctl first */
-	ret = gpio_ioctl(filp, cmd, arg);
-
-	if (ret != -ENOTTY)
-		return ret;
-
-	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
-	switch (_mode) {
-	case MODE_2PWM:
-	case MODE_4PWM:
-		ret = pwm_ioctl(filp, cmd, arg);
-		break;
-
-	default:
-		debug("not in a PWM mode");
-		break;
-	}
-
-	/* if nobody wants it, let CDev have it */
-	if (ret == -ENOTTY)
-		ret = CDev::ioctl(filp, cmd, arg);
-
-	return ret;
-}
-
-int
-PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
-{
-	int ret = OK;
-
-	lock();
-
-	switch (cmd) {
-	case PWM_SERVO_ARM:
-		up_pwm_servo_arm(true);
-		break;
-
-	case PWM_SERVO_DISARM:
-		up_pwm_servo_arm(false);
-		break;
-
-	case PWM_SERVO_SET_UPDATE_RATE:
-		ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
-		break;
-
-	case PWM_SERVO_SELECT_UPDATE_RATE:
-		ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
-		break;
-
-	case PWM_SERVO_SET(2):
-	case PWM_SERVO_SET(3):
-		if (_mode != MODE_4PWM) {
-			ret = -EINVAL;
-			break;
-		}
-
-		/* FALLTHROUGH */
-	case PWM_SERVO_SET(0):
-	case PWM_SERVO_SET(1):
-		if (arg < 2100) {
-			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
-		} else {
-			ret = -EINVAL;
-		}
-
-		break;
-
-	case PWM_SERVO_GET(2):
-	case PWM_SERVO_GET(3):
-		if (_mode != MODE_4PWM) {
-			ret = -EINVAL;
-			break;
-		}
-
-		/* FALLTHROUGH */
-	case PWM_SERVO_GET(0):
-	case PWM_SERVO_GET(1):
-		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
-		break;
-
-	case PWM_SERVO_GET_RATEGROUP(0):
-	case PWM_SERVO_GET_RATEGROUP(1):
-	case PWM_SERVO_GET_RATEGROUP(2):
-	case PWM_SERVO_GET_RATEGROUP(3):
-		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
-		break;
-
-	case PWM_SERVO_GET_COUNT:	
-	case MIXERIOCGETOUTPUTCOUNT:
-		if (_mode == MODE_4PWM) {
-			*(unsigned *)arg = 4;
-
-		} else {
-			*(unsigned *)arg = 2;
-		}
-
-		break;
-
-	case MIXERIOCRESET:
-		if (_mixers != nullptr) {
-			delete _mixers;
-			_mixers = nullptr;
-		}
-
-		break;
-
-	case MIXERIOCADDSIMPLE: {
-			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
-
-			SimpleMixer *mixer = new SimpleMixer(control_callback,
-							     (uintptr_t)&_controls, mixinfo);
-
-			if (mixer->check()) {
-				delete mixer;
-				ret = -EINVAL;
-
-			} else {
-				if (_mixers == nullptr)
-					_mixers = new MixerGroup(control_callback,
-								 (uintptr_t)&_controls);
-
-				_mixers->add_mixer(mixer);
-			}
-
-			break;
-		}
-
-	case MIXERIOCLOADBUF: {
-			const char *buf = (const char *)arg;
-			unsigned buflen = strnlen(buf, 1024);
-
-			if (_mixers == nullptr)
-				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
-
-			if (_mixers == nullptr) {
-				ret = -ENOMEM;
-
-			} else {
-
-				ret = _mixers->load_from_buf(buf, buflen);
-
-				if (ret != 0) {
-					debug("mixer load failed with %d", ret);
-					delete _mixers;
-					_mixers = nullptr;
-					ret = -EINVAL;
-				}
-			}
-			break;
-		}
-
-	default:
-		ret = -ENOTTY;
-		break;
-	}
-
-	unlock();
-
-	return ret;
-}
-
-/*
-  this implements PWM output via a write() method, for compatibility
-  with px4io
- */
-ssize_t
-PX4FMU::write(file *filp, const char *buffer, size_t len)
-{
-	unsigned count = len / 2;
-	uint16_t values[4];
-
-	if (count > 4) {
-		// we only have 4 PWM outputs on the FMU
-		count = 4;
-	}
-
-	// allow for misaligned values
-	memcpy(values, buffer, count*2);
-
-	for (uint8_t i=0; i<count; i++) {
-		up_pwm_servo_set(i, values[i]);
-	}
-	return count * 2;
-}
-
-void
-PX4FMU::gpio_reset(void)
-{
-	/*
-	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
-	 * to input mode.
-	 */
-	for (unsigned i = 0; i < _ngpio; i++)
-		stm32_configgpio(_gpio_tab[i].input);
-
-	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-	stm32_configgpio(GPIO_GPIO_DIR);
-}
-
-void
-PX4FMU::gpio_set_function(uint32_t gpios, int function)
-{
-	/*
-	 * GPIOs 0 and 1 must have the same direction as they are buffered
-	 * by a shared 2-port driver.  Any attempt to set either sets both.
-	 */
-	if (gpios & 3) {
-		gpios |= 3;
-
-		/* flip the buffer to output mode if required */
-		if (GPIO_SET_OUTPUT == function)
-			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
-	}
-
-	/* configure selected GPIOs as required */
-	for (unsigned i = 0; i < _ngpio; i++) {
-		if (gpios & (1 << i)) {
-			switch (function) {
-			case GPIO_SET_INPUT:
-				stm32_configgpio(_gpio_tab[i].input);
-				break;
-
-			case GPIO_SET_OUTPUT:
-				stm32_configgpio(_gpio_tab[i].output);
-				break;
-
-			case GPIO_SET_ALT_1:
-				if (_gpio_tab[i].alt != 0)
-					stm32_configgpio(_gpio_tab[i].alt);
-
-				break;
-			}
-		}
-	}
-
-	/* flip buffer to input mode if required */
-	if ((GPIO_SET_INPUT == function) && (gpios & 3))
-		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-}
-
-void
-PX4FMU::gpio_write(uint32_t gpios, int function)
-{
-	int value = (function == GPIO_SET) ? 1 : 0;
-
-	for (unsigned i = 0; i < _ngpio; i++)
-		if (gpios & (1 << i))
-			stm32_gpiowrite(_gpio_tab[i].output, value);
-}
-
-uint32_t
-PX4FMU::gpio_read(void)
-{
-	uint32_t bits = 0;
-
-	for (unsigned i = 0; i < _ngpio; i++)
-		if (stm32_gpioread(_gpio_tab[i].input))
-			bits |= (1 << i);
-
-	return bits;
-}
-
-int
-PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int	ret = OK;
-
-	lock();
-
-	switch (cmd) {
-
-	case GPIO_RESET:
-		gpio_reset();
-		break;
-
-	case GPIO_SET_OUTPUT:
-	case GPIO_SET_INPUT:
-	case GPIO_SET_ALT_1:
-		gpio_set_function(arg, cmd);
-		break;
-
-	case GPIO_SET_ALT_2:
-	case GPIO_SET_ALT_3:
-	case GPIO_SET_ALT_4:
-		ret = -EINVAL;
-		break;
-
-	case GPIO_SET:
-	case GPIO_CLEAR:
-		gpio_write(arg, cmd);
-		break;
-
-	case GPIO_GET:
-		*(uint32_t *)arg = gpio_read();
-		break;
-
-	default:
-		ret = -ENOTTY;
-	}
-
-	unlock();
-
-	return ret;
-}
-
-namespace
-{
-
-enum PortMode {
-	PORT_MODE_UNSET = 0,
-	PORT_FULL_GPIO,
-	PORT_FULL_SERIAL,
-	PORT_FULL_PWM,
-	PORT_GPIO_AND_SERIAL,
-	PORT_PWM_AND_SERIAL,
-	PORT_PWM_AND_GPIO,
-};
-
-PortMode g_port_mode;
-
-int
-fmu_new_mode(PortMode new_mode)
-{
-	uint32_t gpio_bits;
-	PX4FMU::Mode servo_mode;
-
-	/* reset to all-inputs */
-	g_fmu->ioctl(0, GPIO_RESET, 0);
-
-	gpio_bits = 0;
-	servo_mode = PX4FMU::MODE_NONE;
-
-	switch (new_mode) {
-	case PORT_FULL_GPIO:
-	case PORT_MODE_UNSET:
-		/* nothing more to do here */
-		break;
-
-	case PORT_FULL_SERIAL:
-		/* set all multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_FULL_PWM:
-		/* select 4-pin PWM mode */
-		servo_mode = PX4FMU::MODE_4PWM;
-		break;
-
-	case PORT_GPIO_AND_SERIAL:
-		/* set RX/TX multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_PWM_AND_SERIAL:
-		/* select 2-pin PWM mode */
-		servo_mode = PX4FMU::MODE_2PWM;
-		/* set RX/TX multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_PWM_AND_GPIO:
-		/* select 2-pin PWM mode */
-		servo_mode = PX4FMU::MODE_2PWM;
-		break;
-	}
-
-	/* adjust GPIO config for serial mode(s) */
-	if (gpio_bits != 0)
-		g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
-
-	/* (re)set the PWM output mode */
-	g_fmu->set_mode(servo_mode);
-
-	return OK;
-}
-
-int
-fmu_start(void)
-{
-	int ret = OK;
-
-	if (g_fmu == nullptr) {
-
-		g_fmu = new PX4FMU;
-
-		if (g_fmu == nullptr) {
-			ret = -ENOMEM;
-
-		} else {
-			ret = g_fmu->init();
-
-			if (ret != OK) {
-				delete g_fmu;
-				g_fmu = nullptr;
-			}
-		}
-	}
-
-	return ret;
-}
-
-void
-test(void)
-{
-	int	fd;
-
-	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
-
-	if (fd < 0)
-		errx(1, "open fail");
-
-	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0)       err(1, "servo arm failed");
-
-	if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
-
-	if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
-
-	if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
-
-	if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
-
-	close(fd);
-
-	exit(0);
-}
-
-void
-fake(int argc, char *argv[])
-{
-	if (argc < 5)
-		errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
-
-	actuator_controls_s ac;
-
-	ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
-
-	ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
-
-	ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
-
-	ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
-
-	orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
-
-	if (handle < 0)
-		errx(1, "advertise failed");
-
-	actuator_armed_s aa;
-
-	aa.armed = true;
-	aa.lockdown = false;
-
-	handle = orb_advertise(ORB_ID(actuator_armed), &aa);
-
-	if (handle < 0)
-		errx(1, "advertise failed 2");
-
-	exit(0);
-}
-
-} // namespace
-
-extern "C" __EXPORT int fmu_main(int argc, char *argv[]);
-
-int
-fmu_main(int argc, char *argv[])
-{
-	PortMode new_mode = PORT_MODE_UNSET;
-	const char *verb = argv[1];
-
-	if (fmu_start() != OK)
-		errx(1, "failed to start the FMU driver");
-
-	/*
-	 * Mode switches.
-	 */
-	if (!strcmp(verb, "mode_gpio")) {
-		new_mode = PORT_FULL_GPIO;
-
-	} else if (!strcmp(verb, "mode_serial")) {
-		new_mode = PORT_FULL_SERIAL;
-
-	} else if (!strcmp(verb, "mode_pwm")) {
-		new_mode = PORT_FULL_PWM;
-
-	} else if (!strcmp(verb, "mode_gpio_serial")) {
-		new_mode = PORT_GPIO_AND_SERIAL;
-
-	} else if (!strcmp(verb, "mode_pwm_serial")) {
-		new_mode = PORT_PWM_AND_SERIAL;
-
-	} else if (!strcmp(verb, "mode_pwm_gpio")) {
-		new_mode = PORT_PWM_AND_GPIO;
-	}
-
-	/* was a new mode set? */
-	if (new_mode != PORT_MODE_UNSET) {
-
-		/* yes but it's the same mode */
-		if (new_mode == g_port_mode)
-			return OK;
-
-		/* switch modes */
-		int ret = fmu_new_mode(new_mode);
-		exit(ret == OK ? 0 : 1);
-	}
-
-	if (!strcmp(verb, "test"))
-		test();
-
-	if (!strcmp(verb, "fake"))
-		fake(argc - 1, argv + 1);
-
-	fprintf(stderr, "FMU: unrecognised command, try:\n");
-	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
-	exit(1);
-}
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 1f3f7707e..123bbb120 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -372,7 +372,9 @@ Sensors	*g_sensors;
 }
 
 Sensors::Sensors() :
+#ifdef CONFIG_HRT_PPM
 	_ppm_last_valid(0),
+#endif
 
 	_fd_adc(-1),
 	_last_adc(0),
diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c
deleted file mode 100644
index e34be44e3..000000000
--- a/apps/systemcmds/eeprom/24xxxx_mtd.c
+++ /dev/null
@@ -1,571 +0,0 @@
-/************************************************************************************
- * Driver for 24xxxx-style I2C EEPROMs.
- *
- * Adapted from:
- *
- * drivers/mtd/at24xx.c
- * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256)
- *
- *   Copyright (C) 2011 Li Zhuoyi. All rights reserved.
- *   Author: Li Zhuoyi <lzyy.cn@gmail.com>
- *   History: 0.1 2011-08-20 initial version
- *
- *   2011-11-1 Added support for larger MTD block sizes: Hal Glenn <hglenn@2g-eng.com>
- *
- * Derived from drivers/mtd/m25px.c
- *
- *   Copyright (C) 2009-2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ************************************************************************************/
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <string.h>
-#include <errno.h>
-#include <debug.h>
-
-#include <nuttx/kmalloc.h>
-#include <nuttx/fs/ioctl.h>
-#include <nuttx/i2c.h>
-#include <nuttx/mtd.h>
-
-#include "systemlib/perf_counter.h"
-
-/************************************************************************************
- * Pre-processor Definitions
- ************************************************************************************/
-
-/*
- * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be
- * omitted in order to avoid building the AT24XX driver as well.
- */
-
-/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */
-
-#ifndef CONFIG_AT24XX_SIZE
-#  warning "Assuming AT24 size 64"
-#  define CONFIG_AT24XX_SIZE 64
-#endif
-#ifndef CONFIG_AT24XX_ADDR
-#  warning "Assuming AT24 address of 0x50"
-#  define CONFIG_AT24XX_ADDR 0x50
-#endif
-
-/* Get the part configuration based on the size configuration */
-
-#if CONFIG_AT24XX_SIZE == 32
-#  define AT24XX_NPAGES     128
-#  define AT24XX_PAGESIZE   32
-#elif CONFIG_AT24XX_SIZE == 48
-#  define AT24XX_NPAGES     192
-#  define AT24XX_PAGESIZE   32
-#elif CONFIG_AT24XX_SIZE == 64
-#  define AT24XX_NPAGES     256
-#  define AT24XX_PAGESIZE   32
-#elif CONFIG_AT24XX_SIZE == 128
-#  define AT24XX_NPAGES     256
-#  define AT24XX_PAGESIZE   64
-#elif CONFIG_AT24XX_SIZE == 256
-#  define AT24XX_NPAGES     512
-#  define AT24XX_PAGESIZE   64
-#endif
-
-/* For applications where a file system is used on the AT24, the tiny page sizes
- * will result in very inefficient FLASH usage.  In such cases, it is better if
- * blocks are comprised of "clusters" of pages so that the file system block
- * size is, say, 256 or 512 bytes.  In any event, the block size *must* be an
- * even multiple of the pages.
- */
-
-#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE
-#  warning "Assuming driver block size is the same as the FLASH page size"
-#  define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE
-#endif
-
-/* The AT24 does not respond on the bus during write cycles, so we depend on a long
- * timeout to detect problems.  The max program time is typically ~5ms.
- */
-#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS
-#  define CONFIG_AT24XX_WRITE_TIMEOUT_MS  20
-#endif
-
-/************************************************************************************
- * Private Types
- ************************************************************************************/
-
-/* This type represents the state of the MTD device.  The struct mtd_dev_s
- * must appear at the beginning of the definition so that you can freely
- * cast between pointers to struct mtd_dev_s and struct at24c_dev_s.
- */
-
-struct at24c_dev_s {
-	struct mtd_dev_s      mtd;      /* MTD interface */
-	FAR struct i2c_dev_s *dev;      /* Saved I2C interface instance */
-	uint8_t               addr;     /* I2C address */
-	uint16_t              pagesize; /* 32, 63 */
-	uint16_t              npages;   /* 128, 256, 512, 1024 */
-
-	perf_counter_t        perf_reads;
-	perf_counter_t        perf_writes;
-	perf_counter_t        perf_resets;
-	perf_counter_t        perf_read_retries;
-	perf_counter_t        perf_read_errors;
-	perf_counter_t        perf_write_errors;
-};
-
-/************************************************************************************
- * Private Function Prototypes
- ************************************************************************************/
-
-/* MTD driver methods */
-
-static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks);
-static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
-			   size_t nblocks, FAR uint8_t *buf);
-static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock,
-			    size_t nblocks, FAR const uint8_t *buf);
-static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg);
-
-void at24c_test(void);
-
-/************************************************************************************
- * Private Data
- ************************************************************************************/
-
-/* At present, only a single AT24 part is supported.  In this case, a statically
- * allocated state structure may be used.
- */
-
-static struct at24c_dev_s g_at24c;
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
-
-static int at24c_eraseall(FAR struct at24c_dev_s *priv)
-{
-	int startblock = 0;
-	uint8_t buf[AT24XX_PAGESIZE + 2];
-
-	struct i2c_msg_s msgv[1] = {
-		{
-			.addr = priv->addr,
-			.flags = 0,
-			.buffer = &buf[0],
-			.length = sizeof(buf),
-		}
-	};
-
-	memset(&buf[2], 0xff, priv->pagesize);
-
-	for (startblock = 0; startblock < priv->npages; startblock++) {
-		uint16_t offset = startblock * priv->pagesize;
-		buf[1] = offset & 0xff;
-		buf[0] = (offset >> 8) & 0xff;
-
-		while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) {
-			fvdbg("erase stall\n");
-			usleep(10000);
-		}
-	}
-
-	return OK;
-}
-
-/************************************************************************************
- * Name: at24c_erase
- ************************************************************************************/
-
-static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks)
-{
-	/* EEprom need not erase */
-
-	return (int)nblocks;
-}
-
-/************************************************************************************
- * Name: at24c_test
- ************************************************************************************/
-
-void at24c_test(void)
-{
-	uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE];
-	unsigned count = 0;
-	unsigned errors = 0;
-
-	for (count = 0; count < 10000; count++) {
-		ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf);
-		if (result == ERROR) {
-			if (errors++ > 2) {
-				vdbg("too many errors\n");
-				return;
-			}
-		} else if (result != 1) {
-			vdbg("unexpected %u\n", result);
-		}
-		if ((count % 100) == 0)
-			vdbg("test %u errors %u\n", count, errors);
-	}
-}
-
-/************************************************************************************
- * Name: at24c_bread
- ************************************************************************************/
-
-static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
-			   size_t nblocks, FAR uint8_t *buffer)
-{
-	FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
-	size_t blocksleft;
-	uint8_t addr[2];
-	int ret;
-
-	struct i2c_msg_s msgv[2] = {
-		{
-			.addr = priv->addr,
-			.flags = 0,
-			.buffer = &addr[0],
-			.length = sizeof(addr),
-		},
-		{
-			.addr = priv->addr,
-			.flags = I2C_M_READ,
-			.buffer = 0,
-			.length = priv->pagesize,
-		}
-	};
-
-#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-	startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
-	nblocks    *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
-#endif
-	blocksleft  = nblocks;
-
-	fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
-
-	if (startblock >= priv->npages) {
-		return 0;
-	}
-
-	if (startblock + nblocks > priv->npages) {
-		nblocks = priv->npages - startblock;
-	}
-
-	while (blocksleft-- > 0) {
-		uint16_t offset = startblock * priv->pagesize;
-		unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
-
-		addr[1] = offset & 0xff;
-		addr[0] = (offset >> 8) & 0xff;
-		msgv[1].buffer = buffer;
-
-		for (;;) {
-
-			perf_begin(priv->perf_reads);
-			ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
-			perf_end(priv->perf_reads);
-
-			if (ret >= 0)
-				break;
-
-			fvdbg("read stall");
-			usleep(1000);
-
-			/* We should normally only be here on the first read after
-			 * a write.
-			 *
-			 * XXX maybe do special first-read handling with optional
-			 * bus reset as well?
-			 */
-			perf_count(priv->perf_read_retries);
-
-			if (--tries == 0) {
-				perf_count(priv->perf_read_errors);
-				return ERROR;
-			}
-		}
-
-		startblock++;
-		buffer += priv->pagesize;
-	}
-
-#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-	return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
-#else
-	return nblocks;
-#endif
-}
-
-/************************************************************************************
- * Name: at24c_bwrite
- *
- * Operates on MTD block's and translates to FLASH pages
- *
- ************************************************************************************/
-
-static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks,
-			    FAR const uint8_t *buffer)
-{
-	FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
-	size_t blocksleft;
-	uint8_t buf[AT24XX_PAGESIZE + 2];
-	int ret;
-
-	struct i2c_msg_s msgv[1] = {
-		{
-			.addr = priv->addr,
-			.flags = 0,
-			.buffer = &buf[0],
-			.length = sizeof(buf),
-		}
-	};
-
-#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-	startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
-	nblocks    *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
-#endif
-	blocksleft  = nblocks;
-
-	if (startblock >= priv->npages) {
-		return 0;
-	}
-
-	if (startblock + nblocks > priv->npages) {
-		nblocks = priv->npages - startblock;
-	}
-
-	fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
-
-	while (blocksleft-- > 0) {
-		uint16_t offset = startblock * priv->pagesize;
-		unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
-
-		buf[1] = offset & 0xff;
-		buf[0] = (offset >> 8) & 0xff;
-		memcpy(&buf[2], buffer, priv->pagesize);
-
-		for (;;) {
-
-			perf_begin(priv->perf_writes);
-			ret = I2C_TRANSFER(priv->dev, &msgv[0], 1);
-			perf_end(priv->perf_writes);
-
-			if (ret >= 0)
-				break;
-
-			fvdbg("write stall");
-			usleep(1000);
-
-			/* We expect to see a number of retries per write cycle as we
-			 * poll for write completion.
-			 */
-			if (--tries == 0) {
-				perf_count(priv->perf_write_errors);
-				return ERROR;
-			}
-		}
-
-		startblock++;
-		buffer += priv->pagesize;
-	}
-
-#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-	return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
-#else
-	return nblocks;
-#endif
-}
-
-/************************************************************************************
- * Name: at24c_ioctl
- ************************************************************************************/
-
-static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
-{
-	FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
-	int ret = -EINVAL; /* Assume good command with bad parameters */
-
-	fvdbg("cmd: %d \n", cmd);
-
-	switch (cmd) {
-	case MTDIOC_GEOMETRY: {
-			FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg);
-
-			if (geo) {
-				/* Populate the geometry structure with information need to know
-				 * the capacity and how to access the device.
-				 *
-				 * NOTE: that the device is treated as though it where just an array
-				 * of fixed size blocks.  That is most likely not true, but the client
-				 * will expect the device logic to do whatever is necessary to make it
-				 * appear so.
-				 *
-				 * blocksize:
-				 *   May be user defined. The block size for the at24XX devices may be
-				 *   larger than the page size in order to better support file systems.
-				 *   The read and write functions translate BLOCKS to pages for the
-				 *   small flash devices
-				 * erasesize:
-				 *   It has to be at least as big as the blocksize, bigger serves no
-				 *   purpose.
-				 * neraseblocks
-				 *   Note that the device size is in kilobits and must be scaled by
-				 *   1024 / 8
-				 */
-
-#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-				geo->blocksize    = CONFIG_AT24XX_MTD_BLOCKSIZE;
-				geo->erasesize    = CONFIG_AT24XX_MTD_BLOCKSIZE;
-				geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE;
-#else
-				geo->blocksize    = priv->pagesize;
-				geo->erasesize    = priv->pagesize;
-				geo->neraseblocks = priv->npages;
-#endif
-				ret               = OK;
-
-				fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n",
-				      geo->blocksize, geo->erasesize, geo->neraseblocks);
-			}
-		}
-		break;
-
-	case MTDIOC_BULKERASE:
-		ret = at24c_eraseall(priv);
-		break;
-
-	case MTDIOC_XIPBASE:
-	default:
-		ret = -ENOTTY; /* Bad command */
-		break;
-	}
-
-	return ret;
-}
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: at24c_initialize
- *
- * Description:
- *   Create an initialize MTD device instance.  MTD devices are not registered
- *   in the file system, but are created as instances that can be bound to
- *   other functions (such as a block or character driver front end).
- *
- ************************************************************************************/
-
-FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
-	FAR struct at24c_dev_s *priv;
-
-	fvdbg("dev: %p\n", dev);
-
-	/* Allocate a state structure (we allocate the structure instead of using
-	 * a fixed, static allocation so that we can handle multiple FLASH devices.
-	 * The current implementation would handle only one FLASH part per I2C
-	 * device (only because of the SPIDEV_FLASH definition) and so would have
-	 * to be extended to handle multiple FLASH parts on the same I2C bus.
-	 */
-
-	priv = &g_at24c;
-
-	if (priv) {
-		/* Initialize the allocated structure */
-
-		priv->addr       = CONFIG_AT24XX_ADDR;
-		priv->pagesize   = AT24XX_PAGESIZE;
-		priv->npages     = AT24XX_NPAGES;
-
-		priv->mtd.erase  = at24c_erase;
-		priv->mtd.bread  = at24c_bread;
-		priv->mtd.bwrite = at24c_bwrite;
-		priv->mtd.ioctl  = at24c_ioctl;
-		priv->dev        = dev;
-
-		priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read");
-		priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write");
-		priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset");
-		priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries");
-		priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors");
-		priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors");
-	}
-
-	/* attempt to read to validate device is present */
-	unsigned char buf[5];
-	uint8_t addrbuf[2] = {0, 0};
-
-	struct i2c_msg_s msgv[2] = {
-		{
-			.addr = priv->addr,
-			.flags = 0,
-			.buffer = &addrbuf[0],
-			.length = sizeof(addrbuf),
-		},
-		{
-			.addr = priv->addr,
-			.flags = I2C_M_READ,
-			.buffer = &buf[0],
-			.length = sizeof(buf),
-		}
-	};
-
-	perf_begin(priv->perf_reads);
-	int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
-	perf_end(priv->perf_reads);
-
-	if (ret < 0) {
-		return NULL;
-	}
-
-	/* Return the implementation-specific state structure as the MTD device */
-
-	fvdbg("Return %p\n", priv);
-	return (FAR struct mtd_dev_s *)priv;
-}
-
-/*
- * XXX: debug hackery
- */
-int at24c_nuke(void)
-{
-	return at24c_eraseall(&g_at24c);
-}
diff --git a/apps/systemcmds/eeprom/Makefile b/apps/systemcmds/eeprom/Makefile
deleted file mode 100644
index 58c8cb5ec..000000000
--- a/apps/systemcmds/eeprom/Makefile
+++ /dev/null
@@ -1,45 +0,0 @@
-############################################################################
-#
-#   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 eeprom tool.
-#
-
-APPNAME		 = eeprom
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 4096
-MAXOPTIMIZATION	 = -Os
-
-include $(APPDIR)/mk/app.mk
-
-MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c
deleted file mode 100644
index 49da51358..000000000
--- a/apps/systemcmds/eeprom/eeprom.c
+++ /dev/null
@@ -1,265 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file eeprom.c
- *
- * EEPROM service and utility app.
- */
-
-#include <nuttx/config.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/mount.h>
-#include <sys/ioctl.h>
-#include <sys/stat.h>
-
-#include <nuttx/i2c.h>
-#include <nuttx/mtd.h>
-#include <nuttx/fs/nxffs.h>
-#include <nuttx/fs/ioctl.h>
-
-#include <arch/board/board.h>
-
-#include "systemlib/systemlib.h"
-#include "systemlib/param/param.h"
-#include "systemlib/err.h"
-
-#ifndef PX4_I2C_BUS_ONBOARD
-#  error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
-#endif
-
-__EXPORT int eeprom_main(int argc, char *argv[]);
-
-static void	eeprom_attach(void);
-static void	eeprom_start(void);
-static void	eeprom_erase(void);
-static void	eeprom_ioctl(unsigned operation);
-static void	eeprom_save(const char *name);
-static void	eeprom_load(const char *name);
-static void	eeprom_test(void);
-
-static bool attached = false;
-static bool started = false;
-static struct mtd_dev_s *eeprom_mtd;
-
-int eeprom_main(int argc, char *argv[])
-{
-	if (argc >= 2) {
-		if (!strcmp(argv[1], "start"))
-			eeprom_start();
-
-		if (!strcmp(argv[1], "save_param"))
-			eeprom_save(argv[2]);
-
-		if (!strcmp(argv[1], "load_param"))
-			eeprom_load(argv[2]);
-
-		if (!strcmp(argv[1], "erase"))
-			eeprom_erase();
-
-		if (!strcmp(argv[1], "test"))
-			eeprom_test();
-
-		if (0) {	/* these actually require a file on the filesystem... */
-
-			if (!strcmp(argv[1], "reformat"))
-				eeprom_ioctl(FIOC_REFORMAT);
-
-			if (!strcmp(argv[1], "repack"))
-				eeprom_ioctl(FIOC_OPTIMIZE);
-		}
-	}
-
-	errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
-}
-
-
-static void
-eeprom_attach(void)
-{
-	/* find the right I2C */
-	struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
-	/* this resets the I2C bus, set correct bus speed again */
-	I2C_SETFREQUENCY(i2c, 400000);
-
-	if (i2c == NULL)
-		errx(1, "failed to locate I2C bus");
-
-	/* start the MTD driver, attempt 5 times */
-	for (int i = 0; i < 5; i++) {
-		eeprom_mtd = at24c_initialize(i2c);
-		if (eeprom_mtd) {
-			/* abort on first valid result */
-			if (i > 0) {
-				warnx("warning: EEPROM needed %d attempts to attach", i+1);
-			}
-			break;
-		}
-	}
-
-	/* if last attempt is still unsuccessful, abort */
-	if (eeprom_mtd == NULL)
-		errx(1, "failed to initialize EEPROM driver");
-
-	attached = true;
-}
-
-static void
-eeprom_start(void)
-{
-	int ret;
-
-	if (started)
-		errx(1, "EEPROM already mounted");
-
-	if (!attached)
-		eeprom_attach();
-
-	/* start NXFFS */
-	ret = nxffs_initialize(eeprom_mtd);
-
-	if (ret < 0)
-		errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
-
-	/* mount the EEPROM */
-	ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
-
-	if (ret < 0)
-		errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
-
-	started = true;
-	warnx("mounted EEPROM at /eeprom");
-	exit(0);
-}
-
-extern int at24c_nuke(void);
-
-static void
-eeprom_erase(void)
-{
-	if (!attached)
-		eeprom_attach();
-
-	if (at24c_nuke())
-		errx(1, "erase failed");
-
-	errx(0, "erase done, reboot now");
-}
-
-static void
-eeprom_ioctl(unsigned operation)
-{
-	int fd;
-
-	fd = open("/eeprom/.", 0);
-
-	if (fd < 0)
-		err(1, "open /eeprom");
-
-	if (ioctl(fd, operation, 0) < 0)
-		err(1, "ioctl");
-
-	exit(0);
-}
-
-static void
-eeprom_save(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/eeprom/parameters'");
-
-	warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
-
-	/* delete the file in case it exists */
-	unlink(name);
-
-	/* create the file */
-	int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
-
-	if (fd < 0)
-		err(1, "opening '%s' failed", name);
-
-	int result = param_export(fd, false);
-	close(fd);
-
-	if (result < 0) {
-		unlink(name);
-		errx(1, "error exporting to '%s'", name);
-	}
-
-	exit(0);
-}
-
-static void
-eeprom_load(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/eeprom/parameters'");
-
-	warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
-
-	int fd = open(name, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "open '%s'", name);
-
-	int result = param_load(fd);
-	close(fd);
-
-	if (result < 0)
-		errx(1, "error importing from '%s'", name);
-
-	exit(0);
-}
-
-extern void at24c_test(void);
-
-static void
-eeprom_test(void)
-{
-	at24c_test();
-	exit(0);
-}