Merge working changes into export-build branch.

36 files changed, 6822 insertions, 0 deletions

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
new file mode 100755
index 000000000..1a50dde0f
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -0,0 +1,472 @@
+/****************************************************************************
+ *
+ *   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/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
new file mode 100755
index 000000000..7d3812abd
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   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/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
new file mode 100755
index 000000000..09817d58e
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
new file mode 100755
index 000000000..9e97ad11a
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
@@ -0,0 +1,1148 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
new file mode 100755
index 000000000..afa63c1a9
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
@@ -0,0 +1,34 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
new file mode 100755
index 000000000..7d620d7fa
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
@@ -0,0 +1,31 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
new file mode 100755
index 000000000..8b599be66
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
@@ -0,0 +1,34 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
new file mode 100755
index 000000000..7f9727419
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
@@ -0,0 +1,31 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
new file mode 100755
index 000000000..da84a5024
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
@@ -0,0 +1,34 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
new file mode 100755
index 000000000..30fd1e75e
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
@@ -0,0 +1,16 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c
new file mode 100755
index 000000000..84ada9002
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/cross.c
@@ -0,0 +1,37 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h
new file mode 100755
index 000000000..e727f0684
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/cross.h
@@ -0,0 +1,34 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c
new file mode 100755
index 000000000..b89ab58ef
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/eye.c
@@ -0,0 +1,51 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h
new file mode 100755
index 000000000..94fbe7671
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/eye.h
@@ -0,0 +1,35 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
new file mode 100755
index 000000000..a810f22e4
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
@@ -0,0 +1,357 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
new file mode 100755
index 000000000..2d3b0d51f
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
@@ -0,0 +1,36 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c
new file mode 100755
index 000000000..0c418cc7b
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/norm.c
@@ -0,0 +1,54 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h
new file mode 100755
index 000000000..60cf77b57
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/norm.h
@@ -0,0 +1,34 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c
new file mode 100755
index 000000000..d035dae5e
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.c
@@ -0,0 +1,38 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h
new file mode 100755
index 000000000..4bbebebe2
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.h
@@ -0,0 +1,34 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
new file mode 100755
index 000000000..34164d104
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
@@ -0,0 +1,139 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
new file mode 100755
index 000000000..145373cd0
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
@@ -0,0 +1,23 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
new file mode 100755
index 000000000..d84ca9573
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
@@ -0,0 +1,96 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
new file mode 100755
index 000000000..65fdaa96f
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
@@ -0,0 +1,21 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
new file mode 100755
index 000000000..356498363
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
@@ -0,0 +1,24 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
new file mode 100755
index 000000000..303d1d9d2
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
@@ -0,0 +1,87 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
new file mode 100755
index 000000000..bd56b30d9
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
@@ -0,0 +1,53 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
new file mode 100755
index 000000000..9a5c96267
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
@@ -0,0 +1,159 @@
+/*

+ * 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/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk
new file mode 100644
index 000000000..5ce545112
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -0,0 +1,16 @@
+
+MODULE_NAME	 = attitude_estimator_ekf
+SRCS		 = attitude_estimator_ekf_main.cpp \
+			   attitude_estimator_ekf_params.c \
+			   codegen/attitudeKalmanfilter_initialize.c \
+			   codegen/attitudeKalmanfilter_terminate.c \
+			   codegen/attitudeKalmanfilter.c \
+			   codegen/cross.c \
+			   codegen/eye.c \
+			   codegen/mrdivide.c \
+			   codegen/norm.c \
+			   codegen/rdivide.c \
+			   codegen/rt_nonfinite.c \
+			   codegen/rtGetInf.c \
+			   codegen/rtGetNaN.c
+
diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c
new file mode 100644
index 000000000..a26938637
--- /dev/null
+++ b/src/modules/commander/calibration_routines.c
@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   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/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
new file mode 100644
index 000000000..e3e7fbafd
--- /dev/null
+++ b/src/modules/commander/calibration_routines.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   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/src/modules/commander/commander.c b/src/modules/commander/commander.c
new file mode 100644
index 000000000..7c0a25f86
--- /dev/null
+++ b/src/modules/commander/commander.c
@@ -0,0 +1,2181 @@
+/****************************************************************************
+ *
+ *   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/src/modules/commander/commander.h b/src/modules/commander/commander.h
new file mode 100644
index 000000000..04b4e72ab
--- /dev/null
+++ b/src/modules/commander/commander.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   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/src/modules/commander/module.mk b/src/modules/commander/module.mk
new file mode 100644
index 000000000..b90da8289
--- /dev/null
+++ b/src/modules/commander/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Main system state machine
+#
+
+MODULE_COMMAND	 = commander
+SRCS		 = commander.c \
+			   state_machine_helper.c \
+			   calibration_routines.c
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
new file mode 100644
index 000000000..bea388a10
--- /dev/null
+++ b/src/modules/commander/state_machine_helper.c
@@ -0,0 +1,752 @@
+/****************************************************************************
+ *
+ *   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/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
new file mode 100644
index 000000000..2f2ccc729
--- /dev/null
+++ b/src/modules/commander/state_machine_helper.h
@@ -0,0 +1,209 @@
+/****************************************************************************
+ *
+ *   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_ */