Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

6 files changed, 142 insertions, 50 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index f8dcec8cb..5954dcbb1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -619,6 +619,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	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");
 
 	/* welcome user */
 	warnx("starting");
@@ -627,9 +628,9 @@ int commander_thread_main(int argc, char *argv[])
 	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
 	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
 	main_states_str[MAIN_STATE_POSCTL]			= "POSCTL";
-	main_states_str[MAIN_STATE_AUTO_MISSION]	= "AUTO_MISSION";
-	main_states_str[MAIN_STATE_AUTO_LOITER]		= "AUTO_LOITER";
-	main_states_str[MAIN_STATE_AUTO_RTL]		= "AUTO_RTL";
+	main_states_str[MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	main_states_str[MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
+	main_states_str[MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
 	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
@@ -637,21 +638,23 @@ int commander_thread_main(int argc, char *argv[])
 	arming_states_str[ARMING_STATE_STANDBY]			= "STANDBY";
 	arming_states_str[ARMING_STATE_ARMED]			= "ARMED";
 	arming_states_str[ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
-	arming_states_str[ARMING_STATE_STANDBY_ERROR]	= "STANDBY_ERROR";
+	arming_states_str[ARMING_STATE_STANDBY_ERROR]		= "STANDBY_ERROR";
 	arming_states_str[ARMING_STATE_REBOOT]			= "REBOOT";
-	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]	= "IN_AIR_RESTORE";
+	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
 
 	char *nav_states_str[NAVIGATION_STATE_MAX];
-	nav_states_str[NAVIGATION_STATE_MANUAL]		= "MANUAL";
-	nav_states_str[NAVIGATION_STATE_ALTCTL]		= "ALTCTL";
-	nav_states_str[NAVIGATION_STATE_POSCTL]		= "POSCTL";
-	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]	= "AUTO_MISSION";
-	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]	= "AUTO_LOITER";
+	nav_states_str[NAVIGATION_STATE_MANUAL]			= "MANUAL";
+	nav_states_str[NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
+	nav_states_str[NAVIGATION_STATE_POSCTL]			= "POSCTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS]	= "AUTO_FS_RC_LOSS";
+	nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS]	= "AUTO_FS_DL_LOSS";
 	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
 	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
 	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
-	nav_states_str[NAVIGATION_STATE_TERMINATION]	= "TERMINATION";
+	nav_states_str[NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -853,6 +856,8 @@ int commander_thread_main(int argc, char *argv[])
 
 	transition_result_t arming_ret;
 
+	int32_t datalink_loss_enabled = false;
+
 	/* check which state machines for changes, clear "changed" flag */
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
@@ -907,6 +912,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* navigation parameters */
 			param_get(_param_takeoff_alt, &takeoff_alt);
 			param_get(_param_enable_parachute, &parachute_enabled);
+			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -1309,8 +1315,8 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* data link check */
-		if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) {
-			/* handle the case where RC signal was regained */
+		if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
+			/* handle the case where data link was regained */
 			if (status.data_link_lost) {
 				mavlink_log_critical(mavlink_fd, "#audio: data link regained");
 				status.data_link_lost = false;
@@ -1320,7 +1326,7 @@ int commander_thread_main(int argc, char *argv[])
 		} else {
 			if (!status.data_link_lost) {
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
-				status.data_link_lost = false;
+				status.data_link_lost = true;
 				status_changed = true;
 			}
 		}
@@ -1378,7 +1384,8 @@ int commander_thread_main(int argc, char *argv[])
 		was_armed = armed.armed;
 
 		/* now set navigation state according to failsafe and main state */
-		bool nav_state_changed = set_nav_state(&status);
+		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
+						       mission_result.mission_finished);
 
 		// TODO handle mode changes by commands
 		if (main_state_changed) {
@@ -1727,6 +1734,8 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
+	case NAVIGATION_STATE_AUTO_FS_DL_LOSS:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 80ca68f21..4750f9d5c 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -39,7 +39,7 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <nuttx/config.h>
@@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
  * @group Battery Calibration
  */
 PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
+
+/**
+ * Datalink loss mode enabled.
+ *
+ * Set to 1 to enable actions triggered when the datalink is lost.
+ *
+ * @group commander
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index c0d730949..f0fe13921 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -366,7 +366,7 @@ 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)
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -383,6 +383,16 @@ bool set_nav_state(struct vehicle_status_s *status)
 		if (status->rc_signal_lost && armed) {
 			status->failsafe = true;
 
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
+			} 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;
+			}
+
 		} else {
 			switch (status->main_state) {
 			case MAIN_STATE_ACRO:
@@ -409,45 +419,116 @@ bool set_nav_state(struct vehicle_status_s *status)
 		break;
 
 	case MAIN_STATE_AUTO_MISSION:
-		/* require data link and global position */
-		if ((status->data_link_lost || !status->condition_global_position_valid) && armed) {
+		/* go into failsafe
+		 * - 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) ||
+		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
 			status->failsafe = true;
 
-		} else {
-			if (armed) {
-				status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
+			} 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;
+			}
+
+		/* also go into failsafe if just datalink is lost */
+		} else if (status->data_link_lost && data_link_loss_enabled) {
+			status->failsafe = true;
 
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
+			} 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 {
-				// TODO which mode should we set when disarmed?
-				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
+
+		/* don't bother if RC is lost and mission is not yet finished */
+		} else if (status->rc_signal_lost) {
+
+			/* this mode is ok, we don't need RC for missions */
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		} else {
+			/* everything is perfect */
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
 
 	case MAIN_STATE_AUTO_LOITER:
-		/* require data link and local position */
-		if ((status->data_link_lost || !status->condition_local_position_valid) && armed) {
+		/* 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_FS_RC_LOSS;
+			} 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;
+			}
+
+		/* also go into failsafe if just datalink is lost */
+		} else if (status->data_link_lost && data_link_loss_enabled) {
 			status->failsafe = true;
 
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
+			} 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 if RC is lost and datalink loss is not set up */
+		} else if (status->rc_signal_lost && !data_link_loss_enabled) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
+			} 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;
+			}
+
+		/* don't bother if RC is lost if datalink is connected */
+		} else if (status->rc_signal_lost) {
+
+			/* this mode is ok, we don't need RC for loitering */
+			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 		} else {
-			// TODO which mode should we set when disarmed?
+			/* everything is perfect */
 			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 		}
 		break;
 
 	case MAIN_STATE_AUTO_RTL:
 		/* require global position and home */
-		if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) {
+		if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
 			status->failsafe = true;
 
-		} else {
-			if (armed) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
-
+			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 {
-				// TODO which mode should we set when disarmed?
-				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
 		}
 		break;
 
@@ -455,21 +536,6 @@ bool set_nav_state(struct vehicle_status_s *status)
 		break;
 	}
 
-	if (status->failsafe) {
-		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;
-		}
-	}
-
 	return status->nav_state != nav_state_old;
 }
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index cffbc9b8f..2e076cdae 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -65,6 +65,6 @@ 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);
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 70da5393f..50bcfd58d 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -351,8 +351,12 @@ Navigator::task_main()
 				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
+			case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
 				_navigation_mode = &_rtl;
 				break;
+			case NAVIGATION_STATE_AUTO_FS_DL_LOSS:
+				_navigation_mode = &_rtl; /* TODO: change this to something else */
+				break;
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
 			default:
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 851938795..16652fddf 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -101,6 +101,8 @@ typedef enum {
 	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
 	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
 	NAVIGATION_STATE_AUTO_RTL,		/**< Auto RTL mode */
+	NAVIGATION_STATE_AUTO_FS_RC_LOSS,	/**< Auto failsafe mode on RC loss */
+	NAVIGATION_STATE_AUTO_FS_DL_LOSS,	/**< Auto failsafe mode on DL loss */
 	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
 	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */