добавлоен полосовой фильтр (не доработано)

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

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