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

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

9 files changed, 3889 insertions, 0 deletions

diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c
new file mode 100644
index 000000000..231739962
--- /dev/null
+++ b/src/modules/commander/accelerometer_calibration.c
@@ -0,0 +1,404 @@
+/*
+ * accelerometer_calibration.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Transform acceleration vector to true orientation and scale
+ *
+ * * * * Model * * *
+ * accel_corr = accel_T * (accel_raw - accel_offs)
+ *
+ * accel_corr[3] - fully corrected acceleration vector in body frame
+ * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
+ * accel_raw[3]  - raw acceleration vector
+ * accel_offs[3] - acceleration offset vector
+ *
+ * * * * Calibration * * *
+ *
+ * Reference vectors
+ * accel_corr_ref[6][3] = [  g  0  0 ]     // nose up
+ *                        | -g  0  0 |     // nose down
+ *                        |  0  g  0 |     // left side down
+ *                        |  0 -g  0 |     // right side down
+ *                        |  0  0  g |     // on back
+ *                        [  0  0 -g ]     // level
+ * accel_raw_ref[6][3]
+ *
+ * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
+ *
+ * 6 reference vectors * 3 axes = 18 equations
+ * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
+ *
+ * Find accel_offs
+ *
+ * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
+ *
+ *
+ * Find accel_T
+ *
+ * 9 unknown constants
+ * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
+ *
+ * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
+ *
+ * Solve separate system for each row of accel_T:
+ *
+ * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
+ *
+ * A * x = b
+ *
+ * x = [ accel_T[0][i] ]
+ *     | accel_T[1][i] |
+ *     [ accel_T[2][i] ]
+ *
+ * b = [ accel_corr_ref[0][i] ]	// One measurement per axis is enough
+ *     | accel_corr_ref[2][i] |
+ *     [ accel_corr_ref[4][i] ]
+ *
+ * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
+ *
+ * Matrix A is common for all three systems:
+ * A = [ a[0][0]  a[0][1]  a[0][2] ]
+ *     | a[2][0]  a[2][1]  a[2][2] |
+ *     [ a[4][0]  a[4][1]  a[4][2] ]
+ *
+ * x = A^-1 * b
+ *
+ * accel_T = A^-1 * g
+ * g = 9.80665
+ */
+
+#include "accelerometer_calibration.h"
+
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_accel.h>
+#include <systemlib/conversions.h>
+#include <mavlink/mavlink_log.h>
+
+void do_accel_calibration(int mavlink_fd);
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
+int detect_orientation(int mavlink_fd, int sub_sensor_combined);
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
+int mat_invert3(float src[3][3], float dst[3][3]);
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
+
+void do_accel_calibration(int mavlink_fd) {
+	/* announce change */
+	mavlink_log_info(mavlink_fd, "accel calibration started");
+
+	/* measure and calculate offsets & scales */
+	float accel_offs[3];
+	float accel_scale[3];
+	int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
+
+	if (res == OK) {
+		/* measurements complete successfully, set parameters */
+		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
+			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
+			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
+			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
+			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
+			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
+		}
+
+		int fd = open(ACCEL_DEVICE_PATH, 0);
+		struct accel_scale ascale = {
+			accel_offs[0],
+			accel_scale[0],
+			accel_offs[1],
+			accel_scale[1],
+			accel_offs[2],
+			accel_scale[2],
+		};
+
+		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");
+		}
+
+		mavlink_log_info(mavlink_fd, "accel calibration done");
+		tune_positive();
+	} else {
+		/* measurements error */
+		mavlink_log_info(mavlink_fd, "accel calibration aborted");
+		tune_negative();
+	}
+
+	/* exit accel calibration mode */
+}
+
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
+	const int samples_num = 2500;
+	float accel_ref[6][3];
+	bool data_collected[6] = { false, false, false, false, false, false };
+	const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
+
+	/* reset existing calibration */
+	int fd = open(ACCEL_DEVICE_PATH, 0);
+	struct accel_scale ascale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+	int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
+	close(fd);
+
+	if (OK != ioctl_res) {
+		warn("ERROR: failed to set scale / offsets for accel");
+		return ERROR;
+	}
+
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	while (true) {
+		bool done = true;
+		char str[80];
+		int str_ptr;
+		str_ptr = sprintf(str, "keep vehicle still:");
+		for (int i = 0; i < 6; i++) {
+			if (!data_collected[i]) {
+				str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
+				done = false;
+			}
+		}
+		if (done)
+			break;
+		mavlink_log_info(mavlink_fd, str);
+
+		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
+		if (orient < 0)
+			return ERROR;
+
+		sprintf(str, "meas started: %s", orientation_strs[orient]);
+		mavlink_log_info(mavlink_fd, str);
+		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
+		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
+		mavlink_log_info(mavlink_fd, str);
+		data_collected[orient] = true;
+		tune_neutral();
+	}
+	close(sensor_combined_sub);
+
+	/* calculate offsets and rotation+scale matrix */
+	float accel_T[3][3];
+	int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+	if (res != 0) {
+		mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error");
+		return ERROR;
+	}
+
+	/* convert accel transform matrix to scales,
+	 * rotation part of transform matrix is not used by now
+	 */
+	for (int i = 0; i < 3; i++) {
+		accel_scale[i] = accel_T[i][i];
+	}
+
+	return OK;
+}
+
+/*
+ * Wait for vehicle become still and detect it's orientation.
+ *
+ * @return 0..5 according to orientation when vehicle is still and ready for measurements,
+ * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
+ */
+int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
+	struct sensor_combined_s sensor;
+	/* exponential moving average of accel */
+	float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
+	/* max-hold dispersion of accel */
+	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
+	/* EMA time constant in seconds*/
+	float ema_len = 0.2f;
+	/* set "still" threshold to 0.1 m/s^2 */
+	float still_thr2 = pow(0.1f, 2);
+	/* set accel error threshold to 5m/s^2 */
+	float accel_err_thr = 5.0f;
+	/* still time required in us */
+	int64_t still_time = 2000000;
+	struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+
+	hrt_abstime t_start = hrt_absolute_time();
+	/* set timeout to 30s */
+	hrt_abstime timeout = 30000000;
+	hrt_abstime t_timeout = t_start + timeout;
+	hrt_abstime t = t_start;
+	hrt_abstime t_prev = t_start;
+	hrt_abstime t_still = 0;
+	while (true) {
+		/* wait blocking for new data */
+		int poll_ret = poll(fds, 1, 1000);
+		if (poll_ret) {
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
+			t = hrt_absolute_time();
+			float dt = (t - t_prev) / 1000000.0f;
+			t_prev = t;
+			float w = dt / ema_len;
+			for (int i = 0; i < 3; i++) {
+				accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
+				float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
+				d = d * d;
+				accel_disp[i] = accel_disp[i] * (1.0f - w);
+				if (d > accel_disp[i])
+					accel_disp[i] = d;
+			}
+			/* still detector with hysteresis */
+			if (  accel_disp[0] < still_thr2 &&
+				  accel_disp[1] < still_thr2 &&
+				  accel_disp[2] < still_thr2 ) {
+				/* is still now */
+				if (t_still == 0) {
+					/* first time */
+					mavlink_log_info(mavlink_fd, "still...");
+					t_still = t;
+					t_timeout = t + timeout;
+				} else {
+					/* still since t_still */
+					if ((int64_t) t - (int64_t) t_still > still_time) {
+						/* vehicle is still, exit from the loop to detection of its orientation */
+						break;
+					}
+				}
+			} else if ( accel_disp[0] > still_thr2 * 2.0f ||
+					    accel_disp[1] > still_thr2 * 2.0f ||
+					    accel_disp[2] > still_thr2 * 2.0f) {
+				/* not still, reset still start time */
+				if (t_still != 0) {
+					mavlink_log_info(mavlink_fd, "moving...");
+					t_still = 0;
+				}
+			}
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_info(mavlink_fd, "ERROR: poll failure");
+			return -3;
+		}
+		if (t > t_timeout) {
+			mavlink_log_info(mavlink_fd, "ERROR: timeout");
+			return -1;
+		}
+	}
+
+	if (  fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabs(accel_ema[1]) < accel_err_thr &&
+		  fabs(accel_ema[2]) < accel_err_thr  )
+		return 0;	// [ g, 0, 0 ]
+	if (  fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabs(accel_ema[1]) < accel_err_thr &&
+		  fabs(accel_ema[2]) < accel_err_thr  )
+		return 1;	// [ -g, 0, 0 ]
+	if (  fabs(accel_ema[0]) < accel_err_thr &&
+		  fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabs(accel_ema[2]) < accel_err_thr  )
+		return 2;	// [ 0, g, 0 ]
+	if (  fabs(accel_ema[0]) < accel_err_thr &&
+		  fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabs(accel_ema[2]) < accel_err_thr  )
+		return 3;	// [ 0, -g, 0 ]
+	if (  fabs(accel_ema[0]) < accel_err_thr &&
+		  fabs(accel_ema[1]) < accel_err_thr &&
+		  fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
+		return 4;	// [ 0, 0, g ]
+	if (  fabs(accel_ema[0]) < accel_err_thr &&
+		  fabs(accel_ema[1]) < accel_err_thr &&
+		  fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
+		return 5;	// [ 0, 0, -g ]
+
+	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
+
+	return -2;	// Can't detect orientation
+}
+
+/*
+ * Read specified number of accelerometer samples, calculate average and dispersion.
+ */
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
+	struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
+	int count = 0;
+	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
+
+	while (count < samples_num) {
+		int poll_ret = poll(fds, 1, 1000);
+		if (poll_ret == 1) {
+			struct sensor_combined_s sensor;
+			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+			for (int i = 0; i < 3; i++)
+				accel_sum[i] += sensor.accelerometer_m_s2[i];
+			count++;
+		} else {
+			return ERROR;
+		}
+	}
+
+	for (int i = 0; i < 3; i++) {
+		accel_avg[i] = accel_sum[i] / count;
+	}
+
+	return OK;
+}
+
+int mat_invert3(float src[3][3], float dst[3][3]) {
+	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
+			    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
+			    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
+	if (det == 0.0)
+		return ERROR;	// Singular matrix
+
+	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
+	dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
+	dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
+	dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
+	dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
+	dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
+	dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
+	dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
+	dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
+
+	return OK;
+}
+
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
+	/* calculate offsets */
+	for (int i = 0; i < 3; i++) {
+		accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
+	}
+
+	/* fill matrix A for linear equations system*/
+	float mat_A[3][3];
+	memset(mat_A, 0, sizeof(mat_A));
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			float a = accel_ref[i * 2][j] - accel_offs[j];
+			mat_A[i][j] = a;
+		}
+	}
+
+	/* calculate inverse matrix for A */
+	float mat_A_inv[3][3];
+	if (mat_invert3(mat_A, mat_A_inv) != OK)
+		return ERROR;
+
+	/* copy results to accel_T */
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			/* simplify matrices mult because b has only one non-zero element == g at index i */
+			accel_T[j][i] = mat_A_inv[j][i] * g;
+		}
+	}
+
+	return OK;
+}
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
new file mode 100644
index 000000000..6a920c4ef
--- /dev/null
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -0,0 +1,16 @@
+/*
+ * accelerometer_calibration.h
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#ifndef ACCELEROMETER_CALIBRATION_H_
+#define ACCELEROMETER_CALIBRATION_H_
+
+#include <stdint.h>
+#include <uORB/topics/vehicle_status.h>
+
+void do_accel_calibration(int mavlink_fd);
+
+#endif /* ACCELEROMETER_CALIBRATION_H_ */
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..a34e526a8
--- /dev/null
+++ b/src/modules/commander/commander.c
@@ -0,0 +1,2269 @@
+/****************************************************************************
+ *
+ *   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 <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 <drivers/drv_led.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/cpuload.h>
+
+#include "state_machine_helper.h"
+
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
+
+#include "calibration_routines.h"
+#include "accelerometer_calibration.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);
+
+
+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)
+
+#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */
+
+/* File descriptors */
+static int leds;
+static int buzzer;
+static int mavlink_fd;
+
+/* flags */
+static bool commander_initialized = false;
+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 */
+
+/* Main state machine */
+static struct vehicle_status_s current_status;
+static orb_advert_t stat_pub;
+
+/* timout until lowlevel failsafe */
+static unsigned int failsafe_lowlevel_timeout_ms;
+
+/* tasks waiting for low prio thread */
+enum {
+	LOW_PRIO_TASK_NONE = 0,
+	LOW_PRIO_TASK_PARAM_SAVE,
+	LOW_PRIO_TASK_PARAM_LOAD,
+	LOW_PRIO_TASK_GYRO_CALIBRATION,
+	LOW_PRIO_TASK_MAG_CALIBRATION,
+	LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
+	LOW_PRIO_TASK_RC_CALIBRATION,
+	LOW_PRIO_TASK_ACCEL_CALIBRATION,
+	LOW_PRIO_TASK_AIRSPEED_CALIBRATION
+} low_prio_task;
+
+
+/* pthread loops */
+void *orb_receive_loop(void *arg);
+void *commander_low_prio_loop(void *arg);
+
+__EXPORT int commander_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of commander.
+ */
+int commander_thread_main(int argc, char *argv[]);
+
+int buzzer_init(void);
+void buzzer_deinit(void);
+int led_init(void);
+void led_deinit(void);
+int led_toggle(int led);
+int led_on(int led);
+int led_off(int led);
+void tune_error(void);
+void tune_positive(void);
+void tune_neutral(void);
+void tune_negative(void);
+void do_reboot(void);
+void do_gyro_calibration(void);
+void do_mag_calibration(void);
+void do_rc_calibration(void);
+void do_airspeed_calibration(void);
+
+
+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.
+ */
+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);
+
+int buzzer_init()
+{
+	buzzer = open("/dev/tone_alarm", O_WRONLY);
+
+	if (buzzer < 0) {
+		warnx("Buzzer: open fail\n");
+		return ERROR;
+	}
+
+	return 0;
+}
+
+void buzzer_deinit()
+{
+	close(buzzer);
+}
+
+
+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;
+}
+
+void led_deinit()
+{
+	close(leds);
+}
+
+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);
+}
+
+int led_on(int led)
+{
+	return ioctl(leds, LED_ON, led);
+}
+
+int led_off(int led)
+{
+	return ioctl(leds, LED_OFF, led);
+}
+
+
+void tune_error()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 2);
+}
+
+void tune_positive()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 3);
+}
+
+void tune_neutral()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 4);
+}
+
+void tune_negative()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 5);
+}
+
+void do_reboot()
+{
+	mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
+	usleep(500000);
+	up_systemreset();
+	/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
+}
+
+
+void do_rc_calibration()
+{
+	mavlink_log_info(mavlink_fd, "trim calibration starting");
+
+	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 */
+	int save_ret = param_save_default();
+
+	if (save_ret != 0) {
+		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+	}
+
+	tune_positive();
+
+	mavlink_log_info(mavlink_fd, "trim calibration done");
+}
+
+void do_mag_calibration()
+{
+	mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
+
+	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);
+
+	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;
+	}
+
+	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_neutral();
+
+			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_positive();
+		/* third beep by cal end routine */
+
+	} else {
+		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
+	}
+
+	close(sub_mag);
+}
+
+void do_gyro_calibration()
+{
+	mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
+
+	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;
+
+	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_positive();
+		/* third beep by cal end routine */
+
+	} else {
+		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
+	}
+
+	close(sub_sensor_combined);
+}
+
+
+void do_airspeed_calibration()
+{
+	/* give directions */
+	mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
+
+	const int calibration_count = 2500;
+
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
+
+	int calibration_counter = 0;
+	float diff_pres_offset = 0.0f;
+
+	while (calibration_counter < calibration_count) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+			diff_pres_offset += diff_pres.differential_pressure_pa;
+			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;
+		}
+	}
+
+	diff_pres_offset = diff_pres_offset / calibration_count;
+
+	if (isfinite(diff_pres_offset)) {
+
+		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_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_positive();
+
+	} else {
+		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+	}
+
+	close(diff_pres_sub);
+}
+
+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;
+
+	/* request to set different system mode */
+	switch (cmd->command) {
+		case VEHICLE_CMD_DO_SET_MODE:
+
+			/* request to activate HIL */
+			if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) {
+
+				if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
+			}
+
+			if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
+			} else {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) {
+					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 == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
+			/* request to disarm */
+			} else if ((int)cmd->param1 == 0) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
+			}
+
+			break;
+
+		case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
+
+			/* request for an autopilot reboot */
+			if ((int)cmd->param1 == 1) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) {
+						/* reboot is executed later, after positive tune is sent */
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
+						tune_positive();
+						/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
+						do_reboot();
+
+					} else {
+						/* system may return here */
+						result = VEHICLE_CMD_RESULT_DENIED;
+						tune_negative();
+					}
+				}
+			}
+			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:
+
+			/* gyro calibration */
+			if ((int)(cmd->param1) == 1) {
+
+				/* check if no other task is scheduled */
+				if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+					/* try to go to INIT/PREFLIGHT arming state */
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
+						/* now set the task for the low prio thread */
+						low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION;
+					} else {
+						result = VEHICLE_CMD_RESULT_DENIED;
+					}
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+			}
+
+			/* magnetometer calibration */
+			if ((int)(cmd->param2) == 1) {
+
+				/* check if no other task is scheduled */
+				if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+					/* try to go to INIT/PREFLIGHT arming state */
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
+						/* now set the task for the low prio thread */
+						low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION;
+					} else {
+						result = VEHICLE_CMD_RESULT_DENIED;
+					}
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+			}
+
+
+			// /* zero-altitude pressure calibration */
+			// if ((int)(cmd->param3) == 1) {
+
+			// 	/* check if no other task is scheduled */
+			// 	if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+			// 		/* try to go to INIT/PREFLIGHT arming state */
+			// 		if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+			// 			result = VEHICLE_CMD_RESULT_ACCEPTED;
+			// 			/* now set the task for the low prio thread */
+			// 			low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION;
+			// 		} else {
+			// 			result = VEHICLE_CMD_RESULT_DENIED;
+			// 		}
+			// 	} else {
+			// 		result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			// 	}
+			// }
+
+
+
+			// /* trim calibration */
+			// if ((int)(cmd->param4) == 1) {
+
+			// 	/* check if no other task is scheduled */
+			// 	if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+			// 		/* try to go to INIT/PREFLIGHT arming state */
+			// 		if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+			// 			result = VEHICLE_CMD_RESULT_ACCEPTED;
+			// 			/* now set the task for the low prio thread */
+			// 			low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION;
+			// 		} else {
+			// 			result = VEHICLE_CMD_RESULT_DENIED;
+			// 		}
+			// 	} else {
+			// 		result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			// 	}
+			// }
+
+
+			/* accel calibration */
+			if ((int)(cmd->param5) == 1) {
+
+				/* check if no other task is scheduled */
+				if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+					/* try to go to INIT/PREFLIGHT arming state */
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
+						/* now set the task for the low prio thread */
+						low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION;
+					} else {
+						result = VEHICLE_CMD_RESULT_DENIED;
+					}
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+			}
+
+			/* airspeed calibration */
+			if ((int)(cmd->param6) == 1) {
+
+				/* check if no other task is scheduled */
+				if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+					/* try to go to INIT/PREFLIGHT arming state */
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
+						/* now set the task for the low prio thread */
+						low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION;
+					} else {
+						result = VEHICLE_CMD_RESULT_DENIED;
+					}
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+			}
+			break;
+
+		case VEHICLE_CMD_PREFLIGHT_STORAGE:
+
+			if (((int)(cmd->param1)) == 0) {
+
+				/* check if no other task is scheduled */
+				if(low_prio_task == LOW_PRIO_TASK_NONE) {
+					low_prio_task = LOW_PRIO_TASK_PARAM_LOAD;
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+
+
+			} else if (((int)(cmd->param1)) == 1) {
+
+				/* check if no other task is scheduled */
+				if(low_prio_task == LOW_PRIO_TASK_NONE) {
+					low_prio_task = LOW_PRIO_TASK_PARAM_LOAD;
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+			}
+		break;
+
+	default:
+		mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
+		result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+		break;
+	}
+
+	/* supported command handling stop */
+	if (result == VEHICLE_CMD_RESULT_FAILED ||
+	    result == VEHICLE_CMD_RESULT_DENIED ||
+	    result == VEHICLE_CMD_RESULT_UNSUPPORTED ||
+	    result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
+
+		tune_negative();
+
+	} else if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+
+		tune_positive();
+	}
+
+	/* send any requested ACKs */
+	if (cmd->confirmation > 0) {
+		/* send acknowledge command */
+		// XXX TODO
+	}
+
+}
+
+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;
+}
+
+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("[commander] starting");
+
+	/* pthreads for command and subsystem info handling */
+	// pthread_t command_handling_thread;
+	pthread_t subsystem_info_thread;
+	pthread_t commander_low_prio_thread;
+
+	/* initialize */
+	if (led_init() != 0) {
+		warnx("ERROR: Failed to initialize leds");
+	}
+
+	if (buzzer_init() != 0) {
+		warnx("ERROR: Failed to initialize buzzer");
+	}
+
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	if (mavlink_fd < 0) {
+		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.");
+	}
+
+	/* make sure we are in preflight state */
+	memset(&current_status, 0, sizeof(current_status));
+
+
+	current_status.navigation_state = NAVIGATION_STATE_INIT;
+	current_status.arming_state = ARMING_STATE_INIT;
+	current_status.hil_state = HIL_STATE_OFF;
+	current_status.flag_hil_enabled = false;
+	current_status.flag_fmu_armed = false;
+	current_status.flag_io_armed = false; // XXX read this from somewhere
+
+	/* 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;
+
+	// XXX for now just set sensors as initialized
+	current_status.condition_system_sensors_initialized = true;
+
+	/* 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);
+	}
+
+	// XXX needed?
+	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);
+
+	pthread_attr_t commander_low_prio_attr;
+	pthread_attr_init(&commander_low_prio_attr);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2048);
+
+	struct sched_param param;
+	/* low priority */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
+	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
+	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+
+	/* Start monitoring loop */
+	uint16_t counter = 0;
+
+	/* 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;
+	float bat_remain = 1.0f;
+
+	uint16_t stick_off_counter = 0;
+	uint16_t stick_on_counter = 0;
+
+	/* XXX what exactly is this for? */
+	uint64_t last_print_time = 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));
+
+	/* Subscribe to global position */
+	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;
+
+	/* Subscribe to local position data */
+	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.
+	 */
+
+	/* Subscribe to GPS topic */
+	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	struct vehicle_gps_position_s gps_position;
+	memset(&gps_position, 0, sizeof(gps_position));
+
+	/* Subscribe to sensor topic */
+	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	struct sensor_combined_s sensors;
+	memset(&sensors, 0, sizeof(sensors));
+
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
+	memset(&diff_pres, 0, sizeof(diff_pres));
+	uint64_t last_diff_pres_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();
+
+	/* XXX use this! */
+	//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(diff_pres_sub, &new_data);
+
+		if (new_data) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+			last_diff_pres_time = diff_pres.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_fmu_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;
+		}
+
+		/* set the condition to valid if there has recently been a local position estimate */
+		if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) {
+			current_status.condition_local_position_valid = true;
+		} else {
+			current_status.condition_local_position_valid = false;
+		}
+
+		/* 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) {
+
+			/* XXX if armed */
+			if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
+			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
+			     current_status.state_machine == SYSTEM_STATE_MANUAL*/)) {
+				/* armed, solid */
+				led_on(LED_AMBER);
+
+			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+				/* not armed */
+				led_toggle(LED_AMBER);
+			}
+
+			if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
+
+				/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
+				if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
+					&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
+					/* GPS lock */
+					led_on(LED_BLUE);
+
+				} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+					/* no GPS lock, but GPS module is aquiring lock */
+					led_toggle(LED_BLUE);
+				}
+
+			} else {
+				/* no GPS info, don't light the blue led */
+				led_off(LED_BLUE);
+			}
+
+			/* toggle GPS led at 5 Hz in HIL mode */
+			if (current_status.flag_hil_enabled) {
+				/* hil enabled */
+				led_toggle(LED_BLUE);
+
+			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
+				/* toggle arming (red) at 5 Hz on low battery or error */
+				led_toggle(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_positive();
+
+		} 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, 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;
+				// XXX implement this
+				// 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 */
+
+		/* If in INIT state, try to proceed to STANDBY state */
+		if (current_status.arming_state == ARMING_STATE_INIT) {
+			// XXX check for sensors
+			arming_state_transition(stat_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
+		} else {
+			// XXX: Add emergency stuff if sensors are lost
+		}
+
+
+		/*
+		 * 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.condition_global_position_valid;
+		bool local_pos_valid = current_status.condition_local_position_valid;
+		bool airspeed_valid = current_status.condition_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.condition_global_position_valid = true;
+			// XXX check for controller status and home position as well
+
+		} else {
+			current_status.condition_global_position_valid = false;
+		}
+
+		if (hrt_absolute_time() - last_local_position_time < 2000000) {
+			current_status.condition_local_position_valid = true;
+			// XXX check for controller status and home position as well
+
+		} else {
+			current_status.condition_local_position_valid = false;
+		}
+
+		/* Check for valid airspeed/differential pressure measurements */
+		if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
+			current_status.condition_airspeed_valid = true;
+
+		} else {
+			current_status.condition_airspeed_valid = false;
+		}
+
+		/*
+		 * Consolidate global position and local position valid flags
+		 * for vector flight mode.
+		 */
+		// if (current_status.condition_local_position_valid ||
+		//     current_status.condition_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 (global_pos_valid != current_status.condition_global_position_valid ||
+		    local_pos_valid != current_status.condition_local_position_valid ||
+		    airspeed_valid != current_status.condition_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(gps_sub, &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) // XXX note that vdop is 0 for mtk
+				&& !home_position_set
+				&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
+				&& !current_status.flag_fmu_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_positive();
+			}
+		}
+
+		/* 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.mode_switch)) {
+
+					warnx("mode sw not finite");
+					/* no valid stick position, go to default */
+
+				} else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
+
+					/* bottom stick position, go to manual mode */
+					current_status.mode_switch = MODE_SWITCH_MANUAL;
+					printf("mode switch: manual\n");
+
+				} else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
+
+					/* top stick position, set auto/mission for all vehicle types */
+					current_status.mode_switch = MODE_SWITCH_AUTO;
+					printf("mode switch: auto\n");
+
+				} else {
+
+					/* center stick position, set seatbelt/simple control */
+					current_status.mode_switch = MODE_SWITCH_SEATBELT;
+					printf("mode switch: seatbelt\n");
+				}
+
+				// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
+
+				/*
+				* Check if land/RTL is requested
+				*/
+				if (!isfinite(sp_man.return_switch)) {
+
+					/* this switch is not properly mapped, set default */
+					current_status.return_switch = RETURN_SWITCH_NONE;
+
+				} else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) {
+
+					/* bottom stick position, set altitude hold */
+					current_status.return_switch = RETURN_SWITCH_NONE;
+
+				} else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) {
+
+					/* top stick position */
+					current_status.return_switch = RETURN_SWITCH_RETURN;
+
+				} else {
+					/* center stick position, set default */
+					current_status.return_switch = RETURN_SWITCH_NONE;
+				}
+
+				/* check mission switch  */
+				if (!isfinite(sp_man.mission_switch)) {
+
+					/* this switch is not properly mapped, set default */
+					current_status.mission_switch = MISSION_SWITCH_NONE;
+
+				} else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) {
+
+					/* top switch position */
+					current_status.mission_switch = MISSION_SWITCH_MISSION;
+
+				} else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) {
+
+					/* bottom switch position */
+					current_status.mission_switch = MISSION_SWITCH_NONE;
+
+				} else {
+
+					/* center switch position, set default */
+					current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default?
+				}
+
+				/* Now it's time to handle the stick inputs */
+				
+				switch (current_status.arming_state) {
+			
+					/* evaluate the navigation state when disarmed */
+					case ARMING_STATE_STANDBY:
+			
+						/* just manual, XXX this might depend on the return switch */
+						if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
+							
+							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
+								// These is not supposed to happen
+								warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
+							}
+			
+						/* Try seatbelt or fallback to manual */
+						} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
+			
+							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
+								// fallback to MANUAL_STANDBY
+								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
+								// These is not supposed to happen
+								warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
+								}
+							}
+			
+						/* Try auto or fallback to seatbelt or even manual */
+						} else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
+							
+							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) {
+								// first fallback to SEATBELT_STANDY
+								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
+									// or fallback to MANUAL_STANDBY
+									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
+									// These is not supposed to happen
+									warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
+									}
+								}
+							}
+						}
+			
+						break;
+			
+					/* evaluate the navigation state when armed */
+					case ARMING_STATE_ARMED:
+			
+						/* Always accept manual mode */
+						if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
+
+							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+								// These is not supposed to happen
+								warnx("ERROR: Navigation state MANUAL rejected");
+							}
+			
+						/* SEATBELT_STANDBY (fallback: MANUAL) */
+						} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
+								&& current_status.return_switch == RETURN_SWITCH_NONE) {
+			
+							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
+								// fallback to MANUAL_STANDBY
+								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+								// These is not supposed to happen
+								warnx("ERROR: Navigation state MANUAL rejected");
+								}
+							}
+			
+						/* SEATBELT_DESCENT (fallback: MANUAL) */
+						} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
+								&& current_status.return_switch == RETURN_SWITCH_RETURN) {
+			
+							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
+								// fallback to MANUAL_STANDBY
+								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+								// These is not supposed to happen
+								warnx("ERROR: Navigation state MANUAL rejected");
+								}
+							}
+			
+						/* AUTO_LOITER (fallback: SEATBELT, MANUAL) */
+						} else if (current_status.mode_switch == MODE_SWITCH_AUTO
+								&& current_status.return_switch == RETURN_SWITCH_NONE
+								&& current_status.mission_switch == MISSION_SWITCH_NONE) {
+			
+							/* we might come from the disarmed state AUTO_STANDBY */
+							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) {
+								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
+									// fallback to MANUAL_STANDBY
+									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+									// These is not supposed to happen
+									warnx("ERROR: Navigation state MANUAL rejected");
+									}
+								}
+							/* or from some other armed state like SEATBELT or MANUAL */
+							} else if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) {
+								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
+									// fallback to MANUAL_STANDBY
+									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+									// These is not supposed to happen
+									warnx("ERROR: Navigation state MANUAL rejected");
+									}
+								}
+							}
+			
+						/* AUTO_MISSION (fallback: SEATBELT, MANUAL) */
+						} else if (current_status.mode_switch == MODE_SWITCH_AUTO
+								&& current_status.return_switch == RETURN_SWITCH_NONE
+								&& current_status.mission_switch == MISSION_SWITCH_MISSION) {
+							
+							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) {
+								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
+									// fallback to MANUAL_STANDBY
+									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+									// These is not supposed to happen
+									warnx("ERROR: Navigation state MANUAL rejected");
+									}
+								}
+							}
+			
+						/* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */
+						} else if (current_status.mode_switch == MODE_SWITCH_AUTO
+								&& current_status.return_switch == RETURN_SWITCH_RETURN
+								&& (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) {
+			
+							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) {
+								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
+									// fallback to MANUAL_STANDBY
+									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+									// These is not supposed to happen
+									warnx("ERROR: Navigation state MANUAL rejected");
+									}
+								}
+							}
+						}
+						break;
+
+						// XXX we might be missing something that triggers a transition from RTL to LAND
+			
+					case ARMING_STATE_ARMED_ERROR:
+			
+						// XXX work out fail-safe scenarios
+						break;
+			
+					case ARMING_STATE_STANDBY_ERROR:
+			
+						// XXX work out fail-safe scenarios
+						break;
+			
+					case ARMING_STATE_REBOOT:
+			
+						// XXX I don't think we should end up here
+						break;
+			
+					case ARMING_STATE_IN_AIR_RESTORE:
+			
+						// XXX not sure what to do here
+						break;
+					default:
+						break;
+				}
+				/* 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!");
+					}
+				}
+
+				/*
+				* 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) {
+						arming_state_transition(stat_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
+						stick_off_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) {
+						arming_state_transition(stat_pub, &current_status, ARMING_STATE_ARMED, mavlink_fd);
+						stick_on_counter = 0;
+
+					} else {
+						stick_on_counter++;
+						stick_off_counter = 0;
+					}
+				}
+
+				current_status.rc_signal_cutting_off = false;
+				current_status.rc_signal_lost = false;
+				current_status.rc_signal_lost_interval = 0;
+
+			} else {
+
+				/* 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!");
+
+					if (!current_status.rc_signal_cutting_off) {
+						printf("Reason: not rc_signal_cutting_off\n");
+					} else {
+						printf("last print time: %llu\n", last_print_time);
+					}
+
+					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_positive();
+
+					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_positive();
+					}
+				}
+
+				current_status.offboard_control_signal_weak = false;
+				current_status.offboard_control_signal_lost = false;
+				current_status.offboard_control_signal_lost_interval = 0;
+
+				// XXX check if this is correct
+				/* arm / disarm on request */
+				if (sp_offboard.armed && !current_status.flag_fmu_armed) {
+
+					arming_state_transition(stat_pub, &current_status, ARMING_STATE_ARMED, mavlink_fd);
+
+				} else if (!sp_offboard.armed && current_status.flag_fmu_armed) {
+
+					arming_state_transition(stat_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
+				}
+
+			} else {
+
+				/* 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_positive();
+
+					/* 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();
+
+
+		// XXX this is missing
+		/* 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) {
+
+			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;
+		/* XXX use this voltage_previous */
+		fflush(stdout);
+		counter++;
+		usleep(COMMANDER_MONITORING_INTERVAL);
+	}
+
+	/* wait for threads to complete */
+	// pthread_join(command_handling_thread, NULL);
+	pthread_join(subsystem_info_thread, NULL);
+	pthread_join(commander_low_prio_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");
+	fflush(stdout);
+
+	thread_running = false;
+
+	return 0;
+}
+
+
+void *commander_low_prio_loop(void *arg)
+{
+	/* Set thread name */
+	prctl(PR_SET_NAME, "commander low prio", getpid());
+
+	while (!thread_should_exit) {
+
+		switch (low_prio_task) {
+
+			case LOW_PRIO_TASK_PARAM_LOAD:
+
+				if (0 == param_load_default()) {
+					mavlink_log_info(mavlink_fd, "Param load success");
+				} else {
+					mavlink_log_critical(mavlink_fd, "Param load ERROR");
+					tune_error();
+				}
+				low_prio_task = LOW_PRIO_TASK_NONE;
+				break;
+
+			case LOW_PRIO_TASK_PARAM_SAVE:
+
+				if (0 == param_save_default()) {
+					mavlink_log_info(mavlink_fd, "Param save success");
+				} else {
+					mavlink_log_critical(mavlink_fd, "Param save ERROR");
+					tune_error();
+				}
+				low_prio_task = LOW_PRIO_TASK_NONE;
+				break;
+
+			case LOW_PRIO_TASK_GYRO_CALIBRATION:
+
+				do_gyro_calibration();
+
+				low_prio_task = LOW_PRIO_TASK_NONE;
+				break;
+
+			case LOW_PRIO_TASK_MAG_CALIBRATION:
+
+				do_mag_calibration();
+
+				low_prio_task = LOW_PRIO_TASK_NONE;
+				break;
+
+			case LOW_PRIO_TASK_ALTITUDE_CALIBRATION:
+
+				// do_baro_calibration();
+
+			case LOW_PRIO_TASK_RC_CALIBRATION:
+
+				// do_rc_calibration();
+
+				low_prio_task = LOW_PRIO_TASK_NONE;
+				break;
+
+			case LOW_PRIO_TASK_ACCEL_CALIBRATION:
+
+				do_accel_calibration(mavlink_fd);
+
+				low_prio_task = LOW_PRIO_TASK_NONE;
+				break;
+
+			case LOW_PRIO_TASK_AIRSPEED_CALIBRATION:
+
+				do_airspeed_calibration();
+
+				low_prio_task = LOW_PRIO_TASK_NONE;
+				break;
+
+			case LOW_PRIO_TASK_NONE:
+			default:
+				/* slow down to 10Hz */
+				usleep(100000);
+				break;
+		}
+
+	}
+
+	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..fe44e955a
--- /dev/null
+++ b/src/modules/commander/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Main system state machine
+#
+
+MODULE_COMMAND	 	= commander
+SRCS		 	= commander.c \
+			state_machine_helper.c \
+			calibration_routines.c \
+			accelerometer_calibration.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..daed81553
--- /dev/null
+++ b/src/modules/commander/state_machine_helper.c
@@ -0,0 +1,758 @@
+/****************************************************************************
+ *
+ *   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 <stdint.h>
+#include <stdbool.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <mavlink/mavlink_log.h>
+
+#include "state_machine_helper.h"
+
+int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) {
+	
+	int ret = ERROR;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_arming_state == current_state->arming_state) {
+		ret = OK;
+	} else {
+
+		switch (new_arming_state) {
+			case ARMING_STATE_INIT:
+
+				/* allow going back from INIT for calibration */
+				if (current_state->arming_state == ARMING_STATE_STANDBY) {
+					ret = OK;
+					current_state->flag_fmu_armed = false;
+				}
+				break;
+			case ARMING_STATE_STANDBY:
+
+				/* allow coming from INIT and disarming from ARMED */
+				if (current_state->arming_state == ARMING_STATE_INIT
+				 || current_state->arming_state == ARMING_STATE_ARMED) {
+
+					/* sensors need to be initialized for STANDBY state */
+					if (current_state->condition_system_sensors_initialized) {
+						ret = OK;
+						current_state->flag_fmu_armed = false;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized");
+					}
+				}
+				break;
+			case ARMING_STATE_ARMED:
+
+				/* allow arming from STANDBY and IN-AIR-RESTORE */
+				if (current_state->arming_state == ARMING_STATE_STANDBY
+				 || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
+
+					/* XXX conditions for arming? */
+					ret = OK;
+					current_state->flag_fmu_armed = true;
+				}
+				break;
+			case ARMING_STATE_ARMED_ERROR:
+
+				/* an armed error happens when ARMED obviously */
+				if (current_state->arming_state == ARMING_STATE_ARMED) {
+					
+					/* XXX conditions for an error state? */
+					ret = OK;
+					current_state->flag_fmu_armed = true;
+				}
+				break;
+			case ARMING_STATE_STANDBY_ERROR:
+				/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
+				if (current_state->arming_state == ARMING_STATE_STANDBY
+				 || current_state->arming_state == ARMING_STATE_INIT
+				 || current_state->arming_state == ARMING_STATE_ARMED_ERROR) {
+					ret = OK;
+					current_state->flag_fmu_armed = false;
+				}
+				break;
+			case ARMING_STATE_REBOOT:
+
+				/* an armed error happens when ARMED obviously */
+				if (current_state->arming_state == ARMING_STATE_INIT
+				 || current_state->arming_state == ARMING_STATE_STANDBY
+				 || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) {
+
+					ret = OK;
+					current_state->flag_fmu_armed = false;
+
+				}
+				break;
+			case ARMING_STATE_IN_AIR_RESTORE:
+
+				/* XXX implement */
+				break;
+			default:
+				break;
+		}
+
+		if (ret == OK) {
+			current_state->arming_state = new_arming_state;
+			state_machine_publish(status_pub, current_state, mavlink_fd);
+		}
+	}
+
+	return ret;
+}
+
+
+
+/*
+ * This functions does not evaluate any input flags but only checks if the transitions
+ * are valid.
+ */
+int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) {
+
+	int ret = ERROR;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_navigation_state == current_state->navigation_state) {
+		ret = OK;
+	} else {
+
+		switch (new_navigation_state) {
+			case NAVIGATION_STATE_INIT:
+
+				/* transitions back to INIT are possible for calibration */
+				if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) {
+
+					ret = OK;
+					current_state->flag_control_rates_enabled = false;
+					current_state->flag_control_attitude_enabled = false;
+					current_state->flag_control_velocity_enabled = false;
+					current_state->flag_control_position_enabled = false;
+					current_state->flag_control_manual_enabled = false;
+				}
+				break;
+
+			case NAVIGATION_STATE_MANUAL_STANDBY:
+
+				/* transitions from INIT and other STANDBY states as well as MANUAL are possible */
+				if (current_state->navigation_state == NAVIGATION_STATE_INIT
+				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
+
+					/* need to be disarmed first */
+					if (current_state->arming_state != ARMING_STATE_STANDBY) {
+						mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = false;
+						current_state->flag_control_position_enabled = false;
+						current_state->flag_control_manual_enabled = true;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_MANUAL:
+
+				/* need to be armed first */
+				if (current_state->arming_state != ARMING_STATE_ARMED) {
+					mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed");
+				} else {
+					ret = OK;
+					current_state->flag_control_rates_enabled = true;
+					current_state->flag_control_attitude_enabled = true;
+					current_state->flag_control_velocity_enabled = false;
+					current_state->flag_control_position_enabled = false;
+					current_state->flag_control_manual_enabled = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_SEATBELT_STANDBY:
+
+				/* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */
+				if (current_state->navigation_state == NAVIGATION_STATE_INIT
+				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) {
+
+					/* need to be disarmed and have a position estimate */
+					if (current_state->arming_state != ARMING_STATE_STANDBY) {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed");
+					} else if (!current_state->condition_local_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = true;
+						current_state->flag_control_position_enabled = false;
+						current_state->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_SEATBELT:
+
+				/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/
+				if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
+				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+
+					/* need to be armed and have a position estimate */
+					if (current_state->arming_state != ARMING_STATE_ARMED) {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed");
+					} else if (!current_state->condition_local_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = true;
+						current_state->flag_control_position_enabled = false;
+						current_state->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_SEATBELT_DESCENT:
+
+				/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */
+				if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+
+					/* need to be armed and have a position estimate */
+					if (current_state->arming_state != ARMING_STATE_ARMED) {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed");
+					} else if (!current_state->condition_local_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = true;
+						current_state->flag_control_position_enabled = false;
+						current_state->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_STANDBY:
+
+				/* transitions from INIT or from other STANDBY modes or from AUTO READY */
+				if (current_state->navigation_state == NAVIGATION_STATE_INIT
+				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) {
+
+					/* need to be disarmed and have a position and home lock */
+					if (current_state->arming_state != ARMING_STATE_STANDBY) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed");
+					} else if (!current_state->condition_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock");
+					} else if (!current_state->condition_home_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = true;
+						current_state->flag_control_position_enabled = true;
+						current_state->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_READY:
+
+				/* transitions from AUTO_STANDBY or AUTO_LAND */
+				if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+
+					// XXX flag test needed?
+
+					/* need to be armed and have a position and home lock */
+					if (current_state->arming_state != ARMING_STATE_ARMED) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = true;
+						current_state->flag_control_position_enabled = true;
+						current_state->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_TAKEOFF:
+
+				/* only transitions from AUTO_READY */
+				if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) {
+
+					ret = OK;
+					current_state->flag_control_rates_enabled = true;
+					current_state->flag_control_attitude_enabled = true;
+					current_state->flag_control_velocity_enabled = true;
+					current_state->flag_control_position_enabled = true;
+					current_state->flag_control_manual_enabled = false;
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_LOITER:
+
+				/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
+				if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
+
+					/* need to have a position and home lock */
+					if (!current_state->condition_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock");
+					} else if (!current_state->condition_home_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = true;
+						current_state->flag_control_position_enabled = true;
+						current_state->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_MISSION:
+
+				/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
+				if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
+
+					/* need to have a mission ready */
+					if (!current_state-> condition_auto_mission_available) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = true;
+						current_state->flag_control_position_enabled = true;
+						current_state->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_RTL:
+
+				/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
+				if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
+				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
+
+					/* need to have a position and home lock */
+					if (!current_state->condition_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock");
+					} else if (!current_state->condition_home_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = true;
+						current_state->flag_control_position_enabled = true;
+						current_state->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_LAND:
+				/* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */
+				if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
+
+					/* need to have a position and home lock */
+					if (!current_state->condition_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock");
+					} else if (!current_state->condition_home_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos");
+					} else {
+						ret = OK;
+						current_state->flag_control_rates_enabled = true;
+						current_state->flag_control_attitude_enabled = true;
+						current_state->flag_control_velocity_enabled = true;
+						current_state->flag_control_position_enabled = true;
+						current_state->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		if (ret == OK) {
+			current_state->navigation_state = new_navigation_state;
+			state_machine_publish(status_pub, current_state, mavlink_fd);
+		}
+	}
+
+	
+
+	return ret;
+}
+
+
+/**
+* Transition from one hil state to another
+*/
+int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state)
+{
+	bool valid_transition = false;
+	int ret = ERROR;
+
+	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
+
+	if (current_status->hil_state == new_state) {
+		warnx("Hil state not changed");
+		valid_transition = true;
+
+	} else {
+
+		switch (new_state) {
+
+			case HIL_STATE_OFF:
+
+				if (current_status->arming_state == ARMING_STATE_INIT
+				 || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+					current_status->flag_hil_enabled = false;
+					mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
+					valid_transition = true;
+				}
+				break;
+
+			case HIL_STATE_ON:
+
+				if (current_status->arming_state == ARMING_STATE_INIT
+				 || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+					current_status->flag_hil_enabled = true;
+					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+					valid_transition = true;
+				}
+				break;
+
+			default:
+				warnx("Unknown hil state");
+				break;
+		}
+	}
+
+	if (valid_transition) {
+		current_status->hil_state = new_state;
+		state_machine_publish(status_pub, current_status, mavlink_fd);
+		ret = OK;
+	} else {
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+	}
+
+	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);
+}
+
+// void publish_armed_status(const struct vehicle_status_s *current_status)
+// {
+// 	struct actuator_armed_s armed;
+// 	armed.armed = current_status->flag_system_armed;
+// 
+//	 /* XXX allow arming by external components on multicopters only if not yet armed by RC */
+//	/* XXX allow arming only if core sensors are ok */
+//	armed.ready_to_arm = true;
+// 
+//	/* 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);
+// }
+
+
+// /*
+//  * 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;
+// 	}
+
+// }
+
+
+
+///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
+//
+//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
+//{
+//	int 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_fmu_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_fmu_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_fmu_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;
+//}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
new file mode 100644
index 000000000..5b57cffb7
--- /dev/null
+++ b/src/modules/commander/state_machine_helper.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   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>
+
+void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
+
+void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd);
+int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd);
+
+int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state);
+
+#endif /* STATE_MACHINE_HELPER_H_ */
\ No newline at end of file