diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3d235689fe..84ce0147f0 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -352,6 +352,13 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp AP_SUBGROUPPTR(loiter_nav, "LOIT_", 2, QuadPlane, AC_Loiter), + // @Param: TAILSIT_THSCMX + // @DisplayName: Maximum control throttle scaling value + // @Description: Maximum value of throttle scaling for tailsitter velocity scaling, reduce this value to remove low thorottle D ossilaitons + // @Range: 1 5 + // @User: Standard + AP_GROUPINFO("TAILSIT_THSCMX", 3, QuadPlane, tailsitter.throttle_scale_max, 5), + AP_GROUPEND }; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index da73b00713..90535400b3 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -401,6 +401,7 @@ private: AP_Float vectored_forward_gain; AP_Float vectored_hover_gain; AP_Float vectored_hover_power; + AP_Float throttle_scale_max; } tailsitter; // the attitude view of the VTOL attitude controller diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 98626994e2..7edf9e9694 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -209,13 +209,14 @@ 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; + float scaling; + if (is_zero(throttle)) { - scaling = scaling_max; + scaling = tailsitter.throttle_scale_max; } else { - scaling = constrain_float(hover_throttle / throttle, 1/scaling_max, scaling_max); + scaling = constrain_float(hover_throttle / throttle, 0, tailsitter.throttle_scale_max); } + const SRV_Channel::Aux_servo_function_t functions[2] = { SRV_Channel::Aux_servo_function_t::k_aileron, SRV_Channel::Aux_servo_function_t::k_elevator};