diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index dc6597d27e..3c49e5cc0c 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -73,13 +73,18 @@ void Copter::check_dynamic_flight(void) // should be run between the rate controller and the servo updates. void Copter::update_heli_control_dynamics(void) { - // Use Leaky_I if we are not moving fast -// attitude_control->use_leaky_i(!heli_flags.dynamic_flight); - attitude_control->use_leaky_i(false); - if (ap.land_complete || ap.land_complete_maybe) { - motors->set_land_complete(true); + if (!motors->using_leaky_integrator()) { + //turn off leaky_I + attitude_control->use_leaky_i(false); + if (ap.land_complete || ap.land_complete_maybe) { + motors->set_land_complete(true); + } else { + motors->set_land_complete(false); + } } else { + // Use Leaky_I if we are not moving fast + attitude_control->use_leaky_i(!heli_flags.dynamic_flight); motors->set_land_complete(false); } diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 36f757d080..e5d2f24ffe 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -51,13 +51,13 @@ void ModeAcro_Heli::run() case AP_Motors::SpoolState::GROUND_IDLE: // If aircraft is landed, set target heading to current and reset the integrator // Otherwise motors could be at ground idle for practice autorotation - if (copter.ap.land_complete) { + if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) { attitude_control->set_attitude_target_to_current_attitude(); attitude_control->reset_rate_controller_I_terms(); } break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: - if (copter.ap.land_complete) { + if (copter.ap.land_complete && !motors->using_leaky_integrator()) { attitude_control->reset_rate_controller_I_terms(); } break; diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index f92f4a3642..a216f9fb6d 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -56,13 +56,13 @@ void ModeStabilize_Heli::run() case AP_Motors::SpoolState::GROUND_IDLE: // If aircraft is landed, set target heading to current and reset the integrator // Otherwise motors could be at ground idle for practice autorotation - if (copter.ap.land_complete) { + if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) { attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); } break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: - if (copter.ap.land_complete) { + if (copter.ap.land_complete && !motors->using_leaky_integrator()) { attitude_control->reset_rate_controller_I_terms(); } case AP_Motors::SpoolState::SPOOLING_UP: