From c75862d0e60df9c80410195bad056623cf0dfd94 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 5 Jul 2019 12:40:48 +1000 Subject: [PATCH] AP_NavEKF3: don't use disabled gyros in opticalflow takeoff detection --- libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 4a479a2803..01c564ea13 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -455,7 +455,7 @@ void NavEKF3_core::detectOptFlowTakeoff(void) Vector3f angRateVec; Vector3f gyroBias; getGyroBias(gyroBias); - bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1); + bool dual_ins = ins.use_gyro(0) && ins.use_gyro(1); if (dual_ins) { angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias; } else {