|
|
|
@ -324,6 +324,13 @@ bool AP_AHRS_NavEKF::using_EKF(void) const
@@ -324,6 +324,13 @@ bool AP_AHRS_NavEKF::using_EKF(void) const
|
|
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_APMrover2) |
|
|
|
|
nav_filter_status filt_state; |
|
|
|
|
EKF.getFilterStatus(filt_state); |
|
|
|
|
if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _gps.status() >= AP_GPS::GPS_OK_FIX_3D) { |
|
|
|
|
// if the EKF is not fusing GPS and we have a 3D lock, then
|
|
|
|
|
// plane and rover would prefer to use the GPS position from
|
|
|
|
|
// DCM. This is a safety net while some issues with the EKF
|
|
|
|
|
// get sorted out
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|