Copter: check for EKF lane switch to avoid EKF failsafe

this fixes an issue with mismatch between the EKF lane switch
threshold and the copter EKF failsafe threshold

ArduCopter/ekf_check.cpp

@@ -49,6 +49,13 @@ void Copter::ekf_check()
         if (!ekf_check_state.bad_variance) {
             // increase counter
             ekf_check_state.fail_count++;
+#if EKF_CHECK_ITERATIONS_MAX > 2
+            if (ekf_check_state.fail_count == EKF_CHECK_ITERATIONS_MAX-1) {
+                // we are just about to declare a EKF failsafe, ask the EKF if we can change lanes
+                // to resolve the issue
+                ahrs.check_lane_switch();
+            }
+#endif
             // if counter above max then trigger failsafe
             if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
                 // limit count from climbing too high