Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

Conflicts:
	src/modules/navigator/navigator.h
	src/modules/navigator/navigator_main.cpp

src/modules/commander/commander.cpp

@@ -128,8 +128,6 @@ extern struct system_load_s system_load;
 
 #define POSITION_TIMEOUT		(2 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 600ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
-#define RC_TIMEOUT			500000
-#define DL_TIMEOUT			(10 * 1000 * 1000)
 #define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
 
@@ -555,10 +553,35 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			if (cmd->param1 > 0.5f) {
 				//XXX update state machine?
 				armed_local->force_failsafe = true;
-				warnx("forcing failsafe");
+				warnx("forcing failsafe (termination)");
 			} else {
 				armed_local->force_failsafe = false;
-				warnx("disabling failsafe");
+				warnx("disabling failsafe (termination)");
+			}
+			/* param2 is currently used for other failsafe modes */
+			status_local->engine_failure_cmd = false;
+			status_local->data_link_lost_cmd = false;
+			status_local->gps_failure_cmd = false;
+			status_local->rc_signal_lost_cmd = false;
+			if ((int)cmd->param2 <= 0) {
+				/* reset all commanded failure modes */
+				warnx("reset all non-flighttermination failsafe commands");
+			} else if ((int)cmd->param2 == 1) {
+				/* trigger engine failure mode */
+				status_local->engine_failure_cmd = true;
+				warnx("engine failure mode commanded");
+			} else if ((int)cmd->param2 == 2) {
+				/* trigger data link loss mode */
+				status_local->data_link_lost_cmd = true;
+				warnx("data link loss mode commanded");
+			} else if ((int)cmd->param2 == 3) {
+				/* trigger gps loss mode */
+				status_local->gps_failure_cmd = true;
+				warnx("gps loss mode commanded");
+			} else if ((int)cmd->param2 == 4) {
+				/* trigger rc loss mode */
+				status_local->rc_signal_lost_cmd = true;
+				warnx("rc loss mode commanded");
 			}
 			cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 		}
@@ -637,6 +660,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
 	case VEHICLE_CMD_PREFLIGHT_STORAGE:
+	case VEHICLE_CMD_CUSTOM_0:
+	case VEHICLE_CMD_CUSTOM_1:
+	case VEHICLE_CMD_CUSTOM_2:
+	case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
+	case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
 		/* ignore commands that handled in low prio loop */
 		break;
 
@@ -676,6 +704,12 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
 	param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
 	param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
+	param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
+	param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
+	param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
+	param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
+	param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
+	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
 
 	/* welcome user */
 	warnx("starting");
@@ -763,6 +797,8 @@ int commander_thread_main(int argc, char *argv[])
 	// CIRCUIT BREAKERS
 	status.circuit_breaker_engaged_power_check = false;
 	status.circuit_breaker_engaged_airspd_check = false;
+	status.circuit_breaker_engaged_enginefailure_check = false;
+	status.circuit_breaker_engaged_gpsfailure_check = false;
 
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -868,11 +904,13 @@ int commander_thread_main(int argc, char *argv[])
 	/* Subscribe to telemetry status topics */
 	int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
 	uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
+	uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM];
 	bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
 
 	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
 		telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
 		telemetry_last_heartbeat[i] = 0;
+		telemetry_last_dl_loss[i] = 0;
 		telemetry_lost[i] = true;
 	}
 
@@ -958,6 +996,15 @@ int commander_thread_main(int argc, char *argv[])
 	transition_result_t arming_ret;
 
 	int32_t datalink_loss_enabled = false;
+	int32_t datalink_loss_timeout = 10;
+	float rc_loss_timeout = 0.5;
+	int32_t datalink_regain_timeout = 0;
+
+	/* Thresholds for engine failure detection */
+	int32_t ef_throttle_thres = 1.0f;
+	int32_t ef_current2throttle_thres = 0.0f;
+	int32_t ef_time_thres = 1000.0f;
+	uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */
 
 	/* check which state machines for changes, clear "changed" flag */
 	bool arming_state_changed = false;
@@ -1005,8 +1052,14 @@ int commander_thread_main(int argc, char *argv[])
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
 
-				status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
-				status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
+				status.circuit_breaker_engaged_power_check =
+					circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+				status.circuit_breaker_engaged_airspd_check =
+					circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
+				status.circuit_breaker_engaged_enginefailure_check =
+					circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
+				status.circuit_breaker_engaged_gpsfailure_check =
+					circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
 
 				status_changed = true;
 
@@ -1018,6 +1071,12 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_takeoff_alt, &takeoff_alt);
 			param_get(_param_enable_parachute, &parachute_enabled);
 			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
+			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
+			param_get(_param_rc_loss_timeout, &rc_loss_timeout);
+			param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
+			param_get(_param_ef_throttle_thres, &ef_throttle_thres);
+			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
+			param_get(_param_ef_time_thres, &ef_time_thres);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -1058,7 +1117,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (mavlink_fd &&
 					telemetry_last_heartbeat[i] == 0 &&
 					telemetry.heartbeat_time > 0 &&
-					hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
+					hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
 
 					(void)rc_calibration_check(mavlink_fd);
 				}
@@ -1071,6 +1130,25 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+			/* Check if the barometer is healthy and issue a warning in the GCS if not so.
+			 * Because the barometer is used for calculating AMSL altitude which is used to ensure
+			 * vertical separation from other airtraffic the operator has to know when the
+			 * barometer is inoperational.
+			 * */
+			if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
+				/* handle the case where baro was regained */
+				if (status.barometer_failure) {
+					status.barometer_failure = false;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "baro healthy");
+				}
+			} else {
+				if (!status.barometer_failure) {
+					status.barometer_failure = true;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "baro failed");
+				}
+			}
 		}
 
 		orb_check(diff_pres_sub, &updated);
@@ -1157,8 +1235,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
 
-		/* check if GPS fix is ok */
-
 		/* update home position */
 		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
 		    (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
@@ -1370,15 +1446,51 @@ int commander_thread_main(int argc, char *argv[])
 			globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
 		}
 
+		/* check if GPS fix is ok */
+		if (status.circuit_breaker_engaged_gpsfailure_check ||
+				(gps_position.fix_type >= 3 &&
+				hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
+			/* handle the case where gps was regained */
+			if (status.gps_failure) {
+				status.gps_failure = false;
+				status_changed = true;
+				mavlink_log_critical(mavlink_fd, "gps regained");
+			}
+		} else {
+			if (!status.gps_failure) {
+				status.gps_failure = true;
+				status_changed = true;
+				mavlink_log_critical(mavlink_fd, "gps fix lost");
+			}
+		}
+
 		/* start mission result check */
 		orb_check(mission_result_sub, &updated);
 
 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+
+			/* Check for geofence violation */
+			if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
+				//XXX: make this configurable to select different actions (e.g. navigation modes)
+				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
+				armed.force_failsafe = true;
+				status_changed = true;
+				static bool flight_termination_printed = false;
+				if (!flight_termination_printed) {
+					warnx("Flight termination because of navigator request or geofence");
+					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+					flight_termination_printed = true;
+				}
+				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
+					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+				}
+			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}
 
 		/* RC input check */
-		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
+				hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
@@ -1489,15 +1601,25 @@ int commander_thread_main(int argc, char *argv[])
 		/* data links check */
 		bool have_link = false;
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-			if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
-				/* handle the case where data link was regained */
-				if (telemetry_lost[i]) {
+			if (telemetry_last_heartbeat[i] != 0 &&
+					hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
+				/* handle the case where data link was regained,
+				 * accept datalink as healthy only after datalink_regain_timeout seconds
+				 * */
+				if (telemetry_lost[i] &&
+					hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
+
 					mavlink_log_critical(mavlink_fd, "data link %i regained", i);
 					telemetry_lost[i] = false;
+					have_link = true;
+				} else if (!telemetry_lost[i]) {
+					/* telemetry was healthy also in last iteration
+					 * we don't have to check a timeout */
+					have_link = true;
 				}
-				have_link = true;
 
 			} else {
+				telemetry_last_dl_loss[i]  = hrt_absolute_time();
 				if (!telemetry_lost[i]) {
 					mavlink_log_critical(mavlink_fd, "data link %i lost", i);
 					telemetry_lost[i] = true;
@@ -1516,10 +1638,40 @@ int commander_thread_main(int argc, char *argv[])
 			if (!status.data_link_lost) {
 				mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
 				status.data_link_lost = true;
+				status.data_link_lost_counter++;
 				status_changed = true;
 			}
 		}
 
+		/* Check engine failure
+		 * only for fixed wing for now
+		 */
+		if (!status.circuit_breaker_engaged_enginefailure_check &&
+				status.is_rotary_wing == false &&
+				armed.armed &&
+				((actuator_controls.control[3] > ef_throttle_thres &&
+				battery.current_a/actuator_controls.control[3] <
+				ef_current2throttle_thres) ||
+				 (status.engine_failure))) {
+			/* potential failure, measure time */
+			if (timestamp_engine_healthy > 0 &&
+					hrt_elapsed_time(&timestamp_engine_healthy) >
+					ef_time_thres * 1e6 &&
+					!status.engine_failure) {
+					status.engine_failure = true;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "Engine Failure");
+			}
+		} else {
+			/* no failure reset flag */
+			timestamp_engine_healthy = hrt_absolute_time();
+			if (status.engine_failure) {
+				status.engine_failure = false;
+				status_changed = true;
+			}
+		}
+
+
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
 
@@ -1533,6 +1685,53 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* Check for failure combinations which lead to flight termination */
+		if (armed.armed) {
+			/* At this point the data link and the gps system have been checked
+			 * If  we are not in a manual (RC stick controlled mode)
+			 * and both failed we want to terminate the flight */
+			if (status.main_state != MAIN_STATE_MANUAL &&
+					status.main_state != MAIN_STATE_ACRO &&
+					status.main_state != MAIN_STATE_ALTCTL &&
+					status.main_state != MAIN_STATE_POSCTL &&
+					((status.data_link_lost && status.gps_failure) ||
+					(status.data_link_lost_cmd && status.gps_failure_cmd))) {
+				armed.force_failsafe = true;
+				status_changed = true;
+				static bool flight_termination_printed = false;
+				if (!flight_termination_printed) {
+					warnx("Flight termination because of data link loss && gps failure");
+					mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
+					flight_termination_printed = true;
+				}
+				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
+					mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
+				}
+			}
+
+			/* At this point the rc signal and the gps system have been checked
+			 * If we are in manual (controlled with RC):
+			 * if both failed we want to terminate the flight */
+			if ((status.main_state == MAIN_STATE_ACRO ||
+					status.main_state == MAIN_STATE_MANUAL ||
+					status.main_state == MAIN_STATE_ALTCTL ||
+					status.main_state == MAIN_STATE_POSCTL) &&
+					((status.rc_signal_lost && status.gps_failure) ||
+					(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
+				armed.force_failsafe = true;
+				status_changed = true;
+				static bool flight_termination_printed = false;
+				if (!flight_termination_printed) {
+					warnx("Flight termination because of RC signal loss && gps failure");
+					flight_termination_printed = true;
+				}
+				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
+					mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination");
+				}
+			}
+		}
+
+
 		hrt_abstime t1 = hrt_absolute_time();
 
 		/* print new state */
@@ -1574,7 +1773,8 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* now set navigation state according to failsafe and main state */
 		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
-						       mission_result.finished);
+						       mission_result.finished,
+						       mission_result.stay_in_failsafe);
 
 		// TODO handle mode changes by commands
 		if (main_state_changed) {
@@ -2024,6 +2224,7 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
 	case NAVIGATION_STATE_AUTO_RTGS:
+	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
@@ -2035,6 +2236,18 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+		control_mode.flag_control_manual_enabled = false;
+		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 = false;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
 	case NAVIGATION_STATE_LAND:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;

src/modules/commander/commander_params.c

@@ -105,3 +105,69 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
  * @max 1
  */
 PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
+
+ /** Datalink loss time threshold
+ *
+ * After this amount of seconds without datalink the data link lost mode triggers
+ *
+ * @group commander
+ * @unit second
+ * @min 0
+ * @max 30
+ */
+PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
+
+/** Datalink regain time threshold
+ *
+ * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
+ * flag is set back to false
+ *
+ * @group commander
+ * @unit second
+ * @min 0
+ * @max 30
+ */
+PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
+
+/** Engine Failure Throttle Threshold
+ *
+ * Engine failure triggers only above this throttle value
+ *
+ * @group commander
+ * @min 0.0f
+ * @max 1.0f
+ */
+PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
+
+/** Engine Failure Current/Throttle Threshold
+ *
+ * Engine failure triggers only below this current/throttle value
+ *
+ * @group commander
+ * @min 0.0f
+ * @max 7.0f
+ */
+PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
+
+/** Engine Failure Time Threshold
+ *
+ * Engine failure triggers only if the throttle threshold and the
+ * current to throttle threshold are violated for this time
+ *
+ * @group commander
+ * @unit second
+ * @min 0.0f
+ * @max 7.0f
+ */
+PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
+
+/** RC loss time threshold
+ *
+ * After this amount of seconds without RC connection the rc lost flag is set to true
+ *
+ * @group commander
+ * @unit second
+ * @min 0
+ * @max 35
+ */
+PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5);

src/modules/commander/commander_tests/commander_tests.cpp

@@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
 
 int commander_tests_main(int argc, char *argv[])
 {
-	stateMachineHelperTest();
-
-	return 0;
+	return stateMachineHelperTest() ? 0 : -1;
 }

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -49,7 +49,7 @@ public:
 	StateMachineHelperTest();
 	virtual ~StateMachineHelperTest();
 
-	virtual void runTests(void);
+	virtual bool run_tests(void);
 
 private:
 	bool armingStateTransitionTest();
@@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void)
 	return true;
 }
 
-void StateMachineHelperTest::runTests(void)
+bool StateMachineHelperTest::run_tests(void)
 {
 	ut_run_test(armingStateTransitionTest);
 	ut_run_test(mainStateTransitionTest);
 	ut_run_test(isSafeTest);
+	
+	return (_tests_failed == 0);
 }
 
-void stateMachineHelperTest(void)
-{
-	StateMachineHelperTest* test = new StateMachineHelperTest();
-    test->runTests();
-	test->printResults();
-}
+ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)

src/modules/commander/commander_tests/state_machine_helper_test.h

@@ -39,6 +39,6 @@
 #ifndef STATE_MACHINE_HELPER_TEST_H_
 #define STATE_MACHINE_HELPER_TEST_
 
-void stateMachineHelperTest(void);
+bool stateMachineHelperTest(void);
 
 #endif /* STATE_MACHINE_HELPER_TEST_H_ */

src/modules/commander/state_machine_helper.cpp

@@ -443,7 +443,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 /**
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
-bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
+		const bool stay_in_failsafe)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -457,11 +458,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	case MAIN_STATE_ALTCTL:
 	case MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
-		if (status->rc_signal_lost && armed) {
+		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -497,14 +498,29 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 	case MAIN_STATE_AUTO_MISSION:
 		/* go into failsafe
+		 * - if commanded to do so
+		 * - if we have an engine failure
 		 * - if either the datalink is enabled and lost as well as RC is lost
 		 * - if there is no datalink and the mission is finished */
-		if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
+		if (status->engine_failure_cmd) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if (status->data_link_lost_cmd) {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+		} else if (status->gps_failure_cmd) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+		} else if (status->rc_signal_lost_cmd) {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
+		/* Finished handling commands which have priority , now handle failures */
+		} else if (status->gps_failure) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+		} else if (status->engine_failure) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
 		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -528,31 +544,20 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			}
 
 		/* don't bother if RC is lost and mission is not yet finished */
-		} else if (status->rc_signal_lost) {
+		} else if (status->rc_signal_lost && !stay_in_failsafe) {
 
 			/* this mode is ok, we don't need RC for missions */
 			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
-		} else {
+		} else if (!stay_in_failsafe){
 			/* everything is perfect */
 			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
 
 	case MAIN_STATE_AUTO_LOITER:
-		/* go into failsafe if datalink and RC is lost */
-		if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
-			status->failsafe = true;
-
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
-			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
-			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
-			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
-			}
-
+		/* go into failsafe on a engine failure */
+		if (status->engine_failure) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		/* also go into failsafe if just datalink is lost */
 		} else if (status->data_link_lost && data_link_loss_enabled) {
 			status->failsafe = true;
@@ -593,8 +598,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		break;
 
 	case MAIN_STATE_AUTO_RTL:
-		/* require global position and home */
-		if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
+		/* require global position and home, also go into failsafe on an engine failure */
+
+		if (status->engine_failure) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if ((!status->condition_global_position_valid ||
+					!status->condition_home_position_valid)) {
 			status->failsafe = true;
 
 			if (status->condition_local_position_valid) {

src/modules/commander/state_machine_helper.h

@@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
-bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
 
 int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);