Merge branch 'beta' into offboard2

1 files changed, 348 insertions, 169 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bec6884e2..e1cca8bfb 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -200,9 +200,11 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
+void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
 
-transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+transition_result_t set_main_state_rc(struct vehicle_status_s *status);
+
+void set_control_mode();
 
 void print_reject_mode(const char *msg);
 
@@ -414,51 +416,45 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			/* set main state */
 			transition_result_t main_res = TRANSITION_DENIED;
 
-			if (status->rc_signal_lost) {
-				/* allow mode switching by command only if no RC signal */
-				if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-					/* use autopilot-specific mode */
-					if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
-						/* MANUAL */
-						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
-
-					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
-						/* SEATBELT */
-						main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+				/* use autopilot-specific mode */
+				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+					/* MANUAL */
+					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
 
-					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
-						/* EASY */
-						main_res = main_state_transition(status, MAIN_STATE_EASY);
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
+					/* SEATBELT */
+					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
-						/* AUTO */
-						main_res = main_state_transition(status, MAIN_STATE_AUTO);
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
+					/* EASY */
+					main_res = main_state_transition(status, MAIN_STATE_EASY);
 
-					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
-						/* OFFBOARD */
-						main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
-					}
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
 
-				} else {
-					/* use base mode */
-					if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
-						/* AUTO */
-						main_res = main_state_transition(status, MAIN_STATE_AUTO);
-
-					} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-						if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
-							/* EASY */
-							main_res = main_state_transition(status, MAIN_STATE_EASY);
-
-						} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
-							/* MANUAL */
-							main_res = main_state_transition(status, MAIN_STATE_MANUAL);
-						}
-					}
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
+					/* OFFBOARD */
+					main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
 				}
 
 			} else {
-				mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd");
+				/* use base mode */
+				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+						/* EASY */
+						main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+						/* MANUAL */
+						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					}
+				}
 			}
 
 			if (main_res == TRANSITION_CHANGED)
@@ -527,7 +523,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
 			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
-				transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
+				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
 
@@ -566,17 +562,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 }
 
 static struct vehicle_status_s status;
-
-/* armed topic */
+static struct vehicle_control_mode_s control_mode;
 static struct actuator_armed_s armed;
-
 static struct safety_s safety;
 
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
 	commander_initialized = false;
-	bool home_position_set = false;
 
 	bool battery_tune_played = false;
 	bool arm_tune_played = false;
@@ -590,6 +583,28 @@ int commander_thread_main(int argc, char *argv[])
 	/* welcome user */
 	warnx("starting");
 
+	char *main_states_str[MAIN_STATE_MAX];
+	main_states_str[0] = "MANUAL";
+	main_states_str[1] = "SEATBELT";
+	main_states_str[2] = "EASY";
+	main_states_str[3] = "AUTO";
+	main_states_str[4] = "OFFBOARD";
+
+	char *arming_states_str[ARMING_STATE_MAX];
+	arming_states_str[0] = "INIT";
+	arming_states_str[1] = "STANDBY";
+	arming_states_str[2] = "ARMED";
+	arming_states_str[3] = "ARMED_ERROR";
+	arming_states_str[4] = "STANDBY_ERROR";
+	arming_states_str[5] = "REBOOT";
+	arming_states_str[6] = "IN_AIR_RESTORE";
+
+	char *failsafe_states_str[FAILSAFE_STATE_MAX];
+	failsafe_states_str[0] = "NORMAL";
+	failsafe_states_str[1] = "RTL";
+	failsafe_states_str[2] = "LAND";
+	failsafe_states_str[3] = "TERMINATION";
+
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
 
@@ -604,21 +619,15 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	/* Main state machine */
-	/* make sure we are in preflight state */
+	/* vehicle status topic */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
-
-	/* armed topic */
-	orb_advert_t armed_pub;
-	/* Initialize armed with all false */
-	memset(&armed, 0, sizeof(armed));
-
 	status.main_state = MAIN_STATE_MANUAL;
 	status.set_nav_state = NAV_STATE_NONE;
 	status.set_nav_state_timestamp = 0;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
+	status.failsafe_state = FAILSAFE_STATE_NORMAL;
 
 	/* neither manual nor offboard control commands have been received */
 	status.offboard_control_signal_found_once = false;
@@ -635,14 +644,20 @@ int commander_thread_main(int argc, char *argv[])
 	// XXX for now just set sensors as initialized
 	status.condition_system_sensors_initialized = true;
 
-	/* advertise to ORB */
-	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
-	/* publish current state machine */
-
-	/* publish initial state */
 	status.counter++;
 	status.timestamp = hrt_absolute_time();
-	orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
+	/* publish initial state */
+	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+
+	/* armed topic */
+	orb_advert_t armed_pub;
+	/* Initialize armed with all false */
+	memset(&armed, 0, sizeof(armed));
+
+	/* vehicle control mode topic */
+	memset(&control_mode, 0, sizeof(control_mode));
+	orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
 
 	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 
@@ -703,10 +718,15 @@ int commander_thread_main(int argc, char *argv[])
 	struct manual_control_setpoint_s sp_man;
 	memset(&sp_man, 0, sizeof(sp_man));
 
+    /* Subscribe to offboard control data */
+    int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+    struct offboard_control_setpoint_s offboard_sp;
+    memset(&offboard_sp, 0, sizeof(offboard_sp));
+
 	/* Subscribe to offboard control data */
-	int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s offboard_sp;
-	memset(&offboard_sp, 0, sizeof(offboard_sp));
+	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+	struct offboard_control_setpoint_s sp_offboard;
+	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -820,12 +840,16 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
 		}
 
-		orb_check(offboard_sp_sub, &updated);
-
 		if (updated) {
 			orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp);
 		}
 
+		orb_check(sp_offboard_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+		}
+
 		orb_check(sensor_sub, &updated);
 
 		if (updated) {
@@ -862,7 +886,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update condition_global_position_valid */
-		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed);
 
 		/* update local position estimate */
 		orb_check(local_position_sub, &updated);
@@ -1018,19 +1042,18 @@ int commander_thread_main(int argc, char *argv[])
 			 * position to the current position.
 			 */
 
-			if (!home_position_set && gps_position.fix_type >= 3 &&
-			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
+			if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
+			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
 			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
-			    && global_position.valid) {
-				/* copy position data to uORB home message, store it locally as well */
-
+			    && global_position.global_valid) {
 
-				home.lat = (double)global_position.lat / 1e7d;
-				home.lon = (double)global_position.lon / 1e7d;
-				home.altitude = (float)global_position.alt;
+				/* copy position data to uORB home message, store it locally as well */
+				home.lat = global_position.lat;
+				home.lon = global_position.lon;
+				home.alt = global_position.alt;
 
-				warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
-				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
+				warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
 
 				/* announce new home position */
 				if (home_pub > 0) {
@@ -1041,13 +1064,13 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* mark home position as set */
-				home_position_set = true;
+				status.condition_home_position_valid = true;
 				tune_positive();
 			}
 		}
 
 		/* start RC input check */
-		if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+		if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
@@ -1122,24 +1145,27 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 			} else if (res == TRANSITION_DENIED) {
-				warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-				mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				/* 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 */
+				transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 			}
 
-			/* fill current_status according to mode switches */
+			/* fill status according to mode switches */
 			check_mode_switches(&sp_man, &status);
 
-			/* evaluate the main state machine */
-			res = check_main_state_machine(&status);
+			/* evaluate the main state machine according to mode switches */
+			res = set_main_state_rc(&status);
 
 			if (res == TRANSITION_CHANGED) {
-				//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
 				tune_positive();
 
 			} else if (res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
-				warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-				mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
 
 		} else {
@@ -1149,26 +1175,66 @@ int commander_thread_main(int argc, char *argv[])
 				status_changed = true;
 			}
 
-			if (status.main_state != MAIN_STATE_AUTO && armed.armed && status.main_state != MAIN_STATE_OFFBOARD) {
-				transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+			/* switch to OFFBOARD mode if offboard signal available */
+			transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD);
 
-				if (res == TRANSITION_CHANGED) {
-					mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
-					status.set_nav_state = NAV_STATE_RTL;
-					status.set_nav_state_timestamp = hrt_absolute_time();
+			if (res == TRANSITION_DENIED) {
+				/* can't switch to OFFBOARD, do normal failsafe if needed */
+				if (armed.armed) {
+					if (status.main_state == MAIN_STATE_AUTO) {
+						/* check if AUTO mode still allowed */
+						transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
 
-				} else if (status.main_state != MAIN_STATE_SEATBELT) {
-					res = main_state_transition(&status, MAIN_STATE_SEATBELT);
+						if (res == TRANSITION_DENIED) {
+							/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
+							res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
 
-					if (res == TRANSITION_CHANGED) {
-						mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
+							if (res == TRANSITION_DENIED) {
+								/* LAND not allowed, set TERMINATION state */
+								transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							}
+						}
+
+					} else if (status.main_state == MAIN_STATE_OFFBOARD) {
+						/* check if OFFBOARD mode still allowed */
+						transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD);
+
+						if (res == TRANSITION_DENIED) {
+							/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
+							res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
+							if (res == TRANSITION_DENIED) {
+								/* LAND not allowed, set TERMINATION state */
+								transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							}
+						}
+
+					} else {
+						/* failsafe for manual modes */
+						transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+
+						if (res == TRANSITION_DENIED) {
+							/* RTL not allowed (no global position estimate), try LAND */
+							res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
+							if (res == TRANSITION_DENIED) {
+								/* LAND not allowed, set TERMINATION state */
+								res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							}
+						}
+					}
+
+				} else {
+					if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+						/* reset failsafe when disarmed */
+						transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 					}
 				}
 			}
 		}
 
 		/* check offboard signal */
-		if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) {
+		if (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) {
 			if (!status.offboard_control_signal_found_once) {
 				status.offboard_control_signal_found_once = true;
 				mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time");
@@ -1189,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[])
 						transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
 
 						if (res == TRANSITION_CHANGED) {
-							mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
+								mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
 						}
 					}
 
@@ -1211,19 +1277,14 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/*  Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
-		if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
-			transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
-
-			if (flighttermination_res == TRANSITION_CHANGED) {
+		// TODO remove this hack
+		/* flight termination in manual mode if assisted switch is on easy position */
+		if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
 				tune_positive();
 			}
-
-		} else {
-			flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
 		}
 
-
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
 
@@ -1239,19 +1300,35 @@ 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 flighttermination_state_changed = check_flighttermination_state_changed();
+		bool failsafe_state_changed = check_failsafe_state_changed();
 
 		hrt_abstime t1 = hrt_absolute_time();
 
-		if (arming_state_changed || main_state_changed) {
-			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
+		/* print new state */
+		if (arming_state_changed) {
+			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
+		}
+
+		if (main_state_changed) {
+			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
+		}
+
+		if (failsafe_state_changed) {
 			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
 		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+			set_control_mode();
+			control_mode.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+
 			status.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
 			armed.timestamp = t1;
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
@@ -1317,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[])
 	led_deinit();
 	buzzer_deinit();
 	close(sp_man_sub);
-	close(offboard_sp_sub);
+	close(sp_offboard_sub);
 	close(local_position_sub);
 	close(global_position_sub);
 	close(gps_sub);
@@ -1424,133 +1501,235 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 }
 
 void
-check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
 {
 	/* main mode switch */
 	if (!isfinite(sp_man->mode_switch)) {
 		warnx("mode sw not finite");
-		current_status->mode_switch = MODE_SWITCH_MANUAL;
+		status->mode_switch = MODE_SWITCH_MANUAL;
 
 	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
-		current_status->mode_switch = MODE_SWITCH_AUTO;
+		status->mode_switch = MODE_SWITCH_AUTO;
 
 	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
-		current_status->mode_switch = MODE_SWITCH_MANUAL;
+		status->mode_switch = MODE_SWITCH_MANUAL;
 
 	} else {
-		current_status->mode_switch = MODE_SWITCH_ASSISTED;
+		status->mode_switch = MODE_SWITCH_ASSISTED;
 	}
 
 	/* return switch */
 	if (!isfinite(sp_man->return_switch)) {
-		current_status->return_switch = RETURN_SWITCH_NONE;
+		status->return_switch = RETURN_SWITCH_NONE;
 
 	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
-		current_status->return_switch = RETURN_SWITCH_RETURN;
+		status->return_switch = RETURN_SWITCH_RETURN;
 
 	} else {
-		current_status->return_switch = RETURN_SWITCH_NORMAL;
+		status->return_switch = RETURN_SWITCH_NORMAL;
 	}
 
 	/* assisted switch */
 	if (!isfinite(sp_man->assisted_switch)) {
-		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
 
 	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
-		current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+		status->assisted_switch = ASSISTED_SWITCH_EASY;
 
 	} else {
-		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
 	}
 
 	/* mission switch  */
 	if (!isfinite(sp_man->mission_switch)) {
-		current_status->mission_switch = MISSION_SWITCH_NONE;
+		status->mission_switch = MISSION_SWITCH_NONE;
 
 	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
-		current_status->mission_switch = MISSION_SWITCH_LOITER;
+		status->mission_switch = MISSION_SWITCH_LOITER;
 
 	} else {
-		current_status->mission_switch = MISSION_SWITCH_MISSION;
+		status->mission_switch = MISSION_SWITCH_MISSION;
 	}
 
-	/* offboard switch  */
+	 /* offboard switch  */
 	if (!isfinite(sp_man->offboard_switch)) {
-		current_status->offboard_switch = OFFBOARD_SWITCH_NONE;
+		status->offboard_switch = OFFBOARD_SWITCH_NONE;
 
 	} else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) {
-		current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD;
+		status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD;
 
 	} else {
-		current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD;
+		status->offboard_switch = OFFBOARD_SWITCH_ONBOARD;
 	}
 }
 
 transition_result_t
-check_main_state_machine(struct vehicle_status_s *current_status)
+set_main_state_rc(struct vehicle_status_s *status)
 {
-	/* evaluate the main state machine */
+	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
-	if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) {
-		/* offboard switch overrides main switch */
-		res = main_state_transition(current_status, MAIN_STATE_OFFBOARD);
+	/* offboard switch overrides main switch */
+	if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) {
+		res = main_state_transition(status, MAIN_STATE_OFFBOARD);
+		if (res == TRANSITION_DENIED) {
+			print_reject_mode("OFFBOARD");
 
-	} else {
-		switch (current_status->mode_switch) {
-		case MODE_SWITCH_MANUAL:
-			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-			// TRANSITION_DENIED is not possible here
-			break;
+		} else {
+			return res;
+		}
+	}
 
-		case MODE_SWITCH_ASSISTED:
-			if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
-				res = main_state_transition(current_status, MAIN_STATE_EASY);
+	/* offboard switched off or denied, check mode switch */
+	switch (status->mode_switch) {
+	case MODE_SWITCH_MANUAL:
+		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
 
-				if (res != TRANSITION_DENIED)
-					break;	// changed successfully or already in this state
+	case MODE_SWITCH_ASSISTED:
+		if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
+			res = main_state_transition(status, MAIN_STATE_EASY);
 
-				// else fallback to SEATBELT
-				print_reject_mode("EASY");
-			}
+			if (res != TRANSITION_DENIED)
+				break;	// changed successfully or already in this state
 
-			res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+			// else fallback to SEATBELT
+			print_reject_mode("EASY");
+		}
 
-			if (res != TRANSITION_DENIED)
-				break;	// changed successfully or already in this mode
+		res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-			if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
-				print_reject_mode("SEATBELT");
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this mode
 
-			// else fallback to MANUAL
-			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-			// TRANSITION_DENIED is not possible here
-			break;
+		if (status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+			print_reject_mode("SEATBELT");
 
-		case MODE_SWITCH_AUTO:
-			res = main_state_transition(current_status, MAIN_STATE_AUTO);
+		// else fallback to MANUAL
+		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
 
-			if (res != TRANSITION_DENIED)
-				break;	// changed successfully or already in this state
+	case MODE_SWITCH_AUTO:
+		res = main_state_transition(status, MAIN_STATE_AUTO);
 
-			// else fallback to SEATBELT (EASY likely will not work too)
-			print_reject_mode("AUTO");
-			res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
 
-			if (res != TRANSITION_DENIED)
-				break;	// changed successfully or already in this state
+		// else fallback to SEATBELT (EASY likely will not work too)
+		print_reject_mode("AUTO");
+		res = main_state_transition(status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
+
+		// else fallback to MANUAL
+		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	default:
+		break;
+	}
 
-			// else fallback to MANUAL
-			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-			// TRANSITION_DENIED is not possible here
+	return res;
+}
+
+void
+set_control_mode()
+{
+	/* set vehicle_control_mode according to main state and failsafe state */
+	control_mode.flag_armed = armed.armed;
+	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_SEATBELT:
+			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_EASY:
+			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;
+
 		default:
 			break;
 		}
+
+		break;
+
+	case FAILSAFE_STATE_RTL:
+		navigator_enabled = true;
+		break;
+
+	case FAILSAFE_STATE_LAND:
+		navigator_enabled = true;
+		break;
+
+	case FAILSAFE_STATE_TERMINATION:
+		/* disable all controllers on termination */
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = false;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_termination_enabled = true;
+		break;
+
+	default:
+		break;
 	}
 
-	return res;
+	/* 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;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+	}
 }
 
 void