From 636358b668d15f6739dec2227073b5b96ae2fff8 Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Sat, 21 Aug 2021 12:01:57 -0500 Subject: [PATCH] Plane: add param for throttle control in transition to VTOL --- ArduPlane/tailsitter.cpp | 18 +++++++++++++++--- ArduPlane/tailsitter.h | 1 + 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 315a751930..bf7de01605 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -134,6 +134,13 @@ const AP_Param::GroupInfo Tailsitter::var_info[] = { // @Range: 10 500 AP_GROUPINFO("RAT_VT", 17, Tailsitter, transition_rate_vtol, 50), + // @Param: THR_VT + // @DisplayName: Tailsitter forward flight to VTOL transition throttle + // @Description: Throttle used during FW->VTOL transition, -1 uses hover throttle + // @Units: % + // @Range: -1 100 + AP_GROUPINFO("THR_VT", 18, Tailsitter, transition_throttle_vtol, -1), + AP_GROUPEND }; @@ -256,9 +263,14 @@ void Tailsitter::output(void) /* during transitions to vtol mode set the throttle to hover thrust, center the rudder */ - throttle = motors->get_throttle_hover(); - // work out equivelent motors throttle level for cruise - throttle = MAX(throttle,motors->actuator_to_thrust(plane.aparm.throttle_cruise.get() * 0.01)); + if (!is_negative(transition_throttle_vtol)) { + // Q_TAILSIT_THR_VT is positive use it until transition is complete + throttle = motors->actuator_to_thrust(MIN(transition_throttle_vtol*0.01,1.0)); + } else { + throttle = motors->get_throttle_hover(); + // work out equivelent motors throttle level for cruise + throttle = MAX(throttle,motors->actuator_to_thrust(plane.aparm.throttle_cruise.get() * 0.01)); + } SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); plane.rudder_dt = 0; diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index 68b8e9af01..27b3466ce8 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -87,6 +87,7 @@ public: AP_Float transition_rate_fw; AP_Int8 transition_angle_vtol; AP_Float transition_rate_vtol; + AP_Float transition_throttle_vtol; AP_Int8 input_type; AP_Float vectored_forward_gain; AP_Float vectored_hover_gain;