diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 8884269b4c..1b6bdf57ee 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -82,16 +82,24 @@ void QuadPlane::tailsitter_output(void) during transitions to vtol mode set the throttle to hover thrust, center the rudder and set the altitude controller integrator to the same throttle level + convert the hover throttle to the same output that would result if used via AP_Motors + apply expo, battery scaling and SPIN min/max. */ - throttle = motors->get_throttle_hover() * 100; + throttle = motors->thrust_to_actuator(motors->get_throttle_hover()) * 100; throttle = MAX(throttle,plane.aparm.throttle_cruise.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); pos_control->get_accel_z_pid().set_integrator(throttle*10); // override AP_MotorsTailsitter throttles during back transition + + // apply PWM min and MAX to throttle left and right, just as via AP_Motors + uint16_t throttle_pwm = motors->get_pwm_output_min() + (motors->get_pwm_output_max() - motors->get_pwm_output_min()) * throttle * 0.01f; + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, throttle_pwm); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, throttle_pwm); + + // throttle output is not used by AP_Motors so might have diffrent PWM range, set scaled SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle); } if (!assisted_flight) { @@ -146,7 +154,7 @@ void QuadPlane::tailsitter_output(void) SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, (motors->get_yaw())*-SERVO_MAX); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, (motors->get_pitch())*SERVO_MAX); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, (motors->get_roll())*SERVO_MAX); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, (motors->get_throttle()) * 100); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, motors->thrust_to_actuator(motors->get_throttle()) * 100); if (hal.util->get_soft_armed()) { // scale surfaces for throttle