From edffc3001b434b20d55b74173f949ebfef204c39 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 3b2796d634..3cfa419cb2 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -180,7 +180,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 { @@ -253,7 +253,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 {