diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 69c60a7119..dacc97dc47 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -353,7 +353,13 @@ bool Copter::optflow_position_ok() // get filter status from EKF nav_filter_status filt_status = inertial_nav.get_filter_status(); - return (filt_status.flags.horiz_pos_rel || filt_status.flags.pred_horiz_pos_rel); + + // if disarmed we accept a predicted horizontal relative position + if (!motors.armed()) { + return (filt_status.flags.pred_horiz_pos_rel); + } else { + return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode); + } #endif }