// 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; } // We don't know where we are. Is this a problem? if (copter.flightmode->requires_GPS()) { // Oh, yes, we have a problem return true; } // sometimes LAND *does* require GPS: if (control_mode == LAND && landing_with_GPS()) { return true; } // we're in a non-GPS mode (e.g. althold/stabilize) if (g.fs_ekf_action == FS_EKF_ACTION_LAND_EVEN_STABILIZE) { // the user is making an issue out of it return true; } return false; }
// position_ok - returns true if the horizontal absolute position is ok and home position is set bool Copter::position_ok() { // return false if ekf failsafe has triggered if (failsafe.ekf) { return false; } // check ekf position estimate return (ekf_position_ok() || optflow_position_ok()); }
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance bool Copter::ekf_over_threshold() { // return false immediately if disabled if (g.fs_ekf_thresh <= 0.0f) { return false; } // return true immediately if position is bad if (!ekf_position_ok() && !optflow_position_ok()) { return true; } // use EKF to get variance float posVar, hgtVar, tasVar; Vector3f magVar; Vector2f offset; float compass_variance; float vel_variance; ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset); compass_variance = magVar.length(); // return true if compass and velocity variance over the threshold return (compass_variance >= g.fs_ekf_thresh && vel_variance >= g.fs_ekf_thresh); }