Improve attenuation

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) {

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

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;