Добавлен дефайн для определения количества элементов в структуре

Добавлен биквадратный фильтр от CMSIS-DSP
Доработки фильтров:
2025-11-14 18:02:43 +03:00 · 2025-11-13 17:13:04 +03:00 · 2025-11-11 17:24:53 +03:00
3 changed files with 582 additions and 334 deletions
--- a/MyLibs/Inc/filters.h
+++ b/MyLibs/Inc/filters.h
@ -11,11 +11,12 @@
 - Экспоненциальное скользящее среднее (float и int32_t)  
 - Скользящее среднее арифметическое (float и int32_t)
 - Полиномиальную коррекцию (float и int32_t)
+- Табличный фильтр LUT (Look-Up Table) (float и int32_t)

 Параметры для конфигурации:
 - @ref FILTERS_ENABLE               - Включить библиотеку фильтров
- @ref FILTER_MEDIAN_MAX_SIZE           - Размер окна медианного фильтра (по умолчанию 5)
- @ref FILTER_AVERAGE_MAX_SIZE          - Размер окна усредняющего фильтра (по умолчанию 8)
+- @ref FILTER_MEDIAN_MAX_SIZE       - Размер окна медианного фильтра (по умолчанию 5)
+- @ref FILTER_AVERAGE_MAX_SIZE      - Размер окна усредняющего фильтра (по умолчанию 8)
 - @ref FILTER_POLY_MAX_ORDER        - Максимальный порядок полинома (по умолчанию 4)

@par Пример использования:
@ -83,120 +84,223 @@ 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   ///< Размер окна медианного фильтра
+#define FILTER_AVERAGE_MAX_SIZE     100   ///< Размер окна усредняющего фильтра
 #endif

 #ifndef FILTER_MEDIAN_MAX_SIZE
-#define FILTER_MEDIAN_MAX_SIZE      8     ///< Размер окна усредняющего фильтра
+#define FILTER_MEDIAN_MAX_SIZE      10    ///< Размер окна медианного фильтра
 #endif

 #ifndef FILTER_POLY_MAX_ORDER
 #define FILTER_POLY_MAX_ORDER       4     ///< Максимальный порядок полинома
 #endif

-#define FILTER_GET_STATE(_fltr_)    (_fltr_)->state

-// ==================== FLOAT ВЕРСИИ ====================
+#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 Указатель на структуру фильтра
+  * @details  Запускает фильтр только если он в состоянии готовности. 
+  *           Если он не инициализирован или уже запущен - ничего не делается
+  */
+#define Filter_Start(_fltr_) \
+do{ if(Filter_isReady(_fltr_)) (_fltr_)->state = FILTER_ENABLE; }while(0)
+
+/**
+  * @brief Остановка работы фильтра
+  * @param filter Указатель на структуру фильтра
+  * @details  Останавливет фильтр только если он запущен @ref Filter_Start. 
+  *           Если он не инициализирован или уже остановлен - ничего не делается
+  */
+#define Filter_Stop(_fltr_) \
+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_isEnable(_fltr_)     (Filter_GetState(_fltr_) == FILTER_ENABLE)
+
 typedef enum
 {
-  FILTER_DISABLE,
+  FILTER_NOT_INIT,
+  FILTER_READY,
  FILTER_ENABLE
 }FilterState_t;
+
+typedef enum
+{
+  FILTER_MODE_DEFAULT = 0,
+  FILTER_MODE_MOVING,
+} FilterMode_t;
+
+
+// ==================== FLOAT ВЕРСИИ ====================
+
 /**
  * @brief Структура медианного фильтра (float)
  */
-typedef struct {
-    float buffer[FILTER_MEDIAN_MAX_SIZE];  ///< Буфер значений
-    uint8_t index;                     ///< Текущий индекс
-    uint8_t size;                      ///< Фактический размер фильтра
+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]; ///< Буфер значений
  uint8_t size;     ///< Фактический размер фильтра
  float sum;        ///< Сумма значений
  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 {
-    float coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома
-    uint8_t order;                                 ///< Порядок полинома
+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 версии функций
-void FilterMedian_Init(FilterMedian_t* filter, uint8_t size);
+int FilterMedian_Init(FilterMedian_t* filter, uint8_t size);
 float FilterMedian_Process(FilterMedian_t* filter, float input);
-void FilterExp_Init(FilterExp_t* filter, float alpha);
+int FilterExp_Init(FilterExp_t* filter, float alpha);
 float FilterExp_Process(FilterExp_t* filter, float input);
-void FilterAverage_Init(FilterAverage_t* filter, uint8_t size);
+int FilterAverage_Init(FilterAverage_t* filter, uint8_t size, FilterMode_t mode);
 float FilterAverage_Process(FilterAverage_t* filter, float input);
 int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order);
 float FilterPoly_Process(FilterPoly_t* filter, float input);
-void FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, uint16_t size, uint8_t interpolation);
+int FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, uint16_t size, uint8_t interpolation);
 float FilterLUT_Process(FilterLUT_t* filter, float input);
 // ==================== INT32_T ВЕРСИИ ====================

 /**
  * @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 {
-  FilterState_t state;         ///< Состояние фильтра
+typedef struct _FilterAverageInt_t{
+  FilterState_t state;        ///< Состояние фильтра
+  FilterMode_t mode;          ///< Режим фильтра
  int32_t buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений
  uint8_t size;                        ///< Фактический размер фильтра
  int64_t sum;                         ///< Сумма значений
  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;


@ -204,70 +308,69 @@ 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* output_values;  // Массив выходных значений
-  uint16_t size;           // Размер таблицы
-  uint8_t interpolation;   // Флаг интерполяции
+  int32_t* input_values;   ///< Массив входных значений
+  int32_t* output_values;  ///< Массив выходных значений
+  uint16_t size;           ///< Размер таблицы
+  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 версии функций
-void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size);
+int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size);
 int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input);
-void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale);
+int FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale);
 int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input);
-void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size);
+int FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size, FilterMode_t mode);
 int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input);
 int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, int32_t scale);
 int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input);
-void FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* output_arr, uint16_t size, uint8_t interpolation);
+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
-// Заглушки для float
-typedef struct { uint8_t dummy; } FilterMedian_t;
-typedef struct { uint8_t dummy; } FilterExp_t;
-typedef struct { uint8_t dummy; } FilterAverage_t;
-typedef struct { uint8_t dummy; } FilterPoly_t;
-
-#define FilterMedian_Init(filter, size)
-#define FilterMedian_Process(filter, input) (input)
-#define FilterExp_Init(filter, alpha)
-#define FilterExp_Process(filter, input) (input)
-#define FilterAverage_Init(filter, size)
-#define FilterAverage_Process(filter, input) (input)
-#define FilterPoly_Init(filter, coeffs, order) (0)
-#define FilterPoly_Process(filter, input) (input)
-#define FilterLUT_Init(filter, coeffs, order) (0)
-#define FilterLUT_Process(filter, input) (input)
-
-// Заглушки для int32_t
-typedef struct { uint8_t dummy; } FilterMedianInt_t;
-typedef struct { uint8_t dummy; } FilterExpInt_t;
-typedef struct { uint8_t dummy; } FilterAverageInt_t;
-typedef struct { uint8_t dummy; } FilterPolyInt_t;
-
-#define FilterMedianInt_Init(filter, size)
-#define FilterMedianInt_Process(filter, input) (input)
-#define FilterExpInt_Init(filter, alpha, scale)
-#define FilterExpInt_Process(filter, input) (input)
-#define FilterAverageInt_Init(filter, size)
-#define FilterAverageInt_Process(filter, input) (input)
-#define FilterPolyInt_Init(filter, coeffs, order, scale) (0)
-#define FilterPolyInt_Process(filter, input) (input)
-#define FilterLUTInt_Init(filter, coeffs, order) (0)
-#define FilterLUTInt_Process(filter, input) (input)

 #endif // FILTERS_ENABLE

--- a/MyLibs/Inc/mylibs_defs.h
+++ b/MyLibs/Inc/mylibs_defs.h
@ -160,6 +160,12 @@ extern void Error_Handler(void);
  * @{
  */

+  /**
+   * @brief Размер структуры в её элементах.
+   * @param _struct_ Структура, размер которой надо вычислить.
+   */
+#define numbof(_arr_)     (sizeof(_arr_)/sizeof(_arr_[0]))
+
 /**
 * @brief Обнуление структуры.
 * @param _struct_ Структура, которую нужно обнулить.
--- a/MyLibs/Src/filters.c
+++ b/MyLibs/Src/filters.c
@ -8,6 +8,14 @@

 #ifdef FILTERS_ENABLE

+
+#define check_init_filter(_filter_) \
+do{   if (filter == NULL) return -1;   \
+      filter->state = FILTER_NOT_INIT;}while(0);
+
+#define check_process_filter(_filter_) \
+do{   if ((filter == NULL) || (filter->state != FILTER_ENABLE)) return input;}while(0);
+
 // ==================== FLOAT ВЕРСИИ ====================

 // Вспомогательная функция для сравнения float
@ -22,15 +30,20 @@ static int Filter_float_compare(const void *a, const void *b) {
 /**
  * @brief Инициализация медианного фильтра (float)
  * @param filter Указатель на структуру фильтра
+  * @return 0 - успех, -1 - ошибка
  */
-void FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
-    if (filter == NULL) return;
-    if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return;
+int FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
+  check_init_filter(filter);
+  if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1;
  
-    memset(filter->buffer, 0, sizeof(filter->buffer));
-    filter->index = 0;
-    filter->size = size;
+  memset(filter->buffer, 0, sizeof(filter->buffer));
+  filter->index = 0;
+  filter->size = size;
  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterMedian_Init;
+  filter->process = &FilterMedian_Process;
+  return 0;
 }

 /**
@ -40,32 +53,38 @@ void FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
  * @return Отфильтрованное значение
  */
 float FilterMedian_Process(FilterMedian_t* filter, float input) {
-    if (filter == NULL) return input;
+  check_process_filter(filter);
  
-    // Добавляем значение в буфер
-    filter->buffer[filter->index] = input;
-    filter->index = (filter->index + 1) % filter->size;
+  // Добавляем значение в буфер
+  filter->buffer[filter->index] = input;
+  filter->index = (filter->index + 1) % filter->size;
  
-    // Копируем буфер для сортировки
-    float sort_buffer[FILTER_MEDIAN_MAX_SIZE];
-    memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer));
+  // Копируем буфер для сортировки
+  float sort_buffer[FILTER_MEDIAN_MAX_SIZE];
+  memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer));
  
-    // Сортируем и возвращаем медиану
-    qsort(sort_buffer, filter->size, sizeof(float), Filter_float_compare);
-    return sort_buffer[filter->size / 2];
+  // Сортируем и возвращаем медиану
+  qsort(sort_buffer, filter->size, sizeof(float), Filter_float_compare);
+  return sort_buffer[filter->size / 2];
 }

 /**
  * @brief Инициализация экспоненциального фильтра (float)
  * @param filter Указатель на структуру фильтра
  * @param alpha Коэффициент сглаживания (0..1)
+  * @return 0 - успех, -1 - ошибка
  */
-void FilterExp_Init(FilterExp_t* filter, float alpha) {
-    if (filter == NULL) return;
+int FilterExp_Init(FilterExp_t* filter, float alpha) {
+  check_init_filter(filter);
  
-    filter->alpha = alpha;
-    filter->value = 0.0f;
-    filter->initialized = 0;
+  filter->alpha = alpha;
+  filter->value = 0.0f;
+  filter->initialized = 0;
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterExp_Init;
+  filter->process = &FilterExp_Process;
+  return 0;
 }

 /**
@ -75,31 +94,38 @@ void FilterExp_Init(FilterExp_t* filter, float alpha) {
  * @return Отфильтрованное значение
  */
 float FilterExp_Process(FilterExp_t* filter, float input) {
-    if (filter == NULL) return input;
+  check_process_filter(filter);
  
-    if (!filter->initialized) {
-        filter->value = input;
-        filter->initialized = 1;
-        return input;
-    }
+  if (!filter->initialized) {
+      filter->value = input;
+      filter->initialized = 1;
+      return input;
+  }
  
-    filter->value = filter->alpha * input + (1.0f - filter->alpha) * filter->value;
-    return filter->value;
+  filter->value = filter->alpha * input + (1.0f - filter->alpha) * filter->value;
+  return filter->value;
 }

 /**
  * @brief Инициализация фильтра скользящего среднего (float)
  * @param filter Указатель на структуру фильтра
+  * @return 0 - успех, -1 - ошибка
  */
-void FilterAverage_Init(FilterAverage_t* filter, uint8_t size) {
-    if (filter == NULL) return;
-    if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return;
+int FilterAverage_Init(FilterAverage_t* filter, uint8_t size, FilterMode_t mode) {
+  check_init_filter(filter);
+  if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return -1;
  
-    memset(filter->buffer, 0, sizeof(filter->buffer));
-    filter->size = size;
-    filter->sum = 0.0f;
-    filter->index = 0;
-    filter->count = 0;
+  memset(filter->buffer, 0, sizeof(filter->buffer));
+  filter->size = size;
+  filter->sum = 0.0f;
+  filter->index = 0;
+  filter->count = 0;
+  filter->mode = mode;
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterAverage_Init;
+  filter->process = &FilterAverage_Process;
+  return 0;
 }

 /**
@ -109,21 +135,33 @@ void FilterAverage_Init(FilterAverage_t* filter, uint8_t size) {
  * @return Отфильтрованное значение
  */
 float FilterAverage_Process(FilterAverage_t* filter, float input) {
-    if (filter == NULL) return input;
+  check_process_filter(filter);
  
-    // Вычитаем старое значение из суммы
-    if (filter->count == FILTER_AVERAGE_MAX_SIZE) {
-        filter->sum -= filter->buffer[filter->index];
-    } else {
-        filter->count++;
+  // Общая логика для обоих режимов
+  filter->sum += input;
+  filter->count++;
+  
+  // Логика скользящего среднего
+  if (filter->mode == FILTER_MODE_MOVING) {
+    if (filter->count > filter->size) {
+      filter->sum -= filter->buffer[filter->index];
+      filter->count = filter->size; // Поддерживаем фиксированный размер окна
    }
-    
-    // Добавляем новое значение
    filter->buffer[filter->index] = input;
-    filter->sum += input;
-    filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE;
+    filter->index = (filter->index + 1) % filter->size;
+    filter->lastValue = filter->sum / filter->count;
+  }
+  else
+  {
+    if (filter->count > filter->size)
+    {
+      filter->lastValue = filter->sum / filter->count;
+      filter->count = 0;
+      filter->sum = 0;
+    }
+  }
  
-    return filter->sum / filter->count;
+  return filter->lastValue;  
 }

 /**
@ -134,12 +172,16 @@ float FilterAverage_Process(FilterAverage_t* filter, float input) {
  * @return 0 - успех, -1 - ошибка
  */
 int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) {
-    if (filter == NULL || coeffs == NULL) return -1;
-    if (order > FILTER_POLY_MAX_ORDER) return -1;
+  check_init_filter(filter);
+  if ((coeffs == NULL) || (order > FILTER_POLY_MAX_ORDER)) return -1;
  
-    filter->order = order;
-    memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(float));
-    return 0;
+  filter->order = order;
+  memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(float));
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterPoly_Init;
+  filter->process = &FilterPoly_Process;
+  return 0;
 }

 /**
@ -149,17 +191,17 @@ int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) {
  * @return Скорректированное значение
  */
 float FilterPoly_Process(FilterPoly_t* filter, float input) {
-    if (filter == NULL) return input;
+  check_process_filter(filter);
  
-    float result = 0.0f;
-    float x_power = 1.0f;
+  float result = 0.0f;
+  float x_power = 1.0f;
  
-    for (uint8_t i = 0; i <= filter->order; i++) {
-        result += filter->coefficients[i] * x_power;
-        x_power *= input;
-    }
+  for (uint8_t i = 0; i <= filter->order; i++) {
+    result += filter->coefficients[i] * x_power;
+    x_power *= input;
+  }
  
-    return result;
+  return result;
 }

 /**
@ -169,14 +211,21 @@ float FilterPoly_Process(FilterPoly_t* filter, float input) {
  * @param output_arr Массив выходных значений
  * @param size Размер таблицы
  * @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция)
+  * @return 0 - успех, -1 - ошибка
  */
-void FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, uint16_t size, uint8_t interpolation) {
-    if (filter == NULL || input_arr == NULL || output_arr == NULL) return;
+int FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, uint16_t size, uint8_t interpolation) {
+  check_init_filter(filter);
+  if ((input_arr == NULL) || (output_arr == NULL)) return -1;
  
-    filter->input_values = input_arr;
-    filter->output_values = output_arr;
-    filter->size = size;
-    filter->interpolation = interpolation;
+  filter->input_values = input_arr;
+  filter->output_values = output_arr;
+  filter->size = size;
+  filter->interpolation = interpolation;
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterLUT_Init;
+  filter->process = &FilterLUT_Process;
+  return 0;
 }

 /**
@ -186,71 +235,78 @@ void FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, ui
  * @return Выходное значение по таблице
  */
 float FilterLUT_Process(FilterLUT_t* filter, float input) {
-    if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) {
-        return input;
+  check_process_filter(filter);
+  if((filter->input_values == NULL) || (filter->output_values == NULL)) {
+    return input;
+  }
+  
+  // Поиск ближайших значений в таблице
+  uint16_t left_index = 0;
+  uint16_t right_index = filter->size - 1;
+  
+  // Если значение за пределами таблицы - возвращаем крайние значения
+  if (input <= filter->input_values[0]) {
+    return filter->output_values[0];
+  }
+  if (input >= filter->input_values[right_index]) {
+    return filter->output_values[right_index];
+  }
+  
+  // Бинарный поиск позиции
+  while (right_index - left_index > 1) {
+    uint16_t mid_index = left_index + (right_index - left_index) / 2;
+    if (input <= filter->input_values[mid_index]) {
+      right_index = mid_index;
+    } else {
+      left_index = mid_index;
    }
+  }
  
-    // Поиск ближайших значений в таблице
-    uint16_t left_index = 0;
-    uint16_t right_index = filter->size - 1;
+  // Без интерполяции - возвращаем значение левой границы
+  if (!filter->interpolation) {
+    return filter->output_values[left_index];
+  }
  
-    // Если значение за пределами таблицы - возвращаем крайние значения
-    if (input <= filter->input_values[0]) {
-        return filter->output_values[0];
-    }
-    if (input >= filter->input_values[right_index]) {
-        return filter->output_values[right_index];
-    }
+  // Линейная интерполяция
+  float x0 = filter->input_values[left_index];
+  float x1 = filter->input_values[right_index];
+  float y0 = filter->output_values[left_index];
+  float y1 = filter->output_values[right_index];
  
-    // Бинарный поиск позиции
-    while (right_index - left_index > 1) {
-        uint16_t mid_index = left_index + (right_index - left_index) / 2;
-        if (input <= filter->input_values[mid_index]) {
-            right_index = mid_index;
-        } else {
-            left_index = mid_index;
-        }
-    }
+  if (x1 == x0) {
+    return y0; // Избегаем деления на ноль
+  }
  
-    // Без интерполяции - возвращаем значение левой границы
-    if (!filter->interpolation) {
-        return filter->output_values[left_index];
-    }
-    
-    // Линейная интерполяция
-    float x0 = filter->input_values[left_index];
-    float x1 = filter->input_values[right_index];
-    float y0 = filter->output_values[left_index];
-    float y1 = filter->output_values[right_index];
-    
-    if (x1 == x0) {
-        return y0; // Избегаем деления на ноль
-    }
-    
-    return y0 + (input - x0) * (y1 - y0) / (x1 - x0);
+  return y0 + (input - x0) * (y1 - y0) / (x1 - x0);
 }
 // ==================== INT32_T ВЕРСИИ ====================

 // Вспомогательная функция для сравнения int32_t
 static int Filter_int32_compare(const void *a, const void *b) {
-    int32_t ia = *(const int32_t*)a;
-    int32_t ib = *(const int32_t*)b;
-    if (ia < ib) return -1;
-    if (ia > ib) return 1;
-    return 0;
+  int32_t ia = *(const int32_t*)a;
+  int32_t ib = *(const int32_t*)b;
+  if (ia < ib) return -1;
+  if (ia > ib) return 1;
+  return 0;
 }

 /**
  * @brief Инициализация медианного фильтра (int32_t)
  * @param filter Указатель на структуру фильтра
+  * @return 0 - успех, -1 - ошибка
  */
-void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
-    if (filter == NULL) return;
-    if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return;
+int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
+  check_init_filter(filter);
+  if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1;
  
-    memset(filter->buffer, 0, sizeof(filter->buffer));
-    filter->index = 0;
-    filter->size = size;
+  memset(filter->buffer, 0, sizeof(filter->buffer));
+  filter->index = 0;
+  filter->size = size;
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterMedianInt_Init;
+  filter->process = &FilterMedianInt_Process;
+  return 0;
 }

 /**
@ -260,19 +316,19 @@ void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
  * @return Отфильтрованное значение
  */
 int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) {
-    if (filter == NULL) return input;
+  check_process_filter(filter);
  
-    // Добавляем значение в буфер
-    filter->buffer[filter->index] = input;
-    filter->index = (filter->index + 1) % filter->size;
+  // Добавляем значение в буфер
+  filter->buffer[filter->index] = input;
+  filter->index = (filter->index + 1) % filter->size;
  
-    // Копируем буфер для сортировки
-    int32_t sort_buffer[FILTER_MEDIAN_MAX_SIZE];
-    memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer));
+  // Копируем буфер для сортировки
+  int32_t sort_buffer[FILTER_MEDIAN_MAX_SIZE];
+  memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer));
  
-    // Сортируем и возвращаем медиану
-    qsort(sort_buffer, filter->size, sizeof(int32_t), Filter_int32_compare);
-    return sort_buffer[filter->size / 2];
+  // Сортируем и возвращаем медиану
+  qsort(sort_buffer, filter->size, sizeof(int32_t), Filter_int32_compare);
+  return sort_buffer[filter->size / 2];
 }

 /**
@ -280,14 +336,20 @@ int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) {
  * @param filter Указатель на структуру фильтра
  * @param alpha Коэффициент сглаживания в масштабированном виде
  * @param scale Масштаб коэффициента (например 100 для работы с процентами)
+  * @return 0 - успех, -1 - ошибка
  */
-void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) {
-    if (filter == NULL) return;
+int FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) {
+  check_init_filter(filter);
  
-    filter->alpha = alpha;
-    filter->scale = scale;
-    filter->value = 0;
-    filter->initialized = 0;
+  filter->alpha = alpha;
+  filter->scale = scale;
+  filter->value = 0;
+  filter->initialized = 0;
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterExpInt_Init;
+  filter->process = &FilterExpInt_Process;
+  return 0;
 }

 /**
@ -297,35 +359,42 @@ void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) {
  * @return Отфильтрованное значение
  */
 int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input) {
-    if (filter == NULL) return input;
+  check_process_filter(filter);
  
-    if (!filter->initialized) {
-        filter->value = input;
-        filter->initialized = 1;
-        return input;
-    }
+  if (!filter->initialized) {
+    filter->value = input;
+    filter->initialized = 1;
+    return input;
+  }
  
-    // value = (alpha * input + (scale - alpha) * value) / scale
-    int64_t result = (int64_t)filter->alpha * input + 
-                    (int64_t)(filter->scale - filter->alpha) * filter->value;
-    filter->value = (int32_t)(result / filter->scale);
+  // value = (alpha * input + (scale - alpha) * value) / scale
+  int64_t result = (int64_t)filter->alpha * input + 
+                  (int64_t)(filter->scale - filter->alpha) * filter->value;
+  filter->value = (int32_t)(result / filter->scale);
  
-    return filter->value;
+  return filter->value;
 }

 /**
  * @brief Инициализация фильтра скользящего среднего (int32_t)
  * @param filter Указатель на структуру фильтра
+  * @return 0 - успех, -1 - ошибка
  */
-void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size) {
-    if (filter == NULL) return;
-    if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return;
+int FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size, FilterMode_t mode) {
+  check_init_filter(filter);
+  if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return - 1;
  
-    memset(filter->buffer, 0, sizeof(filter->buffer));
-    filter->size = size;
-    filter->sum = 0;
-    filter->index = 0;
-    filter->count = 0;
+  memset(filter->buffer, 0, sizeof(filter->buffer));
+  filter->size = size;
+  filter->sum = 0;
+  filter->index = 0;
+  filter->count = 0;
+  filter->mode = mode;
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterAverageInt_Init;
+  filter->process = &FilterAverageInt_Process;
+  return 0;
 }

 /**
@ -335,21 +404,33 @@ void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size) {
  * @return Отфильтрованное значение
  */
 int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) {
-    if (filter == NULL) return input;
+  check_process_filter(filter);
  
-    // Вычитаем старое значение из суммы
-    if (filter->count == FILTER_AVERAGE_MAX_SIZE) {
-        filter->sum -= filter->buffer[filter->index];
-    } else {
-        filter->count++;
+  // Общая логика для обоих режимов
+  filter->sum += input;
+  filter->count++;
+  
+  // Логика скользящего среднего
+  if (filter->mode == FILTER_MODE_MOVING) {
+    if (filter->count > filter->size) {
+      filter->sum -= filter->buffer[filter->index];
+      filter->count = filter->size; // Поддерживаем фиксированный размер окна
    }
-    
-    // Добавляем новое значение
    filter->buffer[filter->index] = input;
-    filter->sum += input;
-    filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE;
+    filter->index = (filter->index + 1) % filter->size;
+    filter->lastValue = filter->sum / filter->count;
+  }
+  else
+  {
+    if (filter->count == filter->size)
+    {
+      filter->lastValue = filter->sum / filter->count;
+      filter->count = 0;
+      filter->sum = 0;
+    }
+  }
  
-    return (int32_t)(filter->sum / filter->count);
+  return filter->lastValue;  
 }

 /**
@ -361,13 +442,17 @@ int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) {
  * @return 0 - успех, -1 - ошибка
  */
 int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, int32_t scale) {
-    if (filter == NULL || coeffs == NULL) return -1;
-    if (order > FILTER_POLY_MAX_ORDER) return -1;
+  check_init_filter(filter);
+  if ((coeffs == NULL) || (order > FILTER_POLY_MAX_ORDER)) return -1;
  
-    filter->order = order;
-    filter->scale = scale;
-    memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(int32_t));
-    return 0;
+  filter->order = order;
+  filter->scale = scale;
+  memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(int32_t));
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterPolyInt_Init;
+  filter->process = &FilterPolyInt_Process;
+  return 0;
 }

 /**
@ -377,24 +462,24 @@ int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order,
  * @return Скорректированное значение
  */
 int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) {
-    if (filter == NULL) return input;
+  check_process_filter(filter);
  
-    // coefficients[0] = a_n * scale
-    // coefficients[1] = a_{n-1} * scale  
-    // ...
-    // coefficients[n] = a_0 * scale
+  // coefficients[0] = a_n * scale
+  // coefficients[1] = a_{n-1} * scale  
+  // ...
+  // coefficients[n] = a_0 * scale
  
-    int64_t result = filter->coefficients[0]; // Старший коэффициент
-    int64_t x_scaled = input;
+  int64_t result = filter->coefficients[0]; // Старший коэффициент
+  int64_t x_scaled = input;
  
-    for (uint8_t i = 1; i <= filter->order; i++) {
-        result = (result * x_scaled) / filter->scale + filter->coefficients[i];
-    }
+  for (uint8_t i = 1; i <= filter->order; i++) {
+    result = (result * x_scaled) / filter->scale + filter->coefficients[i];
+  }
  
-    // Домножаем на scale для a_0
-    result = (result * filter->scale);
+  // Домножаем на scale для a_0
+  result = (result * filter->scale);
  
-    return (int32_t)(result / filter->scale);
+  return (int32_t)(result / filter->scale);
 }

 /**
@ -404,14 +489,21 @@ int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) {
  * @param output_arr Массив выходных значений
  * @param size Размер таблицы
  * @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция)
+  * @return 0 - успех, -1 - ошибка
  */
-void FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* output_arr, uint16_t size, uint8_t interpolation) {
-    if (filter == NULL || input_arr == NULL || output_arr == NULL) return;
+int FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* output_arr, uint16_t size, uint8_t interpolation) {
+  check_init_filter(filter);
+  if ((input_arr == NULL) || (output_arr == NULL)) return -1;
  
-    filter->input_values = input_arr;
-    filter->output_values = output_arr;
-    filter->size = size;
-    filter->interpolation = interpolation;
+  filter->input_values = input_arr;
+  filter->output_values = output_arr;
+  filter->size = size;
+  filter->interpolation = interpolation;
+  
+  filter->state = FILTER_READY;
+  filter->reset   = &FilterLUTInt_Init;
+  filter->process = &FilterLUTInt_Process;
+  return 0;
 }

 /**
@ -421,48 +513,95 @@ void FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* outp
  * @return Выходное значение по таблице
  */
 int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) {
-    if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) {
-        return input;
+  check_process_filter(filter);    
+  if((filter->input_values == NULL) || (filter->output_values == NULL)) {
+    return input;
+  }
+  
+  // Поиск ближайших значений в таблице
+  uint16_t left_index = 0;
+  uint16_t right_index = filter->size - 1;
+  
+  // Если значение за пределами таблицы - возвращаем крайние значения
+  if (input <= filter->input_values[0]) {
+    return filter->output_values[0];
+  }
+  if (input >= filter->input_values[right_index]) {
+    return filter->output_values[right_index];
+  }
+  
+  // Бинарный поиск позиции
+  while (right_index - left_index > 1) {
+    uint16_t mid_index = left_index + (right_index - left_index) / 2;
+    if (input <= filter->input_values[mid_index]) {
+      right_index = mid_index;
+    } else {
+      left_index = mid_index;
    }
+  }
  
-    // Поиск ближайших значений в таблице
-    uint16_t left_index = 0;
-    uint16_t right_index = filter->size - 1;
+  // Без интерполяции - возвращаем значение левой границы
+  if (!filter->interpolation) {
+    return filter->output_values[left_index];
+  }
  
-    // Если значение за пределами таблицы - возвращаем крайние значения
-    if (input <= filter->input_values[0]) {
-        return filter->output_values[0];
-    }
-    if (input >= filter->input_values[right_index]) {
-        return filter->output_values[right_index];
-    }
+  // Линейная интерполяция (целочисленная)
+  int64_t x0 = filter->input_values[left_index];
+  int64_t x1 = filter->input_values[right_index];
+  int64_t y0 = filter->output_values[left_index];
+  int64_t y1 = filter->output_values[right_index];
  
-    // Бинарный поиск позиции
-    while (right_index - left_index > 1) {
-        uint16_t mid_index = left_index + (right_index - left_index) / 2;
-        if (input <= filter->input_values[mid_index]) {
-            right_index = mid_index;
-        } else {
-            left_index = mid_index;
-        }
-    }
+  if (x1 == x0) {
+    return (int32_t)y0; // Избегаем деления на ноль
+  }
  
-    // Без интерполяции - возвращаем значение левой границы
-    if (!filter->interpolation) {
-        return filter->output_values[left_index];
-    }
-    
-    // Линейная интерполяция (целочисленная)
-    int64_t x0 = filter->input_values[left_index];
-    int64_t x1 = filter->input_values[right_index];
-    int64_t y0 = filter->output_values[left_index];
-    int64_t y1 = filter->output_values[right_index];
-    
-    if (x1 == x0) {
-        return (int32_t)y0; // Избегаем деления на ноль
-    }
-    
-    int64_t result = y0 + (input - x0) * (y1 - y0) / (x1 - x0);
-    return (int32_t)result;
+  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
Author	SHA1	Message	Date
Razvalyaev	606058ef55	Добавлен дефайн для определения количества элементов в структуре	2025-11-14 18:02:43 +03:00
Razvalyaev	48daccef2d	Добавлен биквадратный фильтр от CMSIS-DSP	2025-11-13 17:13:04 +03:00
Razvalyaev	9b9969dd7c	Доработки фильтров: - универсализированы проверки состояния инициализации фильтров - avg сделан скользящий и обычный - плюс еще что-то наверное	2025-11-11 17:24:53 +03:00