From 141e2aae9154afa2153b9bbbc7057408a56ba760 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 19 Jul 2021 21:27:36 +1000 Subject: [PATCH] AP_AHRS: pass NavEKF failures back up to callers --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index e72322c217..5c620f9d86 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -2458,15 +2458,13 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: // use EKF to get innovations - EKF2.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); - return true; + return EKF2.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); #endif #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: // use EKF to get innovations - EKF3.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); - return true; + return EKF3.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); #endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -2503,8 +2501,7 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, case EKFType::TWO: { // use EKF to get variance Vector2f offset; - EKF2.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); - return true; + return EKF2.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); } #endif @@ -2512,8 +2509,7 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, case EKFType::THREE: { // use EKF to get variance Vector2f offset; - EKF3.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); - return true; + return EKF3.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); } #endif