vadychka/fft-filters
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

dhsot

Browse Source 
Co-authored-by: Copilot <copilot@github.com>
This commit is contained in:
vadyschka01
2026-05-27 16:10:40 +03:00
parent 29749c6212
commit babc5a24e3
12 changed files with 1436 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dsp_manager.c
+62 -145
View File
@@ -8,38 +8,33 @@ 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")));
// Коэффициенты биквадратного фильтра
float32_t b[3] = {1.0f, -2.0f, 1.0f}; // Примерные значения
float32_t a[3] = {1.0f, -1.8f, 0.81f};
// Буфер состояния фильтра
float32_t x[3] = {0};
float32_t y[3] = {0};
// Структура БПФ из библиотеки
// FFT handler
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};
// Публичные переменные (см. dsp_manager.h)
uint8_t fft_ready = 0;
uint16_t fft_index = 0;
volatile uint16_t notch_report_hz[3] = {0, 0, 0};
// Переменные для сглаживания частоты (чтобы не прыгала)
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 notch_freq_smoothed[3] = {0.0f, 0.0f, 0.0f};
volatile float32_t motor_erpm_avg = 0.0f;
volatile float32_t motor_mech_rpm = 0.0f;
volatile float32_t motor_elec_hz = 0.0f;
void DSP_SetMotorErpm(const uint16_t eRPM[4], uint8_t pole_pairs) {
// ERPM используем только как debug-метрику, не как источник notch-частот
float base_hz = (float)eRPM[3] / 60.0f;
motor_erpm_avg = (float)eRPM[3];
motor_elec_hz = base_hz;
motor_mech_rpm = (float)eRPM[3] / (float)pole_pairs;
}
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;
@@ -57,13 +52,13 @@ void DSP_Init(void) {
}
void DSP_AddSample(float32_t sample) {
if (dsp_buffer_ready) return; // Ждем, пока обработают прошлую пачку
if (fft_ready) return; // Ждем, пока обработают прошлую пачку
fft_input[sample_count++] = sample;
fft_input[fft_index++] = sample;
if (sample_count >= FFT_SIZE) {
sample_count = 0;
dsp_buffer_ready = 1; // Сигнализируем в main
if (fft_index >= FFT_SIZE) {
fft_index = 0;
fft_ready = 1; // Сигнализируем в main
}
}
@@ -77,137 +72,59 @@ void DSP_Process(void) {
// 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};
// Ищем один базовый пик в спектре и строим гармоники от него.
// Это убирает прыжки второго и третьего notch'ей между соседними пиками.
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);
for (int k = 0; k < 3; k++) {
float32_t max_val = 0.0f;
int32_t max_idx = -1;
float32_t base_peak_freq = 0.0f;
float32_t max_val = 0.0f;
int32_t max_idx = -1;
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 = search_start_bin; i < search_end_bin; i++) {
float32_t freq_hz = ((float32_t)i * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
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;
}
}
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 (uint32_t i = start_bin; i < (FFT_SIZE / 2); i++) {
if (magnitudes[i] > max_val) {
max_val = magnitudes[i];
max_idx = (int32_t)i;
}
}
// Применяем задержку подтверждения и сглаживание к частотам
for (int k = 0; k < 3; k++) {
if (current_iteration_freqs[k] > 1.0f) {
freq_missing_frames[k] = 0;
if (max_idx >= 0) {
base_peak_freq = ((float32_t)max_idx * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
}
if (freq_seen_frames[k] < 255) {
freq_seen_frames[k]++;
}
if (filtered_freqs[k] < 1.0f) {
if (freq_seen_frames[k] >= REQUIRED_STABLE_FRAMES) {
filtered_freqs[k] = current_iteration_freqs[k];
}
} else {
float32_t alpha = SMOOTH_ALPHA;
if (fabsf(current_iteration_freqs[k] - filtered_freqs[k]) > FAST_TRACK_DELTA_HZ) {
alpha = FAST_TRACK_ALPHA;
}
filtered_freqs[k] = (alpha * current_iteration_freqs[k]) +
((1.0f - alpha) * filtered_freqs[k]);
}
// Лёгкое сглаживание только для базовой частоты
if (base_peak_freq > 1.0f) {
if (notch_freq_smoothed[0] < 1.0f) {
notch_freq_smoothed[0] = base_peak_freq;
} else {
freq_seen_frames[k] = 0;
const float32_t alpha = 0.25f;
notch_freq_smoothed[0] = (alpha * base_peak_freq) + ((1.0f - alpha) * notch_freq_smoothed[0]);
}
if (freq_missing_frames[k] < 255) {
freq_missing_frames[k]++;
}
// Гармоники считаем строго от базы
notch_freq_smoothed[1] = notch_freq_smoothed[0] * 2.0f;
notch_freq_smoothed[2] = notch_freq_smoothed[0] * 3.0f;
}
if (freq_missing_frames[k] >= DISABLE_AFTER_MISSING_FRAMES) {
filtered_freqs[k] = 0.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;
}
}
// 5. ПЕРЕНАСТРОЙКА ФИЛЬТРОВ
float32_t Notch_Q = 2.5f; // Чуть шире яма, чтобы лучше удерживать пик и ловить дрейф
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);
biquad_init_notch(&notch1, notch_freq_smoothed[0], Notch_Q, DSP_SAMPLE_RATE_HZ);
biquad_init_notch(&notch2, notch_freq_smoothed[1], Notch_Q, DSP_SAMPLE_RATE_HZ);
biquad_init_notch(&notch3, notch_freq_smoothed[2], Notch_Q, DSP_SAMPLE_RATE_HZ);
// В телеметрию
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];
notch_report_hz[0] = (uint16_t)notch_freq_smoothed[0];
notch_report_hz[1] = (uint16_t)notch_freq_smoothed[1];
notch_report_hz[2] = (uint16_t)notch_freq_smoothed[2];
dsp_buffer_ready = 0;
// Готово
fft_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;
}