From 1f723837e96e37ea05e0aba99b983d91c1d87152 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 7 Apr 2024 20:28:45 +0200
Subject: [PATCH] Improve attenuation

---
 src/main/common/maths.c | 18 +++++++++++++++++-
 src/main/common/maths.h |  2 ++
 src/main/flight/pid.c   |  4 +++-
 3 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 1bc61ae4c3..eed7b863ba 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -516,9 +516,25 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
     return sensorCalibrationValidateResult(result);
 }
 
+float gaussian(const float x, const float mu, const float sigma) {
+    return exp(-pow(x - mu, 2) / (2 * pow(sigma, 2)));
+}
+
 float bellCurve(const float x, const float curveWidth)
 {
-    return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
+    return gaussian(x, 0.0f, curveWidth);
+}
+
+/**
+ * @brief Calculate the attenuation of a value using a Gaussian function.
+ * Retuns 1 for input 0 and ~0 for input width.
+ * @param input The input value.
+ * @param width The width of the Gaussian function.
+ * @return The attenuation of the input value.
+*/
+float attenuation(const float input, const float width) {
+    const float sigma = width / 2.35482f; // Approximately width / sqrt(2 * ln(2))
+    return gaussian(input, 0.0f, sigma);
 }
 
 float fast_fsqrtf(const float value) {
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index f3f2fd6274..ce96b7064f 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -195,6 +195,8 @@ float acos_approx(float x);
 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 
 float bellCurve(const float x, const float curveWidth);
+float attenuation(const float input, const float width);
+float gaussian(const float x, const float mu, const float sigma);
 float fast_fsqrtf(const float value);
 float calc_length_pythagorean_2D(const float firstElement, const float secondElement);
 float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index d6c8fe81f3..9e50522ae3 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -759,7 +759,9 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
 
     const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
-    const float dampingFactor = bellCurve(MIN(rateTarget, maxRate), maxRate);
+    const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
+
+    DEBUG_SET(DEBUG_ALWAYS, axis, dampingFactor * 1000);
 
     const float rateError = rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor;