From 5e5775c3ca755e3a8627e81f69295cde347ad666 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 22 Apr 2019 13:53:47 +1000 Subject: [PATCH] AP_AHRS: ensure AHRS never uses an unhealthy gyro --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 75180f1aab..a04d1e8481 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -185,7 +185,7 @@ void AP_AHRS_NavEKF::update_EKF2(void) // calculate corrected gyro estimate for get_gyro() _gyro_estimate.zero(); - if (primary_imu == -1) { + if (primary_imu == -1 || !_ins.get_gyro_health(primary_imu)) { // the primary IMU is undefined so use an uncorrected default value from the INS library _gyro_estimate = _ins.get_gyro(); } else { @@ -258,7 +258,7 @@ void AP_AHRS_NavEKF::update_EKF3(void) // calculate corrected gyro estimate for get_gyro() _gyro_estimate.zero(); - if (primary_imu == -1) { + if (primary_imu == -1 || !_ins.get_gyro_health(primary_imu)) { // the primary IMU is undefined so use an uncorrected default value from the INS library _gyro_estimate = _ins.get_gyro(); } else {