From a907c10733a297f1db46a1805e23d51753d426a0 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Sun, 29 Nov 2020 09:51:58 +1100
Subject: [PATCH] SITL: Add momentum drag to multicopter model

---
 libraries/SITL/SIM_Frame.cpp | 41 +++++++++++++++++++++++++++++-------
 libraries/SITL/SIM_Frame.h   |  8 +++++++
 2 files changed, 41 insertions(+), 8 deletions(-)

diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp
index d0c4c0b345..f55196445b 100644
--- a/libraries/SITL/SIM_Frame.cpp
+++ b/libraries/SITL/SIM_Frame.cpp
@@ -395,6 +395,8 @@ void Frame::load_frame_params(const char *model_json)
         FRAME_VAR(spin_min),
         FRAME_VAR(spin_max),
         FRAME_VAR(slew_max),
+        FRAME_VAR(disc_area),
+        FRAME_VAR(mdrag_coef),
     };
     static_assert(sizeof(model) == sizeof(float)*ARRAY_SIZE(vars), "incorrect model vars");
 
@@ -430,9 +432,21 @@ void Frame::init(const char *frame_str, Battery *_battery)
 
     const float drag_force = model.mass * GRAVITY_MSS * tanf(radians(model.refAngle));
 
+    const float cos_tilt = cosf(radians(model.refAngle));
+    const float airspeed_bf = model.refSpd * cos_tilt;
+    const float ref_thrust = model.mass * GRAVITY_MSS / cos_tilt;
     float ref_air_density = get_air_density(model.refAlt);
 
-    areaCd = drag_force / (0.5 * ref_air_density * sq(model.refSpd));
+    const float momentum_drag = cos_tilt * model.mdrag_coef * airspeed_bf * sqrtf(ref_thrust * ref_air_density * model.disc_area);
+
+    if (momentum_drag > drag_force) {
+        model.mdrag_coef *= drag_force / momentum_drag;
+        areaCd = 0.0;
+        ::printf("Suggested EK3_BCOEF_* = 0, EK3_MCOEF = %.3f\n", (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density));
+    } else {
+        areaCd = (drag_force - momentum_drag) / (0.5f * ref_air_density * sq(model.refSpd));
+        ::printf("Suggested EK3_BCOEF_* = %.3f, EK3_MCOEF = %.3f\n", model.mass / areaCd, (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density));
+    }
 
     terminal_rotation_rate = model.refRotRate;
 
@@ -534,14 +548,25 @@ void Frame::calculate_forces(const Aircraft &aircraft,
 
     if (use_drag) {
         // use the model params to calculate drag
-        const Vector3f vel_air_ef = aircraft.get_velocity_air_ef();
-        const float speed = vel_air_ef.length();
-        float drag_force = areaCd * 0.5 * air_density * sq(speed);
-        Vector3f drag_ef;
-        if (is_positive(speed)) {
-            drag_ef = -(vel_air_ef / speed) * drag_force;
+        Vector3f drag_bf;
+        drag_bf.x = areaCd * 0.5f * air_density * sq(vel_air_bf.x) +
+                    model.mdrag_coef * fabsf(vel_air_bf.x) * sqrtf(fabsf(thrust.z) * air_density * model.disc_area);
+        if (is_positive(vel_air_bf.x)) {
+            drag_bf.x = -drag_bf.x;
         }
-        body_accel += aircraft.get_dcm().transposed() * drag_ef / mass;
+
+        drag_bf.y = areaCd * 0.5f * air_density * sq(vel_air_bf.y) +
+                    model.mdrag_coef * fabsf(vel_air_bf.y) * sqrtf(fabsf(thrust.z) * air_density * model.disc_area);
+        if (is_positive(vel_air_bf.y)) {
+            drag_bf.y = -drag_bf.y;
+        }
+
+        drag_bf.z = areaCd * 0.5f * air_density * sq(vel_air_bf.z);
+        if (is_positive(vel_air_bf.z)) {
+            drag_bf.z = -drag_bf.z;
+        }
+
+        body_accel += drag_bf / mass;
     }
 
     // add some noise
diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h
index 4456411a70..5eddcde535 100644
--- a/libraries/SITL/SIM_Frame.h
+++ b/libraries/SITL/SIM_Frame.h
@@ -119,6 +119,14 @@ private:
 
         // maximum slew rate of motors
         float slew_max = 150;
+
+        // rotor disc area in m**2 for 4 x 0.35m dia rotors
+        // Note that coaxial rotors count as one rotor only when cauclating effective disc area
+        float disc_area = 0.385;
+
+        // momentum drag coefficient
+        float mdrag_coef = 0.2;
+
     } default_model;
 
     struct Model model;