Razvalyaev/STM32_ExtendedLibs
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 2 Wiki Activity

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

Browse Source 
- в целом улучшена логика хендлов
- добавлена фильтрация по таблице
- добавлен флаг включенности (в либе пока не используется)
This commit is contained in:
Razvalyaev 2025-11-09 23:33:11 +03:00
parent 60629aaa3b
commit 3b162c9f8c
2 changed files with 252 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
MyLibs/Inc/filters.h
View File

@ -14,8 +14,8 @@
Параметры для конфигурации: Параметры для конфигурации:
- @ref FILTERS_ENABLE - Включить библиотеку фильтров - @ref FILTERS_ENABLE - Включить библиотеку фильтров
- @ref FILTER_MEDIAN_SIZE - Размер окна медианного фильтра (по умолчанию 5) - @ref FILTER_MEDIAN_MAX_SIZE - Размер окна медианного фильтра (по умолчанию 5)
- @ref FILTER_AVERAGE_SIZE - Размер окна усредняющего фильтра (по умолчанию 8) - @ref FILTER_AVERAGE_MAX_SIZE - Размер окна усредняющего фильтра (по умолчанию 8)
- @ref FILTER_POLY_MAX_ORDER - Максимальный порядок полинома (по умолчанию 4) - @ref FILTER_POLY_MAX_ORDER - Максимальный порядок полинома (по умолчанию 4)
@par Пример использования: @par Пример использования:
@ -76,40 +76,47 @@ int32_t process_value_int(int32_t raw_adc_quant) {
*****************************************************************************/ *****************************************************************************/
#ifndef __FILTERS_H_ #ifndef __FILTERS_H_
#define __FILTERS_H_ #define __FILTERS_H_
#include "mylibs_defs.h"
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#ifdef FILTERS_ENABLE #ifdef FILTERS_ENABLE
#ifndef FILTER_MEDIAN_SIZE #ifndef FILTER_AVERAGE_MAX_SIZE
#define FILTER_MEDIAN_SIZE 5 ///< Размер окна медианного фильтра #define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна медианного фильтра
#endif #endif
#ifndef FILTER_AVERAGE_SIZE #ifndef FILTER_MEDIAN_MAX_SIZE
#define FILTER_AVERAGE_SIZE 8 ///< Размер окна усредняющего фильтра #define FILTER_MEDIAN_MAX_SIZE 8 ///< Размер окна усредняющего фильтра
#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
// ==================== FLOAT ВЕРСИИ ==================== #define FILTER_GET_STATE(_fltr_) (_fltr_)->state
// ==================== FLOAT ВЕРСИИ ====================
typedef enum
{
FILTER_DISABLE,
FILTER_ENABLE
}FilterState_t;
/** /**
* @brief Структура медианного фильтра (float) * @brief Структура медианного фильтра (float)
*/ */
typedef struct { typedef struct {
float buffer[FILTER_MEDIAN_SIZE]; ///< Буфер значений float buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений
uint8_t index; ///< Текущий индекс uint8_t index; ///< Текущий индекс
uint8_t size; ///< Размер буфера uint8_t size; ///< Фактический размер фильтра
} FilterMedian_t; } FilterMedian_t;
/** /**
* @brief Структура экспоненциального фильтра (float) * @brief Структура экспоненциального фильтра (float)
*/ */
typedef struct { typedef struct {
FilterState_t state; ///< Состояние фильтра
float alpha; ///< Коэффициент сглаживания (0..1) float alpha; ///< Коэффициент сглаживания (0..1)
float value; ///< Текущее значение float value; ///< Текущее значение
uint8_t initialized; ///< Флаг инициализации uint8_t initialized; ///< Флаг инициализации
@ -119,7 +126,9 @@ typedef struct {
* @brief Структура фильтра скользящего среднего (float) * @brief Структура фильтра скользящего среднего (float)
*/ */
typedef struct { typedef struct {
float buffer[FILTER_AVERAGE_SIZE]; ///< Буфер значений FilterState_t state; ///< Состояние фильтра
float buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений
uint8_t size; ///< Фактический размер фильтра
float sum; ///< Сумма значений float sum; ///< Сумма значений
uint8_t index; ///< Текущий индекс uint8_t index; ///< Текущий индекс
uint8_t count; ///< Количество элементов uint8_t count; ///< Количество элементов
@ -133,31 +142,45 @@ typedef struct {
uint8_t order; ///< Порядок полинома uint8_t order; ///< Порядок полинома
} FilterPoly_t; } 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 версии функций // Float версии функций
void FilterMedian_Init(FilterMedian_t* filter); void 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); void 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); void FilterAverage_Init(FilterAverage_t* filter, uint8_t size);
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);
float FilterLUT_Process(FilterLUT_t* filter, float input);
// ==================== INT32_T ВЕРСИИ ==================== // ==================== INT32_T ВЕРСИИ ====================
/** /**
* @brief Структура медианного фильтра (int32_t) * @brief Структура медианного фильтра (int32_t)
*/ */
typedef struct { typedef struct {
int32_t buffer[FILTER_MEDIAN_SIZE]; ///< Буфер значений FilterState_t state; ///< Состояние фильтра
int32_t buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений
uint8_t index; ///< Текущий индекс uint8_t index; ///< Текущий индекс
uint8_t size; ///< Размер буфера uint8_t size; ///< Фактический размер фильтра
} FilterMedianInt_t; } FilterMedianInt_t;
/** /**
* @brief Структура экспоненциального фильтра (int32_t) * @brief Структура экспоненциального фильтра (int32_t)
*/ */
typedef struct { typedef struct {
FilterState_t state; ///< Состояние фильтра
int32_t alpha; ///< Коэффициент сглаживания (в масштабе scale) int32_t alpha; ///< Коэффициент сглаживания (в масштабе scale)
int32_t value; ///< Текущее значение int32_t value; ///< Текущее значение
uint8_t initialized; ///< Флаг инициализации uint8_t initialized; ///< Флаг инициализации
@ -168,30 +191,48 @@ typedef struct {
* @brief Структура фильтра скользящего среднего (int32_t) * @brief Структура фильтра скользящего среднего (int32_t)
*/ */
typedef struct { typedef struct {
int32_t buffer[FILTER_AVERAGE_SIZE]; ///< Буфер значений FilterState_t state; ///< Состояние фильтра
int32_t buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений
uint8_t size; ///< Фактический размер фильтра
int64_t sum; ///< Сумма значений int64_t sum; ///< Сумма значений
uint8_t index; ///< Текущий индекс uint8_t index; ///< Текущий индекс
uint8_t count; ///< Количество элементов uint8_t count; ///< Количество элементов
} FilterAverageInt_t; } FilterAverageInt_t;
/** /**
* @brief Структура полиномиальной коррекции (int32_t) * @brief Структура полиномиальной коррекции (int32_t)
*/ */
typedef struct { typedef struct {
FilterState_t state; ///< Состояние фильтра
int32_t coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома int32_t coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома
uint8_t order; ///< Порядок полинома uint8_t order; ///< Порядок полинома
int32_t scale; ///< Масштаб коэффициентов int32_t scale; ///< Масштаб коэффициентов
} FilterPolyInt_t; } 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 версии функций // 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); int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input);
void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale); void 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); void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size);
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);
int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input);
#else // FILTERS_ENABLE #else // FILTERS_ENABLE
// Заглушки для float // Заглушки для float
@ -200,14 +241,16 @@ typedef struct { uint8_t dummy; } FilterExp_t;
typedef struct { uint8_t dummy; } FilterAverage_t; typedef struct { uint8_t dummy; } FilterAverage_t;
typedef struct { uint8_t dummy; } FilterPoly_t; typedef struct { uint8_t dummy; } FilterPoly_t;
#define FilterMedian_Init(filter) #define FilterMedian_Init(filter, size)
#define FilterMedian_Process(filter, input) (input) #define FilterMedian_Process(filter, input) (input)
#define FilterExp_Init(filter, alpha) #define FilterExp_Init(filter, alpha)
#define FilterExp_Process(filter, input) (input) #define FilterExp_Process(filter, input) (input)
#define FilterAverage_Init(filter) #define FilterAverage_Init(filter, size)
#define FilterAverage_Process(filter, input) (input) #define FilterAverage_Process(filter, input) (input)
#define FilterPoly_Init(filter, coeffs, order) (0) #define FilterPoly_Init(filter, coeffs, order) (0)
#define FilterPoly_Process(filter, input) (input) #define FilterPoly_Process(filter, input) (input)
#define FilterLUT_Init(filter, coeffs, order) (0)
#define FilterLUT_Process(filter, input) (input)
// Заглушки для int32_t // Заглушки для int32_t
typedef struct { uint8_t dummy; } FilterMedianInt_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; } FilterAverageInt_t;
typedef struct { uint8_t dummy; } FilterPolyInt_t; typedef struct { uint8_t dummy; } FilterPolyInt_t;
#define FilterMedianInt_Init(filter) #define FilterMedianInt_Init(filter, size)
#define FilterMedianInt_Process(filter, input) (input) #define FilterMedianInt_Process(filter, input) (input)
#define FilterExpInt_Init(filter, alpha, scale) #define FilterExpInt_Init(filter, alpha, scale)
#define FilterExpInt_Process(filter, input) (input) #define FilterExpInt_Process(filter, input) (input)
#define FilterAverageInt_Init(filter) #define FilterAverageInt_Init(filter, size)
#define FilterAverageInt_Process(filter, input) (input) #define FilterAverageInt_Process(filter, input) (input)
#define FilterPolyInt_Init(filter, coeffs, order, scale) (0) #define FilterPolyInt_Init(filter, coeffs, order, scale) (0)
#define FilterPolyInt_Process(filter, input) (input) #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

184
MyLibs/Src/filters.c
View File

@ -4,7 +4,6 @@
* @brief Реализация библиотеки фильтров * @brief Реализация библиотеки фильтров
****************************************************************************** ******************************************************************************
*/ */
#include "filters.h" #include "filters.h"
#ifdef FILTERS_ENABLE #ifdef FILTERS_ENABLE
@ -24,12 +23,14 @@ static int Filter_float_compare(const void *a, const void *b) {
* @brief Инициализация медианного фильтра (float) * @brief Инициализация медианного фильтра (float)
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
*/ */
void FilterMedian_Init(FilterMedian_t* filter) { void FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
if (filter == NULL) return; if (filter == NULL) return;
if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return;
memset(filter->buffer, 0, sizeof(filter->buffer)); memset(filter->buffer, 0, sizeof(filter->buffer));
filter->index = 0; 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; 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)); memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer));
// Сортируем и возвращаем медиану // Сортируем и возвращаем медиану
@ -90,10 +91,12 @@ float FilterExp_Process(FilterExp_t* filter, float input) {
* @brief Инициализация фильтра скользящего среднего (float) * @brief Инициализация фильтра скользящего среднего (float)
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
*/ */
void FilterAverage_Init(FilterAverage_t* filter) { void FilterAverage_Init(FilterAverage_t* filter, uint8_t size) {
if (filter == NULL) return; if (filter == NULL) return;
if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return;
memset(filter->buffer, 0, sizeof(filter->buffer)); memset(filter->buffer, 0, sizeof(filter->buffer));
filter->size = size;
filter->sum = 0.0f; filter->sum = 0.0f;
filter->index = 0; filter->index = 0;
filter->count = 0; filter->count = 0;
@ -109,7 +112,7 @@ float FilterAverage_Process(FilterAverage_t* filter, float input) {
if (filter == NULL) return 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]; filter->sum -= filter->buffer[filter->index];
} else { } else {
filter->count++; filter->count++;
@ -118,7 +121,7 @@ float FilterAverage_Process(FilterAverage_t* filter, float input) {
// Добавляем новое значение // Добавляем новое значение
filter->buffer[filter->index] = input; filter->buffer[filter->index] = input;
filter->sum += 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; return filter->sum / filter->count;
} }
@ -159,6 +162,73 @@ float FilterPoly_Process(FilterPoly_t* filter, float input) {
return result; 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 ВЕРСИИ ====================
// Вспомогательная функция для сравнения int32_t // Вспомогательная функция для сравнения int32_t
@ -174,12 +244,13 @@ static int Filter_int32_compare(const void *a, const void *b) {
* @brief Инициализация медианного фильтра (int32_t) * @brief Инициализация медианного фильтра (int32_t)
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
*/ */
void FilterMedianInt_Init(FilterMedianInt_t* filter) { void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
if (filter == NULL) return; if (filter == NULL) return;
if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return;
memset(filter->buffer, 0, sizeof(filter->buffer)); memset(filter->buffer, 0, sizeof(filter->buffer));
filter->index = 0; 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; 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)); 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) * @brief Инициализация фильтра скользящего среднего (int32_t)
* @param filter Указатель на структуру фильтра * @param filter Указатель на структуру фильтра
*/ */
void FilterAverageInt_Init(FilterAverageInt_t* filter) { void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size) {
if (filter == NULL) return; if (filter == NULL) return;
if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return;
memset(filter->buffer, 0, sizeof(filter->buffer)); memset(filter->buffer, 0, sizeof(filter->buffer));
filter->size = size;
filter->sum = 0; filter->sum = 0;
filter->index = 0; filter->index = 0;
filter->count = 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 == NULL) return input;
// Вычитаем старое значение из суммы // Вычитаем старое значение из суммы
if (filter->count == FILTER_AVERAGE_SIZE) { if (filter->count == FILTER_AVERAGE_MAX_SIZE) {
filter->sum -= filter->buffer[filter->index]; filter->sum -= filter->buffer[filter->index];
} else { } else {
filter->count++; filter->count++;
@ -274,7 +347,7 @@ int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) {
// Добавляем новое значение // Добавляем новое значение
filter->buffer[filter->index] = input; filter->buffer[filter->index] = input;
filter->sum += 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); 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) { int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) {
if (filter == NULL) return input; if (filter == NULL) return input;
int64_t result = 0; // coefficients[0] = a_n * scale
int64_t x_power = filter->scale; // Начинаем с scale для правильного масштабирования // coefficients[1] = a_{n-1} * scale
// ...
// coefficients[n] = a_0 * scale
for (uint8_t i = 0; i <= filter->order; i++) { int64_t result = filter->coefficients[0]; // Старший коэффициент
result += (int64_t)filter->coefficients[i] * x_power; int64_t x_scaled = input;
x_power = (x_power * input) / filter->scale;
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); 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 #endif // FILTERS_ENABLE