diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index b7033857dd..ed65f2a63a 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -101,6 +101,9 @@ public: // check if we have completed transition to vtol bool tailsitter_transition_vtol_complete(void) const; + + // account for surface speed scaling in hover + void tailsitter_speed_scaling(void); // user initiated takeoff for guided mode bool do_user_takeoff(float takeoff_altitude); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 77ca4f5d76..f2ad816a54 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -80,11 +80,17 @@ void QuadPlane::tailsitter_output(void) } return; } - + motors_output(); plane.pitchController.reset_I(); plane.rollController.reset_I(); + if (hal.util->get_soft_armed()) { + // scale surfaces for throttle + tailsitter_speed_scaling(); + } + + if (tailsitter.vectored_hover_gain > 0) { // thrust vectoring VTOL modes float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); @@ -194,3 +200,30 @@ bool QuadPlane::in_tailsitter_vtol_transition(void) const { return is_tailsitter() && in_vtol_mode() && transition_state == TRANSITION_ANGLE_WAIT_VTOL; } + +/* + account for speed scaling of control surfaces in hover +*/ +void QuadPlane::tailsitter_speed_scaling(void) +{ + const float hover_throttle = motors->get_throttle_hover(); + const float throttle = motors->get_throttle(); + const float scaling_max = 5; + float scaling = 1; + if (is_zero(throttle)) { + scaling = scaling_max; + } else { + scaling = constrain_float(hover_throttle / throttle, 1/scaling_max, scaling_max); + } + const SRV_Channel::Aux_servo_function_t functions[3] = { + SRV_Channel::Aux_servo_function_t::k_aileron, + SRV_Channel::Aux_servo_function_t::k_elevator, + SRV_Channel::Aux_servo_function_t::k_rudder}; + for (uint8_t i=0; i