Improve peak estimation

fix an issue where the old estimation can cause some very off very bad numbers. When the old denom would be near 0 it would create wildly inaccurate peak estimations, this fixes that.

Author: Quick-Flash

src/main/flight/dyn_notch_filter.c

@@ -83,7 +83,7 @@
 // Each SDFT output bin has width sdftSampleRateHz/72, ie 18.5Hz per bin at 1333Hz.
 // Usable bandwidth is half this, ie 666Hz if sdftSampleRateHz is 1333Hz, i.e. bin 1 is 18.5Hz, bin 2 is 37.0Hz etc.
 
-#define DYN_NOTCH_SMOOTH_HZ        4
+#define DYN_NOTCH_SMOOTH_HZ        8
 #define DYN_NOTCH_CALC_TICKS       (XYZ_AXIS_COUNT * STEP_COUNT) // 3 axes and 4 steps per axis
 #define DYN_NOTCH_OSD_MIN_THROTTLE 20
 #define DYN_NOTCH_UPDATE_MIN_HZ    2000
@@ -343,14 +343,16 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
                     float meanBin = peaks[p].bin;
 
                     // Height of peak bin (y1) and shoulder bins (y0, y2)
-                    const float y0 = sdftData[peaks[p].bin - 1];
+                    const float y0 = sdftData[peaks[p].bin - 1] * 0.95;
                     const float y1 = sdftData[peaks[p].bin];
-                    const float y2 = sdftData[peaks[p].bin + 1];
+                    const float y2 = sdftData[peaks[p].bin + 1] * 1.25;
 
-                    // Estimate true peak position aka. meanBin (fit parabola y(x) over y0, y1 and y2, solve dy/dx=0 for x)
-                    const float denom = 2.0f * (y0 - 2 * y1 + y2);
+                    // Estimate true peak position
+                    const float denom = y0 + y1 + y2;
                     if (denom != 0.0f) {
-                        meanBin += (y0 - y2) / denom;
+                        float upper_ratio = y2 / denom;
+                        float lower_ratio = y0 / denom;
+                        meanBin += upper_ratio - lower_ratio;
                     }
 
                     // Convert bin to frequency: freq = bin * binResoultion (bin 0 is 0Hz)