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

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