diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index b8192c4b59..c90029b51a 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -16,6 +16,7 @@ #include #include +#include #include "AP_MotorsHeli_Single.h" extern const AP_HAL::HAL& hal; @@ -320,6 +321,14 @@ void AP_MotorsHeli_Single::update_motor_control(AP_MotorsHeli_RSC::MotorControlS _tail_rotor.output(state); _main_rotor.output(state); + if (state == AP_MotorsHeli_RSC::ROTOR_CONTROL_STOP){ + // set engine run enable aux output to not run position to kill engine when disarmed + RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_engine_run_enable); + } else { + // else if armed, set engine run enable output to run position + RC_Channel_aux::set_radio_to_max(RC_Channel_aux::k_engine_run_enable); + } + // Check if both rotors are run-up, tail rotor controller always returns true if not enabled _heliflags.rotor_runup_complete = ( _main_rotor.is_runup_complete() && _tail_rotor.is_runup_complete() ); }