From 60629aaa3bb3a068cbfe5a93f43f676bd154e85b Mon Sep 17 00:00:00 2001 From: Razvalyaev Date: Sun, 9 Nov 2025 21:03:10 +0300 Subject: [PATCH] =?UTF-8?q?=D0=97=D0=B0=D0=B3=D0=BE=D1=82=D0=BE=D0=B2?= =?UTF-8?q?=D0=BA=D0=B8=20=D0=B4=D0=BB=D1=8F=20=D1=84=D0=B8=D0=BB=D1=8C?= =?UTF-8?q?=D1=82=D1=80=D0=BE=D0=B2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MyLibs/Inc/filters.h | 229 +++++++++++++++++++++++++++++++ MyLibs/Src/filters.c | 320 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 549 insertions(+) create mode 100644 MyLibs/Inc/filters.h create mode 100644 MyLibs/Src/filters.c diff --git a/MyLibs/Inc/filters.h b/MyLibs/Inc/filters.h new file mode 100644 index 0000000..be94d7b --- /dev/null +++ b/MyLibs/Inc/filters.h @@ -0,0 +1,229 @@ +/** +****************************************************************************** +* @file filters.h +* @brief Заголовочный файл библиотеки фильтров +****************************************************************************** +* @addtogroup FILTERS Filters Library +* @brief Библиотека математических фильтров и коррекций +* @details +Поддерживает: +- Медианную фильтрацию (float и int32_t) +- Экспоненциальное скользящее среднее (float и int32_t) +- Скользящее среднее арифметическое (float и int32_t) +- Полиномиальную коррекцию (float и int32_t) + +Параметры для конфигурации: +- @ref FILTERS_ENABLE - Включить библиотеку фильтров +- @ref FILTER_MEDIAN_SIZE - Размер окна медианного фильтра (по умолчанию 5) +- @ref FILTER_AVERAGE_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 +#include +#include + +#ifdef FILTERS_ENABLE + +#ifndef FILTER_MEDIAN_SIZE +#define FILTER_MEDIAN_SIZE 5 ///< Размер окна медианного фильтра +#endif + +#ifndef FILTER_AVERAGE_SIZE +#define FILTER_AVERAGE_SIZE 8 ///< Размер окна усредняющего фильтра +#endif + +#ifndef FILTER_POLY_MAX_ORDER +#define FILTER_POLY_MAX_ORDER 4 ///< Максимальный порядок полинома +#endif + +// ==================== FLOAT ВЕРСИИ ==================== + +/** + * @brief Структура медианного фильтра (float) + */ +typedef struct { + float buffer[FILTER_MEDIAN_SIZE]; ///< Буфер значений + uint8_t index; ///< Текущий индекс + uint8_t size; ///< Размер буфера +} FilterMedian_t; + +/** + * @brief Структура экспоненциального фильтра (float) + */ +typedef struct { + float alpha; ///< Коэффициент сглаживания (0..1) + float value; ///< Текущее значение + uint8_t initialized; ///< Флаг инициализации +} FilterExp_t; + +/** + * @brief Структура фильтра скользящего среднего (float) + */ +typedef struct { + float buffer[FILTER_AVERAGE_SIZE]; ///< Буфер значений + float sum; ///< Сумма значений + uint8_t index; ///< Текущий индекс + uint8_t count; ///< Количество элементов +} FilterAverage_t; + +/** + * @brief Структура полиномиальной коррекции (float) + */ +typedef struct { + float coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома + uint8_t order; ///< Порядок полинома +} FilterPoly_t; + +// Float версии функций +void FilterMedian_Init(FilterMedian_t* filter); +float FilterMedian_Process(FilterMedian_t* filter, float input); +void FilterExp_Init(FilterExp_t* filter, float alpha); +float FilterExp_Process(FilterExp_t* filter, float input); +void FilterAverage_Init(FilterAverage_t* filter); +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); + +// ==================== INT32_T ВЕРСИИ ==================== + +/** + * @brief Структура медианного фильтра (int32_t) + */ +typedef struct { + int32_t buffer[FILTER_MEDIAN_SIZE]; ///< Буфер значений + uint8_t index; ///< Текущий индекс + uint8_t size; ///< Размер буфера +} FilterMedianInt_t; + +/** + * @brief Структура экспоненциального фильтра (int32_t) + */ +typedef struct { + int32_t alpha; ///< Коэффициент сглаживания (в масштабе scale) + int32_t value; ///< Текущее значение + uint8_t initialized; ///< Флаг инициализации + int32_t scale; ///< Масштаб коэффициента (например 100 для 0.01) +} FilterExpInt_t; + +/** + * @brief Структура фильтра скользящего среднего (int32_t) + */ +typedef struct { + int32_t buffer[FILTER_AVERAGE_SIZE]; ///< Буфер значений + int64_t sum; ///< Сумма значений + uint8_t index; ///< Текущий индекс + uint8_t count; ///< Количество элементов +} FilterAverageInt_t; + +/** + * @brief Структура полиномиальной коррекции (int32_t) + */ +typedef struct { + int32_t coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома + uint8_t order; ///< Порядок полинома + int32_t scale; ///< Масштаб коэффициентов +} FilterPolyInt_t; + +// Int32_t версии функций +void FilterMedianInt_Init(FilterMedianInt_t* filter); +int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input); +void 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); +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); + +#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) +#define FilterMedian_Process(filter, input) (input) +#define FilterExp_Init(filter, alpha) +#define FilterExp_Process(filter, input) (input) +#define FilterAverage_Init(filter) +#define FilterAverage_Process(filter, input) (input) +#define FilterPoly_Init(filter, coeffs, order) (0) +#define FilterPoly_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) +#define FilterMedianInt_Process(filter, input) (input) +#define FilterExpInt_Init(filter, alpha, scale) +#define FilterExpInt_Process(filter, input) (input) +#define FilterAverageInt_Init(filter) +#define FilterAverageInt_Process(filter, input) (input) +#define FilterPolyInt_Init(filter, coeffs, order, scale) (0) +#define FilterPolyInt_Process(filter, input) (input) + +#endif // FILTERS_ENABLE + +#endif // __FILTERS_H_ \ No newline at end of file diff --git a/MyLibs/Src/filters.c b/MyLibs/Src/filters.c new file mode 100644 index 0000000..243ae1c --- /dev/null +++ b/MyLibs/Src/filters.c @@ -0,0 +1,320 @@ +/** +****************************************************************************** +* @file filters.c +* @brief Реализация библиотеки фильтров +****************************************************************************** +*/ + +#include "filters.h" + +#ifdef FILTERS_ENABLE + +// ==================== FLOAT ВЕРСИИ ==================== + +// Вспомогательная функция для сравнения float +static int Filter_float_compare(const void *a, const void *b) { + float fa = *(const float*)a; + float fb = *(const float*)b; + if (fa < fb) return -1; + if (fa > fb) return 1; + return 0; +} + +/** + * @brief Инициализация медианного фильтра (float) + * @param filter Указатель на структуру фильтра + */ +void FilterMedian_Init(FilterMedian_t* filter) { + if (filter == NULL) return; + + memset(filter->buffer, 0, sizeof(filter->buffer)); + filter->index = 0; + filter->size = FILTER_MEDIAN_SIZE; +} + +/** + * @brief Обработка значения медианным фильтром (float) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @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_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) + */ +void FilterExp_Init(FilterExp_t* filter, float alpha) { + if (filter == NULL) return; + + filter->alpha = alpha; + filter->value = 0.0f; + filter->initialized = 0; +} + +/** + * @brief Обработка значения экспоненциальным фильтром (float) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @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; +} + +/** + * @brief Инициализация фильтра скользящего среднего (float) + * @param filter Указатель на структуру фильтра + */ +void FilterAverage_Init(FilterAverage_t* filter) { + if (filter == NULL) return; + + memset(filter->buffer, 0, sizeof(filter->buffer)); + filter->sum = 0.0f; + filter->index = 0; + filter->count = 0; +} + +/** + * @brief Обработка значения фильтром скользящего среднего (float) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Отфильтрованное значение + */ +float FilterAverage_Process(FilterAverage_t* filter, float input) { + if (filter == NULL) return input; + + // Вычитаем старое значение из суммы + if (filter->count == FILTER_AVERAGE_SIZE) { + filter->sum -= filter->buffer[filter->index]; + } else { + filter->count++; + } + + // Добавляем новое значение + filter->buffer[filter->index] = input; + filter->sum += input; + filter->index = (filter->index + 1) % FILTER_AVERAGE_SIZE; + + return filter->sum / filter->count; +} + +/** + * @brief Инициализация полиномиальной коррекции (float) + * @param filter Указатель на структуру коррекции + * @param coeffs Массив коэффициентов полинома + * @param order Порядок полинома + * @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; +} + +/** + * @brief Применение полиномиальной коррекции к значению (float) + * @param filter Указатель на структуру коррекции + * @param input Входное значение + * @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; +} + +// ==================== 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; +} + +/** + * @brief Инициализация медианного фильтра (int32_t) + * @param filter Указатель на структуру фильтра + */ +void FilterMedianInt_Init(FilterMedianInt_t* filter) { + if (filter == NULL) return; + + memset(filter->buffer, 0, sizeof(filter->buffer)); + filter->index = 0; + filter->size = FILTER_MEDIAN_SIZE; +} + +/** + * @brief Обработка значения медианным фильтром (int32_t) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @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_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]; +} + +/** + * @brief Инициализация экспоненциального фильтра (int32_t) + * @param filter Указатель на структуру фильтра + * @param alpha Коэффициент сглаживания в масштабированном виде + * @param scale Масштаб коэффициента (например 100 для работы с процентами) + */ +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; +} + +/** + * @brief Обработка значения экспоненциальным фильтром (int32_t) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @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; +} + +/** + * @brief Инициализация фильтра скользящего среднего (int32_t) + * @param filter Указатель на структуру фильтра + */ +void FilterAverageInt_Init(FilterAverageInt_t* filter) { + if (filter == NULL) return; + + memset(filter->buffer, 0, sizeof(filter->buffer)); + filter->sum = 0; + filter->index = 0; + filter->count = 0; +} + +/** + * @brief Обработка значения фильтром скользящего среднего (int32_t) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Отфильтрованное значение + */ +int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) { + if (filter == NULL) return input; + + // Вычитаем старое значение из суммы + if (filter->count == FILTER_AVERAGE_SIZE) { + filter->sum -= filter->buffer[filter->index]; + } else { + filter->count++; + } + + // Добавляем новое значение + filter->buffer[filter->index] = input; + filter->sum += input; + filter->index = (filter->index + 1) % FILTER_AVERAGE_SIZE; + + return (int32_t)(filter->sum / filter->count); +} + +/** + * @brief Инициализация полиномиальной коррекции (int32_t) + * @param filter Указатель на структуру коррекции + * @param coeffs Массив коэффициентов полинома в масштабированном виде + * @param order Порядок полинома + * @param scale Масштаб коэффициентов + * @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; +} + +/** + * @brief Применение полиномиальной коррекции к значению (int32_t) + * @param filter Указатель на структуру коррекции + * @param input Входное значение + * @return Скорректированное значение + */ +int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) { + if (filter == NULL) return input; + + int64_t result = 0; + int64_t x_power = filter->scale; // Начинаем с scale для правильного масштабирования + + for (uint8_t i = 0; i <= filter->order; i++) { + result += (int64_t)filter->coefficients[i] * x_power; + x_power = (x_power * input) / filter->scale; + } + + return (int32_t)(result / filter->scale); +} + +#endif // FILTERS_ENABLE