add method to block fallback to mission

failsafe navigation modes can use a flag in mission_result to tell the
commander to not switch back to mission

3 files changed, 6 insertions, 4 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5c89e0123..cb09a68e3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1461,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[])
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
 			if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) {
 				/* handle the case where data link was regained */
-				if (telemetry_lost[i]) {
+				if (telemetry_lost[i]) {//XXX also add hysteresis here
 					mavlink_log_critical(mavlink_fd, "data link %i regained", i);
 					telemetry_lost[i] = false;
 				}
@@ -1545,7 +1545,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) {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 7b26e3e8c..4e1cfb987 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -435,7 +435,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;
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index bb1b87e71..4285d2977 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/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);