diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index cc8c601888..96deff075e 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -193,17 +193,21 @@ void Copter::heli_update_rotor_speed_targets() void Copter::heli_update_autorotation() { #if MODE_AUTOROTATE_ENABLED == ENABLED - //set autonomous autorotation flight mode - if (!ap.land_complete && !motors->get_interlock() && !flightmode->has_manual_throttle() && g2.arot.is_enable()) { + // check if flying and interlock disengaged + if (!ap.land_complete && !motors->get_interlock()) { + if (!flightmode->has_manual_throttle() && g2.arot.is_enable()) { + // set autonomous autorotation flight mode + set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START); + } + // set flag to facilitate both auto and manual autorotations heli_flags.in_autorotation = true; - set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START); } else { heli_flags.in_autorotation = false; } // sets autorotation flags through out libraries heli_set_autorotation(heli_flags.in_autorotation); - if (!ap.land_complete && g2.arot.is_enable()) { + if (!ap.land_complete) { motors->set_enable_bailout(true); } else { motors->set_enable_bailout(false); @@ -215,7 +219,7 @@ void Copter::heli_update_autorotation() } #if MODE_AUTOROTATE_ENABLED == ENABLED -// heli_set_autorotation - set the autorotation f`lag throughout libraries +// heli_set_autorotation - set the autorotation flag throughout libraries void Copter::heli_set_autorotation(bool autorotation) { motors->set_in_autorotation(autorotation);