filter_with_fmac

Co-authored-by: Copilot <copilot@github.com>

dsp_manager.c

@@ -1,5 +1,6 @@
 #include "dsp_manager.h"
 #include "imu.h"
+#include <math.h>
 
 // Буферы для расчета
 static float32_t fft_input[FFT_SIZE];
@@ -9,12 +10,20 @@ static float32_t magnitudes[FFT_SIZE / 2];
 // Буфер для окна Ханна (чтобы убрать шумы по краям выборки)
 static float32_t hann_window[FFT_SIZE];
 
-static uint16_t sample_count = 0;
-uint8_t dsp_buffer_ready = 0;
+// Коэффициенты биквадратного фильтра
+static float32_t b[3] = {1.0f, -2.0f, 1.0f}; // Примерные значения
+static float32_t a[3] = {1.0f, -1.8f, 0.81f};
+
+// Буфер состояния фильтра
+static float32_t x[3] = {0};
+static float32_t y[3] = {0};
 
 // Структура БПФ из библиотеки
 static arm_rfft_fast_instance_f32 fft_handler;
 
+// Уточнение области видимости переменной sample_count
+static uint16_t sample_count = 0;
+
 void DSP_Init(void) {
     // Инициализируем структуру БПФ
     arm_rfft_fast_init_f32(&fft_handler, FFT_SIZE);
@@ -69,5 +78,13 @@ void DSP_Process(void) {
     if (top_mags[1] > 10.0f) biquad_init_notch(&notch2, top_freqs[1], 1.0f, 1000.0f);
     if (top_mags[2] > 10.0f) biquad_init_notch(&notch3, top_freqs[2], 1.0f, 1000.0f);
 
+    // 6. Применяем биквадратный фильтр к входным данным
+    for (int i = 0; i < FFT_SIZE; i++) {
+        fft_input[i] = Biquad_Filter(fft_input[i]);
+    }
+
     dsp_buffer_ready = 0; // Разрешаем новый сбор данных
-}
+}
+
+// Прототип функции Biquad_Filter
+float32_t Biquad_Filter(float32_t input);