0.3.1

Рефакторинг полосового фильтра. Добавление функции для расчета его сдвига

MyLibs/Inc/filters.h

@@ -467,7 +467,7 @@ int32_t FilterRMSInt_Process(FilterRMSInt_t* filter, int32_t input);
   * @details Комбинация дифференциатора и полосового фильтра 2-го порядка.
   *          Используется для выделения сетевой частоты и подготовки к детектированию нуля.
   */
-typedef struct _FilterBandPassDerivative_t {
+typedef struct _FilterBandPass_t {
   FilterState_t state;         ///< Состояние фильтра
   
   // Дифференциатор
@@ -485,15 +485,19 @@ typedef struct _FilterBandPassDerivative_t {
   uint8_t dataProcessing;      ///< Флаг обработки данных
   
   // Указатели на функции
-  int   (*reset)(struct _FilterBandPassDerivative_t *filter,
+  int   (*reset)(struct _FilterBandPass_t *filter,
                  float center_freq_ratio, float bandwidth_ratio);
-  float (*process)(struct _FilterBandPassDerivative_t *filter, float input);
-} FilterBandPassDerivative_t;
+  float (*process)(struct _FilterBandPass_t *filter, float input);
+} FilterBandPass_t;
 
-int FilterBandPassDerivative_Init(FilterBandPassDerivative_t* filter,
+int FilterBandPass_Init(FilterBandPass_t* filter,
                                   float center_freq_ratio, 
                                   float bandwidth_ratio);
-float FilterBandPassDerivative_Process(FilterBandPassDerivative_t* filter, 
+int FilterBandPass_CalcPhaseDegTable(FilterBandPass_t* filter,
+                                     float* phase_table,
+                                     const float* freq_points,
+                                     int num_points);
+float FilterBandPass_Process(FilterBandPass_t* filter, 
                                        float input);
 
 #ifdef DSP_FITLERS

MyLibs/Src/filters.c

@@ -767,7 +767,7 @@ int32_t FilterRMSInt_Process(FilterRMSInt_t* filter, int32_t input) {
   *                        Например: 0.1 = полоса 10% от центральной частоты
   * @return 0 - успех, -1 - ошибка
   */
-int FilterBandPassDerivative_Init(FilterBandPassDerivative_t* filter,
+int FilterBandPass_Init(FilterBandPass_t* filter,
                                   float center_freq_ratio,
                                   float bandwidth_ratio) {
   check_init_filter(filter);
@@ -810,8 +810,71 @@ int FilterBandPassDerivative_Init(FilterBandPassDerivative_t* filter,
   filter->dataProcessing = 0;
   
   filter->state = FILTER_READY;
-  filter->reset = &FilterBandPassDerivative_Init;
-  filter->process = &FilterBandPassDerivative_Process;
+  filter->reset = &FilterBandPass_Init;
+  filter->process = &FilterBandPass_Process;
+  
+  return 0;
+}
+/**
+  * @brief Расчет таблицы фазовых сдвигов в градусах
+  * @param filter Указатель на фильтр
+  * @param phase_table Указатель на массив для фаз (в градусах)
+  * @param freq_points Указатель на массив частот (0..0.5)
+  * @param num_points Количество точек
+  * @return 0 - успех, -1 - ошибка
+  *//**
+  * @brief Расчет с повышенной точностью через double
+  */
+int FilterBandPass_CalcPhaseDegTable(FilterBandPass_t* filter,
+  float* phase_table,
+  const float* freq_points,
+  int num_points) {
+
+  if (!filter || !phase_table || !freq_points || num_points <= 0) {
+    return -1;
+  }
+
+  // Копируем коэффициенты в double для точности
+  double b0 = (double)filter->b0;
+  double b1 = (double)filter->b1;
+  double b2 = (double)filter->b2;
+  double a1 = (double)filter->a1;
+  double a2 = (double)filter->a2;
+
+  for (int i = 0; i < num_points; i++) {
+    double omega = 2.0 * PI * (double)freq_points[i];
+
+    double cos_w = cos(omega);
+    double sin_w = sin(omega);
+    double cos_2w = cos(2.0 * omega);
+    double sin_2w = sin(2.0 * omega);
+
+    double num_real = b0 + b1 * cos_w + b2 * cos_2w;
+    double num_imag = -b1 * sin_w - b2 * sin_2w;
+
+    double den_real = 1.0 + a1 * cos_w + a2 * cos_2w;
+    double den_imag = -a1 * sin_w - a2 * sin_2w;
+
+    // Используем устойчивое деление
+    double mag_sq = den_real * den_real + den_imag * den_imag;
+
+    if (mag_sq < 1e-30) {  // Более строгий порог для double
+      phase_table[i] = 0.0f;
+      continue;
+    }
+
+    double H_real = (num_real * den_real + num_imag * den_imag) / mag_sq;
+    double H_imag = (num_imag * den_real - num_real * den_imag) / mag_sq;
+
+    double phase_rad = atan2(H_imag, H_real);
+    double phase_deg = phase_rad * 180.0 / PI;
+
+    // Нормализация
+    while (phase_deg > 180.0) phase_deg -= 360.0;
+    while (phase_deg < -180.0) phase_deg += 360.0;
+
+    phase_table[i] = (float)phase_deg;
+  }
 
   return 0;
 }
@@ -823,11 +886,11 @@ int FilterBandPassDerivative_Init(FilterBandPassDerivative_t* filter,
   * @return Отфильтрованное значение
   * 
   * @details Алгоритм:
-  * 1. Вычисление производной: diff = input - prev_input
+//  * 1. Вычисление производной: diff = input - prev_input
   * 2. Полосовая фильтрация diff
   * 3. Обновление состояний
   */
-float FilterBandPassDerivative_Process(FilterBandPassDerivative_t* filter, 
+float FilterBandPass_Process(FilterBandPass_t* filter, 
                                        float input) {
   check_process_filter(filter);
   
@@ -857,8 +920,6 @@ float FilterBandPassDerivative_Process(FilterBandPassDerivative_t* filter,
 }
 
 
-
-
 #ifdef DSP_FITLERS
 
 /**