From a6b27da4ce1d3cf60a251225beff3e6d0af1193e Mon Sep 17 00:00:00 2001 From: Razvalyaev Date: Sat, 6 Dec 2025 17:58:58 +0300 Subject: [PATCH] =?UTF-8?q?=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=BE?= =?UTF-8?q?=D0=B5=D0=BD=20=D0=BF=D0=BE=D0=BB=D0=BE=D1=81=D0=BE=D0=B2=D0=BE?= =?UTF-8?q?=D0=B9=20=D1=84=D0=B8=D0=BB=D1=8C=D1=82=D1=80=20(=D0=BD=D0=B5?= =?UTF-8?q?=20=D0=B4=D0=BE=D1=80=D0=B0=D0=B1=D0=BE=D1=82=D0=B0=D0=BD=D0=BE?= =?UTF-8?q?)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MyLibs/Inc/filters.h | 38 +++++++++++++++- MyLibs/Src/filters.c | 102 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 139 insertions(+), 1 deletion(-) diff --git a/MyLibs/Inc/filters.h b/MyLibs/Inc/filters.h index f21e451..a53980d 100644 --- a/MyLibs/Inc/filters.h +++ b/MyLibs/Inc/filters.h @@ -457,7 +457,43 @@ int32_t FilterRMSInt_Process(FilterRMSInt_t* filter, int32_t input); -// ==================== CMSIS ВЕРСИИ ==================== +// ==================== ДРУГИЕ ФИЛЬТРЫ ==================== + + +/** + * @brief Структура полосового фильтра с дифференциатором + * @details Комбинация дифференциатора и полосового фильтра 2-го порядка. + * Используется для выделения сетевой частоты и подготовки к детектированию нуля. + */ +typedef struct _FilterBandPassDerivative_t { + FilterState_t state; ///< Состояние фильтра + + // Дифференциатор + float prev_input; ///< Предыдущее входное значение + + // Полосовой фильтр (биквадратный, прямая форма II) + float b0, b1, b2; ///< Коэффициенты числителя + float a1, a2; ///< Коэффициенты знаменателя + + // Состояния фильтра + float x1, x2; ///< Состояния входа + float y1, y2; ///< Состояния выхода + + float last_output; ///< Последнее выходное значение + uint8_t dataProcessing; ///< Флаг обработки данных + + // Указатели на функции + int (*reset)(struct _FilterBandPassDerivative_t *filter, + float center_freq_ratio, float bandwidth_ratio); + float (*process)(struct _FilterBandPassDerivative_t *filter, float input); +} FilterBandPassDerivative_t; + +int FilterBandPassDerivative_Init(FilterBandPassDerivative_t* filter, + float center_freq_ratio, + float bandwidth_ratio); +float FilterBandPassDerivative_Process(FilterBandPassDerivative_t* filter, + float input); + #ifdef DSP_FITLERS /** * @brief Структура биквадратного фильтра с CMSIS-DSP diff --git a/MyLibs/Src/filters.c b/MyLibs/Src/filters.c index ed4ab61..cb8eb58 100644 --- a/MyLibs/Src/filters.c +++ b/MyLibs/Src/filters.c @@ -756,6 +756,108 @@ int32_t FilterRMSInt_Process(FilterRMSInt_t* filter, int32_t input) { +// ==================== ДРУГИЕ ФИЛЬТРЫ ==================== + +/** + * @brief Инициализация полосового фильтра с дифференциатором + * @param filter Указатель на структуру фильтра + * @param center_freq_ratio Отношение центральной частоты к частоте дискретизации (0..0.5) + * Например: 50 Гц / 1000 Гц = 0.05 + * @param bandwidth_ratio Относительная ширина полосы (0..1) + * Например: 0.1 = полоса 10% от центральной частоты + * @return 0 - успех, -1 - ошибка + */ +int FilterBandPassDerivative_Init(FilterBandPassDerivative_t* filter, + float center_freq_ratio, + float bandwidth_ratio) { + check_init_filter(filter); + + // Проверка параметров + if (center_freq_ratio <= 0.0f || center_freq_ratio >= 0.5f) return -1; + if (bandwidth_ratio <= 0.0f || bandwidth_ratio > 1.0f) return -1; + + // 1. Расчет параметров полосового фильтра + float w0 = 2.0f * PI * center_freq_ratio; // Нормированная угловая частота + float Q = 1.0f / bandwidth_ratio; // Добротность + float alpha = sinf(w0) / (2.0f * Q); + float cos_w0 = cosf(w0); + + // Коэффициенты биквадратного полосового фильтра + // H_bp(z) = (alpha - alpha*z^-2) / (1 + a1*z^-1 + a2*z^-2) + float b0_bp = alpha; + float b1_bp = 0.0f; + float b2_bp = -alpha; + float a0_bp = 1.0f + alpha; + float a1_bp = -2.0f * cos_w0; + float a2_bp = 1.0f - alpha; + + // Нормализация (a0 = 1) + filter->b0 = b0_bp / a0_bp; + filter->b1 = b1_bp / a0_bp; + filter->b2 = b2_bp / a0_bp; + filter->a1 = a1_bp / a0_bp; + filter->a2 = a2_bp / a0_bp; + + // 2. Дифференциатор реализуется отдельно в process() + filter->prev_input = 0.0f; + + // 3. Инициализация состояний фильтра + filter->x1 = 0.0f; + filter->x2 = 0.0f; + filter->y1 = 0.0f; + filter->y2 = 0.0f; + filter->last_output = 0.0f; + filter->dataProcessing = 0; + + filter->state = FILTER_READY; + filter->reset = &FilterBandPassDerivative_Init; + filter->process = &FilterBandPassDerivative_Process; + + return 0; +} + +/** + * @brief Обработка значения фильтром + * @param filter Указатель на структуру фильтра + * @param input Входное значение в диапазоне [-1.0, 1.0] + * @return Отфильтрованное значение + * + * @details Алгоритм: + * 1. Вычисление производной: diff = input - prev_input + * 2. Полосовая фильтрация diff + * 3. Обновление состояний + */ +float FilterBandPassDerivative_Process(FilterBandPassDerivative_t* filter, + float input) { + check_process_filter(filter); + + // 1. Дифференциатор (разностный фильтр 1-го порядка) + //float diff = input - filter->prev_input; + //filter->prev_input = input; + float diff = input; + + // 2. Полосовой фильтр (биквадратный, прямая форма II) + // y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2] + float output = filter->b0 * diff + + filter->b1 * filter->x1 + + filter->b2 * filter->x2 - + filter->a1 * filter->y1 - + filter->a2 * filter->y2; + + // 3. Обновление состояний + filter->x2 = filter->x1; // x[n-2] = x[n-1] + filter->x1 = diff; // x[n-1] = x[n] + filter->y2 = filter->y1; // y[n-2] = y[n-1] + filter->y1 = output; // y[n-1] = y[n] + + filter->last_output = output; + filter->dataProcessing = 0; // Данные обработаны + + return output; +} + + + #ifdef DSP_FITLERS