diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 63ff4ab5fc..92ef73f0bd 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -876,6 +876,7 @@ private: void init_ardupilot(); void startup_ground(bool force_gyro_cal); bool position_ok(); + bool ekf_position_ok(); bool optflow_position_ok(); void update_auto_armed(); void check_usb_mux(void); diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index e908244ee6..10dfcdc6b4 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -91,7 +91,7 @@ bool Copter::ekf_over_threshold() } // return true immediately if position is bad - if (!position_ok() && !optflow_position_ok()) { + if (!ekf_position_ok() && !optflow_position_ok()) { return true; } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index d0b59ee660..c05006bd99 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -309,13 +309,20 @@ void Copter::startup_ground(bool force_gyro_cal) // position_ok - returns true if the horizontal absolute position is ok and home position is set bool Copter::position_ok() { - if (!ahrs.have_inertial_nav()) { - // do not allow navigation with dcm position + // return false if ekf failsafe has triggered + if (failsafe.ekf) { return false; } - // return false if ekf failsafe has triggered - if (failsafe.ekf) { + // check ekf position estimate + return ekf_position_ok(); +} + +// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set +bool Copter::ekf_position_ok() +{ + if (!ahrs.have_inertial_nav()) { + // do not allow navigation with dcm position return false; }