Добавлен биквадратный фильтр от CMSIS-DSP
This commit is contained in:
parent
9b9969dd7c
commit
48daccef2d
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user