Доработки фильтров:

- универсализированы проверки состояния инициализации фильтров
- avg сделан скользящий и обычный
- плюс еще что-то наверное
This commit is contained in:
Razvalyaev 2025-11-11 12:45:27 +03:00
parent 3b162c9f8c
commit 9b9969dd7c
2 changed files with 399 additions and 320 deletions

View File

@ -11,6 +11,7 @@
- Экспоненциальное скользящее среднее (float и int32_t) - Экспоненциальное скользящее среднее (float и int32_t)
- Скользящее среднее арифметическое (float и int32_t) - Скользящее среднее арифметическое (float и int32_t)
- Полиномиальную коррекцию (float и int32_t) - Полиномиальную коррекцию (float и int32_t)
- Табличный фильтр LUT (Look-Up Table) (float и int32_t)
Параметры для конфигурации: Параметры для конфигурации:
- @ref FILTERS_ENABLE - Включить библиотеку фильтров - @ref FILTERS_ENABLE - Включить библиотеку фильтров
@ -84,29 +85,62 @@ int32_t process_value_int(int32_t raw_adc_quant) {
#ifdef FILTERS_ENABLE #ifdef FILTERS_ENABLE
#ifndef FILTER_AVERAGE_MAX_SIZE #ifndef FILTER_AVERAGE_MAX_SIZE
#define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна медианного фильтра #define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна усредняющего фильтра
#endif #endif
#ifndef FILTER_MEDIAN_MAX_SIZE #ifndef FILTER_MEDIAN_MAX_SIZE
#define FILTER_MEDIAN_MAX_SIZE 8 ///< Размер окна усредняющего фильтра #define FILTER_MEDIAN_MAX_SIZE 10 ///< Размер окна медианного фильтра
#endif #endif
#ifndef FILTER_POLY_MAX_ORDER #ifndef FILTER_POLY_MAX_ORDER
#define FILTER_POLY_MAX_ORDER 4 ///< Максимальный порядок полинома #define FILTER_POLY_MAX_ORDER 4 ///< Максимальный порядок полинома
#endif #endif
#define FILTER_GET_STATE(_fltr_) (_fltr_)->state
// ==================== FLOAT ВЕРСИИ ==================== /**
* @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_isStart(_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)
typedef enum typedef enum
{ {
FILTER_DISABLE, FILTER_NOT_INIT,
FILTER_READY,
FILTER_ENABLE FILTER_ENABLE
}FilterState_t; }FilterState_t;
typedef enum
{
FILTER_MODE_DEFAULT = 0,
FILTER_MODE_MOVING,
} FilterMode_t;
// ==================== FLOAT ВЕРСИИ ====================
/** /**
* @brief Структура медианного фильтра (float) * @brief Структура медианного фильтра (float)
*/ */
typedef struct { typedef struct {
FilterState_t state; ///< Состояние фильтра
float buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений float buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений
uint8_t index; ///< Текущий индекс uint8_t index; ///< Текущий индекс
uint8_t size; ///< Фактический размер фильтра uint8_t size; ///< Фактический размер фильтра
@ -127,17 +161,20 @@ typedef struct {
*/ */
typedef struct { typedef struct {
FilterState_t state; ///< Состояние фильтра FilterState_t state; ///< Состояние фильтра
FilterMode_t mode; ///< Режим фильтра
float buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений float buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений
uint8_t size; ///< Фактический размер фильтра uint8_t size; ///< Фактический размер фильтра
float sum; ///< Сумма значений float sum; ///< Сумма значений
uint8_t index; ///< Текущий индекс uint8_t index; ///< Текущий индекс
uint8_t count; ///< Количество элементов uint8_t count; ///< Количество элементов
float lastValue; ///< Последнее измеренное значение
} FilterAverage_t; } FilterAverage_t;
/** /**
* @brief Структура полиномиальной коррекции (float) * @brief Структура полиномиальной коррекции (float)
*/ */
typedef struct { typedef struct {
FilterState_t state; ///< Состояние фильтра
float coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома float coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома
uint8_t order; ///< Порядок полинома uint8_t order; ///< Порядок полинома
} FilterPoly_t; } FilterPoly_t;
@ -154,15 +191,15 @@ typedef struct {
} FilterLUT_t; } FilterLUT_t;
// Float версии функций // 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); 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); 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); float FilterAverage_Process(FilterAverage_t* filter, float input);
int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order); int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order);
float FilterPoly_Process(FilterPoly_t* filter, float input); 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); float FilterLUT_Process(FilterLUT_t* filter, float input);
// ==================== INT32_T ВЕРСИИ ==================== // ==================== INT32_T ВЕРСИИ ====================
@ -192,11 +229,13 @@ typedef struct {
*/ */
typedef struct { typedef struct {
FilterState_t state; ///< Состояние фильтра FilterState_t state; ///< Состояние фильтра
FilterMode_t mode; ///< Режим фильтра
int32_t buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений int32_t buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений
uint8_t size; ///< Фактический размер фильтра uint8_t size; ///< Фактический размер фильтра
int64_t sum; ///< Сумма значений int64_t sum; ///< Сумма значений
uint8_t index; ///< Текущий индекс uint8_t index; ///< Текущий индекс
uint8_t count; ///< Количество элементов uint8_t count; ///< Количество элементов
uint32_t lastValue; ///< Последнее измеренное значение
} FilterAverageInt_t; } FilterAverageInt_t;
@ -223,51 +262,18 @@ typedef struct {
} FilterLUTInt_t; } FilterLUTInt_t;
// Int32_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); 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); 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); 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); 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); 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); int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input);
#else // FILTERS_ENABLE #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 #endif // FILTERS_ENABLE

View File

@ -8,6 +8,14 @@
#ifdef FILTERS_ENABLE #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 ВЕРСИИ ====================
// Вспомогательная функция для сравнения float // Вспомогательная функция для сравнения float
@ -22,15 +30,18 @@ static int Filter_float_compare(const void *a, const void *b) {
/** /**
* @brief Инициализация медианного фильтра (float) * @brief Инициализация медианного фильтра (float)
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
* @return 0 - успех, -1 - ошибка
*/ */
void FilterMedian_Init(FilterMedian_t* filter, uint8_t size) { int FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
if (filter == NULL) return; check_init_filter(filter);
if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return; if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1;
memset(filter->buffer, 0, sizeof(filter->buffer)); memset(filter->buffer, 0, sizeof(filter->buffer));
filter->index = 0; filter->index = 0;
filter->size = size; filter->size = size;
filter->state = FILTER_READY;
return 0;
} }
/** /**
@ -40,7 +51,7 @@ void FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
* @return Отфильтрованное значение * @return Отфильтрованное значение
*/ */
float FilterMedian_Process(FilterMedian_t* filter, float input) { float FilterMedian_Process(FilterMedian_t* filter, float input) {
if (filter == NULL) return input; check_process_filter(filter);
// Добавляем значение в буфер // Добавляем значение в буфер
filter->buffer[filter->index] = input; filter->buffer[filter->index] = input;
@ -59,13 +70,17 @@ float FilterMedian_Process(FilterMedian_t* filter, float input) {
* @brief Инициализация экспоненциального фильтра (float) * @brief Инициализация экспоненциального фильтра (float)
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
* @param alpha Коэффициент сглаживания (0..1) * @param alpha Коэффициент сглаживания (0..1)
* @return 0 - успех, -1 - ошибка
*/ */
void FilterExp_Init(FilterExp_t* filter, float alpha) { int FilterExp_Init(FilterExp_t* filter, float alpha) {
if (filter == NULL) return; check_init_filter(filter);
filter->alpha = alpha; filter->alpha = alpha;
filter->value = 0.0f; filter->value = 0.0f;
filter->initialized = 0; filter->initialized = 0;
filter->state = FILTER_READY;
return 0;
} }
/** /**
@ -75,7 +90,7 @@ void FilterExp_Init(FilterExp_t* filter, float alpha) {
* @return Отфильтрованное значение * @return Отфильтрованное значение
*/ */
float FilterExp_Process(FilterExp_t* filter, float input) { float FilterExp_Process(FilterExp_t* filter, float input) {
if (filter == NULL) return input; check_process_filter(filter);
if (!filter->initialized) { if (!filter->initialized) {
filter->value = input; filter->value = input;
@ -90,16 +105,21 @@ float FilterExp_Process(FilterExp_t* filter, float input) {
/** /**
* @brief Инициализация фильтра скользящего среднего (float) * @brief Инициализация фильтра скользящего среднего (float)
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
* @return 0 - успех, -1 - ошибка
*/ */
void FilterAverage_Init(FilterAverage_t* filter, uint8_t size) { int FilterAverage_Init(FilterAverage_t* filter, uint8_t size, FilterMode_t mode) {
if (filter == NULL) return; check_init_filter(filter);
if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return; if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return -1;
memset(filter->buffer, 0, sizeof(filter->buffer)); memset(filter->buffer, 0, sizeof(filter->buffer));
filter->size = size; filter->size = size;
filter->sum = 0.0f; filter->sum = 0.0f;
filter->index = 0; filter->index = 0;
filter->count = 0; filter->count = 0;
filter->mode = mode;
filter->state = FILTER_READY;
return 0;
} }
/** /**
@ -109,21 +129,33 @@ void FilterAverage_Init(FilterAverage_t* filter, uint8_t size) {
* @return Отфильтрованное значение * @return Отфильтрованное значение
*/ */
float FilterAverage_Process(FilterAverage_t* filter, float input) { 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 += input;
filter->sum -= filter->buffer[filter->index];
} else {
filter->count++; 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->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->lastValue;
filter->buffer[filter->index] = input;
filter->sum += input;
filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE;
return filter->sum / filter->count;
} }
/** /**
@ -134,11 +166,13 @@ float FilterAverage_Process(FilterAverage_t* filter, float input) {
* @return 0 - успех, -1 - ошибка * @return 0 - успех, -1 - ошибка
*/ */
int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) { int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) {
if (filter == NULL || coeffs == NULL) return -1; check_init_filter(filter);
if (order > FILTER_POLY_MAX_ORDER) return -1; if ((coeffs == NULL) || (order > FILTER_POLY_MAX_ORDER)) return -1;
filter->order = order; filter->order = order;
memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(float)); memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(float));
filter->state = FILTER_READY;
return 0; return 0;
} }
@ -149,7 +183,7 @@ int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) {
* @return Скорректированное значение * @return Скорректированное значение
*/ */
float FilterPoly_Process(FilterPoly_t* filter, float input) { float FilterPoly_Process(FilterPoly_t* filter, float input) {
if (filter == NULL) return input; check_process_filter(filter);
float result = 0.0f; float result = 0.0f;
float x_power = 1.0f; float x_power = 1.0f;
@ -169,14 +203,19 @@ float FilterPoly_Process(FilterPoly_t* filter, float input) {
* @param output_arr Массив выходных значений * @param output_arr Массив выходных значений
* @param size Размер таблицы * @param size Размер таблицы
* @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция) * @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) { int 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; check_init_filter(filter);
if ((input_arr == NULL) || (output_arr == NULL)) return -1;
filter->input_values = input_arr; filter->input_values = input_arr;
filter->output_values = output_arr; filter->output_values = output_arr;
filter->size = size; filter->size = size;
filter->interpolation = interpolation; filter->interpolation = interpolation;
filter->state = FILTER_READY;
return 0;
} }
/** /**
@ -186,7 +225,8 @@ void FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, ui
* @return Выходное значение по таблице * @return Выходное значение по таблице
*/ */
float FilterLUT_Process(FilterLUT_t* filter, float input) { float FilterLUT_Process(FilterLUT_t* filter, float input) {
if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) { check_process_filter(filter);
if((filter->input_values == NULL) || (filter->output_values == NULL)) {
return input; return input;
} }
@ -243,14 +283,18 @@ static int Filter_int32_compare(const void *a, const void *b) {
/** /**
* @brief Инициализация медианного фильтра (int32_t) * @brief Инициализация медианного фильтра (int32_t)
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
* @return 0 - успех, -1 - ошибка
*/ */
void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) { int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
if (filter == NULL) return; check_init_filter(filter);
if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return; if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1;
memset(filter->buffer, 0, sizeof(filter->buffer)); memset(filter->buffer, 0, sizeof(filter->buffer));
filter->index = 0; filter->index = 0;
filter->size = size; filter->size = size;
filter->state = FILTER_READY;
return 0;
} }
/** /**
@ -260,7 +304,7 @@ void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
* @return Отфильтрованное значение * @return Отфильтрованное значение
*/ */
int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) { 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->buffer[filter->index] = input;
@ -280,14 +324,18 @@ int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) {
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
* @param alpha Коэффициент сглаживания в масштабированном виде * @param alpha Коэффициент сглаживания в масштабированном виде
* @param scale Масштаб коэффициента (например 100 для работы с процентами) * @param scale Масштаб коэффициента (например 100 для работы с процентами)
* @return 0 - успех, -1 - ошибка
*/ */
void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) { int FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) {
if (filter == NULL) return; check_init_filter(filter);
filter->alpha = alpha; filter->alpha = alpha;
filter->scale = scale; filter->scale = scale;
filter->value = 0; filter->value = 0;
filter->initialized = 0; filter->initialized = 0;
filter->state = FILTER_READY;
return 0;
} }
/** /**
@ -297,7 +345,7 @@ void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) {
* @return Отфильтрованное значение * @return Отфильтрованное значение
*/ */
int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input) { int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input) {
if (filter == NULL) return input; check_process_filter(filter);
if (!filter->initialized) { if (!filter->initialized) {
filter->value = input; filter->value = input;
@ -316,16 +364,21 @@ int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input) {
/** /**
* @brief Инициализация фильтра скользящего среднего (int32_t) * @brief Инициализация фильтра скользящего среднего (int32_t)
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
* @return 0 - успех, -1 - ошибка
*/ */
void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size) { int FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size, FilterMode_t mode) {
if (filter == NULL) return; check_init_filter(filter);
if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return; if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return - 1;
memset(filter->buffer, 0, sizeof(filter->buffer)); memset(filter->buffer, 0, sizeof(filter->buffer));
filter->size = size; filter->size = size;
filter->sum = 0; filter->sum = 0;
filter->index = 0; filter->index = 0;
filter->count = 0; filter->count = 0;
filter->mode = mode;
filter->state = FILTER_READY;
return 0;
} }
/** /**
@ -335,21 +388,33 @@ void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size) {
* @return Отфильтрованное значение * @return Отфильтрованное значение
*/ */
int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) { 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 += input;
filter->sum -= filter->buffer[filter->index];
} else {
filter->count++; 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->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->lastValue;
filter->buffer[filter->index] = input;
filter->sum += input;
filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE;
return (int32_t)(filter->sum / filter->count);
} }
/** /**
@ -361,12 +426,14 @@ int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) {
* @return 0 - успех, -1 - ошибка * @return 0 - успех, -1 - ошибка
*/ */
int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, int32_t scale) { int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, int32_t scale) {
if (filter == NULL || coeffs == NULL) return -1; check_init_filter(filter);
if (order > FILTER_POLY_MAX_ORDER) return -1; if ((coeffs == NULL) || (order > FILTER_POLY_MAX_ORDER)) return -1;
filter->order = order; filter->order = order;
filter->scale = scale; filter->scale = scale;
memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(int32_t)); memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(int32_t));
filter->state = FILTER_READY;
return 0; return 0;
} }
@ -377,7 +444,7 @@ int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order,
* @return Скорректированное значение * @return Скорректированное значение
*/ */
int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) { 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[0] = a_n * scale
// coefficients[1] = a_{n-1} * scale // coefficients[1] = a_{n-1} * scale
@ -404,14 +471,19 @@ int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) {
* @param output_arr Массив выходных значений * @param output_arr Массив выходных значений
* @param size Размер таблицы * @param size Размер таблицы
* @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция) * @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) { int 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; check_init_filter(filter);
if ((input_arr == NULL) || (output_arr == NULL)) return -1;
filter->input_values = input_arr; filter->input_values = input_arr;
filter->output_values = output_arr; filter->output_values = output_arr;
filter->size = size; filter->size = size;
filter->interpolation = interpolation; filter->interpolation = interpolation;
filter->state = FILTER_READY;
return 0;
} }
/** /**
@ -421,7 +493,8 @@ void FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* outp
* @return Выходное значение по таблице * @return Выходное значение по таблице
*/ */
int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) { int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) {
if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) { check_process_filter(filter);
if((filter->input_values == NULL) || (filter->output_values == NULL)) {
return input; return input;
} }