diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde index 7e2de943b81d8b45cf3534e68eb9a966e9371d3b..da73ac74ea74c0cd503beb9e6100d3a89dbfb506 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -81,12 +81,12 @@ void ekf_dcm_check() } } } else { - // if compass is flagged as bad - if (ekf_check_state.bad_compass) { - // reduce counter + // reduce counter + if (ekf_check_state.fail_count_compass > 0) { ekf_check_state.fail_count_compass--; - // if counter reaches zero then clear flag - if (ekf_check_state.fail_count_compass == 0) { + + // if compass is flagged as bad and the counter reaches zero then clear flag + if (ekf_check_state.bad_compass && ekf_check_state.fail_count_compass == 0) { ekf_check_state.bad_compass = false; // log recovery in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED);