From 48daccef2d1fefe965f020e5b342ed683fe40a62 Mon Sep 17 00:00:00 2001 From: Razvalyaev Date: Thu, 13 Nov 2025 17:13:04 +0300 Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=20=D0=B1=D0=B8=D0=BA=D0=B2=D0=B0=D0=B4=D1=80=D0=B0=D1=82?= =?UTF-8?q?=D0=BD=D1=8B=D0=B9=20=D1=84=D0=B8=D0=BB=D1=8C=D1=82=D1=80=20?= =?UTF-8?q?=D0=BE=D1=82=20CMSIS-DSP?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MyLibs/Inc/filters.h | 131 +++++++++++++++++++++++++++++++++++++------ MyLibs/Src/filters.c | 66 ++++++++++++++++++++++ 2 files changed, 180 insertions(+), 17 deletions(-) diff --git a/MyLibs/Inc/filters.h b/MyLibs/Inc/filters.h index a0801d9..e974db3 100644 --- a/MyLibs/Inc/filters.h +++ b/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* 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 версии функций @@ -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 diff --git a/MyLibs/Src/filters.c b/MyLibs/Src/filters.c index e05d7a3..cf8b444 100644 --- a/MyLibs/Src/filters.c +++ b/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