From 172695949e2d5fbef80391ffd8e59d4dd9dabb9c Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Sat, 15 Jul 2023 01:01:47 +0200
Subject: [PATCH] sdft: make looptime independent, less samples

---
 src/flight/sdft.c | 37 +++++++++++++++++++++++++------------
 src/flight/sdft.h |  6 +-----
 src/util/util.h   |  7 +++++++
 3 files changed, 33 insertions(+), 17 deletions(-)

diff --git a/src/flight/sdft.c b/src/flight/sdft.c
index ae9dafaaa..f1c599304 100644
--- a/src/flight/sdft.c
+++ b/src/flight/sdft.c
@@ -12,22 +12,26 @@
 // and E. Jacobsen and R. Lyons, “An Update to the Sliding DFT”
 
 #define LOOPTIME_S (state.looptime_autodetect * 1e-6)
-
-#define SWAP(x, y)      \
-  {                     \
-    typeof(x) temp = x; \
-    x = y;              \
-    y = temp;           \
-  }
+#define SAMPLE_HZ (1e6f / state.looptime_autodetect)
 
 static float r_to_N;
 static complex_float twiddle[SDFT_SAMPLE_SIZE];
 
-static const uint32_t bin_min_index = (float)SDFT_MIN_HZ / SDFT_HZ_RESOLUTION + 0.5f;
-static const uint32_t bin_max_index = (float)SDFT_MAX_HZ / SDFT_HZ_RESOLUTION + 0.5f;
-static const uint32_t bin_batches = (bin_max_index - bin_min_index) / SDFT_SUBSAMPLES + 1;
+static uint32_t sub_samples;
+static uint32_t resolution_hz;
+
+static uint32_t bin_min_index;
+static uint32_t bin_max_index;
+static uint32_t bin_batches;
 
 void sdft_init(sdft_t *sdft) {
+  sub_samples = (SAMPLE_HZ / (2.0f * SDFT_MAX_HZ));
+  resolution_hz = ((SAMPLE_HZ / (float)sub_samples) / SDFT_SAMPLE_SIZE);
+
+  bin_min_index = (float)SDFT_MIN_HZ / (float)resolution_hz + 0.5f;
+  bin_max_index = (float)SDFT_MAX_HZ / (float)resolution_hz + 0.5f;
+  bin_batches = (bin_max_index - bin_min_index) / sub_samples + 1;
+
   r_to_N = powf(SDFT_DAMPING_FACTOR, SDFT_SAMPLE_SIZE);
 
   const complex_float j = 0.0f + _Complex_I * 1.0f;
@@ -69,7 +73,7 @@ bool sdft_push(sdft_t *sdft, float val) {
   sdft->sample_accumulator += val;
   sdft->sample_count++;
 
-  if (sdft->sample_count >= (uint32_t)SDFT_SUBSAMPLES) {
+  if (sdft->sample_count >= sub_samples) {
     sdft->sample_avg = sdft->sample_accumulator / (float)sdft->sample_count;
     sdft->sample_accumulator = 0;
     sdft->sample_count = 0;
@@ -194,7 +198,7 @@ bool sdft_update(sdft_t *sdft) {
         meanBin += upper_ratio - lower_ratio;
       }
 
-      const float f_hz = meanBin * SDFT_HZ_RESOLUTION;
+      const float f_hz = meanBin * (float)resolution_hz;
 
       const float filter_multi = constrain(sdft->peak_values[peak] / sdft->noise_floor, 1.0f, 10.0f);
       const float gain = LOOPTIME_S / (1 / (2.0f * M_PI_F * (filter_multi * SDFT_FILTER_HZ)) + LOOPTIME_S);
@@ -208,6 +212,15 @@ bool sdft_update(sdft_t *sdft) {
 
   case SDFT_UPDATE_FILTERS:
     sdft->state = SDFT_UPDATE_MAGNITUE;
+
+    // re-compute in case looptime changed
+    sub_samples = (SAMPLE_HZ / (2.0f * SDFT_MAX_HZ));
+    resolution_hz = ((SAMPLE_HZ / (float)sub_samples) / SDFT_SAMPLE_SIZE);
+
+    bin_min_index = (float)SDFT_MIN_HZ / (float)resolution_hz + 0.5f;
+    bin_max_index = (float)SDFT_MAX_HZ / (float)resolution_hz + 0.5f;
+    bin_batches = (bin_max_index - bin_min_index) / sub_samples + 1;
+
     filters_updated = true;
     break;
   }
diff --git a/src/flight/sdft.h b/src/flight/sdft.h
index cfd792fdd..2610d14f8 100644
--- a/src/flight/sdft.h
+++ b/src/flight/sdft.h
@@ -14,14 +14,10 @@
 #define SDFT_MIN_HZ 80
 #define SDFT_MAX_HZ 500
 
-#define SDFT_SAMPLE_SIZE 100
+#define SDFT_SAMPLE_SIZE 62
 #define SDFT_BIN_COUNT (SDFT_SAMPLE_SIZE / 2)
 
 #define SDFT_DAMPING_FACTOR 0.9999f
-#define SDFT_SAMPLE_HZ (1e6f / LOOPTIME)
-
-#define SDFT_SUBSAMPLES (SDFT_SAMPLE_HZ / (2 * SDFT_MAX_HZ))
-#define SDFT_HZ_RESOLUTION ((SDFT_SAMPLE_HZ / SDFT_SUBSAMPLES) / SDFT_SAMPLE_SIZE)
 
 typedef float complex complex_float;
 
diff --git a/src/util/util.h b/src/util/util.h
index 983832457..9ef11e999 100644
--- a/src/util/util.h
+++ b/src/util/util.h
@@ -19,6 +19,13 @@
 #define LOG2_16BIT(v) (8 * ((v) > 255) + LOG2_8BIT((v) >> 8 * ((v) > 255)))
 #define LOG2_32BIT(v) (16 * ((v) > 65535L) + LOG2_16BIT((v)*1L >> 16 * ((v) > 65535L)))
 
+#define SWAP(x, y)      \
+  {                     \
+    typeof(x) temp = x; \
+    x = y;              \
+    y = temp;           \
+  }
+
 #define constrain(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
 #define min(a, b) \
   ({ __typeof__ (a) _a = (a); \