failsafe: enable support for commands

src/modules/commander/commander.cpp

@@ -557,6 +557,31 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				armed_local->force_failsafe = false;
 				warnx("disabling failsafe");
 			}
+			/* 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("revert to normal mode");
+			} 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;
 		}
 		break;

src/modules/commander/state_machine_helper.cpp

@@ -491,10 +491,19 @@ 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->engine_failure) {
+		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_***;
+		} else if (status->rc_signal_lost_cmd) {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
+		} 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)) {

src/modules/navigator/navigator_main.cpp

@@ -384,7 +384,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_RTL:
 				_navigation_mode = &_rtl;
 				break;
-			case NAVIGATION_STATE_AUTO_RTGS:
+			case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
 				if (_param_datalinkloss_obc.get() != 0) {

src/modules/uORB/topics/vehicle_status.h

@@ -200,13 +200,16 @@ struct vehicle_status_s {
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception lost */
+	bool rc_signal_lost_cmd;				/**< true if RC lost mode is commanded */
 	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
 	bool data_link_lost;				/**< datalink to GCS lost */
+	bool data_link_lost_cmd;			/**< datalink to GCS lost mode commanded */
 	uint8_t data_link_lost_counter;			/**< counts unique data link lost events */
-
 	bool engine_failure;				/** Set to true if an engine failure is detected */
+	bool engine_failure_cmd;			/** Set to true if an engine failure mode is commanded */
 	bool gps_failure;				/** Set to true if a gps failure is detected */
+	bool gps_failure_cmd;				/** Set to true if a gps failure mode is commanded */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;