Добавлен биквадратный фильтр от CMSIS-DSP

MyLibs/Inc/filters.h

@@ -84,6 +84,12 @@ int32_t process_value_int(int32_t raw_adc_quant) {
 
 #ifdef FILTERS_ENABLE
 
+#ifdef ARM_MATH_CM4
+#include "arm_math.h"
+#define DSP_FITLERS 1
+#endif
+
+
 #ifndef FILTER_AVERAGE_MAX_SIZE
 #define FILTER_AVERAGE_MAX_SIZE     100   ///< Размер окна усредняющего фильтра
 #endif
@@ -97,6 +103,40 @@ int32_t process_value_int(int32_t raw_adc_quant) {
 #endif
 
 
+#define check_proccess_func(_ptr_) \
+((_fltr_)->process != NULL) 1 : \
+((_fltr_)->process == &FilterMedian_Init) 0 : \
+((_fltr_)->process == &FilterExp_Init) 0 : \
+((_fltr_)->process == &FilterAverage_Init) 0 : \
+((_fltr_)->process == &FilterPoly_Init) 0 : \
+((_fltr_)->process == &FilterLUT_Init) 0 : 1
+
+
+#define check_init_func(_ptr_) \
+((_fltr_)->process != NULL) 1 : \
+((_fltr_)->process == &FilterMedian_Process) 0 : \
+((_fltr_)->process == &FilterExp_Process) 0 : \
+((_fltr_)->process == &FilterExp_Process) 0 : \
+((_fltr_)->process == &FilterPoly_Process) 0 : \
+((_fltr_)->process == &FilterLUT_Process) 0 : 1
+
+/**
+  * @brief Сброс фильтра (повторная инициализация)
+  * @param filter Указатель на структуру фильтра
+  * @details  Запускает функцию инициализации, если указатель инициализирован
+  */
+#define Filter_Reset(_fltr_, _input_) \
+((_fltr_)->reset != NULL) ? (_fltr_)->reset(_fltr_, _input_): -1
+
+
+/**
+  * @brief Обработать число фильтром
+  * @param filter Указатель на структуру фильтра
+  * @details  Запускает функцию фильтрации, если указатель инициализирован
+  */
+#define Filter_Process(_fltr_, _input_) \
+((_fltr_)->process != NULL) ? (_fltr_)->process(_fltr_, _input_): 0
+
 /**
   * @brief Запуск фильтра
   * @param filter Указатель на структуру фильтра
@@ -104,7 +144,7 @@ int32_t process_value_int(int32_t raw_adc_quant) {
   *           Если он не инициализирован или уже запущен - ничего не делается
   */
 #define Filter_Start(_fltr_) \
-do{ if(Filter_isReady(_fltr_)) (_fltr_)->state = FILTER_ENABLE; }while(0);
+do{ if(Filter_isReady(_fltr_)) (_fltr_)->state = FILTER_ENABLE; }while(0)
 
 /**
   * @brief Остановка работы фильтра
@@ -113,12 +153,12 @@ do{ if(Filter_isReady(_fltr_)) (_fltr_)->state = FILTER_ENABLE; }while(0);
   *           Если он не инициализирован или уже остановлен - ничего не делается
   */
 #define Filter_Stop(_fltr_) \
-do{ if(Filter_isStart(_fltr_)) (_fltr_)->state = FILTER_READY; }while(0);
+do{ if(Filter_isEnable(_fltr_)) (_fltr_)->state = FILTER_READY; }while(0)
 
 #define Filter_GetState(_fltr_)     (_fltr_)->state
 #define Filter_isInit(_fltr_)       !(Filter_GetState(_fltr_) == FILTER_NOT_INIT)
 #define Filter_isReady(_fltr_)      (Filter_GetState(_fltr_) == FILTER_READY)
-#define Filter_isStart(_fltr_)      (Filter_GetState(_fltr_) == FILTER_ENABLE)
+#define Filter_isEnable(_fltr_)     (Filter_GetState(_fltr_) == FILTER_ENABLE)
 
 typedef enum
 {
@@ -139,27 +179,33 @@ typedef enum
 /**
   * @brief Структура медианного фильтра (float)
   */
-typedef struct {
+typedef struct _FilterMedian_t{
   FilterState_t state;         ///< Состояние фильтра
   float buffer[FILTER_MEDIAN_MAX_SIZE];  ///< Буфер значений
   uint8_t index;                     ///< Текущий индекс
   uint8_t size;                      ///< Фактический размер фильтра
+  
+  int     (*reset)(struct _FilterMedian_t *filter, uint8_t size);
+  float   (*process)(struct _FilterMedian_t *filter, float input);
 } FilterMedian_t;
 
 /**
   * @brief Структура экспоненциального фильтра (float)
   */
-typedef struct {
+typedef struct _FilterExp_t {
   FilterState_t state;         ///< Состояние фильтра
   float alpha;        ///< Коэффициент сглаживания (0..1)
   float value;        ///< Текущее значение
   uint8_t initialized; ///< Флаг инициализации
+  
+  int     (*reset)(struct _FilterExp_t *filter, float alpha);
+  float   (*process)(struct _FilterExp_t *filter, float input);
 } FilterExp_t;
 
 /**
   * @brief Структура фильтра скользящего среднего (float)
   */
-typedef struct {
+typedef struct _FilterAverage_t{
   FilterState_t state;       ///< Состояние фильтра
   FilterMode_t mode;          ///< Режим фильтра
   float buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений
@@ -168,28 +214,38 @@ typedef struct {
   uint8_t index;    ///< Текущий индекс
   uint8_t count;    ///< Количество элементов
   float lastValue;  ///< Последнее измеренное значение
+  
+  int     (*reset)(struct _FilterAverage_t *filter, uint8_t size, FilterMode_t mode);
+  float   (*process)(struct _FilterAverage_t *filter, float input);
 } FilterAverage_t;
 
 /**
   * @brief Структура полиномиальной коррекции (float)
   */
-typedef struct {
+typedef struct _FilterPoly_t{
   FilterState_t state;         ///< Состояние фильтра
   float coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома
   uint8_t order;                                 ///< Порядок полинома
+  
+  int     (*reset)(struct _FilterPoly_t *filter, float* coeffs, uint8_t order);
+  float   (*process)(struct _FilterPoly_t *filter, float input);
 } FilterPoly_t;
 
 /**
   * @brief Структура табличного фильтра (float)
   */
-typedef struct {
+typedef struct _FilterLUT_t{
   FilterState_t state;      ///< Состояние фильтра
   float* input_values;     // Массив входных значений
   float* output_values;    // Массив выходных значений  
   uint16_t size;           // Размер таблицы
   uint8_t interpolation;   // Флаг интерполяции (0 - отключена, 1 - линейная)
+  
+  int     (*reset)(struct _FilterLUT_t *filter, float* input_arr, float* output_arr, uint16_t size, uint8_t interpolation);
+  float   (*process)(struct _FilterLUT_t *filter, float input);
 } FilterLUT_t;
 
+
 // Float версии функций
 int FilterMedian_Init(FilterMedian_t* filter, uint8_t size);
 float FilterMedian_Process(FilterMedian_t* filter, float input);
@@ -206,28 +262,34 @@ float FilterLUT_Process(FilterLUT_t* filter, float input);
 /**
   * @brief Структура медианного фильтра (int32_t)
   */
-typedef struct {
+typedef struct _FilterMedianInt_t{
   FilterState_t state;         ///< Состояние фильтра
   int32_t buffer[FILTER_MEDIAN_MAX_SIZE];  ///< Буфер значений
   uint8_t index;                       ///< Текущий индекс
   uint8_t size;                        ///< Фактический размер фильтра
+  
+  int     (*reset)(struct _FilterMedianInt_t *filter, uint8_t size);
+  int32_t (*process)(struct _FilterMedianInt_t *filter, int32_t input);
 } FilterMedianInt_t;
 
 /**
   * @brief Структура экспоненциального фильтра (int32_t)
   */
-typedef struct {
+typedef struct _FilterExpInt_t{
   FilterState_t state;         ///< Состояние фильтра
   int32_t alpha;        ///< Коэффициент сглаживания (в масштабе scale)
   int32_t value;        ///< Текущее значение
   uint8_t initialized;  ///< Флаг инициализации
   int32_t scale;        ///< Масштаб коэффициента (например 100 для 0.01)
+  
+  int     (*reset)(struct _FilterExpInt_t *filter, int32_t alpha, int32_t scale);
+  int32_t (*process)(struct _FilterExpInt_t *filter, int32_t input);
 } FilterExpInt_t;
 
 /**
   * @brief Структура фильтра скользящего среднего (int32_t)
   */
-typedef struct {
+typedef struct _FilterAverageInt_t{
   FilterState_t state;        ///< Состояние фильтра
   FilterMode_t mode;          ///< Режим фильтра
   int32_t buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений
@@ -236,6 +298,9 @@ typedef struct {
   uint8_t index;                       ///< Текущий индекс
   uint8_t count;                       ///< Количество элементов
   uint32_t lastValue;                   ///< Последнее измеренное значение
+  
+  int     (*reset)(struct _FilterAverageInt_t *filter, uint8_t size, FilterMode_t mode);
+  int32_t (*process)(struct _FilterAverageInt_t *filter, int32_t input);
 } FilterAverageInt_t;
 
 
@@ -243,22 +308,28 @@ typedef struct {
 /**
   * @brief Структура полиномиальной коррекции (int32_t)
   */
-typedef struct {
+typedef struct _FilterPolyInt_t{
   FilterState_t state;         ///< Состояние фильтра
   int32_t coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома
   uint8_t order;                                   ///< Порядок полинома
   int32_t scale;                                   ///< Масштаб коэффициентов
+  
+  int     (*reset)(struct _FilterPolyInt_t *filter, int32_t* coeffs, uint8_t order, int32_t scale);
+  int32_t (*process)(struct _FilterPolyInt_t *filter, int32_t input);
 } FilterPolyInt_t;
 
 /**
   * @brief Структура табличного фильтра (int32_t)
   */
-typedef struct {
+typedef struct _FilterLUTInt_t{
   FilterState_t state;         ///< Состояние фильтра
-  int32_t* input_values;   // Массив входных значений
+  int32_t* input_values;   ///< Массив входных значений
-  int32_t* output_values;  // Массив выходных значений
+  int32_t* output_values;  ///< Массив выходных значений
-  uint16_t size;           // Размер таблицы
+  uint16_t size;           ///< Размер таблицы
-  uint8_t interpolation;   // Флаг интерполяции
+  uint8_t interpolation;   ///< Флаг интерполяции
+  
+  int     (*reset)(struct _FilterLUTInt_t *filter, int32_t* input_arr, int32_t* output_arr, uint16_t size, uint8_t interpolation);
+  int32_t (*process)(struct _FilterLUTInt_t *filter, int32_t input);
 } FilterLUTInt_t;
 
 // Int32_t версии функций
@@ -273,6 +344,32 @@ int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input);
 int FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* output_arr, uint16_t size, uint8_t interpolation);
 int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input);
 
+
+
+// ==================== CMSIS ВЕРСИИ ====================
+#ifdef DSP_FITLERS
+/**
+  * @brief Структура биквадратного фильтра с CMSIS-DSP
+  */
+typedef struct _FilterBiquad_t{
+  FilterState_t state;         ///< Состояние фильтра
+  arm_biquad_cascade_df2T_instance_f32 instance; ///< CMSIS-DSP instance
+  float32_t coeffs[5];          ///< Коэффициенты [b0, b1, b2, a1, a2]
+  float32_t state_buffer[4];    ///< Буфер состояний (2 состояния на каскад)
+  
+  int   (*reset)(struct _FilterBiquad_t *filter, const float32_t coeffs[5]);
+  float (*process)(struct _FilterBiquad_t *filter, float input);
+} FilterBiquad_t;
+
+
+// CMSIS версии функций
+int FilterBiquad_Init(FilterBiquad_t* filter, const float32_t coeffs[5]);
+float FilterBiquad_Process(FilterBiquad_t* filter, float input);
+#endif //DSP_FITLERS
+
+
+
+
 #else // FILTERS_ENABLE
 
 #endif // FILTERS_ENABLE

MyLibs/Src/filters.c

@@ -41,6 +41,8 @@ int FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
   filter->size = size;
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterMedian_Init;
+  filter->process = &FilterMedian_Process;
   return 0;
 }
 
@@ -80,6 +82,8 @@ int FilterExp_Init(FilterExp_t* filter, float alpha) {
   filter->initialized = 0;
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterExp_Init;
+  filter->process = &FilterExp_Process;
   return 0;
 }
 
@@ -119,6 +123,8 @@ int FilterAverage_Init(FilterAverage_t* filter, uint8_t size, FilterMode_t mode)
   filter->mode = mode;
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterAverage_Init;
+  filter->process = &FilterAverage_Process;
   return 0;
 }
 
@@ -173,6 +179,8 @@ int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) {
   memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(float));
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterPoly_Init;
+  filter->process = &FilterPoly_Process;
   return 0;
 }
 
@@ -215,6 +223,8 @@ int FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, uin
   filter->interpolation = interpolation;
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterLUT_Init;
+  filter->process = &FilterLUT_Process;
   return 0;
 }
 
@@ -294,6 +304,8 @@ int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
   filter->size = size;
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterMedianInt_Init;
+  filter->process = &FilterMedianInt_Process;
   return 0;
 }
 
@@ -335,6 +347,8 @@ int FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) {
   filter->initialized = 0;
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterExpInt_Init;
+  filter->process = &FilterExpInt_Process;
   return 0;
 }
 
@@ -378,6 +392,8 @@ int FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size, FilterMode_t
   filter->mode = mode;
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterAverageInt_Init;
+  filter->process = &FilterAverageInt_Process;
   return 0;
 }
 
@@ -434,6 +450,8 @@ int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order,
   memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(int32_t));
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterPolyInt_Init;
+  filter->process = &FilterPolyInt_Process;
   return 0;
 }
 
@@ -483,6 +501,8 @@ int FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* outpu
   filter->interpolation = interpolation;
   
   filter->state = FILTER_READY;
+  filter->reset   = &FilterLUTInt_Init;
+  filter->process = &FilterLUTInt_Process;
   return 0;
 }
 
@@ -538,4 +558,50 @@ int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) {
   int64_t result = y0 + (input - x0) * (y1 - y0) / (x1 - x0);
   return (int32_t)result;
 }
+
+
+
+
+#ifdef DSP_FITLERS
+
+/**
+  * @brief Инициализация биквадратного фильтра с CMSIS-DSP
+  */
+int FilterBiquad_Init(FilterBiquad_t* filter, const float32_t coeffs[5])
+{
+  check_init_filter(filter);
+  
+  if (coeffs == NULL) return -1;
+  
+  memcpy(filter->coeffs, coeffs, sizeof(filter->coeffs));
+  memset(filter->state_buffer, 0, sizeof(filter->state_buffer));
+  
+  // Инициализация CMSIS-DSP структуры (1 каскад)
+  arm_biquad_cascade_df2T_init_f32(&filter->instance, 
+                                    1, 
+                                    filter->coeffs, 
+                                    filter->state_buffer);
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterBiquad_Init;
+  filter->process = &FilterBiquad_Process;
+  return 0;
+}
+
+/**
+  * @brief Обработка значения биквадратным фильтром CMSIS-DSP
+  */
+float FilterBiquad_Process(FilterBiquad_t* filter, float input)
+{
+    check_process_filter(filter);
+    
+    float32_t in_arr[1] = {input};
+    float32_t out_arr[1];
+    
+    arm_biquad_cascade_df2T_f32(&filter->instance, in_arr, out_arr, 1);
+    
+    return out_arr[0];
+}
+#endif
+
 #endif // FILTERS_ENABLE