Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: use a switch statement when falling back to DCM

This commit is contained in:
Peter Barker 2024-01-12 13:51:47 +11:00 committed by Andrew Tridgell
parent a77df87b01
commit 18c5daaa38
1 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -1932,35 +1932,38 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const
#if AP_AHRS_DCM_ENABLED #if AP_AHRS_DCM_ENABLED
// Handle fallback for fixed wing planes (including VTOL's) and ground vehicles. // Handle fallback for fixed wing planes (including VTOL's) and ground vehicles.
if (ret != EKFType::DCM && if (_vehicle_class == VehicleClass::FIXED_WING ||
(_vehicle_class == VehicleClass::FIXED_WING || _vehicle_class == VehicleClass::GROUND) {
_vehicle_class == VehicleClass::GROUND)) {
bool should_use_gps = true; 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 HAL_NAVEKF2_AVAILABLE
if (ret == EKFType::TWO) { case EKFType::TWO:
EKF2.getFilterStatus(filt_state); EKF2.getFilterStatus(filt_state);
should_use_gps = EKF2.configuredToUseGPSForPosXY(); should_use_gps = EKF2.configuredToUseGPSForPosXY();
} break;
#endif #endif
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
if (ret == EKFType::THREE) { case EKFType::THREE:
EKF3.getFilterStatus(filt_state); EKF3.getFilterStatus(filt_state);
should_use_gps = EKF3.configuredToUseGPSForPosXY(); should_use_gps = EKF3.configuredToUseGPSForPosXY();
} break;
#endif #endif
#if AP_AHRS_SIM_ENABLED #if AP_AHRS_SIM_ENABLED
if (ret == EKFType::SIM) { case EKFType::SIM:
get_filter_status(filt_state); get_filter_status(filt_state);
} break;
#endif #endif
#if HAL_EXTERNAL_AHRS_ENABLED #if HAL_EXTERNAL_AHRS_ENABLED
if (ret == EKFType::EXTERNAL) { case EKFType::EXTERNAL:
get_filter_status(filt_state); get_filter_status(filt_state);
should_use_gps = true; should_use_gps = true;
} break;
#endif #endif
}
// Handle fallback for the case where the DCM or EKF is unable to provide attitude or height data. // 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; const bool can_use_dcm = dcm.yaw_source_available() || fly_forward;