fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 07:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

attitude_q : more verbose failsafe output

Browse Source
This commit is contained in:
Kabir Mohammed
2015-11-12 12:18:19 +05:30
parent 578b250069
commit 0f489a194f
2 changed files with 19 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+18 -5
View File
@@ -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 ||