diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index c0942c842f..b228fff0d7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -207,10 +207,8 @@ void AP_MotorsHeli::output() calculate_armed_scalars(); if (!_flags.interlock) { output_armed_zero_throttle(); - } else if (_flags.stabilizing) { - output_armed_stabilizing(); } else { - output_armed_not_stabilizing(); + output_armed_stabilizing(); } } else { output_disarmed(); @@ -230,19 +228,6 @@ void AP_MotorsHeli::output_armed_stabilizing() update_motor_control(ROTOR_CONTROL_ACTIVE); } -void AP_MotorsHeli::output_armed_not_stabilizing() -{ - // if manual override active after arming, deactivate it and reinitialize servos - if (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED) { - reset_flight_controls(); - } - - // helicopters always run stabilizing flight controls - move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - - update_motor_control(ROTOR_CONTROL_ACTIVE); -} - // output_armed_zero_throttle - sends commands to the motors void AP_MotorsHeli::output_armed_zero_throttle() { diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index fc3fa67882..e18257ece1 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -178,7 +178,6 @@ protected: // output - sends commands to the motors void output_armed_stabilizing(); - void output_armed_not_stabilizing(); void output_armed_zero_throttle(); void output_disarmed();