Massive improvements in state machine, still tracing wrong throttle scaling in manual input path

10 files changed, 268 insertions, 167 deletions

diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile
index 23e2d49fb..2038e982b 100644
--- a/apps/ardrone_control/Makefile
+++ b/apps/ardrone_control/Makefile
@@ -37,7 +37,7 @@
 
 APPNAME		 = ardrone_control
 PRIORITY	 = SCHED_PRIORITY_MAX - 15
-STACKSIZE	 = 2048
+STACKSIZE	 = 3096
 
 # explicit list of sources - not everything is built currently
 CSRCS		 = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c
diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c
index e9a48bbfe..d5c31dc70 100644
--- a/apps/ardrone_control/ardrone_control.c
+++ b/apps/ardrone_control/ardrone_control.c
@@ -187,6 +187,9 @@ int ardrone_control_main(int argc, char *argv[])
 
 	while (1) {
 
+		/* get a local copy of the vehicle state */
+		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+
 		if (state.state_machine == SYSTEM_STATE_MANUAL ||
 			state.state_machine == SYSTEM_STATE_GROUND_READY ||
 			state.state_machine == SYSTEM_STATE_STABILIZED ||
@@ -205,8 +208,6 @@ int ardrone_control_main(int argc, char *argv[])
 				/* hardcore, last-resort safety checking */
 				//if (status->rc_signal_lost) {
 
-				/* get a local copy of the vehicle state */
-				orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 				/* get a local copy of manual setpoint */
 				orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
 				/* get a local copy of attitude */
@@ -217,8 +218,12 @@ int ardrone_control_main(int argc, char *argv[])
 				att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
 				att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
 				att_sp.yaw_body = -manual.yaw * M_PI_F;
+				att_sp.thrust = manual.throttle/2.0f;
 
 				control_attitude(ardrone_write, &att_sp, &att, &state);
+			} else {
+				/* invalid mode, complain */
+				if (counter % 200 == 0) printf("[multirotor control] INVALID CONTROL MODE, locking down propulsion\n");
 			}
 		}
 
diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c
index 034a9c5a6..37edbf0e5 100644
--- a/apps/ardrone_control/attitude_control.c
+++ b/apps/ardrone_control/attitude_control.c
@@ -83,7 +83,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 	static float pid_yawspeed_lim;
 	static float pid_att_lim;
 
-	static bool initialized;
+	static bool initialized = false;
 
 	/* initialize the pid controllers when the function is called for the first time */
 	if (initialized == false) {
@@ -117,8 +117,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 			 PID_MODE_DERIVATIV_SET, 157);
 
 		pid_yawpos_lim = 	global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
-		pid_yawspeed_lim =	(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
-		pid_att_lim =	(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
+		pid_yawspeed_lim =	global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
+		pid_att_lim =	global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
 
 		initialized = true;
 	}
@@ -150,8 +150,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
 
 		pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
-		pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
-		pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
+		pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
+		pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
 	}
 
 	/*Calculate Controllers*/
@@ -188,17 +188,13 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 	float motor_thrust;
 
 	// FLYING MODES
-	if (status->state_machine == SYSTEM_STATE_MANUAL) {
-		motor_thrust = att_sp->thrust;
-
-	} else if (status->state_machine == SYSTEM_STATE_GROUND_READY ||
+	if (status->state_machine == SYSTEM_STATE_MANUAL ||
+		status->state_machine == SYSTEM_STATE_GROUND_READY ||
 		status->state_machine == SYSTEM_STATE_STABILIZED ||
 		status->state_machine == SYSTEM_STATE_AUTO ||
-		status->state_machine == SYSTEM_STATE_MISSION_ABORT) {
-		motor_thrust = att_sp->thrust;	//TODO
-
-	} else if (status->state_machine == SYSTEM_STATE_EMCY_LANDING) {
-		motor_thrust = att_sp->thrust;	//TODO
+		status->state_machine == SYSTEM_STATE_MISSION_ABORT ||
+		status->state_machine == SYSTEM_STATE_EMCY_LANDING) {
+		motor_thrust = att_sp->thrust;
 
 	} else if (status->state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
 		/* immediately cut off motors */
@@ -209,6 +205,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 		motor_thrust = 0.0f;
 	}
 
+	printf("mot0: %3.1f\n", motor_thrust);
+
 	/* compensate thrust vector for roll / pitch contributions */
 	motor_thrust *= zcompensation;
 
@@ -244,6 +242,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 		roll_controller.saturated = 1;
 	}
 
+	printf("mot1: %3.1f\n", motor_thrust);
+
 	float output_band = 0.0f;
 	float band_factor = 0.75f;
 	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
@@ -332,12 +332,11 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 
 	/* send motors via UART */
 	if (motor_skip_counter % 5 == 0) {
-		if (motor_skip_counter % 50) printf("mot: %1.3f-%i-%i-%i-%i\n", motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
+		if (motor_skip_counter % 50 == 0) printf("mot: %3.1f-%i-%i-%i-%i\n", motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
 		uint8_t buf[5] = {1, 2, 3, 4, 5};
 		ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
 		write(ardrone_write, buf, sizeof(buf));
 	}
 
 	motor_skip_counter++;
-
 }
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 68bb36b2d..19fc81f63 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -230,11 +230,11 @@ void cal_bsort(int16_t a[], int n)
 	}
 }
 
-void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
+void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 {
 	/* set to mag calibration mode */
-	current_status->preflight_mag_calibration = true;
-	state_machine_publish(status_pub, current_status);
+	status->preflight_mag_calibration = true;
+	state_machine_publish(status_pub, status, mavlink_fd);
 
 	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
 	struct sensor_combined_s raw;
@@ -312,8 +312,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
 	}
 
 	/* disable calibration mode */
-	current_status->preflight_mag_calibration = false;
-	state_machine_publish(status_pub, current_status);
+	status->preflight_mag_calibration = false;
+	state_machine_publish(status_pub, status, mavlink_fd);
 
 	/* sort values */
 	cal_bsort(mag_minima[0], peak_samples);
@@ -393,7 +393,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
 	free(mag_minima[2]);
 
 	char offset_output[50];
-	sprintf(offset_output, "[commander] mag calibration finished, offsets: x:%d, y:%d, z:%d", mag_offset[0], mag_offset[1], mag_offset[2]);
+	sprintf(offset_output, "[commander] mag cal: x:%8.4f y:%8.4f z:%8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
 	mavlink_log_info(mavlink_fd, offset_output);
 
 	close(sub_sensor_combined);
@@ -436,7 +436,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status
 	global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2];
 
 	char offset_output[50];
-	sprintf(offset_output, "[commander] gyro calibration finished, offsets: x:%d, y:%d, z:%d", (int)gyro_offset[0], (int)gyro_offset[1], (int)gyro_offset[2]);
+	sprintf(offset_output, "[commander] gyro 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, offset_output);
 
 	close(sub_sensor_combined);
@@ -458,7 +458,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 	switch (cmd->command) {
 		case MAV_CMD_DO_SET_MODE:
 		{
-			if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1)) {
+			if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
 				result = MAV_RESULT_ACCEPTED;
 			} else {
 				result = MAV_RESULT_DENIED;
@@ -466,18 +466,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		}
 		break;
 
-		case MAV_CMD_COMPONENT_ARM_DISARM:
-		{
+		case MAV_CMD_COMPONENT_ARM_DISARM: {
 			/* request to arm */
 			if ((int)cmd->param1 == 1) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
 					result = MAV_RESULT_ACCEPTED;
 				} else {
 					result = MAV_RESULT_DENIED;
 				}
 			/* request to disarm */
 			} else if ((int)cmd->param1 == 0) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
 					result = MAV_RESULT_ACCEPTED;
 				} else {
 					result = MAV_RESULT_DENIED;
@@ -487,19 +486,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		break;
 
 		/* request for an autopilot reboot */
-		case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
-		{
-			// if ((int)cmd->param1 == 1) {
-			// 	if (OK == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) {
-			// 		result = MAV_RESULT_ACCEPTED;//TODO: this has no effect
-			// 	} else {
-			// 		result = MAV_RESULT_DENIED;
-			// 	}
-			// }
-
+		case MAV_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 = MAV_RESULT_ACCEPTED;
+				} else {
+					/* system may return here */
+					result = MAV_RESULT_DENIED;
+				}
+			}
 		}
 		break;
-//
+
 //		/* request to land */
 //		case MAV_CMD_NAV_LAND:
 //		 {
@@ -524,18 +523,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 //		break;
 //
 		/* preflight calibration */
-	case MAV_CMD_PREFLIGHT_CALIBRATION: {
+		case MAV_CMD_PREFLIGHT_CALIBRATION: {
 			bool handled = false;
 
 			/* gyro calibration */
 			if ((int)(cmd->param1) == 1) {
 				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, SYSTEM_STATE_PREFLIGHT);
+				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, "[commander] starting gyro calibration");
 					do_gyro_calibration(status_pub, &current_status);
-					do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY);
+					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;
 				} else {
 					mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration");
@@ -547,12 +546,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			/* magnetometer calibration */
 			if ((int)(cmd->param2) == 1) {
 				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, SYSTEM_STATE_PREFLIGHT);
+				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, "[commander] starting mag calibration");
 					do_mag_calibration(status_pub, &current_status);
-					do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY);
+					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;
 				} else {
 					mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration");
@@ -571,7 +570,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		break;
 
 		/* preflight parameter load / store */
-	case MAV_CMD_PREFLIGHT_STORAGE: {
+		case MAV_CMD_PREFLIGHT_STORAGE: {
 			/* Read all parameters from EEPROM to RAM */
 
 			if (((int)cmd->param1) == 0)	{
@@ -610,7 +609,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		}
 		break;
 
-	default: {
+		default: {
 			mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
 			result = MAV_RESULT_UNSUPPORTED;
 		}
@@ -625,8 +624,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		/* send acknowledge command */
 		mavlink_message_t msg;
 		mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result);
-		//global_data_send_mavlink_message_out(&msg);
-
 	}
 
 }
@@ -736,6 +733,8 @@ enum BAT_CHEM {
  *
  * @return the estimated remaining capacity in 0..1
  */
+float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage);
+
 float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage)
 {
 	float ret = 0;
@@ -783,17 +782,19 @@ int commander_main(int argc, char *argv[])
 		fprintf(stderr, "[commander] 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;
+
 	/* advertise to ORB */
 	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
+	state_machine_publish(stat_pub, &current_status, mavlink_fd);
 
 	if (stat_pub < 0) {
 		printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n");
 		exit(ERROR);
 	}
 
-	/* make sure we are in preflight state */
-	//do_state_update(stat_pub, &current_status, (commander_state_machine_t)SYSTEM_STATE_PREFLIGHT);
-
 	mavlink_log_info(mavlink_fd, "[commander] system is running");
 
 	/* load EEPROM parameters */
@@ -854,11 +855,10 @@ int commander_main(int argc, char *argv[])
 	struct sensor_combined_s sensors;
 	memset(&sensors, 0, sizeof(sensors));
 
-	uint8_t vehicle_state_previous = current_status.state_machine;
+	// uint8_t vehicle_state_previous = current_status.state_machine;
 	float voltage_previous = 0.0f;
 
 	uint64_t last_idle_time = 0;
-	unsigned int signal_loss_counter = 0;
 
 	/* now initialized */
 	commander_initialized = true;
@@ -891,7 +891,7 @@ int commander_main(int argc, char *argv[])
 			}
 
 			/* toggle error led at 5 Hz in HIL mode */
-			if ((current_status.mode & MAV_MODE_FLAG_HIL_ENABLED)) {
+			if ((current_status.mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
 				/* armed */
 				led_toggle(LED_AMBER);
 
@@ -994,7 +994,7 @@ int commander_main(int argc, char *argv[])
 //
 //
 //		if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests
-		update_state_machine_got_position_fix(stat_pub, &current_status);
+		update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
 		/* end: check gps */
 
 		/* Check battery voltage */
@@ -1017,7 +1017,7 @@ int commander_main(int argc, char *argv[])
 			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				critical_battery_voltage_actions_done = true;
 				mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
-				state_machine_emergency(stat_pub, &current_status);
+				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
 			}
 
 			critical_voltage_counter++;
@@ -1044,7 +1044,7 @@ int commander_main(int argc, char *argv[])
 			/* check if left stick is in lower left position --> switch to standby state */
 			if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-					update_state_machine_disarm(stat_pub, &current_status);
+					update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
 					stick_on_counter = 0;
 
 				} else {
@@ -1056,7 +1056,7 @@ int commander_main(int argc, char *argv[])
 			/* check if left stick is in lower right position --> arm */
 			if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-					update_state_machine_arm(stat_pub, &current_status);
+					update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
 					stick_on_counter = 0;
 
 				} else {
@@ -1067,13 +1067,13 @@ int commander_main(int argc, char *argv[])
 			//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
 
 			if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) {
-				update_state_machine_mode_manual(stat_pub, &current_status);
+				update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
 
 			} else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) {
-				update_state_machine_mode_auto(stat_pub, &current_status);
+				update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
 
 			} else {
-				update_state_machine_mode_stabilized(stat_pub, &current_status);
+				update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
 			}
 
 			/* Publish RC signal */
@@ -1112,11 +1112,11 @@ int commander_main(int argc, char *argv[])
 		    current_status.preflight_gyro_calibration == false &&
 		    current_status.preflight_mag_calibration == false) {
 		    	/* All ok, no calibration going on, go to standby */
-			do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY);
+			do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY, mavlink_fd);
 		}
 
 		/* Store old modes to detect and act on state transitions */
-		vehicle_state_previous = current_status.state_machine;
+		// vehicle_state_previous = current_status.state_machine;
 		voltage_previous = current_status.voltage_battery;
 
 		fflush(stdout);
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index ebae1c61c..a62b1437a 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -45,6 +45,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <systemlib/systemlib.h>
 #include <arch/board/up_hrt.h>
+#include <mavlink/mavlink_log.h>
 
 static const char* system_state_txt[] = {
 	"SYSTEM_STATE_PREFLIGHT",
@@ -61,10 +62,13 @@ static const char* system_state_txt[] = {
 
 };
 
-
-void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state)
+/**
+ * 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;
 
@@ -74,10 +78,10 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
 			uint8_t flight_environment_parameter = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
 
 			if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-				do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
+				ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
 
 			} else {
-				do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
+				ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
 			}
 
 			return;
@@ -89,63 +93,63 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
 		//TODO: add emcy landing code here
 
 		fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
-		//global_data_send_mavlink_statustext_message_out("Commander: state: emergency landing", MAV_SEVERITY_INFO);
+		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
 		break;
 
 	case SYSTEM_STATE_EMCY_CUTOFF:
-		/* Tell the controller to cutoff the motors (thrust = 0), make sure that this is not overwritten by another app and stays at 0 */
-		//TODO: add emcy cutoff code here
-
+		/* Tell the controller to cutoff the motors (thrust = 0) */
 		fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
-		//global_data_send_mavlink_statustext_message_out("Commander: state: emergency cutoff", MAV_SEVERITY_INFO);
+		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
 		break;
 
 	case SYSTEM_STATE_GROUND_ERROR:
 		fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
-		//global_data_send_mavlink_statustext_message_out("Commander: state: ground error", MAV_SEVERITY_INFO);
+		mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
 		break;
 
 	case SYSTEM_STATE_PREFLIGHT:
-		//global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO);
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY
 		 || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
 			invalid_state = false;
+			mavlink_log_info(mavlink_fd, "[commander] Switched to PREFLIGHT state");
 		} else {
 			invalid_state = true;
+			mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
 		}
 		break;
 
 	case SYSTEM_STATE_REBOOT:
-		usleep(500000);
-		reboot();
+		if (current_status->state_machine == SYSTEM_STATE_STANDBY
+		 || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+			invalid_state = false;
+			mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
+			usleep(500000);
+			reboot();
+			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
+		} else {
+			invalid_state = true;
+			mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
+		}
 		break;
 
 	case SYSTEM_STATE_STANDBY:
-		//global_data_send_mavlink_statustext_message_out("Commander: state: standby", MAV_SEVERITY_INFO);
+		mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
 		break;
 
 	case SYSTEM_STATE_GROUND_READY:
-		//global_data_send_mavlink_statustext_message_out("Commander: state: armed", MAV_SEVERITY_INFO);
-
-		//if in manual mode switch to manual state
-//		if (current_status->remote_manual) {
-//			printf("[commander] manual mode\n");
-//			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-//			return;
-//		}
-
+		mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
 		break;
 
 	case SYSTEM_STATE_AUTO:
-		//global_data_send_mavlink_statustext_message_out("Commander: state: auto", MAV_SEVERITY_INFO);
+		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
 		break;
 
 	case SYSTEM_STATE_STABILIZED:
-		//global_data_send_mavlink_statustext_message_out("Commander: state: stabilized", MAV_SEVERITY_INFO);
+		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
 		break;
 
 	case SYSTEM_STATE_MANUAL:
-		//global_data_send_mavlink_statustext_message_out("Commander: state: manual", MAV_SEVERITY_INFO);
+		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
 		break;
 
 	default:
@@ -155,11 +159,11 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
 
 	if (invalid_state == false || old_state != new_state) {
 		current_status->state_machine = new_state;
-		state_machine_publish(status_pub, current_status);
+		state_machine_publish(status_pub, current_status, mavlink_fd);
 	}
 }
 
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status) {
+void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
 	/* publish the new state */
 	current_status->counter++;
 	current_status->timestamp = hrt_absolute_time();
@@ -171,26 +175,26 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 /*
  * Private functions, update the state machine
  */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status)
+void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	fprintf(stderr, "[commander] 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, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
+		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_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
 
 	} else {
 		fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
 	}
 }
 
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
+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);
+		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
 
 	} else {
 		//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
@@ -399,28 +403,28 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
 /* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
 
 
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status)
+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, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+		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)
+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);
+		state_machine_emergency(status_pub, current_status, mavlink_fd);
 	}
 }
 
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status)
+void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	// XXX CHANGE BACK
 	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
 		printf("[commander] arming\n");
-		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 	} /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) {
 
 		printf("[commander] landing\n");
@@ -428,52 +432,67 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
 	}*/
 }
 
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status)
+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("[commander] going standby\n");
-		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+		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("[commander] MISSION ABORT!\n");
-		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+		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)
+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->control_manual_enabled = true;
+	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("[commander] manual mode\n");
-		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+		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)
+void update_state_machine_mode_stabilized(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_STABILIZED;
+	current_status->control_manual_enabled = true;
+	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_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
 		printf("[commander] stabilized mode\n");
-		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
 	}
 }
 
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status)
+void update_state_machine_mode_auto(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_AUTO;
+	current_status->control_manual_enabled = true;
+	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_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
 		printf("[commander] auto mode\n");
-		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO);
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
 	}
 }
 
 
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode)
+uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
 {
 	printf("in update state request\n");
 	uint8_t ret = 1;
 
-				current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+				current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
 			/* Set manual input enabled flag */
-			current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+			current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 
 	/* vehicle is disarmed, mode requests arming */
 	if (!(current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
@@ -481,10 +500,10 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		// XXX REMOVE
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
 			/* Set armed flag */
-			current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+			current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
 			/* Set manual input enabled flag */
-			current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+			current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 			ret = OK;
 			printf("[commander] arming due to command request\n");
 		}
@@ -495,7 +514,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		/* only disarm in ground ready */
 		//if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
 			/* Clear armed flag, leave manual input enabled */
-		//	current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+		//	current_status->mode &= ~VEHICLE_MODE_FLAG_SAFETY_ARMED;
 		//	do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
 		//	ret = OK;
 		//	printf("[commander] disarming due to command request\n");
@@ -503,16 +522,16 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 	//}
 
 	/* Switch on HIL if in standby */
-	if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) {
+	if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
 		/* Enable HIL on request */
-		current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED;
+		current_status->mode |= VEHICLE_MODE_FLAG_HIL_ENABLED;
 		ret = OK;
-		state_machine_publish(status_pub, current_status);
+		state_machine_publish(status_pub, current_status, mavlink_fd);
 		printf("[commander] Enabling HIL\n");
 	}
 
 	/* NEVER actually switch off HIL without reboot */
-	if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) {
+	if ((current_status->mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
 		fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
 		ret = ERROR;
 	}
@@ -520,7 +539,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 	return ret;
 }
 
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
+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;
 
@@ -539,7 +558,7 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
 		if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
 			printf("system will reboot\n");
 			//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
-			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
 			ret = 0;
 		}
 
@@ -549,7 +568,7 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
 		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, (commander_state_machine_t)SYSTEM_STATE_AUTO);
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
 			printf("state: auto\n");
 			ret = 0;
 		}
@@ -560,7 +579,7 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
 		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, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
 			printf("state: manual\n");
 			ret = 0;
 		}
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index 540182e4f..4908c799a 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -46,8 +46,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
-#include <v1.0/common/mavlink.h>
-
 
 /**
  * Switch to new state with no checking.
@@ -56,8 +54,13 @@
  * 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)
  */
-void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state);
+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);
@@ -72,43 +75,75 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
 
 /**
  * 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);
+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);
+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);
+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);
+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);
+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);
+void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
 /**
  * Handle state machine if mode switch is 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);
+void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
 /**
  * Publish current state information
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
  */
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status);
+void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
 
 /*
@@ -119,17 +154,39 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 
 /**
  * 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, uint8_t mode);
+uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
 
 /**
  * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
  */
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode);
+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);
 
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status);
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status);
+/**
+ * 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);
 
 
 
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 6745c0102..1e6156557 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -314,7 +314,14 @@ int set_hil_on_off(uint8_t vehicle_mode)
 
 void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_state, uint8_t *mavlink_mode)
 {
-	//TODO: Make this correct
+	/* reset MAVLink mode bitfield */
+	*mavlink_mode = 0;
+
+	/* set mode flags independent of system state */
+	if (c_status->control_manual_enabled) {
+		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+	}
+
 	switch (c_status->state_machine) {
 	case SYSTEM_STATE_PREFLIGHT:
 		if (c_status->preflight_gyro_calibration || c_status->preflight_mag_calibration) {
@@ -579,8 +586,10 @@ static void *uorb_receiveloop(void *arg)
 			/* XXX this is seriously bad - should be an emergency */
 		} else {
 
+			int ifds = 0;
+
 			/* --- SENSORS RAW VALUE --- */
-			if (fds[0].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 
 				/* copy sensors raw data into local buffer */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw);
@@ -596,7 +605,7 @@ static void *uorb_receiveloop(void *arg)
 			}
 
 			/* --- ATTITUDE VALUE --- */
-			if (fds[1].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 
 				/* copy attitude data into local buffer */
 				orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att);
@@ -608,7 +617,7 @@ static void *uorb_receiveloop(void *arg)
 			}
 
 			/* --- GPS VALUE --- */
-			if (fds[2].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 				/* copy gps data into local buffer */
 				orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps);
 				/* GPS position */
@@ -622,7 +631,7 @@ static void *uorb_receiveloop(void *arg)
 			}
 
 			// /* --- ARDRONE CONTROL OUTPUTS --- */
-			// if (fds[3].revents & POLLIN) {
+			// if (fds[ifds++].revents & POLLIN) {
 			// 	/* copy ardrone control data into local buffer */
 			// 	orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control);
 			// 	uint64_t timestamp    = buf.ar_control.timestamp;
@@ -642,7 +651,7 @@ static void *uorb_receiveloop(void *arg)
 			// }
 
 			/* --- SYSTEM STATUS --- */
-			if (fds[4].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 				/* immediately communicate state changes back to user */
 				orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
 				/* enable or disable HIL */
@@ -658,7 +667,7 @@ static void *uorb_receiveloop(void *arg)
 			}
 
 			/* --- RC CHANNELS --- */
-			if (fds[5].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 				/* copy rc channels into local buffer */
 				orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
 				/* Channels are sent in MAVLink main loop at a fixed interval */
@@ -666,7 +675,7 @@ static void *uorb_receiveloop(void *arg)
 			}
 
 			/* --- FIXED WING CONTROL CHANNELS --- */
-			if (fds[6].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 				/* copy fixed wing control into local buffer */
 				orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control);
 				/* send control output via MAVLink */
@@ -731,7 +740,7 @@ static void *uorb_receiveloop(void *arg)
 			}
 
 			/* --- VEHICLE GLOBAL POSITION --- */
-			if (fds[7].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 				/* copy global position data into local buffer */
 				orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
 				uint64_t timestamp = global_pos.timestamp;
@@ -749,14 +758,14 @@ static void *uorb_receiveloop(void *arg)
 			}
 
 			/* --- VEHICLE LOCAL POSITION --- */
-			if (fds[8].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 				/* copy local position data into local buffer */
 				orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
 				mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz);
 			}
 
 			/* --- VEHICLE GLOBAL SETPOINT --- */
-			if (fds[9].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 				/* copy local position data into local buffer */
 				orb_copy(ORB_ID(vehicle_global_position_setpoint), spg_sub, &buf.global_sp);
 				uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
@@ -765,14 +774,14 @@ static void *uorb_receiveloop(void *arg)
 			}
 
 			/* --- VEHICLE LOCAL SETPOINT --- */
-			if (fds[10].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 				/* copy local position data into local buffer */
 				orb_copy(ORB_ID(vehicle_local_position_setpoint), spl_sub, &buf.local_sp);
 				mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw);
 			}
 
 			/* --- VEHICLE ATTITUDE SETPOINT --- */
-			if (fds[11].revents & POLLIN) {
+			if (fds[ifds++].revents & POLLIN) {
 				/* copy local position data into local buffer */
 				orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp);
 				mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_tait_bryan, buf.att_sp.pitch_tait_bryan, buf.att_sp.yaw_tait_bryan, buf.att_sp.thrust);
@@ -1148,15 +1157,15 @@ int mavlink_main(int argc, char *argv[])
 
 	//default values for arguments
 	char *uart_name = "/dev/ttyS0";
-	int baudrate = 115200;
-	const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n";
+	int baudrate = 57600;
+	const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n\t\tdefault: -d %s -b %i\n";
 
 	/* read program arguments */
 	int i;
 
 	for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */
 		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
-			printf(commandline_usage, argv[0]);
+			printf(commandline_usage, argv[0], uart_name, baudrate);
 			return 0;
 		}
 
@@ -1166,7 +1175,7 @@ int mavlink_main(int argc, char *argv[])
 				uart_name = argv[i + 1];
 
 			} else {
-				printf(commandline_usage, argv[0]);
+				printf(commandline_usage, argv[0], uart_name, baudrate);
 				return 0;
 			}
 		}
@@ -1177,7 +1186,7 @@ int mavlink_main(int argc, char *argv[])
 				baudrate = atoi(argv[i + 1]);
 
 			} else {
-				printf(commandline_usage, argv[0]);
+				printf(commandline_usage, argv[0], uart_name, baudrate);
 				return 0;
 			}
 		}
@@ -1223,7 +1232,7 @@ int mavlink_main(int argc, char *argv[])
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
 	/* Set stack size, needs more than 2048 bytes */
-	pthread_attr_setstacksize(&uorb_attr, 4096);
+	pthread_attr_setstacksize(&uorb_attr, 5096);
 	pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL);
 
 	/* initialize waypoint manager */
diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile
index 0bc5b52d6..dadc2993d 100644
--- a/apps/sensors/Makefile
+++ b/apps/sensors/Makefile
@@ -37,6 +37,6 @@
 
 APPNAME		= sensors
 PRIORITY	= SCHED_PRIORITY_MAX-5
-STACKSIZE	= 2560
+STACKSIZE	= 2048
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index 098db4456..d847c4ffc 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -398,7 +398,7 @@ int sensors_main(int argc, char *argv[])
 	int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
 
 	if (manual_control_pub < 0) {
-		printf("[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n");
+		fprintf(stderr, "[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n");
 	}
 
 	/* advertise the rc topic */
@@ -406,6 +406,10 @@ int sensors_main(int argc, char *argv[])
 	memset(&rc, 0, sizeof(rc));
 	int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
 
+	if (rc_pub < 0) {
+		fprintf(stderr, "[sensors] ERROR: orb_advertise for topic rc_channels failed.\n");
+	}
+
 	/* subscribe to system status */
 	struct vehicle_status_s vstatus;
 	memset(&vstatus, 0, sizeof(vstatus));
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 9f72c8f78..8b2b6525c 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -86,6 +86,12 @@ 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,
+	VEHICLE_FLIGHT_MODE_STABILIZED,
+	VEHICLE_FLIGHT_MODE_AUTO
+};
+
 /**
  * state machine / state of vehicle.
  *
@@ -98,7 +104,9 @@ 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 */
 
-	commander_state_machine_t state_machine;
+	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 */
+
 	uint8_t mode;
 
 	bool control_manual_enabled;			/**< true if manual input is mixed in */