From 664ce5c16ef1d88ff5eb3e0ae6584af66eee3499 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 1 May 2016 22:16:06 +1000 Subject: [PATCH] AP_AHRS: don't use disabled gyro in rate controllers obey INS_USE* parameters in gyro estimate --- 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 68c860d911..091c176a3a 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -143,7 +143,7 @@ void AP_AHRS_NavEKF::update_EKF1(void) _gyro_estimate.zero(); uint8_t healthy_count = 0; for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { - if (_ins.get_gyro_health(i) && healthy_count < 2) { + if (_ins.get_gyro_health(i) && healthy_count < 2 && _ins.use_gyro(i)) { _gyro_estimate += _ins.get_gyro(i); healthy_count++; } @@ -214,7 +214,7 @@ void AP_AHRS_NavEKF::update_EKF2(void) _gyro_estimate.zero(); uint8_t healthy_count = 0; for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { - if (_ins.get_gyro_health(i) && healthy_count < 2) { + if (_ins.get_gyro_health(i) && healthy_count < 2 && _ins.use_gyro(i)) { _gyro_estimate += _ins.get_gyro(i); healthy_count++; }