321 lines
11 KiB
C
321 lines
11 KiB
C
/**
|
||
******************************************************************************
|
||
* @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
|