Заготовки для фильтров

2025-11-09 21:03:10 +03:00 · 2025-11-09 21:03:10 +03:00 · 60629aaa3b
commit 60629aaa3b
parent dd21466ee9
2 changed files with 549 additions and 0 deletions
--- a/MyLibs/Inc/filters.h
+++ 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 <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#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_
--- a/MyLibs/Src/filters.c
+++ 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