Checkpoint, arming/disarming still has a bug

5 files changed, 304 insertions, 244 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 1bfdd5660..4e470f1d9 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -817,22 +817,24 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 			/* request to arm */
 			if ((int)cmd->param1 == 1) {
-				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
+				// XXX add this again
+//				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
+//					result = VEHICLE_CMD_RESULT_ACCEPTED;
+//
+//				} else {
+//					result = VEHICLE_CMD_RESULT_DENIED;
+//				}
 
 				/* request to disarm */
 			// XXX this arms it instad of disarming
 			} else if ((int)cmd->param1 == 0) {
-				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
+				// XXX add this again
+//				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
+//					result = VEHICLE_CMD_RESULT_ACCEPTED;
+//
+//				} else {
+//					result = VEHICLE_CMD_RESULT_DENIED;
+//				}
 			}
 		}
 		break;
@@ -840,7 +842,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		/* request for an autopilot reboot */
 	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
 			if ((int)cmd->param1 == 1) {
-				if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) {
+				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) {
 					/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -883,25 +885,26 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			if ((int)(cmd->param1) == 1) {
 
 				/* transition to init state */
-				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-
-					mavlink_log_info(mavlink_fd, "starting gyro cal");
-					tune_confirm();
-					do_gyro_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished gyro cal");
-					tune_confirm();
-
-					/* back to standby state */
-					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
+				// XXX add this again
+//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
+//
+//					mavlink_log_info(mavlink_fd, "starting gyro cal");
+//					tune_confirm();
+//					do_gyro_calibration(status_pub, &current_status);
+//					mavlink_log_info(mavlink_fd, "finished gyro cal");
+//					tune_confirm();
+//
+//					/* back to standby state */
+//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+//
+//					result = VEHICLE_CMD_RESULT_ACCEPTED;
+//
+//				} else {
+//					mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
+//					result = VEHICLE_CMD_RESULT_DENIED;
+//				}
 
 				handled = true;
 			}
@@ -910,24 +913,25 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			if ((int)(cmd->param2) == 1) {
 
 				/* transition to init state */
-				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-					mavlink_log_info(mavlink_fd, "starting mag cal");
-					tune_confirm();
-					do_mag_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished mag cal");
-					tune_confirm();
-
-					/* back to standby state */
-					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
+				// XXX add this again
+//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
+//					mavlink_log_info(mavlink_fd, "starting mag cal");
+//					tune_confirm();
+//					do_mag_calibration(status_pub, &current_status);
+//					mavlink_log_info(mavlink_fd, "finished mag cal");
+//					tune_confirm();
+//
+//					/* back to standby state */
+//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+//
+//					result = VEHICLE_CMD_RESULT_ACCEPTED;
+//
+//				} else {
+//					mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
+//					result = VEHICLE_CMD_RESULT_DENIED;
+//				}
 
 				handled = true;
 			}
@@ -935,21 +939,22 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			/* zero-altitude pressure calibration */
 			if ((int)(cmd->param3) == 1) {
 				/* transition to calibration state */
-				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-
-					// XXX implement this
-					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
-					tune_confirm();
-
-					/* back to standby state */
-					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
+				// XXX add this again
+//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
+//
+//					// XXX implement this
+//					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
+//					tune_confirm();
+//
+//					/* back to standby state */
+//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+//
+//				} else {
+//					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
+//					result = VEHICLE_CMD_RESULT_DENIED;
+//				}
 
 				handled = true;
 			}
@@ -957,49 +962,51 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			/* trim calibration */
 			if ((int)(cmd->param4) == 1) {
 				/* transition to calibration state */
-				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-					mavlink_log_info(mavlink_fd, "starting trim cal");
-					tune_confirm();
-					do_rc_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished trim cal");
-					tune_confirm();
-
-					/* back to standby state */
-					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
+				// XXX add this again
+//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
+//					mavlink_log_info(mavlink_fd, "starting trim cal");
+//					tune_confirm();
+//					do_rc_calibration(status_pub, &current_status);
+//					mavlink_log_info(mavlink_fd, "finished trim cal");
+//					tune_confirm();
+//
+//					/* back to standby state */
+//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+//
+//					result = VEHICLE_CMD_RESULT_ACCEPTED;
+//
+//				} else {
+//					mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
+//					result = VEHICLE_CMD_RESULT_DENIED;
+//				}
 
 				handled = true;
 			}
 
 			/* accel calibration */
 			if ((int)(cmd->param5) == 1) {
-				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-
-					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
-					tune_confirm();
-					do_accel_calibration(status_pub, &current_status);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
-
-					/* back to standby state */
-					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
+				// XXX add this again
+//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
+//
+//					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
+//					tune_confirm();
+//					do_accel_calibration(status_pub, &current_status);
+//					tune_confirm();
+//					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
+//
+//					/* back to standby state */
+//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+//
+//					result = VEHICLE_CMD_RESULT_ACCEPTED;
+//
+//				} else {
+//					mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
+//					result = VEHICLE_CMD_RESULT_DENIED;
+//				}
 
 				handled = true;
 			}
@@ -1315,7 +1322,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_INIT;
+	current_status.navigation_state = NAVIGATION_STATE_STANDBY;
 	current_status.arming_state = ARMING_STATE_INIT;
 	current_status.flag_system_armed = false;
 
@@ -1336,6 +1343,9 @@ int commander_thread_main(int argc, char *argv[])
 	/* set battery warning flag */
 	current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
 
+	// XXX for now just set sensors as initialized
+	current_status.flag_system_sensors_initialized = true;
+
 	/* advertise to ORB */
 	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
 	/* publish current state machine */
@@ -1886,8 +1896,8 @@ int commander_thread_main(int argc, char *argv[])
 				    ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
 				    (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
 					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						current_status.flag_system_armed = false;
-						stick_on_counter = 0;
+						do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+						stick_off_counter = 0;
 
 					} else {
 						stick_off_counter++;
@@ -1898,7 +1908,7 @@ int commander_thread_main(int argc, char *argv[])
 				/* check if left stick is in lower right position --> arm */
 				if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
 					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						current_status.flag_system_armed = true;
+						do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_ARMED);
 						stick_on_counter = 0;
 
 					} else {
@@ -2054,9 +2064,6 @@ int commander_thread_main(int argc, char *argv[])
 			state_changed = false;
 		}
 
-		/* make changes in state machine if needed */
-		update_state_machine(stat_pub, &current_status, mavlink_fd);
-
 
 
 		/* Store old modes to detect and act on state transitions */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index d89be781a..a0b786aab 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -53,33 +53,12 @@
 #include "state_machine_helper.h"
 
 
-void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* check arming first */
-	if (current_status->flag_system_armed && current_status->flag_system_emergency) {
-		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT);
-	} else if(current_status->flag_system_armed && !current_status->flag_system_emergency) {
-		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED);
-	} else if(!current_status->flag_system_armed && current_status->flag_system_emergency) {
-		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR);
-	} else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) {
-		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY);
-	} else if (current_status->flag_system_sensors_ok) {
-		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY);
-	} else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) {
-		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT);
-	} else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) {
-		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR);
-	}
-
-	/* now determine the navigation state */
-}
-
 /**
  * Transition from one navigation state to another
  */
 int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state)
 {
+	bool valid_path = false;
 	bool valid_transition = false;
 	int ret = ERROR;
 
@@ -89,41 +68,43 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
 	} else {
 
 		switch (new_state) {
-			case NAVIGATION_STATE_INIT:
-
-				if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
-
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE");
-					valid_transition = true;
-				}
-				break;
-
 			case NAVIGATION_STATE_STANDBY:
 
-				if (current_status->navigation_state == NAVIGATION_STATE_INIT
-				 || current_status->navigation_state == NAVIGATION_STATE_MANUAL
+				if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
 				 || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE");
-					valid_transition = true;
+					if (!current_status->flag_system_armed) {
+						mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
+						valid_transition = true;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed");
+					}
+					valid_path = true;
 				}
 				break;
 
 			case NAVIGATION_STATE_MANUAL:
 
-				if (
-						( current_status->navigation_state == NAVIGATION_STATE_STANDBY
-					   || current_status->navigation_state == NAVIGATION_STATE_SEATBELT
-					   || current_status->navigation_state == NAVIGATION_STATE_LOITER
-					   || current_status->navigation_state == NAVIGATION_STATE_MISSION
-					   || current_status->navigation_state == NAVIGATION_STATE_RTL
-					   || current_status->navigation_state == NAVIGATION_STATE_LAND
-					   || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
-					   || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY)
-					 && current_status->arming_state == ARMING_STATE_ARMED) {
-
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE");
+				if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
+
+					/* only check for armed flag when coming from STANDBY XXX does that make sense? */
+					if (current_status->flag_system_armed) {
+						mavlink_log_critical(mavlink_fd, "Switched to MANUAL state");
+						valid_transition = true;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed");
+					}
+					valid_path = true;
+				} else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT
+						|| current_status->navigation_state == NAVIGATION_STATE_LOITER
+					    || current_status->navigation_state == NAVIGATION_STATE_MISSION
+						|| current_status->navigation_state == NAVIGATION_STATE_RTL
+						|| current_status->navigation_state == NAVIGATION_STATE_LAND
+						|| current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
+						|| current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
+					mavlink_log_critical(mavlink_fd, "Switched to MANUAL state");
 					valid_transition = true;
+					valid_path = true;
 				}
 				break;
 
@@ -133,67 +114,132 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
 				 || current_status->navigation_state == NAVIGATION_STATE_LOITER
 				 || current_status->navigation_state == NAVIGATION_STATE_MISSION) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE");
+					mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state");
 					valid_transition = true;
+					valid_path = true;
 				}
 				break;
 
 			case NAVIGATION_STATE_LOITER:
 
-				if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT)
-						&& current_status->flag_global_position_valid)
-					|| current_status->navigation_state == NAVIGATION_STATE_MISSION) {
-
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE");
+				/* Check for position lock when coming from MANUAL or SEATBELT */
+				if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
+					mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
+					valid_transition = true;
+					if (current_status->flag_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
+						valid_transition = true;
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock");
+					}
+					valid_path = true;
+				} else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) {
+					mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
 					valid_transition = true;
-				 }
+					valid_path = true;
+				}
 				break;
 
 			case NAVIGATION_STATE_AUTO_READY:
 
-				if (
-						(current_status->navigation_state == NAVIGATION_STATE_STANDBY
-				   	  && current_status->flag_global_position_valid
-				   	  && current_status->flag_valid_launch_position)
-				   	  || current_status->navigation_state == NAVIGATION_STATE_LAND) {
+				/* coming from STANDBY pos and home lock are needed */
+				if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE");
-					valid_transition = true;
+					if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) {
+						mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state");
+						valid_transition = true;
+					} else if (!current_status->flag_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock");
+					} else if (!current_status->flag_valid_launch_position) {
+						mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock");
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock");
+					}
+					valid_path = true;
+				/* coming from LAND home lock is needed */
+				} else if (current_status->navigation_state == NAVIGATION_STATE_LAND) {
+
+					if (current_status->flag_valid_launch_position) {
+						mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state");
+						valid_transition = true;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock");
+					}
+					valid_path = true;
 				}
 				break;
 
 			case NAVIGATION_STATE_MISSION:
 
-				if (
-					current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
-				 || current_status->navigation_state == NAVIGATION_STATE_RTL
-				 || (
-						 (current_status->navigation_state == NAVIGATION_STATE_MANUAL
-					   || current_status->navigation_state == NAVIGATION_STATE_SEATBELT
-					   || current_status->navigation_state == NAVIGATION_STATE_LOITER)
-					  	  && current_status->flag_global_position_valid
-					   	  && current_status->flag_valid_launch_position)
-				  ) {
-
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE");
+				/* coming from TAKEOFF or RTL is always possible */
+				if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
+				 || current_status->navigation_state == NAVIGATION_STATE_RTL) {
+					mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
 					valid_transition = true;
+					valid_path = true;
+
+				/* coming from MANUAL or SEATBELT requires home and pos lock */
+				} else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
+						|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
+
+					if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) {
+						mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
+						valid_transition = true;
+					} else if (!current_status->flag_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock");
+					} else if (!current_status->flag_valid_launch_position) {
+						mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock");
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock");
+					}
+					valid_path = true;
+
+				/* coming from loiter a home lock is needed */
+				} else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) {
+					if (current_status->flag_valid_launch_position) {
+						mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
+						valid_transition = true;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock");
+					}
+					valid_path = true;
 				}
 				break;
 
 			case NAVIGATION_STATE_RTL:
 
-				if (
-					current_status->navigation_state == NAVIGATION_STATE_MISSION
-				 || (
-						 (current_status->navigation_state == NAVIGATION_STATE_MANUAL
-					   || current_status->navigation_state == NAVIGATION_STATE_SEATBELT
-					   || current_status->navigation_state == NAVIGATION_STATE_LOITER)
-						  && current_status->flag_global_position_valid
-						  && current_status->flag_valid_launch_position)
-				  ) {
-
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE");
+				/* coming from MISSION is always possible */
+				if (current_status->navigation_state == NAVIGATION_STATE_MISSION) {
+					mavlink_log_critical(mavlink_fd, "Switched to RTL state");
 					valid_transition = true;
+					valid_path = true;
+
+				/* coming from MANUAL or SEATBELT requires home and pos lock */
+				} else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
+						|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
+
+					if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) {
+						mavlink_log_critical(mavlink_fd, "Switched to RTL state");
+						valid_transition = true;
+					} else if (!current_status->flag_global_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock");
+					} else if (!current_status->flag_valid_launch_position) {
+						mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock");
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock");
+					}
+					valid_path = true;
+
+				/* coming from loiter a home lock is needed */
+				} else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) {
+					if (current_status->flag_valid_launch_position) {
+						mavlink_log_critical(mavlink_fd, "Switched to RTL state");
+						valid_transition = true;
+					} else {
+						mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock");
+					}
+					valid_path = true;
 				}
 				break;
 
@@ -201,8 +247,10 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
 
 				if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE");
+					/* TAKEOFF is straight forward from AUTO READY */
+					mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state");
 					valid_transition = true;
+					valid_path = true;
 				}
 				break;
 
@@ -210,27 +258,10 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
 				if (current_status->navigation_state == NAVIGATION_STATE_RTL
 				 || current_status->navigation_state == NAVIGATION_STATE_LOITER) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE");
-					valid_transition = true;
-				}
-				break;
-
-			case NAVIGATION_STATE_REBOOT:
-				if (current_status->navigation_state == NAVIGATION_STATE_STANDBY
-					|| current_status->navigation_state == NAVIGATION_STATE_INIT
-					|| current_status->flag_hil_enabled) {
+					mavlink_log_critical(mavlink_fd, "Switched to LAND state");
 					valid_transition = true;
-					/* set system flags according to state */
-					current_status->flag_system_armed = false;
-					mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
-					usleep(500000);
-					up_systemreset();
-					/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
+					valid_path = true;
 				}
-
 				break;
 
 			default:
@@ -244,7 +275,9 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
 		state_machine_publish(status_pub, current_status, mavlink_fd);
 //		publish_armed_status(current_status);
 		ret = OK;
-	} else {
+	}
+
+	if (!valid_path){
 		mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition");
 	}
 
@@ -269,7 +302,8 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
 			case ARMING_STATE_INIT:
 
 				if (current_status->arming_state == ARMING_STATE_STANDBY) {
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE");
+
+					mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE");
 					valid_transition = true;
 				}
 				break;
@@ -280,8 +314,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
 				// XXX check if coming from reboot?
 				if (current_status->arming_state == ARMING_STATE_INIT) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE");
-					valid_transition = true;
+					if (current_status->flag_system_sensors_initialized) {
+						mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
+						valid_transition = true;
+					} else {
+						mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init.");
+					}
 				}
 				break;
 
@@ -290,16 +328,16 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
 				if (current_status->arming_state == ARMING_STATE_STANDBY
 				 || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE");
+					mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state");
 					valid_transition = true;
 				}
 				break;
 
-			case ARMING_STATE_MISSION_ABORT:
+			case ARMING_STATE_ABORT:
 
 				if (current_status->arming_state == ARMING_STATE_ARMED) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE");
+					mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state");
 					valid_transition = true;
 				}
 				break;
@@ -307,10 +345,10 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
 			case ARMING_STATE_ERROR:
 
 				if (current_status->arming_state == ARMING_STATE_ARMED
-				 || current_status->arming_state == ARMING_STATE_MISSION_ABORT
+				 || current_status->arming_state == ARMING_STATE_ABORT
 				 || current_status->arming_state == ARMING_STATE_INIT) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE");
+					mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state");
 					valid_transition = true;
 				}
 				break;
@@ -320,16 +358,18 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
 				if (current_status->arming_state == ARMING_STATE_STANDBY
 				 || current_status->arming_state == ARMING_STATE_ERROR) {
 
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE");
 					valid_transition = true;
-					// XXX reboot here?
+					mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
+					usleep(500000);
+					up_systemreset();
+					/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
 				}
 				break;
 
 			case ARMING_STATE_IN_AIR_RESTORE:
 
 				if (current_status->arming_state == ARMING_STATE_INIT) {
-					mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE");
+					mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state");
 					valid_transition = true;
 				}
 				break;
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index ed18bfbd2..bf9296caf 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -47,8 +47,6 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 
-void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
 int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
 
 int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 4636ee36e..e25f1be27 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -248,26 +248,19 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 
 		*mavlink_state = MAV_STATE_CALIBRATING;
 
-	} else if (v_status.flag_system_emergency) {
+	} else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) {
 
 		*mavlink_state = MAV_STATE_EMERGENCY;
 
-	} else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL
-	 || v_status.navigation_state == NAVIGATION_STATE_SEATBELT
-	 || v_status.navigation_state == NAVIGATION_STATE_LOITER
-	 || v_status.navigation_state == NAVIGATION_STATE_MISSION
-	 || v_status.navigation_state == NAVIGATION_STATE_RTL
-	 || v_status.navigation_state == NAVIGATION_STATE_LAND
-	 || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF
-	 || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
+	} else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) {
 
 		*mavlink_state = MAV_STATE_ACTIVE;
 
-	} else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) {
+	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
 
 		*mavlink_state = MAV_STATE_STANDBY;
 
-	} else if (v_status.navigation_state == NAVIGATION_STATE_INIT) {
+	} else if (v_status.arming_state == ARMING_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 ba9b9793b..874cf256c 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -60,8 +60,7 @@
 
 /* State Machine */
 typedef enum {
-	NAVIGATION_STATE_INIT = 0,
-	NAVIGATION_STATE_STANDBY,
+	NAVIGATION_STATE_STANDBY=0,
 	NAVIGATION_STATE_MANUAL,
 	NAVIGATION_STATE_SEATBELT,
 	NAVIGATION_STATE_LOITER,
@@ -70,15 +69,13 @@ typedef enum {
 	NAVIGATION_STATE_RTL,
 	NAVIGATION_STATE_TAKEOFF,
 	NAVIGATION_STATE_LAND,
-	NAVIGATION_STATE_GROUND_ERROR,
-	NAVIGATION_STATE_REBOOT
 } navigation_state_t;
 
 typedef enum {
 	ARMING_STATE_INIT = 0,
 	ARMING_STATE_STANDBY,
 	ARMING_STATE_ARMED,
-	ARMING_STATE_MISSION_ABORT,
+	ARMING_STATE_ABORT,
 	ARMING_STATE_ERROR,
 	ARMING_STATE_REBOOT,
 	ARMING_STATE_IN_AIR_RESTORE
@@ -95,6 +92,22 @@ enum VEHICLE_MODE_FLAG {
 	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_ASSISTED,
+	MODE_SWITCH_AUTO
+} mode_switch_pos_t;
+
+typedef enum {
+	RETURN_SWITCH_NONE = 0,
+	RETURN_SWITCH_RETURN
+} return_switch_pos_t;
+
+typedef enum {
+	MISSION_SWITCH_NONE = 0,
+	MISSION_SWITCH_MISSION
+} mission_switch_pos_t;
+
 //enum VEHICLE_FLIGHT_MODE {
 //	VEHICLE_FLIGHT_MODE_MANUAL = 0,		/**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
 //	VEHICLE_FLIGHT_MODE_STAB,		/**< attitude or rate stabilization plus velocity or position stabilization */
@@ -168,9 +181,18 @@ struct vehicle_status_s
 
 	/* system flags - these represent the state predicates */
 
+	mode_switch_pos_t mode_switch;
+	return_switch_pos_t return_switch;
+	mission_switch_pos_t mission_switch;
+
 	bool flag_system_armed;				/**< true is motors / actuators are armed */
 	bool flag_system_emergency;
-	bool flag_system_sensors_ok;
+	bool flag_system_in_air_restore;	/**< true if we can restore in mid air */
+	bool flag_system_sensors_initialized;
+	bool flag_system_arming_requested;
+	bool flag_system_disarming_requested;
+	bool flag_system_reboot_requested;
+	bool flag_system_on_ground;
 
 	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
 	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */