diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index 09e9398ebb..f99b6a38b4 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -24,8 +24,8 @@ extern const AP_HAL::HAL& hal; // init_servo - servo initialization on start-up void AP_MotorsHeli_RSC::init_servo() { - // set servo range - _servo_output.set_range_out(0,1000); + // setup RSC on specified channel by default + RC_Channel_aux::set_aux_channel_default(_aux_fn, _default_channel); } // recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters @@ -170,9 +170,6 @@ void AP_MotorsHeli_RSC::write_rsc(int16_t servo_out) // ToDo: We should probably use RC_Channel_Aux to avoid this problem return; } else { - _servo_output.servo_out = servo_out; - _servo_output.calc_pwm(); - - hal.rcout->write(_servo_output_channel, _servo_output.radio_out); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_heli_rsc, servo_out); } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index afe8d1b41c..9808a1c642 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -25,11 +25,11 @@ enum RotorControlMode { class AP_MotorsHeli_RSC { public: - AP_MotorsHeli_RSC(RC_Channel& servo_output, - int8_t servo_output_channel, - uint16_t loop_rate) : - _servo_output(servo_output), - _servo_output_channel(servo_output_channel), + AP_MotorsHeli_RSC(RC_Channel_aux::Aux_servo_function_t aux_fn, + uint8_t default_channel, + uint16_t loop_rate) : + _aux_fn(aux_fn), + _default_channel(default_channel), _loop_rate(loop_rate) {}; @@ -84,10 +84,12 @@ public: private: // external variables - RC_Channel& _servo_output; - int8_t _servo_output_channel; // output channel to rotor esc float _loop_rate; // main loop rate + // channel setup for aux function + RC_Channel_aux::Aux_servo_function_t _aux_fn; + uint8_t _default_channel; + // internal variables RotorControlMode _control_mode = ROTOR_CONTROL_MODE_DISABLED; // motor control mode, Passthrough or Setpoint int16_t _critical_speed = 0; // rotor speed below which flight is not possible diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 0d06dc3c19..bc885d4339 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -144,10 +144,6 @@ void AP_MotorsHeli_Single::enable() hal.rcout->enable_ch(AP_MOTORS_MOT_4); // yaw hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc - - // disable channels 7 and 8 from being used by RC_Channel_aux - RC_Channel_aux::disable_aux_channel(AP_MOTORS_HELI_SINGLE_AUX); - RC_Channel_aux::disable_aux_channel(AP_MOTORS_HELI_SINGLE_RSC); } // init_outputs - initialise Servo/PWM ranges and endpoints diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 18273f994e..e9588bd2ad 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -62,8 +62,8 @@ public: _swash_servo_2(servo_2), _swash_servo_3(servo_3), _yaw_servo(servo_4), - _main_rotor(servo_rsc, AP_MOTORS_HELI_SINGLE_RSC, loop_rate), - _tail_rotor(servo_aux, AP_MOTORS_HELI_SINGLE_AUX, loop_rate) + _main_rotor(RC_Channel_aux::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC, loop_rate), + _tail_rotor(RC_Channel_aux::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_AUX, loop_rate) { AP_Param::setup_object_defaults(this, var_info); };