From e464909ddfd939a77f4552d23f5abccb97d054ea Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 5 Jan 2015 14:14:27 +0900 Subject: [PATCH] Copter: position_ok true when EKF predicts it will be ok This resolves the chicken and egg problem of the EKF filter status's position flags not becoming true until after the vehicle has been armed at least once. --- ArduCopter/system.pde | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index a3496a4a65..938214fdfe 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -340,7 +340,8 @@ static bool position_ok() { if (ahrs.have_inertial_nav()) { // with EKF use filter status and ekf check - return (inertial_nav.get_filter_status().flags.horiz_pos_abs && !failsafe.ekf); + nav_filter_status filt_status = inertial_nav.get_filter_status(); + return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs) && !failsafe.ekf); } else { // with interial nav use GPS based checks return (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D && @@ -361,7 +362,8 @@ static bool optflow_position_ok() } // get filter status from inertial nav or EKF - return inertial_nav.get_filter_status().flags.horiz_pos_rel; + nav_filter_status filt_status = inertial_nav.get_filter_status(); + return (filt_status.flags.horiz_pos_rel || filt_status.flags.pred_horiz_pos_rel); #endif }