From 0f489a194fe066848fe92add6cdcdeeca143d0d3 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Thu, 12 Nov 2015 12:18:19 +0530 Subject: [PATCH] attitude_q : more verbose failsafe output --- .../ecl/validation/data_validator_group.cpp | 2 +- .../attitude_estimator_q_main.cpp | 23 +++++++++++++++---- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index ac4bcc8b9b..e82e476416 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -199,7 +199,7 @@ void DataValidatorGroup::print() { /* print the group's state */ - ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)", + ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", _toggle_count); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 1531e32778..b62c3b6cc5 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -369,12 +369,25 @@ void AttitudeEstimatorQ::task_main() _data_good = true; - if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { + if (!_failsafe) { + if (_voter_gyro.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Gyro failure!"); + } - _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + if (_voter_accel.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Accel failure!"); + } + + if (_voter_mag.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Mag failure!"); + } + + if (_failsafe) { + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } } if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||