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 ||