From 27131380a89ee98e31064739fe07a16a95b03cf3 Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Wed, 11 May 2022 15:15:56 -0600
Subject: [PATCH] Improve the peak estimate

Fixes bugs with peak estimate that can occur when denom is low. This is still highly accurate :)
---
 src/flight/sdft.c | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/flight/sdft.c b/src/flight/sdft.c
index baf547552..f1c599304 100644
--- a/src/flight/sdft.c
+++ b/src/flight/sdft.c
@@ -185,15 +185,17 @@ bool sdft_update(sdft_t *sdft) {
         continue;
       }
 
-      const float y0 = sdft->magnitude[sdft->peak_indicies[peak] - 1];
-      const float y1 = 1.75f * sdft->magnitude[sdft->peak_indicies[peak]];
+      const float y0 = 0.95 * sdft->magnitude[sdft->peak_indicies[peak] - 1];
+      const float y1 = sdft->magnitude[sdft->peak_indicies[peak]];
       const float y2 = 1.25f * sdft->magnitude[sdft->peak_indicies[peak] + 1];
 
       // Estimate true peak position aka. meanBin (fit parabola y(x) over y0, y1 and y2, solve dy/dx=0 for x)
       float meanBin = sdft->peak_indicies[peak];
-      const float denom = 2.0f * (y0 - y1 + y2);
+      const float denom = y0 + y1 + y2;
       if (denom != 0.0f) {
-        meanBin += (y0 - y2) / denom;
+        float lower_ratio = y0 / denom;
+        float upper_ratio = y2 / denom;
+        meanBin += upper_ratio - lower_ratio;
       }
 
       const float f_hz = meanBin * (float)resolution_hz;
@@ -224,4 +226,4 @@ bool sdft_update(sdft_t *sdft) {
   }
 
   return filters_updated;
-}
\ No newline at end of file
+}