Checkpoint: implement new state machine, compiling, WIP

14 files changed, 899 insertions, 915 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 6b1bc0f9b..8716caef7 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -124,18 +124,24 @@ extern struct system_load_s system_load;
 static int leds;
 static int buzzer;
 static int mavlink_fd;
+
+/* flags */
 static bool commander_initialized = false;
-static struct vehicle_status_s current_status; /**< Main state machine */
+static bool thread_should_exit = false;		/**< daemon exit flag */
+static bool thread_running = false;		/**< daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+/* Main state machine */
+static struct vehicle_status_s current_status;
 static orb_advert_t stat_pub;
 
+/* XXX ? */
 // static uint16_t nofix_counter = 0;
 // static uint16_t gotfix_counter = 0;
 
+/* XXX ? */
 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);
@@ -252,18 +258,19 @@ enum AUDIO_PATTERN {
 int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
 {
 
+#warning add alarms for state transitions
 	/* 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);
-	}
+//	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);
-	}
+//	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 */
 
@@ -277,6 +284,7 @@ void tune_confirm(void)
 
 void tune_error(void)
 {
+	/* XXX change alarm to 2 tones*/
 	ioctl(buzzer, TONE_SET_ALARM, 4);
 }
 
@@ -795,19 +803,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 	/* 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;
+		// XXX implement this
 
-			} else {
+//			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)) {
+				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
@@ -815,9 +825,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				}
 
 				/* request to disarm */
-
+			// XXX this arms it instad of disarming
 			} 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)) {
+				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
@@ -830,7 +840,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		/* 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)) {
+				if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) {
 					/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -871,16 +881,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 			/* 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) {
+				/* transition to init state */
+				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
+
 					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);
+
+					/* back to standby state */
+					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
@@ -893,16 +908,20 @@ 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, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
 
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+				/* transition to init state */
+				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
 					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);
+
+					/* back to standby state */
+					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
@@ -916,12 +935,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			/* 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 (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
 
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+					// XXX implement this
 					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
 					tune_confirm();
 
+					/* back to standby state */
+					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+
 				} else {
 					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
 					result = VEHICLE_CMD_RESULT_DENIED;
@@ -933,15 +957,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			/* 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) {
+				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
 					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);
+
+					/* back to standby state */
+					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
@@ -954,16 +981,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 			/* accel calibration */
 			if ((int)(cmd->param5) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
+				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
 
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
 					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
 					tune_confirm();
 					do_accel_calibration(status_pub, &current_status);
 					tune_confirm();
 					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
+
+					/* back to standby state */
+					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
@@ -1262,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_sys_type = param_find("MAV_TYPE");
 
 	/* welcome user */
-	warnx("I am in command now!\n");
+	warnx("[commander] starting");
 
 	/* pthreads for command and subsystem info handling */
 	// pthread_t command_handling_thread;
@@ -1270,33 +1300,39 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* initialize */
 	if (led_init() != 0) {
-		warnx("ERROR: Failed to initialize leds\n");
+		warnx("ERROR: Failed to initialize leds");
 	}
 
 	if (buzzer_init() != 0) {
-		warnx("ERROR: Failed to initialize buzzer\n");
+		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.\n");
+		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.");
 	}
 
 	/* make sure we are in preflight state */
 	memset(&current_status, 0, sizeof(current_status));
-	current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
+	current_status.navigation_state = NAVIGATION_STATE_INIT;
+	current_status.arming_state = ARMING_STATE_INIT;
 	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;
 
@@ -1315,6 +1351,7 @@ int commander_thread_main(int argc, char *argv[])
 		exit(ERROR);
 	}
 
+	// XXX needed?
 	mavlink_log_info(mavlink_fd, "system is running");
 
 	/* create pthreads */
@@ -1325,7 +1362,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* Start monitoring loop */
 	uint16_t counter = 0;
-	uint8_t flight_env;
 
 	/* Initialize to 0.0V */
 	float battery_voltage = 0.0f;
@@ -1350,11 +1386,13 @@ int commander_thread_main(int argc, char *argv[])
 	struct offboard_control_setpoint_s sp_offboard;
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
+	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	struct vehicle_global_position_s global_position;
 	memset(&global_position, 0, sizeof(global_position));
 	uint64_t last_global_position_time = 0;
 
+	/* Subscribe to local position data */
 	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	struct vehicle_local_position_s local_position;
 	memset(&local_position, 0, sizeof(local_position));
@@ -1365,10 +1403,13 @@ int commander_thread_main(int argc, char *argv[])
 	 * 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));
@@ -1502,45 +1543,36 @@ int commander_thread_main(int argc, char *argv[])
 		/* Slow but important 8 Hz checks */
 		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
 			/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
-			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
-			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
-			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-				/* armed */
-				led_toggle(LED_BLUE);
-
-			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				/* not armed */
-				led_toggle(LED_BLUE);
-			}
-
-			/* toggle error led at 5 Hz in HIL mode */
-			if (current_status.flag_hil_enabled) {
-				/* hil enabled */
-				led_toggle(LED_AMBER);
+//			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
+//			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
+//			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
+//				/* armed */
+//				led_toggle(LED_BLUE);
+
+		} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+			/* not armed */
+			led_toggle(LED_BLUE);
+		}
 
-			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
-				/* toggle error (red) at 5 Hz on low battery or error */
-				led_toggle(LED_AMBER);
+		/* toggle error led at 5 Hz in HIL mode */
+		if (current_status.flag_hil_enabled) {
+			/* hil enabled */
+			led_toggle(LED_AMBER);
 
-			} else {
-				// /* Constant error indication in standby mode without GPS */
-				// if (!current_status.gps_valid) {
-				// 	led_on(LED_AMBER);
+		} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
+			/* toggle error (red) at 5 Hz on low battery or error */
+			led_toggle(LED_AMBER);
 
-				// } else {
-				// 	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 (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
+			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;
-			}
+			last_idle_time = system_load.tasks[0].total_runtime;
 		}
 
 		// // XXX Export patterns and threshold to parameters
@@ -1583,13 +1615,14 @@ int commander_thread_main(int argc, char *argv[])
 			low_voltage_counter++;
 		}
 
-		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
+		/* Critical, this is rather an emergency, change state machine */
 		else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
 			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				critical_battery_voltage_actions_done = true;
 				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
 				current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
-				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
+				// XXX implement this
+//				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
 			}
 
 			critical_voltage_counter++;
@@ -1698,7 +1731,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			if (gps_position.fix_type == GPS_FIX_TYPE_3D
 				&& (hdop_m < hdop_threshold_m)
-				&& (vdop_m < vdop_threshold_m)
+				&& (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk
 				&& !home_position_set
 				&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
 				&& !current_status.flag_system_armed) {
@@ -1734,85 +1767,112 @@ int commander_thread_main(int argc, char *argv[])
 
 			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;
-				// }
+				 /*
+				  * Check if manual control modes have to be switched
+				  */
+				 if (!isfinite(sp_man.mode_switch)) {
+				 	warnx("mode sw not finite");
+
+				 	/* this switch is not properly mapped, set attitude stabilized */
+				 	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_attitude_enabled = true;
+				 		current_status.flag_control_rates_enabled = true;
+				 	} else {
+
+				 		/* assuming a fixed wing, fall back to direct pass-through */
+				 		current_status.flag_control_attitude_enabled = false;
+				 		current_status.flag_control_rates_enabled = false;
+				 	}
+
+				 } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
+
+					 /* this switch is not properly mapped, set attitude stabilized */
+				 	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 m */
+				 		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.flag_control_attitude_enabled = false;
+				 		current_status.flag_control_rates_enabled = false;
+				 	}
+
+				 } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
+
+				 	/* top stick position, set auto/mission for all vehicle types */
+					current_status.flag_control_position_enabled = true;
+					current_status.flag_control_velocity_enabled = true;
+				 	current_status.flag_control_attitude_enabled = true;
+				 	current_status.flag_control_rates_enabled = true;
+
+				 } else {
+				 	/* center stick position, set seatbelt/simple control */
+					current_status.flag_control_velocity_enabled = true;
+				 	current_status.flag_control_attitude_enabled = true;
+				 	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
+				 * Check if land/RTL is requested
 				 */
-				if (!isfinite(sp_man.manual_sas_switch)) {
+				if (!isfinite(sp_man.return_switch)) {
 
 					/* this switch is not properly mapped, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
+					current_status.flag_land_requested = false; // XXX default?
 
-				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
+				} else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) {
 
 					/* bottom stick position, set altitude hold */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
+					current_status.flag_land_requested = false;
 
-				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
+				} else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) {
 
 					/* top stick position */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
+					current_status.flag_land_requested = true;
 
 				} else {
 					/* center stick position, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
+					current_status.flag_land_requested = false; // XXX default?
+				}
+
+				/* check mission switch  */
+				if (!isfinite(sp_man.mission_switch)) {
+
+					/* this switch is not properly mapped, set default */
+					current_status.flag_mission_activated = false;
+
+				} else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) {
+
+					/* top switch position */
+					current_status.flag_mission_activated = true;
+
+				} else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) {
+
+					/* bottom switch position */
+					current_status.flag_mission_activated = false;
+
+				} else {
+
+					/* center switch position, set default */
+					current_status.flag_mission_activated = false; // XXX default?
+				}
+
+				/* 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!");
 				}
 
 				/*
@@ -1826,7 +1886,7 @@ int commander_thread_main(int argc, char *argv[])
 				    ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
 				    (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
 					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
+						current_status.flag_system_armed = false;
 						stick_on_counter = 0;
 
 					} else {
@@ -1838,7 +1898,7 @@ int commander_thread_main(int argc, char *argv[])
 				/* check if left stick is in lower right position --> arm */
 				if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
 					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
+						current_status.flag_system_armed = true;
 						stick_on_counter = 0;
 
 					} else {
@@ -1847,37 +1907,6 @@ int commander_thread_main(int argc, char *argv[])
 					}
 				}
 
-				/* 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;
@@ -1965,12 +1994,13 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* arm / disarm on request */
 				if (sp_offboard.armed && !current_status.flag_system_armed) {
-					update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
+#warning fix this
+//					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);
+//					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);
+//					update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
 				}
 
 			} else {
@@ -2006,18 +2036,20 @@ int commander_thread_main(int argc, char *argv[])
 		current_status.timestamp = hrt_absolute_time();
 
 
+		// XXX what is this?
 		/* 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);
-		}
+//		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);
+#warning fix this
+//			publish_armed_status(&current_status);
 			orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
 			state_changed = false;
 		}
@@ -2043,7 +2075,7 @@ int commander_thread_main(int argc, char *argv[])
 	close(sensor_sub);
 	close(cmd_sub);
 
-	warnx("exiting..\n");
+	warnx("exiting");
 	fflush(stdout);
 
 	thread_running = false;
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index bea388a10..e1ec73110 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -40,182 +40,297 @@
 
 #include <stdio.h>
 #include <unistd.h>
+#include <stdbool.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/err.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
+ * Transition from one navigation 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 do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state)
 {
-	int invalid_state = false;
+	bool valid_transition = false;
 	int ret = ERROR;
 
-	commander_state_machine_t old_state = current_status->state_machine;
+	if (current_status->navigation_state == new_state) {
+		warnx("Navigation state not changed");
 
-	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 {
 
-			// } else {
-			// 	ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
-			// }
+		switch (new_state) {
+			case NAVIGATION_STATE_INIT:
+
+				if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE");
+					valid_transition = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_STANDBY:
+
+				if (current_status->navigation_state == NAVIGATION_STATE_INIT
+				 || current_status->navigation_state == NAVIGATION_STATE_MANUAL
+				 || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE");
+					valid_transition = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_MANUAL:
+
+				if (
+						( current_status->navigation_state == NAVIGATION_STATE_STANDBY
+					   || current_status->navigation_state == NAVIGATION_STATE_SEATBELT
+					   || current_status->navigation_state == NAVIGATION_STATE_LOITER
+					   || current_status->navigation_state == NAVIGATION_STATE_MISSION
+					   || current_status->navigation_state == NAVIGATION_STATE_RTL
+					   || current_status->navigation_state == NAVIGATION_STATE_LAND
+					   || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
+					   || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY)
+					 && current_status->arming_state == ARMING_STATE_ARMED) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE");
+					valid_transition = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_SEATBELT:
+
+				if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
+				 || current_status->navigation_state == NAVIGATION_STATE_LOITER
+				 || current_status->navigation_state == NAVIGATION_STATE_MISSION) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE");
+					valid_transition = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_LOITER:
+
+				if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT)
+						&& current_status->flag_global_position_valid)
+					|| current_status->navigation_state == NAVIGATION_STATE_MISSION) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE");
+					valid_transition = true;
+				 }
+				break;
+
+			case NAVIGATION_STATE_AUTO_READY:
+
+				if (
+						(current_status->navigation_state == NAVIGATION_STATE_STANDBY
+				   	  && current_status->flag_global_position_valid
+				   	  && current_status->flag_valid_launch_position)
+				   	  || current_status->navigation_state == NAVIGATION_STATE_LAND) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE");
+					valid_transition = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_MISSION:
+
+				if (
+					current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
+				 || current_status->navigation_state == NAVIGATION_STATE_RTL
+				 || (
+						 (current_status->navigation_state == NAVIGATION_STATE_MANUAL
+					   || current_status->navigation_state == NAVIGATION_STATE_SEATBELT
+					   || current_status->navigation_state == NAVIGATION_STATE_LOITER)
+					  	  && current_status->flag_global_position_valid
+					   	  && current_status->flag_valid_launch_position)
+				  ) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE");
+					valid_transition = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_RTL:
+
+				if (
+					current_status->navigation_state == NAVIGATION_STATE_MISSION
+				 || (
+						 (current_status->navigation_state == NAVIGATION_STATE_MANUAL
+					   || current_status->navigation_state == NAVIGATION_STATE_SEATBELT
+					   || current_status->navigation_state == NAVIGATION_STATE_LOITER)
+						  && current_status->flag_global_position_valid
+						  && current_status->flag_valid_launch_position)
+				  ) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE");
+					valid_transition = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_TAKEOFF:
+
+				if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE");
+					valid_transition = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_LAND:
+				if (current_status->navigation_state == NAVIGATION_STATE_RTL
+				 || current_status->navigation_state == NAVIGATION_STATE_LOITER) {
+
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE");
+					valid_transition = true;
+				}
+				break;
+
+			case NAVIGATION_STATE_REBOOT:
+				if (current_status->navigation_state == NAVIGATION_STATE_STANDBY
+					|| current_status->navigation_state == NAVIGATION_STATE_INIT
+					|| current_status->flag_hil_enabled) {
+					valid_transition = true;
+					/* set system flags according to state */
+					current_status->flag_system_armed = false;
+					mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
+					usleep(500000);
+					up_systemreset();
+					/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
+
+				} else {
+					mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
+				}
+
+				break;
+
+			default:
+				warnx("Unknown navigation state");
+				break;
 		}
-		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:
+	if (valid_transition) {
+		current_status->navigation_state = new_state;
+		state_machine_publish(status_pub, current_status, mavlink_fd);
+//		publish_armed_status(current_status);
+		ret = OK;
+	} else {
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition");
+	}
 
-		/* set system flags according to state */
+	return ret;
+}
 
-		/* prevent actuators from arming */
-		current_status->flag_system_armed = false;
+/**
+ * Transition from one arming state to another
+ */
+int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state)
+{
+	bool valid_transition = false;
+	int ret = ERROR;
 
-		warnx("GROUND ERROR, locking down propulsion system\n");
-		mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
-		break;
+	if (current_status->arming_state == new_state) {
+		warnx("Arming state not changed");
 
-	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 {
 
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
-		}
+		switch (new_state) {
 
-		break;
+			case ARMING_STATE_INIT:
 
-	case SYSTEM_STATE_REBOOT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-			|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
-			|| current_status->flag_hil_enabled) {
-			invalid_state = false;
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
-			usleep(500000);
-			up_systemreset();
-			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
+				if (current_status->arming_state == ARMING_STATE_STANDBY) {
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE");
+					valid_transition = true;
+				}
+				break;
 
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
-		}
+			case ARMING_STATE_STANDBY:
 
-		break;
+				// TODO check for sensors
+				// XXX check if coming from reboot?
+				if (current_status->arming_state == ARMING_STATE_INIT) {
 
-	case SYSTEM_STATE_STANDBY:
-		/* set system flags according to state */
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE");
+					valid_transition = true;
+				}
+				break;
 
-		/* standby enforces disarmed */
-		current_status->flag_system_armed = false;
+			case ARMING_STATE_ARMED:
 
-		mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
-		break;
+				if (current_status->arming_state == ARMING_STATE_STANDBY
+				 || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
 
-	case SYSTEM_STATE_GROUND_READY:
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE");
+					valid_transition = true;
+				}
+				break;
 
-		/* set system flags according to state */
+			case ARMING_STATE_MISSION_ABORT:
 
-		/* ground ready has motors / actuators armed */
-		current_status->flag_system_armed = true;
+				if (current_status->arming_state == ARMING_STATE_ARMED) {
 
-		mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
-		break;
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE");
+					valid_transition = true;
+				}
+				break;
 
-	case SYSTEM_STATE_AUTO:
+			case ARMING_STATE_ERROR:
 
-		/* set system flags according to state */
+				if (current_status->arming_state == ARMING_STATE_ARMED
+				 || current_status->arming_state == ARMING_STATE_MISSION_ABORT
+				 || current_status->arming_state == ARMING_STATE_INIT) {
 
-		/* auto is airborne and in auto mode, motors armed */
-		current_status->flag_system_armed = true;
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE");
+					valid_transition = true;
+				}
+				break;
 
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
-		break;
+			case ARMING_STATE_REBOOT:
 
-	case SYSTEM_STATE_STABILIZED:
+				if (current_status->arming_state == ARMING_STATE_STANDBY
+				 || current_status->arming_state == ARMING_STATE_ERROR) {
 
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE");
+					valid_transition = true;
+					// XXX reboot here?
+				}
+				break;
 
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
-		break;
+			case ARMING_STATE_IN_AIR_RESTORE:
 
-	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 (current_status->arming_state == ARMING_STATE_INIT) {
+					mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE");
+					valid_transition = true;
+				}
+				break;
+			default:
+				warnx("Unknown arming state");
+				break;
+		}
 	}
 
-	if (invalid_state == false || old_state != new_state) {
-		current_status->state_machine = new_state;
+	if (valid_transition) {
+		current_status->arming_state = new_state;
 		state_machine_publish(status_pub, current_status, mavlink_fd);
-		publish_armed_status(current_status);
+//		publish_armed_status(current_status);
 		ret = OK;
-	}
-
-	if (invalid_state) {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
-		ret = ERROR;
+	} else {
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition");
 	}
 
 	return ret;
 }
 
+
+
 void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	/* publish the new state */
@@ -223,70 +338,69 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 	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;
+//	if (current_status->flag_control_rates_enabled) {
+//		current_status->onboard_control_sensors_present |= 0x400;
+//
+//	} else {
+//		current_status->onboard_control_sensors_present &= ~0x400;
+//	}
+
+//	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+//	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+//	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+//	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
+//
+//	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
+//	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+//	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+//	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+//	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
 
 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
 }
 
-void publish_armed_status(const struct vehicle_status_s *current_status)
-{
-	struct actuator_armed_s armed;
-	armed.armed = current_status->flag_system_armed;
-	/* lock down actuators if required, only in HIL */
-	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
-	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
-	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
-}
+//void publish_armed_status(const struct vehicle_status_s *current_status)
+//{
+//	struct actuator_armed_s armed;
+//	armed.armed = current_status->flag_system_armed;
+//	/* lock down actuators if required, only in HIL */
+//	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
+//	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+//	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+//}
 
 
 /*
  * Private functions, update the state machine
  */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	warnx("EMERGENCY HANDLER\n");
-	/* Depending on the current state go to one of the error states */
-
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
-
-		// DO NOT abort mission
-		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
-
-	} else {
-		warnx("Unknown system state: #%d\n", current_status->state_machine);
-	}
-}
-
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
-{
-	if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
-		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
-
-	} else {
-		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
-	}
-
-}
+//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);
+//	}
+//
+//}
 
 
 
@@ -488,6 +602,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
 
 /* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
 
+#if 0
 
 void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
@@ -750,3 +865,4 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
 	return ret;
 }
 
+#endif
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index 2f2ccc729..bf9296caf 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -47,163 +47,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 
-/**
- * Switch to new state with no checking.
- *
- * do_state_update: this is the functions that all other functions have to call in order to update the state.
- * the function does not question the state change, this must be done before
- * The function performs actions that are connected with the new state (buzzer, reboot, ...)
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- *
- * @return 0 (macro OK) or 1 on error (macro ERROR)
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
-
-/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-
+int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
 
-/**
- * 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);
+int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
 
-/**
- * Handle state machine if position fix lost
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if user wants to arm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if user wants to disarm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is manual
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is stabilized
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is guided
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is auto
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish current state information
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
 void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
-
-/*
- *  Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
- *  If the request is obeyed the functions return 0
- *
- */
-
-/**
- * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
-
-/**
- * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
-
-/**
- * Always switches to critical mode under any circumstances.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Switches to emergency if required.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish the armed state depending on the current system state
- *
- * @param current_status the current system status
- */
-void publish_armed_status(const struct vehicle_status_s *current_status);
-
-
-
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp
index d9637b4a7..9a6919535 100644
--- a/apps/controllib/fixedwing.cpp
+++ b/apps/controllib/fixedwing.cpp
@@ -39,6 +39,7 @@
 
 #include "fixedwing.hpp"
 
+
 namespace control
 {
 
@@ -294,7 +295,7 @@ void BlockMultiModeBacksideAutopilot::update()
 		_actuators.control[i] = 0.0f;
 
 	// only update guidance in auto mode
-	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+	if (_status.navigation_state == NAVIGATION_STATE_MISSION) {
 		// update guidance
 		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
 	}
@@ -304,8 +305,8 @@ void BlockMultiModeBacksideAutopilot::update()
 	// the setpoint should update to loitering around this position
 
 	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-		_status.state_machine == SYSTEM_STATE_STABILIZED) {
+	if (_status.navigation_state == NAVIGATION_STATE_MISSION ||
+		_status.navigation_state == NAVIGATION_STATE_MANUAL) {
 
 		// update guidance
 		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
@@ -340,25 +341,25 @@ void BlockMultiModeBacksideAutopilot::update()
 		}
 
 
-	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-
-		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+	} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
 
+//		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+#warning fix the different manual modes
 			_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) {
-
-			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
-			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
-
-			_actuators.control[CH_AIL] = _stabilization.getAileron();
-			_actuators.control[CH_ELV] = - _stabilization.getElevator();
-			_actuators.control[CH_RDR] = _stabilization.getRudder();
-			_actuators.control[CH_THR] = _manual.throttle;
-		}
+//		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+//
+//			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
+//			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
+//
+//			_actuators.control[CH_AIL] = _stabilization.getAileron();
+//			_actuators.control[CH_ELV] = - _stabilization.getElevator();
+//			_actuators.control[CH_RDR] = _stabilization.getRudder();
+//			_actuators.control[CH_THR] = _manual.throttle;
+//		}
 	}
 
 	// update all publications
diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp
index fc929284c..6aff27e4c 100644
--- a/apps/drivers/blinkm/blinkm.cpp
+++ b/apps/drivers/blinkm/blinkm.cpp
@@ -554,26 +554,27 @@ BlinkM::led()
 						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;
-
-							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:
-								led_color_4 = LED_GREEN;
-								break;
-						}
+#warning fix this
+//						switch((int)vehicle_status_raw.flight_mode) {
+//							case VEHICLE_FLIGHT_MODE_MANUAL:
+//								led_color_4 = LED_AMBER;
+//								break;
+//
+//							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:
+//								led_color_4 = LED_GREEN;
+//								break;
+//						}
 
 						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;
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index aa9db6d52..12f06cdd2 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -185,87 +185,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/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index b958d5f96..4636ee36e 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -195,95 +195,85 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 	/* reset MAVLink mode bitfield */
 	*mavlink_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;
 	}
 
-	/* manual input */
-	if (v_status.flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-	}
-
-	/* attitude or rate control */
-	if (v_status.flag_control_attitude_enabled ||
-	    v_status.flag_control_rates_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-	}
-
-	/* 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) {
+	if (v_status.navigation_state == NAVIGATION_STATE_MISSION
+	 || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF
+	 || v_status.navigation_state == NAVIGATION_STATE_RTL
+	 || v_status.navigation_state == NAVIGATION_STATE_LAND
+	 || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
 		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
 	}
 
 	/* set arming state */
-	if (armed.armed) {
+	if (v_status.flag_system_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;
+	if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
 
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
-		}
+		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+	}
 
-		break;
+	if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
 
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
+		*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
+	}
 
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
+	if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
 
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
+		*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
+	}
 
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
 
-	case SYSTEM_STATE_AUTO:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
+	/**
+	 * Set mavlink state
+	 **/
 
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
+	/* set calibration state */
+	if (v_status.flag_preflight_gyro_calibration ||
+		v_status.flag_preflight_mag_calibration ||
+		v_status.flag_preflight_accel_calibration) {
 
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
+		*mavlink_state = MAV_STATE_CALIBRATING;
 
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
+	} else if (v_status.flag_system_emergency) {
 
-	case SYSTEM_STATE_GROUND_ERROR:
 		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
 
-	case SYSTEM_STATE_REBOOT:
-		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
+	} else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL
+	 || v_status.navigation_state == NAVIGATION_STATE_SEATBELT
+	 || v_status.navigation_state == NAVIGATION_STATE_LOITER
+	 || v_status.navigation_state == NAVIGATION_STATE_MISSION
+	 || v_status.navigation_state == NAVIGATION_STATE_RTL
+	 || v_status.navigation_state == NAVIGATION_STATE_LAND
+	 || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF
+	 || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
+
+		*mavlink_state = MAV_STATE_ACTIVE;
+
+	} else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) {
+
+		*mavlink_state = MAV_STATE_STANDBY;
+
+	} else if (v_status.navigation_state == NAVIGATION_STATE_INIT) {
+
+		*mavlink_state = MAV_STATE_UNINIT;
+	} else {
+
+		warnx("Unknown mavlink state");
+		*mavlink_state = MAV_STATE_CRITICAL;
 	}
 
 }
@@ -688,7 +678,7 @@ int mavlink_thread_main(int argc, char *argv[])
 			get_mavlink_mode_and_state(&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);
 
 			/* switch HIL mode if required */
 			set_hil_on_off(v_status.flag_hil_enabled);
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index 9f85d5801..15066010c 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -275,7 +275,7 @@ l_vehicle_status(struct listener *l)
 				   mavlink_system.type,
 				   MAV_AUTOPILOT_PX4,
 				   mavlink_mode,
-				   v_status.state_machine,
+				   v_status.navigation_state,
 				   mavlink_state);
 }
 
diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c
index 33ebe8600..6babe14ae 100644
--- a/apps/mavlink_onboard/mavlink.c
+++ b/apps/mavlink_onboard/mavlink.c
@@ -296,60 +296,60 @@ 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;
-		*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;
-	}
+//	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;
+//	}
 
 }
 
@@ -434,7 +434,7 @@ int mavlink_thread_main(int argc, char *argv[])
 			get_mavlink_mode_and_state(&v_status, &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,
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index d94c0a69c..da7550f79 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -296,38 +296,39 @@ mc_thread_main(int argc, char *argv[])
 							}
 
 							/* 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;
-									}
-								}
-							}
+#warning fix this
+//							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();
@@ -348,16 +349,17 @@ mc_thread_main(int argc, char *argv[])
 						}
 
 					} else {
+#warning fix this
 						/* 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();
-						}
+//						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();
+//						}
 					}
 
 				}
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index d8d200ea9..28579bc70 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -195,13 +195,11 @@ 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_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;
 
@@ -241,13 +239,11 @@ 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_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;
 
@@ -436,16 +432,15 @@ 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_OVER_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_mission_sw = param_find("RC_MAP_MISSION_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");
@@ -579,33 +574,25 @@ 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_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx("Failed getting flaps 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_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_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
+		warnx("Failed getting return 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");
@@ -649,15 +636,13 @@ 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[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;
@@ -1122,10 +1107,10 @@ 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.mission_switch = NAN;
+//		manual_control.auto_offboard_input_switch = NAN;
 
 		manual_control.flaps = NAN;
 		manual_control.aux1 = NAN;
@@ -1211,11 +1196,14 @@ 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);
+
+		/* land switch input */
+		manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 
 		/* flaps */
 		if (_rc.function[FLAPS] >= 0) {
@@ -1227,21 +1215,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[SAS_MODE] >= 0) {
-			manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_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[RTL] >= 0) {
-			manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled);
+		if (_rc.function[MISSION] >= 0) {
+			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].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) {
diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h
index 261a8a4ad..cfee81ea2 100644
--- a/apps/uORB/topics/manual_control_setpoint.h
+++ b/apps/uORB/topics/manual_control_setpoint.h
@@ -56,17 +56,17 @@ 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 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/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h
index 9dd54df91..a0bb25af4 100644
--- a/apps/uORB/topics/rc_channels.h
+++ b/apps/uORB/topics/rc_channels.h
@@ -68,18 +68,16 @@ 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,
+  MISSION = 6,
+  OFFBOARD_MODE = 7,
+  FLAPS   = 8,
+  AUX_1   = 9,
+  AUX_2   = 10,
+  AUX_3   = 11,
+  AUX_4   = 12,
+  AUX_5   = 13,
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 06b4c5ca5..f9c4a5fff 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -59,21 +59,30 @@
  */
 
 /* 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;
+typedef enum {
+	NAVIGATION_STATE_INIT = 0,
+	NAVIGATION_STATE_STANDBY,
+	NAVIGATION_STATE_MANUAL,
+	NAVIGATION_STATE_SEATBELT,
+	NAVIGATION_STATE_LOITER,
+	NAVIGATION_STATE_AUTO_READY,
+	NAVIGATION_STATE_MISSION,
+	NAVIGATION_STATE_RTL,
+	NAVIGATION_STATE_TAKEOFF,
+	NAVIGATION_STATE_LAND,
+	NAVIGATION_STATE_GROUND_ERROR,
+	NAVIGATION_STATE_REBOOT
+} navigation_state_t;
+
+typedef enum {
+	ARMING_STATE_INIT = 0,
+	ARMING_STATE_STANDBY,
+	ARMING_STATE_ARMED,
+	ARMING_STATE_MISSION_ABORT,
+	ARMING_STATE_ERROR,
+	ARMING_STATE_REBOOT,
+	ARMING_STATE_IN_AIR_RESTORE
+} arming_state_t;
 
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -86,25 +95,25 @@ 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 */
-};
+//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 +143,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,17 +159,17 @@ 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
+
+	navigation_state_t navigation_state;	/**< current navigation state */
+	arming_state_t arming_state;			/**< current arming 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 MAVLinks VEHICLE_TYPE enum */
 
 	/* system flags - these represent the state predicates */
 
 	bool flag_system_armed;				/**< true is motors / actuators are armed */
+	bool flag_system_emergency;
 	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 */
@@ -170,6 +179,9 @@ struct vehicle_status_s
 	bool flag_control_velocity_enabled;		/**< true if speed (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 
+	bool flag_land_requested;				/**< true if land requested */
+	bool flag_mission_activated;			/**< true if in mission mode */
+
 	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;
@@ -185,7 +197,7 @@ 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;
 
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;