From fdff6ea325fc25abb25a3d1d1fb017640edd7fb2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 6 Jun 2016 11:19:26 +0200 Subject: [PATCH] Revert "commander: remove unused/wrong failsafe handling" This reverts commit 9e704a5e121c7516e2133d02b328500d9d66fb67. --- src/modules/commander/commander.cpp | 52 +++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9d20a0ed0b..4ec4bcbc69 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2629,6 +2629,58 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check for failure combinations which lead to flight termination */ + if (armed.armed) { + /* At this point the data link and the gps system have been checked + * If we are not in a manual (RC stick controlled mode) + * and both failed we want to terminate the flight */ + if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL && + internal_state.main_state != commander_state_s::MAIN_STATE_ACRO && + internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE && + internal_state.main_state != commander_state_s::MAIN_STATE_STAB && + internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL && + internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL && + ((status.data_link_lost && status_flags.gps_failure) || + (status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + static bool flight_termination_printed = false; + + if (!flight_termination_printed) { + mavlink_and_console_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination"); + flight_termination_printed = true; + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + mavlink_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination"); + } + } + + /* At this point the rc signal and the gps system have been checked + * If we are in manual (controlled with RC): + * if both failed we want to terminate the flight */ + if ((internal_state.main_state == commander_state_s::MAIN_STATE_ACRO || + internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE || + internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || + internal_state.main_state == commander_state_s::MAIN_STATE_STAB || + internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || + internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status_flags.gps_failure) || + (status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + static bool flight_termination_printed = false; + + if (!flight_termination_printed) { + warnx("Flight termination because of RC signal loss and GPS failure"); + flight_termination_printed = true; + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + mavlink_log_critical(&mavlink_log_pub, "RC and GPS lost: flight termination"); + } + } + } /* Get current timestamp */ const hrt_abstime now = hrt_absolute_time();