feat(FFT): добавлено FFT, выделение основной частоты

firmware/App/Src/audio_processor.c

@@ -0,0 +1,112 @@
+#include "audio_processor.h"
+#include <math.h>
+#include <string.h>
+#include "arm_math.h"
+#include "stm32f1xx.h"
+
+#ifndef AUDIO_FFT_SIZE
+#define AUDIO_FFT_SIZE 512U
+#endif
+
+#if (AUDIO_FFT_SIZE != 512U)
+#error "This module currently expects AUDIO_FFT_SIZE == 512"
+#endif
+
+#define ADC_FULL_SCALE 4095.0f
+#define ADC_MID_SCALE 2048.0f
+#define EPS_RMS 1e-12f
+
+static arm_rfft_fast_instance_f32 rfft;
+
+// ОПТИМИЗАЦИЯ: используем один буфер для in/out FFT
+static float32_t fft_buffer[AUDIO_FFT_SIZE];  // 2KB
+static float32_t mag[AUDIO_FFT_SIZE / 2U];    // 1KB (bins 0..N/2-1)
+
+static uint32_t bin_min = 0;
+static uint32_t bin_max = 0;
+static float32_t hz_per_bin = 0.0f;
+
+// Inline Hann window (без хранения коэффициентов)
+static inline float32_t hann_coeff(uint32_t i, uint32_t n) {
+    const float32_t two_pi = 6.28318530717958647693f;
+    const float32_t denom = (float32_t)(n - 1U);
+    return 0.5f - 0.5f * arm_cos_f32(two_pi * (float32_t)i / denom);
+}
+
+bool audio_processor_init(void) {
+    // FFT init
+    if (arm_rfft_fast_init_512_f32(&rfft) != ARM_MATH_SUCCESS) { return false; }
+
+    hz_per_bin = ((float32_t)AUDIO_SAMPLE_RATE) / ((float32_t)AUDIO_FFT_SIZE);
+
+    // Диапазон поиска пика 100..8000 Hz
+    bin_min = (uint32_t)ceilf(100.0f / hz_per_bin);
+    bin_max = (uint32_t)floorf(8000.0f / hz_per_bin);
+
+    // safety
+    if (bin_min < 1U) bin_min = 1U;
+    const uint32_t last_bin = (AUDIO_FFT_SIZE / 2U) - 1U;
+    if (bin_max > last_bin) bin_max = last_bin;
+
+    return true;
+}
+
+bool audio_processor_process_512(
+    const audio_sample_t* samples,
+    audio_metrics_t* out) {
+    if (!samples || !out) return false;
+
+    // 1) Mean + clipping detect
+    uint32_t sum = 0;
+    uint8_t clipped = 0;
+
+    for (uint32_t i = 0; i < AUDIO_FFT_SIZE; i++) {
+        const uint16_t s = samples[i];
+        sum += s;
+        if (s == 0U || s == 4095U) clipped = 1;
+    }
+
+    const float32_t mean = (float32_t)sum / (float32_t)AUDIO_FFT_SIZE;
+
+    // 2) RMS of AC component + prepare FFT input (normalized, windowed)
+    float32_t acc = 0.0f;
+
+    for (uint32_t i = 0; i < AUDIO_FFT_SIZE; i++) {
+        // centered around 0, normalized to roughly [-1..1]
+        float32_t x = ((float32_t)samples[i] - mean) / ADC_MID_SCALE;
+        acc += x * x;
+
+        // apply window inline (saves 2KB RAM)
+        fft_buffer[i] = x * hann_coeff(i, AUDIO_FFT_SIZE);
+    }
+
+    const float32_t rms = sqrtf(acc / (float32_t)AUDIO_FFT_SIZE);
+    const float32_t rms_dbfs = 20.0f * log10f(rms + EPS_RMS);
+
+    // 3) FFT (in-place: fft_buffer используется для in/out)
+    arm_rfft_fast_f32(&rfft, fft_buffer, fft_buffer, 0);
+
+    // 4) Magnitudes for bins 0..N/2-1
+    // CMSIS layout: [Re(0), Im(0)=0, Re(1), Im(1), ..., Re(N/2), Im(N/2)=0]
+    // Мы берём bin 1..N/2-1 для поиска пика
+    arm_cmplx_mag_f32(fft_buffer, mag, AUDIO_FFT_SIZE / 2U);
+
+    // 5) Peak search in desired band (skip DC bin 0)
+    uint32_t best_bin = bin_min;
+    float32_t best_mag = 0.0f;
+
+    for (uint32_t k = bin_min; k <= bin_max; k++) {
+        const float32_t m = mag[k];
+        if (m > best_mag) {
+            best_mag = m;
+            best_bin = k;
+        }
+    }
+
+    out->rms_dbfs = rms_dbfs;
+    out->peak_mag = best_mag;
+    out->peak_hz = (float32_t)best_bin * hz_per_bin;
+    out->clipped = clipped;
+
+    return true;
+}