From 18c5daaa38b3e5f1855a7b680f10e9e9caa58291 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 12 Jan 2024 13:51:47 +1100 Subject: [PATCH] AP_AHRS: use a switch statement when falling back to DCM --- libraries/AP_AHRS/AP_AHRS.cpp | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index d0985bd803..6be34de803 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1932,35 +1932,38 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const #if AP_AHRS_DCM_ENABLED // Handle fallback for fixed wing planes (including VTOL's) and ground vehicles. - if (ret != EKFType::DCM && - (_vehicle_class == VehicleClass::FIXED_WING || - _vehicle_class == VehicleClass::GROUND)) { - + if (_vehicle_class == VehicleClass::FIXED_WING || + _vehicle_class == VehicleClass::GROUND) { bool should_use_gps = true; - nav_filter_status filt_state; + nav_filter_status filt_state {}; + switch (ret) { + case EKFType::DCM: + // already using DCM + break; #if HAL_NAVEKF2_AVAILABLE - if (ret == EKFType::TWO) { + case EKFType::TWO: EKF2.getFilterStatus(filt_state); should_use_gps = EKF2.configuredToUseGPSForPosXY(); - } + break; #endif #if HAL_NAVEKF3_AVAILABLE - if (ret == EKFType::THREE) { + case EKFType::THREE: EKF3.getFilterStatus(filt_state); should_use_gps = EKF3.configuredToUseGPSForPosXY(); - } + break; #endif #if AP_AHRS_SIM_ENABLED - if (ret == EKFType::SIM) { + case EKFType::SIM: get_filter_status(filt_state); - } + break; #endif #if HAL_EXTERNAL_AHRS_ENABLED - if (ret == EKFType::EXTERNAL) { + case EKFType::EXTERNAL: get_filter_status(filt_state); should_use_gps = true; - } + break; #endif + } // Handle fallback for the case where the DCM or EKF is unable to provide attitude or height data. const bool can_use_dcm = dcm.yaw_source_available() || fly_forward;