diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 12c4f9f42e..1ce44822e7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -201,21 +201,6 @@ bool AP_MotorsHeli::rotor_runup_complete() const return _heliflags.rotor_runup_complete; } -void AP_MotorsHeli::output_armed_not_stabilizing() -{ - // stabilizing servos always operate for helicopters - output_armed_stabilizing(); -} - -// output_armed_zero_throttle - sends commands to the motors -void AP_MotorsHeli::output_armed_zero_throttle() -{ - // stabilizing servos always operate for helicopters - // ToDo: Bring RSC Master On/Off into this function - output_armed_stabilizing(); -} - - // reset_swash - free up swash for maximum movements. Used for set-up void AP_MotorsHeli::reset_swash() { diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 4d3af583a1..6f2613233b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -168,8 +168,8 @@ protected: // output - sends commands to the motors virtual void output_armed_stabilizing() = 0; - void output_armed_not_stabilizing(); - void output_armed_zero_throttle(); + virtual void output_armed_not_stabilizing() = 0; + virtual void output_armed_zero_throttle() = 0; virtual void output_disarmed() = 0; // update the throttle input filter diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 421426a0ed..5b956dce68 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -257,6 +257,12 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask() // sends commands to the motors void AP_MotorsHeli_Single::output_armed_stabilizing() { + // if manual override active after arming, deactivate it. + if (_servo_manual == 1) { + reset_radio_passthrough(); + _servo_manual = 0; + } + move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { @@ -274,9 +280,69 @@ void AP_MotorsHeli_Single::output_armed_stabilizing() _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); } +void AP_MotorsHeli_Single::output_armed_not_stabilizing() +{ + // if manual override active after arming, deactivate it. + if (_servo_manual == 1) { + reset_radio_passthrough(); + _servo_manual = 0; + } + + move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); + + if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.output_armed(); + + if (!_tail_rotor.is_runup_complete()) + { + _heliflags.rotor_runup_complete = false; + return; + } + } + + _main_rotor.output_armed(); + + _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); +} + +// output_armed_zero_throttle - sends commands to the motors +void AP_MotorsHeli_Single::output_armed_zero_throttle() +{ + // if manual override active after arming, deactivate it. + if (_servo_manual == 1) { + reset_radio_passthrough(); + _servo_manual = 0; + } + + move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); + + if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.output_armed(); + + if (!_tail_rotor.is_runup_complete()) + { + _heliflags.rotor_runup_complete = false; + return; + } + } + + _main_rotor.output_armed(); + + _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); +} + + // output_disarmed - sends commands to the motors void AP_MotorsHeli_Single::output_disarmed() { + // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash + if (_servo_manual == 1) { + _roll_control_input = _roll_radio_passthrough; + _pitch_control_input = _pitch_radio_passthrough; + _throttle_control_input = _throttle_radio_passthrough; + _yaw_control_input = _yaw_radio_passthrough; + } + move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { @@ -355,14 +421,6 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() // void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) { - // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash - if (_servo_manual == 1) { - _roll_control_input = _roll_radio_passthrough; - _pitch_control_input = _pitch_radio_passthrough; - _throttle_control_input = _throttle_radio_passthrough; - _yaw_control_input = _yaw_radio_passthrough; - } - int16_t yaw_offset = 0; int16_t coll_out_scaled; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 492cd26778..57d1daa3aa 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -126,6 +126,8 @@ protected: // output - sends commands to the motors void output_armed_stabilizing(); + void output_armed_not_stabilizing(); + void output_armed_zero_throttle(); void output_disarmed(); // reset_servos - free up the swash servos for maximum movement