// 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());
}
// loiter_init - initialise loiter controller
bool Copter::loiter_init(bool ignore_checks)
{
if (position_ok() || optflow_position_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// initialize vertical speed and accelerationj
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
return true;
}else{
return false;
}
}
// brake_init - initialise brake controller
bool Copter::brake_init(bool ignore_checks)
{
if (position_ok() || optflow_position_ok() || ignore_checks) {
// set desired acceleration to zero
wp_nav.clear_pilot_desired_acceleration();
// set target to current position
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
// initialize vertical speed and acceleration
pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
return true;
} else {
return false;
}
}
// 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);
}
void Copter::update_ground_effect_detector(void)
{
if(!motors.armed()) {
// disarmed - disable ground effect and return
gndeffect_state.takeoff_expected = false;
gndeffect_state.touchdown_expected = false;
ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
return;
}
// variable initialization
uint32_t tnow_ms = millis();
float xy_des_speed_cms = 0.0f;
float xy_speed_cms = 0.0f;
float des_climb_rate_cms = pos_control.get_desired_velocity().z;
if (pos_control.is_active_xy()) {
Vector3f vel_target = pos_control.get_vel_target();
vel_target.z = 0.0f;
xy_des_speed_cms = vel_target.length();
}
if (position_ok() || optflow_position_ok()) {
Vector3f vel = inertial_nav.get_velocity();
vel.z = 0.0f;
xy_speed_cms = vel.length();
}
// takeoff logic
// if we are armed and haven't yet taken off
if (motors.armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
gndeffect_state.takeoff_expected = true;
}
// if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
bool throttle_up = mode_has_manual_throttle(control_mode) && g.rc_3.control_in > 0;
if (!throttle_up && ap.land_complete) {
gndeffect_state.takeoff_time_ms = tnow_ms;
gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
}
// if we are in takeoff_expected and we meet the conditions for having taken off
// end the takeoff_expected state
if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) {
gndeffect_state.takeoff_expected = false;
}
// landing logic
Vector3f angle_target_rad = attitude_control.get_att_target_euler_cd() * radians(0.01f);
bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 125.0f;
bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);
bool descent_demanded = pos_control.is_active_z() && des_climb_rate_cms < 0.0f;
bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
bool z_speed_low = abs(inertial_nav.get_velocity_z()) <= 60.0f;
bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));
gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;
// Prepare the EKF for ground effect if either takeoff or touchdown is expected.
ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
}
