Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: active_EKF_type fallback to DCM checks if GPS configured for use

Browse Source
This commit is contained in:
Randy Mackay 2020-11-24 14:55:24 +09:00 committed by Andrew Tridgell
parent adfc92523b
commit 8c2688605e
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1254,15 +1254,18 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const
(_vehicle_class == AHRS_VEHICLE_FIXED_WING || (_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND) && _vehicle_class == AHRS_VEHICLE_GROUND) &&
(_flags.fly_forward || !hal.util->get_soft_armed())) { (_flags.fly_forward || !hal.util->get_soft_armed())) {
bool should_use_gps = true;
nav_filter_status filt_state; nav_filter_status filt_state;
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
if (ret == EKFType::TWO) { if (ret == EKFType::TWO) {
EKF2.getFilterStatus(-1,filt_state); EKF2.getFilterStatus(-1,filt_state);
should_use_gps = EKF2.configuredToUseGPSForPosXY();
} }
#endif #endif
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
if (ret == EKFType::THREE) { if (ret == EKFType::THREE) {
EKF3.getFilterStatus(-1,filt_state); EKF3.getFilterStatus(-1,filt_state);
should_use_gps = EKF3.configuredToUseGPSForPosXY();
} }
#endif #endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -1273,6 +1276,7 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const
if (hal.util->get_soft_armed() && if (hal.util->get_soft_armed() &&
(!filt_state.flags.using_gps || (!filt_state.flags.using_gps ||
!filt_state.flags.horiz_pos_abs) && !filt_state.flags.horiz_pos_abs) &&
should_use_gps &&
AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) { AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// if the EKF is not fusing GPS or doesn't have a 2D fix // if the EKF is not fusing GPS or doesn't have a 2D fix
// and we have a 3D lock, then plane and rover would // and we have a 3D lock, then plane and rover would