diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 152b89b642..67dbbd7a45 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -635,7 +635,8 @@ bool QuadPlane::setup(void) break; case AP_Motors::MOTOR_FRAME_TAILSITTER: // this is a duo-motor tailsitter - motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed); + tailsitter.tailsitter_motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed); + motors = tailsitter.tailsitter_motors; motors_var_info = AP_MotorsTailsitter::var_info; break; case AP_Motors::MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX: diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 0b95bafc28..37d96c029a 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -159,6 +159,12 @@ const AP_Param::GroupInfo Tailsitter::var_info[] = { // @Range: 0 2 AP_GROUPINFO("VT_Y_P", 21, Tailsitter, VTOL_yaw_scale, 1), + // @Param: MIN_VO + // @DisplayName: Tailsitter Disk loading minimum outflow speed + // @Description: Use in conjunction with disk therory gain scaling and Q_TAILSIT_DSKLD to specify minumum airspeed over control surfaces, this will be used to boost throttle, when decending for example, 0 disables + // @Range: 0 15 + AP_GROUPINFO("MIN_VO", 22, Tailsitter, disk_loading_min_outflow, 0), + AP_GROUPEND }; @@ -388,6 +394,8 @@ void Tailsitter::output(void) if (hal.util->get_soft_armed()) { // scale surfaces for throttle speed_scaling(); + } else if (tailsitter_motors != nullptr) { + tailsitter_motors->set_min_throttle(0.0); } tilt_left = 0.0f; @@ -584,8 +592,9 @@ int8_t Tailsitter::get_transition_angle_vtol() const void Tailsitter::speed_scaling(void) { const float hover_throttle = motors->get_throttle_hover(); - const float throttle = motors->get_throttle(); + const float throttle = motors->get_throttle_out(); float spd_scaler = 1.0f; + float disk_loading_min_throttle = 0.0; // Scaleing with throttle float throttle_scaler = throttle_scale_max; @@ -679,6 +688,24 @@ void Tailsitter::speed_scaling(void) if (is_positive(sq_outflow)) { spd_scaler = constrain_float(sq_hover_outflow / sq_outflow, gain_scaling_min.get(), throttle_scale_max.get()); } + + if (is_positive(disk_loading_min_outflow)) { + // calculate throttle required to give minimum outflow speed over control surfaces + if (is_positive(airspeed)) { + disk_loading_min_throttle = (((sq(disk_loading_min_outflow) - sq(airspeed)) * (0.5 * rho)) / (disk_loading.get() * GRAVITY_MSS)) * hover_throttle; + } else { + // estimate backwards airspeed + float reverse_airspeed = 0.0; + Vector3f vel; + if (quadplane.ahrs.get_velocity_NED(vel)) { + reverse_airspeed = quadplane.ahrs.earth_to_body(vel - quadplane.ahrs.wind_estimate()).x; + } + // make sure actually negative + reverse_airspeed = MIN(reverse_airspeed, 0.0); + disk_loading_min_throttle = (((sq(disk_loading_min_outflow) + sq(reverse_airspeed)) * (0.5 * rho)) / (disk_loading.get() * GRAVITY_MSS)) * hover_throttle; + } + disk_loading_min_throttle = MAX(disk_loading_min_throttle, 0.0); + } } } else if ((gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0) { @@ -709,6 +736,10 @@ void Tailsitter::speed_scaling(void) } SRV_Channels::set_output_scaled(functions[i], v); } + + if (tailsitter_motors != nullptr) { + tailsitter_motors->set_min_throttle(disk_loading_min_throttle); + } } // return true if pitch control should be relaxed diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index ef7579728b..3abfbd5deb 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -16,6 +16,7 @@ #include #include "transition.h" +#include class QuadPlane; class AP_MotorsMulticopter; @@ -105,6 +106,9 @@ public: AP_Float VTOL_roll_scale; AP_Float VTOL_pitch_scale; AP_Float VTOL_yaw_scale; + AP_Float disk_loading_min_outflow; + + AP_MotorsTailsitter* tailsitter_motors; private: