From 72f2317c95c4cfa13906e1713fcb03a67cf1afa6 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 9 Aug 2018 15:06:13 +0200 Subject: [PATCH] Failure Detector - move "force_failsafe" flag inside circuit_breaker protection; remove PX4_WARN and send mavlink msg only once --- src/modules/commander/Commander.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 2e8a3c6715..afa8618825 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2294,20 +2294,17 @@ Commander::run() if (failure_status.roll || failure_status.pitch) { - armed.force_failsafe = true; status.attitude_failure = true; status_changed = true; // Only display an user message if the circuit-breaker is disabled if (!status_flags.circuit_breaker_flight_termination_disabled) { - if (!_failure_detector_termination_printed) { - PX4_WARN("Flight termination because of attitude exceeding maximum values"); - _failure_detector_termination_printed = true; - } + armed.force_failsafe = true; - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + if (!_failure_detector_termination_printed) { mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected: force failsafe"); + _failure_detector_termination_printed = true; } } }