commander and navigator: lot's of changes, failsafe handling in commander, navigator only for execution (WIP)

11 files changed, 411 insertions, 905 deletions

diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 5c502f682..98c491ce6 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -655,13 +655,14 @@ BlinkM::led()
 							/* indicate main control state */
 							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
+							/* TODO: add other Auto modes */
+							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
 								led_color_4 = LED_BLUE;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
 								led_color_4 = LED_WHITE;
-							else 
+							else
 								led_color_4 = LED_OFF;
 							led_color_5 = led_color_4;
 						}
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index ea4e95fb2..41716cd97 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -251,8 +251,8 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 		_gps_position->lon = 0;
 
 		// Indicate this data is not usable and bail out
-		_gps_position->eph_m = 1000.0f;
-		_gps_position->epv_m = 1000.0f;
+		_gps_position->eph = 1000.0f;
+		_gps_position->epv = 1000.0f;
 		_gps_position->fix_type = 0;
 		return;
 	}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 77d810a81..908384bb6 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1,11 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,8 +33,13 @@
 
 /**
  * @file commander.cpp
- * Main system state machine implementation.
+ * Main fail-safe handling.
  *
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -76,6 +76,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
+#include <uORB/topics/mission_result.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -87,6 +88,7 @@
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
+#include <systemlib/state_table.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -387,103 +389,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
 
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
 {
-	/* result of the command */
-	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
 		return false;
 	}
 
-	/* only handle high-priority commands here */
+	/* result of the command */
+	enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 
 	/* request to set different system mode */
 	switch (cmd->command) {
 	case VEHICLE_CMD_DO_SET_MODE: {
-		uint8_t base_mode = (uint8_t) cmd->param1;
-		uint8_t custom_main_mode = (uint8_t) cmd->param2;
+			uint8_t base_mode = (uint8_t)cmd->param1;
+			uint8_t custom_main_mode = (uint8_t)cmd->param2;
 
-		/* set HIL state */
-		hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
-		int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
-
-		/* if HIL got enabled, reset battery status state */
-		if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
-			/* reset the arming mode to disarmed */
-			arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
-		}
+			transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
 
-		// Transition the arming state
-		transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
+			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
-		/* set main state */
-		transition_result_t main_res = TRANSITION_DENIED;
+			/* set HIL state */
+			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+			transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
 
-		if (hil_ret == OK) {
-			ret = true;
-		}
-
-		// Transition the arming state
-		arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
-
-		if (arming_res == TRANSITION_CHANGED) {
-			ret = true;
-		}
+			// Transition the arming state
+			arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
-			} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
-				/* AUTO */
-				main_res = main_state_transition(status, MAIN_STATE_AUTO);
-			}
+			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+				/* use autopilot-specific mode */
+				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+					/* MANUAL */
+					main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
 					/* ALTCTL */
-					main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
+					main_ret = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
 					/* POSCTL */
-					main_res = main_state_transition(status, MAIN_STATE_POSCTL);
+					main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+					main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
 					/* ACRO */
-					main_res = main_state_transition(status, MAIN_STATE_ACRO);
+					main_ret = main_state_transition(status, MAIN_STATE_ACRO);
 				}
 
-				} else {
+			} else {
 				/* use base mode */
 				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
 					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+					main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
 					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
 						/* POSCTL */
-						main_res = main_state_transition(status, MAIN_STATE_POSCTL);
+						main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
 
 					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
 						/* MANUAL */
-						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+						main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
 					}
 				}
 			}
-		}
 
-		if (main_res == TRANSITION_CHANGED) {
-			ret = true;
-		}
+			if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
-		if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
-			result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-		} else {
-			result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			} else {
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
 		}
-
 		break;
-	}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 			// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
@@ -502,10 +481,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 				if (arming_res == TRANSITION_DENIED) {
 					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
 				} else {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 				}
 			}
 		}
@@ -516,14 +495,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			unsigned int mav_goto = cmd->param1;
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status->set_nav_state = NAVIGATION_STATE_LOITER;
+				status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
 				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status->set_nav_state = NAVIGATION_STATE_MISSION;
+				status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
 				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
@@ -531,6 +510,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
+#if 0
 	/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
@@ -538,15 +518,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
 			if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
 				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				/* reject parachute depoyment not armed */
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 
 		}
 		break;
+#endif
 
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
@@ -560,10 +541,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 					home->timestamp = hrt_absolute_time();
 
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
 
 			} else {
@@ -574,10 +555,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 				home->timestamp = hrt_absolute_time();
 
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 			}
 
-			if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+			if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) {
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt);
 				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
 
@@ -609,13 +590,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		break;
 	}
 
-	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+	if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
-		answer_command(*cmd, result);
+		answer_command(*cmd, cmd_result);
 	}
 
 	/* send any requested ACKs */
-	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+	if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* send acknowledge command */
 		// XXX TODO
 	}
@@ -682,7 +663,7 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAVIGATION_STATE_NONE;
+	status.set_nav_state = NAVIGATION_STATE_MANUAL;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
 	status.failsafe_state = FAILSAFE_STATE_NORMAL;
@@ -772,6 +753,11 @@ int commander_thread_main(int argc, char *argv[])
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
 
+	/* Subscribe to mission result topic */
+	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+	struct mission_result_s mission_result;
+	memset(&mission_result, 0, sizeof(mission_result));
+
 	/* Subscribe to manual control data */
 	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp_man;
@@ -849,6 +835,13 @@ int commander_thread_main(int argc, char *argv[])
 
 	start_time = hrt_absolute_time();
 
+	transition_result_t arming_ret;
+
+	/* check which state machines for changes, clear "changed" flag */
+	bool arming_state_changed = false;
+	bool main_state_changed = false;
+	bool failsafe_state_changed = false;
+
 	while (!thread_should_exit) {
 
 		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
@@ -856,6 +849,9 @@ int commander_thread_main(int argc, char *argv[])
 			mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		arming_ret = TRANSITION_NOT_CHANGED;
+
+
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);
 
@@ -935,6 +931,7 @@ int commander_thread_main(int argc, char *argv[])
 
 				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
+					arming_state_changed = true;
 				}
 			}
 		}
@@ -1131,12 +1128,21 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+
+				if (arming_ret == TRANSITION_CHANGED) {
+					warnx("changed 1");
+					arming_state_changed = true;
+				}
 
 			} else {
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
-			}
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 
+				if (arming_ret == TRANSITION_CHANGED) {
+					warnx("changed 2");
+					arming_state_changed = true;
+				}
+			}
 			status_changed = true;
 		}
 
@@ -1144,11 +1150,18 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
-			// XXX check for sensors
-			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			/* TODO: check for sensors */
+			warnx("arming status1: %d", status.arming_state);
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			warnx("arming status2: %d", status.arming_state);
+
+			if (arming_ret == TRANSITION_CHANGED) {
+				warnx("changed");
+				arming_state_changed = true;
+			}
 
 		} else {
-			// XXX: Add emergency stuff if sensors are lost
+			/* TODO: Add emergency stuff if sensors are lost */
 		}
 
 
@@ -1167,6 +1180,12 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
 		}
 
+		orb_check(mission_result_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+		}
+
 		/* start RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
@@ -1184,11 +1203,6 @@ int commander_thread_main(int argc, char *argv[])
 
 			status.rc_signal_lost = false;
 
-			transition_result_t arming_res;	// store all transitions results here
-
-			/* arm/disarm by RC */
-			arming_res = TRANSITION_NOT_CHANGED;
-
 			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
@@ -1199,7 +1213,11 @@ int commander_thread_main(int argc, char *argv[])
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					if (arming_ret == TRANSITION_CHANGED) {
+						warnx("stick 1");
+						arming_state_changed = true;
+					}
 					stick_off_counter = 0;
 
 				} else {
@@ -1221,7 +1239,11 @@ int commander_thread_main(int argc, char *argv[])
 						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
-						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						if (arming_ret == TRANSITION_CHANGED) {
+							warnx("stick 2");
+							arming_state_changed = true;
+						}
 					}
 
 					stick_on_counter = 0;
@@ -1234,23 +1256,25 @@ int commander_thread_main(int argc, char *argv[])
 				stick_on_counter = 0;
 			}
 
-			if (arming_res == TRANSITION_CHANGED) {
+			if (arming_ret == TRANSITION_CHANGED) {
 				if (status.arming_state == ARMING_STATE_ARMED) {
 					mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
 
 				} else {
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
 				}
+				arming_state_changed = true;
 
-			} else if (arming_res == TRANSITION_DENIED) {
+			} else if (arming_ret == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
 			}
 
-			if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-				/* recover from failsafe */
-				(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
-			}
+
+			// if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+			//	/* recover from failsafe */
+			//	(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+			// }
 
 			/* evaluate the main state machine according to mode switches */
 			transition_result_t main_res = set_main_state_rc(&status, &sp_man);
@@ -1258,102 +1282,41 @@ int commander_thread_main(int argc, char *argv[])
 			/* play tune on mode change only if armed, blink LED always */
 			if (main_res == TRANSITION_CHANGED) {
 				tune_positive(armed.armed);
+				main_state_changed = true;
 
 			} else if (main_res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
 
-			/* set navigation state */
-			/* RETURN switch, overrides MISSION switch */
-			if (sp_man.return_switch == SWITCH_POS_ON) {
-				/* switch to RTL if not already landed after RTL and home position set */
-				status.set_nav_state = NAVIGATION_STATE_RTL;
-
-
-			} else {
-
-				/* LOITER switch */
-				if (sp_man.loiter_switch == SWITCH_POS_ON) {
-					/* stick is in LOITER position */
-					status.set_nav_state = NAVIGATION_STATE_LOITER;
-
-				} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
-					/* stick is in MISSION position */
-					status.set_nav_state = NAVIGATION_STATE_MISSION;
-
-				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
-					   pos_sp_triplet.nav_state == NAV_STATE_RTL) {
-					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-					status.set_nav_state = NAVIGATION_STATE_MISSION;
-				}
-			}
-
 		} else {
 			if (!status.rc_signal_lost) {
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
 				status.rc_signal_lost = true;
 				status_changed = true;
 			}
+		}
 
-			if (armed.armed) {
-				if (status.main_state == MAIN_STATE_AUTO) {
-					/* check if AUTO mode still allowed */
-					transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
-
-					if (auto_res == TRANSITION_NOT_CHANGED) {
-						last_auto_state_valid = hrt_absolute_time();
-					}
-
-					/* still invalid state after the timeout interval, execute failsafe */
-					if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
-						/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
-						auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
-
-						if (auto_res == TRANSITION_DENIED) {
-							/* LAND not allowed, set TERMINATION state */
-							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-						}
-					} else {
-						status.set_nav_state = NAVIGATION_STATE_MISSION;
-					}
+		if (armed.armed) {
+			/* if RC is lost, switch to RTL_RC or Termination unless a mission is running
+			 * and not yet finished) */
+			if (status.rc_signal_lost
+		            && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) {
 
+				/* if we have a global position, we can switch to RTL, if not, we can try to land */
+				if (status.condition_global_position_valid) {
+					status.failsafe_state = FAILSAFE_STATE_RTL_RC;
 				} else {
-					/* failsafe for manual modes */
-					transition_result_t manual_res = TRANSITION_DENIED;
-
-					if (!status.condition_landed) {
-						/* vehicle is not landed, try to perform RTL */
-						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
-					}
-
-					if (manual_res == TRANSITION_DENIED) {
-						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
-						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
-
-						if (manual_res == TRANSITION_DENIED) {
-							/* LAND not allowed, set TERMINATION state */
-							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-						}
-					} else {
-						status.set_nav_state = NAVIGATION_STATE_RTL;
-					}
-				}
-
-			} else {
-				if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-					/* reset failsafe when disarmed */
-					(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+					status.failsafe_state = FAILSAFE_STATE_LAND;
 				}
 			}
-		}
 
-		// TODO remove this hack
-		/* flight termination in manual mode if assist switch is on POSCTL position */
-		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
-			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
-				tune_positive(armed.armed);
-			}
+			/* if the data link is lost, switch to RTL_DL or Termination */
+			/* TODO: add this */
+
+		} else {
+			/* reset failsafe when disarmed */
+			status.failsafe_state = FAILSAFE_STATE_NORMAL;
 		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
@@ -1369,11 +1332,6 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* check which state machines for changes, clear "changed" flag */
-		bool arming_state_changed = check_arming_state_changed();
-		bool main_state_changed = check_main_state_changed();
-		bool failsafe_state_changed = check_failsafe_state_changed();
-
 		hrt_abstime t1 = hrt_absolute_time();
 
 		/* print new state */
@@ -1408,18 +1366,24 @@ int commander_thread_main(int argc, char *argv[])
 				/* mark home position as set */
 				status.condition_home_position_valid = true;
 			}
+			arming_state_changed = false;
 		}
 
 		was_armed = armed.armed;
 
+		/* now set nav state according to failsafe and main state */
+		set_nav_state(&status);
+
 		if (main_state_changed) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
+			main_state_changed = false;
 		}
 
 		if (failsafe_state_changed) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
+			failsafe_state_changed = false;
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@@ -1608,6 +1572,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 	switch (sp_man->mode_switch) {
 	case SWITCH_POS_NONE:
 		res = TRANSITION_NOT_CHANGED;
+		warnx("NONE");
 		break;
 
 	case SWITCH_POS_OFF:		// MANUAL
@@ -1648,14 +1613,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 		break;
 
 	case SWITCH_POS_ON:			// AUTO
-		res = main_state_transition(status, MAIN_STATE_AUTO);
+		res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
 		}
 
 		// else fallback to ALTCTL (POSCTL likely will not work too)
-		print_reject_mode(status, "AUTO");
 		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
@@ -1675,85 +1639,93 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 }
 
 void
-
 set_control_mode()
 {
-	/* set vehicle_control_mode according to main state and failsafe state */
+	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
+	/* TODO: check this */
 	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
 
-	control_mode.flag_control_termination_enabled = false;
-
-	/* set this flag when navigator should act */
-	bool navigator_enabled = false;
-
-	switch (status.failsafe_state) {
-	case FAILSAFE_STATE_NORMAL:
-		switch (status.main_state) {
-		case MAIN_STATE_MANUAL:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = status.is_rotary_wing;
-			control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case MAIN_STATE_ALTCTL:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case MAIN_STATE_POSCTL:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true;
-			control_mode.flag_control_velocity_enabled = true;
-			break;
-
-		case MAIN_STATE_AUTO:
-			navigator_enabled = true;
-			break;
+	switch (status.set_nav_state) {
+	case NAVIGATION_STATE_MANUAL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = status.is_rotary_wing;
+		control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
 
-		case MAIN_STATE_ACRO:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
+	case NAVIGATION_STATE_ACRO:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
 
-		default:
-			break;
-		}
+	case NAVIGATION_STATE_ALTCTL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
 
+	case NAVIGATION_STATE_POSCTL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_RTL:
-		navigator_enabled = true;
+	case NAVIGATION_STATE_AUTO_MISSION:
+	case NAVIGATION_STATE_AUTO_LOITER:
+	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_RTL_RC:
+	case NAVIGATION_STATE_AUTO_RTL_DL:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_LAND:
-		navigator_enabled = true;
+	case NAVIGATION_STATE_LAND:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		/* in failsafe LAND mode position may be not available */
+		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_TERMINATION:
+	case NAVIGATION_STATE_TERMINATION:
 		/* disable all controllers on termination */
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
@@ -1769,21 +1741,6 @@ set_control_mode()
 	default:
 		break;
 	}
-
-	/* navigator has control, set control mode flags according to nav state*/
-	if (navigator_enabled) {
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = true;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = true;
-
-		/* in failsafe LAND mode position may be not available */
-		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
-		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
-
-		control_mode.flag_control_altitude_enabled = true;
-		control_mode.flag_control_climb_rate_enabled = true;
-	}
 }
 
 void
@@ -1927,7 +1884,6 @@ void *commander_low_prio_loop(void *arg)
 
 				/* try to go to INIT/PREFLIGHT arming state */
 
-				// XXX disable interrupts in arming_state_transition
 				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 6f0e9794a..214480079 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +34,9 @@
 /**
  * @file state_machine_helper.cpp
  * State machine helper functions implementations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <stdio.h>
@@ -59,30 +60,20 @@
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-static bool arming_state_changed = true;
-static bool main_state_changed = true;
-static bool failsafe_state_changed = true;
-
 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current  state you can index into the array which
 // will be true for a valid transition or false for a invalid transition. In some cases even
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
 static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
-	//                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
-	{ /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
-	{ /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
-	{ /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
-	{ /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
-	{ /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
-	{ /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
-	{ /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
+	//                                  INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
+	{ /* ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
+	{ /* ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
+	{ /* ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
+	{ /* ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
+	{ /* ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
+	{ /* ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
+	{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
 };
 
 // You can index into the array with an arming_state_t in order to get it's textual representation
@@ -165,7 +156,6 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 			armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
 			ret = TRANSITION_CHANGED;
 			status->arming_state = new_arming_state;
-			arming_state_changed = true;
 		}
 	}
 
@@ -199,69 +189,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
 	}
 }
 
-bool
-check_arming_state_changed()
-{
-	if (arming_state_changed) {
-		arming_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 transition_result_t
 main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* transition may be denied even if requested the same state because conditions may be changed */
+	/* transition may be denied even if the same state is requested because conditions may have changed */
 	switch (new_main_state) {
 	case MAIN_STATE_MANUAL:
-		ret = TRANSITION_CHANGED;
-		break;
-
 	case MAIN_STATE_ACRO:
 		ret = TRANSITION_CHANGED;
 		break;
 
 	case MAIN_STATE_ALTCTL:
-
 		/* need at minimum altitude estimate */
+		/* TODO: add this for fixedwing as well */
 		if (!status->is_rotary_wing ||
 		    (status->condition_local_altitude_valid ||
 		     status->condition_global_position_valid)) {
 			ret = TRANSITION_CHANGED;
 		}
-
 		break;
 
 	case MAIN_STATE_POSCTL:
-
 		/* need at minimum local position estimate */
 		if (status->condition_local_position_valid ||
 		    status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
-
 		break;
 
-	case MAIN_STATE_AUTO:
-
+	case MAIN_STATE_AUTO_MISSION:
+	case MAIN_STATE_AUTO_LOITER:
 		/* need global position estimate */
 		if (status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
+		break;
 
+	case MAIN_STATE_AUTO_RTL:
+		/* need global position and home position */
+		if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
 		break;
-	}
 
+	case MAIN_STATE_MAX:
+	default:
+		break;
+	}
 	if (ret == TRANSITION_CHANGED) {
 		if (status->main_state != new_main_state) {
 			status->main_state = new_main_state;
-			main_state_changed = true;
-
 		} else {
 			ret = TRANSITION_NOT_CHANGED;
 		}
@@ -270,70 +249,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	return ret;
 }
 
-bool
-check_main_state_changed()
-{
-	if (main_state_changed) {
-		main_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
-bool
-check_failsafe_state_changed()
-{
-	if (failsafe_state_changed) {
-		failsafe_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 /**
-* Transition from one hil state to another
-*/
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+ * Transition from one hil state to another
+ */
+transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	bool valid_transition = false;
-	int ret = ERROR;
+	transition_result_t ret = TRANSITION_DENIED;
 
 	if (current_status->hil_state == new_state) {
-		valid_transition = true;
+		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
-
 		switch (new_state) {
-
 		case HIL_STATE_OFF:
-
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
 			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
-			valid_transition = false;
-
+			ret = TRANSITION_DENIED;
 			break;
 
 		case HIL_STATE_ON:
-
 			if (current_status->arming_state == ARMING_STATE_INIT
 			    || current_status->arming_state == ARMING_STATE_STANDBY
 			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
 
-				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
-				valid_transition = true;
-
-				// Disable publication of all attached sensors
-
+				/* Disable publication of all attached sensors */
 				/* list directory */
 				DIR		*d;
 				d = opendir("/dev");
 
 				if (d) {
-
 					struct dirent	*direntry;
 					char devname[24];
 
@@ -388,288 +332,98 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
 					}
-
 					closedir(d);
+					ret = TRANSITION_CHANGED;
+					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+
 
 				} else {
 					/* failed opening dir */
-					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
-					return 1;
+					mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
+					ret = TRANSITION_DENIED;
 				}
+			} else {
+				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
+				ret = TRANSITION_DENIED;
 			}
-
 			break;
 
 		default:
-			warnx("Unknown hil state");
+			warnx("Unknown HIL state");
 			break;
 		}
 	}
 
-	if (valid_transition) {
+	if (ret = TRANSITION_CHANGED) {
 		current_status->hil_state = new_state;
-
 		current_status->timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
 		// XXX also set lockdown here
-
-		ret = OK;
-
-	} else {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 	}
-
 	return ret;
 }
 
 
 /**
-* Transition from one failsafe state to another
-*/
-transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
+ * Check failsafe and main status and set navigation status for navigator accordingly
+ */
+void set_nav_state(struct vehicle_status_s *status)
 {
-	transition_result_t ret = TRANSITION_DENIED;
-
-	/* transition may be denied even if requested the same state because conditions may be changed */
-	if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
-		/* transitions from TERMINATION to other states not allowed */
-		if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
-			ret = TRANSITION_NOT_CHANGED;
-		}
-
-	} else {
-		switch (new_failsafe_state) {
-		case FAILSAFE_STATE_NORMAL:
-			/* always allowed (except from TERMINATION state) */
-			ret = TRANSITION_CHANGED;
+	switch (status->failsafe_state) {
+	case FAILSAFE_STATE_NORMAL:
+		/* evaluate main state to decide in normal (non-failsafe) mode */
+		switch (status->main_state) {
+		case MAIN_STATE_MANUAL:
+			status->set_nav_state = NAVIGATION_STATE_MANUAL;
 			break;
 
-		case FAILSAFE_STATE_RTL:
-
-			/* global position and home position required for RTL */
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->set_nav_state = NAVIGATION_STATE_RTL;
-				ret = TRANSITION_CHANGED;
-			}
+		case MAIN_STATE_ALTCTL:
+			status->set_nav_state = NAVIGATION_STATE_ALTCTL;
+			break;
 
+		case MAIN_STATE_POSCTL:
+			status->set_nav_state = NAVIGATION_STATE_POSCTL;
 			break;
 
-		case FAILSAFE_STATE_LAND:
+		case MAIN_STATE_AUTO_MISSION:
+			status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
+			break;
 
-			/* at least relative altitude estimate required for landing */
-			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
-				status->set_nav_state = NAVIGATION_STATE_LAND;
-				ret = TRANSITION_CHANGED;
-			}
+		case MAIN_STATE_AUTO_LOITER:
+			status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			break;
 
+		case MAIN_STATE_AUTO_RTL:
+			status->set_nav_state = NAVIGATION_STATE_AUTO_RTL;
 			break;
 
-		case FAILSAFE_STATE_TERMINATION:
-			/* always allowed */
-			ret = TRANSITION_CHANGED;
+		case MAIN_STATE_ACRO:
+			status->set_nav_state = NAVIGATION_STATE_ACRO;
 			break;
 
 		default:
 			break;
 		}
+		break;
 
-		if (ret == TRANSITION_CHANGED) {
-			if (status->failsafe_state != new_failsafe_state) {
-				status->failsafe_state = new_failsafe_state;
-				failsafe_state_changed = true;
-
-			} else {
-				ret = TRANSITION_NOT_CHANGED;
-			}
-		}
-	}
+	case FAILSAFE_STATE_RTL_RC:
+		status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC;
+		break;
 
-	return ret;
-}
+	case FAILSAFE_STATE_RTL_DL:
+		status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL;
+		break;
 
+	case FAILSAFE_STATE_LAND:
+		status->set_nav_state = NAVIGATION_STATE_LAND;
+		break;
 
+	case FAILSAFE_STATE_TERMINATION:
+		status->set_nav_state = NAVIGATION_STATE_TERMINATION;
+		break;
 
-// /*
-//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
-//  */
-
-// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
-//  *
-//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
-//  * */
-
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was removed something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was disabled something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-//
-//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-//{
-//	int ret = 1;
-//
-////	/* Switch on HIL if in standby and not already in HIL mode */
-////	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-////	    && !current_status->flag_hil_enabled) {
-////		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
-////			/* Enable HIL on request */
-////			current_status->flag_hil_enabled = true;
-////			ret = OK;
-////			state_machine_publish(status_pub, current_status, mavlink_fd);
-////			publish_armed_status(current_status);
-////			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-////
-////		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-////			   current_status->flag_fmu_armed) {
-////
-////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-////
-////		} else {
-////
-////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
-////		}
-////	}
-//
-//	/* switch manual / auto */
-//	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
-//		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
-//		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
-//		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-//		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
-//	}
-//
-//	/* vehicle is disarmed, mode requests arming */
-//	if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-//		/* only arm in standby state */
-//		// XXX REMOVE
-//		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-//			ret = OK;
-//			printf("[cmd] arming due to command request\n");
-//		}
-//	}
-//
-//	/* vehicle is armed, mode requests disarming */
-//	if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-//		/* only disarm in ground ready */
-//		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-//			ret = OK;
-//			printf("[cmd] disarming due to command request\n");
-//		}
-//	}
-//
-//	/* NEVER actually switch off HIL without reboot */
-//	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-//		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
-//		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
-//		ret = ERROR;
-//	}
-//
-//	return ret;
-//}
+	default:
+		break;
+	}
+}
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0ddd4f05a..594bf23a1 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -56,25 +56,17 @@ typedef enum {
 
 } transition_result_t;
 
-transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
-
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
-bool check_arming_state_changed();
+transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
 
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
-bool check_main_state_changed();
-
 transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
 
-bool check_navigation_state_changed();
-
-bool check_failsafe_state_changed();
-
-void set_navigation_state_changed();
+transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
+void set_nav_state(struct vehicle_status_s *status);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 70087cf3e..31c0c8f79 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -118,51 +118,71 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
 
-	if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND
-		|| pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) {
-		/* use main state when navigator is not active */
-		if (status->main_state == MAIN_STATE_MANUAL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-
-		} else if (status->main_state == MAIN_STATE_ALTCTL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
+	switch (status->set_nav_state) {
 
-		} else if (status->main_state == MAIN_STATE_POSCTL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
-
-		} else if (status->main_state == MAIN_STATE_AUTO) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		case NAVIGATION_STATE_MANUAL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+			break;
 
-		} else if (status->main_state == MAIN_STATE_ACRO) {
+		case NAVIGATION_STATE_ACRO:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
-		}
-
-	} else {
-		/* use navigation state when navigator is active */
-		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			break;
 
-		if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		case NAVIGATION_STATE_ALTCTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		case NAVIGATION_STATE_POSCTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
+		case NAVIGATION_STATE_AUTO_MISSION:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
+		case NAVIGATION_STATE_AUTO_LOITER:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+			break;
+
+		case NAVIGATION_STATE_AUTO_RTL:
+		case NAVIGATION_STATE_AUTO_RTL_RC:
+		case NAVIGATION_STATE_AUTO_RTL_DL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
+		case NAVIGATION_STATE_LAND:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-		}
+			break;
+
+		case NAVIGATION_STATE_TERMINATION:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+			break;
+
 	}
 
 	*mavlink_custom_mode = custom_mode.data;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 74694447a..f30cd9a94 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -32,7 +32,6 @@
  ****************************************************************************/
 /**
  * @file navigator_main.cpp
- * Implementation of the main navigation state machine.
  *
  * Handles mission items, geo fencing and failsafe navigation behavior.
  * Published the position setpoint triplet for the position controller.
@@ -75,7 +74,6 @@
 
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <systemlib/state_table.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
@@ -102,7 +100,7 @@ static const int ERROR = -1;
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
-class Navigator : public StateTable
+class Navigator
 {
 public:
 	/**
@@ -116,7 +114,7 @@ public:
 	~Navigator();
 
 	/**
-	* Start the navigator task.
+	 * Start the navigator task.
 	 *
 	 * @return		OK on success.
 	 */
@@ -200,24 +198,6 @@ private:
 		param_t takeoff_alt;
 	}		_parameter_handles;		/**< handles for parameters */
 
-	enum Event {
-		EVENT_NONE_REQUESTED,
-		EVENT_LOITER_REQUESTED,
-		EVENT_MISSION_REQUESTED,
-		EVENT_RTL_REQUESTED,
-		EVENT_TAKEN_OFF,
-		EVENT_LANDED,
-		EVENT_MISSION_CHANGED,
-		EVENT_HOME_POSITION_CHANGED,
-		EVENT_MISSION_ITEM_REACHED,
-		MAX_EVENT
-	};						/**< possible events that can be thrown at state machine */
-
-	/**
-	 * State machine transition table
-	 */
-	static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
-
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -269,41 +249,6 @@ private:
 	void		task_main();
 
 	/**
-	 * Functions that are triggered when a new state is entered.
-	 */
-	bool		start_none_on_ground();
-	bool		start_none_in_air();
-	bool		start_auto_on_ground();
-	bool		start_loiter();
-	bool		start_mission();
-	bool		advance_mission();
-	bool		start_rtl();
-	bool		advance_rtl();
-	bool		start_land();
-
-	/**
-	 * Reset all "mission item reached" flags.
-	 */
-	void		reset_reached();
-
-	/**
-	 * Check if current mission item has been reached.
-	 */
-	bool		check_mission_item_reached();
-
-	/**
-	 * Set mission items by accessing the mission class.
-	 * If failing, tell the state machine to loiter.
-	 */
-	bool		set_mission_items();
-
-	/**
-	 * Set a RTL item by accessing the RTL class.
-	 * If failing, tell the state machine to loiter.
-	 */
-	void		set_rtl_item();
-
-	/**
 	 * Translate mission item to a position setpoint.
 	 */
 	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
@@ -329,7 +274,6 @@ Navigator	*g_navigator;
 Navigator::Navigator() :
 
 	/* state machine transition table */
-	StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
 	_task_should_exit(false),
 	_navigator_task(-1),
 	_mavlink_fd(-1),
@@ -368,11 +312,6 @@ Navigator::Navigator() :
 	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
 	_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
 	_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
-
-	/* Initialize state machine */
-	myState = NAV_STATE_NONE_ON_GROUND;
-
-	start_none_on_ground();
 }
 
 Navigator::~Navigator()
@@ -615,41 +554,6 @@ Navigator::task_main()
 		/* vehicle status updated */
 		if (fds[6].revents & POLLIN) {
 			vehicle_status_update();
-
-			/* commander requested new navigation mode, forward it to state machine */
-			switch (_vstatus.set_nav_state) {
-			case NAVIGATION_STATE_NONE:
-				dispatch(EVENT_NONE_REQUESTED);
-				break;
-
-			case NAVIGATION_STATE_LOITER:
-				dispatch(EVENT_LOITER_REQUESTED);
-				break;
-
-			case NAVIGATION_STATE_MISSION:
-				dispatch(EVENT_MISSION_REQUESTED);
-				break;
-
-			case NAVIGATION_STATE_RTL:
-				dispatch(EVENT_RTL_REQUESTED);
-				break;
-
-			case NAVIGATION_STATE_LAND:
-				/* TODO: add and test this */
-				// dispatch(EVENT_LAND_REQUESTED);
-				break;
-
-			default:
-				warnx("ERROR: Requested navigation state not supported");
-				break;
-			}
-
-			/* commander sets in-air/on-ground flag as well */
-			if (_vstatus.condition_landed) {
-				dispatch(EVENT_LANDED);
-			} else {
-				dispatch(EVENT_TAKEN_OFF);
-			}
 		}
 
 		/* parameters updated */
@@ -666,30 +570,21 @@ Navigator::task_main()
 		/* offboard mission updated */
 		if (fds[4].revents & POLLIN) {
 			offboard_mission_update();
-			/* TODO: check if mission really changed */
-			dispatch(EVENT_MISSION_CHANGED);
 		}
 
 		/* onboard mission updated */
 		if (fds[5].revents & POLLIN) {
 			onboard_mission_update();
-			/* TODO: check if mission really changed */
-			dispatch(EVENT_MISSION_CHANGED);
 		}
 
 		/* home position updated */
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
-			/* TODO check if home position really changed */
-			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
 		/* global position updated */
 		if (fds[1].revents & POLLIN) {
 			global_position_update();
-			if (check_mission_item_reached()) {
-				dispatch(EVENT_MISSION_ITEM_REACHED);
-			}
 
 			/* Check geofence violation */
 			if (!_geofence.inside(&_global_pos)) {
@@ -765,125 +660,8 @@ Navigator::status()
 	} else {
 		warnx("Geofence not set");
 	}
-
-	switch (myState) {
-	case NAV_STATE_NONE_ON_GROUND:
-		warnx("State: None on ground");
-		break;
-
-	case NAV_STATE_NONE_IN_AIR:
-		warnx("State: None in air");
-		break;
-
-	case NAV_STATE_LOITER:
-		warnx("State: Loiter");
-		break;
-
-	case NAV_STATE_MISSION:
-		warnx("State: Mission");
-		break;
-
-	case NAV_STATE_RTL:
-		warnx("State: RTL");
-		break;
-
-	case NAV_STATE_LAND:
-		warnx("State: Land");
-		break;
-
-	default:
-		warnx("State: Unknown");
-		break;
-	}
 }
-
-StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
-	{
-		/* NAV_STATE_NONE_ON_GROUND */
-		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_TAKEN_OFF */			{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-	},
-	{
-		/* NAV_STATE_NONE_IN_AIR */
-		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LANDED */			{ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-	},
-	{
-		/* NAV_STATE_AUTO_ON_GROUND */
-		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_TAKEN_OFF */			{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-	},
-	{
-		/* NAV_STATE_LOITER */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_LOITER},
-	},
-	{
-		/* NAV_STATE_MISSION */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_MISSION},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_MISSION},
-		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_MISSION},
-		/* EVENT_MISSION_ITEM_REACHED */	{ACTION(&Navigator::advance_mission), NAV_STATE_MISSION},
-	},
-	{
-		/* NAV_STATE_RTL */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_MISSION_ITEM_REACHED */	{ACTION(&Navigator::advance_rtl), NAV_STATE_RTL},
-	},
-	{
-		/* NAV_STATE_LAND */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_LAND},
-		/* EVENT_LANDED */			{ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LAND},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LAND},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_LAND},
-	},
-};
-
+#if 0
 bool
 Navigator::start_none_on_ground()
 {
@@ -984,13 +762,11 @@ Navigator::set_mission_items()
 	/* if we fail to set the current mission, continue to loiter */
 	if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
 
-		dispatch(EVENT_LOITER_REQUESTED);
 		return false;
 	}
 
 	/* if we got an RTL mission item, switch to RTL mode and give up */
 	if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-		dispatch(EVENT_RTL_REQUESTED);
 		return false;
 	}
 
@@ -1042,7 +818,6 @@ Navigator::start_rtl()
 	}
 
 	/* if RTL doesn't work, fallback to loiter */
-	dispatch(EVENT_LOITER_REQUESTED);
 	return false;
 }
 
@@ -1066,7 +841,6 @@ Navigator::advance_rtl()
 		return true;
 	}
 
-	dispatch(EVENT_LOITER_REQUESTED);
 	return false;
 }
 
@@ -1105,7 +879,7 @@ Navigator::start_land()
 	_update_triplet = true;
 	return true;
 }
-
+#endif
 void
 Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
 {
@@ -1147,7 +921,7 @@ Navigator::mission_item_to_position_setpoint(const mission_item_s *item, positio
 		sp->type = SETPOINT_TYPE_NORMAL;
 	}
 }
-
+#if 0
 bool
 Navigator::check_mission_item_reached()
 {
@@ -1251,12 +1025,12 @@ Navigator::reset_reached()
 	_waypoint_yaw_reached = false;
 
 }
-
+#endif
 void
 Navigator::publish_position_setpoint_triplet()
 {
 	/* update navigation state */
-	_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
+	/* TODO: set nav_state */
 
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub > 0) {
diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h
index 38378651b..e6011fdef 100644
--- a/src/modules/systemlib/state_table.h
+++ b/src/modules/systemlib/state_table.h
@@ -33,7 +33,7 @@
 
 /**
  * @file state_table.h
- * 
+ *
  * Finite-State-Machine helper class for state table
  * @author: Julian Oes <julian@oes.ch>
  */
@@ -44,32 +44,32 @@
 class StateTable
 {
 public:
-	typedef bool (StateTable::*Action)();
+	typedef void (StateTable::*Action)();
 	struct Tran {
 		Action action;
 		unsigned nextState;
 	};
-	
+
 	StateTable(Tran const *table, unsigned nStates, unsigned nSignals)
 	: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
-	
+
 	#define NO_ACTION &StateTable::doNothing
 	#define ACTION(_target) StateTable::Action(_target)
 
 	virtual ~StateTable() {}
-	
+
 	void dispatch(unsigned const sig) {
 		/* get transition using state table */
 		Tran const *t = myTable + myState*myNsignals + sig;
-		/* first up change state, this allows to do further dispatchs in the state functions */
-		
-		/* now execute state function, if it runs with success, accept new state */
-		if ((this->*(t->action))()) {
-			myState = t->nextState;
-		}
+
+		/* accept new state */
+		myState = t->nextState;
+
+		/*  */
+		(this->*(t->action))();
 	}
-	bool doNothing() {
-		return true;
+	void doNothing() {
+		return;
 	}
 protected:
 	unsigned myState;
@@ -79,4 +79,4 @@ private:
 	unsigned myNstates;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index 7c3921198..ad654a9ff 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -56,6 +56,7 @@ struct mission_result_s
 	bool mission_reached;		/**< true if mission has been reached			*/
 	unsigned mission_index_reached;	/**< index of the mission which has been reached	*/
 	unsigned index_current_mission; /**< index of the current mission			*/
+	bool mission_finished;		/**< true if mission has been completed			*/
 };
 
 /**
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 85e8ef8a5..c2741a05b 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -74,14 +74,13 @@ struct position_setpoint_s
 };
 
 typedef enum {
-	NAV_STATE_NONE_ON_GROUND = 0,
-	NAV_STATE_NONE_IN_AIR,
-	NAV_STATE_AUTO_ON_GROUND,
-	NAV_STATE_LOITER,
-	NAV_STATE_MISSION,
-	NAV_STATE_RTL,
-	NAV_STATE_LAND,
-	NAV_STATE_MAX
+	NAV_STATE_MANUAL = 0,
+	NAV_STATE_POSCTL,
+	NAV_STATE_AUTO,
+	NAV_STATE_RC_LOSS,
+	NAV_STATE_DL_LOSS,
+	NAV_STATE_TERMINATION,
+	MAX_NAV_STATE
 } nav_state_t;
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index d902dc49e..259d7329e 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -63,9 +63,11 @@ typedef enum {
 	MAIN_STATE_MANUAL = 0,
 	MAIN_STATE_ALTCTL,
 	MAIN_STATE_POSCTL,
-	MAIN_STATE_AUTO,
+	MAIN_STATE_AUTO_MISSION,
+	MAIN_STATE_AUTO_LOITER,
+	MAIN_STATE_AUTO_RTL,
 	MAIN_STATE_ACRO,
-	MAIN_STATE_MAX
+	MAIN_STATE_MAX,
 } main_state_t;
 
 // If you change the order, add or remove arming_state_t states make sure to update the arrays
@@ -78,7 +80,7 @@ typedef enum {
 	ARMING_STATE_STANDBY_ERROR,
 	ARMING_STATE_REBOOT,
 	ARMING_STATE_IN_AIR_RESTORE,
-	ARMING_STATE_MAX
+	ARMING_STATE_MAX,
 } arming_state_t;
 
 typedef enum {
@@ -88,18 +90,25 @@ typedef enum {
 
 typedef enum {
 	FAILSAFE_STATE_NORMAL = 0,		/**< Normal operation */
-	FAILSAFE_STATE_RTL,				/**< Return To Launch */
-	FAILSAFE_STATE_LAND,			/**< Land without position control */
+	FAILSAFE_STATE_RTL_RC,			/**< Return To Launch on remote control loss */
+	FAILSAFE_STATE_RTL_DL,			/**< Return To Launch on datalink loss */
+	FAILSAFE_STATE_LAND,			/**< Land as safe as possible */
 	FAILSAFE_STATE_TERMINATION,		/**< Disable motors and use parachute, can't be recovered */
-	FAILSAFE_STATE_MAX
+	FAILSAFE_STATE_MAX,
 } failsafe_state_t;
 
 typedef enum {
-	NAVIGATION_STATE_NONE = 0,
-	NAVIGATION_STATE_MISSION,
-	NAVIGATION_STATE_LOITER,
-	NAVIGATION_STATE_RTL,
-	NAVIGATION_STATE_LAND
+	NAVIGATION_STATE_MANUAL = 0,
+	NAVIGATION_STATE_ACRO,
+	NAVIGATION_STATE_ALTCTL,
+	NAVIGATION_STATE_POSCTL,
+	NAVIGATION_STATE_AUTO_MISSION,
+	NAVIGATION_STATE_AUTO_LOITER,
+	NAVIGATION_STATE_AUTO_RTL,
+	NAVIGATION_STATE_AUTO_RTL_RC,
+	NAVIGATION_STATE_AUTO_RTL_DL,
+	NAVIGATION_STATE_LAND,
+	NAVIGATION_STATE_TERMINATION,
 } navigation_state_t;
 
 enum VEHICLE_MODE_FLAG {
@@ -160,10 +169,10 @@ 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 */
 
-	main_state_t main_state;				/**< main state machine */
+	main_state_t main_state;		    	/**< main state machine */
 	navigation_state_t set_nav_state;		/**< set navigation state machine to specified value */
 	arming_state_t arming_state;			/**< current arming state */
-	hil_state_t hil_state;					/**< current hil state */
+	hil_state_t hil_state;				/**< current hil state */
 	failsafe_state_t failsafe_state;		/**< current failsafe state */
 
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */