// 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);
}
