diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index f672ad0b6c..e32b67af10 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -473,10 +473,10 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Param: TAILSIT_GSCMSK // @DisplayName: Tailsitter gain scaling mask - // @Description: Bitmask of gain scaling methods to be applied: BOOST: boost gain at low throttle, ATT_THR: reduce gain at high throttle/tilt + // @Description: Bitmask of gain scaling methods to be applied: Throttle: scale gains with throttle, ATT_THR: reduce gain at high throttle/tilt, 2:Disk theory velocity calculation, requires Q_TAILSIT_DSKLD to be set, ATT_THR must not be set, 3:Altitude correction, scale with air density // @User: Standard - // @Bitmask: 0:BOOST,1:ATT_THR - AP_GROUPINFO("TAILSIT_GSCMSK", 17, QuadPlane, tailsitter.gain_scaling_mask, TAILSITTER_GSCL_BOOST), + // @Bitmask: 0:Throttle,1:ATT_THR,2:Disk Theory,3:Altitude correction + AP_GROUPINFO("TAILSIT_GSCMSK", 17, QuadPlane, tailsitter.gain_scaling_mask, TAILSITTER_GSCL_THROTTLE), // @Param: TAILSIT_GSCMIN // @DisplayName: Minimum gain scaling based on throttle and attitude @@ -501,6 +501,14 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @RebootRequired: False AP_GROUPINFO("FWD_MANTHR_MAX", 20, QuadPlane, fwd_thr_max, 0), + // @Param: TAILSIT_DSKLD + // @DisplayName: Tailsitter disk loading + // @Description: This is the vehicle weight in kg divided by the total disk area of all propellers in m^2. Only used with Q_TAILSIT_GSCMSK = 4 + // @Units: kg/m.m + // @Range: 0 50 + // @User: Standard + AP_GROUPINFO("TAILSIT_DSKLD", 21, QuadPlane, tailsitter.disk_loading, 0), + AP_GROUPEND }; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index c50fa7722b..70348b8a67 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -507,8 +507,10 @@ private: }; enum tailsitter_gscl_mask { - TAILSITTER_GSCL_BOOST = (1U<<0), + TAILSITTER_GSCL_THROTTLE = (1U<<0), TAILSITTER_GSCL_ATT_THR = (1U<<1), + TAILSITTER_GSCL_DISK_THEORY = (1U<<2), + TAILSITTER_GSCL_ALTITUDE = (1U<<3), }; // tailsitter control variables @@ -527,6 +529,7 @@ private: AP_Float scaling_speed_min; AP_Float scaling_speed_max; AP_Int16 gain_scaling_mask; + AP_Float disk_loading; } tailsitter; // tailsitter speed scaler diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index cc2ecd603c..6f8a0aea0c 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -312,7 +312,13 @@ void QuadPlane::tailsitter_speed_scaling(void) const float throttle = motors->get_throttle(); float spd_scaler = 1.0f; - if (tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ATT_THR) { + // Scaleing with throttle + float throttle_scaler = tailsitter.throttle_scale_max; + if (is_positive(throttle)) { + throttle_scaler = constrain_float(hover_throttle / throttle, tailsitter.gain_scaling_min, tailsitter.throttle_scale_max); + } + + if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ATT_THR) != 0) { // reduce gains when flying at high speed in Q modes: // critical parameter: violent oscillations if too high @@ -354,16 +360,59 @@ void QuadPlane::tailsitter_speed_scaling(void) const float negdelta = plane.G_Dt / negTC; spd_scaler = constrain_float(spd_scaler, last_spd_scaler - negdelta, last_spd_scaler + posdelta); last_spd_scaler = spd_scaler; - } - // if gain attenuation isn't active and boost is enabled - if ((spd_scaler >= 1.0f) && (tailsitter.gain_scaling_mask & TAILSITTER_GSCL_BOOST)) { - // boost gains at low throttle - if (is_zero(throttle)) { - spd_scaler = tailsitter.throttle_scale_max; + // also apply throttle scaling if enabled + if ((spd_scaler >= 1.0f) && ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0)) { + spd_scaler = MAX(throttle_scaler,1.0f); + } + + } else if (((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_DISK_THEORY) != 0) && is_positive(tailsitter.disk_loading.get())) { + // Use disk theory to estimate the velocity over the control surfaces + // https://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/node86.html + + float airspeed; + if (!ahrs.airspeed_estimate(airspeed)) { + // No airspeed estimate, use throttle scaling + spd_scaler = throttle_scaler; + } else { - spd_scaler = constrain_float(hover_throttle / throttle, 1.0f, tailsitter.throttle_scale_max); + + + // use the equation: T = 0.5 * rho * A (Ue^2 - U0^2) solved for Ue^2: + // Ue^2 = (T / (0.5 * rho *A)) + U0^2 + // We don't know thrust or disk area, use T = (throttle/throttle_hover) * weight + // ((t / t_h ) * weight) / (0.5 * rho * A) = ((t / t_h) * mass * 9.81) / (0.5 * rho * A) + // (mass / A) is disk loading DL so: + // Ue^2 = (((t / t_h) * DL * 9.81)/(0.5 * rho)) + U0^2 + + const float rho = SSL_AIR_DENSITY * plane.barometer.get_air_density_ratio(); + float hover_rho = rho; + if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) { + // if applying altitude correction use sea level density for hover case + hover_rho = SSL_AIR_DENSITY; + } + + // hover case: (t / t_h) = 1 and U0 = 0 + const float sq_hover_outflow = (tailsitter.disk_loading.get() * GRAVITY_MSS) / (0.5f * hover_rho); + + + // calculate the true outflow speed + const float sq_outflow = (((throttle/hover_throttle) * tailsitter.disk_loading.get() * GRAVITY_MSS) / (0.5f * rho)) + sq(MAX(airspeed,0)); + + // Scale by the ratio of squared hover outflow velocity to squared actual outflow velocity + spd_scaler = tailsitter.throttle_scale_max; + if (is_positive(sq_outflow)) { + spd_scaler = constrain_float(sq_hover_outflow / sq_outflow, tailsitter.gain_scaling_min.get(), tailsitter.throttle_scale_max.get()); + } } + + } else if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0) { + spd_scaler = throttle_scaler; + } + + if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) { + // air density correction + spd_scaler /= plane.barometer.get_air_density_ratio(); } // record for QTUN log @@ -372,10 +421,17 @@ void QuadPlane::tailsitter_speed_scaling(void) const SRV_Channel::Aux_servo_function_t functions[] = { SRV_Channel::Aux_servo_function_t::k_aileron, SRV_Channel::Aux_servo_function_t::k_elevator, - SRV_Channel::Aux_servo_function_t::k_rudder}; + SRV_Channel::Aux_servo_function_t::k_rudder, + SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft, + SRV_Channel::Aux_servo_function_t::k_tiltMotorRight}; for (uint8_t i=0; i