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]);
+            }
         }
     }
 

imu.c

@@ -17,34 +17,34 @@ float biquad_apply(biquad_t *f, float x) {
     return out;
 }
 
-static float fast_sin_approx(float x) {
-    float x2 = x * x;
-    return x * (1.0f - (x2 * 0.16666667f) + (x2 * x2 * 0.0083333338f));
-}
-
-static float fast_cos_approx(float x) {
-    float x2 = x * x;
-    return 1.0f - (x2 * 0.5f) + (x2 * x2 * 0.041666668f) - (x2 * x2 * x2 * 0.0013888889f);
-}
-
 void biquad_init_notch(biquad_t *f, float center_freq, float Q, float fs) {
-    if (center_freq < 1.0f) {
+    // Защита: при слишком низкой/высокой частоте уводим фильтр в bypass.
+    if (center_freq < 1.0f || center_freq > (0.45f * fs) || Q <= 0.05f) {
         f->b0 = 1.0f; f->b1 = 0; f->b2 = 0;
         f->a1 = 0; f->a2 = 0;
+        f->d1 = 0; f->d2 = 0;
         return;
     }
-    
+
     float w0 = 2.0f * 3.14159265f * center_freq / fs;
-    float alpha = fast_sin_approx(w0) / (2.0f * Q);
-    float cosw0 = fast_cos_approx(w0);
-    
+    float alpha = sinf(w0) / (2.0f * Q);
+    float cosw0 = cosf(w0);
+
     float inv_a0 = 1.0f / (1.0f + alpha); // Считаем один раз инверсию
-    
+
     f->b0 = inv_a0;
     f->b1 = -2.0f * cosw0 * inv_a0;
     f->b2 = inv_a0;
     f->a1 = -2.0f * cosw0 * inv_a0;
     f->a2 = (1.0f - alpha) * inv_a0;
+
+    // Защита от NaN/Inf, чтобы не обнулялся выход при некорректной перенастройке.
+    if (!isfinite(f->b0) || !isfinite(f->b1) || !isfinite(f->b2) ||
+        !isfinite(f->a1) || !isfinite(f->a2)) {
+        f->b0 = 1.0f; f->b1 = 0; f->b2 = 0;
+        f->a1 = 0; f->a2 = 0;
+        f->d1 = 0; f->d2 = 0;
+    }
 }
 
 void I2C1_Init(void) {