From 4ed21bdd0651e7f64b0510b39a0c1113e5ca37ca Mon Sep 17 00:00:00 2001
From: ctzsnooze <chris.thompson@sydney.edu.au>
Date: Sun, 29 Sep 2024 08:00:11 +1000
Subject: [PATCH] Fix DMax calculations (#13933)

Refactor DMax calculations
---
 src/main/flight/pid.c      | 22 ++++++++++++----------
 src/main/flight/pid_init.c |  7 ++++---
 2 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 0d4e7b1fd42..696915abee4 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -1199,26 +1199,28 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 #endif
 
 #ifdef USE_D_MAX
-            float dMaxFactor = 1.0f;
-            if (pidRuntime.dMaxPercent[axis] > 0) {
+            float dMaxMultiplier = 1.0f;
+            if (pidRuntime.dMaxPercent[axis] > 1.0f) {
                 float dMaxGyroFactor = pt2FilterApply(&pidRuntime.dMaxRange[axis], delta);
                 dMaxGyroFactor = fabsf(dMaxGyroFactor) * pidRuntime.dMaxGyroGain;
                 const float dMaxSetpointFactor = fabsf(pidSetpointDelta) * pidRuntime.dMaxSetpointGain;
-                dMaxFactor = MAX(dMaxGyroFactor, dMaxSetpointFactor);
-                dMaxFactor = 1.0f + (1.0f - pidRuntime.dMaxPercent[axis]) * dMaxFactor;
-                dMaxFactor = pt2FilterApply(&pidRuntime.dMaxLowpass[axis], dMaxFactor);
-                dMaxFactor = MIN(dMaxFactor, 1.0f / pidRuntime.dMaxPercent[axis]);
+                const float dMaxBoost = fmaxf(dMaxGyroFactor, dMaxSetpointFactor);
+                // dMaxBoost starts at zero, and by 1.0 we get Dmax, but it can exceed 1.
+                dMaxMultiplier += (pidRuntime.dMaxPercent[axis] - 1.0f) * dMaxBoost;
+                dMaxMultiplier = pt2FilterApply(&pidRuntime.dMaxLowpass[axis], dMaxMultiplier);
+                // limit the gain to the fraction that DMax is greater than Min
+                dMaxMultiplier = MIN(dMaxMultiplier, pidRuntime.dMaxPercent[axis]);
                 if (axis == FD_ROLL) {
                     DEBUG_SET(DEBUG_D_MAX, 0, lrintf(dMaxGyroFactor * 100));
                     DEBUG_SET(DEBUG_D_MAX, 1, lrintf(dMaxSetpointFactor * 100));
-                    DEBUG_SET(DEBUG_D_MAX, 2, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMaxFactor * 10 / DTERM_SCALE));
+                    DEBUG_SET(DEBUG_D_MAX, 2, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMaxMultiplier * 10 / DTERM_SCALE)); // actual D
                 } else if (axis == FD_PITCH) {
-                    DEBUG_SET(DEBUG_D_MAX, 3, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMaxFactor * 10 / DTERM_SCALE));
+                    DEBUG_SET(DEBUG_D_MAX, 3, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMaxMultiplier * 10 / DTERM_SCALE));
                 }
             }
 
-            // Apply the dMaxFactor
-            preTpaD *= dMaxFactor;
+            // Apply the gain that increases D towards Dmax
+            preTpaD *= dMaxMultiplier;
 #endif
 
             pidData[axis].D = preTpaD * pidRuntime.tpaFactor;
diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c
index 74aaf0c1ad4..28bbcf8c84c 100644
--- a/src/main/flight/pid_init.c
+++ b/src/main/flight/pid_init.c
@@ -449,10 +449,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
 #ifdef USE_D_MAX
     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
         const uint8_t dMax = pidProfile->d_max[axis];
-        if ((dMax > 0) && (dMax > pidProfile->pid[axis].D)) {
-            pidRuntime.dMaxPercent[axis] = (float) pidProfile->pid[axis].D / dMax;
+        if ((pidProfile->pid[axis].D > 0) && dMax > pidProfile->pid[axis].D) {
+            pidRuntime.dMaxPercent[axis] = (float) dMax / pidProfile->pid[axis].D;
+            // fraction that Dmax is higher than D, eg if D is 8 and Dmax is 10, Dmax is 1.25 times bigger
         } else {
-            pidRuntime.dMaxPercent[axis] = 0;
+            pidRuntime.dMaxPercent[axis] = 1.0f;
         }
     }
     pidRuntime.dMaxGyroGain = D_MAX_GAIN_FACTOR * pidProfile->d_max_gain / D_MAX_LOWPASS_HZ;