From 9b9969dd7ce85e3e317e6fef71c9e1e591d52e46 Mon Sep 17 00:00:00 2001 From: Razvalyaev Date: Tue, 11 Nov 2025 12:45:27 +0300 Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D1=80=D0=B0=D0=B1=D0=BE=D1=82?= =?UTF-8?q?=D0=BA=D0=B8=20=D1=84=D0=B8=D0=BB=D1=8C=D1=82=D1=80=D0=BE=D0=B2?= =?UTF-8?q?:?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - универсализированы проверки состояния инициализации фильтров - avg сделан скользящий и обычный - плюс еще что-то наверное --- MyLibs/Inc/filters.h | 114 ++++---- MyLibs/Src/filters.c | 605 ++++++++++++++++++++++++------------------- 2 files changed, 399 insertions(+), 320 deletions(-) diff --git a/MyLibs/Inc/filters.h b/MyLibs/Inc/filters.h index 4f23423..a0801d9 100644 --- a/MyLibs/Inc/filters.h +++ b/MyLibs/Inc/filters.h @@ -11,11 +11,12 @@ - Экспоненциальное скользящее среднее (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_MEDIAN_MAX_SIZE - Размер окна медианного фильтра (по умолчанию 5) +- @ref FILTER_AVERAGE_MAX_SIZE - Размер окна усредняющего фильтра (по умолчанию 8) - @ref FILTER_POLY_MAX_ORDER - Максимальный порядок полинома (по умолчанию 4) @par Пример использования: @@ -84,32 +85,65 @@ int32_t process_value_int(int32_t raw_adc_quant) { #ifdef FILTERS_ENABLE #ifndef FILTER_AVERAGE_MAX_SIZE -#define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна медианного фильтра +#define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна усредняющего фильтра #endif #ifndef FILTER_MEDIAN_MAX_SIZE -#define FILTER_MEDIAN_MAX_SIZE 8 ///< Размер окна усредняющего фильтра +#define FILTER_MEDIAN_MAX_SIZE 10 ///< Размер окна медианного фильтра #endif #ifndef FILTER_POLY_MAX_ORDER #define FILTER_POLY_MAX_ORDER 4 ///< Максимальный порядок полинома #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 { - FILTER_DISABLE, + 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 { - float buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений - uint8_t index; ///< Текущий индекс - uint8_t size; ///< Фактический размер фильтра + FilterState_t state; ///< Состояние фильтра + float buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений + uint8_t index; ///< Текущий индекс + uint8_t size; ///< Фактический размер фильтра } FilterMedian_t; /** @@ -127,19 +161,22 @@ typedef struct { */ typedef struct { FilterState_t state; ///< Состояние фильтра + FilterMode_t mode; ///< Режим фильтра float buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений uint8_t size; ///< Фактический размер фильтра float sum; ///< Сумма значений uint8_t index; ///< Текущий индекс uint8_t count; ///< Количество элементов + float lastValue; ///< Последнее измеренное значение } FilterAverage_t; /** * @brief Структура полиномиальной коррекции (float) */ typedef struct { - float coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома - uint8_t order; ///< Порядок полинома + FilterState_t state; ///< Состояние фильтра + float coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома + uint8_t order; ///< Порядок полинома } FilterPoly_t; /** @@ -154,15 +191,15 @@ typedef struct { } FilterLUT_t; // 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); -void FilterExp_Init(FilterExp_t* filter, float alpha); +int FilterExp_Init(FilterExp_t* filter, float alpha); 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); int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order); 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); // ==================== INT32_T ВЕРСИИ ==================== @@ -191,12 +228,14 @@ typedef struct { * @brief Структура фильтра скользящего среднего (int32_t) */ typedef struct { - FilterState_t state; ///< Состояние фильтра + FilterState_t state; ///< Состояние фильтра + FilterMode_t mode; ///< Режим фильтра int32_t buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений uint8_t size; ///< Фактический размер фильтра int64_t sum; ///< Сумма значений uint8_t index; ///< Текущий индекс uint8_t count; ///< Количество элементов + uint32_t lastValue; ///< Последнее измеренное значение } FilterAverageInt_t; @@ -223,51 +262,18 @@ typedef struct { } FilterLUTInt_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); -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); -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); 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); -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); #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 diff --git a/MyLibs/Src/filters.c b/MyLibs/Src/filters.c index 2a3ac66..e05d7a3 100644 --- a/MyLibs/Src/filters.c +++ b/MyLibs/Src/filters.c @@ -8,6 +8,14 @@ #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 @@ -22,15 +30,18 @@ static int Filter_float_compare(const void *a, const void *b) { /** * @brief Инициализация медианного фильтра (float) * @param filter Указатель на структуру фильтра + * @return 0 - успех, -1 - ошибка */ -void FilterMedian_Init(FilterMedian_t* filter, uint8_t size) { - if (filter == NULL) return; - if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return; - - memset(filter->buffer, 0, sizeof(filter->buffer)); - filter->index = 0; - filter->size = size; - +int FilterMedian_Init(FilterMedian_t* filter, uint8_t size) { + check_init_filter(filter); + if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1; + + memset(filter->buffer, 0, sizeof(filter->buffer)); + filter->index = 0; + filter->size = size; + + filter->state = FILTER_READY; + return 0; } /** @@ -40,32 +51,36 @@ void FilterMedian_Init(FilterMedian_t* filter, uint8_t size) { * @return Отфильтрованное значение */ float FilterMedian_Process(FilterMedian_t* filter, float input) { - if (filter == NULL) return input; - - // Добавляем значение в буфер - filter->buffer[filter->index] = input; - filter->index = (filter->index + 1) % filter->size; - - // Копируем буфер для сортировки - float sort_buffer[FILTER_MEDIAN_MAX_SIZE]; - memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer)); - - // Сортируем и возвращаем медиану - qsort(sort_buffer, filter->size, sizeof(float), Filter_float_compare); - return sort_buffer[filter->size / 2]; + check_process_filter(filter); + + // Добавляем значение в буфер + filter->buffer[filter->index] = input; + filter->index = (filter->index + 1) % filter->size; + + // Копируем буфер для сортировки + float sort_buffer[FILTER_MEDIAN_MAX_SIZE]; + memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer)); + + // Сортируем и возвращаем медиану + qsort(sort_buffer, filter->size, sizeof(float), Filter_float_compare); + return sort_buffer[filter->size / 2]; } /** * @brief Инициализация экспоненциального фильтра (float) * @param filter Указатель на структуру фильтра * @param alpha Коэффициент сглаживания (0..1) + * @return 0 - успех, -1 - ошибка */ -void FilterExp_Init(FilterExp_t* filter, float alpha) { - if (filter == NULL) return; - - filter->alpha = alpha; - filter->value = 0.0f; - filter->initialized = 0; +int FilterExp_Init(FilterExp_t* filter, float alpha) { + check_init_filter(filter); + + filter->alpha = alpha; + filter->value = 0.0f; + filter->initialized = 0; + + filter->state = FILTER_READY; + return 0; } /** @@ -75,31 +90,36 @@ void FilterExp_Init(FilterExp_t* filter, float alpha) { * @return Отфильтрованное значение */ float FilterExp_Process(FilterExp_t* filter, float input) { - if (filter == NULL) return input; - - if (!filter->initialized) { - filter->value = input; - filter->initialized = 1; - return input; - } - - filter->value = filter->alpha * input + (1.0f - filter->alpha) * filter->value; - return filter->value; + check_process_filter(filter); + + if (!filter->initialized) { + filter->value = input; + filter->initialized = 1; + return input; + } + + filter->value = filter->alpha * input + (1.0f - filter->alpha) * filter->value; + return filter->value; } /** * @brief Инициализация фильтра скользящего среднего (float) * @param filter Указатель на структуру фильтра + * @return 0 - успех, -1 - ошибка */ -void FilterAverage_Init(FilterAverage_t* filter, uint8_t size) { - if (filter == NULL) return; - if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return; - - memset(filter->buffer, 0, sizeof(filter->buffer)); - filter->size = size; - filter->sum = 0.0f; - filter->index = 0; - filter->count = 0; +int FilterAverage_Init(FilterAverage_t* filter, uint8_t size, FilterMode_t mode) { + check_init_filter(filter); + if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return -1; + + memset(filter->buffer, 0, sizeof(filter->buffer)); + filter->size = size; + filter->sum = 0.0f; + filter->index = 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 Отфильтрованное значение */ float FilterAverage_Process(FilterAverage_t* filter, float input) { - if (filter == NULL) return input; - - // Вычитаем старое значение из суммы - if (filter->count == FILTER_AVERAGE_MAX_SIZE) { - filter->sum -= filter->buffer[filter->index]; - } else { - filter->count++; + check_process_filter(filter); + + // Общая логика для обоих режимов + filter->sum += input; + 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->sum += input; - filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE; - - return filter->sum / filter->count; + 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; } /** @@ -134,12 +166,14 @@ float FilterAverage_Process(FilterAverage_t* filter, float input) { * @return 0 - успех, -1 - ошибка */ int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) { - if (filter == NULL || coeffs == NULL) return -1; - if (order > FILTER_POLY_MAX_ORDER) return -1; - - filter->order = order; - memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(float)); - return 0; + check_init_filter(filter); + if ((coeffs == NULL) || (order > FILTER_POLY_MAX_ORDER)) return -1; + + filter->order = order; + memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(float)); + + filter->state = FILTER_READY; + return 0; } /** @@ -149,17 +183,17 @@ int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) { * @return Скорректированное значение */ float FilterPoly_Process(FilterPoly_t* filter, float input) { - if (filter == NULL) return input; - - float result = 0.0f; - float x_power = 1.0f; - - for (uint8_t i = 0; i <= filter->order; i++) { - result += filter->coefficients[i] * x_power; - x_power *= input; - } - - return result; + check_process_filter(filter); + + float result = 0.0f; + float x_power = 1.0f; + + for (uint8_t i = 0; i <= filter->order; i++) { + result += filter->coefficients[i] * x_power; + x_power *= input; + } + + return result; } /** @@ -169,14 +203,19 @@ float FilterPoly_Process(FilterPoly_t* filter, float input) { * @param output_arr Массив выходных значений * @param size Размер таблицы * @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) { - if (filter == NULL || input_arr == NULL || output_arr == NULL) return; - - filter->input_values = input_arr; - filter->output_values = output_arr; - filter->size = size; - filter->interpolation = interpolation; +int FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, uint16_t size, uint8_t interpolation) { + check_init_filter(filter); + if ((input_arr == NULL) || (output_arr == NULL)) return -1; + + filter->input_values = input_arr; + filter->output_values = output_arr; + filter->size = size; + filter->interpolation = interpolation; + + filter->state = FILTER_READY; + return 0; } /** @@ -186,71 +225,76 @@ void FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, ui * @return Выходное значение по таблице */ float FilterLUT_Process(FilterLUT_t* filter, float input) { - if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) { - return input; + check_process_filter(filter); + if((filter->input_values == NULL) || (filter->output_values == NULL)) { + return input; + } + + // Поиск ближайших значений в таблице + uint16_t left_index = 0; + uint16_t right_index = filter->size - 1; + + // Если значение за пределами таблицы - возвращаем крайние значения + if (input <= filter->input_values[0]) { + return filter->output_values[0]; + } + if (input >= filter->input_values[right_index]) { + return filter->output_values[right_index]; + } + + // Бинарный поиск позиции + while (right_index - left_index > 1) { + uint16_t mid_index = left_index + (right_index - left_index) / 2; + if (input <= filter->input_values[mid_index]) { + right_index = mid_index; + } else { + left_index = mid_index; } - - // Поиск ближайших значений в таблице - uint16_t left_index = 0; - uint16_t right_index = filter->size - 1; - - // Если значение за пределами таблицы - возвращаем крайние значения - if (input <= filter->input_values[0]) { - return filter->output_values[0]; - } - if (input >= filter->input_values[right_index]) { - return filter->output_values[right_index]; - } - - // Бинарный поиск позиции - while (right_index - left_index > 1) { - uint16_t mid_index = left_index + (right_index - left_index) / 2; - if (input <= filter->input_values[mid_index]) { - right_index = mid_index; - } else { - left_index = mid_index; - } - } - - // Без интерполяции - возвращаем значение левой границы - if (!filter->interpolation) { - return filter->output_values[left_index]; - } - - // Линейная интерполяция - float x0 = filter->input_values[left_index]; - float x1 = filter->input_values[right_index]; - float y0 = filter->output_values[left_index]; - float y1 = filter->output_values[right_index]; - - if (x1 == x0) { - return y0; // Избегаем деления на ноль - } - - return y0 + (input - x0) * (y1 - y0) / (x1 - x0); + } + + // Без интерполяции - возвращаем значение левой границы + if (!filter->interpolation) { + return filter->output_values[left_index]; + } + + // Линейная интерполяция + float x0 = filter->input_values[left_index]; + float x1 = filter->input_values[right_index]; + float y0 = filter->output_values[left_index]; + float y1 = filter->output_values[right_index]; + + if (x1 == x0) { + return y0; // Избегаем деления на ноль + } + + return y0 + (input - x0) * (y1 - y0) / (x1 - x0); } // ==================== INT32_T ВЕРСИИ ==================== // Вспомогательная функция для сравнения int32_t static int Filter_int32_compare(const void *a, const void *b) { - int32_t ia = *(const int32_t*)a; - int32_t ib = *(const int32_t*)b; - if (ia < ib) return -1; - if (ia > ib) return 1; - return 0; + int32_t ia = *(const int32_t*)a; + int32_t ib = *(const int32_t*)b; + if (ia < ib) return -1; + if (ia > ib) return 1; + return 0; } /** * @brief Инициализация медианного фильтра (int32_t) * @param filter Указатель на структуру фильтра + * @return 0 - успех, -1 - ошибка */ -void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) { - if (filter == NULL) return; - if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return; - - memset(filter->buffer, 0, sizeof(filter->buffer)); - filter->index = 0; - filter->size = size; +int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) { + check_init_filter(filter); + if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1; + + memset(filter->buffer, 0, sizeof(filter->buffer)); + filter->index = 0; + filter->size = size; + + filter->state = FILTER_READY; + return 0; } /** @@ -260,19 +304,19 @@ void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) { * @return Отфильтрованное значение */ int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) { - if (filter == NULL) return input; - - // Добавляем значение в буфер - filter->buffer[filter->index] = input; - filter->index = (filter->index + 1) % filter->size; - - // Копируем буфер для сортировки - int32_t sort_buffer[FILTER_MEDIAN_MAX_SIZE]; - memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer)); - - // Сортируем и возвращаем медиану - qsort(sort_buffer, filter->size, sizeof(int32_t), Filter_int32_compare); - return sort_buffer[filter->size / 2]; + check_process_filter(filter); + + // Добавляем значение в буфер + filter->buffer[filter->index] = input; + filter->index = (filter->index + 1) % filter->size; + + // Копируем буфер для сортировки + int32_t sort_buffer[FILTER_MEDIAN_MAX_SIZE]; + memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer)); + + // Сортируем и возвращаем медиану + qsort(sort_buffer, filter->size, sizeof(int32_t), Filter_int32_compare); + return sort_buffer[filter->size / 2]; } /** @@ -280,14 +324,18 @@ int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) { * @param filter Указатель на структуру фильтра * @param alpha Коэффициент сглаживания в масштабированном виде * @param scale Масштаб коэффициента (например 100 для работы с процентами) + * @return 0 - успех, -1 - ошибка */ -void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) { - if (filter == NULL) return; - - filter->alpha = alpha; - filter->scale = scale; - filter->value = 0; - filter->initialized = 0; +int FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) { + check_init_filter(filter); + + filter->alpha = alpha; + filter->scale = scale; + filter->value = 0; + filter->initialized = 0; + + filter->state = FILTER_READY; + return 0; } /** @@ -297,35 +345,40 @@ void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) { * @return Отфильтрованное значение */ int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input) { - if (filter == NULL) return input; - - if (!filter->initialized) { - filter->value = input; - filter->initialized = 1; - return input; - } - - // value = (alpha * input + (scale - alpha) * value) / scale - int64_t result = (int64_t)filter->alpha * input + - (int64_t)(filter->scale - filter->alpha) * filter->value; - filter->value = (int32_t)(result / filter->scale); - - return filter->value; + check_process_filter(filter); + + if (!filter->initialized) { + filter->value = input; + filter->initialized = 1; + return input; + } + + // value = (alpha * input + (scale - alpha) * value) / scale + int64_t result = (int64_t)filter->alpha * input + + (int64_t)(filter->scale - filter->alpha) * filter->value; + filter->value = (int32_t)(result / filter->scale); + + return filter->value; } /** * @brief Инициализация фильтра скользящего среднего (int32_t) * @param filter Указатель на структуру фильтра + * @return 0 - успех, -1 - ошибка */ -void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size) { - if (filter == NULL) return; - if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return; - - memset(filter->buffer, 0, sizeof(filter->buffer)); - filter->size = size; - filter->sum = 0; - filter->index = 0; - filter->count = 0; +int FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size, FilterMode_t mode) { + check_init_filter(filter); + if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return - 1; + + memset(filter->buffer, 0, sizeof(filter->buffer)); + filter->size = size; + filter->sum = 0; + filter->index = 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 Отфильтрованное значение */ int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) { - if (filter == NULL) return input; - - // Вычитаем старое значение из суммы - if (filter->count == FILTER_AVERAGE_MAX_SIZE) { - filter->sum -= filter->buffer[filter->index]; - } else { - filter->count++; + check_process_filter(filter); + + // Общая логика для обоих режимов + filter->sum += input; + 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->sum += input; - filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE; - - return (int32_t)(filter->sum / filter->count); + 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; } /** @@ -361,13 +426,15 @@ int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) { * @return 0 - успех, -1 - ошибка */ int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, int32_t scale) { - if (filter == NULL || coeffs == NULL) return -1; - if (order > FILTER_POLY_MAX_ORDER) return -1; - - filter->order = order; - filter->scale = scale; - memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(int32_t)); - return 0; + check_init_filter(filter); + if ((coeffs == NULL) || (order > FILTER_POLY_MAX_ORDER)) return -1; + + filter->order = order; + filter->scale = scale; + memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(int32_t)); + + filter->state = FILTER_READY; + return 0; } /** @@ -377,24 +444,24 @@ int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, * @return Скорректированное значение */ int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) { - if (filter == NULL) return input; - - // coefficients[0] = a_n * scale - // coefficients[1] = a_{n-1} * scale - // ... - // coefficients[n] = a_0 * scale - - int64_t result = filter->coefficients[0]; // Старший коэффициент - int64_t x_scaled = input; - - for (uint8_t i = 1; i <= filter->order; i++) { - result = (result * x_scaled) / filter->scale + filter->coefficients[i]; - } - - // Домножаем на scale для a_0 - result = (result * filter->scale); - - return (int32_t)(result / filter->scale); + check_process_filter(filter); + + // coefficients[0] = a_n * scale + // coefficients[1] = a_{n-1} * scale + // ... + // coefficients[n] = a_0 * scale + + int64_t result = filter->coefficients[0]; // Старший коэффициент + int64_t x_scaled = input; + + for (uint8_t i = 1; i <= filter->order; i++) { + result = (result * x_scaled) / filter->scale + filter->coefficients[i]; + } + + // Домножаем на scale для a_0 + result = (result * filter->scale); + + return (int32_t)(result / filter->scale); } /** @@ -404,14 +471,19 @@ int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) { * @param output_arr Массив выходных значений * @param size Размер таблицы * @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) { - if (filter == NULL || input_arr == NULL || output_arr == NULL) return; - - filter->input_values = input_arr; - filter->output_values = output_arr; - filter->size = size; - filter->interpolation = interpolation; +int FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* output_arr, uint16_t size, uint8_t interpolation) { + check_init_filter(filter); + if ((input_arr == NULL) || (output_arr == NULL)) return -1; + + filter->input_values = input_arr; + filter->output_values = output_arr; + filter->size = size; + filter->interpolation = interpolation; + + filter->state = FILTER_READY; + return 0; } /** @@ -421,48 +493,49 @@ void FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* outp * @return Выходное значение по таблице */ int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) { - if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) { - return input; + check_process_filter(filter); + if((filter->input_values == NULL) || (filter->output_values == NULL)) { + return input; + } + + // Поиск ближайших значений в таблице + uint16_t left_index = 0; + uint16_t right_index = filter->size - 1; + + // Если значение за пределами таблицы - возвращаем крайние значения + if (input <= filter->input_values[0]) { + return filter->output_values[0]; + } + if (input >= filter->input_values[right_index]) { + return filter->output_values[right_index]; + } + + // Бинарный поиск позиции + while (right_index - left_index > 1) { + uint16_t mid_index = left_index + (right_index - left_index) / 2; + if (input <= filter->input_values[mid_index]) { + right_index = mid_index; + } else { + left_index = mid_index; } - - // Поиск ближайших значений в таблице - uint16_t left_index = 0; - uint16_t right_index = filter->size - 1; - - // Если значение за пределами таблицы - возвращаем крайние значения - if (input <= filter->input_values[0]) { - return filter->output_values[0]; - } - if (input >= filter->input_values[right_index]) { - return filter->output_values[right_index]; - } - - // Бинарный поиск позиции - while (right_index - left_index > 1) { - uint16_t mid_index = left_index + (right_index - left_index) / 2; - if (input <= filter->input_values[mid_index]) { - right_index = mid_index; - } else { - left_index = mid_index; - } - } - - // Без интерполяции - возвращаем значение левой границы - if (!filter->interpolation) { - return filter->output_values[left_index]; - } - - // Линейная интерполяция (целочисленная) - int64_t x0 = filter->input_values[left_index]; - int64_t x1 = filter->input_values[right_index]; - int64_t y0 = filter->output_values[left_index]; - int64_t y1 = filter->output_values[right_index]; - - if (x1 == x0) { - return (int32_t)y0; // Избегаем деления на ноль - } - - int64_t result = y0 + (input - x0) * (y1 - y0) / (x1 - x0); - return (int32_t)result; + } + + // Без интерполяции - возвращаем значение левой границы + if (!filter->interpolation) { + return filter->output_values[left_index]; + } + + // Линейная интерполяция (целочисленная) + int64_t x0 = filter->input_values[left_index]; + int64_t x1 = filter->input_values[right_index]; + int64_t y0 = filter->output_values[left_index]; + int64_t y1 = filter->output_values[right_index]; + + if (x1 == x0) { + return (int32_t)y0; // Избегаем деления на ноль + } + + int64_t result = y0 + (input - x0) * (y1 - y0) / (x1 - x0); + return (int32_t)result; } #endif // FILTERS_ENABLE