/** ****************************************************************************** * @file filters.h * @brief Заголовочный файл библиотеки фильтров ****************************************************************************** * @addtogroup FILTERS Filters Library * @brief Библиотека математических фильтров и коррекций * @details Поддерживает: - Медианную фильтрацию (float и int32_t) - Экспоненциальное скользящее среднее (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_POLY_MAX_ORDER - Максимальный порядок полинома (по умолчанию 4) @par Пример использования: @code #include "filters.h" // Фильтры для float FilterMedian_t median_f; FilterExp_t exp_f; FilterAverage_t avg_f; FilterPoly_t poly_f; // Фильтры для int32_t FilterMedianInt_t median_i; FilterExpInt_t exp_i; FilterAverageInt_t avg_i; FilterPolyInt_t poly_i; // Коэффициенты полинома float poly_coeffs[3] = {0.0f, 1.1f, -0.05f}; int32_t poly_coeffs_int[3] = {0, 110, -5}; // 1.1 и -0.05 с масштабом 100 void filters_init(void) { // Float версии FilterMedian_Init(&median_f); FilterExp_Init(&exp_f, 0.1f); FilterAverage_Init(&avg_f); FilterPoly_Init(&poly_f, poly_coeffs, 3); // Int версии FilterMedianInt_Init(&median_i); FilterExpInt_Init(&exp_i, 10); // alpha = 0.1 с масштабом 100 FilterAverageInt_Init(&avg_i); FilterPolyInt_Init(&poly_i, poly_coeffs_int, 3, 100); // масштаб 100 } // Обработка float значений float process_value_float(float raw) { float filtered; filtered = FilterMedian_Process(&median_f, raw); filtered = FilterExp_Process(&exp_f, filtered); filtered = FilterAverage_Process(&avg_f, filtered); filtered = FilterPoly_Process(&poly_f, filtered); return filtered; } // Обработка int32_t значений (квантов АЦП) int32_t process_value_int(int32_t raw_adc_quant) { int32_t filtered; filtered = FilterMedianInt_Process(&median_i, raw_adc_quant); filtered = FilterExpInt_Process(&exp_i, filtered); filtered = FilterAverageInt_Process(&avg_i, filtered); filtered = FilterPolyInt_Process(&poly_i, filtered); return filtered; } @endcode * @{ *****************************************************************************/ #ifndef __FILTERS_H_ #define __FILTERS_H_ #include "mylibs_defs.h" #include #include #include #ifdef FILTERS_ENABLE #ifdef ARM_MATH_CM4 #include "arm_math.h" #define DSP_FITLERS 1 #endif #ifndef FILTER_AVERAGE_MAX_SIZE #ifndef FILTERS_DISABLE_MOVING_AVERAGE #define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна усредняющего фильтра #else #define FILTER_AVERAGE_MAX_SIZE 100000 ///< Размер окна усредняющего фильтра без буфера #endif #endif #ifndef FILTER_MEDIAN_MAX_SIZE #define FILTER_MEDIAN_MAX_SIZE 10 ///< Размер окна медианного фильтра #endif #ifndef FILTER_POLY_MAX_ORDER #define FILTER_POLY_MAX_ORDER 4 ///< Максимальный порядок полинома #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_ReInit(_fltr_, ...) \ ((_fltr_)->reset != NULL) ? (_fltr_)->reset(_fltr_, __VA_ARGS__): -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_isDataReady(_fltr_) ((_fltr_)->dataProcessing == 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_NOT_INIT, FILTER_READY, FILTER_ENABLE }FilterState_t; typedef enum { FILTER_MODE_DEFAULT = 0, FILTER_MODE_MOVING, } FilterMode_t; // ==================== FLOAT ВЕРСИИ ==================== /** * @brief Структура медианного фильтра (float) */ typedef struct _FilterMedian_t{ FilterState_t state; ///< Состояние фильтра float buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений uint8_t index; ///< Текущий индекс uint8_t size; ///< Фактический размер фильтра uint8_t dataProcessing; ///< Флаг - данные в обработке int (*reset)(struct _FilterMedian_t *filter, uint8_t size); float (*process)(struct _FilterMedian_t *filter, float input); } FilterMedian_t; /** * @brief Структура экспоненциального фильтра (float) */ typedef struct _FilterExp_t { FilterState_t state; ///< Состояние фильтра float alpha; ///< Коэффициент сглаживания (0..1) float value; ///< Текущее значение uint8_t initialized; ///< Флаг инициализации uint8_t dataProcessing; ///< Флаг - данные в обработке int (*reset)(struct _FilterExp_t *filter, float alpha); float (*process)(struct _FilterExp_t *filter, float input); } FilterExp_t; /** * @brief Структура фильтра скользящего среднего (float) */ typedef struct _FilterAverage_t{ FilterState_t state; ///< Состояние фильтра FilterMode_t mode; ///< Режим фильтра #ifndef FILTERS_DISABLE_MOVING_AVERAGE float buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений #endif uint32_t size; ///< Фактический размер фильтра float sum; ///< Сумма значений uint32_t index; ///< Текущий индекс uint32_t count; ///< Количество элементов float lastValue; ///< Последнее измеренное значение uint8_t dataProcessing; ///< Флаг - данные в обработке int (*reset)(struct _FilterAverage_t *filter, uint32_t size, FilterMode_t mode); float (*process)(struct _FilterAverage_t *filter, float input); } FilterAverage_t; /** * @brief Структура полиномиальной коррекции (float) */ typedef struct _FilterPoly_t{ FilterState_t state; ///< Состояние фильтра float coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома uint8_t order; ///< Порядок полинома uint8_t dataProcessing; ///< Флаг - данные в обработке 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 _FilterLUT_t{ FilterState_t state; ///< Состояние фильтра float* input_values; // Массив входных значений float* output_values; // Массив выходных значений uint16_t size; // Размер таблицы uint8_t interpolation; // Флаг интерполяции (0 - отключена, 1 - линейная) uint8_t dataProcessing; ///< Флаг - данные в обработке 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); int FilterExp_Init(FilterExp_t* filter, float alpha); float FilterExp_Process(FilterExp_t* filter, float input); int FilterAverage_Init(FilterAverage_t* filter, uint32_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); 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); // Расчет всякого #define FilterExp_CalcAlpha(tau, TsUs) (((float)TsUs/1000000) / (((float)TsUs/1000000) + (tau))) /* Расчет alpha для времени нарастания до 63% */ #define FilterExp_CalcAlpha63(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 1.0f, TsUs) /* Расчет alpha для времени нарастания до 86% */ #define FilterExp_CalcAlpha86(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 2.0f, TsUs) /* Расчет alpha для времени нарастания до 95% */ #define FilterExp_CalcAlpha95(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 3.0f, TsUs) /* Расчет alpha для времени нарастания до 98% */ #define FilterExp_CalcAlpha98(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 4.0f, TsUs) /* Расчет alpha для времени нарастания до 99% */ #define FilterExp_CalcAlpha99(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 5.0f, TsUs) // ==================== INT32_T ВЕРСИИ ==================== /** * @brief Структура медианного фильтра (int32_t) */ typedef struct _FilterMedianInt_t{ FilterState_t state; ///< Состояние фильтра int32_t buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений uint8_t index; ///< Текущий индекс uint8_t size; ///< Фактический размер фильтра uint8_t dataProcessing; ///< Флаг - данные в обработке 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 _FilterExpInt_t{ FilterState_t state; ///< Состояние фильтра int32_t alpha; ///< Коэффициент сглаживания (в масштабе scale) int32_t value; ///< Текущее значение uint8_t initialized; ///< Флаг инициализации int32_t scale; ///< Масштаб коэффициента (например 100 для 0.01) uint8_t dataProcessing; ///< Флаг - данные в обработке 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 _FilterAverageInt_t{ FilterState_t state; ///< Состояние фильтра FilterMode_t mode; ///< Режим фильтра #ifndef FILTERS_DISABLE_MOVING_AVERAGE int32_t buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений #endif uint32_t size; ///< Фактический размер фильтра int64_t sum; ///< Сумма значений uint32_t index; ///< Текущий индекс uint32_t count; ///< Количество элементов uint32_t lastValue; ///< Последнее измеренное значение uint8_t dataProcessing; ///< Флаг - данные в обработке int (*reset)(struct _FilterAverageInt_t *filter, uint32_t size, FilterMode_t mode); int32_t (*process)(struct _FilterAverageInt_t *filter, int32_t input); } FilterAverageInt_t; /** * @brief Структура полиномиальной коррекции (int32_t) */ typedef struct _FilterPolyInt_t{ FilterState_t state; ///< Состояние фильтра int32_t coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома uint8_t order; ///< Порядок полинома int32_t scale; ///< Масштаб коэффициентов uint8_t dataProcessing; ///< Флаг - данные в обработке 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 _FilterLUTInt_t{ FilterState_t state; ///< Состояние фильтра int32_t* input_values; ///< Массив входных значений int32_t* output_values; ///< Массив выходных значений uint16_t size; ///< Размер таблицы uint8_t interpolation; ///< Флаг интерполяции uint8_t dataProcessing; ///< Флаг - данные в обработке 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 версии функций int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size); int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input); int FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale); int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input); int FilterAverageInt_Init(FilterAverageInt_t* filter, uint32_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); 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 #endif // __FILTERS_H_