From d7249cc1e6912480cf55a719459de460f3d11112 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Apr 2015 16:11:24 -0700 Subject: [PATCH] AP_AHRS: only use EKF for plane and rover if all data available we need position and velocity to be working for plane and rover --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 10c9074ade..f4ab513e96 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -20,6 +20,7 @@ */ #include #include +#include #if AP_AHRS_NAVEKF_AVAILABLE @@ -311,7 +312,25 @@ bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const bool AP_AHRS_NavEKF::using_EKF(void) const { - return ekf_started && _ekf_use && EKF.healthy(); + bool ret = ekf_started && _ekf_use && EKF.healthy(); + if (!ret) { + return false; + } +#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.const_pos_mode) { + return false; + } + if (!filt_state.flags.attitude || + !filt_state.flags.horiz_vel || + !filt_state.flags.vert_vel || + !filt_state.flags.horiz_pos_abs || + !filt_state.flags.vert_pos) { + return false; + } +#endif + return ret; } /*