diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg
index a50366bcb3..cbdad91c9c 100644
Binary files a/Documentation/flight_mode_state_machine.odg and b/Documentation/flight_mode_state_machine.odg differ
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 8b9e7c49cd..4e2b4907ba 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1319,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* make sure we are in preflight state */
 	memset(&current_status, 0, sizeof(current_status));
-	current_status.navigation_state = NAVIGATION_STATE_STANDBY;
+	current_status.navigation_state = NAVIGATION_STATE_INIT;
 	current_status.arming_state = ARMING_STATE_INIT;
 	current_status.hil_state = HIL_STATE_OFF;
 	current_status.flag_system_armed = false;
@@ -1857,19 +1857,7 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* Now it's time to handle the stick inputs */
-
-				if (current_status.arming_state == ARMING_STATE_ARMED) {
-
-					if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
-						do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_MANUAL );
-					} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
-						do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_SEATBELT );
-					} else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
-						if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) {
-							do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_MISSION );
-						}
-					}
-				}
+				navigation_state_update(stat_pub, &current_status, mavlink_fd);
 
 				/* handle the case where RC signal was regained */
 				if (!current_status.rc_signal_found_once) {
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 2e49354714..e6344f1a84 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -56,60 +56,195 @@
 /**
  * Transition from one sytem state to another
  */
-void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	int ret = ERROR;
-	system_state_t new_system_state;
+	navigation_state_t new_navigation_state;
 
+	/* do the navigation state update depending on the arming state */
+	switch (current_status->arming_state) {
 
-	/*
-	 * Firstly evaluate mode switch position
-	 * */
+		/* evaluate the navigation state when disarmed */
+		case ARMING_STATE_STANDBY:
 
-	/* Always accept manual mode */
-	if (current_status->mode_switch == MODE_SWITCH_MANUAL) {
-		new_system_state = SYSTEM_STATE_MANUAL;
+			/* Always accept manual mode */
+			if (current_status->mode_switch == MODE_SWITCH_MANUAL) {
+				new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY;
 
-	/* Accept SEATBELT if there is a local position estimate */
-	} else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) {
+			/* Accept SEATBELT if there is a local position estimate */
+			} else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) {
 
-		if (current_status->flag_local_position_valid) {
-			new_system_state = SYSTEM_STATE_SEATBELT;
-		} else {
-			/* or just fall back to manual */
-			mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
-			new_system_state = SYSTEM_STATE_MANUAL);
-		}
+				if (current_status->flag_local_position_valid) {
+					new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY;
+				} else {
+					/* or just fall back to manual */
+					mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position");
+					new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY;
+				}
 
-	/* Accept AUTO if there is a global position estimate */
-	} else if (current_status->mode_switch == MODE_SWITCH_AUTO) {
-		if (current_status->flag_local_position_valid) {
-			new_system_state = SYSTEM_STATE_SEATBELT);
-		} else {
-			mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position");
+			/* Accept AUTO if there is a global position estimate */
+			} else if (current_status->mode_switch == MODE_SWITCH_AUTO) {
+				if (current_status->flag_local_position_valid) {
+					new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY;
+				} else {
+					mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position");
 
-			/* otherwise fallback to seatbelt or even manual */
-			if (current_status->flag_local_position_valid) {
-				new_system_state = SYSTEM_STATE_SEATBELT;
-			} else {
-				mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
-				new_system_state = SYSTEM_STATE_MANUAL;
+					/* otherwise fallback to seatbelt or even manual */
+					if (current_status->flag_local_position_valid) {
+						new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position");
+						new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY;
+					}
+				}
 			}
-		}
-	}
 
-	/*
-	 * Next up, check for
-	 */
+			break;
+
+		/* evaluate the navigation state when armed */
+		case ARMING_STATE_ARMED:
+
+			/* Always accept manual mode */
+			if (current_status->mode_switch == MODE_SWITCH_MANUAL) {
+				new_navigation_state = NAVIGATION_STATE_MANUAL;
+
+			/* Accept SEATBELT if there is a local position estimate */
+			} else if (current_status->mode_switch == MODE_SWITCH_SEATBELT
+					&& current_status->return_switch == RETURN_SWITCH_NONE) {
+
+				if (current_status->flag_local_position_valid) {
+					new_navigation_state = NAVIGATION_STATE_SEATBELT;
+				} else {
+					/* or just fall back to manual */
+					mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position");
+					new_navigation_state = NAVIGATION_STATE_MANUAL;
+				}
+
+			/* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */
+			} else if (current_status->mode_switch == MODE_SWITCH_SEATBELT
+					&& current_status->return_switch == RETURN_SWITCH_RETURN) {
+
+				if (current_status->flag_local_position_valid) {
+					new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT;
+				} else {
+					/* descent not possible without baro information, fall back to manual */
+					mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position");
+					new_navigation_state = NAVIGATION_STATE_MANUAL;
+				}
+
+			/* Accept LOITER if there is a global position estimate */
+			} else if (current_status->mode_switch == MODE_SWITCH_AUTO
+					&& current_status->return_switch == RETURN_SWITCH_NONE
+					&& current_status->mission_switch == MISSION_SWITCH_NONE) {
+
+				if (current_status->flag_global_position_valid) {
+					new_navigation_state = NAVIGATION_STATE_AUTO_LOITER;
+				} else {
+					mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position");
+
+					/* otherwise fallback to SEATBELT or even manual */
+					if (current_status->flag_local_position_valid) {
+						new_navigation_state = NAVIGATION_STATE_SEATBELT;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position");
+						new_navigation_state = NAVIGATION_STATE_MANUAL;
+					}
+				}
+
+			/* Accept MISSION if there is a global position estimate and home position */
+			} else if (current_status->mode_switch == MODE_SWITCH_AUTO
+					&& current_status->return_switch == RETURN_SWITCH_NONE
+					&& current_status->mission_switch == MISSION_SWITCH_MISSION) {
+
+				if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) {
+					new_navigation_state = NAVIGATION_STATE_AUTO_MISSION;
+				} else {
+
+					/* spit out what exactly is unavailable */
+					if (current_status->flag_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position");
+					} else if (current_status->flag_valid_home_position) {
+						mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position");
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position");
+					}
+
+					/* otherwise fallback to SEATBELT or even manual */
+					if (current_status->flag_local_position_valid) {
+						new_navigation_state = NAVIGATION_STATE_SEATBELT;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position");
+						new_navigation_state = NAVIGATION_STATE_MANUAL;
+					}
+				}
+
+			/* Go to RTL or land if global position estimate and home position is available */
+			} else if (current_status->mode_switch == MODE_SWITCH_AUTO
+					&& current_status->return_switch == RETURN_SWITCH_RETURN
+					&& (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) {
+
+				if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) {
+
+					/* after RTL go to LAND */
+					if (current_status->flag_system_returned_to_home) {
+						new_navigation_state = NAVIGATION_STATE_AUTO_LAND;
+					} else {
+						new_navigation_state = NAVIGATION_STATE_AUTO_RTL;
+					}
+
+				} else {
+
+					/* spit out what exactly is unavailable */
+					if (current_status->flag_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position");
+					} else if (current_status->flag_valid_home_position) {
+						mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position");
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position");
+					}
+
+					/* otherwise fallback to SEATBELT_DESCENT or even MANUAL */
+					if (current_status->flag_local_position_valid) {
+						new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position");
+						new_navigation_state = NAVIGATION_STATE_MANUAL;
+					}
+				}
+			}
+			break;
+
+		case ARMING_STATE_ARMED_ERROR:
+
+			// TODO work out fail-safe scenarios
+			break;
+
+		case ARMING_STATE_STANDBY_ERROR:
+
+			// TODO work out fail-safe scenarios
+			break;
+
+		case ARMING_STATE_REBOOT:
+
+			// XXX I don't think we should end up here
+			break;
+
+		case ARMING_STATE_IN_AIR_RESTORE:
+
+			// XXX not sure what to do here
+			break;
+		default:
+			break;
+	}
 
 
 
 	/* Update the system state in case it has changed */
-	if (current_status->system_state != new_system_state) {
+	if (current_status->navigation_state != new_navigation_state) {
 
 		/* Check if the transition is valid */
-		if (system_state_update(current_status->system_state, new_system_state) == OK) {
-			current_status->system_state = new_system_state;
+		if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) {
+			current_status->navigation_state = new_navigation_state;
 			state_machine_publish(status_pub, current_status, mavlink_fd);
 		} else {
 			mavlink_log_critical(mavlink_fd, "System state transition not valid");
@@ -123,74 +258,164 @@ void state_machine_update(int status_pub, struct vehicle_status_s *current_statu
  * This functions does not evaluate any input flags but only checks if the transitions
  * are valid.
  */
-int system_state_update(system_state_t current_system_state, system_state_t new_system_state) {
+int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) {
 
 	int ret = ERROR;
 
 	/* only check transition if the new state is actually different from the current one */
-	if (new_system_state != current_system_state) {
+	if (new_navigation_state != current_navigation_state) {
 
-		switch (new_system_state) {
-			case SYSTEM_STATE_INIT:
+		switch (new_navigation_state) {
+			case NAVIGATION_STATE_INIT:
 
 				/* transitions back to INIT are possible for calibration */
-				if (current_system_state == SYSTEM_STATE_MANUAL
-				 || current_system_state == SYSTEM_STATE_SEATBELT
-				 || current_system_state == SYSTEM_STATE_AUTO) {
-
-					ret = OK;
-				}
-
-				break;
-
-			case SYSTEM_STATE_MANUAL:
-
-				/* transitions from INIT or from other modes */
-				if (current_system_state == SYSTEM_STATE_INIT
-				 || current_system_state == SYSTEM_STATE_SEATBELT
-				 || current_system_state == SYSTEM_STATE_AUTO) {
-
-					ret = OK;
-				}
-
-				break;
-
-			case SYSTEM_STATE_SEATBELT:
-
-				/* transitions from INIT or from other modes */
-				if (current_system_state == SYSTEM_STATE_INIT
-				 || current_system_state == SYSTEM_STATE_MANUAL
-				 || current_system_state == SYSTEM_STATE_AUTO) {
-
-					ret = OK;
-				}
-
-				break;
-
-			case SYSTEM_STATE_AUTO:
-
-				/* transitions from INIT or from other modes */
-				if (current_system_state == SYSTEM_STATE_INIT
-				 || current_system_state == SYSTEM_STATE_MANUAL
-				 || current_system_state == SYSTEM_STATE_SEATBELT) {
+				if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) {
 
 					ret = OK;
 				}
 				break;
 
-			case SYSTEM_STATE_REBOOT:
+			case NAVIGATION_STATE_MANUAL_STANDBY:
 
-				/* from INIT you can go straight to REBOOT */
-				if (current_system_state == SYSTEM_STATE_INIT) {
+				/* transitions from INIT and other STANDBY states as well as MANUAL are possible */
+				if (current_navigation_state == NAVIGATION_STATE_INIT
+				 || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_MANUAL) {
 
 					ret = OK;
 				}
 				break;
 
-			case SYSTEM_STATE_IN_AIR_RESTORE:
+			case NAVIGATION_STATE_MANUAL:
 
-				/* from INIT you can go straight to an IN AIR RESTORE */
-				if (current_system_state == SYSTEM_STATE_INIT) {
+				/* all transitions possible */
+				ret = OK;
+				break;
+
+			case NAVIGATION_STATE_SEATBELT_STANDBY:
+
+				/* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */
+				if (current_navigation_state == NAVIGATION_STATE_INIT
+				 || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) {
+
+					ret = OK;
+				}
+				break;
+
+			case NAVIGATION_STATE_SEATBELT:
+
+				/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/
+				if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
+				 || current_navigation_state == NAVIGATION_STATE_MANUAL
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_LAND
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_READY
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+
+					ret = OK;
+				}
+				break;
+
+			case NAVIGATION_STATE_SEATBELT_DESCENT:
+
+				/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */
+				if (current_navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_navigation_state == NAVIGATION_STATE_MANUAL
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_LAND
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_READY
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+
+					ret = OK;
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_STANDBY:
+
+				/* transitions from INIT or from other STANDBY modes or from AUTO READY */
+				if (current_navigation_state == NAVIGATION_STATE_INIT
+				 || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_READY) {
+
+					ret = OK;
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_READY:
+
+				/* transitions from AUTO_STANDBY or AUTO_LAND */
+				if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+
+					ret = OK;
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_TAKEOFF:
+
+				/* only transitions from AUTO_READY */
+				if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) {
+
+					ret = OK;
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_LOITER:
+
+				/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
+				if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_navigation_state == NAVIGATION_STATE_MANUAL) {
+
+					ret = OK;
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_MISSION:
+
+				/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
+				if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_navigation_state == NAVIGATION_STATE_MANUAL) {
+
+					ret = OK;
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_RTL:
+
+				/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
+				if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER
+				 || current_navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_navigation_state == NAVIGATION_STATE_MANUAL) {
+
+					ret = OK;
+				}
+				break;
+
+			case NAVIGATION_STATE_AUTO_LAND:
+				/* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */
+				if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
 
 					ret = OK;
 				}
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index 57b3db8f19..1c0564d077 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -47,10 +47,11 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 
-int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
+void navigation_state_update(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);
 
 void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
+int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state);
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp
index 9a69195352..584ca2306a 100644
--- a/apps/controllib/fixedwing.cpp
+++ b/apps/controllib/fixedwing.cpp
@@ -294,8 +294,9 @@ void BlockMultiModeBacksideAutopilot::update()
 	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
 		_actuators.control[i] = 0.0f;
 
+#warning this if is incomplete, should be based on flags
 	// only update guidance in auto mode
-	if (_status.navigation_state == NAVIGATION_STATE_MISSION) {
+	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
 		// update guidance
 		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
 	}
@@ -304,8 +305,9 @@ void BlockMultiModeBacksideAutopilot::update()
 	// once the system switches from manual or auto to stabilized
 	// the setpoint should update to loitering around this position
 
+#warning should be base on flags
 	// handle autopilot modes
-	if (_status.navigation_state == NAVIGATION_STATE_MISSION ||
+	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
 		_status.navigation_state == NAVIGATION_STATE_MANUAL) {
 
 		// update guidance
@@ -340,7 +342,7 @@ void BlockMultiModeBacksideAutopilot::update()
 							_actuators.control[CH_THR] : _manual.throttle;
 		}
 
-
+#warning should be base on flags
 	} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
 
 //		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index e25f1be27f..34b267d56b 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -205,36 +205,40 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 	}
 
 	/* autonomous mode */
-	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) {
+	if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
 		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
 	}
 
 	/* set arming state */
-	if (v_status.flag_system_armed) {
+	if (v_status.arming_state == ARMING_STATE_ARMED
+			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
 		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 
 	} else {
 		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
+	if (v_status.navigation_state == NAVIGATION_STATE_MANUAL
+			|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
 
 		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 	}
 
-	if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
-
-		*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
-	}
-
-	if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
-
-		*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
-	}
+//	if (v_status.system_state == NAVIGATION_STATE_SEATBELT) {
+//
+//		*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
+//	}
+//
+//	if (v_status.system_state == NAVIGATION_STATE_SEATBELT) {
+//
+//		*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
+//	}
 
 
 	/**
@@ -248,19 +252,30 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 
 		*mavlink_state = MAV_STATE_CALIBRATING;
 
-	} else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) {
+	} else if (v_status.flag_system_emergency) {
 
 		*mavlink_state = MAV_STATE_EMERGENCY;
 
-	} else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) {
+	// XXX difference between active and armed? is AUTO_READY active?
+	} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
+			|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT
+			|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
+			|| v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
 
 		*mavlink_state = MAV_STATE_ACTIVE;
 
-	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
+	} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct?
+			|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+			|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
 
 		*mavlink_state = MAV_STATE_STANDBY;
 
-	} else if (v_status.arming_state == ARMING_STATE_INIT) {
+	} else if (v_status.navigation_state == NAVIGATION_STATE_INIT) {
 
 		*mavlink_state = MAV_STATE_UNINIT;
 	} else {
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 20cb25cc02..29baff34bd 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -60,13 +60,20 @@
 
 /* State Machine */
 typedef enum {
-	SYSTEM_STATE_INIT=0,
-	SYSTEM_STATE_MANUAL,
-	SYSTEM_STATE_SEATBELT,
-	SYSTEM_STATE_AUTO,
-	SYSTEM_STATE_REBOOT,
-	SYSTEM_STATE_IN_AIR_RESTORE
-} system_state_t;
+	NAVIGATION_STATE_INIT=0,
+	NAVIGATION_STATE_MANUAL_STANDBY,
+	NAVIGATION_STATE_MANUAL,
+	NAVIGATION_STATE_SEATBELT_STANDBY,
+	NAVIGATION_STATE_SEATBELT,
+	NAVIGATION_STATE_SEATBELT_DESCENT,
+	NAVIGATION_STATE_AUTO_STANDBY,
+	NAVIGATION_STATE_AUTO_READY,
+	NAVIGATION_STATE_AUTO_TAKEOFF,
+	NAVIGATION_STATE_AUTO_LOITER,
+	NAVIGATION_STATE_AUTO_MISSION,
+	NAVIGATION_STATE_AUTO_RTL,
+	NAVIGATION_STATE_AUTO_LAND
+} navigation_state_t;
 
 typedef enum {
 	MANUAL_STANDBY = 0,
@@ -75,50 +82,20 @@ typedef enum {
 } manual_state_t;
 
 typedef enum {
-	SEATBELT_STANDBY,
-	SEATBELT_READY,
-	SEATBELT,
-	SEATBELT_ASCENT,
-	SEATBELT_DESCENT,
-} seatbelt_state_t;
-
-typedef enum {
-	AUTO_STANDBY,
-	AUTO_READY,
-	AUTO_LOITER,
-	AUTO_MISSION,
-	AUTO_RTL,
-	AUTO_TAKEOFF,
-	AUTO_LAND,
-} auto_state_t;
-
-//typedef enum {
-//	ARMING_STATE_INIT = 0,
-//	ARMING_STATE_STANDBY,
-//	ARMING_STATE_ARMED_GROUND,
-//	ARMING_STATE_ARMED_AIRBORNE,
-//	ARMING_STATE_ERROR_GROUND,
-//	ARMING_STATE_ERROR_AIRBORNE,
-//	ARMING_STATE_REBOOT,
-//	ARMING_STATE_IN_AIR_RESTORE
-//} arming_state_t;
+	ARMING_STATE_INIT = 0,
+	ARMING_STATE_STANDBY,
+	ARMING_STATE_ARMED,
+	ARMING_STATE_ARMED_ERROR,
+	ARMING_STATE_STANDBY_ERROR,
+	ARMING_STATE_REBOOT,
+	ARMING_STATE_IN_AIR_RESTORE
+} arming_state_t;
 
 typedef enum {
 	HIL_STATE_OFF = 0,
 	HIL_STATE_ON
 } hil_state_t;
 
-enum VEHICLE_MODE_FLAG {
-	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
-	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
-	VEHICLE_MODE_FLAG_HIL_ENABLED = 32,
-	VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16,
-	VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
-	VEHICLE_MODE_FLAG_AUTO_ENABLED = 4,
-	VEHICLE_MODE_FLAG_TEST_ENABLED = 2,
-	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
-}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
-
 typedef enum {
 	MODE_SWITCH_MANUAL = 0,
 	MODE_SWITCH_SEATBELT,
@@ -135,6 +112,17 @@ typedef enum {
 	MISSION_SWITCH_MISSION
 } mission_switch_pos_t;
 
+enum VEHICLE_MODE_FLAG {
+	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
+	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
+	VEHICLE_MODE_FLAG_HIL_ENABLED = 32,
+	VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16,
+	VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
+	VEHICLE_MODE_FLAG_AUTO_ENABLED = 4,
+	VEHICLE_MODE_FLAG_TEST_ENABLED = 2,
+	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
+}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
+
 /**
  * Should match 1:1 MAVLink's MAV_TYPE ENUM
  */
@@ -181,8 +169,8 @@ struct vehicle_status_s
 	uint64_t failsave_lowlevel_start_time;		/**< time when the lowlevel failsafe flag was set */
 //	uint64_t failsave_highlevel_begin; TO BE COMPLETED
 
-	system_state_t system_state;	/**< current system state */
-//	arming_state_t arming_state;			/**< current arming state */
+	navigation_state_t navigation_state;	/**< current system state */
+	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;					/**< current hil state */
 
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
@@ -203,7 +191,9 @@ struct vehicle_status_s
 	bool flag_system_arming_requested;
 	bool flag_system_disarming_requested;
 	bool flag_system_reboot_requested;
-	bool flag_system_on_ground;
+	bool flag_system_returned_to_home;
+
+	bool flag_auto_enabled;
 
 	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
 	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */