diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index b85c142ead..8e7cac478b 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -972,6 +972,7 @@ private: void ekf_check(); bool ekf_over_threshold(); + bool ekf_check_position_problem(); void failsafe_ekf_event(); void failsafe_ekf_off_event(void); void esc_calibration_startup_check(); diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 1654de4be6..b8c5f34ec8 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -44,7 +44,7 @@ void Copter::ekf_check() } // compare compass and velocity variance vs threshold - if (ekf_over_threshold()) { + if (ekf_over_threshold() || ekf_check_position_problem()) { // if compass is not yet flagged as bad if (!ekf_check_state.bad_variance) { // increase counter @@ -86,6 +86,21 @@ void Copter::ekf_check() // To-Do: add ekf variances to extended status } +// ekf_check_position_problem - returns true if the EKF has a positioning problem +bool Copter::ekf_check_position_problem() +{ + // either otflow or abs means we're OK: + if (optflow_position_ok()) { + return false; + } + if (ekf_position_ok()) { + return false; + } + + return true; +} + + // ekf_over_threshold - returns true if the ekf's variance are over the tolerance bool Copter::ekf_over_threshold() { @@ -94,11 +109,6 @@ bool Copter::ekf_over_threshold() return false; } - // return true immediately if position is bad - if (!ekf_position_ok() && !optflow_position_ok()) { - return true; - } - // use EKF to get variance float position_variance, vel_variance, height_variance, tas_variance; Vector3f mag_variance;