last_fft

dsp_manager.c

@@ -72,44 +72,50 @@ void DSP_Process(void) {
     // 3. Амплитуды
     arm_cmplx_mag_f32(fft_output, magnitudes, FFT_SIZE / 2);
 
-    // Ищем один базовый пик в спектре и строим гармоники от него.
-    // Это убирает прыжки второго и третьего notch'ей между соседними пиками.
+    // Ищем 3 сильнейших пика в спектре и на них строим notch'и.
+    // Телеметрия ERPM здесь не участвует, чтобы не уводить фильтры в неверные Гц.
+    float32_t peak_freqs[3] = {0.0f, 0.0f, 0.0f};
     const uint32_t start_bin = (uint32_t)((50.0f * FFT_SIZE) / DSP_SAMPLE_RATE_HZ);
 
-    float32_t base_peak_freq = 0.0f;
-    float32_t max_val = 0.0f;
-    int32_t max_idx = -1;
-
-    for (uint32_t i = start_bin; i < (FFT_SIZE / 2); i++) {
-        if (magnitudes[i] > max_val) {
-            max_val = magnitudes[i];
-            max_idx = (int32_t)i;
-        }
-    }
-
-    if (max_idx >= 0) {
-        base_peak_freq = ((float32_t)max_idx * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
-    }
-
-    // Лёгкое сглаживание только для базовой частоты
-    if (base_peak_freq > 1.0f) {
-        if (notch_freq_smoothed[0] < 1.0f) {
-            notch_freq_smoothed[0] = base_peak_freq;
-        } else {
-            const float32_t alpha = 0.25f;
-            notch_freq_smoothed[0] = (alpha * base_peak_freq) + ((1.0f - alpha) * notch_freq_smoothed[0]);
-        }
-
-        // Гармоники считаем строго от базы
-        notch_freq_smoothed[1] = notch_freq_smoothed[0] * 2.0f;
-        notch_freq_smoothed[2] = notch_freq_smoothed[0] * 3.0f;
-    }
-
-    // Не даём гармоникам уйти за предел Найквиста
-    const float32_t nyquist = (DSP_SAMPLE_RATE_HZ * 0.5f) - 1.0f;
     for (int k = 0; k < 3; k++) {
-        if (notch_freq_smoothed[k] > nyquist) {
-            notch_freq_smoothed[k] = 0.0f;
+        float32_t max_val = 0.0f;
+        int32_t max_idx = -1;
+
+        for (uint32_t i = start_bin; i < (FFT_SIZE / 2); i++) {
+            float32_t freq_hz = ((float32_t)i * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
+
+            // Не даём второму и третьему ноту садиться на уже выбранные пики.
+            uint8_t too_close = 0;
+            for (int j = 0; j < k; j++) {
+                if (peak_freqs[j] > 1.0f && fabsf(freq_hz - peak_freqs[j]) < 40.0f) {
+                    too_close = 1;
+                    break;
+                }
+            }
+            if (too_close) {
+                continue;
+            }
+
+            if (magnitudes[i] > max_val) {
+                max_val = magnitudes[i];
+                max_idx = (int32_t)i;
+            }
+        }
+
+        if (max_idx >= 0) {
+            peak_freqs[k] = ((float32_t)max_idx * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
+        }
+    }
+
+    // Лёгкое сглаживание только чтобы частоты не дрожали между соседними бинами
+    for (int k = 0; k < 3; k++) {
+        if (peak_freqs[k] > 1.0f) {
+            if (notch_freq_smoothed[k] < 1.0f) {
+                notch_freq_smoothed[k] = peak_freqs[k];
+            } else {
+                const float32_t alpha = 0.25f;
+                notch_freq_smoothed[k] = (alpha * peak_freqs[k]) + ((1.0f - alpha) * notch_freq_smoothed[k]);
+            }
         }
     }