Merge remote-tracking branch 'origin/obcfailsafe' into obcfailsafe

7 files changed, 70 insertions, 20 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index a82ec067f..28aba759f 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -554,10 +554,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("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;
 		}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index ecfe62e03..157e35ef8 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/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)) {
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 19f335633..6c5012bfd 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -58,14 +58,14 @@
 DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100),
-	_param_commsholdwaittime(this, "CH_T"),
-	_param_commsholdlat(this, "CH_LAT"),
-	_param_commsholdlon(this, "CH_LON"),
-	_param_commsholdalt(this, "CH_ALT"),
-	_param_airfieldhomelat(this, "AH_LAT"),
-	_param_airfieldhomelon(this, "AH_LON"),
-	_param_airfieldhomealt(this, "AH_ALT"),
-	_param_numberdatalinklosses(this, "DLL_N"),
+	_param_commsholdwaittime(this, "NAV_DLL_CH_T", false),
+	_param_commsholdlat(this, "NAV_DLL_CH_LAT", false),
+	_param_commsholdlon(this, "NAV_DLL_CH_LON", false),
+	_param_commsholdalt(this, "NAV_DLL_CH_ALT", false),
+	_param_airfieldhomelat(this, "NAV_DLL_AH_LAT", false),
+	_param_airfieldhomelon(this, "NAV_DLL_AH_LON", false),
+	_param_airfieldhomealt(this, "NAV_DLL_AH_ALT", false),
+	_param_numberdatalinklosses(this, "NAV_DLL_N", false),
 	_dll_state(DLL_STATE_NONE)
 {
 	/* load initial params */
@@ -81,7 +81,7 @@ DataLinkLoss::~DataLinkLoss()
 void
 DataLinkLoss::on_inactive()
 {
-	/* reset RTL state only if setpoint moved */
+	/* reset DLL state only if setpoint moved */
 	if (!_navigator->get_can_loiter_at_sp()) {
 		_dll_state = DLL_STATE_NONE;
 	}
@@ -90,7 +90,8 @@ DataLinkLoss::on_inactive()
 void
 DataLinkLoss::on_activation()
 {
-	_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+	_dll_state = DLL_STATE_NONE;
+	advance_dll();
 	set_dll_item();
 }
 
@@ -131,6 +132,10 @@ DataLinkLoss::set_dll_item()
 		_mission_item.origin = ORIGIN_ONBOARD;
 
 		_navigator->set_can_loiter_at_sp(true);
+		warnx("mission item: lat %.7f lon %.7f alt %.3f",
+			_mission_item.lat,
+			_mission_item.lon,
+			(double)_mission_item.altitude);
 		break;
 	}
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
@@ -159,6 +164,10 @@ DataLinkLoss::set_dll_item()
 	/* convert mission item to current position setpoint and make it valid */
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
+	warnx("triplet current: lat %.7f lon %.7f alt %.3f",
+			pos_sp_triplet->current.lat,
+			pos_sp_triplet->current.lon,
+			(double)pos_sp_triplet->current.alt);
 
 	_navigator->set_position_setpoint_triplet_updated();
 }
@@ -166,18 +175,21 @@ DataLinkLoss::set_dll_item()
 void
 DataLinkLoss::advance_dll()
 {
+	warnx("dll_state %u", _dll_state);
 	switch (_dll_state) {
 	case DLL_STATE_NONE:
 		/* Check the number of data link losses. If above home fly home directly */
 		updateSubscriptions();
 		if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) {
+			warnx("too many data link losses, fly to airfield home");
 			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		} else {
+			warnx("fly to comms hold");
 			_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
 		}
 		break;
 	case DLL_STATE_FLYTOCOMMSHOLDWP:
-		//XXX check here if time is over are over
+		warnx("fly to airfield home");
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
index 038c80a1a..77a8763cb 100644
--- a/src/modules/navigator/datalinkloss_params.c
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
  * @min 0.0
  * @group DLL
  */
-PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120);
+PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
 
 /**
  * Comms hold Lon
@@ -100,7 +100,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
  * @min 0.0
  * @group DLL
  */
-PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810);
+PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, -265847810);
 
 /**
  * Airfield home Lon
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index fe6639dfe..536977972 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -65,9 +65,8 @@
 
 /**
  * Number of navigation modes that need on_active/on_inactive calls
- * Currently: mission, loiter, and rtl
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 4
+#define NAVIGATOR_MODE_ARRAY_SIZE 6
 
 class Navigator : public control::SuperBlock
 {
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 043d883b2..18bfc2cec 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -141,6 +141,8 @@ Navigator::Navigator() :
 	_navigation_mode_array[1] = &_loiter;
 	_navigation_mode_array[2] = &_rtl;
 	_navigation_mode_array[3] = &_offboard;
+	_navigation_mode_array[4] = &_dataLinkLoss;
+	_navigation_mode_array[5] = &_engineFailure;
 
 	updateParams();
 }
@@ -384,7 +386,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) {
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 301503b82..b465f8407 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/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;