Plane: Tailsitter add disk theory and altitude gain scailing

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
 };
 

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

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<ARRAY_SIZE(functions); i++) {
         int32_t v = SRV_Channels::get_output_scaled(functions[i]);
-        v *= spd_scaler;
+        if ((functions[i] == SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft) || (functions[i] == SRV_Channel::Aux_servo_function_t::k_tiltMotorRight)) {
+            // always apply throttle scaling to tilts
+            v *= throttle_scaler;
+        } else {
+            v *= spd_scaler;
+        }
         v = constrain_int32(v, -SERVO_MAX, SERVO_MAX);
         SRV_Channels::set_output_scaled(functions[i], v);
     }