Merge remote-tracking branch 'px4/new_state_machine_drton' into fmuv2_bringup_new_state_machine_drton

Conflicts:
	src/drivers/blinkm/blinkm.cpp
	src/drivers/px4io/px4io.cpp
	src/modules/commander/state_machine_helper.c
	src/modules/px4iofirmware/protocol.h
	src/modules/px4iofirmware/registers.c
	src/modules/systemlib/systemlib.h
	src/systemcmds/reboot/reboot.c

86 files changed, 5855 insertions, 3942 deletions

diff --git a/Documentation/dsm_bind.odt b/Documentation/dsm_bind.odt
index 66ea1f1be..587a38883 100644
--- a/Documentation/dsm_bind.odt
+++ b/Documentation/dsm_bind.odt
diff --git a/Documentation/dsm_bind.pdf b/Documentation/dsm_bind.pdf
index e62d1ed83..76155569e 100644
--- a/Documentation/dsm_bind.pdf
+++ b/Documentation/dsm_bind.pdf
diff --git a/Documentation/flight_mode_state_machine.pdf b/Documentation/flight_mode_state_machine.pdf
index cd234ada7..af41953ee 100644
--- a/Documentation/flight_mode_state_machine.pdf
+++ b/Documentation/flight_mode_state_machine.pdf
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index db13cc197..d7d9a1ecb 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -50,6 +50,7 @@ MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/preflight_check
 MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/top
 MODULES		+= systemcmds/tests
@@ -75,7 +76,7 @@ MODULES		+= examples/flow_position_estimator
 #
 # Vehicle Control
 #
-MODULES		+= modules/segway
+#MODULES		+= modules/segway # XXX needs state machine update
 MODULES		+= modules/fixedwing_backside
 MODULES		+= modules/fixedwing_att_control
 MODULES		+= modules/fixedwing_pos_control
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 735bdb41a..b88f61ce8 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -53,8 +53,10 @@
 #include <sys/prctl.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
 #include <systemlib/systemlib.h>
 
@@ -243,16 +245,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int led_counter = 0;
 
 	/* declare and safely initialize all structs */
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 	struct actuator_armed_s armed;
+	//XXX is this necessairy?
 	armed.armed = false;
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	printf("[ardrone_interface] Motors initialized - ready.\n");
@@ -322,8 +322,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		} else {
 			/* MAIN OPERATION MODE */
 
-			/* get a local copy of the vehicle state */
-			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 			/* get a local copy of the actuator controls */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index b4c5fa06e..490002254 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -116,6 +116,8 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
 static const float MAX_CELL_VOLTAGE	= 4.3f;
@@ -375,7 +377,9 @@ BlinkM::led()
 {
 
 	static int vehicle_status_sub_fd;
+	static int vehicle_control_mode_sub_fd;
 	static int vehicle_gps_position_sub_fd;
+	static int actuator_armed_sub_fd;
 
 	static int num_of_cells = 0;
 	static int detected_cells_runcount = 0;
@@ -386,6 +390,8 @@ BlinkM::led()
 	static int led_interval = 1000;
 
 	static int no_data_vehicle_status = 0;
+	static int no_data_vehicle_control_mode = 0;
+	static int no_data_actuator_armed = 0;
 	static int no_data_vehicle_gps_position = 0;
 
 	static bool topic_initialized = false;
@@ -398,6 +404,12 @@ BlinkM::led()
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
 		orb_set_interval(vehicle_status_sub_fd, 1000);
 
+		vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
+		orb_set_interval(vehicle_control_mode_sub_fd, 1000);
+
+		actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
+		orb_set_interval(actuator_armed_sub_fd, 1000);
+
 		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
 		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
 
@@ -452,12 +464,16 @@ BlinkM::led()
 	if (led_thread_runcount == 15) {
 		/* obtained data for the first file descriptor */
 		struct vehicle_status_s vehicle_status_raw;
+		struct vehicle_control_mode_s vehicle_control_mode;
+		struct actuator_armed_s actuator_armed;
 		struct vehicle_gps_position_s vehicle_gps_position_raw;
 
 		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
 		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
 
 		bool new_data_vehicle_status;
+		bool new_data_vehicle_control_mode;
+		bool new_data_actuator_armed;
 		bool new_data_vehicle_gps_position;
 
 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -471,6 +487,28 @@ BlinkM::led()
 				no_data_vehicle_status = 3;
 		}
 
+		orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
+
+		if (new_data_vehicle_control_mode) {
+			orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
+			no_data_vehicle_control_mode = 0;
+		} else {
+			no_data_vehicle_control_mode++;
+			if(no_data_vehicle_control_mode >= 3)
+				no_data_vehicle_control_mode = 3;
+		}
+
+		orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
+
+		if (new_data_actuator_armed) {
+			orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
+			no_data_actuator_armed = 0;
+		} else {
+			no_data_actuator_armed++;
+			if(no_data_actuator_armed >= 3)
+				no_data_actuator_armed = 3;
+		}
+
 		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
 
 		if (new_data_vehicle_gps_position) {
@@ -498,7 +536,7 @@ BlinkM::led()
 				/* looking for lipo cells that are connected */
 				printf("<blinkm> checking cells\n");
 				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
-					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
+					if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
 				}
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
@@ -530,7 +568,7 @@ BlinkM::led()
 				} else {
 					/* no battery warnings here */
 
-					if(vehicle_status_raw.flag_system_armed == false) {
+					if(actuator_armed.armed == false) {
 						/* system not armed */
 						led_color_1 = LED_RED;
 						led_color_2 = LED_RED;
@@ -554,27 +592,24 @@ BlinkM::led()
 						led_color_8 = LED_OFF;
 						led_blink = LED_BLINK;
 
-						/* handle 4th led - flightmode indicator */
-						switch((int)vehicle_status_raw.flight_mode) {
-							case VEHICLE_FLIGHT_MODE_MANUAL:
-								led_color_4 = LED_AMBER;
-								break;
+						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 
-							case VEHICLE_FLIGHT_MODE_STAB:
-								led_color_4 = LED_YELLOW;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_HOLD:
-								led_color_4 = LED_BLUE;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_AUTO:
+							//XXX please check
+							if (vehicle_control_mode.flag_control_position_enabled)
 								led_color_4 = LED_GREEN;
-								break;
+							else if (vehicle_control_mode.flag_control_velocity_enabled)
+								led_color_4 = LED_BLUE;
+							else if (vehicle_control_mode.flag_control_attitude_enabled)
+								led_color_4 = LED_YELLOW;
+							else if (vehicle_control_mode.flag_control_manual_enabled)
+								led_color_4 = LED_AMBER;
+							else 
+								led_color_4 = LED_OFF;
+								
 						}
 
 						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
-							/* handling used sat´s */
+							/* handling used sat�s */
 							if(num_of_used_sats >= 7) {
 								led_color_1 = LED_OFF;
 								led_color_2 = LED_OFF;
@@ -834,10 +869,10 @@ BlinkM::get_firmware_version(uint8_t version[2])
 void blinkm_usage();
 
 void blinkm_usage() {
-	fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
-	fprintf(stderr, "options:\n");
-	fprintf(stderr, "\t-b --bus i2cbus (3)\n");
-	fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
+	warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}");
+	warnx("options:");
+	warnx("\t-b --bus i2cbus (3)");
+	warnx("\t-a --blinkmaddr blinkmaddr (9)");
 }
 
 int
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 52a667403..ec9d4ca09 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -118,11 +118,8 @@ ORB_DECLARE(output_pwm);
 /** start DSM bind */
 #define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 7)
 
-/** stop DSM bind */
-#define DSM_BIND_STOP	_IOC(_PWM_SERVO_BASE, 8)
-
 /** Power up DSM receiver */
-#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9)
+#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
 
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index bd027ce0b..3e30e3292 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,6 +75,7 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 4ad13fc04..1bc3e97a4 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -75,6 +75,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
 
 #include <systemlib/err.h>
@@ -135,7 +136,7 @@ private:
 	int 		_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_armed;
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
@@ -248,7 +249,7 @@ MK::MK(int bus) :
 	_update_rate(50),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_t_esc_status(0),
@@ -513,8 +514,8 @@ MK::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -540,7 +541,7 @@ MK::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
 	log("starting");
@@ -654,7 +655,7 @@ MK::task_main()
 			actuator_armed_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
 			mk_servo_arm(aa.armed && !aa.lockdown);
@@ -700,7 +701,7 @@ MK::task_main()
 	//::close(_t_esc_status);
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_armed);
 
 
 	/* make sure servos are off */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index a6f337486..6d4019f24 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -70,6 +70,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
 
 #include <systemlib/err.h>
 #ifdef HRT_PPM_CHANNEL
@@ -108,7 +109,7 @@ private:
 	unsigned	_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_armed;
 	orb_advert_t	_t_outputs;
 	orb_advert_t	_t_actuators_effective;
 	unsigned	_num_outputs;
@@ -195,7 +196,7 @@ PX4FMU::PX4FMU() :
 	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_num_outputs(0),
@@ -424,8 +425,8 @@ PX4FMU::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -444,7 +445,7 @@ PX4FMU::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
 #ifdef HRT_PPM_CHANNEL
@@ -567,7 +568,7 @@ PX4FMU::task_main()
 			actuator_armed_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
 			bool set_armed = aa.armed && !aa.lockdown;
@@ -603,7 +604,7 @@ PX4FMU::task_main()
 
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_armed);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index ae5d6ce19..3aecd9b69 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -74,7 +74,9 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
@@ -128,8 +130,23 @@ public:
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
 	/**
-	 * Print the current status of IO
+	 * Set the minimum PWM signals when armed
 	 */
+	int 			set_min_values(const uint16_t *vals, unsigned len);
+
+	/**
+	 * Set the maximum PWM signal when armed
+	 */
+	int 			set_max_values(const uint16_t *vals, unsigned len);
+
+	/**
+	 * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
+	 */
+	int 			set_idle_values(const uint16_t *vals, unsigned len);
+
+	/**
+	* Print the current status of IO
+	*/
 	void			print_status();
 
 	inline void		set_dsm_vcc_ctl(bool enable)
@@ -168,8 +185,8 @@ private:
 
 	/* subscribed topics */
 	int			_t_actuators;	///< actuator controls topic
-	int			_t_armed;	///< system armed control topic
-	int 			_t_vstatus;	///< system / vehicle status
+	int			_t_actuator_armed;	///< system armed control topic
+	int 			_t_vehicle_control_mode;	///< vehicle control mode topic
 	int			_t_param;	///< parameter update topic
 
 	/* advertised topics */
@@ -177,6 +194,7 @@ private:
 	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
 	orb_advert_t		_to_outputs;	///< mixed servo outputs topic
 	orb_advert_t		_to_battery;	///< battery status / voltage
+	orb_advert_t		_to_safety;	///< status of safety
 
 	actuator_outputs_s	_outputs;	///< mixed outputs
 	actuator_controls_effective_s _controls_effective; ///< effective controls
@@ -195,6 +213,12 @@ private:
 	bool			_dsm_vcc_ctl;
 
 	/**
+	 * System armed
+	 */
+
+	bool			_system_armed;
+
+	/**
 	 * Trampoline to the worker task
 	 */
 	static void		task_main_trampoline(int argc, char *argv[]);
@@ -351,19 +375,21 @@ PX4IO::PX4IO(device::Device *interface) :
 	_status(0),
 	_alarms(0),
 	_t_actuators(-1),
-	_t_armed(-1),
-	_t_vstatus(-1),
+	_t_actuator_armed(-1),
+	_t_vehicle_control_mode(-1),
 	_t_param(-1),
 	_to_input_rc(0),
 	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
+	_to_safety(0),
 	_primary_pwm_device(false),
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false)
+	_dsm_vcc_ctl(false),
+	_system_armed(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -454,26 +480,27 @@ PX4IO::init()
 	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
 	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+	    	log("INAIR RESTART RECOVERY (needs commander app running)");
 
 		/* WARNING: COMMANDER app/vehicle status must be initialized.
 		 * If this fails (or the app is not started), worst-case IO
 		 * remains untouched (so manual override is still available).
 		 */
 
-		int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+		int safety_sub = orb_subscribe(ORB_ID(actuator_armed));
 		/* fill with initial values, clear updated flag */
-		vehicle_status_s status;
+		struct actuator_armed_s safety;
 		uint64_t try_start_time = hrt_absolute_time();
 		bool updated = false;
 		
-		/* keep checking for an update, ensure we got a recent state,
+		/* keep checking for an update, ensure we got a arming information,
 		   not something that was published a long time ago. */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
 				/* got data, copy and exit loop */
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 				break;
 			}
 
@@ -499,35 +526,41 @@ PX4IO::init()
 		cmd.param6 = 0;
 		cmd.param7 = 0;
 		cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
-		cmd.target_system = status.system_id;
-		cmd.target_component = status.component_id;
-		cmd.source_system = status.system_id;
-		cmd.source_component = status.component_id;
+		// cmd.target_system = status.system_id;
+		// cmd.target_component = status.component_id;
+		// cmd.source_system = status.system_id;
+		// cmd.source_component = status.component_id;
 		/* ask to confirm command */
 		cmd.confirmation =  1;
 
 		/* send command once */
-		(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+		orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
 
 		/* spin here until IO's state has propagated into the system */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 			}
 
-			/* wait 10 ms */
-			usleep(10000);
+			/* wait 50 ms */
+			usleep(50000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time)/1000 > 2000) {
 				log("failed to recover from in-air restart (2), aborting IO driver init.");
 				return 1;
 			}
 
-		/* keep waiting for state change for 10 s */
-		} while (!status.flag_system_armed);
+			/* re-send if necessary */
+			if (!safety.armed) {
+				orb_publish(ORB_ID(vehicle_command), pub, &cmd);
+				log("re-sending arm cmd");
+			}
+
+		/* keep waiting for state change for 2 s */
+		} while (!safety.armed);
 
 	/* regular boot, no in-air restart, init IO */
 	} else {
@@ -537,7 +570,8 @@ PX4IO::init()
 		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
 			PX4IO_P_SETUP_ARMING_FMU_ARMED |
 			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
-			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | 
+			PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
 
 		/* publish RC config to IO */
 		ret = io_set_rc_config();
@@ -580,9 +614,11 @@ void
 PX4IO::task_main()
 {
 	hrt_abstime last_poll_time = 0;
+	int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	log("starting");
 
+
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
 	 * primary PWM output or not.
@@ -591,18 +627,18 @@ PX4IO::task_main()
 				     ORB_ID(actuator_controls_1));
 	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
-	_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
-	orb_set_interval(_t_vstatus, 200);		/* 5Hz update rate max. */
+	_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
+	orb_set_interval(_t_vehicle_control_mode, 200);		/* 5Hz update rate max. */
 
 	_t_param = orb_subscribe(ORB_ID(parameter_update));
 	orb_set_interval(_t_param, 500);		/* 2Hz update rate max. */
 
 	if ((_t_actuators < 0) ||
-		(_t_armed < 0) ||
-		(_t_vstatus < 0) ||
+		(_t_actuator_armed < 0) ||
+		(_t_vehicle_control_mode < 0) ||
 		(_t_param < 0)) {
 		log("subscription(s) failed");
 		goto out;
@@ -612,9 +648,9 @@ PX4IO::task_main()
 	pollfd fds[4];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
-	fds[2].fd = _t_vstatus;
+	fds[2].fd = _t_vehicle_control_mode;
 	fds[2].events = POLLIN;
 	fds[3].fd = _t_param;
 	fds[3].events = POLLIN;
@@ -690,6 +726,25 @@ PX4IO::task_main()
 			 */
 			if (fds[3].revents & POLLIN) {
 				parameter_update_s pupdate;
+				int32_t dsm_bind_val;
+				param_t dsm_bind_param;
+
+				// See if bind parameter has been set, and reset it to 0
+				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
+				if (dsm_bind_val) {
+					if (!_system_armed) {
+						if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
+							mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
+							ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
+						} else {
+							mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected");
+						}
+					} else {
+						mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); 
+					}
+					dsm_bind_val = 0;
+					param_set(dsm_bind_param, &dsm_bind_val);
+				}
 
 				/* copy to reset the notification */
 				orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
@@ -745,30 +800,73 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 }
 
 int
+PX4IO::set_min_values(const uint16_t *vals, unsigned len)
+{
+	uint16_t 		regs[_max_actuators];
+
+	if (len > _max_actuators)
+		/* fail with error */
+		return E2BIG;
+
+	/* copy values to registers in IO */
+	return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
+}
+
+int
+PX4IO::set_max_values(const uint16_t *vals, unsigned len)
+{
+	uint16_t 		regs[_max_actuators];
+
+	if (len > _max_actuators)
+		/* fail with error */
+		return E2BIG;
+
+	/* copy values to registers in IO */
+	return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
+}
+
+int
+PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
+{
+	uint16_t 		regs[_max_actuators];
+
+	if (len > _max_actuators)
+		/* fail with error */
+		return E2BIG;
+
+	printf("Sending IDLE values\n");
+
+	/* copy values to registers in IO */
+	return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
+}
+
+
+int
 PX4IO::io_set_arming_state()
 {
 	actuator_armed_s	armed;		///< system armed state
-	vehicle_status_s	vstatus;	///< overall system state
+	vehicle_control_mode_s	control_mode;	///< vehicle_control_mode
 
-	orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
-	orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
+	orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
+	orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
 
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
+	_system_armed = armed.armed;
+
 	if (armed.armed && !armed.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}
-
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	}
 
-	if (vstatus.flag_external_manual_override_ok) {
+	if (control_mode.flag_external_manual_override_ok) {
 		set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
@@ -895,14 +993,14 @@ PX4IO::io_handle_status(uint16_t status)
 	 */
 
 	/* check for IO reset - force it back to armed if necessary */
-	if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
+	if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
 		&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
 
 		/* set new status */
 		_status = status;
-		_status &= PX4IO_P_STATUS_FLAGS_ARMED;
+		_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 
 		/* set the sync flag */
@@ -917,6 +1015,27 @@ PX4IO::io_handle_status(uint16_t status)
 		_status = status;
 	}
 
+	/**
+	 * Get and handle the safety status
+	 */
+	struct safety_s safety;
+	safety.timestamp = hrt_absolute_time();
+
+	if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
+		safety.safety_off = true;
+		safety.safety_switch_available = true;
+	} else {
+		safety.safety_off = false;
+		safety.safety_switch_available = true;
+	}
+
+	/* lazily publish the safety status */
+	if (_to_safety > 0) {
+		orb_publish(ORB_ID(safety), _to_safety, &safety);
+	} else {
+		_to_safety = orb_advertise(ORB_ID(safety), &safety);
+	}
+
 	return ret;
 }
 
@@ -946,7 +1065,7 @@ PX4IO::io_get_status()
 
 	io_handle_status(regs[0]);
 	io_handle_alarms(regs[1]);
-
+	
 	/* only publish if battery has a valid minimum voltage */
 	if (regs[2] > 3300) {
 		battery_status_s	battery_status;
@@ -980,6 +1099,7 @@ PX4IO::io_get_status()
 			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
 		}
 	}
+
 	return ret;
 }
 
@@ -1287,7 +1407,8 @@ PX4IO::print_status()
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
 	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 		flags,
-		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
@@ -1358,12 +1479,14 @@ PX4IO::print_status()
 	/* setup and state */
 	printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s\n",
+	printf("arming 0x%04x%s%s%s%s%s%s\n",
 		arming,
 		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""));
+		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)  ? " ALWAYS_PWM_ENABLE" : ""));
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
@@ -1390,7 +1513,10 @@ PX4IO::print_status()
 	}
 	printf("failsafe");
 	for (unsigned i = 0; i < _max_actuators; i++)
-		printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+		printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+	printf("\nidle values");
+	for (unsigned i = 0; i < _max_actuators; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
 	printf("\n");
 }
 
@@ -1452,16 +1578,11 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); 
 		usleep(500000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
-		usleep(1000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
-		usleep(100000);
+		usleep(50000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
-		break;
-
-	case DSM_BIND_STOP:
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+		usleep(50000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
-		usleep(500000);
 		break;
 
 	case DSM_BIND_POWER_UP:
@@ -1736,30 +1857,12 @@ bind(int argc, char *argv[])
 	else 
 		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
 
-	/* Open console directly to grab CTRL-C signal */
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-	if (!console)
-		errx(1, "failed opening console");
-
 	warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
-	warnx("Press CTRL-C or 'c' when done.");
 
 	g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
 
-	for (;;) {
-		usleep(500000L);
-		/* Check if user wants to quit */
-		char c;
-		if (read(console, &c, 1) == 1) {
-			if (c == 0x03 || c == 0x63) {
-				warnx("Done\n");
-				g_dev->ioctl(nullptr, DSM_BIND_STOP, 0);
-				g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
-				close(console);
-	            exit(0);
-            }
-		}
-	}
+	exit(0);
+
 }
 
 void
@@ -1779,6 +1882,11 @@ test(void)
 	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count))
 		err(1, "failed to get servo count");
 
+	/* tell IO that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_SET_ARM_OK");
+
 	if (ioctl(fd, PWM_SERVO_ARM, 0))
 		err(1, "failed to arm servos");
 
@@ -1827,7 +1935,7 @@ test(void)
 		/* Check if user wants to quit */
 		char c;
 		if (read(console, &c, 1) == 1) {
-			if (c == 0x03 || c == 0x63) {
+			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
 				close(console);
 				exit(0);
@@ -2002,6 +2110,111 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "min")) {
+
+		if (argc < 3) {
+			errx(1, "min command needs at least one channel value (PWM)");
+		}
+
+		if (g_dev != nullptr) {
+
+			/* set values for first 8 channels, fill unassigned channels with 900. */
+			uint16_t min[8];
+
+			for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++)
+			{
+				/* set channel to commanline argument or to 900 for non-provided channels */
+				if (argc > i + 2) {
+					min[i] = atoi(argv[i+2]);
+					if (min[i] < 900 || min[i] > 1200) {
+						errx(1, "value out of range of 900 < value < 1200. Aborting.");
+					}
+				} else {
+					/* a zero value will the default */
+					min[i] = 0;
+				}
+			}
+
+			int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
+
+			if (ret != OK)
+				errx(ret, "failed setting min values");
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "max")) {
+
+		if (argc < 3) {
+			errx(1, "max command needs at least one channel value (PWM)");
+		}
+
+		if (g_dev != nullptr) {
+
+			/* set values for first 8 channels, fill unassigned channels with 2100. */
+			uint16_t max[8];
+
+			for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++)
+			{
+				/* set channel to commanline argument or to 2100 for non-provided channels */
+				if (argc > i + 2) {
+					max[i] = atoi(argv[i+2]);
+					if (max[i] < 1800 || max[i] > 2100) {
+						errx(1, "value out of range of 1800 < value < 2100. Aborting.");
+					}
+				} else {
+					/* a zero value will the default */
+					max[i] = 0;
+				}
+			}
+
+			int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
+
+			if (ret != OK)
+				errx(ret, "failed setting max values");
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "idle")) {
+
+		if (argc < 3) {
+			errx(1, "max command needs at least one channel value (PWM)");
+		}
+
+		if (g_dev != nullptr) {
+
+			/* set values for first 8 channels, fill unassigned channels with 0. */
+			uint16_t idle[8];
+
+			for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++)
+			{
+				/* set channel to commanline argument or to 0 for non-provided channels */
+				if (argc > i + 2) {
+					idle[i] = atoi(argv[i+2]);
+					if (idle[i] < 900 || idle[i] > 2100) {
+						errx(1, "value out of range of 900 < value < 2100. Aborting.");
+					}
+				} else {
+					/* a zero value will the default */
+					idle[i] = 0;
+				}
+			}
+
+			int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
+
+			if (ret != OK)
+				errx(ret, "failed setting idle values");
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "recovery")) {
 
 		/*
@@ -2068,5 +2281,5 @@ px4io_main(int argc, char *argv[])
 		bind(argc, argv);
 
 	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'");
 }
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 24eec52af..f314b5876 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -237,6 +237,8 @@ private:
 	static const unsigned	_default_ntunes;
 	static const uint8_t	_note_tab[];
 
+	unsigned		_default_tune_number; // number of currently playing default tune (0 for none)
+
 	const char		*_user_tune;
 
 	const char		*_tune;		// current tune string
@@ -452,6 +454,11 @@ const char * const ToneAlarm::_default_tunes[] = {
 	"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
 	"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
 	"O2E2P64",
+	"MNT75L1O2G",					//arming warning
+	"MBNT100a8",					//battery warning slow
+	"MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"
+	"a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"
+	"a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8",		//battery warning fast // XXX why is there a break before a repetition
 };
 
 const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
@@ -467,6 +474,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
 
 ToneAlarm::ToneAlarm() :
 	CDev("tone_alarm", "/dev/tone_alarm"),
+	_default_tune_number(0),
 	_user_tune(nullptr),
 	_tune(nullptr),
 	_next(nullptr)
@@ -799,8 +807,12 @@ tune_error:
 	// stop (and potentially restart) the tune
 tune_end:
 	stop_note();
-	if (_repeat)
+	if (_repeat) {
 		start_tune(_tune);
+	} else {
+		_tune = nullptr;
+		_default_tune_number = 0;
+	}
 	return;
 }
 
@@ -869,8 +881,17 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 				_tune = nullptr;
 				_next = nullptr;
 			} else {
-				// play the selected tune
-				start_tune(_default_tunes[arg - 1]);
+				/* don't interrupt alarms unless they are repeated */
+				if (_tune != nullptr && !_repeat) {
+					/* abort and let the current tune finish */ 
+					result = -EBUSY;
+				} else if (_repeat && _default_tune_number == arg) {
+					/* requested repeating tune already playing */
+				} else {
+					// play the selected tune
+					_default_tune_number = arg;
+					start_tune(_default_tunes[arg - 1]);
+				}
 			}
 		} else {
 			result = -EINVAL;
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index d13ffe414..e29f76877 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -380,9 +380,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 
 				/* control */
 
-				/* if in auto mode, fly global position setpoint */
-				if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
-				    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+#warning fix this
+#if 0
+				if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
+				    vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
 
 				    	/* simple heading control */
 				    	control_heading(&global_pos, &global_sp, &att, &att_sp);
@@ -400,6 +401,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 					/* set flaps to zero */
 					actuators.control[4] = 0.0f;
 
+				} else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
 				/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
 				} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
 					if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
@@ -465,6 +467,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 						}
 					}
 				}
+#endif
 
 				/* publish rates */
 				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
index c96f73155..621d3253f 100644
--- a/src/examples/flow_position_control/flow_position_control_main.c
+++ b/src/examples/flow_position_control/flow_position_control_main.c
@@ -55,7 +55,8 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -158,7 +159,8 @@ flow_position_control_thread_main(int argc, char *argv[])
 	const float time_scale = powf(10.0f,-6.0f);
 
 	/* structures */
-	struct vehicle_status_s vstatus;
+	struct actuator_armed_s armed;
+	struct vehicle_control_mode_s control_mode;
 	struct vehicle_attitude_s att;
 	struct manual_control_setpoint_s manual;
 	struct filtered_bottom_flow_s filtered_flow;
@@ -169,7 +171,8 @@ flow_position_control_thread_main(int argc, char *argv[])
 	/* subscribe to attitude, motor setpoints and system state */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
 	int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -258,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[])
 					perf_begin(mc_loop_perf);
 
 					/* get a local copy of the vehicle state */
-					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 					/* get a local copy of manual setpoint */
 					orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
 					/* get a local copy of attitude */
@@ -268,7 +271,7 @@ flow_position_control_thread_main(int argc, char *argv[])
 					/* get a local copy of local position */
 					orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
 
-					if (vstatus.state_machine == SYSTEM_STATE_AUTO)
+					if (control_mode.flag_control_velocity_enabled)
 					{
 						float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
 						float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
@@ -490,7 +493,7 @@ flow_position_control_thread_main(int argc, char *argv[])
 						/* store actual height for speed estimation */
 						last_local_pos_z = local_pos.z;
 
-						speed_sp.thrust_sp = thrust_control;
+						speed_sp.thrust_sp =  thrust_control; //manual.throttle;
 						speed_sp.timestamp = hrt_absolute_time();
 
 						/* publish new speed setpoint */
@@ -527,7 +530,6 @@ flow_position_control_thread_main(int argc, char *argv[])
 						if(isfinite(manual.throttle))
 							speed_sp.thrust_sp = manual.throttle;
 					}
-
 					/* measure in what intervals the controller runs */
 					perf_count(mc_interval_perf);
 					perf_end(mc_loop_perf);
@@ -576,7 +578,8 @@ flow_position_control_thread_main(int argc, char *argv[])
 	close(parameter_update_sub);
 	close(vehicle_attitude_sub);
 	close(vehicle_local_position_sub);
-	close(vehicle_status_sub);
+	close(armed_sub);
+	close(control_mode_sub);
 	close(manual_control_setpoint_sub);
 	close(speed_sp_pub);
 
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index e40c9081d..5e80066a7 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -56,7 +56,8 @@
 #include <math.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -143,8 +144,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 	printf("[flow position estimator] starting\n");
 
 	/* rotation matrix for transformation of optical flow speed vectors */
-	static const int8_t rotM_flow_sensor[3][3] =   {{  0, 1, 0 },
-													{ -1, 0, 0 },
+	static const int8_t rotM_flow_sensor[3][3] =   {{  0, -1, 0 },
+													{ 1, 0, 0 },
 													{  0, 0, 1 }}; // 90deg rotated
 	const float time_scale = powf(10.0f,-6.0f);
 	static float speed[3] = {0.0f, 0.0f, 0.0f};
@@ -158,8 +159,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 	static float sonar_lp = 0.0f;
 
 	/* subscribe to vehicle status, attitude, sensors and flow*/
-	struct vehicle_status_s vstatus;
-	memset(&vstatus, 0, sizeof(vstatus));
+	struct actuator_armed_s armed;
+	memset(&armed, 0, sizeof(armed));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
@@ -170,8 +173,11 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 	/* subscribe to parameter changes */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to vehicle status */
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	/* subscribe to armed topic */
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	/* subscribe to safety topic */
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* subscribe to attitude */
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -218,6 +224,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 
 	while (!thread_should_exit)
 	{
+
 		if (sensors_ready)
 		{
 			/*This runs at the rate of the sensors */
@@ -263,7 +270,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 					/* got flow, updating attitude and status as well */
 					orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
 					orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
-					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 
 					/* vehicle state estimation */
 					float sonar_new = flow.ground_distance_m;
@@ -273,14 +281,15 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 					 * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time)
 					 * -> minimum sonar value 0.3m
 					 */
+
 					if (!vehicle_liftoff)
 					{
-						if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
+						if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
 							vehicle_liftoff = true;
 					}
 					else
 					{
-						if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
+						if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
 							vehicle_liftoff = false;
 					}
 
@@ -347,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 					}
 
 					/* filtering ground distance */
-					if (!vehicle_liftoff || !vstatus.flag_system_armed)
+					if (!vehicle_liftoff || !armed.armed)
 					{
 						/* not possible to fly */
 						sonar_valid = false;
@@ -444,7 +453,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 
 	close(vehicle_attitude_setpoint_sub);
 	close(vehicle_attitude_sub);
-	close(vehicle_status_sub);
+	close(armed_sub);
+	close(control_mode_sub);
 	close(parameter_update_sub);
 	close(optical_flow_sub);
 
diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c
index 8b3881c43..6af955cd7 100644
--- a/src/examples/flow_speed_control/flow_speed_control_main.c
+++ b/src/examples/flow_speed_control/flow_speed_control_main.c
@@ -55,7 +55,8 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
@@ -155,7 +156,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
 	uint32_t counter = 0;
 
 	/* structures */
-	struct vehicle_status_s vstatus;
+	struct actuator_armed_s armed;
+	struct vehicle_control_mode_s control_mode;
 	struct filtered_bottom_flow_s filtered_flow;
 	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
 
@@ -164,7 +166,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
 	/* subscribe to attitude, motor setpoints and system state */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
 	int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
 
@@ -226,14 +229,16 @@ flow_speed_control_thread_main(int argc, char *argv[])
 				{
 					perf_begin(mc_loop_perf);
 
-					/* get a local copy of the vehicle state */
-					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+					/* get a local copy of the armed topic */
+					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+					/* get a local copy of the control mode */
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 					/* get a local copy of filtered bottom flow */
 					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
 					/* get a local copy of bodyframe speed setpoint */
 					orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
 
-					if (vstatus.state_machine == SYSTEM_STATE_AUTO)
+					if (control_mode.flag_control_velocity_enabled)
 					{
 						/* calc new roll/pitch */
 						float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
@@ -350,7 +355,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
 	close(vehicle_attitude_sub);
 	close(vehicle_bodyframe_speed_setpoint_sub);
 	close(filtered_bottom_flow_sub);
-	close(vehicle_status_sub);
+	close(armed_sub);
+	close(control_mode_sub);
 	close(att_sp_pub);
 
 	perf_print_counter(mc_loop_perf);
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 9e533ccdf..65abcde1e 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -57,7 +57,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -216,8 +216,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	memset(&raw, 0, sizeof(raw));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
@@ -230,8 +230,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to system state*/
-	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+	/* subscribe to control mode*/
+	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -282,9 +282,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			/* XXX this is seriously bad - should be an emergency */
 		} else if (ret == 0) {
 			/* check if we're in HIL - not getting sensor data is fine then */
-			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
-			if (!state.flag_hil_enabled) {
+			if (!control_mode.flag_system_hil_enabled) {
 				fprintf(stderr,
 					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
 			}
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index 107c2dfb1..236052b56 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -35,7 +35,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -547,8 +547,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
@@ -561,8 +561,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to system state*/
-	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+	/* subscribe to control mode */
+	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -612,9 +612,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			/* XXX this is seriously bad - should be an emergency */
 		} else if (ret == 0) {
 			/* check if we're in HIL - not getting sensor data is fine then */
-			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
-			if (!state.flag_hil_enabled) {
+			if (!control_mode.flag_system_hil_enabled) {
 				fprintf(stderr,
 					"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
 			}
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp
index 6a304896a..b3a5d012b 100644
--- a/src/modules/commander/accelerometer_calibration.c
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file accelerometer_calibration.c
+ * @file accelerometer_calibration.cpp
  *
  * Implementation of accelerometer calibration.
  *
@@ -104,32 +104,43 @@
  */
 
 #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 status_pub, struct vehicle_status_s *status, int mavlink_fd);
-int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+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 status_pub, struct vehicle_status_s *status, int mavlink_fd) {
+void do_accel_calibration(int mavlink_fd) {
 	/* announce change */
 	mavlink_log_info(mavlink_fd, "accel calibration started");
-	/* set to accel calibration mode */
-	status->flag_preflight_accel_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
 
 	/* measure and calculate offsets & scales */
 	float accel_offs[3];
 	float accel_scale[3];
-	int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
+	int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
 
 	if (res == OK) {
 		/* measurements complete successfully, set parameters */
@@ -165,24 +176,17 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
 		}
 
 		mavlink_log_info(mavlink_fd, "accel calibration done");
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
+		tune_positive();
 	} else {
 		/* measurements error */
 		mavlink_log_info(mavlink_fd, "accel calibration aborted");
-		tune_error();
-		sleep(2);
+		tune_negative();
 	}
 
 	/* exit accel calibration mode */
-	status->flag_preflight_accel_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
 }
 
-int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
+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 };
@@ -229,10 +233,13 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float
 		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]);
+		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient],
+			(double)accel_ref[orient][0],
+			(double)accel_ref[orient][1],
+			(double)accel_ref[orient][2]);
 		mavlink_log_info(mavlink_fd, str);
 		data_collected[orient] = true;
-		tune_confirm();
+		tune_neutral();
 	}
 	close(sensor_combined_sub);
 
@@ -274,7 +281,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	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 } };
+	struct pollfd fds[1];
+	fds[0].fd = sub_sensor_combined;
+	fds[0].events = POLLIN;
 
 	hrt_abstime t_start = hrt_absolute_time();
 	/* set timeout to 30s */
@@ -342,29 +351,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 		}
 	}
 
-	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  )
+	if (  fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(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  )
+	if (  fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(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  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(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  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(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  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(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  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
 		return 5;	// [ 0, 0, -g ]
 
 	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
@@ -376,7 +385,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
  * 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 } };
+	struct pollfd fds[1];
+	fds[0].fd = sensor_combined_sub;
+	fds[0].events = POLLIN;
 	int count = 0;
 	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
 
@@ -404,7 +415,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 f93a867ba..3275d9461 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -44,8 +44,7 @@
 #define ACCELEROMETER_CALIBRATION_H_
 
 #include <stdint.h>
-#include <uORB/topics/vehicle_status.h>
 
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
+void do_accel_calibration(int mavlink_fd);
 
 #endif /* ACCELEROMETER_CALIBRATION_H_ */
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
new file mode 100644
index 000000000..df08292e3
--- /dev/null
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ * 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];
+		fds[0].fd = diff_pres_sub;
+		fds[0].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.cpp b/src/modules/commander/baro_calibration.cpp
new file mode 100644
index 000000000..d7515b3d9
--- /dev/null
+++ b/src/modules/commander/baro_calibration.cpp
@@ -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.cpp
+ * 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.cpp
index a26938637..be38ea104 100644
--- a/src/modules/commander/calibration_routines.c
+++ b/src/modules/commander/calibration_routines.cpp
@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file calibration_routines.c
+ * @file calibration_routines.cpp
  * Calibration routines implementations.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
@@ -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
deleted file mode 100644
index e9d1f3954..000000000
--- a/src/modules/commander/commander.c
+++ /dev/null
@@ -1,2097 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file commander.c
- * Main system state machine implementation.
- *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- *
- */
-
-#include "commander.h"
-
-#include <nuttx/config.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <sys/prctl.h>
-#include <string.h>
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-#include "state_machine_helper.h"
-#include "systemlib/systemlib.h"
-#include <math.h>
-#include <poll.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/subsystem_info.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/differential_pressure.h>
-#include <mavlink/mavlink_log.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-
-/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_mag.h>
-#include <drivers/drv_baro.h>
-
-#include "calibration_routines.h"
-#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);
-
-#include <systemlib/cpuload.h>
-extern struct system_load_s system_load;
-
-/* Decouple update interval and hysteris counters, all depends on intervals */
-#define COMMANDER_MONITORING_INTERVAL 50000
-#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define STICK_ON_OFF_LIMIT 0.75f
-#define STICK_THRUST_RANGE 1.0f
-#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
-#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define GPS_FIX_TYPE_2D 2
-#define GPS_FIX_TYPE_3D 3
-#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
-#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-/* File descriptors */
-static int leds;
-static int buzzer;
-static int mavlink_fd;
-static bool commander_initialized = false;
-static struct vehicle_status_s current_status; /**< Main state machine */
-static orb_advert_t stat_pub;
-
-// static uint16_t nofix_counter = 0;
-// static uint16_t gotfix_counter = 0;
-
-static unsigned int failsafe_lowlevel_timeout_ms;
-
-static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
-static int daemon_task;				/**< Handle of daemon task / thread */
-
-/* pthread loops */
-static void *orb_receive_loop(void *arg);
-
-__EXPORT int commander_main(int argc, char *argv[]);
-
-/**
- * Mainloop of commander.
- */
-int commander_thread_main(int argc, char *argv[]);
-
-static int buzzer_init(void);
-static void buzzer_deinit(void);
-static int led_init(void);
-static void led_deinit(void);
-static int led_toggle(int led);
-static int led_on(int led);
-static int led_off(int led);
-static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
-static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
-
-
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/**
- * Sort calibration values.
- *
- * Sorts the calibration values with bubble sort.
- *
- * @param a 	The array to sort
- * @param n 	The number of entries in the array
- */
-// static void cal_bsort(float a[], int n);
-
-static int buzzer_init()
-{
-	buzzer = open("/dev/tone_alarm", O_WRONLY);
-
-	if (buzzer < 0) {
-		warnx("Buzzer: open fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void buzzer_deinit()
-{
-	close(buzzer);
-}
-
-
-static int led_init()
-{
-	leds = open(LED_DEVICE_PATH, 0);
-
-	if (leds < 0) {
-		warnx("LED: open fail\n");
-		return ERROR;
-	}
-
-	if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
-		warnx("LED: ioctl fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void led_deinit()
-{
-	close(leds);
-}
-
-static int led_toggle(int led)
-{
-	static int last_blue = LED_ON;
-	static int last_amber = LED_ON;
-
-	if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
-
-	if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
-
-	return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
-}
-
-static int led_on(int led)
-{
-	return ioctl(leds, LED_ON, led);
-}
-
-static int led_off(int led)
-{
-	return ioctl(leds, LED_OFF, led);
-}
-
-enum AUDIO_PATTERN {
-	AUDIO_PATTERN_ERROR = 2,
-	AUDIO_PATTERN_NOTIFY_POSITIVE = 3,
-	AUDIO_PATTERN_NOTIFY_NEUTRAL = 4,
-	AUDIO_PATTERN_NOTIFY_NEGATIVE = 5,
-	AUDIO_PATTERN_NOTIFY_CHARGE = 6
-};
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
-{
-
-	/* Trigger alarm if going into any error state */
-	if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) ||
-	    ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR);
-	}
-
-	/* Trigger neutral on arming / disarming */
-	if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL);
-	}
-
-	/* Trigger Tetris on being bored */
-
-	return 0;
-}
-
-void tune_confirm(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 3);
-}
-
-void tune_error(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 4);
-}
-
-void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	if (current_status.rc_signal_lost) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
-		return;
-	}
-
-	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp;
-	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
-
-	/* set parameters */
-
-	float p = sp.roll;
-	param_set(param_find("TRIM_ROLL"), &p);
-	p = sp.pitch;
-	param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.yaw;
-	param_set(param_find("TRIM_YAW"), &p);
-
-	/* store to permanent storage */
-	/* auto-save to EEPROM */
-	int save_ret = param_save_default();
-
-	if (save_ret != 0) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
-	}
-
-	mavlink_log_info(mavlink_fd, "trim calibration done");
-}
-
-void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
-{
-
-	/* set to mag calibration mode */
-	status->flag_preflight_mag_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
-	struct mag_report mag;
-
-	/* 45 seconds */
-	uint64_t calibration_interval = 45 * 1000 * 1000;
-
-	/* maximum 2000 values */
-	const unsigned int calibration_maxcount = 500;
-	unsigned int calibration_counter = 0;
-
-	/* limit update rate to get equally spaced measurements over time (in ms) */
-	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
-	// XXX old cal
-	//  * FLT_MIN is not the most negative float number,
-	//  * but the smallest number by magnitude float can
-	//  * represent. Use -FLT_MAX to initialize the most
-	//  * negative number
-
-	// float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
-	// float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
-
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	/* erase old calibration */
-	struct mag_scale mscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set scale / offsets for mag");
-		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
-	}
-
-	/* calibrate range */
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
-		warnx("failed to calibrate scale");
-	}
-
-	close(fd);
-
-	/* calibrate offsets */
-
-	// uint64_t calibration_start = hrt_absolute_time();
-
-	uint64_t axis_deadline = hrt_absolute_time();
-	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
-
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = -1;
-
-	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
-
-	if (x == NULL || y == NULL || z == NULL) {
-		warnx("mag cal failed: out of memory");
-		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
-		warnx("x:%p y:%p z:%p\n", x, y, z);
-		return;
-	}
-
-	tune_confirm();
-	sleep(2);
-	tune_confirm();
-
-	while (hrt_absolute_time() < calibration_deadline &&
-	       calibration_counter < calibration_maxcount) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
-
-		/* user guidance */
-		if (hrt_absolute_time() >= axis_deadline &&
-		    axis_index < 3) {
-
-			axis_index++;
-
-			char buf[50];
-			sprintf(buf, "Please rotate around %c", axislabels[axis_index]);
-			mavlink_log_info(mavlink_fd, buf);
-			tune_confirm();
-
-			axis_deadline += calibration_interval / 3;
-		}
-
-		if (!(axis_index < 3)) {
-			break;
-		}
-
-		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
-
-		// if ((axis_left / 1000) == 0 && axis_left > 0) {
-		// 	char buf[50];
-		// 	sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
-		// 	mavlink_log_info(mavlink_fd, buf);
-		// }
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
-
-			x[calibration_counter] = mag.x;
-			y[calibration_counter] = mag.y;
-			z[calibration_counter] = mag.z;
-
-			/* get min/max values */
-
-			// if (mag.x < mag_min[0]) {
-			// 	mag_min[0] = mag.x;
-			// }
-			// else if (mag.x > mag_max[0]) {
-			// 	mag_max[0] = mag.x;
-			// }
-
-			// if (raw.magnetometer_ga[1] < mag_min[1]) {
-			// 	mag_min[1] = raw.magnetometer_ga[1];
-			// }
-			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
-			// 	mag_max[1] = raw.magnetometer_ga[1];
-			// }
-
-			// if (raw.magnetometer_ga[2] < mag_min[2]) {
-			// 	mag_min[2] = raw.magnetometer_ga[2];
-			// }
-			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
-			// 	mag_max[2] = raw.magnetometer_ga[2];
-			// }
-
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
-			break;
-		}
-	}
-
-	float sphere_x;
-	float sphere_y;
-	float sphere_z;
-	float sphere_radius;
-
-	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
-
-	free(x);
-	free(y);
-	free(z);
-
-	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
-
-		fd = open(MAG_DEVICE_PATH, 0);
-
-		struct mag_scale mscale;
-
-		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to get scale / offsets for mag");
-
-		mscale.x_offset = sphere_x;
-		mscale.y_offset = sphere_y;
-		mscale.z_offset = sphere_z;
-
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to set scale / offsets for mag");
-
-		close(fd);
-
-		/* announce and set new offset */
-
-		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
-			warnx("Setting X mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
-			warnx("Setting Y mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
-			warnx("Setting Z mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
-			warnx("Setting X mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
-			warnx("Setting Y mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
-			warnx("Setting Z mag scale failed!\n");
-		}
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
-		}
-
-		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
-		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
-		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
-
-		char buf[52];
-		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
-			(double)mscale.y_offset, (double)mscale.z_offset);
-		mavlink_log_info(mavlink_fd, buf);
-
-		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
-			(double)mscale.y_scale, (double)mscale.z_scale);
-		mavlink_log_info(mavlink_fd, buf);
-
-		mavlink_log_info(mavlink_fd, "mag calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
-	}
-
-	/* disable calibration mode */
-	status->flag_preflight_mag_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(sub_mag);
-}
-
-void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* set to gyro calibration mode */
-	status->flag_preflight_gyro_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 5000;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
-
-	int calibration_counter = 0;
-	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	/* set offsets to zero */
-	int fd = open(GYRO_DEVICE_PATH, 0);
-	struct gyro_scale gscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
-		warn("WARNING: failed to set scale / offsets for gyro");
-
-	close(fd);
-
-	int errcount = 0;
-
-	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) {
-			errcount++;
-		}
-
-		if (errcount > 1000) {
-			/* any persisting poll error is a reason to abort */
-			mavlink_log_info(mavlink_fd, "permanent gyro error, aborted.");
-			return;
-		}
-	}
-
-	gyro_offset[0] = gyro_offset[0] / calibration_count;
-	gyro_offset[1] = gyro_offset[1] / calibration_count;
-	gyro_offset[2] = gyro_offset[2] / calibration_count;
-
-	/* exit gyro calibration mode */
-	status->flag_preflight_gyro_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
-
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
-			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
-			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
-		}
-
-		/* set offsets to actual value */
-		fd = open(GYRO_DEVICE_PATH, 0);
-		struct gyro_scale gscale = {
-			gyro_offset[0],
-			1.0f,
-			gyro_offset[1],
-			1.0f,
-			gyro_offset[2],
-			1.0f,
-		};
-
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		// char buf[50];
-		// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
-		// mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
-	}
-
-	close(sub_sensor_combined);
-}
-
-void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* announce change */
-
-	mavlink_log_info(mavlink_fd, "keep it still");
-	/* set to accel calibration mode */
-	status->flag_preflight_airspeed_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 2500;
-
-	int 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_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
-	}
-
-	/* exit airspeed calibration mode */
-	status->flag_preflight_airspeed_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(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;
-
-	/* announce command handling */
-	tune_confirm();
-
-
-	/* supported command handling start */
-
-	/* request to set different system mode */
-	switch (cmd->command) {
-	case VEHICLE_CMD_DO_SET_MODE: {
-			if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-			} else {
-				result = VEHICLE_CMD_RESULT_DENIED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			/* request to arm */
-			if ((int)cmd->param1 == 1) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				/* request to disarm */
-
-			} else if ((int)cmd->param1 == 0) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-		/* request for an autopilot reboot */
-	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
-			if ((int)cmd->param1 == 1) {
-				if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
-					/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					/* system may return here */
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-//		/* request to land */
-//		case VEHICLE_CMD_NAV_LAND:
-//		 {
-//				//TODO: add check if landing possible
-//				//TODO: add landing maneuver
-//
-//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//		}		}
-//		break;
-//
-//		/* request to takeoff */
-//		case VEHICLE_CMD_NAV_TAKEOFF:
-//		{
-//			//TODO: add check if takeoff possible
-//			//TODO: add takeoff maneuver
-//
-//			if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
-//				result = VEHICLE_CMD_RESULT_ACCEPTED;
-//			}
-//		}
-//		break;
-//
-		/* preflight calibration */
-	case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
-			bool handled = false;
-
-			/* gyro calibration */
-			if ((int)(cmd->param1) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting gyro cal");
-					tune_confirm();
-					do_gyro_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished gyro cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* magnetometer calibration */
-			if ((int)(cmd->param2) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting mag cal");
-					tune_confirm();
-					do_mag_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished mag cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* zero-altitude pressure calibration */
-			if ((int)(cmd->param3) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
-					tune_confirm();
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* trim calibration */
-			if ((int)(cmd->param4) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting trim cal");
-					tune_confirm();
-					do_rc_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished trim cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* accel calibration */
-			if ((int)(cmd->param5) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
-					tune_confirm();
-					do_accel_calibration(status_pub, &current_status, mavlink_fd);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* airspeed calibration */
-			if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
-					tune_confirm();
-					do_airspeed_calibration(status_pub, &current_status);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* none found */
-			if (!handled) {
-				//warnx("refusing unsupported calibration request\n");
-				mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request");
-				result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_PREFLIGHT_STORAGE: {
-			if (current_status.flag_system_armed &&
-			    ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
-				/* do not perform expensive memory tasks on multirotors in flight */
-				// XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
-				mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed");
-
-			} else {
-
-				// XXX move this to LOW PRIO THREAD of commander app
-				/* Read all parameters from EEPROM to RAM */
-
-				if (((int)(cmd->param1)) == 0)	{
-
-					/* read all parameters from EEPROM to RAM */
-					int read_ret = param_load_default();
-
-					if (read_ret == OK) {
-						//warnx("[mavlink pm] Loaded EEPROM params in RAM\n");
-						mavlink_log_info(mavlink_fd, "OK loading params from");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else if (read_ret == 1) {
-						mavlink_log_info(mavlink_fd, "OK no changes in");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (read_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR loading params from");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR no param file named");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else if (((int)(cmd->param1)) == 1)	{
-
-					/* write all parameters from RAM to EEPROM */
-					int write_ret = param_save_default();
-
-					if (write_ret == OK) {
-						mavlink_log_info(mavlink_fd, "OK saved param file");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (write_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR params file does not exit:");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR writing params to");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else {
-					mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
-					result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-				}
-			}
-		}
-		break;
-
-	default: {
-			mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
-			result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			/* announce command rejection */
-			ioctl(buzzer, TONE_SET_ALARM, 4);
-		}
-		break;
-	}
-
-	/* supported command handling stop */
-	if (result == VEHICLE_CMD_RESULT_FAILED ||
-	    result == VEHICLE_CMD_RESULT_DENIED ||
-	    result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		ioctl(buzzer, TONE_SET_ALARM, 5);
-
-	} else if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-		tune_confirm();
-	}
-
-	/* send any requested ACKs */
-	if (cmd->confirmation > 0) {
-		/* send acknowledge command */
-		// XXX TODO
-	}
-
-}
-
-static void *orb_receive_loop(void *arg)  //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
-{
-	/* Set thread name */
-	prctl(PR_SET_NAME, "commander orb rcv", getpid());
-
-	/* Subscribe to command topic */
-	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
-	struct subsystem_info_s info;
-
-	struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg;
-
-	while (!thread_should_exit) {
-		struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } };
-
-		if (poll(fds, 1, 5000) == 0) {
-			/* timeout, but this is no problem, silently ignore */
-		} else {
-			/* got command */
-			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
-
-			warnx("Subsys changed: %d\n", (int)info.subsystem_type);
-
-			/* mark / unmark as present */
-			if (info.present) {
-				vstatus->onboard_control_sensors_present |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_present &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as enabled */
-			if (info.enabled) {
-				vstatus->onboard_control_sensors_enabled |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as ok */
-			if (info.ok) {
-				vstatus->onboard_control_sensors_health |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_health &= ~info.subsystem_type;
-			}
-		}
-	}
-
-	close(subsys_sub);
-
-	return NULL;
-}
-
-/*
- * Provides a coarse estimate of remaining battery power.
- *
- * The estimate is very basic and based on decharging voltage curves.
- *
- * @return the estimated remaining capacity in 0..1
- */
-float battery_remaining_estimate_voltage(float voltage);
-
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
-
-float battery_remaining_estimate_voltage(float voltage)
-{
-	float ret = 0;
-	static param_t bat_volt_empty;
-	static param_t bat_volt_full;
-	static param_t bat_n_cells;
-	static bool initialized = false;
-	static unsigned int counter = 0;
-	static float ncells = 3;
-	// XXX change cells to int (and param to INT32)
-
-	if (!initialized) {
-		bat_volt_empty = param_find("BAT_V_EMPTY");
-		bat_volt_full = param_find("BAT_V_FULL");
-		bat_n_cells = param_find("BAT_N_CELLS");
-		initialized = true;
-	}
-
-	static float chemistry_voltage_empty = 3.2f;
-	static float chemistry_voltage_full = 4.05f;
-
-	if (counter % 100 == 0) {
-		param_get(bat_volt_empty, &chemistry_voltage_empty);
-		param_get(bat_volt_full, &chemistry_voltage_full);
-		param_get(bat_n_cells, &ncells);
-	}
-
-	counter++;
-
-	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
-
-	/* limit to sane values */
-	ret = (ret < 0) ? 0 : ret;
-	ret = (ret > 1) ? 1 : ret;
-	return ret;
-}
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
-}
-
-/**
- * The daemon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int commander_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("commander already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		daemon_task = task_spawn_cmd("commander",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
-					 3000,
-					 commander_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("\tcommander is running\n");
-
-		} else {
-			warnx("\tcommander not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int commander_thread_main(int argc, char *argv[])
-{
-	/* not yet initialized */
-	commander_initialized = false;
-	bool home_position_set = false;
-
-	/* set parameters */
-	failsafe_lowlevel_timeout_ms = 0;
-	param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
-
-	param_t _param_sys_type = param_find("MAV_TYPE");
-	param_t _param_system_id = param_find("MAV_SYS_ID");
-	param_t _param_component_id = param_find("MAV_COMP_ID");
-
-	/* welcome user */
-	warnx("I am in command now!\n");
-
-	/* pthreads for command and subsystem info handling */
-	// pthread_t command_handling_thread;
-	pthread_t subsystem_info_thread;
-
-	/* initialize */
-	if (led_init() != 0) {
-		warnx("ERROR: Failed to initialize leds\n");
-	}
-
-	if (buzzer_init() != 0) {
-		warnx("ERROR: Failed to initialize buzzer\n");
-	}
-
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
-	}
-
-	/* make sure we are in preflight state */
-	memset(&current_status, 0, sizeof(current_status));
-	current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
-	current_status.flag_system_armed = false;
-	/* neither manual nor offboard control commands have been received */
-	current_status.offboard_control_signal_found_once = false;
-	current_status.rc_signal_found_once = false;
-	/* mark all signals lost as long as they haven't been found */
-	current_status.rc_signal_lost = true;
-	current_status.offboard_control_signal_lost = true;
-	/* allow manual override initially */
-	current_status.flag_external_manual_override_ok = true;
-	/* flag position info as bad, do not allow auto mode */
-	current_status.flag_vector_flight_mode_ok = false;
-	/* set battery warning flag */
-	current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
-
-	/* advertise to ORB */
-	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
-	/* publish current state machine */
-	state_machine_publish(stat_pub, &current_status, mavlink_fd);
-
-	/* home position */
-	orb_advert_t home_pub = -1;
-	struct home_position_s home;
-	memset(&home, 0, sizeof(home));
-
-	if (stat_pub < 0) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		exit(ERROR);
-	}
-
-	mavlink_log_info(mavlink_fd, "system is running");
-
-	/* create pthreads */
-	pthread_attr_t subsystem_info_attr;
-	pthread_attr_init(&subsystem_info_attr);
-	pthread_attr_setstacksize(&subsystem_info_attr, 2048);
-	pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, &current_status);
-
-	/* Start monitoring loop */
-	uint16_t counter = 0;
-	uint8_t flight_env;
-
-	/* Initialize to 0.0V */
-	float battery_voltage = 0.0f;
-	bool battery_voltage_valid = false;
-	bool low_battery_voltage_actions_done = false;
-	bool critical_battery_voltage_actions_done = false;
-	uint8_t low_voltage_counter = 0;
-	uint16_t critical_voltage_counter = 0;
-	int16_t mode_switch_rc_value;
-	float bat_remain = 1.0f;
-
-	uint16_t stick_off_counter = 0;
-	uint16_t stick_on_counter = 0;
-
-	/* Subscribe to manual control data */
-	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp_man;
-	memset(&sp_man, 0, sizeof(sp_man));
-
-	/* Subscribe to offboard control data */
-	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s sp_offboard;
-	memset(&sp_offboard, 0, sizeof(sp_offboard));
-
-	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	struct vehicle_global_position_s global_position;
-	memset(&global_position, 0, sizeof(global_position));
-	uint64_t last_global_position_time = 0;
-
-	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	struct vehicle_local_position_s local_position;
-	memset(&local_position, 0, sizeof(local_position));
-	uint64_t last_local_position_time = 0;
-
-	/* 
-	 * The home position is set based on GPS only, to prevent a dependency between
-	 * position estimator and commander. RAW GPS is more than good enough for a
-	 * non-flying vehicle.
-	 */
-	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	struct vehicle_gps_position_s gps_position;
-	memset(&gps_position, 0, sizeof(gps_position));
-
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s sensors;
-	memset(&sensors, 0, sizeof(sensors));
-
-	int 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();
-	uint64_t failsave_ll_start_time = 0;
-
-	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;
-	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_system_armed) {
-				if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
-					warnx("failed setting new system type");
-				}
-
-				/* disable manual override for all systems that rely on electronic stabilization */
-				if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
-					current_status.flag_external_manual_override_ok = false;
-
-				} else {
-					current_status.flag_external_manual_override_ok = true;
-				}
-
-				/* check and update system / component ID */
-				param_get(_param_system_id, &(current_status.system_id));
-				param_get(_param_component_id, &(current_status.component_id));
-
-			}
-		}
-
-		/* update global position estimate */
-		orb_check(global_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
-			last_global_position_time = global_position.timestamp;
-		}
-
-		/* update local position estimate */
-		orb_check(local_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
-			last_local_position_time = local_position.timestamp;
-		}
-
-		/* update battery status */
-		orb_check(battery_sub, &new_data);
-		if (new_data) {
-			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-			battery_voltage = battery.voltage_v;
-			battery_voltage_valid = true;
-
-			/*
-			 * Only update battery voltage estimate if system has
-			 * been running for two and a half seconds.
-			 */
-			if (hrt_absolute_time() - start_time > 2500000) {
-				bat_remain = battery_remaining_estimate_voltage(battery_voltage);
-			}
-		}
-
-		/* Slow but important 8 Hz checks */
-		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
-			/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
-			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
-			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
-			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-				/* armed, 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);
-
-			} else {
-				// /* Constant error indication in standby mode without GPS */
-				// if (!current_status.gps_valid) {
-				// 	led_on(LED_AMBER);
-
-				// } else {
-				// 	led_off(LED_AMBER);
-				// }
-			}
-
-			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				/* compute system load */
-				uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
-
-				if (last_idle_time > 0)
-					current_status.load = 1000 - (interval_runtime / 1000);	//system load is time spent in non-idle
-
-				last_idle_time = system_load.tasks[0].total_runtime;
-			}
-		}
-
-		// // XXX Export patterns and threshold to parameters
-		/* Trigger audio event for low battery */
-		if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) {
-			/* For less than 10%, start be really annoying at 5 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			ioctl(buzzer, TONE_SET_ALARM, 3);
-
-		} else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) {
-			/* For less than 20%, start be slightly annoying at 1 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			tune_confirm();
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-		}
-
-		/* Check battery voltage */
-		/* write to sys_status */
-		if (battery_voltage_valid) {
-			current_status.voltage_battery = battery_voltage;
-
-		} else {
-			current_status.voltage_battery = 0.0f;
-		}
-
-		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
-
-			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
-			}
-
-			low_voltage_counter++;
-		}
-
-		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
-		else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
-			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
-				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
-			}
-
-			critical_voltage_counter++;
-
-		} else {
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
-		}
-
-		/* End battery voltage check */
-
-
-		/*
-		 * Check for valid position information.
-		 *
-		 * If the system has a valid position source from an onboard
-		 * position estimator, it is safe to operate it autonomously.
-		 * The flag_vector_flight_mode_ok flag indicates that a minimum
-		 * set of position measurements is available.
-		 */
-
-		/* store current state to reason later about a state change */
-		bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
-		bool global_pos_valid = current_status.flag_global_position_valid;
-		bool local_pos_valid = current_status.flag_local_position_valid;
-		bool airspeed_valid = current_status.flag_airspeed_valid;
-
-		/* check for global or local position updates, set a timeout of 2s */
-		if (hrt_absolute_time() - last_global_position_time < 2000000) {
-			current_status.flag_global_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_global_position_valid = false;
-		}
-
-		if (hrt_absolute_time() - last_local_position_time < 2000000) {
-			current_status.flag_local_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_local_position_valid = false;
-		}
-
-		/* Check for valid airspeed/differential pressure measurements */
-		if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
-			current_status.flag_airspeed_valid = true;
-
-		} else {
-			current_status.flag_airspeed_valid = false;
-		}
-
-		/*
-		 * Consolidate global position and local position valid flags
-		 * for vector flight mode.
-		 */
-		if (current_status.flag_local_position_valid ||
-		    current_status.flag_global_position_valid) {
-			current_status.flag_vector_flight_mode_ok = true;
-
-		} else {
-			current_status.flag_vector_flight_mode_ok = false;
-		}
-
-		/* consolidate state change, flag as changed if required */
-		if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
-		    global_pos_valid != current_status.flag_global_position_valid ||
-		    local_pos_valid != current_status.flag_local_position_valid ||
-		    airspeed_valid != current_status.flag_airspeed_valid) {
-			state_changed = true;
-		}
-
-		/*
-		 * Mark the position of the first position lock as return to launch (RTL)
-		 * position. The MAV will return here on command or emergency.
-		 *
-		 * Conditions:
-		 *
-		 * 	1) The system aquired position lock just now
-		 *	2) The system has not aquired position lock before
-		 *	3) The system is not armed (on the ground)
-		 */
-		if (!current_status.flag_valid_launch_position &&
-		    !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok &&
-		    !current_status.flag_system_armed) {
-			/* first time a valid position, store it and emit it */
-
-			// XXX implement storage and publication of RTL position
-			current_status.flag_valid_launch_position = true;
-			current_status.flag_auto_flight_mode_ok = true;
-			state_changed = true;
-		}
-
-		if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) {
-
-			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
-
-			/* check for first, long-term and valid GPS lock -> set home position */
-			float hdop_m = gps_position.eph_m;
-			float vdop_m = gps_position.epv_m;
-
-			/* check if gps fix is ok */
-			// XXX magic number
-			float hdop_threshold_m = 4.0f;
-			float vdop_threshold_m = 8.0f;
-
-			/*
-			 * If horizontal dilution of precision (hdop / eph)
-			 * and vertical diluation of precision (vdop / epv)
-			 * are below a certain threshold (e.g. 4 m), AND
-			 * home position is not yet set AND the last GPS
-			 * GPS measurement is not older than two seconds AND
-			 * the system is currently not armed, set home
-			 * position to the current position.
-			 */
-
-			if (gps_position.fix_type == GPS_FIX_TYPE_3D
-				&& (hdop_m < hdop_threshold_m)
-				&& (vdop_m < vdop_threshold_m)
-				&& !home_position_set
-				&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
-				&& !current_status.flag_system_armed) {
-				warnx("setting home position");
-
-				/* copy position data to uORB home message, store it locally as well */
-				home.lat = gps_position.lat;
-				home.lon = gps_position.lon;
-				home.alt = gps_position.alt;
-
-				home.eph_m = gps_position.eph_m;
-				home.epv_m = gps_position.epv_m;
-
-				home.s_variance_m_s = gps_position.s_variance_m_s;
-				home.p_variance_m = gps_position.p_variance_m;
-
-				/* announce new home position */
-				if (home_pub > 0) {
-					orb_publish(ORB_ID(home_position), home_pub, &home);
-				} else {
-					home_pub = orb_advertise(ORB_ID(home_position), &home);
-				}
-
-				/* mark home position as set */
-				home_position_set = true;
-				tune_confirm();
-			}
-		}
-
-		/* ignore RC signals if in offboard control mode */
-		if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
-			/* Start RC state check */
-
-			if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
-
-				// /*
-				//  * Check if manual control modes have to be switched
-				//  */
-				// if (!isfinite(sp_man.manual_mode_switch)) {
-				// 	warnx("man mode sw not finite\n");
-
-				// 	/* this switch is not properly mapped, set default */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, fall back to direct pass-through */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
-
-				// 	/* bottom stick position, set direct mode for vehicles supporting it */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, set to direct pass-through as requested */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
-
-				// 	/* top stick position, set SAS for all vehicle types */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 	current_status.flag_control_attitude_enabled = true;
-				// 	current_status.flag_control_rates_enabled = true;
-
-				// } else {
-				// 	/* center stick position, set rate control */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
-				// 	current_status.flag_control_attitude_enabled = false;
-				// 	current_status.flag_control_rates_enabled = true;
-				// }
-
-				// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
-
-				/*
-				 * Check if manual stability control modes have to be switched
-				 */
-				if (!isfinite(sp_man.manual_sas_switch)) {
-
-					/* this switch is not properly mapped, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
-				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
-
-					/* bottom stick position, set default */
-					/* this MUST be mapped to extremal position to switch easy in case of emergency */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
-				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
-
-					/* top stick position */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
-
-				} else {
-					/* center stick position, set altitude hold */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
-				}
-
-				if (current_status.manual_sas_mode != manual_sas_mode) {
-					/* publish SAS mode changes immediately */
-					manual_sas_mode = current_status.manual_sas_mode;
-					state_changed = true;
-				}
-
-				/*
-				 * 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)
-				    ) &&
-					current_status.flag_control_manual_enabled &&
-					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
-				    sp_man.yaw < -STICK_ON_OFF_LIMIT &&
-				    sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_off_counter++;
-						stick_on_counter = 0;
-					}
-				}
-
-				/* check if left stick is in lower right position --> arm */
-				if (current_status.flag_control_manual_enabled &&
-					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
-					sp_man.yaw > STICK_ON_OFF_LIMIT &&
-					sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_on_counter++;
-						stick_off_counter = 0;
-					}
-				}
-
-				/* check manual override switch - switch to manual or auto mode */
-				if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
-					/* enable manual override */
-					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
-
-				} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
-					// /* check auto mode switch for correct mode */
-					// if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
-					// 	/* enable guided mode */
-					// 	update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
-
-					// } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
-						// XXX hardcode to auto for now
-						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
-
-					// }
-
-				} else {
-					/* center stick position, set SAS for all vehicle types */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-				}
-
-				/* handle the case where RC signal was regained */
-				if (!current_status.rc_signal_found_once) {
-					current_status.rc_signal_found_once = true;
-					mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
-
-				} else {
-					if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
-				}
-
-				current_status.rc_signal_cutting_off = false;
-				current_status.rc_signal_lost = false;
-				current_status.rc_signal_lost_interval = 0;
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					/* only complain if the offboard control is NOT active */
-					current_status.rc_signal_cutting_off = true;
-					mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
-
-				/* if the RC signal is gone for a full second, consider it lost */
-				if (current_status.rc_signal_lost_interval > 1000000) {
-					current_status.rc_signal_lost = true;
-					current_status.failsave_lowlevel = true;
-					state_changed = true;
-				}
-
-				// if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) {
-				// 	publish_armed_status(&current_status);
-				// }
-			}
-		}
-
-
-
-
-		/* End mode switch */
-
-		/* END RC state check */
-
-
-		/* State machine update for offboard control */
-		if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
-			if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
-
-				/* decide about attitude control flag, enable in att/pos/vel */
-				bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
-
-				/* decide about rate control flag, enable it always XXX (for now) */
-				bool rates_ctrl_enabled = true;
-
-				/* set up control mode */
-				if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
-					current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
-					state_changed = true;
-				}
-
-				if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
-					current_status.flag_control_rates_enabled = rates_ctrl_enabled;
-					state_changed = true;
-				}
-
-				/* handle the case where offboard control signal was regained */
-				if (!current_status.offboard_control_signal_found_once) {
-					current_status.offboard_control_signal_found_once = true;
-					/* enable offboard control, disable manual input */
-					current_status.flag_control_manual_enabled = false;
-					current_status.flag_control_offboard_enabled = true;
-					state_changed = true;
-					tune_confirm();
-
-					mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
-
-				} else {
-					if (current_status.offboard_control_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
-						state_changed = true;
-						tune_confirm();
-					}
-				}
-
-				current_status.offboard_control_signal_weak = false;
-				current_status.offboard_control_signal_lost = false;
-				current_status.offboard_control_signal_lost_interval = 0;
-
-				/* arm / disarm on request */
-				if (sp_offboard.armed && !current_status.flag_system_armed) {
-					update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-					/* switch to stabilized mode = takeoff */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-
-				} else if (!sp_offboard.armed && current_status.flag_system_armed) {
-					update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-				}
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					current_status.offboard_control_signal_weak = true;
-					mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
-
-				/* if the signal is gone for 0.1 seconds, consider it lost */
-				if (current_status.offboard_control_signal_lost_interval > 100000) {
-					current_status.offboard_control_signal_lost = true;
-					current_status.failsave_lowlevel_start_time = hrt_absolute_time();
-					tune_confirm();
-
-					/* kill motors after timeout */
-					if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
-						current_status.failsave_lowlevel = true;
-						state_changed = true;
-					}
-				}
-			}
-		}
-
-
-		current_status.counter++;
-		current_status.timestamp = hrt_absolute_time();
-
-
-		/* If full run came back clean, transition to standby */
-		if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
-		    current_status.flag_preflight_gyro_calibration == false &&
-		    current_status.flag_preflight_mag_calibration == false &&
-		    current_status.flag_preflight_accel_calibration == false) {
-			/* All ok, no calibration going on, go to standby */
-			do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-		}
-
-		/* publish at least with 1 Hz */
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
-			publish_armed_status(&current_status);
-			orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
-			state_changed = false;
-		}
-
-		/* Store old modes to detect and act on state transitions */
-		voltage_previous = current_status.voltage_battery;
-
-		fflush(stdout);
-		counter++;
-		usleep(COMMANDER_MONITORING_INTERVAL);
-	}
-
-	/* wait for threads to complete */
-	// pthread_join(command_handling_thread, NULL);
-	pthread_join(subsystem_info_thread, NULL);
-
-	/* close fds */
-	led_deinit();
-	buzzer_deinit();
-	close(sp_man_sub);
-	close(sp_offboard_sub);
-	close(global_position_sub);
-	close(sensor_sub);
-	close(cmd_sub);
-
-	warnx("exiting..\n");
-	fflush(stdout);
-
-	thread_running = false;
-
-	return 0;
-}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
new file mode 100644
index 000000000..41e22d21d
--- /dev/null
+++ b/src/modules/commander/commander.cpp
@@ -0,0 +1,1640 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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.cpp
+ * Main system state machine implementation.
+ *
+ */
+
+#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/vehicle_control_mode.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/safety.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 "px4_custom_mode.h"
+#include "commander_helper.h"
+#include "state_machine_helper.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"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+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))
+#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 */
+
+#define PRINT_INTERVAL	5000000
+#define PRINT_MODE_REJECT_INTERVAL	2000000
+
+enum MAV_MODE_FLAG {
+	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
+	MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
+	MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
+	MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
+	MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
+	MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
+	MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
+	MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
+	MAV_MODE_FLAG_ENUM_END = 129, /*  | */
+};
+
+/* Mavlink file descriptors */
+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 */
+
+/* timout until lowlevel failsafe */
+static unsigned int failsafe_lowlevel_timeout_ms;
+static unsigned int leds_counter;
+/* To remember when last notification was sent */
+static uint64_t last_print_mode_reject_time = 0;
+
+
+/* tasks waiting for low prio thread */
+typedef 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_t;
+
+static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
+
+/**
+ * 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().
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int commander_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+void usage(const char *reason);
+
+/**
+ * React to commands that are sent e.g. from the mavlink module.
+ */
+void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+
+/**
+ * Mainloop of commander.
+ */
+int commander_thread_main(int argc, char *argv[]);
+
+void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position);
+
+void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
+
+transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+
+void print_reject_mode(const char *msg);
+
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
+
+/**
+ * Loop that runs at a lower rate and priority for calibration and parameter tasks.
+ */
+void *commander_low_prio_loop(void *arg);
+
+
+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_cmd("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);
+}
+
+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);
+}
+
+void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+{
+	/* 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: {
+			uint8_t base_mode = (uint8_t) cmd->param1;
+			uint32_t custom_mode = (uint32_t) cmd->param2;
+
+			// TODO remove debug code
+			mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode);
+			/* set arming state */
+			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+
+			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+
+				if (arming_res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
+				}
+
+			} else {
+				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+					arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+					arming_res = arming_state_transition(status, safety, new_arming_state, armed);
+
+					if (arming_res == TRANSITION_CHANGED) {
+						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
+					}
+
+				} else {
+					arming_res = TRANSITION_NOT_CHANGED;
+				}
+			}
+
+			/* set main state */
+			transition_result_t main_res = TRANSITION_DENIED;
+
+			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+				/* use autopilot-specific mode */
+				if (custom_mode == PX4_CUSTOM_MODE_MANUAL) {
+					/* MANUAL */
+					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+
+				} else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) {
+					/* SEATBELT */
+					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+
+				} else if (custom_mode == PX4_CUSTOM_MODE_EASY) {
+					/* EASY */
+					main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+				} else if (custom_mode == PX4_CUSTOM_MODE_AUTO) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+				}
+
+			} else {
+				/* use base mode */
+				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+						/* EASY */
+						main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+						/* MANUAL */
+						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					}
+				}
+			}
+
+			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
+		}
+
+	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
+		break;
+
+	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+			if (is_safe(status, safety, armed)) {
+
+				if (((int)(cmd->param1)) == 1) {
+					/* reboot */
+					systemreset(false);
+				} else if (((int)(cmd->param1)) == 3) {
+					/* reboot to bootloader */
+
+					// XXX implement
+					result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
+			
+			} else {
+				result = VEHICLE_CMD_RESULT_DENIED;
+			}
+		break;
+
+	case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+			low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE;
+
+			if ((int)(cmd->param1) == 1) {
+				/* gyro calibration */
+				new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION;
+
+			} else if ((int)(cmd->param2) == 1) {
+				/* magnetometer calibration */
+				new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION;
+
+			} else if ((int)(cmd->param3) == 1) {
+				/* zero-altitude pressure calibration */
+				//new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION;
+			} else if ((int)(cmd->param4) == 1) {
+				/* RC calibration */
+				new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION;
+
+			} else if ((int)(cmd->param5) == 1) {
+				/* accelerometer calibration */
+				new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION;
+
+			} else if ((int)(cmd->param6) == 1) {
+				/* airspeed calibration */
+				new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION;
+			}
+
+			/* check if we have new task and no other task is scheduled */
+			if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) {
+				/* try to go to INIT/PREFLIGHT arming state */
+				if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					low_prio_task = new_low_prio_task;
+
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
+
+			} else {
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
+		}
+
+	case VEHICLE_CMD_PREFLIGHT_STORAGE: {
+			low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE;
+
+			if (((int)(cmd->param1)) == 0) {
+				new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD;
+
+			} else if (((int)(cmd->param1)) == 1) {
+				new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE;
+			}
+
+			/* check if we have new task and no other task is scheduled */
+			if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) {
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				low_prio_task = new_low_prio_task;
+
+			} else {
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
+		}
+
+	default:
+		break;
+	}
+
+	/* supported command handling stop */
+	if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+		tune_positive();
+
+	} else {
+		tune_negative();
+
+		if (result == VEHICLE_CMD_RESULT_DENIED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command);
+
+		} else if (result == VEHICLE_CMD_RESULT_FAILED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command);
+
+		} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command);
+
+		} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command);
+		}
+	}
+
+	/* send any requested ACKs */
+	if (cmd->confirmation > 0) {
+		/* send acknowledge command */
+		// XXX TODO
+	}
+
+}
+
+int commander_thread_main(int argc, char *argv[])
+{
+	/* not yet initialized */
+	commander_initialized = false;
+	bool home_position_set = false;
+
+	bool battery_tune_played = false;
+	bool arm_tune_played = 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");
+
+	/* pthread for slow low prio thread */
+	pthread_t commander_low_prio_thread;
+
+	/* initialize */
+	if (led_init() != 0) {
+		warnx("ERROR: Failed to initialize leds");
+	}
+
+	if (buzzer_init() != OK) {
+		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.");
+	}
+
+	/* Main state machine */
+	struct vehicle_status_s status;
+	orb_advert_t status_pub;
+	/* make sure we are in preflight state */
+	memset(&status, 0, sizeof(status));
+
+	/* armed topic */
+	struct actuator_armed_s armed;
+	orb_advert_t armed_pub;
+	/* Initialize armed with all false */
+	memset(&armed, 0, sizeof(armed));
+
+	/* flags for control apps */
+	struct vehicle_control_mode_s control_mode;
+	orb_advert_t control_mode_pub;
+
+	/* Initialize all flags to false */
+	memset(&control_mode, 0, sizeof(control_mode));
+
+	status.main_state = MAIN_STATE_MANUAL;
+	status.navigation_state = NAVIGATION_STATE_STANDBY;
+	status.arming_state = ARMING_STATE_INIT;
+	status.hil_state = HIL_STATE_OFF;
+
+	/* neither manual nor offboard control commands have been received */
+	status.offboard_control_signal_found_once = false;
+	status.rc_signal_found_once = false;
+
+	/* mark all signals lost as long as they haven't been found */
+	status.rc_signal_lost = true;
+	status.offboard_control_signal_lost = true;
+
+	/* allow manual override initially */
+	control_mode.flag_external_manual_override_ok = true;
+
+	/* set battery warning flag */
+	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
+
+	// XXX for now just set sensors as initialized
+	status.condition_system_sensors_initialized = true;
+
+	// XXX just disable offboard control for now
+	control_mode.flag_control_offboard_enabled = false;
+
+	/* advertise to ORB */
+	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+	/* publish current state machine */
+
+	/* publish initial state */
+	status.counter++;
+	status.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
+	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+
+	control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
+
+	/* home position */
+	orb_advert_t home_pub = -1;
+	struct home_position_s home;
+	memset(&home, 0, sizeof(home));
+
+	if (status_pub < 0) {
+		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
+		warnx("exiting.");
+		exit(ERROR);
+	}
+
+	mavlink_log_info(mavlink_fd, "[cmd] started");
+
+	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 */
+	unsigned counter = 0;
+	unsigned low_voltage_counter = 0;
+	unsigned critical_voltage_counter = 0;
+	unsigned stick_off_counter = 0;
+	unsigned stick_on_counter = 0;
+
+	/* To remember when last notification was sent */
+	uint64_t last_print_control_time = 0;
+
+	enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE;
+	bool armed_previous = false;
+
+	bool low_battery_voltage_actions_done;
+	bool critical_battery_voltage_actions_done;
+
+	uint64_t last_idle_time = 0;
+
+	uint64_t start_time = 0;
+
+	bool status_changed = true;
+	bool param_init_forced = true;
+
+	bool updated = false;
+
+	/* Subscribe to safety topic */
+	int safety_sub = orb_subscribe(ORB_ID(safety));
+	struct safety_s safety;
+	memset(&safety, 0, sizeof(safety));
+	safety.safety_switch_available = false;
+	safety.safety_off = false;
+
+	/* 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));
+
+	/* 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));
+
+	/*
+	 * 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));
+
+	/* Subscribe to differential pressure topic */
+	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;
+
+	/* Subscribe to subsystem info topic */
+	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
+	struct subsystem_info_s info;
+	memset(&info, 0, sizeof(info));
+
+	/* now initialized */
+	commander_initialized = true;
+	thread_running = true;
+
+	start_time = hrt_absolute_time();
+
+	while (!thread_should_exit) {
+		hrt_abstime t = hrt_absolute_time();
+
+		/* update parameters */
+		orb_check(param_changed_sub, &updated);
+
+		if (updated || param_init_forced) {
+			param_init_forced = false;
+			/* parameters changed */
+			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
+
+			/* update parameters */
+			if (!armed.armed) {
+				if (param_get(_param_sys_type, &(status.system_type)) != OK) {
+					warnx("failed getting new system type");
+				}
+
+				/* disable manual override for all systems that rely on electronic stabilization */
+				if (status.system_type == VEHICLE_TYPE_COAXIAL ||
+				    status.system_type == VEHICLE_TYPE_HELICOPTER ||
+				    status.system_type == VEHICLE_TYPE_TRICOPTER ||
+				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
+				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
+				    status.system_type == VEHICLE_TYPE_OCTOROTOR) {
+					control_mode.flag_external_manual_override_ok = false;
+					status.is_rotary_wing = true;
+
+				} else {
+					control_mode.flag_external_manual_override_ok = true;
+					status.is_rotary_wing = false;
+				}
+
+				/* check and update system / component ID */
+				param_get(_param_system_id, &(status.system_id));
+				param_get(_param_component_id, &(status.component_id));
+				status_changed = true;
+			}
+		}
+
+		orb_check(sp_man_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
+		}
+
+		orb_check(sp_offboard_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+		}
+
+		orb_check(sensor_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+		}
+
+		orb_check(diff_pres_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+			last_diff_pres_time = diff_pres.timestamp;
+		}
+
+		orb_check(cmd_sub, &updated);
+
+		if (updated) {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(&status, &safety, &control_mode, &cmd, &armed);
+		}
+
+		/* update safety topic */
+		orb_check(safety_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(safety), safety_sub, &safety);
+		}
+
+		/* update global position estimate */
+		orb_check(global_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
+		}
+
+		/* update local position estimate */
+		orb_check(local_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+		}
+
+		/* set the condition to valid if there has recently been a local position estimate */
+		if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) {
+			if (!status.condition_local_position_valid) {
+				status.condition_local_position_valid = true;
+				status_changed = true;
+			}
+
+		} else {
+			if (status.condition_local_position_valid) {
+				status.condition_local_position_valid = false;
+				status_changed = true;
+			}
+		}
+
+		/* update battery status */
+		orb_check(battery_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+			status.battery_voltage = battery.voltage_v;
+			status.condition_battery_voltage_valid = true;
+		}
+
+		/*
+		 * Only update battery voltage estimate if system has
+		 * been running for two and a half seconds.
+		 */
+		if (t - start_time > 2500000 && status.condition_battery_voltage_valid) {
+			status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
+
+		} else {
+			status.battery_voltage = 0.0f;
+		}
+
+		/* update subsystem */
+		orb_check(subsys_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
+
+			warnx("subsystem changed: %d\n", (int)info.subsystem_type);
+
+			/* mark / unmark as present */
+			if (info.present) {
+				status.onboard_control_sensors_present |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_present &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as enabled */
+			if (info.enabled) {
+				status.onboard_control_sensors_enabled |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_enabled &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as ok */
+			if (info.ok) {
+				status.onboard_control_sensors_health |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_health &= ~info.subsystem_type;
+			}
+
+			status_changed = true;
+		}
+
+		toggle_status_leds(&status, &armed, &gps_position);
+
+		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)
+				status.load = 1000 - (interval_runtime / 1000);	//system load is time spent in non-idle
+
+			last_idle_time = system_load.tasks[0].total_runtime;
+		}
+
+		/* if battery voltage is getting lower, warn using buzzer, etc. */
+		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !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");
+				status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
+				status_changed = true;
+			}
+
+			low_voltage_counter++;
+
+		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+			/* critical battery voltage, this is rather an emergency, change state machine */
+			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				critical_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
+				status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
+				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				status_changed = true;
+			}
+
+			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 (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
+			// XXX check for sensors
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+
+		} 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 = status.condition_global_position_valid;
+		bool local_pos_valid = status.condition_local_position_valid;
+		bool airspeed_valid = status.condition_airspeed_valid;
+
+
+		/* check for global or local position updates, set a timeout of 2s */
+		if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) {
+			status.condition_global_position_valid = true;
+
+		} else {
+			status.condition_global_position_valid = false;
+		}
+
+		if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) {
+			status.condition_local_position_valid = true;
+
+		} else {
+			status.condition_local_position_valid = false;
+		}
+
+		/* Check for valid airspeed/differential pressure measurements */
+		if (t - last_diff_pres_time < 2000000 && t > 2000000) {
+			status.condition_airspeed_valid = true;
+
+		} else {
+			status.condition_airspeed_valid = false;
+		}
+
+		orb_check(gps_sub, &updated);
+
+		if (updated) {
+			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 */
+			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 (!home_position_set && 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
+			    (t - gps_position.timestamp_position < 2000000)
+			    && !armed.armed) {
+				/* copy position data to uORB home message, store it locally as well */
+				// TODO use global position estimate
+				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;
+
+				double home_lat_d = home.lat * 1e-7;
+				double home_lon_d = home.lon * 1e-7;
+				warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
+
+				/* 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 (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
+			/* start RC input check */
+			if ((t - sp_man.timestamp) < 100000) {
+				/* handle the case where RC signal was regained */
+				if (!status.rc_signal_found_once) {
+					status.rc_signal_found_once = true;
+					mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time");
+					status_changed = true;
+
+				} else {
+					if (status.rc_signal_lost) {
+						mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained");
+						status_changed = true;
+					}
+				}
+
+				status.rc_signal_cutting_off = false;
+				status.rc_signal_lost = false;
+				status.rc_signal_lost_interval = 0;
+
+				transition_result_t res;	// store all transitions results here
+
+				/* arm/disarm by RC */
+				res = TRANSITION_NOT_CHANGED;
+
+				/* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm
+				 * do it only for rotary wings */
+				if (status.is_rotary_wing &&
+				    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
+				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) {
+					if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+						if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+							/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
+							arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+							res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+							stick_off_counter = 0;
+
+						} else {
+							stick_off_counter++;
+						}
+
+						stick_on_counter = 0;
+
+					} else {
+						stick_off_counter = 0;
+					}
+				}
+
+				/* check if left stick is in lower right position and we're in manual mode -> arm */
+				if (status.arming_state == ARMING_STATE_STANDBY &&
+				    status.main_state == MAIN_STATE_MANUAL) {
+					if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+						if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+							res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+							stick_on_counter = 0;
+
+						} else {
+							stick_on_counter++;
+						}
+
+						stick_off_counter = 0;
+
+					} else {
+						stick_on_counter = 0;
+					}
+				}
+
+				if (res == TRANSITION_CHANGED) {
+					if (status.arming_state == ARMING_STATE_ARMED) {
+						mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+
+					} else {
+						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
+					}
+				}
+
+				/* fill current_status according to mode switches */
+				check_mode_switches(&sp_man, &status);
+
+				/* evaluate the main state machine */
+				res = check_main_state_machine(&status);
+
+				if (res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
+					tune_positive();
+
+				} else if (res == TRANSITION_DENIED) {
+					/* DENIED here indicates bug in the commander */
+					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				}
+
+			} else {
+
+				/* print error message for first RC glitch and then every 5s */
+				if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) {
+					// TODO remove debug code
+					if (!status.rc_signal_cutting_off) {
+						warnx("Reason: not rc_signal_cutting_off\n");
+
+					} else {
+						warnx("last print time: %llu\n", last_print_control_time);
+					}
+
+					/* only complain if the offboard control is NOT active */
+					status.rc_signal_cutting_off = true;
+					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL");
+
+					last_print_control_time = t;
+				}
+
+				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
+				status.rc_signal_lost_interval = t - sp_man.timestamp;
+
+				/* if the RC signal is gone for a full second, consider it lost */
+				if (status.rc_signal_lost_interval > 1000000) {
+					status.rc_signal_lost = true;
+					status.failsave_lowlevel = true;
+					status_changed = true;
+				}
+			}
+		}
+
+		/* END mode switch */
+		/* END RC state check */
+
+		// TODO check this
+		/* state machine update for offboard control */
+		if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) {
+			if ((t - sp_offboard.timestamp) < 5000000) {	// TODO 5s is too long ?
+
+				// /* 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();
+				// 	}
+				// }
+
+				status.offboard_control_signal_weak = false;
+				status.offboard_control_signal_lost = false;
+				status.offboard_control_signal_lost_interval = 0;
+
+				// XXX check if this is correct
+				/* arm / disarm on request */
+				if (sp_offboard.armed && !armed.armed) {
+					arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+
+				} else if (!sp_offboard.armed && armed.armed) {
+					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+				}
+
+			} else {
+
+				/* print error message for first offboard signal glitch and then every 5s */
+				if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) {
+					status.offboard_control_signal_weak = true;
+					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL");
+					last_print_control_time = t;
+				}
+
+				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
+				status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp;
+
+				/* if the signal is gone for 0.1 seconds, consider it lost */
+				if (status.offboard_control_signal_lost_interval > 100000) {
+					status.offboard_control_signal_lost = true;
+					status.failsave_lowlevel_start_time = t;
+					tune_positive();
+
+					/* kill motors after timeout */
+					if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
+						status.failsave_lowlevel = true;
+						status_changed = true;
+					}
+				}
+			}
+		}
+
+		/* evaluate the navigation state machine */
+		transition_result_t res = check_navigation_state_machine(&status, &control_mode);
+
+		if (res == TRANSITION_DENIED) {
+			/* DENIED here indicates bug in the commander */
+			warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+			mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+		}
+
+		/* check which state machines for changes, clear "changed" flag */
+		bool arming_state_changed = check_arming_state_changed();
+		bool main_state_changed = check_main_state_changed();
+		bool navigation_state_changed = check_navigation_state_changed();
+
+		if (arming_state_changed || main_state_changed || navigation_state_changed) {
+			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
+			status_changed = true;
+		}
+
+		/* publish arming state */
+		if (arming_state_changed) {
+			armed.timestamp = t;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+		}
+
+		/* publish control mode */
+		if (navigation_state_changed) {
+			/* publish new navigation state */
+			control_mode.counter++;
+			control_mode.timestamp = t;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+		}
+
+		/* publish vehicle status at least with 1 Hz */
+		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+			status.counter++;
+			status.timestamp = t;
+			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+			status_changed = false;
+		}
+
+		/* play arming and battery warning tunes */
+		if (!arm_tune_played && armed.armed) {
+			/* play tune when armed */
+			if (tune_arm() == OK)
+				arm_tune_played = true;
+
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+			/* play tune on battery warning */
+			if (tune_low_bat() == OK)
+				battery_tune_played = true;
+
+		} else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) {
+			/* play tune on battery critical */
+			if (tune_critical_bat() == OK)
+				battery_tune_played = true;
+
+		} else if (battery_tune_played) {
+			tune_stop();
+			battery_tune_played = false;
+		}
+
+		/* reset arm_tune_played when disarmed */
+		if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
+			arm_tune_played = false;
+		}
+
+		/* store old modes to detect and act on state transitions */
+		battery_warning_previous = status.battery_warning;
+		armed_previous = armed.armed;
+
+		fflush(stdout);
+		counter++;
+		usleep(COMMANDER_MONITORING_INTERVAL);
+	}
+
+	/* wait for threads to complete */
+	pthread_join(commander_low_prio_thread, NULL);
+
+	/* close fds */
+	led_deinit();
+	buzzer_deinit();
+	close(sp_man_sub);
+	close(sp_offboard_sub);
+	close(local_position_sub);
+	close(global_position_sub);
+	close(gps_sub);
+	close(sensor_sub);
+	close(safety_sub);
+	close(cmd_sub);
+	close(subsys_sub);
+	close(diff_pres_sub);
+	close(param_changed_sub);
+	close(battery_sub);
+
+	warnx("exiting");
+	fflush(stdout);
+
+	thread_running = false;
+
+	return 0;
+}
+
+void
+toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position)
+{
+	if (leds_counter % 2 == 0) {
+		/* run at 10Hz, full cycle is 16 ticks = 10/16Hz */
+		if (armed->armed) {
+			/* armed, solid */
+			led_on(LED_AMBER);
+
+		} else if (armed->ready_to_arm) {
+			/* ready to arm, blink at 2.5Hz */
+			if (leds_counter & 8) {
+				led_on(LED_AMBER);
+
+			} else {
+				led_off(LED_AMBER);
+			}
+
+		} else {
+			/* not ready to arm, blink at 10Hz */
+			led_toggle(LED_AMBER);
+		}
+
+		if (status->condition_global_position_valid) {
+			/* position estimated, solid */
+			led_on(LED_BLUE);
+
+		} else if (leds_counter == 0) {
+			/* waiting for position estimate, short blink at 1.25Hz */
+			led_on(LED_BLUE);
+
+		} else {
+			/* no position estimator available, off */
+			led_off(LED_BLUE);
+		}
+	}
+
+	leds_counter++;
+
+	if (leds_counter >= 16)
+		leds_counter = 0;
+}
+
+void
+check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+{
+	/* main mode switch */
+	if (!isfinite(sp_man->mode_switch)) {
+		warnx("mode sw not finite");
+		current_status->mode_switch = MODE_SWITCH_MANUAL;
+
+	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
+		current_status->mode_switch = MODE_SWITCH_AUTO;
+
+	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
+		current_status->mode_switch = MODE_SWITCH_MANUAL;
+
+	} else {
+		current_status->mode_switch = MODE_SWITCH_ASSISTED;
+	}
+
+	/* land switch */
+	if (!isfinite(sp_man->return_switch)) {
+		current_status->return_switch = RETURN_SWITCH_NONE;
+
+	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
+		current_status->return_switch = RETURN_SWITCH_RETURN;
+
+	} else {
+		current_status->return_switch = RETURN_SWITCH_NONE;
+	}
+
+	/* assisted switch */
+	if (!isfinite(sp_man->assisted_switch)) {
+		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+
+	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
+		current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+
+	} else {
+		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+	}
+
+	/* mission switch  */
+	if (!isfinite(sp_man->mission_switch)) {
+		current_status->mission_switch = MISSION_SWITCH_MISSION;
+
+	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
+		current_status->mission_switch = MISSION_SWITCH_NONE;
+
+	} else {
+		current_status->mission_switch = MISSION_SWITCH_MISSION;
+	}
+}
+
+transition_result_t
+check_main_state_machine(struct vehicle_status_s *current_status)
+{
+	/* evaluate the main state machine */
+	transition_result_t res = TRANSITION_DENIED;
+
+	switch (current_status->mode_switch) {
+	case MODE_SWITCH_MANUAL:
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	case MODE_SWITCH_ASSISTED:
+		if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
+			res = main_state_transition(current_status, MAIN_STATE_EASY);
+
+			if (res != TRANSITION_DENIED)
+				break;	// changed successfully or already in this state
+
+			// else fallback to SEATBELT
+			print_reject_mode("EASY");
+		}
+
+		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this mode
+
+		if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+			print_reject_mode("SEATBELT");
+
+		// else fallback to MANUAL
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	case MODE_SWITCH_AUTO:
+		res = main_state_transition(current_status, MAIN_STATE_AUTO);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
+
+		// else fallback to SEATBELT (EASY likely will not work too)
+		print_reject_mode("AUTO");
+		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
+
+		// else fallback to MANUAL
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	default:
+		break;
+	}
+
+	return res;
+}
+
+void
+print_reject_mode(const char *msg)
+{
+	hrt_abstime t = hrt_absolute_time();
+
+	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
+		last_print_mode_reject_time = t;
+		char s[80];
+		sprintf(s, "[cmd] WARNING: reject %s", msg);
+		mavlink_log_critical(mavlink_fd, s);
+		tune_negative();
+	}
+}
+
+transition_result_t
+check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode)
+{
+	transition_result_t res = TRANSITION_DENIED;
+
+	if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		/* ARMED */
+		switch (current_status->main_state) {
+		case MAIN_STATE_MANUAL:
+			res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
+			break;
+
+		case MAIN_STATE_SEATBELT:
+			res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
+			break;
+
+		case MAIN_STATE_EASY:
+			res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
+			break;
+
+		case MAIN_STATE_AUTO:
+			if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
+				/* don't act while taking off */
+				if (current_status->condition_landed) {
+					/* if landed: transitions only to AUTO_READY are allowed */
+					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
+					// TRANSITION_DENIED is not possible here
+					break;
+
+				} else {
+					/* if not landed: act depending on switches */
+					if (current_status->return_switch == RETURN_SWITCH_RETURN) {
+						/* RTL */
+						res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
+
+					} else {
+						if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
+							/* MISSION */
+							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+
+						} else {
+							/* LOITER */
+							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+						}
+					}
+				}
+			}
+
+			break;
+
+		default:
+			break;
+		}
+
+	} else {
+		/* DISARMED */
+		res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode);
+	}
+
+	return res;
+}
+
+void *commander_low_prio_loop(void *arg)
+{
+	/* Set thread name */
+	prctl(PR_SET_NAME, "commander_low_prio", getpid());
+
+	low_prio_task = LOW_PRIO_TASK_NONE;
+
+	while (!thread_should_exit) {
+
+		switch (low_prio_task) {
+		case LOW_PRIO_TASK_PARAM_LOAD:
+
+			if (0 == param_load_default()) {
+				mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
+
+			} else {
+				mavlink_log_critical(mavlink_fd, "[cmd] parameters 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, "[cmd] parameters saved");
+
+			} else {
+				mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+				tune_error();
+			}
+
+			low_prio_task = LOW_PRIO_TASK_NONE;
+			break;
+
+		case LOW_PRIO_TASK_GYRO_CALIBRATION:
+
+			do_gyro_calibration(mavlink_fd);
+			low_prio_task = LOW_PRIO_TASK_NONE;
+			break;
+
+		case LOW_PRIO_TASK_MAG_CALIBRATION:
+
+			do_mag_calibration(mavlink_fd);
+			low_prio_task = LOW_PRIO_TASK_NONE;
+			break;
+
+		case LOW_PRIO_TASK_ALTITUDE_CALIBRATION:
+
+			// do_baro_calibration(mavlink_fd);
+			low_prio_task = LOW_PRIO_TASK_NONE;
+			break;
+
+		case LOW_PRIO_TASK_RC_CALIBRATION:
+
+			// do_rc_calibration(mavlink_fd);
+			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(mavlink_fd);
+			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_helper.cpp b/src/modules/commander/commander_helper.cpp
new file mode 100644
index 000000000..681a11568
--- /dev/null
+++ b/src/modules/commander/commander_helper.cpp
@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ * 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 <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+#include <drivers/drv_led.h>
+
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+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);
+}
+
+static int leds;
+
+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);
+}
+
+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.0f) ? 0.0f : ret;
+	ret = (ret > 1.0f) ? 1.0f : ret;
+	return ret;
+}
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
new file mode 100644
index 000000000..c621b9823
--- /dev/null
+++ b/src/modules/commander/commander_helper.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   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_armed.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);
+
+int led_init(void);
+void led_deinit(void);
+int led_toggle(int led);
+int led_on(int led);
+int led_off(int led);
+
+/**
+ * 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);
+
+#endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c
index 04b4e72ab..6832fc5ce 100644
--- a/src/modules/commander/commander.h
+++ b/src/modules/commander/commander_params.c
@@ -1,10 +1,7 @@
 /****************************************************************************
  *
- *   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>
+ *   Copyright (c) 2013 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
@@ -36,23 +33,22 @@
  ****************************************************************************/
 
 /**
- * @file commander.h
- * Main system state machine definition.
+ * @file commander_params.c
+ *
+ * Parameters defined by the sensors task.
  *
- * @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);
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
 
-#endif /* COMMANDER_H_ */
+PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-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);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
+PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
new file mode 100644
index 000000000..f1afb0107
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -0,0 +1,283 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ * 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];
+		fds[0].fd = sub_sensor_combined;
+		fds[0].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) {
+			warnx("WARNING: auto-save of params to storage failed");
+			mavlink_log_critical(mavlink_fd, "gyro store failed");
+			// XXX negative tune
+			return;
+		}
+
+		mavlink_log_info(mavlink_fd, "gyro calibration done");
+
+		tune_positive();
+		/* third beep by cal end routine */
+
+	} else {
+		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
+		return;
+	}
+
+
+	/*** --- SCALING --- ***/
+
+	mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x");
+	mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip.");
+	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
+
+	unsigned rotations_count = 30;
+	float gyro_integral = 0.0f;
+	float baseline_integral = 0.0f;
+
+	// XXX change to mag topic
+	orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+	float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
+	if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
+	if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
+
+
+	uint64_t last_time = hrt_absolute_time();
+	uint64_t start_time = hrt_absolute_time();
+
+	while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
+
+		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
+		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
+			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
+			mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
+			mavlink_log_info(mavlink_fd, "gyro calibration done");
+			tune_positive();
+			return;
+		}
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = sub_sensor_combined;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+
+			float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
+			last_time = hrt_absolute_time();
+
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+			// XXX this is just a proof of concept and needs world / body
+			// transformation and more
+
+			//math::Vector2f magNav(raw.magnetometer_ga);
+
+			// calculate error between estimate and measurement
+			// apply declination correction for true heading as well.
+			//float mag = -atan2f(magNav(1),magNav(0));
+			float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
+			if (mag > M_PI_F) mag -= 2*M_PI_F;
+			if (mag < -M_PI_F) mag += 2*M_PI_F;
+
+			float diff = mag - mag_last;
+
+			if (diff > M_PI_F) diff -= 2*M_PI_F;
+			if (diff < -M_PI_F) diff += 2*M_PI_F;
+
+			baseline_integral += diff;
+			mag_last = mag;
+			// Jump through some timing scale hoops to avoid
+			// operating near the 1e6/1e8 max sane resolution of float.
+			gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
+
+			warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral);
+
+		// } 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;
+		}
+	}
+
+	float gyro_scale = baseline_integral / gyro_integral;
+	float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
+	warnx("gyro scale: yaw (z): %6.4f", gyro_scale);
+	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale);
+
+
+	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
+
+		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
+			|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
+			|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
+			mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
+		}
+
+		/* set offsets to actual value */
+		fd = open(GYRO_DEVICE_PATH, 0);
+		struct gyro_scale gscale = {
+			gyro_offset[0],
+			gyro_scales[0],
+			gyro_offset[1],
+			gyro_scales[1],
+			gyro_offset[2],
+			gyro_scales[2],
+		};
+
+		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.cpp b/src/modules/commander/mag_calibration.cpp
new file mode 100644
index 000000000..9a25103f8
--- /dev/null
+++ b/src/modules/commander/mag_calibration.cpp
@@ -0,0 +1,280 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ * 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];
+		fds[0].fd = sub_mag;
+		fds[0].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..91d75121e 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -36,8 +36,15 @@
 #
 
 MODULE_COMMAND	 	= commander
-SRCS		 	= commander.c \
-			state_machine_helper.c \
-			calibration_routines.c \
-			accelerometer_calibration.c
+SRCS		 	= commander.cpp \
+			commander_params.c \
+			state_machine_helper.cpp \
+			commander_helper.cpp \
+			calibration_routines.cpp \
+			accelerometer_calibration.cpp \
+			gyro_calibration.cpp \
+			mag_calibration.cpp \
+			baro_calibration.cpp \
+			rc_calibration.cpp \
+			airspeed_calibration.cpp
 
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
new file mode 100644
index 000000000..4d4859a5c
--- /dev/null
+++ b/src/modules/commander/px4_custom_mode.h
@@ -0,0 +1,18 @@
+/*
+ * px4_custom_mode.h
+ *
+ *  Created on: 09.08.2013
+ *      Author: ton
+ */
+
+#ifndef PX4_CUSTOM_MODE_H_
+#define PX4_CUSTOM_MODE_H_
+
+enum PX4_CUSTOM_MODE {
+	PX4_CUSTOM_MODE_MANUAL = 1,
+	PX4_CUSTOM_MODE_SEATBELT,
+	PX4_CUSTOM_MODE_EASY,
+	PX4_CUSTOM_MODE_AUTO,
+};
+
+#endif /* PX4_CUSTOM_MODE_H_ */
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
new file mode 100644
index 000000000..0de411713
--- /dev/null
+++ b/src/modules/commander/rc_calibration.cpp
@@ -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.cpp
+ * 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.rc_signal) {
+	// 	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
deleted file mode 100644
index 9b6527c33..000000000
--- a/src/modules/commander/state_machine_helper.c
+++ /dev/null
@@ -1,757 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file state_machine_helper.c
- * State machine helper functions implementations
- */
-
-#include <stdio.h>
-#include <unistd.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <systemlib/systemlib.h>
-#include <drivers/drv_hrt.h>
-#include <mavlink/mavlink_log.h>
-
-#include "state_machine_helper.h"
-
-static const char *system_state_txt[] = {
-	"SYSTEM_STATE_PREFLIGHT",
-	"SYSTEM_STATE_STANDBY",
-	"SYSTEM_STATE_GROUND_READY",
-	"SYSTEM_STATE_MANUAL",
-	"SYSTEM_STATE_STABILIZED",
-	"SYSTEM_STATE_AUTO",
-	"SYSTEM_STATE_MISSION_ABORT",
-	"SYSTEM_STATE_EMCY_LANDING",
-	"SYSTEM_STATE_EMCY_CUTOFF",
-	"SYSTEM_STATE_GROUND_ERROR",
-	"SYSTEM_STATE_REBOOT",
-
-};
-
-/**
- * Transition from one state to another
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state)
-{
-	int invalid_state = false;
-	int ret = ERROR;
-
-	commander_state_machine_t old_state = current_status->state_machine;
-
-	switch (new_state) {
-	case SYSTEM_STATE_MISSION_ABORT: {
-			/* Indoor or outdoor */
-			// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-			ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
-
-			// } else {
-			// 	ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
-			// }
-		}
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		/* Tell the controller to land */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		warnx("EMERGENCY LANDING!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		/* Tell the controller to cutoff the motors (thrust = 0) */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = false;
-
-		warnx("EMERGENCY MOTOR CUTOFF!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-
-		/* set system flags according to state */
-
-		/* prevent actuators from arming */
-		current_status->flag_system_armed = false;
-
-		warnx("GROUND ERROR, locking down propulsion system\n");
-		mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
-		break;
-
-	case SYSTEM_STATE_PREFLIGHT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-		    || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
-		}
-
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-			|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
-			|| current_status->flag_hil_enabled) {
-			invalid_state = false;
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
-			usleep(500000);
-			systemreset(false);
-			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
-		}
-
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		/* set system flags according to state */
-
-		/* standby enforces disarmed */
-		current_status->flag_system_armed = false;
-
-		mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-
-		/* set system flags according to state */
-
-		/* ground ready has motors / actuators armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
-		break;
-
-	case SYSTEM_STATE_AUTO:
-
-		/* set system flags according to state */
-
-		/* auto is airborne and in auto mode, motors armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
-		break;
-
-	default:
-		invalid_state = true;
-		break;
-	}
-
-	if (invalid_state == false || old_state != new_state) {
-		current_status->state_machine = new_state;
-		state_machine_publish(status_pub, current_status, mavlink_fd);
-		publish_armed_status(current_status);
-		ret = OK;
-	}
-
-	if (invalid_state) {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* publish the new state */
-	current_status->counter++;
-	current_status->timestamp = hrt_absolute_time();
-
-	/* assemble state vector based on flag values */
-	if (current_status->flag_control_rates_enabled) {
-		current_status->onboard_control_sensors_present |= 0x400;
-
-	} else {
-		current_status->onboard_control_sensors_present &= ~0x400;
-	}
-
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
-}
-
-void publish_armed_status(const struct vehicle_status_s *current_status)
-{
-	struct actuator_armed_s armed;
-	armed.armed = current_status->flag_system_armed;
-
-	/* 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);
-}
-
-
-/*
- * Private functions, update the state machine
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	warnx("EMERGENCY HANDLER\n");
-	/* Depending on the current state go to one of the error states */
-
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
-
-		// DO NOT abort mission
-		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
-
-	} else {
-		warnx("Unknown system state: #%d\n", current_status->state_machine);
-	}
-}
-
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
-{
-	if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
-		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
-
-	} else {
-		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
-	}
-
-}
-
-
-
-// /*
-//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
-//  */
-
-// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
-//  *
-//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
-//  * */
-
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was removed something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was disabled something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//TODO state machine change (recovering)
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		//TODO state machine change
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-
-// }
-
-
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
-// 	current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// //			//TODO: remove this block
-// //			break;
-// //			///////////////////
-// 		//global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
-
-// //				printf("previosly_healthy = %u\n", previosly_healthy);
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency(status_pub, current_status);
-
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-
-
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		state_machine_emergency(status_pub, current_status, mavlink_fd);
-	}
-}
-
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
-		printf("[cmd] arming\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-	}
-}
-
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		printf("[cmd] going standby\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] MISSION ABORT!\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	int old_mode = current_status->flight_mode;
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-
-	current_status->flag_control_manual_enabled = true;
-
-	/* set behaviour based on airframe */
-	if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-		/* assuming a rotary wing, set to SAS */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-
-	} else {
-
-		/* assuming a fixed wing, set to direct pass-through */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-		current_status->flag_control_attitude_enabled = false;
-		current_status->flag_control_rates_enabled = false;
-	}
-
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] manual mode\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-	}
-}
-
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		int old_mode = current_status->flight_mode;
-		int old_manual_control_mode = current_status->manual_control_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		current_status->flag_control_manual_enabled = true;
-
-		if (old_mode != current_status->flight_mode ||
-		    old_manual_control_mode != current_status->manual_control_mode) {
-			printf("[cmd] att stabilized mode\n");
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-		}
-
-	}
-}
-
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
-		tune_error();
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] position guided mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	}
-}
-
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
-		printf("[cmd] auto mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-	}
-}
-
-
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-{
-	uint8_t ret = 1;
-
-	/* Switch on HIL if in standby and not already in HIL mode */
-	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-	    && !current_status->flag_hil_enabled) {
-		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
-			/* Enable HIL on request */
-			current_status->flag_hil_enabled = true;
-			ret = OK;
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-			publish_armed_status(current_status);
-			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-
-		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-			   current_status->flag_system_armed) {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-
-		} else {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
-		}
-	}
-
-	/* switch manual / auto */
-	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
-		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
-		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
-		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
-	}
-
-	/* vehicle is disarmed, mode requests arming */
-	if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only arm in standby state */
-		// XXX REMOVE
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-			ret = OK;
-			printf("[cmd] arming due to command request\n");
-		}
-	}
-
-	/* vehicle is armed, mode requests disarming */
-	if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only disarm in ground ready */
-		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-			ret = OK;
-			printf("[cmd] disarming due to command request\n");
-		}
-	}
-
-	/* NEVER actually switch off HIL without reboot */
-	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
-		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
-{
-	commander_state_machine_t current_system_state = current_status->state_machine;
-
-	uint8_t ret = 1;
-
-	switch (custom_mode) {
-	case SYSTEM_STATE_GROUND_READY:
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		printf("try to reboot\n");
-
-		if (current_system_state == SYSTEM_STATE_STANDBY
-				|| current_system_state == SYSTEM_STATE_PREFLIGHT
-				|| current_status->flag_hil_enabled) {
-			printf("system will reboot\n");
-			mavlink_log_critical(mavlink_fd, "Rebooting..");
-			usleep(200000);
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		printf("try to switch to auto/takeoff\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-			printf("state: auto\n");
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		printf("try to switch to manual\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			printf("state: manual\n");
-			ret = 0;
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-	return ret;
-}
-
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
new file mode 100644
index 000000000..163aceed2
--- /dev/null
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -0,0 +1,695 @@
+/****************************************************************************
+ *
+ *   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 state_machine_helper.cpp
+ * 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 <uORB/topics/vehicle_control_mode.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"
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static bool arming_state_changed = true;
+static bool main_state_changed = true;
+static bool navigation_state_changed = true;
+
+transition_result_t
+arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_arming_state == status->arming_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		switch (new_arming_state) {
+		case ARMING_STATE_INIT:
+
+			/* allow going back from INIT for calibration */
+			if (status->arming_state == ARMING_STATE_STANDBY) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = false;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_STANDBY:
+
+			/* allow coming from INIT and disarming from ARMED */
+			if (status->arming_state == ARMING_STATE_INIT
+			    || status->arming_state == ARMING_STATE_ARMED) {
+
+				/* sensors need to be initialized for STANDBY state */
+				if (status->condition_system_sensors_initialized) {
+					ret = TRANSITION_CHANGED;
+					armed->armed = false;
+					armed->ready_to_arm = true;
+				}
+			}
+
+			break;
+
+		case ARMING_STATE_ARMED:
+
+			/* allow arming from STANDBY and IN-AIR-RESTORE */
+			if ((status->arming_state == ARMING_STATE_STANDBY
+			    || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
+			    && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */
+			{
+				ret = TRANSITION_CHANGED;
+				armed->armed = true;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_ARMED_ERROR:
+
+			/* an armed error happens when ARMED obviously */
+			if (status->arming_state == ARMING_STATE_ARMED) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = true;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_STANDBY_ERROR:
+
+			/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
+			if (status->arming_state == ARMING_STATE_STANDBY
+			    || status->arming_state == ARMING_STATE_INIT
+			    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = false;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_REBOOT:
+
+			/* an armed error happens when ARMED obviously */
+			if (status->arming_state == ARMING_STATE_INIT
+			    || status->arming_state == ARMING_STATE_STANDBY
+			    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = false;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_IN_AIR_RESTORE:
+
+			/* XXX implement */
+			break;
+
+		default:
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			status->arming_state = new_arming_state;
+			arming_state_changed = true;
+		}
+	}
+
+	return ret;
+}
+
+bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed)
+{
+	// System is safe if:
+	// 1) Not armed
+	// 2) Armed, but in software lockdown (HIL)
+	// 3) Safety switch is present AND engaged -> actuators locked
+	if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
+		return true;
+	} else {
+		return false;
+	}
+}
+
+bool
+check_arming_state_changed()
+{
+	if (arming_state_changed) {
+		arming_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+transition_result_t
+main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_main_state == current_state->main_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		switch (new_main_state) {
+		case MAIN_STATE_MANUAL:
+			ret = TRANSITION_CHANGED;
+			break;
+
+		case MAIN_STATE_SEATBELT:
+
+			/* need position estimate */
+			// TODO only need altitude estimate really
+			if (current_state->condition_local_position_valid) {
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case MAIN_STATE_EASY:
+
+			/* need position estimate */
+			if (current_state->condition_local_position_valid) {
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case MAIN_STATE_AUTO:
+
+			/* need position estimate */
+			if (current_state->condition_local_position_valid) {
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			current_state->main_state = new_main_state;
+			main_state_changed = true;
+		}
+	}
+
+	return ret;
+}
+
+bool
+check_main_state_changed()
+{
+	if (main_state_changed) {
+		main_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+transition_result_t
+navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_navigation_state == current_status->navigation_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		switch (new_navigation_state) {
+		case NAVIGATION_STATE_STANDBY:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = false;
+			control_mode->flag_control_attitude_enabled = false;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_manual_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_DIRECT:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = false;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_manual_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_STABILIZE:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_manual_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_ALTHOLD:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_manual_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_VECTOR:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_manual_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_AUTO_READY:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_AUTO_TAKEOFF:
+
+			/* only transitions from AUTO_READY */
+			if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+				ret = TRANSITION_CHANGED;
+				control_mode->flag_control_rates_enabled = true;
+				control_mode->flag_control_attitude_enabled = true;
+				control_mode->flag_control_velocity_enabled = true;
+				control_mode->flag_control_position_enabled = true;
+				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_manual_enabled = false;
+			}
+
+			break;
+
+		case NAVIGATION_STATE_AUTO_LOITER:
+
+			/* deny transitions from landed states */
+			if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
+			    current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+				ret = TRANSITION_CHANGED;
+				control_mode->flag_control_rates_enabled = true;
+				control_mode->flag_control_attitude_enabled = true;
+				control_mode->flag_control_velocity_enabled = true;
+				control_mode->flag_control_position_enabled = true;
+				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_manual_enabled = false;
+			}
+
+			break;
+
+		case NAVIGATION_STATE_AUTO_MISSION:
+
+			/* deny transitions from landed states */
+			if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
+			    current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+				ret = TRANSITION_CHANGED;
+				control_mode->flag_control_rates_enabled = true;
+				control_mode->flag_control_attitude_enabled = true;
+				control_mode->flag_control_velocity_enabled = true;
+				control_mode->flag_control_position_enabled = true;
+				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_manual_enabled = false;
+			}
+
+			break;
+
+		case NAVIGATION_STATE_AUTO_RTL:
+
+			/* deny transitions from landed states */
+			if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
+			    current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+				ret = TRANSITION_CHANGED;
+				control_mode->flag_control_rates_enabled = true;
+				control_mode->flag_control_attitude_enabled = true;
+				control_mode->flag_control_velocity_enabled = true;
+				control_mode->flag_control_position_enabled = true;
+				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_manual_enabled = false;
+			}
+
+			break;
+
+		case NAVIGATION_STATE_AUTO_LAND:
+
+			/* deny transitions from landed states */
+			if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
+			    current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+				ret = TRANSITION_CHANGED;
+				control_mode->flag_control_rates_enabled = true;
+				control_mode->flag_control_attitude_enabled = true;
+				control_mode->flag_control_velocity_enabled = true;
+				control_mode->flag_control_position_enabled = true;
+				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_manual_enabled = false;
+			}
+
+			break;
+
+		default:
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			current_status->navigation_state = new_navigation_state;
+			navigation_state_changed = true;
+		}
+	}
+
+	return ret;
+}
+
+bool
+check_navigation_state_changed()
+{
+	if (navigation_state_changed) {
+		navigation_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+/**
+* Transition from one hil state to another
+*/
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
+{
+	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_control_mode->flag_system_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_control_mode->flag_system_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;
+
+		current_status->counter++;
+		current_status->timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+		current_control_mode->timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
+
+		ret = OK;
+
+	} else {
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+	}
+
+	return ret;
+}
+
+
+
+// /*
+//  * 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
index 2f2ccc729..a38c2497e 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>
  *
@@ -46,164 +46,32 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
-/**
- * Switch to new state with no checking.
- *
- * do_state_update: this is the functions that all other functions have to call in order to update the state.
- * the function does not question the state change, this must be done before
- * The function performs actions that are connected with the new state (buzzer, reboot, ...)
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- *
- * @return 0 (macro OK) or 1 on error (macro ERROR)
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
-
-/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-
-
-/**
- * Handle state machine if got position fix
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if position fix lost
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if user wants to arm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if user wants to disarm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is manual
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is stabilized
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+typedef enum {
+	TRANSITION_DENIED = -1,
+	TRANSITION_NOT_CHANGED = 0,
+	TRANSITION_CHANGED
 
-/**
- * Handle state machine if mode switch is guided
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+} transition_result_t;
 
-/**
- * Handle state machine if mode switch is auto
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
+	arming_state_t new_arming_state, struct actuator_armed_s *armed);
 
-/**
- * Publish current state information
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
+bool check_arming_state_changed();
 
-/*
- *  Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
- *  If the request is obeyed the functions return 0
- *
- */
+transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
-/**
- * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
+bool check_main_state_changed();
 
-/**
- * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
-
-/**
- * Always switches to critical mode under any circumstances.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Switches to emergency if required.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish the armed state depending on the current system state
- *
- * @param current_status the current system status
- */
-void publish_armed_status(const struct vehicle_status_s *current_status);
+transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
 
+bool check_navigation_state_changed();
 
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
index 2aeca3a98..68c176459 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
@@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	}
 
 	/* Roll (P) */
-	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
+	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL);
 
 
 	/* Pitch (P) */
@@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	/* set pitch plus feedforward roll compensation */
 	rates_sp->pitch = pid_calculate(&pitch_controller,
 					att_sp->pitch_body + pitch_sp_rollcompensation,
-					att->pitch, 0, 0);
+					att->pitch, 0, 0, NULL, NULL, NULL);
 
 	/* Yaw (from coordinated turn constraint or lateral force) */
 	rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
index 6c9c137bb..b96fc3e5a 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
@@ -186,87 +186,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 
 		/* control */
 
-		if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
-		    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
-			/* attitude control */
-			fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
-			/* angular rate control */
-			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-			/* pass through throttle */
-			actuators.control[3] = att_sp.thrust;
-
-			/* set flaps to zero */
-			actuators.control[4] = 0.0f;
-
-		} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-			if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-
-				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-				if (vstatus.rc_signal_lost) {
-
-					/* put plane into loiter */
-					att_sp.roll_body = 0.3f;
-					att_sp.pitch_body = 0.0f;
-
-					/* limit throttle to 60 % of last value if sane */
-					if (isfinite(manual_sp.throttle) &&
-					    (manual_sp.throttle >= 0.0f) &&
-					    (manual_sp.throttle <= 1.0f)) {
-						att_sp.thrust = 0.6f * manual_sp.throttle;
-
-					} else {
-						att_sp.thrust = 0.0f;
-					}
-
-					att_sp.yaw_body = 0;
-
-					// XXX disable yaw control, loiter
-
-				} else {
-
-					att_sp.roll_body = manual_sp.roll;
-					att_sp.pitch_body = manual_sp.pitch;
-					att_sp.yaw_body = 0;
-					att_sp.thrust = manual_sp.throttle;
-				}
-
-				att_sp.timestamp = hrt_absolute_time();
-
-				/* attitude control */
-				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
-				/* angular rate control */
-				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-				/* pass through throttle */
-				actuators.control[3] = att_sp.thrust;
-
-				/* pass through flaps */
-				if (isfinite(manual_sp.flaps)) {
-					actuators.control[4] = manual_sp.flaps;
-
-				} else {
-					actuators.control[4] = 0.0f;
-				}
-
-			} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-				/* directly pass through values */
-				actuators.control[0] = manual_sp.roll;
-				/* positive pitch means negative actuator -> pull up */
-				actuators.control[1] = manual_sp.pitch;
-				actuators.control[2] = manual_sp.yaw;
-				actuators.control[3] = manual_sp.throttle;
-
-				if (isfinite(manual_sp.flaps)) {
-					actuators.control[4] = manual_sp.flaps;
-
-				} else {
-					actuators.control[4] = 0.0f;
-				}
-			}
-		}
+#warning fix this
+//		if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
+//		    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+//			/* attitude control */
+//			fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+//
+//			/* angular rate control */
+//			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+//
+//			/* pass through throttle */
+//			actuators.control[3] = att_sp.thrust;
+//
+//			/* set flaps to zero */
+//			actuators.control[4] = 0.0f;
+//
+//		} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
+//			if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+//
+//				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+//				if (vstatus.rc_signal_lost) {
+//
+//					/* put plane into loiter */
+//					att_sp.roll_body = 0.3f;
+//					att_sp.pitch_body = 0.0f;
+//
+//					/* limit throttle to 60 % of last value if sane */
+//					if (isfinite(manual_sp.throttle) &&
+//					    (manual_sp.throttle >= 0.0f) &&
+//					    (manual_sp.throttle <= 1.0f)) {
+//						att_sp.thrust = 0.6f * manual_sp.throttle;
+//
+//					} else {
+//						att_sp.thrust = 0.0f;
+//					}
+//
+//					att_sp.yaw_body = 0;
+//
+//					// XXX disable yaw control, loiter
+//
+//				} else {
+//
+//					att_sp.roll_body = manual_sp.roll;
+//					att_sp.pitch_body = manual_sp.pitch;
+//					att_sp.yaw_body = 0;
+//					att_sp.thrust = manual_sp.throttle;
+//				}
+//
+//				att_sp.timestamp = hrt_absolute_time();
+//
+//				/* attitude control */
+//				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+//
+//				/* angular rate control */
+//				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+//
+//				/* pass through throttle */
+//				actuators.control[3] = att_sp.thrust;
+//
+//				/* pass through flaps */
+//				if (isfinite(manual_sp.flaps)) {
+//					actuators.control[4] = manual_sp.flaps;
+//
+//				} else {
+//					actuators.control[4] = 0.0f;
+//				}
+//
+//			} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+//				/* directly pass through values */
+//				actuators.control[0] = manual_sp.roll;
+//				/* positive pitch means negative actuator -> pull up */
+//				actuators.control[1] = manual_sp.pitch;
+//				actuators.control[2] = manual_sp.yaw;
+//				actuators.control[3] = manual_sp.throttle;
+//
+//				if (isfinite(manual_sp.flaps)) {
+//					actuators.control[4] = manual_sp.flaps;
+//
+//				} else {
+//					actuators.control[4] = 0.0f;
+//				}
+//			}
+//		}
 
 		/* publish rates */
 		orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
index cdab39edc..66854592a 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 
 	/* roll rate (PI) */
-	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
+	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL);
 	/* pitch rate (PI) */
-	actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
+	actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL);
 	/* yaw rate (PI) */
-	actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
+	actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL);
 
 	counter++;
 
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index f655a13bd..16fcbd864 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -39,6 +39,8 @@
 
 #include "fixedwing.hpp"
 
+#if 0
+
 namespace control
 {
 
@@ -155,8 +157,9 @@ void BlockMultiModeBacksideAutopilot::update()
 	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
 		_actuators.control[i] = 0.0f;
 
+#warning this if is incomplete, should be based on flags
 	// only update guidance in auto mode
-	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
 		// update guidance
 		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 	}
@@ -165,9 +168,10 @@ void BlockMultiModeBacksideAutopilot::update()
 	// once the system switches from manual or auto to stabilized
 	// the setpoint should update to loitering around this position
 
+#warning should be base on flags
 	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+		_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
 
 		// update guidance
 		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
@@ -219,21 +223,23 @@ void BlockMultiModeBacksideAutopilot::update()
 		// This is not a hack, but a design choice.
 
 		/* do not limit in HIL */
-		if (!_status.flag_hil_enabled) {
-			/* limit to value of manual throttle */
-			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-						     _actuators.control[CH_THR] : _manual.throttle;
-		}
-
-	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-
-		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-			_actuators.control[CH_AIL] = _manual.roll;
-			_actuators.control[CH_ELV] = _manual.pitch;
-			_actuators.control[CH_RDR] = _manual.yaw;
-			_actuators.control[CH_THR] = _manual.throttle;
-
-		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+#warning fix this
+		// if (!_status.flag_hil_enabled) {
+		// 	/* limit to value of manual throttle */
+		// 	_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+		// 				     _actuators.control[CH_THR] : _manual.throttle;
+		// }
+
+	} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
+
+#warning fix here too
+//		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+//			_actuators.control[CH_AIL] = _manual.roll;
+//			_actuators.control[CH_ELV] = _manual.pitch;
+//			_actuators.control[CH_RDR] = _manual.yaw;
+//			_actuators.control[CH_THR] = _manual.throttle;
+//
+//		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
 
 			// calculate velocity, XXX should be airspeed, but using ground speed for now
 			// for the purpose of control we will limit the velocity feedback between
@@ -284,12 +290,14 @@ void BlockMultiModeBacksideAutopilot::update()
 			// This is not a hack, but a design choice.
 
 			/* do not limit in HIL */
-			if (!_status.flag_hil_enabled) {
-				/* limit to value of manual throttle */
-				_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-							     _actuators.control[CH_THR] : _manual.throttle;
-			}
-		}
+#warning fix this
+			// if (!_status.flag_hil_enabled) {
+			// 	/* limit to value of manual throttle */
+			// 	_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+			// 				     _actuators.control[CH_THR] : _manual.throttle;
+			// }
+#warning fix this
+//		}
 
 		// body rates controller, disabled for now
 		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
@@ -322,3 +330,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
 
 } // namespace control
 
+#endif
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index f4ea05088..b0de69f55 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -151,12 +151,14 @@ int control_demo_thread_main(int argc, char *argv[])
 
 	using namespace control;
 
-	fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
+#warning fix this
+//	fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
 
 	thread_running = true;
 
 	while (!thread_should_exit) {
-		autopilot.update();
+#warning fix this
+//		autopilot.update();
 	}
 
 	warnx("exiting.");
diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
index 73df3fb9e..87a942ffb 100644
--- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -271,7 +271,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 				pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
 				pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
 				pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
-				pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
+				pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F); //TODO: remove hardcoded value
 			}
 
 			/* only run controller if attitude changed */
@@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 					// XXX what is xtrack_err.past_end?
 					if (distance_res == OK /*&& !xtrack_err.past_end*/) {
 
-						float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
+						float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance
 
 						float psi_c = psi_track + delta_psi_c;
 						float psi_e = psi_c - att.yaw;
@@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 						}
 
 						/* calculate roll setpoint, do this artificially around zero */
-						float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
+						float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL);
 						float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
 						float psi_rate_c = delta_psi_rate_c + psi_rate_track;
 
@@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 						float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
 
-						attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
+						attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL);
 
 						if (verbose) {
 							printf("psi_rate_c %.4f ", (double)psi_rate_c);
@@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 					if (pos_updated) {
 
 						//TODO: take care of relative vs. ab. altitude
-						attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
+						attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL);
 
 					}
 
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 7466dfdb9..d383146f9 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -51,6 +51,7 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 #include <modules/px4iofirmware/protocol.h>
@@ -62,6 +63,8 @@ struct gpio_led_s {
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
+	struct actuator_armed_s armed;
+	int actuator_armed_sub;
 	bool led_state;
 	int counter;
 };
@@ -227,10 +230,15 @@ void gpio_led_cycle(FAR void *arg)
 	if (status_updated)
 		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
 
+	orb_check(priv->vehicle_status_sub, &status_updated);
+
+	if (status_updated)
+		orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
+
 	/* select pattern for current status */
 	int pattern = 0;
 
-	if (priv->status.flag_system_armed) {
+	if (priv->armed.armed) {
 		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x3f;	// ****** solid (armed)
 
@@ -239,11 +247,10 @@ void gpio_led_cycle(FAR void *arg)
 		}
 
 	} else {
-		if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+		if (priv->armed.ready_to_arm) {
 			pattern = 0x00;	// ______ off (disarmed, preflight check)
 
-		} else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
-			   priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+		} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
 		} else {
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 919d01561..6d9ca1120 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -64,6 +64,7 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 #include "waypoints.h"
 #include "orb_topics.h"
@@ -181,102 +182,68 @@ set_hil_on_off(bool hil_enabled)
 }
 
 void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
+get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	/* reset MAVLink mode bitfield */
-	*mavlink_mode = 0;
+	*mavlink_base_mode = 0;
+	*mavlink_custom_mode = 0;
 
-	/* set mode flags independent of system state */
+	/**
+	 * Set mode flags
+	 **/
 
 	/* HIL */
-	if (v_status.flag_hil_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	if (v_status.hil_state == HIL_STATE_ON) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
-	/* manual input */
-	if (v_status.flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+	/* arming state */
+	if (v_status.arming_state == ARMING_STATE_ARMED
+			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	/* attitude or rate control */
-	if (v_status.flag_control_attitude_enabled ||
-	    v_status.flag_control_rates_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+	/* main state */
+	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+	if (v_status.main_state == MAIN_STATE_MANUAL) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+		*mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL;
+	} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+		*mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT;
+	} else if (v_status.main_state == MAIN_STATE_EASY) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		*mavlink_custom_mode = PX4_CUSTOM_MODE_EASY;
+	} else if (v_status.main_state == MAIN_STATE_AUTO) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		*mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO;
 	}
 
-	/* vector control */
-	if (v_status.flag_control_velocity_enabled ||
-	    v_status.flag_control_position_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-	}
-
-	/* autonomous mode */
-	if (v_status.state_machine == SYSTEM_STATE_AUTO) {
-		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
-	}
-
-	/* set arming state */
-	if (armed.armed) {
-		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	switch (v_status.state_machine) {
-	case SYSTEM_STATE_PREFLIGHT:
-		if (v_status.flag_preflight_gyro_calibration ||
-		    v_status.flag_preflight_mag_calibration ||
-		    v_status.flag_preflight_accel_calibration) {
-			*mavlink_state = MAV_STATE_CALIBRATING;
-
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
-		}
+	/**
+	 * Set mavlink state
+	 **/
 
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
+	/* set calibration state */
+	if (v_status.preflight_calibration) {
+		*mavlink_state = MAV_STATE_CALIBRATING;
+	} else if (v_status.system_emergency) {
 		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_REBOOT:
+	} else if (v_status.arming_state == ARMING_STATE_INIT
+			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
+			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		*mavlink_state = MAV_STATE_UNINIT;
+	} else if (v_status.arming_state == ARMING_STATE_ARMED) {
+		*mavlink_state = MAV_STATE_ACTIVE;
+	} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_state = MAV_STATE_CRITICAL;
+	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
+		*mavlink_state = MAV_STATE_STANDBY;
+	} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
 		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
+	} else {
+		warnx("Unknown mavlink state");
+		*mavlink_state = MAV_STATE_CRITICAL;
 	}
-
 }
 
 
@@ -568,6 +535,7 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		default:
 			usage();
+			break;
 		}
 	}
 
@@ -674,14 +642,18 @@ int mavlink_thread_main(int argc, char *argv[])
 
 			/* translate the current system state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			uint8_t mavlink_base_mode = 0;
+			uint32_t mavlink_custom_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
 
 			/* switch HIL mode if required */
-			set_hil_on_off(v_status.flag_hil_enabled);
+			if (v_status.hil_state == HIL_STATE_ON)
+				set_hil_on_off(true);
+			else if (v_status.hil_state == HIL_STATE_OFF)
+				set_hil_on_off(false);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan,
@@ -689,8 +661,8 @@ int mavlink_thread_main(int argc, char *argv[])
 						    v_status.onboard_control_sensors_enabled,
 						    v_status.onboard_control_sensors_health,
 						    v_status.load,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
+						    v_status.battery_voltage * 1000.0f,
+						    v_status.battery_current * 1000.0f,
 						    v_status.battery_remaining,
 						    v_status.drop_rate_comm,
 						    v_status.errors_comm,
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index edb8761b8..2a260861d 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -272,19 +272,23 @@ l_vehicle_status(const struct listener *l)
 	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
 
 	/* enable or disable HIL */
-	set_hil_on_off(v_status.flag_hil_enabled);
+	if (v_status.hil_state == HIL_STATE_ON)
+		set_hil_on_off(true);
+	else if (v_status.hil_state == HIL_STATE_OFF)
+		set_hil_on_off(false);
 
 	/* translate the current syste state to mavlink state and mode */
 	uint8_t mavlink_state = 0;
-	uint8_t mavlink_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+	uint8_t mavlink_base_mode = 0;
+	uint32_t mavlink_custom_mode = 0;
+	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 	/* send heartbeat */
 	mavlink_msg_heartbeat_send(chan,
 				   mavlink_system.type,
 				   MAV_AUTOPILOT_PX4,
-				   mavlink_mode,
-				   v_status.state_machine,
+				   mavlink_base_mode,
+				   mavlink_custom_mode,
 				   mavlink_state);
 }
 
@@ -470,8 +474,9 @@ l_actuator_outputs(const struct listener *l)
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			uint8_t mavlink_base_mode = 0;
+			uint32_t mavlink_custom_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* HIL message as per MAVLink spec */
 
@@ -488,7 +493,7 @@ l_actuator_outputs(const struct listener *l)
 							      -1,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +507,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +521,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
 							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else {
@@ -530,7 +535,7 @@ l_actuator_outputs(const struct listener *l)
 							      (act_outputs.output[5] - 1500.0f) / 500.0f,
 							      (act_outputs.output[6] - 1500.0f) / 500.0f,
 							      (act_outputs.output[7] - 1500.0f) / 500.0f,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 			}
 		}
@@ -673,7 +678,7 @@ uorb_receive_thread(void *arg)
 
 		/* handle the poll result */
 		if (poll_ret == 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+			/* silent */
 
 		} else if (poll_ret < 0) {
 			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 73e278dc6..506f73105 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -55,10 +55,12 @@
 #include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/debug_key_value.h>
@@ -75,8 +77,9 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
+	int armed_sub;
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h
index a4ff06a88..5e5ee8261 100644
--- a/src/modules/mavlink/util.h
+++ b/src/modules/mavlink/util.h
@@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
+extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index 20fb11b2c..c5dbd00dd 100644
--- a/src/modules/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -273,18 +273,18 @@ void mavlink_update_system(void)
 }
 
 void
-get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
 	uint8_t *mavlink_state, uint8_t *mavlink_mode)
 {
 	/* reset MAVLink mode bitfield */
 	*mavlink_mode = 0;
 
 	/* set mode flags independent of system state */
-	if (v_status->flag_control_manual_enabled) {
+	if (control_mode->flag_control_manual_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 	}
 
-	if (v_status->flag_hil_enabled) {
+	if (control_mode->flag_system_hil_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
@@ -295,61 +295,67 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
 		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	switch (v_status->state_machine) {
-	case SYSTEM_STATE_PREFLIGHT:
-		if (v_status->flag_preflight_gyro_calibration ||
-		    v_status->flag_preflight_mag_calibration ||
-		    v_status->flag_preflight_accel_calibration) {
-			*mavlink_state = MAV_STATE_CALIBRATING;
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
-		}
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		*mavlink_state = MAV_STATE_ACTIVE;
+	if (control_mode->flag_control_velocity_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-		break;
-
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
+	} else {
+		*mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
 	}
 
+//	switch (v_status->state_machine) {
+//	case SYSTEM_STATE_PREFLIGHT:
+//		if (v_status->flag_preflight_gyro_calibration ||
+//		    v_status->flag_preflight_mag_calibration ||
+//		    v_status->flag_preflight_accel_calibration) {
+//			*mavlink_state = MAV_STATE_CALIBRATING;
+//		} else {
+//			*mavlink_state = MAV_STATE_UNINIT;
+//		}
+//		break;
+//
+//	case SYSTEM_STATE_STANDBY:
+//		*mavlink_state = MAV_STATE_STANDBY;
+//		break;
+//
+//	case SYSTEM_STATE_GROUND_READY:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		break;
+//
+//	case SYSTEM_STATE_MANUAL:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+//		break;
+//
+//	case SYSTEM_STATE_STABILIZED:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+//		break;
+//
+//	case SYSTEM_STATE_AUTO:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+//		break;
+//
+//	case SYSTEM_STATE_MISSION_ABORT:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_EMCY_LANDING:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_EMCY_CUTOFF:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_GROUND_ERROR:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_REBOOT:
+//		*mavlink_state = MAV_STATE_POWEROFF;
+//		break;
+//	}
+
 }
 
 /**
@@ -361,7 +367,9 @@ int mavlink_thread_main(int argc, char *argv[])
 	char *device_name = "/dev/ttyS1";
 	baudrate = 57600;
 
+	/* XXX this is never written? */
 	struct vehicle_status_s v_status;
+	struct vehicle_control_mode_s control_mode;
 	struct actuator_armed_s armed;
 
 	/* work around some stupidity in task_create's argv handling */
@@ -430,10 +438,10 @@ int mavlink_thread_main(int argc, char *argv[])
 			/* translate the current system state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
 			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
+			get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan,
@@ -441,8 +449,8 @@ int mavlink_thread_main(int argc, char *argv[])
 						    v_status.onboard_control_sensors_enabled,
 						    v_status.onboard_control_sensors_health,
 						    v_status.load,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
+						    v_status.battery_voltage * 1000.0f,
+						    v_status.battery_current * 1000.0f,
 						    v_status.battery_remaining,
 						    v_status.drop_rate_comm,
 						    v_status.errors_comm,
diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
index 68d49c24b..0236e6126 100644
--- a/src/modules/mavlink_onboard/mavlink_receiver.c
+++ b/src/modules/mavlink_onboard/mavlink_receiver.c
@@ -327,4 +327,4 @@ receive_start(int uart)
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
 	return thread;
-}
\ No newline at end of file
+}
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
index f18f56243..1b49c9ce4 100644
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ b/src/modules/mavlink_onboard/orb_topics.h
@@ -52,9 +52,11 @@
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_rc_input.h>
@@ -69,7 +71,7 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
 	int local_pos_sub;
 	int spa_sub;
diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h
index 38a4db372..c84b6fd26 100644
--- a/src/modules/mavlink_onboard/util.h
+++ b/src/modules/mavlink_onboard/util.h
@@ -50,5 +50,6 @@ extern volatile bool thread_should_exit;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+extern void
+get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
 	uint8_t *mavlink_state, uint8_t *mavlink_mode);
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 99f25cfe9..20502c0ea 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -57,12 +57,14 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <drivers/drv_gyro.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_debug.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/parameter_update.h>
@@ -83,14 +85,17 @@ static int mc_task;
 static bool motor_test_mode = false;
 
 static orb_advert_t actuator_pub;
+static orb_advert_t control_debug_pub;
 
-static struct vehicle_status_s state;
 
 static int
 mc_thread_main(int argc, char *argv[])
 {
 	/* declare and safely initialize all structs */
-	memset(&state, 0, sizeof(state));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
+	struct actuator_armed_s armed;
+	memset(&armed, 0, sizeof(armed));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
@@ -107,12 +112,16 @@ mc_thread_main(int argc, char *argv[])
 	struct actuator_controls_s actuators;
 	memset(&actuators, 0, sizeof(actuators));
 
+	struct vehicle_control_debug_s control_debug;
+	memset(&control_debug, 0, sizeof(control_debug));
+
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int param_sub = orb_subscribe(ORB_ID(parameter_update));
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 
@@ -133,6 +142,8 @@ mc_thread_main(int argc, char *argv[])
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
 		actuators.control[i] = 0.0f;
 	}
+	
+	control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug);
 
 	actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
@@ -150,7 +161,9 @@ mc_thread_main(int argc, char *argv[])
 	/* store last control mode to detect mode switches */
 	bool flag_control_manual_enabled = false;
 	bool flag_control_attitude_enabled = false;
-	bool flag_system_armed = false;
+	bool flag_armed = false;
+	bool flag_control_position_enabled = false;
+	bool flag_control_velocity_enabled = false;
 
 	/* store if yaw position or yaw speed has been changed */
 	bool control_yaw_position = true;
@@ -162,7 +175,6 @@ mc_thread_main(int argc, char *argv[])
 	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
 	float failsafe_throttle = 0.0f;
 
-
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
@@ -175,7 +187,6 @@ mc_thread_main(int argc, char *argv[])
 		} else if (ret == 0) {
 			/* no return value, ignore */
 		} else {
-
 			/* only update parameters if they changed */
 			if (fds[1].revents & POLLIN) {
 				/* read from param to clear updated flag */
@@ -193,10 +204,16 @@ mc_thread_main(int argc, char *argv[])
 
 				/* get a local copy of system state */
 				bool updated;
-				orb_check(state_sub, &updated);
+				orb_check(control_mode_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+				}
+
+				orb_check(armed_sub, &updated);
 
 				if (updated) {
-					orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 				}
 
 				/* get a local copy of manual setpoint */
@@ -215,9 +232,8 @@ mc_thread_main(int argc, char *argv[])
 				/* get a local copy of the current sensor values */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
-
 				/** STEP 1: Define which input is the dominating control input */
-				if (state.flag_control_offboard_enabled) {
+				if (control_mode.flag_control_offboard_enabled) {
 					/* offboard inputs */
 					if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
 						rates_sp.roll = offboard_sp.p1;
@@ -240,102 +256,51 @@ mc_thread_main(int argc, char *argv[])
 					}
 
 
-				} else if (state.flag_control_manual_enabled) {
-
-					if (state.flag_control_attitude_enabled) {
-
+				} else if (control_mode.flag_control_manual_enabled) {			
+					if (control_mode.flag_control_attitude_enabled) {
 						/* initialize to current yaw if switching to manual or att control */
-						if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
-						    state.flag_control_manual_enabled != flag_control_manual_enabled ||
-						    state.flag_system_armed != flag_system_armed) {
+						if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
+						    control_mode.flag_control_manual_enabled != flag_control_manual_enabled ||
+						    armed.armed != flag_armed) {
 							att_sp.yaw_body = att.yaw;
 						}
 
 						static bool rc_loss_first_time = true;
 
 						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-						if (state.rc_signal_lost) {
-							/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
-							param_get(failsafe_throttle_handle, &failsafe_throttle);
-							att_sp.roll_body = 0.0f;
-							att_sp.pitch_body = 0.0f;
 
-							/*
-							 * Only go to failsafe throttle if last known throttle was
-							 * high enough to create some lift to make hovering state likely.
-							 *
-							 * This is to prevent that someone landing, but not disarming his
-							 * multicopter (throttle = 0) does not make it jump up in the air
-							 * if shutting down his remote.
-							 */
-							if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
-								att_sp.thrust = failsafe_throttle;
-
-							} else {
-								att_sp.thrust = 0.0f;
-							}
-
-							/* keep current yaw, do not attempt to go to north orientation,
-							 * since if the pilot regains RC control, he will be lost regarding
-							 * the current orientation.
-							 */
-							if (rc_loss_first_time)
-								att_sp.yaw_body = att.yaw;
+						rc_loss_first_time = true;
 
-							rc_loss_first_time = false;
+						att_sp.roll_body = manual.roll;
+						att_sp.pitch_body = manual.pitch;
 
-						} else {
-							rc_loss_first_time = true;
+						/* set attitude if arming */
+						if (!flag_control_attitude_enabled && armed.armed) {
+							att_sp.yaw_body = att.yaw;
+						}
 
-							att_sp.roll_body = manual.roll;
-							att_sp.pitch_body = manual.pitch;
+						/* only move setpoint if manual input is != 0 */
+						if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) {
+							rates_sp.yaw = manual.yaw;
+							control_yaw_position = false;
+							first_time_after_yaw_speed_control = true;
 
-							/* set attitude if arming */
-							if (!flag_control_attitude_enabled && state.flag_system_armed) {
+						} else {
+							if (first_time_after_yaw_speed_control) {
 								att_sp.yaw_body = att.yaw;
+								first_time_after_yaw_speed_control = false;
 							}
 
-							/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
-							if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
-							    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-
-								if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
-									rates_sp.yaw = manual.yaw;
-									control_yaw_position = false;
-
-								} else {
-									/*
-									 * This mode SHOULD be the default mode, which is:
-									 * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
-									 *
-									 * However, we fall back to this setting for all other (nonsense)
-									 * settings as well.
-									 */
-
-									/* only move setpoint if manual input is != 0 */
-									if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
-										rates_sp.yaw = manual.yaw;
-										control_yaw_position = false;
-										first_time_after_yaw_speed_control = true;
-
-									} else {
-										if (first_time_after_yaw_speed_control) {
-											att_sp.yaw_body = att.yaw;
-											first_time_after_yaw_speed_control = false;
-										}
-
-										control_yaw_position = true;
-									}
-								}
-							}
-
-							att_sp.thrust = manual.throttle;
-							att_sp.timestamp = hrt_absolute_time();
+							control_yaw_position = true;
 						}
 
+						att_sp.thrust = manual.throttle;
+						att_sp.timestamp = hrt_absolute_time();
+
 						/* STEP 2: publish the controller output */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
+
 						if (motor_test_mode) {
 							printf("testmode");
 							att_sp.roll_body = 0.0f;
@@ -348,23 +313,26 @@ mc_thread_main(int argc, char *argv[])
 						}
 
 					} else {
-						/* manual rate inputs, from RC control or joystick */
-						if (state.flag_control_rates_enabled &&
-						    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
-							rates_sp.roll = manual.roll;
 
-							rates_sp.pitch = manual.pitch;
-							rates_sp.yaw = manual.yaw;
-							rates_sp.thrust = manual.throttle;
-							rates_sp.timestamp = hrt_absolute_time();
-						}
+						//XXX TODO add acro mode here
+
+						/* manual rate inputs, from RC control or joystick */
+//						if (state.flag_control_rates_enabled &&
+//						    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
+//							rates_sp.roll = manual.roll;
+//
+//							rates_sp.pitch = manual.pitch;
+//							rates_sp.yaw = manual.yaw;
+//							rates_sp.thrust = manual.throttle;
+//							rates_sp.timestamp = hrt_absolute_time();
+//						}
 					}
 
 				}
 
 				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
-				if (state.flag_control_attitude_enabled) {
-					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
+				if (control_mode.flag_control_attitude_enabled) {
+					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, &control_debug_pub, &control_debug);
 
 					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
@@ -372,7 +340,8 @@ mc_thread_main(int argc, char *argv[])
 				/* measure in what intervals the controller runs */
 				perf_count(mc_interval_perf);
 
-				float gyro[3];
+				float rates[3];
+				float rates_acc[3];
 
 				/* get current rate setpoint */
 				bool rates_sp_valid = false;
@@ -383,17 +352,21 @@ mc_thread_main(int argc, char *argv[])
 				}
 
 				/* apply controller */
-				gyro[0] = att.rollspeed;
-				gyro[1] = att.pitchspeed;
-				gyro[2] = att.yawspeed;
+				rates[0] = att.rollspeed;
+				rates[1] = att.pitchspeed;
+				rates[2] = att.yawspeed;
 
-				multirotor_control_rates(&rates_sp, gyro, &actuators);
+				multirotor_control_rates(&rates_sp, rates, &actuators, &control_debug_pub, &control_debug);
 				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-				/* update state */
-				flag_control_attitude_enabled = state.flag_control_attitude_enabled;
-				flag_control_manual_enabled = state.flag_control_manual_enabled;
-				flag_system_armed = state.flag_system_armed;
+				orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug);
+
+				/* update control_mode */
+				flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
+				flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
+				flag_control_position_enabled = control_mode.flag_control_position_enabled;
+				flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled;
+				flag_armed = armed.armed;
 
 				perf_end(mc_loop_perf);
 			} /* end of poll call for attitude updates */
@@ -410,7 +383,7 @@ mc_thread_main(int argc, char *argv[])
 
 
 	close(att_sub);
-	close(state_sub);
+	close(control_mode_sub);
 	close(manual_sub);
 	close(actuator_pub);
 	close(att_sp_pub);
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 8f19c6a4b..083311674 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -65,50 +65,29 @@
 PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
 
 PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
 PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
-
-//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
 
 struct mc_att_control_params {
 	float yaw_p;
 	float yaw_i;
 	float yaw_d;
-	//float yaw_awu;
-	//float yaw_lim;
 
 	float att_p;
 	float att_i;
 	float att_d;
-	//float att_awu;
-	//float att_lim;
-
-	//float att_xoff;
-	//float att_yoff;
 };
 
 struct mc_att_control_param_handles {
 	param_t yaw_p;
 	param_t yaw_i;
 	param_t yaw_d;
-	//param_t yaw_awu;
-	//param_t yaw_lim;
 
 	param_t att_p;
 	param_t att_i;
 	param_t att_d;
-	//param_t att_awu;
-	//param_t att_lim;
-
-	//param_t att_xoff;
-	//param_t att_yoff;
 };
 
 /**
@@ -130,17 +109,10 @@ static int parameters_init(struct mc_att_control_param_handles *h)
 	h->yaw_p 	=	param_find("MC_YAWPOS_P");
 	h->yaw_i 	=	param_find("MC_YAWPOS_I");
 	h->yaw_d 	=	param_find("MC_YAWPOS_D");
-	//h->yaw_awu 	=	param_find("MC_YAWPOS_AWU");
-	//h->yaw_lim 	=	param_find("MC_YAWPOS_LIM");
 
 	h->att_p 	= 	param_find("MC_ATT_P");
 	h->att_i 	= 	param_find("MC_ATT_I");
 	h->att_d 	= 	param_find("MC_ATT_D");
-	//h->att_awu 	= 	param_find("MC_ATT_AWU");
-	//h->att_lim 	= 	param_find("MC_ATT_LIM");
-
-	//h->att_xoff 	= 	param_find("MC_ATT_XOFF");
-	//h->att_yoff 	= 	param_find("MC_ATT_YOFF");
 
 	return OK;
 }
@@ -150,23 +122,17 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 	param_get(h->yaw_p, &(p->yaw_p));
 	param_get(h->yaw_i, &(p->yaw_i));
 	param_get(h->yaw_d, &(p->yaw_d));
-	//param_get(h->yaw_awu, &(p->yaw_awu));
-	//param_get(h->yaw_lim, &(p->yaw_lim));
 
 	param_get(h->att_p, &(p->att_p));
 	param_get(h->att_i, &(p->att_i));
 	param_get(h->att_d, &(p->att_d));
-	//param_get(h->att_awu, &(p->att_awu));
-	//param_get(h->att_lim, &(p->att_lim));
-
-	//param_get(h->att_xoff, &(p->att_xoff));
-	//param_get(h->att_yoff, &(p->att_yoff));
 
 	return OK;
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
+				 const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
 {
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
@@ -207,7 +173,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 		/* apply parameters */
 		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
-		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
+		pid_set_parameters(&roll_controller,  p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 	}
 
 	/* reset integral if on ground */
@@ -221,11 +187,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	/* control pitch (forward) output */
 	rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
-					att->pitch, att->pitchspeed, deltaT);
+					att->pitch, att->pitchspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d);
 
 	/* control roll (left/right) output */
 	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
-				       att->roll, att->rollspeed, deltaT);
+				       att->roll, att->rollspeed, deltaT, NULL, NULL, NULL);
+	
+	// printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT);
 
 	if (control_yaw_position) {
 		/* control yaw rate */
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h
index e78f45c47..6dd5b39fd 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h
@@ -58,8 +58,11 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_debug.h>
+
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
+				 const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index e58d357d5..ee8c37580 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -58,6 +58,9 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+
+
 
 PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
@@ -152,7 +155,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 }
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators)
+			      const float rates[], struct actuator_controls_s *actuators,
+			      const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
 {
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -172,8 +176,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	static struct mc_rate_control_params p;
 	static struct mc_rate_control_param_handles h;
 
+	float pitch_control_last = 0.0f;
+	float roll_control_last = 0.0f;
+
 	static bool initialized = false;
 
+	float diff_filter_factor = 1.0f;
+
 	/* initialize the pid controllers when the function is called for the first time */
 	if (initialized == false) {
 		parameters_init(&h);
@@ -201,11 +210,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	/* control pitch (forward) output */
 	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
-					    rates[1], 0.0f, deltaT);
+					    rates[1], 0.0f, deltaT,
+					    &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d);
 
 	/* control roll (left/right) output */
 	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
-					   rates[0], 0.0f, deltaT);
+					   rates[0], 0.0f, deltaT,
+					   &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d);
 
 	/* control yaw rate */ //XXX use library here
 	float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
index 362b5ed86..695ff3e16 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.h
@@ -57,8 +57,10 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_debug.h>
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators);
+			      const float rates[], struct actuator_controls_s *actuators,
+			      const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 74037d1c2..796c6cd9f 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -300,6 +300,8 @@ controls_tick() {
 		} else {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 		}
+	} else {
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 	}
 }
 
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 598bcee34..4925ced54 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -129,9 +129,9 @@ dsm_bind(uint16_t cmd, int pulses)
 	case dsm_bind_send_pulses:
 		for (int i = 0; i < pulses; i++) {
 			stm32_gpiowrite(usart1RxAsOutp, false);
-			up_udelay(50);
+			up_udelay(25);
 			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(50);
+			up_udelay(25);
 		}
 		break;
 	case dsm_bind_reinit_uart:
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index d8c0e58ba..38727c68c 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -59,6 +59,12 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		200000
 
+/*
+ * Time that the ESCs need to initialize
+ */
+ #define ESC_INIT_TIME_US		1000000
+ #define ESC_RAMP_TIME_US		2000000
+
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
 #define PITCH    1
@@ -68,6 +74,17 @@ extern "C" {
 
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
+static bool should_arm = false;
+static bool should_always_enable_pwm = false;
+static uint64_t esc_init_time;
+
+enum esc_state_e {
+	ESC_OFF,
+	ESC_INIT,
+	ESC_RAMP,
+	ESC_ON
+};
+static esc_state_e esc_state;
 
 /* selected control values and count for mixing */
 enum mixer_source {
@@ -98,7 +115,7 @@ mixer_tick(void)
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
 			isr_debug(1, "AP RX timeout");
 		}
-		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 
 	} else {
@@ -112,12 +129,11 @@ mixer_tick(void)
 	 * Decide which set of controls we're using.
 	 */
 
-	/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+	/* do not mix if RAW_PWM mode is on and FMU is good */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
-	        !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+	        (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values or
-		 not a valid mixer. */
+		/* don't actually mix anything - we already have raw PWM values */
 		source = MIX_NONE;
 
 	} else {
@@ -167,6 +183,48 @@ mixer_tick(void)
 		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
+		uint16_t ramp_promille;
+
+		/* update esc init state, but only if we are truely armed and not just PWM enabled */
+		if (mixer_servos_armed && should_arm) {
+
+			switch (esc_state) {
+
+				/* after arming, some ESCs need an initalization period, count the time from here */
+				case ESC_OFF:
+					esc_init_time = hrt_absolute_time();
+					esc_state = ESC_INIT;
+				break;
+
+				/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
+				case ESC_INIT:
+					if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
+						esc_state = ESC_RAMP;
+					}
+				break;
+
+				/* then ramp until the min speed is reached */
+				case ESC_RAMP:
+					if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
+						esc_state = ESC_ON;
+					}
+				break;
+
+				case ESC_ON:
+				default:
+
+				break;
+			}
+		} else {
+			esc_state = ESC_OFF;
+		}
+
+		/* do the calculations during the ramp for all at once */
+		if(esc_state == ESC_RAMP) {
+			ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
+		}
+
+
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
@@ -176,9 +234,27 @@ mixer_tick(void)
 			/* save actuator values for FMU readback */
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 
-			/* scale to servo output */
-			r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
-
+			switch (esc_state) {
+				case ESC_INIT:
+					r_page_servos[i] = (outputs[i] * 600 + 1500);
+				break;
+
+				case ESC_RAMP:
+					r_page_servos[i] = (outputs[i]
+					 * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+					 + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
+				break;
+
+				case ESC_ON:
+					r_page_servos[i] = (outputs[i]
+					 * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+					 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
+				break;
+
+				case ESC_OFF:
+				default:
+				break;
+			}
 		}
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 			r_page_servos[i] = 0;
@@ -193,30 +269,46 @@ mixer_tick(void)
 	 * XXX correct behaviour for failsafe may require an additional case
 	 * here.
 	 */
-	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
-		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
-		/* FMU is available or FMU is not available but override is an option */
-		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+	should_arm = (
+		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+		/* and FMU is armed */ 		  && (
+							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+		/* or failsafe was set manually */	 || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+						     )
 	);
 
-	if (should_arm && !mixer_servos_armed) {
+	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
+		isr_debug(5, "> PWM enabled");
 
-	} else if (!should_arm && mixer_servos_armed) {
+	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
+		isr_debug(5, "> PWM disabled");
+
 	}
 
-	if (mixer_servos_armed) {
+	if (mixer_servos_armed && should_arm) {
 		/* update the servo outputs. */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
+
+	} else if (mixer_servos_armed && should_always_enable_pwm) {
+		/* set the idle servo outputs. */
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
+			up_pwm_servo_set(i, r_page_servo_idle[i]);
 	}
 }
 
@@ -265,9 +357,8 @@ static unsigned mixer_text_length = 0;
 void
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while fully armed */
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	/* do not allow a mixer change while outputs armed */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 		return;
 	}
 
@@ -344,6 +435,7 @@ mixer_set_failsafe()
 	 * Check if a custom failsafe value has been written,
 	 * or if the mixer is not ok and bail out.
 	 */
+
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
 		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
 		return;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 3c59a75a7..02df76068 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -94,7 +94,7 @@
 #define PX4IO_P_STATUS_CPULOAD			1
 
 #define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_ARMED		(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED	(1 << 0) /* arm-ok and locally armed */
 #define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
 #define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
@@ -106,7 +106,8 @@
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 12) /* DSM input is 11 bit data */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 13) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -157,6 +158,7 @@
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
 #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 3) /* use custom failsafe values, not 0 values of mixer */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
@@ -209,6 +211,15 @@ enum {							/* DSM bind states */
 #define PX4IO_PAGE_TEST			127
 #define PX4IO_P_TEST_LED			0	/* set the amber LED on/off */
 
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM idle values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_IDLE_PWM		108	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
 /**
  * As-needed mixer data upload.
  *
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 9eb092a63..d6cf22b21 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -78,6 +78,9 @@ extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
 extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
 extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
 extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+extern uint16_t			r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
+extern uint16_t			r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
+extern uint16_t			r_page_servo_idle[];	/* PX4IO_PAGE_IDLE_PWM */
 
 /*
  * Register aliases.
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 3f241d29c..3ff9307cd 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -158,7 +158,9 @@ volatile uint16_t	r_page_setup[] =
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
-					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \
+					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
+					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -196,6 +198,30 @@ uint16_t		r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
  */
 uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
 
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+
+/**
+ * PAGE 108
+ *
+ * idle PWM values for difficult ESCs
+ *
+ */
+uint16_t		r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
 int
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
@@ -259,6 +285,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 		break;
 
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_control_min[offset] = 900;
+
+			else if (*values > 1200)
+				r_page_servo_control_min[offset] = 1200;
+			else if (*values < 900)
+				r_page_servo_control_min[offset] = 900;
+			else
+				r_page_servo_control_min[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+	
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_control_max[offset] = 2100;
+
+			else if (*values > 2100)
+				r_page_servo_control_max[offset] = 2100;
+			else if (*values < 1800)
+				r_page_servo_control_max[offset] = 1800;
+			else
+				r_page_servo_control_max[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
+	case PX4IO_PAGE_IDLE_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_idle[offset] = 0;
+
+			else if (*values < 900)
+				r_page_servo_idle[offset] = 900;
+			else if (*values > 2100)
+				r_page_servo_idle[offset] = 2100;
+			else
+				r_page_servo_idle[offset] = *values;
+
+			/* flag the failsafe values as custom */
+			r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
 		mixer_handle_text(values, num_values * sizeof(*values));
@@ -331,9 +426,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * so that an in-air reset of FMU can not lead to a
 			 * lockup of the IO arming state.
 			 */
-			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
-			}
+
+			// XXX do not reset IO's safety state by FMU for now
+			// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			// }
 
 			r_setup_arming = value;
 
@@ -397,9 +494,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 	case PX4IO_PAGE_RC_CONFIG: {
 
-		/* do not allow a RC config change while fully armed */
-		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		/* do not allow a RC config change while outputs armed */
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 			break;
 		}
 
@@ -659,6 +755,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 	case PX4IO_PAGE_FAILSAFE_PWM:
 		SELECT_PAGE(r_page_servo_failsafe);
 		break;
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+		SELECT_PAGE(r_page_servo_control_min);
+		break;
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+		SELECT_PAGE(r_page_servo_control_max);
+		break;
+	case PX4IO_PAGE_IDLE_PWM:
+		SELECT_PAGE(r_page_servo_idle);
+		break;
 
 	default:
 		return -1;
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 4dbecc274..95335f038 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -110,7 +110,7 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
 		(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* switch to armed state */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
-	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
-	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 		if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
 
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ba7cdd91c..fefa539f9 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -60,6 +60,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -75,6 +76,7 @@
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_control_debug.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
@@ -192,7 +194,7 @@ static int file_copy(const char *file_old, const char *file_new);
 
 static void handle_command(struct vehicle_command_s *cmd);
 
-static void handle_status(struct vehicle_status_s *cmd);
+static void handle_status(struct actuator_armed_s *armed);
 
 /**
  * Create folder for current logging session. Store folder name in 'log_folder'.
@@ -623,9 +625,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "can't allocate log buffer, exiting.");
 	}
 
-	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of messages */
-	const ssize_t fdsc = 19;
+	/* --- IMPORTANT: DEFINE MAX NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
+	/* max number of messages */
+	const ssize_t fdsc = 21;
+
 	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
@@ -634,6 +637,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	struct vehicle_status_s buf_status;
 	memset(&buf_status, 0, sizeof(buf_status));
 
+	struct actuator_armed_s buf_armed;
+	memset(&buf_armed, 0, sizeof(buf_armed));
+
 	/* warning! using union here to save memory, elements should be used separately! */
 	union {
 		struct vehicle_command_s cmd;
@@ -650,6 +656,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_global_position_setpoint_s global_pos_sp;
 		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
+		struct vehicle_control_debug_s control_debug;
 		struct optical_flow_s flow;
 		struct rc_channels_s rc;
 		struct differential_pressure_s diff_pres;
@@ -661,6 +668,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	struct {
 		int cmd_sub;
 		int status_sub;
+		int armed_sub;
 		int sensor_sub;
 		int att_sub;
 		int att_sp_sub;
@@ -674,6 +682,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int global_pos_sp_sub;
 		int gps_pos_sub;
 		int vicon_pos_sub;
+		int control_debug_sub;
 		int flow_sub;
 		int rc_sub;
 		int airspeed_sub;
@@ -695,6 +704,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPS_s log_GPS;
 			struct log_ATTC_s log_ATTC;
 			struct log_STAT_s log_STAT;
+			struct log_CTRL_s log_CTRL;
 			struct log_RC_s log_RC;
 			struct log_OUT0_s log_OUT0;
 			struct log_AIRS_s log_AIRS;
@@ -716,6 +726,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- ACTUATOR ARMED --- */
+	subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	fds[fdsc_count].fd = subs.armed_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* --- VEHICLE STATUS --- */
 	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	fds[fdsc_count].fd = subs.status_sub;
@@ -800,6 +816,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- CONTROL DEBUG --- */
+	subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug));
+	fds[fdsc_count].fd = subs.control_debug_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* --- OPTICAL FLOW --- */
 	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	fds[fdsc_count].fd = subs.flow_sub;
@@ -883,22 +905,33 @@ int sdlog2_thread_main(int argc, char *argv[])
 				handled_topics++;
 			}
 
-			/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
+			/* --- ARMED- LOG MANAGEMENT --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
+				orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed);
 
 				if (log_when_armed) {
-					handle_status(&buf_status);
+					handle_status(&buf_armed);
 				}
 
 				handled_topics++;
 			}
 
+			/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
+
+				//if (log_when_armed) {
+				//	handle_status(&buf_armed);
+				//}
+
+				//handled_topics++;
+			}
+
 			if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
 				continue;
 			}
 
-			ifds = 1;	// Begin from fds[1] again
+			ifds = 2;	// Begin from fds[2] again
 
 			pthread_mutex_lock(&logbuffer_mutex);
 
@@ -911,13 +944,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (fds[ifds++].revents & POLLIN) {
 				// Don't orb_copy, it's already done few lines above
 				log_msg.msg_type = LOG_STAT_MSG;
-				log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
-				log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
-				log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
-				log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
-				log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed;
-				log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
-				log_msg.body.log_STAT.battery_current = buf_status.current_battery;
+				// XXX fix this
+				// log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
+				// log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
+				// log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
+				// log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
+				log_msg.body.log_STAT.state = 0;
+				log_msg.body.log_STAT.flight_mode = 0;
+				log_msg.body.log_STAT.manual_control_mode = 0;
+				log_msg.body.log_STAT.manual_sas_mode = 0;
+				log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */
+				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
+				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 				log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
 				LOGBUFFER_WRITE_AND_COUNT(STAT);
@@ -1006,6 +1044,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
 				log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
 				log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
+				log_msg.body.log_ATT.roll_acc = buf.att.rollacc;
+				log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc;
+				log_msg.body.log_ATT.yaw_acc = buf.att.yawacc;
 				LOGBUFFER_WRITE_AND_COUNT(ATT);
 			}
 
@@ -1121,6 +1162,27 @@ int sdlog2_thread_main(int argc, char *argv[])
 				// TODO not implemented yet
 			}
 
+			/* --- CONTROL DEBUG --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug);
+
+				log_msg.msg_type = LOG_CTRL_MSG;
+				log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p;
+				log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i;
+				log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d;
+				log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p;
+				log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i;
+				log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d;
+				log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p;
+				log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i;
+				log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d;
+				log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p;
+				log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i;
+				log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d;
+
+				LOGBUFFER_WRITE_AND_COUNT(CTRL);
+			}
+
 			/* --- FLOW --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
@@ -1295,10 +1357,10 @@ void handle_command(struct vehicle_command_s *cmd)
 	}
 }
 
-void handle_status(struct vehicle_status_s *status)
+void handle_status(struct actuator_armed_s *armed)
 {
-	if (status->flag_system_armed != flag_system_armed) {
-		flag_system_armed = status->flag_system_armed;
+	if (armed->armed != flag_system_armed) {
+		flag_system_armed = armed->armed;
 
 		if (flag_system_armed) {
 			sdlog2_start_log();
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 934e4dec8..9be96a62e 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -63,6 +63,9 @@ struct log_ATT_s {
 	float roll_rate;
 	float pitch_rate;
 	float yaw_rate;
+	float roll_acc;
+	float pitch_acc;
+	float yaw_acc;
 };
 
 /* --- ATSP - ATTITUDE SET POINT --- */
@@ -160,27 +163,44 @@ struct log_STAT_s {
 	uint8_t battery_warning;
 };
 
+/* --- CTRL - CONTROL DEBUG --- */
+#define LOG_CTRL_MSG 11
+struct log_CTRL_s {
+	float roll_p;
+	float roll_i;
+	float roll_d;
+	float roll_rate_p;
+	float roll_rate_i;
+	float roll_rate_d;
+	float pitch_p;
+	float pitch_i;
+	float pitch_d;
+	float pitch_rate_p;
+	float pitch_rate_i;
+	float pitch_rate_d;
+};
+
 /* --- RC - RC INPUT CHANNELS --- */
-#define LOG_RC_MSG 11
+#define LOG_RC_MSG 12
 struct log_RC_s {
 	float channel[8];
 };
 
 /* --- OUT0 - ACTUATOR_0 OUTPUT --- */
-#define LOG_OUT0_MSG 12
+#define LOG_OUT0_MSG 13
 struct log_OUT0_s {
 	float output[8];
 };
 
 /* --- AIRS - AIRSPEED --- */
-#define LOG_AIRS_MSG 13
+#define LOG_AIRS_MSG 14
 struct log_AIRS_s {
 	float indicated_airspeed;
 	float true_airspeed;
 };
 
 /* --- ARSP - ATTITUDE RATE SET POINT --- */
-#define LOG_ARSP_MSG 14
+#define LOG_ARSP_MSG 15
 struct log_ARSP_s {
 	float roll_rate_sp;
 	float pitch_rate_sp;
@@ -188,7 +208,7 @@ struct log_ARSP_s {
 };
 
 /* --- FLOW - OPTICAL FLOW --- */
-#define LOG_FLOW_MSG 15
+#define LOG_FLOW_MSG 16
 struct log_FLOW_s {
 	int16_t flow_raw_x;
 	int16_t flow_raw_y;
@@ -250,7 +270,7 @@ struct log_ESC_s {
 
 static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TIME, "Q", "StartTime"),
-	LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
+	LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"),
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
@@ -259,6 +279,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
+	LOG_FORMAT(CTRL, "ffffffffffff", "RP,RI,RD,RRP,RRI,RRD,PP,PI,PD,PRP,PRI,PRD"),
 	LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index ba4d2186c..a5fc34184 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -156,6 +156,7 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
 PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
+PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
@@ -169,13 +170,12 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 
-PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
+PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
-PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f7b41b120..22374a1fe 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -74,7 +74,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
@@ -191,7 +191,7 @@ private:
 	int		_baro_sub;			/**< raw baro data subscription */
 	int     _airspeed_sub;		/**< airspeed subscription */
 	int		_diff_pres_sub;		/**< raw differential pressure subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
 
@@ -234,13 +234,12 @@ private:
 		int rc_map_yaw;
 		int rc_map_throttle;
 
-		int rc_map_manual_override_sw;
-		int rc_map_auto_mode_sw;
+		int rc_map_mode_sw;
+		int rc_map_return_sw;
+		int rc_map_assisted_sw;
+		int rc_map_mission_sw;
 
-		int rc_map_manual_mode_sw;
-		int rc_map_sas_mode_sw;
-		int rc_map_rtl_sw;
-		int rc_map_offboard_ctrl_mode_sw;
+//		int rc_map_offboard_ctrl_mode_sw;
 
 		int rc_map_flaps;
 
@@ -257,8 +256,6 @@ private:
 
 		float battery_voltage_scaling;
 
-		int   rc_rl1_DSM_VCC_control;
-
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -285,13 +282,12 @@ private:
 		param_t rc_map_yaw;
 		param_t rc_map_throttle;
 
-		param_t rc_map_manual_override_sw;
-		param_t rc_map_auto_mode_sw;
+		param_t rc_map_mode_sw;
+		param_t rc_map_return_sw;
+		param_t rc_map_assisted_sw;
+		param_t rc_map_mission_sw;
 
-		param_t rc_map_manual_mode_sw;
-		param_t rc_map_sas_mode_sw;
-		param_t rc_map_rtl_sw;
-		param_t rc_map_offboard_ctrl_mode_sw;
+//		param_t rc_map_offboard_ctrl_mode_sw;
 
 		param_t rc_map_flaps;
 
@@ -308,8 +304,6 @@ private:
 
 		param_t battery_voltage_scaling;
 
-		param_t rc_rl1_DSM_VCC_control;
-
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -384,9 +378,9 @@ private:
 	void		diff_pres_poll(struct sensor_combined_s &raw);
 
 	/**
-	 * Check for changes in vehicle status.
+	 * Check for changes in vehicle control mode.
 	 */
-	void		vehicle_status_poll();
+	void		vehicle_control_mode_poll();
 
 	/**
 	 * Check for changes in parameters.
@@ -441,7 +435,7 @@ Sensors::Sensors() :
 	_mag_sub(-1),
 	_rc_sub(-1),
 	_baro_sub(-1),
-	_vstatus_sub(-1),
+	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
 
@@ -492,16 +486,16 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
 
 	/* mandatory mode switches, mapped to channel 5 and 6 per default */
-	_parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW");
-	_parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW");
+	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
+	_parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
 
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
 
 	/* optional mode switches, not mapped per default */
-	_parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW");
-	_parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW");
-	_parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW");
-	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
+	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
+	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+
+//	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -544,9 +538,6 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
-	/* DSM VCC relay control */
-	_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
-
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -630,33 +621,29 @@ Sensors::parameters_update()
 		warnx("Failed getting throttle chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) {
-		warnx("Failed getting override sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) {
-		warnx("Failed getting auto mode sw chan index");
+	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
+		warnx("Failed getting mode sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx("Failed getting flaps chan index");
+	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
+		warnx("Failed getting return sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) {
-		warnx("Failed getting manual mode sw chan index");
+	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
+		warnx("Failed getting assisted sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
-		warnx("Failed getting rtl sw chan index");
+	if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
+		warnx("Failed getting mission sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
-		warnx("Failed getting sas mode sw chan index");
+	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
+		warnx("Failed getting flaps chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
-		warnx("Failed getting offboard control mode sw chan index");
-	}
+//	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
+//		warnx("Failed getting offboard control mode sw chan index");
+//	}
 
 	if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
 		warnx("Failed getting mode aux 1 index");
@@ -689,15 +676,14 @@ Sensors::parameters_update()
 	_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
 	_rc.function[YAW] = _parameters.rc_map_yaw - 1;
 
-	_rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1;
-	_rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1;
+	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
+	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
+	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
+	_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
-	_rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1;
-	_rc.function[RTL] = _parameters.rc_map_rtl_sw - 1;
-	_rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1;
-	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
+//	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
 
 	_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
 	_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
@@ -738,11 +724,6 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
-	/* relay 1 DSM VCC control */
-	if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
-		warnx("Failed updating relay 1 DSM VCC control");
-	}
-
 	return OK;
 }
 
@@ -989,21 +970,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 }
 
 void
-Sensors::vehicle_status_poll()
+Sensors::vehicle_control_mode_poll()
 {
-	struct vehicle_status_s vstatus;
-	bool vstatus_updated;
+	struct vehicle_control_mode_s vcontrol_mode;
+	bool vcontrol_mode_updated;
 
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_vstatus_sub, &vstatus_updated);
+	/* Check HIL state if vehicle control mode has changed */
+	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
-	if (vstatus_updated) {
+	if (vcontrol_mode_updated) {
 
-		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus);
+		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
 
 		/* switching from non-HIL to HIL mode */
 		//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
-		if (vstatus.flag_hil_enabled && !_hil_enabled) {
+		if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
 			_hil_enabled = true;
 			_publishing = false;
 
@@ -1206,10 +1187,11 @@ Sensors::ppm_poll()
 		manual_control.yaw = NAN;
 		manual_control.throttle = NAN;
 
-		manual_control.manual_mode_switch = NAN;
-		manual_control.manual_sas_switch = NAN;
-		manual_control.return_to_launch_switch = NAN;
-		manual_control.auto_offboard_input_switch = NAN;
+		manual_control.mode_switch = NAN;
+		manual_control.return_switch = NAN;
+		manual_control.assisted_switch = NAN;
+		manual_control.mission_switch = NAN;
+//		manual_control.auto_offboard_input_switch = NAN;
 
 		manual_control.flaps = NAN;
 		manual_control.aux1 = NAN;
@@ -1309,11 +1291,17 @@ Sensors::ppm_poll()
 			manual_control.yaw *= _parameters.rc_scale_yaw;
 		}
 
-		/* override switch input */
-		manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
-
 		/* mode switch input */
-		manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled);
+		manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
+
+		/* land switch input */
+		manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
+
+		/* assisted switch input */
+		manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+
+		/* mission switch input */
+		manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 
 		/* flaps */
 		if (_rc.function[FLAPS] >= 0) {
@@ -1325,21 +1313,17 @@ Sensors::ppm_poll()
 			}
 		}
 
-		if (_rc.function[MANUAL_MODE] >= 0) {
-			manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled);
+		if (_rc.function[MODE] >= 0) {
+			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
 		}
 
-		if (_rc.function[SAS_MODE] >= 0) {
-			manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled);
+		if (_rc.function[MISSION] >= 0) {
+			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
-		if (_rc.function[RTL] >= 0) {
-			manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled);
-		}
-
-		if (_rc.function[OFFBOARD_MODE] >= 0) {
-			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
-		}
+//		if (_rc.function[OFFBOARD_MODE] >= 0) {
+//			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
+//		}
 
 		/* aux functions, only assign if valid mapping is present */
 		if (_rc.function[AUX_1] >= 0) {
@@ -1412,12 +1396,12 @@ Sensors::task_main()
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	/* rate limit vehicle status updates to 5Hz */
-	orb_set_interval(_vstatus_sub, 200);
+	orb_set_interval(_vcontrol_mode_sub, 200);
 
 	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
 	orb_set_interval(_gyro_sub, 4);
@@ -1473,7 +1457,7 @@ Sensors::task_main()
 		perf_begin(_loop_perf);
 
 		/* check vehicle status for changes to publication state */
-		vehicle_status_poll();
+		vehicle_control_mode_poll();
 
 		/* check parameters for updates */
 		parameter_update_poll();
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 4996a8f66..9308100b0 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
  * @param dt
  * @return
  */
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d)
 {
 	/*  error = setpoint - actual_position
 	 integral = integral + (error*dt)
@@ -196,6 +196,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		pid->last_output = output;
 	}
 
+	*ctrl_p = (error * pid->kp);
+	*ctrl_i = (i * pid->ki);
+	*ctrl_d = (d * pid->kd);
+
 	return pid->last_output;
 }
 
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index eca228464..5f2650f69 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -85,7 +85,7 @@ typedef struct {
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
 //void pid_set(PID_t *pid, float sp);
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d);
 
 __EXPORT void pid_reset_integral(PID_t *pid);
 
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index ae5fc6c61..ed77bb7ef 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -77,6 +77,9 @@ ORB_DEFINE(home_position, struct home_position_s);
 #include "topics/vehicle_status.h"
 ORB_DEFINE(vehicle_status, struct vehicle_status_s);
 
+#include "topics/safety.h"
+ORB_DEFINE(safety, struct safety_s);
+
 #include "topics/battery_status.h"
 ORB_DEFINE(battery_status, struct battery_status_s);
 
@@ -98,6 +101,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s);
 #include "topics/vehicle_command.h"
 ORB_DEFINE(vehicle_command, struct vehicle_command_s);
 
+#include "topics/vehicle_control_mode.h"
+ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s);
+
 #include "topics/vehicle_local_position_setpoint.h"
 ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
 
@@ -119,6 +125,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 
+#include "topics/vehicle_control_debug.h"
+ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
+
 #include "topics/offboard_control_setpoint.h"
 ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
 
@@ -146,6 +155,8 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+
+#include "topics/actuator_armed.h"
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
 /* actuator controls, as set by actuators / mixers after limiting */
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
new file mode 100644
index 000000000..6e944ffee
--- /dev/null
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   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 actuator_armed.h
+ *
+ * Actuator armed topic
+ *
+ */
+
+#ifndef TOPIC_ACTUATOR_ARMED_H
+#define TOPIC_ACTUATOR_ARMED_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/** global 'actuator output is live' control. */
+struct actuator_armed_s {
+
+	uint64_t	timestamp;
+	bool	armed;		/**< Set to true if system is armed */
+	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
+	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+};
+
+ORB_DECLARE(actuator_armed);
+
+#endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index b7c4196c0..26b967237 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -52,6 +52,9 @@
 #define NUM_ACTUATOR_CONTROLS		8
 #define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
 
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
+
 struct actuator_controls_s {
 	uint64_t timestamp;
 	float	control[NUM_ACTUATOR_CONTROLS];
@@ -63,16 +66,4 @@ ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
 
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
-
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
-	bool	armed;		/**< Set to true if system is armed */
-	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
-	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
-};
-
-ORB_DECLARE(actuator_armed);
-
 #endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 261a8a4ad..eac6d6e98 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -56,17 +56,18 @@ struct manual_control_setpoint_s {
 	float yaw;				/**< rudder / yaw rate / yaw */
 	float throttle;				/**< throttle / collective thrust / altitude */
 
-	float manual_override_switch;		/**< manual override mode (mandatory) */
-	float auto_mode_switch;			/**< auto mode switch (mandatory) */
+	float mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
+	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
+	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
+	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
 
 	/**
 	 * Any of the channels below may not be available and be set to NaN
 	 * to indicate that it does not contain valid data.
 	 */
-	float manual_mode_switch;		/**< manual mode (man, sas, alt) switch (optional) */
-	float manual_sas_switch;		/**< sas mode (rates / attitude) switch (optional) */
-	float return_to_launch_switch;		/**< return to launch switch (0 = disabled, 1 = enabled) */
-	float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
+
+	// XXX needed or parameter?
+	//float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
 
 	float flaps;				/**< flap position */
 
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 9dd54df91..2167e44a2 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -53,9 +53,12 @@
 /**
  * The number of RC channel inputs supported.
  * Current (Q1/2013) radios support up to 18 channels,
- * leaving at a sane value of 14.
+ * leaving at a sane value of 15.
+ * This number can be greater then number of RC channels,
+ * because single RC channel can be mapped to multiple
+ * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAX   14
+#define RC_CHANNELS_MAX   15
 
 /** 
  * This defines the mapping of the RC functions.
@@ -68,18 +71,17 @@ enum RC_CHANNELS_FUNCTION
   ROLL     = 1,
   PITCH    = 2,
   YAW      = 3,
-  OVERRIDE = 4,
-  AUTO_MODE = 5,
-  MANUAL_MODE = 6,
-  SAS_MODE = 7,
-  RTL = 8,
-  OFFBOARD_MODE = 9,
-  FLAPS   = 10,
-  AUX_1   = 11,
-  AUX_2   = 12,
-  AUX_3   = 13,
-  AUX_4   = 14,
-  AUX_5   = 15,
+  MODE = 4,
+  RETURN = 5,
+  ASSISTED = 6,
+  MISSION = 7,
+  OFFBOARD_MODE = 8,
+  FLAPS   = 9,
+  AUX_1   = 10,
+  AUX_2   = 11,
+  AUX_3   = 12,
+  AUX_4   = 13,
+  AUX_5   = 14,
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h
new file mode 100644
index 000000000..a5d21cd4a
--- /dev/null
+++ b/src/modules/uORB/topics/safety.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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 safety.h
+ *
+ * Safety topic to pass safety state from px4io driver to commander
+ * This concerns only the safety button of the px4io but has nothing to do
+ * with arming/disarming.
+ */
+
+#ifndef TOPIC_SAFETY_H
+#define TOPIC_SAFETY_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+struct safety_s {
+
+	uint64_t	timestamp;
+	bool	safety_switch_available;	/**< Set to true if a safety switch is connected */
+	bool	safety_off;			/**< Set to true if safety is off */
+};
+
+ORB_DECLARE(safety);
+
+#endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h
new file mode 100644
index 000000000..6184284a4
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_debug.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @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 vehicle_control_debug.h
+ * For debugging purposes to log PID parts of controller
+ */
+
+#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_
+#define TOPIC_VEHICLE_CONTROL_DEBUG_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+struct vehicle_control_debug_s
+{
+	uint64_t timestamp; /**< in microseconds since system start */
+
+	float roll_p;		/**< roll P control part		*/
+	float roll_i;		/**< roll I control part 		*/
+	float roll_d;		/**< roll D control part 		*/
+
+	float roll_rate_p;	/**< roll rate P control part		*/
+	float roll_rate_i;	/**< roll rate I control part 		*/
+	float roll_rate_d;	/**< roll rate D control part 		*/
+
+	float pitch_p;		/**< pitch P control part		*/
+	float pitch_i;		/**< pitch I control part 		*/
+	float pitch_d;		/**< pitch D control part 		*/
+
+	float pitch_rate_p;	/**< pitch rate P control part		*/
+	float pitch_rate_i;	/**< pitch rate I control part 		*/
+	float pitch_rate_d;	/**< pitch rate D control part 		*/
+
+	float yaw_p;		/**< yaw P control part			*/
+	float yaw_i;		/**< yaw I control part 		*/
+	float yaw_d;		/**< yaw D control part 		*/
+
+	float yaw_rate_p;	/**< yaw rate P control part		*/
+	float yaw_rate_i;	/**< yaw rate I control part 		*/
+	float yaw_rate_d;	/**< yaw rate D control part 		*/
+
+}; /**< vehicle_control_debug */
+
+ /**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_debug);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
new file mode 100644
index 000000000..d83eb45d9
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author 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 vehicle_control_mode.h
+ * Definition of the vehicle_control_mode uORB topic.
+ * 
+ * All control apps should depend their actions based on the flags set here.
+ */
+
+#ifndef VEHICLE_CONTROL_MODE
+#define VEHICLE_CONTROL_MODE
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+
+/**
+ * state machine / state of vehicle.
+ *
+ * Encodes the complete system state and is set by the commander app.
+ */
+struct vehicle_control_mode_s
+{
+	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
+	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
+
+	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
+
+	// XXX needs yet to be set by state machine helper
+	bool flag_system_hil_enabled;
+
+	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
+	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
+	// XXX shouldn't be necessairy
+	//bool flag_auto_enabled;
+	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
+	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
+	bool flag_control_velocity_enabled;		/**< true if speed (implies direction) is controlled */
+	bool flag_control_position_enabled;		/**< true if position is controlled */
+	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
+
+	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
+	bool failsave_highlevel;			/**< Set to true if high-level failsafe mode is enabled */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_mode);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index c7c1048f6..e7feaa98e 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -58,22 +58,64 @@
  * @addtogroup topics @{
  */
 
-/* State Machine */
-typedef enum
-{
-	SYSTEM_STATE_PREFLIGHT = 0,
-	SYSTEM_STATE_STANDBY = 1,
-	SYSTEM_STATE_GROUND_READY = 2,
-	SYSTEM_STATE_MANUAL = 3,
-	SYSTEM_STATE_STABILIZED = 4,
-	SYSTEM_STATE_AUTO = 5,
-	SYSTEM_STATE_MISSION_ABORT = 6,
-	SYSTEM_STATE_EMCY_LANDING = 7,
-	SYSTEM_STATE_EMCY_CUTOFF = 8,
-	SYSTEM_STATE_GROUND_ERROR = 9,
-	SYSTEM_STATE_REBOOT= 10,
-
-} commander_state_machine_t;
+/* main state machine */
+typedef enum {
+	MAIN_STATE_MANUAL = 0,
+	MAIN_STATE_SEATBELT,
+	MAIN_STATE_EASY,
+	MAIN_STATE_AUTO,
+} main_state_t;
+
+/* navigation state machine */
+typedef enum {
+	NAVIGATION_STATE_STANDBY = 0,		// standby state, disarmed
+	NAVIGATION_STATE_DIRECT,		// true manual control, no any stabilization
+	NAVIGATION_STATE_STABILIZE,		// attitude stabilization
+	NAVIGATION_STATE_ALTHOLD,		// attitude + altitude stabilization
+	NAVIGATION_STATE_VECTOR,		// attitude + altitude + position stabilization
+	NAVIGATION_STATE_AUTO_READY,	// AUTO, landed, reeady for takeoff
+	NAVIGATION_STATE_AUTO_TAKEOFF,	// detect takeoff using land detector and switch to desired AUTO mode
+	NAVIGATION_STATE_AUTO_LOITER,	// pause mission
+	NAVIGATION_STATE_AUTO_MISSION,	// fly mission
+	NAVIGATION_STATE_AUTO_RTL,		// Return To Launch, when home position switch to LAND
+	NAVIGATION_STATE_AUTO_LAND		// land and switch to AUTO_READY when landed (detect using land detector)
+} navigation_state_t;
+
+typedef enum {
+	ARMING_STATE_INIT = 0,
+	ARMING_STATE_STANDBY,
+	ARMING_STATE_ARMED,
+	ARMING_STATE_ARMED_ERROR,
+	ARMING_STATE_STANDBY_ERROR,
+	ARMING_STATE_REBOOT,
+	ARMING_STATE_IN_AIR_RESTORE
+} arming_state_t;
+
+typedef enum {
+	HIL_STATE_OFF = 0,
+	HIL_STATE_ON
+} hil_state_t;
+
+typedef enum {
+	MODE_SWITCH_MANUAL = 0,
+	MODE_SWITCH_ASSISTED,
+	MODE_SWITCH_AUTO
+} mode_switch_pos_t;
+
+typedef enum {
+	ASSISTED_SWITCH_SEATBELT = 0,
+	ASSISTED_SWITCH_EASY
+} assisted_switch_pos_t;
+
+typedef enum {
+	RETURN_SWITCH_NONE = 0,
+	RETURN_SWITCH_RETURN
+} return_switch_pos_t;
+
+typedef enum {
+	MISSION_SWITCH_NONE = 0,
+	MISSION_SWITCH_MISSION
+} mission_switch_pos_t;
 
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -86,26 +128,6 @@ enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
 
-enum VEHICLE_FLIGHT_MODE {
-	VEHICLE_FLIGHT_MODE_MANUAL = 0,		/**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
-	VEHICLE_FLIGHT_MODE_STAB,		/**< attitude or rate stabilization plus velocity or position stabilization */
-	VEHICLE_FLIGHT_MODE_HOLD,		/**< hold current position (hover or loiter around position when switched) */
-	VEHICLE_FLIGHT_MODE_AUTO		/**< attitude or rate stabilization plus absolute position control and waypoints */
-};
-
-enum VEHICLE_MANUAL_CONTROL_MODE {
-	VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0,	/**< no attitude control, direct stick input mixing (only fixed wing) */
-	VEHICLE_MANUAL_CONTROL_MODE_RATES,	/**< body rates control mode */
-	VEHICLE_MANUAL_CONTROL_MODE_SAS		/**< stability augmented system (SAS) mode */
-};
-
-enum VEHICLE_MANUAL_SAS_MODE {
-	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0,	/**< roll, pitch and yaw absolute */
-	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE,	/**< roll and pitch absolute, yaw rate */
-	VEHICLE_MANUAL_SAS_MODE_SIMPLE,				/**< simple mode (includes altitude hold) */
-	VEHICLE_MANUAL_SAS_MODE_ALTITUDE			/**< altitude hold */
-};
-
 /**
  * Should match 1:1 MAVLink's MAV_TYPE ENUM
  */
@@ -134,7 +156,7 @@ enum VEHICLE_TYPE {
 enum VEHICLE_BATTERY_WARNING {
     VEHICLE_BATTERY_WARNING_NONE = 0,    /**< no battery low voltage warning active */
     VEHICLE_BATTERY_WARNING_WARNING,        /**< warning of low voltage 1. stage */
-    VEHICLE_BATTERY_WARNING_ALERT            /**< aleting of low voltage 2. stage */
+    VEHICLE_BATTERY_WARNING_ALERT            /**< alerting of low voltage 2. stage */
 };
 
 
@@ -150,32 +172,35 @@ struct vehicle_status_s
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 	uint64_t failsave_lowlevel_start_time;		/**< time when the lowlevel failsafe flag was set */
-	//uint64_t failsave_highlevel_begin; TO BE COMPLETED
+//	uint64_t failsave_highlevel_begin; TO BE COMPLETED
+
+	main_state_t main_state;				/**< main state machine */
+	navigation_state_t navigation_state;	/**< navigation state machine */
+	arming_state_t arming_state;			/**< current arming state */
+	hil_state_t hil_state;					/**< current hil state */
 
-	commander_state_machine_t state_machine;	/**< current flight state, main state machine */
-	enum VEHICLE_FLIGHT_MODE flight_mode;		/**< current flight mode, as defined by mode switch */
-	enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode;	/**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
-	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;	/**< current stabilization mode */
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
 	int32_t component_id;				/**< subsystem / component id, inspired by MAVLink's component ID field */
 
-	/* system flags - these represent the state predicates */
-
-	bool flag_system_armed;				/**< true is motors / actuators are armed */
-	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
-	bool flag_hil_enabled;				/**< true if hardware in the loop simulation is enabled */
-
-	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
-	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
-	bool flag_control_velocity_enabled;		/**< true if speed (implies direction) is controlled */
-	bool flag_control_position_enabled;		/**< true if position is controlled */
-
-	bool flag_preflight_gyro_calibration;		/**< true if gyro calibration is requested */
-	bool flag_preflight_mag_calibration;		/**< true if mag calibration is requested */
-	bool flag_preflight_accel_calibration;
-	bool flag_preflight_airspeed_calibration;
+	bool is_rotary_wing;
+
+	mode_switch_pos_t mode_switch;
+	return_switch_pos_t return_switch;
+	assisted_switch_pos_t assisted_switch;
+	mission_switch_pos_t mission_switch;
+
+	bool condition_battery_voltage_valid;
+	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
+	bool condition_system_sensors_initialized;
+	bool condition_system_returned_to_home;
+	bool condition_auto_mission_available;
+	bool condition_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+	bool condition_launch_position_valid;		/**< indicates a valid launch position */
+	bool condition_home_position_valid;		/**< indicates a valid home position (a valid home position is not always a valid launch) */
+	bool condition_local_position_valid;
+	bool condition_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
+	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception is terminally lost */
@@ -188,16 +213,22 @@ struct vehicle_status_s
 	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
 
 	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
-	//bool failsave_highlevel;
+	bool failsave_highlevel;
+
+	bool preflight_calibration;
+
+	bool system_emergency;
 
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;
 	uint32_t onboard_control_sensors_enabled;
 	uint32_t onboard_control_sensors_health;
+	
 	float load;
-	float voltage_battery;
-	float current_battery;
+	float battery_voltage;
+	float battery_current;
 	float battery_remaining;
+
 	enum VEHICLE_BATTERY_WARNING battery_warning;    /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
 	uint16_t drop_rate_comm;
 	uint16_t errors_comm;
@@ -205,15 +236,6 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
-
-	bool flag_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
-	bool flag_local_position_valid;
-	bool flag_vector_flight_mode_ok;		/**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
-	bool flag_auto_flight_mode_ok;			/**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
-	bool flag_external_manual_override_ok;		/**< external override non-fatal for system. Only true for fixed wing */
-	bool flag_valid_launch_position;		/**< indicates a valid launch position */
-	bool flag_valid_home_position;			/**< indicates a valid home position (a valid home position is not always a valid launch) */
-	bool flag_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
 };
 
 /**
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
new file mode 100644
index 000000000..188fa4e37
--- /dev/null
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -0,0 +1,250 @@
+/****************************************************************************
+ *
+ *   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 esc_calib.c
+ *
+ * Tool for ESC calibration
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+#include "drivers/drv_pwm_output.h"
+
+static void	usage(const char *reason);
+__EXPORT int	esc_calib_main(int argc, char *argv[]);
+
+#define MAX_CHANNELS 8
+
+static void
+usage(const char *reason)
+{
+	if (reason != NULL)
+		warnx("%s", reason);
+	errx(1, 
+		"usage:\n"
+		"esc_calib [-d <device>] <channels>\n"
+		"  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"  <channels>         Provide channels (e.g.: 1 2 3 4)\n"
+		);
+
+}
+
+int
+esc_calib_main(int argc, char *argv[])
+{
+	const char *dev = PWM_OUTPUT_DEVICE_PATH;
+	char *ep;
+	bool channels_selected[MAX_CHANNELS] = {false};
+	int ch;
+	int ret;
+	char c;
+
+	if (argc < 2)
+		usage(NULL);
+
+	while ((ch = getopt(argc, argv, "d:")) != EOF) {
+		switch (ch) {
+		
+		case 'd':
+			dev = optarg;
+			argc--;
+			break;
+
+		default:
+			usage(NULL);
+		}
+	}
+
+	if(argc < 1) {
+		usage("no channels provided");
+	}
+
+	while (--argc) {
+		const char *arg = argv[argc];
+		unsigned channel_number = strtol(arg, &ep, 0);
+
+		if (*ep == '\0') {
+			if (channel_number > MAX_CHANNELS || channel_number <= 0) {
+				err(1, "invalid channel number: %d", channel_number);
+			} else {
+				channels_selected[channel_number-1] = true;
+
+			}
+		}
+	}
+
+	/* Wait for confirmation */
+	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+	if (!console)
+		err(1, "failed opening console");
+
+	warnx("ATTENTION, please remove or fix props before starting calibration!\n"
+		"\n"
+		"Also press the arming switch first for safety off\n"
+		"\n"
+		"Do you really want to start calibration: y or n?\n");
+
+	/* wait for user input */
+	while (1) {
+		
+		if (read(console, &c, 1) == 1) {
+			if (c == 'y' || c == 'Y') {
+				
+				break;
+			} else if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("ESC calibration exited");
+				close(console);
+				exit(0);
+			} else if (c == 'n' || c == 'N') {
+				warnx("ESC calibration aborted");
+				close(console);
+				exit(0);
+			} else {
+				warnx("Unknown input, ESC calibration aborted");
+				close(console);
+				exit(0);
+			} 
+		}
+		/* rate limit to ~ 20 Hz */
+		usleep(50000);
+	}
+
+	/* open for ioctl only */
+	int fd = open(dev, 0);
+	if (fd < 0)
+		err(1, "can't open %s", dev);
+
+	// XXX arming not necessaire at the moment
+	// /* Then arm */
+	// ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	// 	if (ret != OK)
+	// 		err(1, "PWM_SERVO_SET_ARM_OK");
+
+	// ret = ioctl(fd, PWM_SERVO_ARM, 0);
+	// 	if (ret != OK)
+	// 		err(1, "PWM_SERVO_ARM");
+
+
+	
+
+	/* Wait for user confirmation */
+	warnx("Set high PWM\n"
+	      "Connect battery now and hit ENTER after the ESCs confirm the first calibration step");
+
+	while (1) {
+
+		/* First set high PWM */
+		for (unsigned i = 0; i<MAX_CHANNELS; i++) {
+			if(channels_selected[i]) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), 2100);
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d)", i);
+			}
+		}
+
+		if (read(console, &c, 1) == 1) {
+			if (c == 13) {
+				
+				break;
+			} else if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("ESC calibration exited");
+				close(console);
+				exit(0);
+			}
+		}
+		/* rate limit to ~ 20 Hz */
+		usleep(50000);
+	}
+
+	/* we don't need any more user input */
+	
+
+	warnx("Set low PWM, hit ENTER when finished");
+
+	while (1) {
+
+		/* Then set low PWM */
+		for (unsigned i = 0; i<MAX_CHANNELS; i++) {
+			if(channels_selected[i]) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), 900);
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d)", i);
+			}
+		}
+
+		if (read(console, &c, 1) == 1) {
+			if (c == 13) {
+				
+				break;
+			} else if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("ESC calibration exited");
+				close(console);
+				exit(0);
+			}
+		}
+		/* rate limit to ~ 20 Hz */
+		usleep(50000);
+	}
+
+	
+	warnx("ESC calibration finished");
+	close(console);
+
+	// XXX disarming not necessaire at the moment
+	/* Now disarm again */
+	// ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+
+	
+
+	exit(0);
+}
\ No newline at end of file
diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk
new file mode 100644
index 000000000..990c56768
--- /dev/null
+++ b/src/systemcmds/esc_calib/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build the esc_calib tool.
+#
+
+MODULE_COMMAND	 = esc_calib
+SRCS		 = esc_calib.c
+
+MODULE_STACKSIZE = 4096
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index e150b5a74..c42a36c7f 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[])
 	}
 
 	/* iterate remaining arguments */
-	unsigned channel = 0;
+	unsigned nchannels = 0;
+	unsigned channel[8] = {0};
 	while (argc--) {
 		const char *arg = argv[0];
 		argv++;
@@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[])
 		}
 		unsigned pwm_value = strtol(arg, &ep, 0);
 		if (*ep == '\0') {
-			ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
-			if (ret != OK)
-				err(1, "PWM_SERVO_SET(%d)", channel);
-			channel++;
+			if (nchannels > sizeof(channel) / sizeof(channel[0]))
+				err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
+
+			channel[nchannels] = pwm_value;
+			nchannels++;
+
 			continue;
 		}
-		usage("unrecognised option");
+		usage("unrecognized option");
 	}
 
 	/* print verbose info */
@@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[])
 		}
 		fflush(stdout);
 	}
+
+	/* perform PWM output */
+	if (nchannels) {
+
+		/* Open console directly to grab CTRL-C signal */
+		int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+		if (!console)
+			err(1, "failed opening console");
+
+		warnx("Press CTRL-C or 'c' to abort.");
+
+		while (1) {
+			for (int i = 0; i < nchannels; i++) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d)", i);
+			}
+
+			/* abort on user request */
+			char c;
+			if (read(console, &c, 1) == 1) {
+				if (c == 0x03 || c == 0x63 || c == 'q') {
+					warnx("User abort\n");
+					close(console);
+					exit(0);
+				}
+			}
+
+			/* rate limit to ~ 20 Hz */
+			usleep(50000);
+		}
+	}
+
 	exit(0);
 }
\ No newline at end of file