@@ -0,0 +1,526 @@
/**
******************************************************************************
* @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 с
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 с
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 <stdint.h>
#include <stdlib.h>
#include <string.h>
#ifdef FILTERS_ENABLE
#ifdef ARM_MATH_CM4
#include "arm_math.h"
#define DSP_FITLERS 1
#else
#include "math.h"
#endif
#ifndef FILTER_AVERAGE_MAX_SIZE
#ifndef FILTERS_DISABLE_MOVING_AVERAGE
#define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна усредняющего фильтра
#else
#define FILTER_AVERAGE_MAX_SIZE 65535 ///< Размер окна усредняющего фильтра без буфера
#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
#ifndef FILTER_RMS_MAX_SIZE
#define FILTER_RMS_MAX_SIZE 128 ///< Максимальный размер окна (рекомендуется степень 2)
#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 init_val);
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;
/**
* @brief Структура True RMS фильтра (float)
*/
typedef struct _FilterRMS_t {
FilterState_t state; ///< Состояние фильтра
float buffer_sq[FILTER_RMS_MAX_SIZE]; ///< Буфер квадратов значений
float sum_squares; ///< Текущая сумма квадратов
float last_rms; ///< Последнее рассчитанное RMS значение
uint32_t window_size; ///< Размер окна усреднения
uint32_t index; ///< Текущий индекс в буфере
uint32_t count; ///< Количество накопленных значений
uint8_t dataProcessing; ///< Флаг - данные в обработке
int (*reset)(struct _FilterRMS_t *filter, uint32_t window_size);
float (*process)(struct _FilterRMS_t *filter, float input);
} FilterRMS_t;
// Float версии функций
int FilterMedian_Init(FilterMedian_t* filter, uint8_t size, float init_val);
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);
int FilterRMS_Init(FilterRMS_t* filter, uint32_t window_size);
float FilterRMS_Process(FilterRMS_t* filter, float input);
/**
* @brief Расчет коэфициента альфа
* @param tau Постоянная времени (время нарастания до 63% от уровня сигнала)
* @param TsUs Период вызова фильтра в микросекундах
* @return Коэфициент альфа
*/
#define FilterExp_CalcAlpha(tau, TsUs) (((float)TsUs/1000000) / (((float)TsUs/1000000) + (tau)))
/**
* @brief Расчет alpha для времени нарастания до 63%
* @param rise_time Требуемое время нарастания до 63%
* @param TsUs Период вызова фильтра в микросекундах
* @return Коэфициент альфа
*/
#define FilterExp_CalcAlpha63(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 1.0f, TsUs)
/**
* @brief Расчет alpha для времени нарастания до 86%
* @param rise_time Требуемое время нарастания до 86%
* @param TsUs Период вызова фильтра в микросекундах
* @return Коэфициент альфа
*/
#define FilterExp_CalcAlpha86(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 2.0f, TsUs)
/**
* @brief Расчет alpha для времени нарастания до 95%
* @param rise_time Требуемое время нарастания до 95%
* @param TsUs Период вызова фильтра в микросекундах
* @return Коэфициент альфа
*/
#define FilterExp_CalcAlpha95(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 3.0f, TsUs)
/**
* @brief Расчет alpha для времени нарастания до 98%
* @param rise_time Требуемое время нарастания до 98%
* @param TsUs Период вызова фильтра в микросекундах
* @return Коэфициент альфа
*/
#define FilterExp_CalcAlpha98(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 4.0f, TsUs)
/**
* @brief Расчет alpha для времени нарастания до 99%
* @param rise_time Требуемое время нарастания до 99%
* @param TsUs Период вызова фильтра в микросекундах
* @return Коэфициент альфа
*/
#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]; ///< Буфер значений
int32_t sorted[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер отсортированных значений
uint8_t index; ///< Текущий индекс
uint8_t size; ///< Фактический размер фильтра
uint8_t dataProcessing; ///< Флаг - данные в обработке
int (*reset)(struct _FilterMedianInt_t *filter, uint8_t size, int32_t init_val);
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;
/**
* @brief Структура True RMS фильтра (int32_t)
*/
typedef struct _FilterRMSInt_t {
FilterState_t state; ///< Состояние фильтра
int64_t buffer_sq[FILTER_RMS_MAX_SIZE]; ///< Буфер квадратов значений
int64_t sum_squares; ///< Текущая сумма квадратов
int32_t last_rms; ///< Последнее рассчитанное RMS значение
uint32_t window_size; ///< Размер окна усреднения
uint32_t index; ///< Текущий индекс в буфере
uint32_t count; ///< Количество накопленных значений
uint8_t dataProcessing; ///< Флаг - данные в обработке
int (*reset)(struct _FilterRMSInt_t *filter, uint32_t window_size);
int32_t (*process)(struct _FilterRMSInt_t *filter, int32_t input);
} FilterRMSInt_t;
// Int32_t версии функций
int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size, int32_t init_val);
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);
int FilterRMSInt_Init(FilterRMSInt_t* filter, uint32_t window_size);
int32_t FilterRMSInt_Process(FilterRMSInt_t* filter, int32_t input);
// ==================== ДРУГИЕ ФИЛЬТРЫ ====================
/**
* @brief Структура полосового фильтра с
* @details Комбинация дифференциатора и полосового фильтра 2-г о
* Используется для выделения сетевой частоты и подготовки к детектированию нуля.
*/
typedef struct _FilterBandPassDerivative_t {
FilterState_t state; ///< Состояние фильтра
// Дифференциатор
float prev_input; ///< Предыдущее входное значение
// Полосовой фильтр (биквадратный, прямая форма II)
float b0, b1, b2; ///< Коэффициенты числителя
float a1, a2; ///< Коэффициенты знаменателя
// Состояния фильтра
float x1, x2; ///< Состояния входа
float y1, y2; ///< Состояния выхода
float last_output; ///< Последнее выходное значение
uint8_t dataProcessing; ///< Флаг обработки данных
// Указатели на функции
int (*reset)(struct _FilterBandPassDerivative_t *filter,
float center_freq_ratio, float bandwidth_ratio);
float (*process)(struct _FilterBandPassDerivative_t *filter, float input);
} FilterBandPassDerivative_t;
int FilterBandPassDerivative_Init(FilterBandPassDerivative_t* filter,
float center_freq_ratio,
float bandwidth_ratio);
float FilterBandPassDerivative_Process(FilterBandPassDerivative_t* filter,
float input);
#ifdef DSP_FITLERS
/**
* @brief Структура биквадратного фильтра с
*/
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_
