diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 2af836a45542fc..7a258bd25cb69e 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -60,7 +60,9 @@ void ModeAcro_Heli::run() case AP_Motors::SpoolState::THROTTLE_UNLIMITED: case AP_Motors::SpoolState::SPOOLING_DOWN: if (copter.ap.land_complete && !motors->using_leaky_integrator()) { - attitude_control->reset_target_and_rate(false); + if (!motors->using_hdg_error_correction()) { + attitude_control->reset_yaw_target_and_rate(false); + } attitude_control->reset_rate_controller_I_terms_smoothly(); } break; diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 45433afbe2d219..825f641e0eba16 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -55,6 +55,9 @@ void ModeAltHold::run() break; case AltHoldModeState::Landed_Ground_Idle: + if (motors->using_hdg_error_correction()) { + attitude_control->reset_yaw_target_and_rate(false); + } FALLTHROUGH; case AltHoldModeState::Landed_Pre_Takeoff: diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 3cf53551f86ca3..f735cf6786185f 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -116,7 +116,9 @@ void ModeDrift::run() } } else { if (copter.ap.land_complete && !motors->using_leaky_integrator()) { - attitude_control->reset_target_and_rate(false); + if (!motors->using_hdg_error_correction()) { + attitude_control->reset_yaw_target_and_rate(false); + } attitude_control->reset_rate_controller_I_terms_smoothly(); } } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index f03e1b2191ad06..729672dc294e26 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -288,6 +288,9 @@ void ModeFlowHold::run() break; case AltHoldModeState::Landed_Ground_Idle: + if (motors->using_hdg_error_correction()) { + attitude_control->reset_yaw_target_and_rate(false); + } FALLTHROUGH; case AltHoldModeState::Landed_Pre_Takeoff: diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index f0d5b23af6760d..1691fe23272e51 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -132,6 +132,9 @@ void ModeLoiter::run() break; case AltHoldModeState::Landed_Ground_Idle: + if (motors->using_hdg_error_correction()) { + attitude_control->reset_yaw_target_and_rate(false); + } FALLTHROUGH; case AltHoldModeState::Landed_Pre_Takeoff: diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index a05c068f30f8b1..c1700c38d65e83 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -116,6 +116,9 @@ void ModePosHold::run() break; case AltHoldModeState::Landed_Ground_Idle: + if (motors->using_hdg_error_correction()) { + attitude_control->reset_yaw_target_and_rate(false); + } init_wind_comp_estimate(); FALLTHROUGH; diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 49f4cc3b440990..05203aff1c06ea 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -81,6 +81,9 @@ void ModeSport::run() break; case AltHoldModeState::Landed_Ground_Idle: + if (motors->using_hdg_error_correction()) { + attitude_control->reset_yaw_target_and_rate(false); + } FALLTHROUGH; case AltHoldModeState::Landed_Pre_Takeoff: diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index 20dccc5b964b30..260d2f519346b1 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -59,7 +59,9 @@ void ModeStabilize_Heli::run() // If aircraft is landed, set target heading to current and reset the integrator // Otherwise motors could be at ground idle for practice autorotation if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) { - attitude_control->reset_yaw_target_and_rate(false); + if (!motors->using_hdg_error_correction()) { + attitude_control->reset_yaw_target_and_rate(false); + } attitude_control->reset_rate_controller_I_terms_smoothly(); } break; diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index f6f57304ab1f3d..bed0093e4b26f9 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -356,6 +356,9 @@ void ModeZigZag::manual_control() break; case AltHoldModeState::Landed_Ground_Idle: + if (motors->using_hdg_error_correction()) { + attitude_control->reset_yaw_target_and_rate(false); + } FALLTHROUGH; case AltHoldModeState::Landed_Pre_Takeoff: