Cleanup: removed build artifacts and cache

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

drone_par_on_gx_up.py

@@ -31,7 +31,7 @@ fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8))
 
 # 1. Временной график
 line_raw, = ax1.plot(raw_buffer, color='red', lw=2.5, alpha=0.3, label='Грязный (Raw)')
-line_filt, = ax1.plot(filt_buffer, color='blue', lw=1.2, label='Фильтр')  # Обновлено название
+line_filt, = ax1.plot(filt_buffer, color='blue', lw=1.2, label='FMAC Фильтр')
 ax1.set_title("Осциллограф: Гироскоп X")
 ax1.set_ylim(-2000, 2000)
 ax1.legend(loc='upper right')

dsp_manager.c

@@ -1,213 +1,152 @@
 #include "dsp_manager.h"
 #include "imu.h"
-#include <math.h>
 
 // Буферы для расчета
-static float32_t fft_input[FFT_SIZE] __attribute__((section(".sram2")));
-static float32_t fft_output[FFT_SIZE] __attribute__((section(".sram2")));
-static float32_t magnitudes[FFT_SIZE / 2] __attribute__((section(".sram2")));
-static float32_t hann_window[FFT_SIZE] __attribute__((section(".sram2")));
+static float32_t fft_input[FFT_SIZE];
+static float32_t fft_output[FFT_SIZE];
+static float32_t magnitudes[FFT_SIZE / 2];
 
-// Коэффициенты биквадратного фильтра
-float32_t b[3] = {1.0f, -2.0f, 1.0f}; // Примерные значения
-float32_t a[3] = {1.0f, -1.8f, 0.81f};
+// Буфер для окна Ханна (убрать шумы по краям выборки)
+static float32_t hann_window[FFT_SIZE];
 
-// Буфер состояния фильтра
-float32_t x[3] = {0};
-float32_t y[3] = {0};
+static uint16_t sample_count = 0;
+uint8_t dsp_buffer_ready = 0;
 
 // Структура БПФ из библиотеки
 static arm_rfft_fast_instance_f32 fft_handler;
 
-// Определение переменных
-uint8_t dsp_buffer_ready = 0;
-uint16_t sample_count = 0;
-volatile uint16_t dsp_notch_freqs[3] = {0, 0, 0};
+// Частоты текущих 3 подавляющих фильтров
+float active_notch_freqs[3] = {0.0f, 0.0f, 0.0f};
 
-// Переменные для сглаживания частоты (чтобы не прыгала)
-static float32_t filtered_freqs[3] = {0.0f, 0.0f, 0.0f};
-const float32_t SMOOTH_ALPHA = 0.2f; // Коэффициент плавности (0.1 - очень медленно, 0.9 - мгновенно)
-const float32_t FAST_TRACK_ALPHA = 0.55f;
-const float32_t PEAK_THRESHOLD = 2200.0f;
-const float32_t FAST_TRACK_DELTA_HZ = 25.0f;
-const float32_t TRACK_WINDOW_HZ = 35.0f;
-static uint8_t freq_seen_frames[3] = {0, 0, 0};
-static uint8_t freq_missing_frames[3] = {0, 0, 0};
-const uint8_t REQUIRED_STABLE_FRAMES = 2;
-const uint8_t DISABLE_AFTER_MISSING_FRAMES = 5;
+//альфа
+static float32_t smoothed_freqs[3] = {0.0f, 0.0f, 0.0f};
+#define FREQ_ALPHA 0.05f  // время зависания nocha на частотном пике
 
 void DSP_Init(void) {
     // Инициализируем структуру БПФ
     arm_rfft_fast_init_f32(&fft_handler, FFT_SIZE);
     
     // Генерируем окно Ханна (делается один раз)
-    const float32_t sin_delta = 0.0246374509f;
-    const float32_t cos_delta = 0.9996964600f;
-    float32_t sin_angle = 0.0f;
-    float32_t cos_angle = 1.0f;
-
     for (int i = 0; i < FFT_SIZE; i++) {
-        hann_window[i] = 0.5f * (1.0f - cos_angle);
-
-        float32_t next_cos = (cos_angle * cos_delta) - (sin_angle * sin_delta);
-        float32_t next_sin = (sin_angle * cos_delta) + (cos_angle * sin_delta);
-        cos_angle = next_cos;
-        sin_angle = next_sin;
+        hann_window[i] = 0.5f * (1.0f - arm_cos_f32(2.0f * 3.14159f * i / (1023.0f)));
     }
 }
 
 void DSP_AddSample(float32_t sample) {
-    if (dsp_buffer_ready) return; // Ждем, пока обработают прошлую пачку
+    if (dsp_buffer_ready) return; // ожидание обработки прошлой пачки
 
     fft_input[sample_count++] = sample;
 
     if (sample_count >= FFT_SIZE) {
         sample_count = 0;
-        dsp_buffer_ready = 1; // Сигнализируем в main
+        dsp_buffer_ready = 1; // Сигнал в main
     }
 }
 
 void DSP_Process(void) {
-    // 1. Окно Ханна
+    // 1. Применяем окно Ханна
     arm_mult_f32(fft_input, hann_window, fft_input, FFT_SIZE);
 
-    // 2. БПФ
+    // 2. САМО БПФ
     arm_rfft_fast_f32(&fft_handler, fft_input, fft_output, 0);
 
-    // 3. Амплитуды
+    // 3. Считаем амплитуды
     arm_cmplx_mag_f32(fft_output, magnitudes, FFT_SIZE / 2);
 
-    // 4. Поиск 3-х режекторных частот.
-    // Активные фильтры трекаем локально вокруг уже захваченной частоты,
-    // а глобальный поиск используем только для первичного захвата.
-    float32_t current_iteration_freqs[3] = {0.0f, 0.0f, 0.0f};
-    const uint32_t start_bin = (uint32_t)((50.0f * FFT_SIZE) / DSP_SAMPLE_RATE_HZ);
-    const float32_t freq_per_bin = DSP_SAMPLE_RATE_HZ / FFT_SIZE;
-    const uint32_t track_window_bins = (uint32_t)(TRACK_WINDOW_HZ / freq_per_bin);
+    // 4. Поиск 3-х независимых самых мощных пиков
+    float32_t top_freq_indices[3] = {0};
+    float32_t top_mags[3] = {0};
+
+    // Индексы для поиска примерно от 95 Гц до 480 Гц
+    // index = freq * FFT_SIZE / fs = freq * 512 / 1000
+    uint32_t start_idx = 48; // ~94 Гц (95 * 512 / 1000 = 48.6)
+    uint32_t end_idx = 245;   // ~479 Гц (480 * 512 / 1000 = 245.8)
 
     for (int k = 0; k < 3; k++) {
-        float32_t max_val = 0.0f;
-        int32_t max_idx = -1;
+        float32_t max_m = 0;
+        uint32_t max_i = 0;
 
-        uint32_t search_start_bin = start_bin;
-        uint32_t search_end_bin = (FFT_SIZE / 2);
-
-        if (filtered_freqs[k] > 1.0f) {
-            int32_t center_bin = (int32_t)(filtered_freqs[k] / freq_per_bin);
-
-            if (center_bin < (int32_t)track_window_bins) {
-                search_start_bin = start_bin;
-            } else {
-                search_start_bin = (uint32_t)(center_bin - (int32_t)track_window_bins);
-            }
-
-            search_end_bin = (uint32_t)(center_bin + (int32_t)track_window_bins);
-            if (search_end_bin > (FFT_SIZE / 2)) {
-                search_end_bin = (FFT_SIZE / 2);
+        // Ищем глобальный максимум
+        for (uint32_t i = start_idx; i < end_idx; i++) {
+            if (magnitudes[i] > max_m) {
+                max_m = magnitudes[i];
+                max_i = i;
             }
         }
 
-        for (uint32_t i = search_start_bin; i < search_end_bin; i++) {
-            float32_t freq_hz = ((float32_t)i * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
+        top_mags[k] = max_m;
+        top_freq_indices[k] = (float32_t)max_i;
 
-            if (filtered_freqs[k] <= 1.0f) {
-                uint8_t too_close_to_locked = 0;
-                for (int locked = 0; locked < k; locked++) {
-                    if (filtered_freqs[locked] > 1.0f && fabsf(freq_hz - filtered_freqs[locked]) < 40.0f) {
-                        too_close_to_locked = 1;
-                        break;
-                    }
-                }
-
-                if (too_close_to_locked) {
-                    continue;
-                }
+        // "Зануляем" гору вокруг найденного пика (±10 бинов, ~±20 Гц)
+        // Чтобы следующий фильтр не прицепился к «склону» того же самого пика
+        if (max_i > 0) {
+            uint32_t clear_start = (max_i > 10) ? (max_i - 10) : 0;
+            uint32_t clear_end   = (max_i + 10 < (FFT_SIZE / 2)) ? (max_i + 10) : ((FFT_SIZE / 2) - 1);
+            for (uint32_t j = clear_start; j <= clear_end; j++) {
+                magnitudes[j] = 0.0f; 
             }
-
-            uint8_t too_close = 0;
-            for (int j = 0; j < k; j++) {
-                if (current_iteration_freqs[j] > 1.0f && fabsf(freq_hz - current_iteration_freqs[j]) < 40.0f) { // Разнос 40Гц
-                    too_close = 1;
-                    break;
-                }
-            }
-
-            if (!too_close && magnitudes[i] > max_val) {
-                max_val = magnitudes[i];
-                max_idx = i;
-            }
-        }
-
-        // Если нашли пик выше порога
-        if (max_idx != -1 && max_val > PEAK_THRESHOLD) {
-            current_iteration_freqs[k] = ((float32_t)max_idx * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
-        } else if (filtered_freqs[k] > 1.0f) {
-            // Если активный notch не нашел точку в окне, удерживаем текущую частоту,
-            // а не переводим его в гонку за соседним пиком.
-            current_iteration_freqs[k] = filtered_freqs[k];
         }
     }
 
-    // Применяем задержку подтверждения и сглаживание к частотам
-    for (int k = 0; k < 3; k++) {
-        if (current_iteration_freqs[k] > 1.0f) {
-            freq_missing_frames[k] = 0;
+    // --- 5. ПЕРЕНАСТРОЙКА ТРЕХ КАСКАДОВ FMAC ---
+    const float fs = 1000.0f;     // Частота дискретизации
+    const float Q = 2.5f;        // Добротность (уменьшена для расширения ямы, чтобы соседние пики подавлялись)
+    const float bin_to_hz = fs / (float)FFT_SIZE; 
 
-            if (freq_seen_frames[k] < 255) {
-                freq_seen_frames[k]++;
+    for (int i = 0; i < 3; i++) {
+        float mag = top_mags[i];
+        float new_freq = top_freq_indices[i] * bin_to_hz;
+        
+        // ГИСТЕРЕЗИС:
+        // Если фильтр сейчас ВЫКЛЮЧЕН (active_notch_freqs == 0)
+        if (active_notch_freqs[i] == 0) {
+            if (mag > 4000.0f) {
+                // Включаем фильтр. Чтобы не полз с нуля, присваиваем частоту сразу:
+                smoothed_freqs[i] = new_freq;
+                active_notch_freqs[i] = new_freq;
             }
-
-            if (filtered_freqs[k] < 1.0f) {
-                if (freq_seen_frames[k] >= REQUIRED_STABLE_FRAMES) {
-                    filtered_freqs[k] = current_iteration_freqs[k];
-                }
+        } 
+        // Если фильтр сейчас ВКЛЮЧЕН
+        else {
+            if (mag < 2000.0f) {
+                // Выключаем фильтр, так как амплитуда сильно упала
+                active_notch_freqs[i] = 0;
             } else {
-                float32_t alpha = SMOOTH_ALPHA;
-
-                if (fabsf(current_iteration_freqs[k] - filtered_freqs[k]) > FAST_TRACK_DELTA_HZ) {
-                    alpha = FAST_TRACK_ALPHA;
+                // Продолжаем отслеживать с Альфой (EMA)
+                if (fabsf(new_freq - smoothed_freqs[i]) > 20.0f) {
+                    // Большой прыжок частоты: сбрасываем память фильтра, чтобы не тянуть старый хвост
+                    dyn_notch_filters[i].d1 = 0.0f;
+                    dyn_notch_filters[i].d2 = 0.0f;
+                    smoothed_freqs[i] = new_freq;
+                } else {
+                    smoothed_freqs[i] = (smoothed_freqs[i] * (1.0f - FREQ_ALPHA)) + (new_freq * FREQ_ALPHA);
                 }
-
-                filtered_freqs[k] = (alpha * current_iteration_freqs[k]) +
-                                    ((1.0f - alpha) * filtered_freqs[k]);
+                active_notch_freqs[i] = smoothed_freqs[i];
             }
+        }
+        
+        // Применяем настройки
+        if (active_notch_freqs[i] > 0) {
+            float real_freq = active_notch_freqs[i];
+            
+            // Математика Notch-фильтра
+            float w0 = 2.0f * 3.14159265f * real_freq / fs;
+            float alpha = arm_sin_f32(w0) / (2.0f * Q);
+            float cosw0 = arm_cos_f32(w0);
+            float a0 = 1.0f + alpha;
+
+            float b0 = 1.0f / a0;
+            float b1 = -2.0f * cosw0 / a0;
+            float b2 = 1.0f / a0;
+            float a1 = -2.0f * cosw0 / a0;
+            float a2 = (1.0f - alpha) / a0;
+
+            Update_FMAC_Coeffs(i, b0, b1, b2, a1, a2);
         } else {
-            freq_seen_frames[k] = 0;
-
-            if (freq_missing_frames[k] < 255) {
-                freq_missing_frames[k]++;
-            }
-
-            if (freq_missing_frames[k] >= DISABLE_AFTER_MISSING_FRAMES) {
-                filtered_freqs[k] = 0.0f;
-            }
+            // Bypass
+            Update_FMAC_Coeffs(i, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f);
         }
     }
 
-    // 5. ПЕРЕНАСТРОЙКА ФИЛЬТРОВ
-    float32_t Notch_Q = 2.5f; // Чуть шире яма, чтобы лучше удерживать пик и ловить дрейф
-
-    // Вызываем инициализацию только если частота > 0
-    biquad_init_notch(&notch1, filtered_freqs[0], Notch_Q, 1000.0f);
-    biquad_init_notch(&notch2, filtered_freqs[1], Notch_Q, 1000.0f);
-    biquad_init_notch(&notch3, filtered_freqs[2], Notch_Q, 1000.0f);
-
-    // В телеметрию
-    dsp_notch_freqs[0] = (uint16_t)filtered_freqs[0];
-    dsp_notch_freqs[1] = (uint16_t)filtered_freqs[1];
-    dsp_notch_freqs[2] = (uint16_t)filtered_freqs[2];
-
-    dsp_buffer_ready = 0; 
+    dsp_buffer_ready = 0; // Разрешаем новый сбор данных
 }
-
-// Реализация функции Biquad_Filter
-float32_t Biquad_Filter(float32_t input) {
-    float32_t output = b[0] * input + b[1] * x[1] + b[2] * x[2] - a[1] * y[1] - a[2] * y[2];
-
-    x[2] = x[1];
-    x[1] = input;
-    y[2] = y[1];
-    y[1] = output;
-
-    return output;
-}

dsp_manager.h

@@ -4,21 +4,16 @@
 #include "arm_math.h"
 
 // Размер окна Фурье (степень двойки)
-// Уменьшен для экономии памяти
-#define FFT_SIZE 256
-#define DSP_SAMPLE_RATE_HZ 1000.0f
+#define FFT_SIZE 512
 
 // Прототипы
 void DSP_Init(void);
 void DSP_AddSample(float32_t sample); // Добавить одну точку в "копилку"
-void DSP_Process(void);              // Запустить расчет (когда накопили FFT_SIZE)
+void DSP_Process(void);              // Запустить расчет (когда накопили 1024)
 
-// Объявление переменных
+// Флаг готовности данных (чтобы main знал, когда пора вызывать Process)
 extern uint8_t dsp_buffer_ready;
-extern uint16_t sample_count;
-extern volatile uint16_t dsp_notch_freqs[3];
-
-// Добавление объявления переменной и прототипа функции
-float32_t Biquad_Filter(float32_t input);
+// Список частот, которые в данный момент подавляются фильтрами
+extern float active_notch_freqs[3];
 
 #endif

fft_az.ewp

@@ -237,6 +237,10 @@
                     <name>CCDefines</name>
                     <state>STM32G431xx</state>
                     <state>ARM_MATH_CM4</state>
+                    <state>ARM_DSP_CONFIG_TABLES_ALL_VALUE=0</state>
+                    <state>ARM_TABLE_TWIDDLECOEF_F32_256</state>
+                    <state>ARM_TABLE_BITREVIDX_FLT_256</state>
+                    <state>ARM_TABLE_BITREVIDX_FLT_1024</state>
                 </option>
                 <option>
                     <name>CCPreprocFile</name>
@@ -684,7 +688,7 @@
                 </option>
                 <option>
                     <name>OOCOutputFile</name>
-                    <state>fft_az.srec</state>
+                    <state></state>
                 </option>
                 <option>
                     <name>OOCCommandLineProducer</name>

imu.c

@@ -1,52 +1,71 @@
 #include "imu.h"
-#include "arm_math.h"
 #include <math.h>
+#include "stm32g4xx.h"
+#include "stm32g431xx.h"
 
-volatile int16_t raw_ax, raw_ay, raw_az;
-volatile int16_t raw_gx, raw_gy, raw_gz;
+#ifndef FMAC_PARAM_FUNC_Pos
+#define FMAC_PARAM_FUNC_Pos   (0U)
+#define FMAC_PARAM_P_Pos      (8U)
+#define FMAC_PARAM_Q_Pos      (16U)
+#define FMAC_PARAM_RSHIFT_Pos (24U)
+#endif
+
+#ifndef FMAC_SR_VLD
+#define FMAC_SR_VLD           (1U << 0)
+
+#endif
+
+
+// Константы смещения в памяти FMAC (всего 256 слов)
+// Каждая Notch-секция (IIR 2-го порядка) требует:
+// 3 коэфф. B, 2 коэфф. A, 2 ячейки истории X, 2 ячейки истории Y.
+#define FMAC_MEM_SIZE      256
+#define STAGE_SIZE         10  // Резервируем с запасом под каждый каскад
+
+volatile int16_t raw_gx; // только  гироскопа X
 float filt_gx;
 float gyro_bias_x = 0;
 
-// Сами фильтры
-biquad_t notch1, notch2, notch3;
+// notch1, notch2, notch3 удалены (заменены на notch_fmac_coeffs[3])
+// biquad_apply и biquad_init_notch удалены (больше не нужны с FMAC)
 
-float biquad_apply(biquad_t *f, float x) {
-    float out = f->b0 * x + f->d1;
-    f->d1 = f->b1 * x - f->a1 * out + f->d2;
-    f->d2 = f->b2 * x - f->a2 * out;
-    return out;
-}
+#define M_PI 3.14159265358979323846f
 
-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) {
-        f->b0 = 1.0f; f->b1 = 0; f->b2 = 0;
-        f->a1 = 0; f->a2 = 0;
-        return;
-    }
+void biquad_init_notch(biquad_t *f, float freq, float q, float fs) {
+    float omega = 2.0f * M_PI * freq / fs;
+    float alpha = sinf(omega) / (2.0f * q);
     
-    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 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;
+    float a0 = 1.0f + alpha;
+    f->b0 = 1.0f / a0;
+    f->b1 = -2.0f * cosf(omega) / a0;
+    f->b2 = 1.0f / a0;
+    f->a1 = -2.0f * cosf(omega) / a0;
+    f->a2 = (1.0f - alpha) / a0;
+    f->d1 = 0;
+    f->d2 = 0;
 }
 
+fmac_coeffs_t notch_fmac_coeffs[3];
+fmac_state_t  notch_fmac_state[3];
+
+// 1. Инициализация (с правильной разметкой памяти)
+void FMAC_Init(void) {
+    RCC->AHB1ENR |= RCC_AHB1ENR_FMACEN;
+    RCC->AHB1RSTR |= RCC_AHB1RSTR_FMACRST;
+    for(volatile int i=0; i<100; i++);
+    RCC->AHB1RSTR &= ~RCC_AHB1RSTR_FMACRST;
+
+    // Конфигурация памяти: X1 (коэф), X2 (входы), Y (выходы)
+    FMAC->X1BUFCFG = (5 << 8) | (0 << 0); // 5 коэф. с адреса 0
+    FMAC->X2BUFCFG = (2 << 8) | (5 << 0); // 2 входа с адреса 5
+    FMAC->YBUFCFG  = (2 << 8) | (7 << 0); // 2 выхода с адреса 7
+
+    FMAC->CR = 0x01; // Включаем модуль
+}
+
+
+
+
 void I2C1_Init(void) {
     RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
     RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
@@ -83,6 +102,18 @@ static void IMU_WriteReg(uint8_t reg, uint8_t val) {
 
 void IMU_SetBank(uint8_t bank) { IMU_WriteReg(0x7F, (bank & 0x03) << 4); }
 
+float biquad_apply(biquad_t *f, float x) {
+    // Реализация Direct Form II (правильная математика для IIR)
+    float w = x - f->a1 * f->d1 - f->a2 * f->d2;
+    float y = f->b0 * w + f->b1 * f->d1 + f->b2 * f->d2;
+    f->d2 = f->d1;
+    f->d1 = w;
+    return y;
+}
+
+// 3 динамических каскада
+biquad_t dyn_notch_filters[3];
+
 void IMU_Init(void) {
     // Пробуждение...
     IMU_SetBank(0);
@@ -91,13 +122,25 @@ void IMU_Init(void) {
     IMU_WriteReg(0x07, 0x00);
     
     IMU_SetBank(2);
-    IMU_WriteReg(0x01, 0x01); // Bypass (отключаем встроенный фильтр для анализа)
+    // ICM-20948: GYRO_CONFIG_1 находится в нулевом регистре второго банка. 
+    // Значение GYRO_DLPFCFG = 7 для макс. ширины без полного байпаса: ~361.4 Hz LPF.
+    // (0 - это 196Hz, 1 - 151Hz, 2 - 119Hz, 3 - 51Hz, 7 - 361.4Hz)
+    // GYRO_FS_SEL = 3 (2000 dps)
+    // ВНИМАНИЕ: На ICM-20948 бит GYRO_FCHOICE (бит 0) включает LPF, если равен 1. (а 0 = Bypass)
+    IMU_WriteReg(0x01, (7 << 3) | (3 << 1) | 1); 
+    
     IMU_SetBank(0);
 
-    // Начальная инициализация ( на 0 Гц dsp_manager сам их включит)
-    biquad_init_notch(&notch1, 0, 1.0f, 1000.0f);
-    biquad_init_notch(&notch2, 0, 1.0f, 1000.0f);
-    biquad_init_notch(&notch3, 0, 1.0f, 1000.0f);
+    // Инициализируем фильтры в режиме Bypass (пропускание)
+    for (int i = 0; i < 3; i++) {
+        dyn_notch_filters[i].b0 = 1.0f;
+        dyn_notch_filters[i].b1 = 0;
+        dyn_notch_filters[i].b2 = 0;
+        dyn_notch_filters[i].a1 = 0;
+        dyn_notch_filters[i].a2 = 0;
+        dyn_notch_filters[i].d1 = 0;
+        dyn_notch_filters[i].d2 = 0;
+    }
 }
 
 void IMU_Calibrate(void) {
@@ -114,14 +157,91 @@ void IMU_ReadRawData(void) {
     uint8_t buf[14];
     I2C_ReadMulti(IMU_ADDR, 0x2D, buf, 14);
     
-    raw_gx = (int16_t)(buf[6] << 8 | buf[7]);
-    float x = (float)raw_gx - gyro_bias_x;
+    // 1. Читаем сырое
+    int16_t gyro_x_raw = (int16_t)(buf[6] << 8 | buf[7]);
+    
+    // 2. Центрируем относительно нуля (убираем дрейф)
+    float x = (float)gyro_x_raw - gyro_bias_x;
+    
+    // 3. Сохраняем это в raw_gx
+    raw_gx = (int16_t)x; 
 
-    // Последовательно применяем 3 режекторных фильтра
-    // dsp_manager будет менять их коэффициенты в фоновом режиме
-    x = biquad_apply(&notch1, x);
-    x = biquad_apply(&notch2, x);
-    x = biquad_apply(&notch3, x);
+    // 4. Прогоняем через 3 каскада программных фильтров (режектор)
+    float x_filtered = x;
+    x_filtered = biquad_apply(&dyn_notch_filters[0], x_filtered);
+    x_filtered = biquad_apply(&dyn_notch_filters[1], x_filtered);
+    x_filtered = biquad_apply(&dyn_notch_filters[2], x_filtered);
 
-    filt_gx = x;
-}
+    // 5. Сохраняем в filt_gx
+    filt_gx = x_filtered;
+}
+
+void Update_FMAC_Coeffs(int stage, float b0, float b1, float b2, float a1, float a2) {
+    if (stage < 0 || stage > 2) return;
+    
+    // Обновляем софтверные фильтры вместо FMAC
+    dyn_notch_filters[stage].b0 = b0;
+    dyn_notch_filters[stage].b1 = b1;
+    dyn_notch_filters[stage].b2 = b2;
+    dyn_notch_filters[stage].a1 = a1;
+    dyn_notch_filters[stage].a2 = a2;
+    
+    // Если переходим из Bypass (b0≈1) в активный фильтр (b0<1),
+    // обнуляем состояние, чтобы избежать скачков
+    if (b0 < 0.9f) {
+        dyn_notch_filters[stage].d1 = 0;
+        dyn_notch_filters[stage].d2 = 0;
+    }
+}
+
+// Внутренняя функция для обработки одного каскада через FMAC
+// 2. Шаг вычислений (с защитой от зависания и обнуления)
+static int16_t FMAC_Step(fmac_coeffs_t *c, fmac_state_t *s, int16_t input) {
+    // Если фильтр в режиме Bypass (b0=16384, b1=0), просто возвращаем вход
+    if (c->b0 == 16384 && c->b1 == 0) return input;
+
+    // Сброс FIFO перед каждой операцией (критично для Polling режима)
+    FMAC->CR &= ~0x01; 
+    FMAC->CR |= 0x01;
+
+    // Пишем коэффы (5 штук)
+    FMAC->WDATA = c->b0; FMAC->WDATA = c->b1; FMAC->WDATA = c->b2;
+    FMAC->WDATA = c->a1; FMAC->WDATA = c->a2;
+    
+    // Пишем историю (4 штуки)
+    FMAC->WDATA = s->x1; FMAC->WDATA = s->x2;
+    FMAC->WDATA = s->y1; FMAC->WDATA = s->y2;
+
+    // Настройка: FUNC=8 (IIR), P=3, Q=2, RSHIFT=1 (бит 24)
+    // RSHIFT=1 компенсирует масштаб 16384
+    FMAC->PARAM = (1U << 24) | (2U << 16) | (3U << 8) | (8 << 0);
+    
+    FMAC->WDATA = input;
+    
+    uint32_t timeout = 1000;
+    while (!(FMAC->SR & 0x01) && --timeout);  // Ждем флаг VLD (Valid Data)
+    
+    if (timeout == 0) return input; 
+
+    int16_t result = (int16_t)FMAC->RDATA;
+
+    // Если FMAC выдал ровно 0 при живом входе - это ошибка, возвращаем вход
+    if (result == 0 && input != 0) return input;
+
+    // Сохраняем состояние
+    s->x2 = s->x1; s->x1 = input;
+    s->y2 = s->y1; s->y1 = result;
+
+    return result;
+}
+
+// 3. Главная точка входа
+float FMAC_Process_Sample(float input) {
+    int16_t val = (int16_t)input;
+    
+    val = FMAC_Step(&notch_fmac_coeffs[0], &notch_fmac_state[0], val);
+    val = FMAC_Step(&notch_fmac_coeffs[1], &notch_fmac_state[1], val);
+    val = FMAC_Step(&notch_fmac_coeffs[2], &notch_fmac_state[2], val);
+    
+    return (float)val;
+}

imu.h

@@ -10,18 +10,45 @@ typedef struct {
     float d1, d2;
 } biquad_t;
 
-// Делаем фильтры видимыми для dsp_manager
-extern biquad_t notch1, notch2, notch3;
+// 3 динамических программных фильтра (вместо FMAC)
+extern biquad_t dyn_notch_filters[3];
+
+void biquad_init_notch(biquad_t *f, float freq, float q, float fs);
+float biquad_apply(biquad_t *f, float x);
 
 // Прототипы
 void I2C1_Init(void);
 void IMU_Init(void);
 void IMU_Calibrate(void);
 void IMU_ReadRawData(void);
-void biquad_init_notch(biquad_t *f, float center_freq, float Q, float fs);
+// biquad_init_notch удалена (заменена на Update_FMAC_Coeffs в dsp_manager)
 
 // Данные
 extern volatile int16_t raw_gx; // Нам для анализа нужен только GX
 extern float filt_gx;
 
+// Добавить в imu.h
+typedef struct {
+    int16_t b0, b1, b2; // Коэффициенты числителя
+    int16_t a1, a2;     // Коэффициенты знаменателя (инвертированные для FMAC)
+} fmac_weights_t;
+
+void FMAC_Init(void);
+float FMAC_Process_Sample(float input);
+void Update_FMAC_Coeffs(int stage, float b0, float b1, float b2, float a1, float a2);
+
+typedef struct {
+    int16_t b0, b1, b2;
+    int16_t a1, a2;
+} fmac_coeffs_t;
+
+typedef struct {
+    int16_t x1, x2; // История входов (x[n-1], x[n-2])
+    int16_t y1, y2; // История выходов (y[n-1], y[n-2])
+} fmac_state_t;
+
+// Внешние структуры для 3-х каскадов
+extern fmac_coeffs_t notch_fmac_coeffs[3];
+extern fmac_state_t  notch_fmac_state[3];
+
 #endif

main.c

@@ -11,18 +11,19 @@ volatile uint32_t m2_speed = 900;
 volatile uint32_t m3_speed = 900;
 volatile uint32_t m4_speed = 900;
 
+// В main.c меняем структуру
 #pragma pack(push, 1)
 typedef struct {
-    uint8_t  header[2];
-    int16_t  gx;
-    int16_t  filt_gx;
-    uint16_t notch1_hz;
-    uint16_t notch2_hz;
-    uint16_t notch3_hz;
+    uint8_t  header[2]; // 0xAA, 0xBB
+    int16_t  gx;        // Сырой гиро
+    int16_t  filt_gx;   // Отфильтрованный гиро
+    uint16_t freq1;     // Пик 1 (Гц)
+    uint16_t freq2;     // Пик 2 (Гц)
+    uint16_t freq3;     // Пик 3 (Гц)
 } Telemetry_t;
 #pragma pack(pop)
 
-// 2. ПРОТОТИПЫ (Чтобы компилятор не ругался)
+// 2. ПРОТОТИПЫ
 void SystemClock_Config_160MHz(void);
 void UART2_Init_921600(void);
 void TIM6_Init_1000Hz(void);
@@ -37,6 +38,9 @@ int main(void) {
 
     I2C1_Init();
     UART2_Init_921600();
+    
+    FMAC_Init(); 
+
     IMU_Init();
     IMU_Calibrate();
     
@@ -48,34 +52,33 @@ int main(void) {
     Telemetry_t pkt;
     pkt.header[0] = 0xAA; pkt.header[1] = 0xBB;
 
-    while (1) {
+  
+    // В основном цикле while(1) внутри if (imu_flag)
+    while (1) { 
         if (imu_flag) {
-              imu_flag = 0;
+            imu_flag = 0;
+            IMU_ReadRawData();
+            DSP_AddSample((float32_t)raw_gx);
+            
+            pkt.gx = raw_gx;
+            pkt.filt_gx = (int16_t)filt_gx;
+            
+            // Заполняем частоты из dsp_manager.c
+            // extern float active_notch_freqs[3]; // Если компилятор не видит, добавьте в заголовок
+            pkt.freq1 = (uint16_t)active_notch_freqs[0];
+            pkt.freq2 = (uint16_t)active_notch_freqs[1];
+            pkt.freq3 = (uint16_t)active_notch_freqs[2];
 
-              IMU_ReadRawData(); 
-
-              //сохраняем во временную переменную
-              int16_t gx_val = raw_gx; 
-              DSP_AddSample((float32_t)gx_val);
-
-              if (dsp_buffer_ready) {
-                  DSP_Process(); 
-              }
-
-              float32_t filt_gx_value = filt_gx;
-              pkt.gx = gx_val;
-              pkt.filt_gx = (int16_t)filt_gx_value;
-              pkt.notch1_hz = dsp_notch_freqs[0];
-              pkt.notch2_hz = dsp_notch_freqs[1];
-              pkt.notch3_hz = dsp_notch_freqs[2];
-              UART_SendPacket(&pkt);
-
-              Set_Motor_Individual(m1_speed, m2_speed, m3_speed, m4_speed);
-          }
+            UART_SendPacket(&pkt);
+            Set_Motor_Individual(m1_speed, m2_speed, m3_speed, m4_speed);
+        }
+        
+        if (dsp_buffer_ready) {
+            DSP_Process();
         }
     }
-
-// 4. РЕАЛИЗАЦИЯ ФУНКЦИЙ (Тут был провал - их не хватало!)
+    }
+// 4. РЕАЛИЗАЦИЯ ФУНКЦИЙ
 
 void SystemClock_Config_160MHz(void) {
     RCC->CR |= RCC_CR_HSION;