save progress

dsp_manager.c

@@ -21,7 +21,7 @@ void DSP_Init(void) {
     
     // Генерируем окно Ханна (делается один раз)
     for (int i = 0; i < FFT_SIZE; i++) {
-        hann_window[i] = 0.5f * (1.0f - arm_cos_f32(2.0f * PI * i / (FFT_SIZE - 1)));
+        hann_window[i] = 0.5f * (1.0f - arm_cos_f32(2.0f * 3.14159f * i / (1023.0f)));
     }
 }
 
@@ -37,37 +37,60 @@ void DSP_AddSample(float32_t sample) {
 }
 
 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. Считаем амплитуды (Magnitudes)
+    // 3. Считаем амплитуды
     arm_cmplx_mag_f32(fft_output, magnitudes, FFT_SIZE / 2);
 
     // 4. Поиск 3-х самых мощных пиков
-    float32_t top_freqs[3] = {0};
+    float32_t top_freq_indices[3] = {0};
     float32_t top_mags[3] = {0};
 
-    // Ищем в диапазоне от 50 Гц до 450 Гц (чтобы не задеть полезный сигнал наклона)
-    // Т.к. частота опроса 1000 Гц, а точек 1024, индекс массива почти равен частоте в Гц
     for (uint32_t i = 50; i < 450; i++) {
         if (magnitudes[i] > top_mags[0]) {
-            // Сдвигаем старые значения
-            top_mags[2] = top_mags[1]; top_freqs[2] = top_freqs[1];
-            top_mags[1] = top_mags[0]; top_freqs[1] = top_freqs[0];
-            // Записываем новый топ-1
+            top_mags[2] = top_mags[1]; top_freq_indices[2] = top_freq_indices[1];
+            top_mags[1] = top_mags[0]; top_freq_indices[1] = top_freq_indices[0];
             top_mags[0] = magnitudes[i];
-            top_freqs[0] = (float32_t)i;
+            top_freq_indices[0] = (float32_t)i;
         }
     }
 
-    // 5. ПЕРЕНАСТРОЙКА ФИЛЬТРОВ в imu.c "на лету"
-    //  динамически меняем частоты notch1, notch2, notch3
-    if (top_mags[0] > 10.0f) biquad_init_notch(&notch1, top_freqs[0], 1.0f, 1000.0f);
-    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);
+    // --- 5. ПЕРЕНАСТРОЙКА ТРЕХ КАСКАДОВ FMAC ---
+    const float fs = 1000.0f;     // Частота дискретизации
+    const float Q = 1.5f;        // Добротность (ширина выреза, 1.0 - 2.0 норм)
+    const float bin_to_hz = fs / (float)FFT_SIZE; 
+
+    for (int i = 0; i < 3; i++) {
+        // Если амплитуда выше порога, настраиваем фильтр
+        if (top_mags[i] > 3.0f) {
+            float real_freq = top_freq_indices[i] * bin_to_hz;
+            
+            // Математика 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;
+
+            // Коэффициенты для передачи в FMAC
+            // Мы делим на a0 сразу здесь
+            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 {
+            // Если пика нет, ставим фильтр в режим Bypass (пропускает сигнал без изменений)
+            // b0 = 1.0, остальные 0. Это даст y[n] = 1.0 * x[n]
+            Update_FMAC_Coeffs(i, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+        }
+    }
 
     dsp_buffer_ready = 0; // Разрешаем новый сбор данных
 }