diff --git a/MyLibs/Inc/filters.h b/MyLibs/Inc/filters.h index be94d7b..4f23423 100644 --- a/MyLibs/Inc/filters.h +++ b/MyLibs/Inc/filters.h @@ -14,8 +14,8 @@ Параметры для конфигурации: - @ref FILTERS_ENABLE - Включить библиотеку фильтров -- @ref FILTER_MEDIAN_SIZE - Размер окна медианного фильтра (по умолчанию 5) -- @ref FILTER_AVERAGE_SIZE - Размер окна усредняющего фильтра (по умолчанию 8) +- @ref FILTER_MEDIAN_MAX_SIZE - Размер окна медианного фильтра (по умолчанию 5) +- @ref FILTER_AVERAGE_MAX_SIZE - Размер окна усредняющего фильтра (по умолчанию 8) - @ref FILTER_POLY_MAX_ORDER - Максимальный порядок полинома (по умолчанию 4) @par Пример использования: @@ -76,53 +76,62 @@ int32_t process_value_int(int32_t raw_adc_quant) { *****************************************************************************/ #ifndef __FILTERS_H_ #define __FILTERS_H_ - +#include "mylibs_defs.h" #include #include #include #ifdef FILTERS_ENABLE -#ifndef FILTER_MEDIAN_SIZE -#define FILTER_MEDIAN_SIZE 5 ///< Размер окна медианного фильтра +#ifndef FILTER_AVERAGE_MAX_SIZE +#define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна медианного фильтра #endif -#ifndef FILTER_AVERAGE_SIZE -#define FILTER_AVERAGE_SIZE 8 ///< Размер окна усредняющего фильтра +#ifndef FILTER_MEDIAN_MAX_SIZE +#define FILTER_MEDIAN_MAX_SIZE 8 ///< Размер окна усредняющего фильтра #endif #ifndef FILTER_POLY_MAX_ORDER -#define FILTER_POLY_MAX_ORDER 4 ///< Максимальный порядок полинома +#define FILTER_POLY_MAX_ORDER 4 ///< Максимальный порядок полинома #endif -// ==================== FLOAT ВЕРСИИ ==================== +#define FILTER_GET_STATE(_fltr_) (_fltr_)->state +// ==================== FLOAT ВЕРСИИ ==================== +typedef enum +{ + FILTER_DISABLE, + FILTER_ENABLE +}FilterState_t; /** * @brief Структура медианного фильтра (float) */ typedef struct { - float buffer[FILTER_MEDIAN_SIZE]; ///< Буфер значений + float buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений uint8_t index; ///< Текущий индекс - uint8_t size; ///< Размер буфера + uint8_t size; ///< Фактический размер фильтра } FilterMedian_t; /** * @brief Структура экспоненциального фильтра (float) */ typedef struct { - float alpha; ///< Коэффициент сглаживания (0..1) - float value; ///< Текущее значение - uint8_t initialized; ///< Флаг инициализации + FilterState_t state; ///< Состояние фильтра + 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; ///< Количество элементов + FilterState_t state; ///< Состояние фильтра + float buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений + uint8_t size; ///< Фактический размер фильтра + float sum; ///< Сумма значений + uint8_t index; ///< Текущий индекс + uint8_t count; ///< Количество элементов } FilterAverage_t; /** @@ -133,65 +142,97 @@ typedef struct { uint8_t order; ///< Порядок полинома } FilterPoly_t; +/** + * @brief Структура табличного фильтра (float) + */ +typedef struct { + FilterState_t state; ///< Состояние фильтра + float* input_values; // Массив входных значений + float* output_values; // Массив выходных значений + uint16_t size; // Размер таблицы + uint8_t interpolation; // Флаг интерполяции (0 - отключена, 1 - линейная) +} FilterLUT_t; + // Float версии функций -void FilterMedian_Init(FilterMedian_t* filter); +void FilterMedian_Init(FilterMedian_t* filter, uint8_t size); 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); +void FilterAverage_Init(FilterAverage_t* filter, uint8_t size); 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); +float FilterLUT_Process(FilterLUT_t* filter, float input); // ==================== INT32_T ВЕРСИИ ==================== /** * @brief Структура медианного фильтра (int32_t) */ typedef struct { - int32_t buffer[FILTER_MEDIAN_SIZE]; ///< Буфер значений - uint8_t index; ///< Текущий индекс - uint8_t size; ///< Размер буфера + FilterState_t state; ///< Состояние фильтра + int32_t buffer[FILTER_MEDIAN_MAX_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) + FilterState_t state; ///< Состояние фильтра + 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; ///< Количество элементов + FilterState_t state; ///< Состояние фильтра + int32_t buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений + uint8_t 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; ///< Масштаб коэффициентов + FilterState_t state; ///< Состояние фильтра + int32_t coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома + uint8_t order; ///< Порядок полинома + int32_t scale; ///< Масштаб коэффициентов } FilterPolyInt_t; +/** + * @brief Структура табличного фильтра (int32_t) + */ +typedef struct { + FilterState_t state; ///< Состояние фильтра + int32_t* input_values; // Массив входных значений + int32_t* output_values; // Массив выходных значений + uint16_t size; // Размер таблицы + uint8_t interpolation; // Флаг интерполяции +} FilterLUTInt_t; + // Int32_t версии функций -void FilterMedianInt_Init(FilterMedianInt_t* filter); +void 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); int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input); -void FilterAverageInt_Init(FilterAverageInt_t* filter); +void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size); 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); +int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input); #else // FILTERS_ENABLE // Заглушки для float @@ -200,14 +241,16 @@ 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_Init(filter, size) #define FilterMedian_Process(filter, input) (input) #define FilterExp_Init(filter, alpha) #define FilterExp_Process(filter, input) (input) -#define FilterAverage_Init(filter) +#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; @@ -215,14 +258,16 @@ 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_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) +#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 243ae1c..2a3ac66 100644 --- a/MyLibs/Src/filters.c +++ b/MyLibs/Src/filters.c @@ -4,7 +4,6 @@ * @brief Реализация библиотеки фильтров ****************************************************************************** */ - #include "filters.h" #ifdef FILTERS_ENABLE @@ -24,12 +23,14 @@ static int Filter_float_compare(const void *a, const void *b) { * @brief Инициализация медианного фильтра (float) * @param filter Указатель на структуру фильтра */ -void FilterMedian_Init(FilterMedian_t* filter) { +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 = FILTER_MEDIAN_SIZE; + filter->size = size; + } /** @@ -46,7 +47,7 @@ float FilterMedian_Process(FilterMedian_t* filter, float input) { filter->index = (filter->index + 1) % filter->size; // Копируем буфер для сортировки - float sort_buffer[FILTER_MEDIAN_SIZE]; + float sort_buffer[FILTER_MEDIAN_MAX_SIZE]; memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer)); // Сортируем и возвращаем медиану @@ -90,10 +91,12 @@ float FilterExp_Process(FilterExp_t* filter, float input) { * @brief Инициализация фильтра скользящего среднего (float) * @param filter Указатель на структуру фильтра */ -void FilterAverage_Init(FilterAverage_t* filter) { +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; @@ -109,7 +112,7 @@ float FilterAverage_Process(FilterAverage_t* filter, float input) { if (filter == NULL) return input; // Вычитаем старое значение из суммы - if (filter->count == FILTER_AVERAGE_SIZE) { + if (filter->count == FILTER_AVERAGE_MAX_SIZE) { filter->sum -= filter->buffer[filter->index]; } else { filter->count++; @@ -118,7 +121,7 @@ float FilterAverage_Process(FilterAverage_t* filter, float input) { // Добавляем новое значение filter->buffer[filter->index] = input; filter->sum += input; - filter->index = (filter->index + 1) % FILTER_AVERAGE_SIZE; + filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE; return filter->sum / filter->count; } @@ -159,6 +162,73 @@ float FilterPoly_Process(FilterPoly_t* filter, float input) { return result; } +/** + * @brief Инициализация табличного фильтра (float) + * @param filter Указатель на структуру фильтра + * @param input_arr Массив входных значений (должен быть отсортирован по возрастанию) + * @param output_arr Массив выходных значений + * @param size Размер таблицы + * @param interpolation Флаг интерполяции (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; +} + +/** + * @brief Обработка значения табличным фильтром (float) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Выходное значение по таблице + */ +float FilterLUT_Process(FilterLUT_t* filter, float input) { + if (filter == NULL || 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; + } + } + + // Без интерполяции - возвращаем значение левой границы + 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 @@ -174,12 +244,13 @@ static int Filter_int32_compare(const void *a, const void *b) { * @brief Инициализация медианного фильтра (int32_t) * @param filter Указатель на структуру фильтра */ -void FilterMedianInt_Init(FilterMedianInt_t* filter) { +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 = FILTER_MEDIAN_SIZE; + filter->size = size; } /** @@ -196,7 +267,7 @@ int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) { filter->index = (filter->index + 1) % filter->size; // Копируем буфер для сортировки - int32_t sort_buffer[FILTER_MEDIAN_SIZE]; + int32_t sort_buffer[FILTER_MEDIAN_MAX_SIZE]; memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer)); // Сортируем и возвращаем медиану @@ -246,10 +317,12 @@ int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input) { * @brief Инициализация фильтра скользящего среднего (int32_t) * @param filter Указатель на структуру фильтра */ -void FilterAverageInt_Init(FilterAverageInt_t* filter) { +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; @@ -265,7 +338,7 @@ int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) { if (filter == NULL) return input; // Вычитаем старое значение из суммы - if (filter->count == FILTER_AVERAGE_SIZE) { + if (filter->count == FILTER_AVERAGE_MAX_SIZE) { filter->sum -= filter->buffer[filter->index]; } else { filter->count++; @@ -274,7 +347,7 @@ int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) { // Добавляем новое значение filter->buffer[filter->index] = input; filter->sum += input; - filter->index = (filter->index + 1) % FILTER_AVERAGE_SIZE; + filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE; return (int32_t)(filter->sum / filter->count); } @@ -306,15 +379,90 @@ int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, 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 для правильного масштабирования + // coefficients[0] = a_n * scale + // coefficients[1] = a_{n-1} * scale + // ... + // coefficients[n] = a_0 * 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; + 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); } +/** + * @brief Инициализация табличного фильтра (int32_t) + * @param filter Указатель на структуру фильтра + * @param input_arr Массив входных значений (должен быть отсортирован по возрастанию) + * @param output_arr Массив выходных значений + * @param size Размер таблицы + * @param interpolation Флаг интерполяции (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; +} + +/** + * @brief Обработка значения табличным фильтром (int32_t) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Выходное значение по таблице + */ +int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) { + if (filter == NULL || 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; + } + } + + // Без интерполяции - возвращаем значение левой границы + 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