Merge pull request #909 from PX4/failsafe_reporting

Much better failsafe reporting on failsafe state changes

1 files changed, 17 insertions, 0 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index aa4e643dc..dfab9d4d6 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1285,6 +1285,13 @@ int commander_thread_main(int argc, char *argv[])
 						if (res == TRANSITION_DENIED) {
 							/* LAND not allowed, set TERMINATION state */
 							transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+
+							if (res == TRANSITION_CHANGED) {
+								mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
+							}
+
+						} else if (res == TRANSITION_CHANGED) {
+							mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING");
 						}
 					}
 
@@ -1295,15 +1302,25 @@ int commander_thread_main(int argc, char *argv[])
 					if (!status.condition_landed) {
 						/* vehicle is not landed, try to perform RTL */
 						res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+
+						if (res == TRANSITION_CHANGED) {
+							mavlink_log_critical(mavlink_fd, "#a FAILSAFE: RETURN TO LAND");
+						}
 					}
 
 					if (res == TRANSITION_DENIED) {
 						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
 						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
 
+						if (res == TRANSITION_CHANGED) {
+							mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING");
+						}
+
 						if (res == TRANSITION_DENIED) {
 							/* LAND not allowed, set TERMINATION state */
 							res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+						} else if (res == TRANSITION_CHANGED) {
+							mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
 						}
 					}
 				}