Shrinking the main commander file a bit

20 files changed, 1159 insertions, 564 deletions

diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c
index 231739962..7bae8ad40 100644
--- a/src/modules/commander/accelerometer_calibration.c
+++ b/src/modules/commander/accelerometer_calibration.c
@@ -70,15 +70,25 @@
  */
 
 #include "accelerometer_calibration.h"
+#include "commander_helper.h"
 
+#include <unistd.h>
+#include <stdio.h>
 #include <poll.h>
+#include <fcntl.h>
+#include <sys/prctl.h>
+#include <math.h>
+#include <string.h>
+
+
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_accel.h>
 #include <systemlib/conversions.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.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);
@@ -355,7 +365,7 @@ 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)
+	if (det == 0.0f)
 		return ERROR;	// Singular matrix
 
 	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
index 6a920c4ef..4175a25f4 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -9,7 +9,6 @@
 #define ACCELEROMETER_CALIBRATION_H_
 
 #include <stdint.h>
-#include <uORB/topics/vehicle_status.h>
 
 void do_accel_calibration(int mavlink_fd);
 
diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.c
new file mode 100644
index 000000000..feaf11aee
--- /dev/null
+++ b/src/modules/commander/airspeed_calibration.c
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file airspeed_calibration.c
+ * Airspeed sensor calibration routine
+ */
+
+#include "airspeed_calibration.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/differential_pressure.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+void do_airspeed_calibration(int mavlink_fd)
+{
+	/* 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);
+}
diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h
new file mode 100644
index 000000000..92f5651ec
--- /dev/null
+++ b/src/modules/commander/airspeed_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gyro_calibration.h
+ * Airspeed sensor calibration routine
+ */
+
+#ifndef AIRSPEED_CALIBRATION_H_
+#define AIRSPEED_CALIBRATION_H_
+
+#include <stdint.h>
+
+void do_airspeed_calibration(int mavlink_fd);
+
+#endif /* AIRSPEED_CALIBRATION_H_ */
\ No newline at end of file
diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.c
new file mode 100644
index 000000000..a70594794
--- /dev/null
+++ b/src/modules/commander/baro_calibration.c
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file baro_calibration.c
+ * Barometer calibration routine
+ */
+
+#include "baro_calibration.h"
+
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_baro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+
+void do_baro_calibration(int mavlink_fd)
+{
+	// TODO implement this
+	return;
+}
diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h
new file mode 100644
index 000000000..ac0f4be36
--- /dev/null
+++ b/src/modules/commander/baro_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag_calibration.h
+ * Barometer calibration routine
+ */
+
+#ifndef BARO_CALIBRATION_H_
+#define BARO_CALIBRATION_H_
+
+#include <stdint.h>
+
+void do_baro_calibration(int mavlink_fd);
+
+#endif /* BARO_CALIBRATION_H_ */
\ No newline at end of file
diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c
index a26938637..799cd4269 100644
--- a/src/modules/commander/calibration_routines.c
+++ b/src/modules/commander/calibration_routines.c
@@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 
 	return 0;
 }
+
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
index 4a5eb23ad..41baa34d5 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -91,15 +91,15 @@
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
 
+#include "commander_helper.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"
+#include "gyro_calibration.h"
+#include "mag_calibration.h"
+#include "baro_calibration.h"
+#include "rc_calibration.h"
+#include "airspeed_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 */
@@ -110,6 +110,9 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
 
 extern struct system_load_s system_load;
 
+#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
+#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
+
 /* 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))
@@ -130,7 +133,6 @@ extern struct system_load_s system_load;
 
 /* File descriptors */
 static int leds;
-static int buzzer;
 static int mavlink_fd;
 
 /* flags */
@@ -167,27 +169,17 @@ __EXPORT int commander_main(int argc, char *argv[]);
  */
 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 safety_pub, struct actuator_safety_s *safety);
 
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
+// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
 
 
 
@@ -206,23 +198,6 @@ void usage(const char *reason);
  */
 // 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()
 {
@@ -268,27 +243,6 @@ 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");
@@ -298,424 +252,6 @@ void do_reboot()
 }
 
 
-void do_rc_calibration()
-{
-	mavlink_log_info(mavlink_fd, "trim calibration starting");
-
-	/* XXX fix this */
-	// 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, int safety_pub, struct actuator_safety_s *safety)
 {
@@ -1182,7 +718,7 @@ int commander_thread_main(int argc, char *argv[])
 		warnx("ERROR: Failed to initialize leds");
 	}
 
-	if (buzzer_init() != 0) {
+	if (buzzer_init() != OK) {
 		warnx("ERROR: Failed to initialize buzzer");
 	}
 
@@ -1246,7 +782,12 @@ int commander_thread_main(int argc, char *argv[])
 	/* advertise to ORB */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
 	/* publish current state machine */
-	state_machine_publish(status_pub, &current_status, mavlink_fd);
+	
+	/* publish the new state */
+	current_status.counter++;
+	current_status.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(vehicle_status), status_pub, &current_status);
+
 
 	safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety);
 
@@ -1715,7 +1256,7 @@ int commander_thread_main(int argc, char *argv[])
 		// 	state_changed = true;
 		// }
 
-		orb_check(ORB_ID(vehicle_gps_position), &new_data);
+		orb_check(gps_sub, &new_data);
 		if (new_data) {
 
 
@@ -2222,18 +1763,18 @@ int commander_thread_main(int argc, char *argv[])
 		/* play tone according to evaluation result */
 		/* check if we recently armed */
 		if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) {
-			if (ioctl(buzzer, TONE_SET_ALARM, 12) == OK)
+			if (tune_arm() == OK)
 				arm_tune_played = true;
 
 		/* Trigger audio event for low battery */
 		} else if (bat_remain < 0.1f && battery_voltage_valid) {
-			if (ioctl(buzzer, TONE_SET_ALARM, 14) == OK)
+			if (tune_critical_bat() == OK)
 				battery_tune_played = true;
 		} else if (bat_remain < 0.2f && battery_voltage_valid) {
-			if (ioctl(buzzer, TONE_SET_ALARM, 13) == OK)
+			if (tune_low_bat() == OK)
 				battery_tune_played = true;
 		} else if(battery_tune_played) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
+			tune_stop();
 			battery_tune_played = false;
 		}
 
@@ -2305,25 +1846,25 @@ void *commander_low_prio_loop(void *arg)
 
 			case LOW_PRIO_TASK_GYRO_CALIBRATION:
 
-				do_gyro_calibration();
+				do_gyro_calibration(mavlink_fd);
 
 				low_prio_task = LOW_PRIO_TASK_NONE;
 				break;
 
 			case LOW_PRIO_TASK_MAG_CALIBRATION:
 
-				do_mag_calibration();
+				do_mag_calibration(mavlink_fd);
 
 				low_prio_task = LOW_PRIO_TASK_NONE;
 				break;
 
 			case LOW_PRIO_TASK_ALTITUDE_CALIBRATION:
 
-				// do_baro_calibration();
+				// do_baro_calibration(mavlink_fd);
 
 			case LOW_PRIO_TASK_RC_CALIBRATION:
 
-				// do_rc_calibration();
+				// do_rc_calibration(mavlink_fd);
 
 				low_prio_task = LOW_PRIO_TASK_NONE;
 				break;
@@ -2337,7 +1878,7 @@ void *commander_low_prio_loop(void *arg)
 
 			case LOW_PRIO_TASK_AIRSPEED_CALIBRATION:
 
-				do_airspeed_calibration();
+				do_airspeed_calibration(mavlink_fd);
 
 				low_prio_task = LOW_PRIO_TASK_NONE;
 				break;
diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h
index 04b4e72ab..6e57c0ba5 100644
--- a/src/modules/commander/commander.h
+++ b/src/modules/commander/commander.h
@@ -49,10 +49,6 @@
 #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/commander_helper.c b/src/modules/commander/commander_helper.c
new file mode 100644
index 000000000..a75b5dec3
--- /dev/null
+++ b/src/modules/commander/commander_helper.c
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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 commander_helper.c
+ * Commander helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <fcntl.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include "commander_helper.h"
+
+bool is_multirotor(const struct vehicle_status_s *current_status)
+{
+	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+}
+
+bool is_rotary_wing(const struct vehicle_status_s *current_status)
+{
+	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
+	|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+}
+
+static int buzzer;
+
+int buzzer_init()
+{
+	buzzer = open("/dev/tone_alarm", O_WRONLY);
+
+	if (buzzer < 0) {
+		warnx("Buzzer: open fail\n");
+		return ERROR;
+	}
+
+	return OK;
+}
+
+void buzzer_deinit()
+{
+	close(buzzer);
+}
+
+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);
+}
+
+int tune_arm()
+{
+	return ioctl(buzzer, TONE_SET_ALARM, 12);
+}
+
+int tune_critical_bat()
+{
+	return ioctl(buzzer, TONE_SET_ALARM, 14);
+}
+
+int tune_low_bat()
+{
+	return ioctl(buzzer, TONE_SET_ALARM, 13);
+}
+
+void tune_stop()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 0);
+}
+
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
new file mode 100644
index 000000000..ea96d72a6
--- /dev/null
+++ b/src/modules/commander/commander_helper.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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 commander_helper.h
+ * Commander helper functions definitions
+ */
+
+#ifndef COMMANDER_HELPER_H_
+#define COMMANDER_HELPER_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
+
+
+bool is_multirotor(const struct vehicle_status_s *current_status);
+bool is_rotary_wing(const struct vehicle_status_s *current_status);
+
+int buzzer_init(void);
+void buzzer_deinit(void);
+
+void tune_error(void);
+void tune_positive(void);
+void tune_neutral(void);
+void tune_negative(void);
+int tune_arm(void);
+int tune_critical_bat(void);
+int tune_low_bat(void);
+
+void tune_stop(void);
+
+#endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c
new file mode 100644
index 000000000..f452910c9
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.c
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gyro_calibration.c
+ * Gyroscope calibration routine
+ */
+
+#include "gyro_calibration.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_gyro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+
+void do_gyro_calibration(int mavlink_fd)
+{
+	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);
+}
diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h
new file mode 100644
index 000000000..cd262507d
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gyro_calibration.h
+ * Gyroscope calibration routine
+ */
+
+#ifndef GYRO_CALIBRATION_H_
+#define GYRO_CALIBRATION_H_
+
+#include <stdint.h>
+
+void do_gyro_calibration(int mavlink_fd);
+
+#endif /* GYRO_CALIBRATION_H_ */
\ No newline at end of file
diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.c
new file mode 100644
index 000000000..dbd31a7e7
--- /dev/null
+++ b/src/modules/commander/mag_calibration.c
@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag_calibration.c
+ * Magnetometer calibration routine
+ */
+
+#include "mag_calibration.h"
+#include "commander_helper.h"
+#include "calibration_routines.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_mag.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+
+void do_mag_calibration(int mavlink_fd)
+{
+	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);
+}
\ No newline at end of file
diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h
new file mode 100644
index 000000000..fd2085f14
--- /dev/null
+++ b/src/modules/commander/mag_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag_calibration.h
+ * Magnetometer calibration routine
+ */
+
+#ifndef MAG_CALIBRATION_H_
+#define MAG_CALIBRATION_H_
+
+#include <stdint.h>
+
+void do_mag_calibration(int mavlink_fd);
+
+#endif /* MAG_CALIBRATION_H_ */
\ No newline at end of file
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index fe44e955a..fef8e366b 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -38,6 +38,12 @@
 MODULE_COMMAND	 	= commander
 SRCS		 	= commander.c \
 			state_machine_helper.c \
+			commander_helper.c \
 			calibration_routines.c \
-			accelerometer_calibration.c
+			accelerometer_calibration.c \
+			gyro_calibration.c \
+			mag_calibration.c \
+			baro_calibration.c \
+			rc_calibration.c \
+			airspeed_calibration.c
 
diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.c
new file mode 100644
index 000000000..9aa6b86fe
--- /dev/null
+++ b/src/modules/commander/rc_calibration.c
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_calibration.c
+ * Remote Control calibration routine
+ */
+
+#include "rc_calibration.h"
+#include "commander_helper.h"
+
+#include <poll.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+
+void do_rc_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, "trim calibration starting");
+
+	/* XXX fix this */
+	// 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");
+}
\ No newline at end of file
diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h
new file mode 100644
index 000000000..6505c7364
--- /dev/null
+++ b/src/modules/commander/rc_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_calibration.h
+ * Remote Control calibration routine
+ */
+
+#ifndef RC_CALIBRATION_H_
+#define RC_CALIBRATION_H_
+
+#include <stdint.h>
+
+void do_rc_calibration(int mavlink_fd);
+
+#endif /* RC_CALIBRATION_H_ */
\ No newline at end of file
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
index 87aad6270..c15fc91a0 100644
--- a/src/modules/commander/state_machine_helper.c
+++ b/src/modules/commander/state_machine_helper.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
@@ -54,21 +54,8 @@
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"
-#include "commander.h"
+#include "commander_helper.h"
 
-bool is_multirotor(const struct vehicle_status_s *current_status)
-{
-	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
-}
-
-bool is_rotary_wing(const struct vehicle_status_s *current_status)
-{
-	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
-	|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
-}
 
 int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) {
 
@@ -568,7 +555,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status
 
 	if (valid_transition) {
 		current_status->hil_state = new_state;
-		state_machine_publish(status_pub, current_status, mavlink_fd);
+
+		current_status->counter++;
+		current_status->timestamp = hrt_absolute_time();
+
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 		ret = OK;
 	} else {
 		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
@@ -579,50 +570,6 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status
 
 
 
-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
 //  */
@@ -805,3 +752,4 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 //
 //	return ret;
 //}
+
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index b015c4efe..b553a4b56 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
@@ -50,15 +50,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 
 
-void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-bool is_multirotor(const struct vehicle_status_s *current_status);
-
-bool is_rotary_wing(const struct vehicle_status_s *current_status);
-
-//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, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd);
 
 int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd);