966 lines
33 KiB
C
966 lines
33 KiB
C
/**
|
||
******************************************************************************
|
||
* @file filters.c
|
||
* @brief Реализация библиотеки фильтров
|
||
******************************************************************************
|
||
*/
|
||
#include "filters.h"
|
||
|
||
#ifdef FILTERS_ENABLE
|
||
|
||
|
||
#define check_init_filter(_filter_) \
|
||
do{ if (filter == NULL) return -1; \
|
||
filter->state = FILTER_NOT_INIT;}while(0);
|
||
|
||
#define check_process_filter(_filter_) \
|
||
do{ if ((filter == NULL) || (filter->state != FILTER_ENABLE)) return input;}while(0);
|
||
|
||
// ==================== 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 Указатель на структуру фильтра
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterMedian_Init(FilterMedian_t* filter, uint8_t size, float init_val) {
|
||
check_init_filter(filter);
|
||
if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1;
|
||
|
||
for (int i = 0; i < size; i++)
|
||
{
|
||
filter->buffer[i] = init_val;
|
||
}
|
||
filter->index = 0;
|
||
filter->size = size;
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterMedian_Init;
|
||
filter->process = &FilterMedian_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения медианным фильтром (float)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input Входное значение
|
||
* @return Отфильтрованное значение
|
||
*/
|
||
float FilterMedian_Process(FilterMedian_t* filter, float input) {
|
||
check_process_filter(filter);
|
||
|
||
// Добавляем значение в буфер
|
||
filter->buffer[filter->index] = input;
|
||
filter->index = (filter->index + 1) % filter->size;
|
||
|
||
// Копируем буфер для сортировки
|
||
float sort_buffer[FILTER_MEDIAN_MAX_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)
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterExp_Init(FilterExp_t* filter, float alpha) {
|
||
check_init_filter(filter);
|
||
|
||
filter->alpha = alpha;
|
||
filter->value = 0.0f;
|
||
filter->initialized = 0;
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterExp_Init;
|
||
filter->process = &FilterExp_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения экспоненциальным фильтром (float)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input Входное значение
|
||
* @return Отфильтрованное значение
|
||
*/
|
||
float FilterExp_Process(FilterExp_t* filter, float input) {
|
||
check_process_filter(filter);
|
||
|
||
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 Указатель на структуру фильтра
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterAverage_Init(FilterAverage_t* filter, uint32_t size, FilterMode_t mode) {
|
||
check_init_filter(filter);
|
||
if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return -1;
|
||
|
||
#ifndef FILTERS_DISABLE_MOVING_AVERAGE
|
||
memset(filter->buffer, 0, sizeof(filter->buffer));
|
||
#endif
|
||
filter->size = size;
|
||
filter->sum = 0.0f;
|
||
filter->index = 0;
|
||
filter->count = 0;
|
||
filter->mode = mode;
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterAverage_Init;
|
||
filter->process = &FilterAverage_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения фильтром скользящего среднего (float)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input Входное значение
|
||
* @return Отфильтрованное значение
|
||
*/
|
||
float FilterAverage_Process(FilterAverage_t* filter, float input) {
|
||
check_process_filter(filter);
|
||
|
||
// Общая логика для обоих режимов
|
||
filter->sum += input;
|
||
filter->count++;
|
||
filter->dataProcessing = 1;
|
||
// Логика скользящего среднего
|
||
if (filter->mode == FILTER_MODE_MOVING) {
|
||
#ifndef FILTERS_DISABLE_MOVING_AVERAGE
|
||
if (filter->count > filter->size) {
|
||
filter->sum -= filter->buffer[filter->index];
|
||
filter->count = filter->size; // Поддерживаем фиксированный размер окна
|
||
}
|
||
filter->buffer[filter->index] = input;
|
||
filter->index = (filter->index + 1) % filter->size;
|
||
filter->lastValue = filter->sum / filter->count;
|
||
filter->dataProcessing = 0;
|
||
#endif
|
||
}
|
||
else
|
||
{
|
||
if (filter->count >= filter->size)
|
||
{
|
||
filter->lastValue = filter->sum / filter->count;
|
||
filter->count = 0;
|
||
filter->sum = 0;
|
||
filter->dataProcessing = 0;
|
||
}
|
||
}
|
||
|
||
return filter->lastValue;
|
||
}
|
||
|
||
/**
|
||
* @brief Инициализация полиномиальной коррекции (float)
|
||
* @param filter Указатель на структуру коррекции
|
||
* @param coeffs Массив коэффициентов полинома
|
||
* @param order Порядок полинома
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) {
|
||
check_init_filter(filter);
|
||
if ((coeffs == NULL) || (order > FILTER_POLY_MAX_ORDER)) return -1;
|
||
|
||
filter->order = order;
|
||
memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(float));
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterPoly_Init;
|
||
filter->process = &FilterPoly_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Применение полиномиальной коррекции к значению (float)
|
||
* @param filter Указатель на структуру коррекции
|
||
* @param input Входное значение
|
||
* @return Скорректированное значение
|
||
*/
|
||
float FilterPoly_Process(FilterPoly_t* filter, float input) {
|
||
check_process_filter(filter);
|
||
|
||
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;
|
||
}
|
||
|
||
/**
|
||
* @brief Инициализация табличного фильтра (float)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input_arr Массив входных значений (должен быть отсортирован по возрастанию)
|
||
* @param output_arr Массив выходных значений
|
||
* @param size Размер таблицы
|
||
* @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция)
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, uint16_t size, uint8_t interpolation) {
|
||
check_init_filter(filter);
|
||
if ((input_arr == NULL) || (output_arr == NULL)) return -1;
|
||
|
||
filter->input_values = input_arr;
|
||
filter->output_values = output_arr;
|
||
filter->size = size;
|
||
filter->interpolation = interpolation;
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterLUT_Init;
|
||
filter->process = &FilterLUT_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения табличным фильтром (float)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input Входное значение
|
||
* @return Выходное значение по таблице
|
||
*/
|
||
float FilterLUT_Process(FilterLUT_t* filter, float input) {
|
||
check_process_filter(filter);
|
||
if((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);
|
||
}
|
||
|
||
/**
|
||
* @brief Инициализация RMS фильтра (float)
|
||
*/
|
||
int FilterRMS_Init(FilterRMS_t* filter, uint32_t window_size) {
|
||
check_init_filter(filter);
|
||
|
||
if (window_size == 0 || window_size > FILTER_RMS_MAX_SIZE) return -1;
|
||
|
||
filter->window_size = window_size;
|
||
filter->sum_squares = 0.0f;
|
||
filter->last_rms = 0.0f;
|
||
filter->index = 0;
|
||
filter->count = 0;
|
||
|
||
// Инициализируем буфер нулями
|
||
memset(filter->buffer_sq, 0, sizeof(filter->buffer_sq[0]) * window_size);
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterRMS_Init;
|
||
filter->process = &FilterRMS_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения RMS фильтром (float)
|
||
* @details Эффективный алгоритм с циклическим буфером
|
||
*/
|
||
float FilterRMS_Process(FilterRMS_t* filter, float input) {
|
||
check_process_filter(filter);
|
||
|
||
// Вычисляем квадрат входного значения
|
||
float square = input * input;
|
||
|
||
if (filter->count < filter->window_size) {
|
||
// Фаза накопления - просто добавляем в сумму
|
||
filter->sum_squares += square;
|
||
filter->buffer_sq[filter->index] = square;
|
||
filter->count++;
|
||
} else {
|
||
// Фаза скользящего окна - вычитаем старое, добавляем новое
|
||
filter->sum_squares -= filter->buffer_sq[filter->index]; // Вычитаем старое значение
|
||
filter->sum_squares += square; // Добавляем новое
|
||
filter->buffer_sq[filter->index] = square; // Сохраняем в буфер
|
||
}
|
||
|
||
// Увеличиваем индекс с зацикливанием
|
||
filter->index++;
|
||
if (filter->index >= filter->window_size) {
|
||
filter->index = 0;
|
||
}
|
||
|
||
// Вычисляем RMS (проверяем что есть хотя бы одно значение)
|
||
if (filter->count > 0) {
|
||
float mean_square = filter->sum_squares / filter->count;
|
||
// Защита от отрицательных значений из-за ошибок округления
|
||
if (mean_square < 0.0f) mean_square = 0.0f;
|
||
filter->last_rms = _sqrtf(mean_square);
|
||
}
|
||
|
||
return filter->last_rms;
|
||
}
|
||
|
||
|
||
// ==================== 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 Указатель на структуру фильтра
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size, int32_t init_val) {
|
||
check_init_filter(filter);
|
||
if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1;
|
||
|
||
for (int i = 0; i < size; i++)
|
||
{
|
||
filter->buffer[i] = init_val;
|
||
filter->sorted[i] = init_val;
|
||
}
|
||
|
||
filter->index = 0;
|
||
filter->size = size;
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterMedianInt_Init;
|
||
filter->process = &FilterMedianInt_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения медианным фильтром (int32_t)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input Входное значение
|
||
* @return Отфильтрованное значение
|
||
*/
|
||
int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) {
|
||
check_process_filter(filter);
|
||
|
||
register int32_t old_value = filter->buffer[filter->index];
|
||
register uint8_t size = filter->size;
|
||
register uint8_t idx = filter->index;
|
||
int32_t* sorted = filter->sorted;
|
||
|
||
// Обновляем circular buffer
|
||
filter->buffer[idx] = input;
|
||
idx++;
|
||
if (idx >= size) idx = 0;
|
||
filter->index = idx;
|
||
|
||
// Одновременно ищем позицию для удаления и вставки
|
||
uint8_t remove_pos = 0;
|
||
uint8_t insert_pos = 0;
|
||
uint8_t found_remove = 0;
|
||
|
||
// Проход по массиву sorted (только один раз!)
|
||
for (uint8_t i = 0; i < size; i++) {
|
||
int32_t current = sorted[i];
|
||
|
||
// Ищем позицию для удаления старого значения
|
||
if (!found_remove && current == old_value) {
|
||
remove_pos = i;
|
||
found_remove = 1;
|
||
}
|
||
|
||
// Ищем позицию для вставки нового значения
|
||
if (input > current) {
|
||
insert_pos = i + 1;
|
||
}
|
||
}
|
||
|
||
// Если insert_pos указывает на место после удаляемого элемента,
|
||
// нужно скорректировать, так как массив уменьшится на 1 элемент
|
||
if (insert_pos > remove_pos) {
|
||
insert_pos--;
|
||
}
|
||
|
||
// Оптимизированное удаление и вставка за один проход
|
||
if (insert_pos == remove_pos) {
|
||
// Случай 1: Вставляем на то же место, откуда удаляем
|
||
sorted[remove_pos] = input;
|
||
}
|
||
else if (insert_pos < remove_pos) {
|
||
// Случай 2: Вставляем левее, чем удаляем
|
||
// Сдвигаем элементы [insert_pos, remove_pos-1] вправо
|
||
for (uint8_t j = remove_pos; j > insert_pos; j--) {
|
||
sorted[j] = sorted[j - 1];
|
||
}
|
||
sorted[insert_pos] = input;
|
||
}
|
||
else {
|
||
// Случай 3: Вставляем правее, чем удаляем (insert_pos > remove_pos)
|
||
// Сдвигаем элементы [remove_pos+1, insert_pos] влево
|
||
for (uint8_t j = remove_pos; j < insert_pos; j++) {
|
||
sorted[j] = sorted[j + 1];
|
||
}
|
||
sorted[insert_pos - 1] = input;
|
||
}
|
||
|
||
return sorted[size >> 1];
|
||
}
|
||
|
||
/**
|
||
* @brief Инициализация экспоненциального фильтра (int32_t)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param alpha Коэффициент сглаживания в масштабированном виде
|
||
* @param scale Масштаб коэффициента (например 100 для работы с процентами)
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) {
|
||
check_init_filter(filter);
|
||
|
||
filter->alpha = alpha;
|
||
filter->scale = scale;
|
||
filter->value = 0;
|
||
filter->initialized = 0;
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterExpInt_Init;
|
||
filter->process = &FilterExpInt_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения экспоненциальным фильтром (int32_t)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input Входное значение
|
||
* @return Отфильтрованное значение
|
||
*/
|
||
int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input) {
|
||
check_process_filter(filter);
|
||
|
||
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 Указатель на структуру фильтра
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterAverageInt_Init(FilterAverageInt_t* filter, uint32_t size, FilterMode_t mode) {
|
||
check_init_filter(filter);
|
||
if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return - 1;
|
||
#ifndef FILTERS_DISABLE_MOVING_AVERAGE
|
||
memset(filter->buffer, 0, sizeof(filter->buffer));
|
||
#endif
|
||
filter->size = size;
|
||
filter->sum = 0;
|
||
filter->index = 0;
|
||
filter->count = 0;
|
||
filter->mode = mode;
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterAverageInt_Init;
|
||
filter->process = &FilterAverageInt_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения фильтром скользящего среднего (int32_t)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input Входное значение
|
||
* @return Отфильтрованное значение
|
||
*/
|
||
int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) {
|
||
check_process_filter(filter);
|
||
|
||
// Общая логика для обоих режимов
|
||
filter->sum += input;
|
||
filter->count++;
|
||
|
||
// Логика скользящего среднего
|
||
if (filter->mode == FILTER_MODE_MOVING) {
|
||
#ifndef FILTERS_DISABLE_MOVING_AVERAGE
|
||
if (filter->count > filter->size) {
|
||
filter->sum -= filter->buffer[filter->index];
|
||
filter->count = filter->size; // Поддерживаем фиксированный размер окна
|
||
}
|
||
filter->buffer[filter->index] = input;
|
||
filter->index = (filter->index + 1) % filter->size;
|
||
filter->lastValue = filter->sum / filter->count;
|
||
#endif
|
||
}
|
||
else
|
||
{
|
||
if (filter->count >= filter->size)
|
||
{
|
||
filter->lastValue = filter->sum / filter->count;
|
||
filter->count = 0;
|
||
filter->sum = 0;
|
||
}
|
||
}
|
||
|
||
return filter->lastValue;
|
||
}
|
||
|
||
/**
|
||
* @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) {
|
||
check_init_filter(filter);
|
||
if ((coeffs == NULL) || (order > FILTER_POLY_MAX_ORDER)) return -1;
|
||
|
||
filter->order = order;
|
||
filter->scale = scale;
|
||
memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(int32_t));
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterPolyInt_Init;
|
||
filter->process = &FilterPolyInt_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Применение полиномиальной коррекции к значению (int32_t)
|
||
* @param filter Указатель на структуру коррекции
|
||
* @param input Входное значение
|
||
* @return Скорректированное значение
|
||
*/
|
||
int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) {
|
||
check_process_filter(filter);
|
||
|
||
// coefficients[0] = a_n * scale
|
||
// coefficients[1] = a_{n-1} * scale
|
||
// ...
|
||
// coefficients[n] = a_0 * 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 - линейная интерполяция)
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* output_arr, uint16_t size, uint8_t interpolation) {
|
||
check_init_filter(filter);
|
||
if ((input_arr == NULL) || (output_arr == NULL)) return -1;
|
||
|
||
filter->input_values = input_arr;
|
||
filter->output_values = output_arr;
|
||
filter->size = size;
|
||
filter->interpolation = interpolation;
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterLUTInt_Init;
|
||
filter->process = &FilterLUTInt_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения табличным фильтром (int32_t)
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input Входное значение
|
||
* @return Выходное значение по таблице
|
||
*/
|
||
int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) {
|
||
check_process_filter(filter);
|
||
if((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;
|
||
}
|
||
|
||
|
||
|
||
|
||
/**
|
||
* @brief Инициализация RMS фильтра (int32_t)
|
||
*/
|
||
int FilterRMSInt_Init(FilterRMSInt_t* filter, uint32_t window_size) {
|
||
check_init_filter(filter);
|
||
|
||
if (window_size == 0 || window_size > FILTER_RMS_MAX_SIZE) return -1;
|
||
|
||
filter->window_size = window_size;
|
||
filter->sum_squares = 0;
|
||
filter->last_rms = 0;
|
||
filter->index = 0;
|
||
filter->count = 0;
|
||
|
||
// Инициализируем буфер нулями
|
||
memset(filter->buffer_sq, 0, sizeof(filter->buffer_sq[0]) * window_size);
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterRMSInt_Init;
|
||
filter->process = &FilterRMSInt_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения RMS фильтром (int32_t)
|
||
*/
|
||
int32_t FilterRMSInt_Process(FilterRMSInt_t* filter, int32_t input) {
|
||
check_process_filter(filter);
|
||
|
||
// Вычисляем квадрат входного значения
|
||
int64_t square = (int64_t)input * input;
|
||
|
||
if (filter->count < filter->window_size) {
|
||
// Фаза накопления
|
||
filter->sum_squares += square;
|
||
filter->buffer_sq[filter->index] = square;
|
||
filter->count++;
|
||
} else {
|
||
// Фаза скользящего окна
|
||
filter->sum_squares -= filter->buffer_sq[filter->index]; // Вычитаем старое
|
||
filter->sum_squares += square; // Добавляем новое
|
||
filter->buffer_sq[filter->index] = square; // Сохраняем в буфер
|
||
}
|
||
|
||
// Увеличиваем индекс с зацикливанием
|
||
filter->index++;
|
||
if (filter->index >= filter->window_size) {
|
||
filter->index = 0;
|
||
}
|
||
|
||
// Вычисляем RMS
|
||
if (filter->count > 0) {
|
||
int64_t mean_square = filter->sum_squares / filter->count;
|
||
// Защита от отрицательных значений
|
||
if (mean_square < 0) mean_square = 0;
|
||
filter->last_rms = (int32_t)_sqrtf((float)mean_square);
|
||
}
|
||
|
||
return filter->last_rms;
|
||
}
|
||
|
||
|
||
|
||
// ==================== ДРУГИЕ ФИЛЬТРЫ ====================
|
||
|
||
/**
|
||
* @brief Инициализация полосового фильтра с дифференциатором
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param center_freq_ratio Отношение центральной частоты к частоте дискретизации (0..0.5)
|
||
* Например: 50 Гц / 1000 Гц = 0.05
|
||
* @param bandwidth_ratio Относительная ширина полосы (0..1)
|
||
* Например: 0.1 = полоса 10% от центральной частоты
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int FilterBandPass_Init(FilterBandPass_t* filter,
|
||
float center_freq_ratio,
|
||
float bandwidth_ratio) {
|
||
check_init_filter(filter);
|
||
|
||
// Проверка параметров
|
||
if (center_freq_ratio <= 0.0f || center_freq_ratio >= 0.5f) return -1;
|
||
if (bandwidth_ratio <= 0.0f || bandwidth_ratio > 1.0f) return -1;
|
||
|
||
// 1. Расчет параметров полосового фильтра
|
||
float w0 = 2.0f * PI * center_freq_ratio; // Нормированная угловая частота
|
||
float Q = 1.0f / bandwidth_ratio; // Добротность
|
||
float alpha = sinf(w0) / (2.0f * Q);
|
||
float cos_w0 = cosf(w0);
|
||
|
||
// Коэффициенты биквадратного полосового фильтра
|
||
// H_bp(z) = (alpha - alpha*z^-2) / (1 + a1*z^-1 + a2*z^-2)
|
||
float b0_bp = alpha;
|
||
float b1_bp = 0.0f;
|
||
float b2_bp = -alpha;
|
||
float a0_bp = 1.0f + alpha;
|
||
float a1_bp = -2.0f * cos_w0;
|
||
float a2_bp = 1.0f - alpha;
|
||
|
||
// Нормализация (a0 = 1)
|
||
filter->b0 = b0_bp / a0_bp;
|
||
filter->b1 = b1_bp / a0_bp;
|
||
filter->b2 = b2_bp / a0_bp;
|
||
filter->a1 = a1_bp / a0_bp;
|
||
filter->a2 = a2_bp / a0_bp;
|
||
|
||
// 2. Дифференциатор реализуется отдельно в process()
|
||
filter->prev_input = 0.0f;
|
||
|
||
// 3. Инициализация состояний фильтра
|
||
filter->x1 = 0.0f;
|
||
filter->x2 = 0.0f;
|
||
filter->y1 = 0.0f;
|
||
filter->y2 = 0.0f;
|
||
filter->last_output = 0.0f;
|
||
filter->dataProcessing = 0;
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterBandPass_Init;
|
||
filter->process = &FilterBandPass_Process;
|
||
|
||
return 0;
|
||
}
|
||
/**
|
||
* @brief Расчет таблицы фазовых сдвигов в градусах
|
||
* @param filter Указатель на фильтр
|
||
* @param phase_table Указатель на массив для фаз (в градусах)
|
||
* @param freq_points Указатель на массив частот (0..0.5)
|
||
* @param num_points Количество точек
|
||
* @return 0 - успех, -1 - ошибка
|
||
*//**
|
||
* @brief Расчет с повышенной точностью через double
|
||
*/
|
||
int FilterBandPass_CalcPhaseDegTable(FilterBandPass_t* filter,
|
||
float* phase_table,
|
||
const float* freq_points,
|
||
int num_points) {
|
||
|
||
if (!filter || !phase_table || !freq_points || num_points <= 0) {
|
||
return -1;
|
||
}
|
||
|
||
// Копируем коэффициенты в double для точности
|
||
double b0 = (double)filter->b0;
|
||
double b1 = (double)filter->b1;
|
||
double b2 = (double)filter->b2;
|
||
double a1 = (double)filter->a1;
|
||
double a2 = (double)filter->a2;
|
||
|
||
for (int i = 0; i < num_points; i++) {
|
||
double omega = 2.0 * PI * (double)freq_points[i];
|
||
|
||
double cos_w = cos(omega);
|
||
double sin_w = sin(omega);
|
||
double cos_2w = cos(2.0 * omega);
|
||
double sin_2w = sin(2.0 * omega);
|
||
|
||
double num_real = b0 + b1 * cos_w + b2 * cos_2w;
|
||
double num_imag = -b1 * sin_w - b2 * sin_2w;
|
||
|
||
double den_real = 1.0 + a1 * cos_w + a2 * cos_2w;
|
||
double den_imag = -a1 * sin_w - a2 * sin_2w;
|
||
|
||
// Используем устойчивое деление
|
||
double mag_sq = den_real * den_real + den_imag * den_imag;
|
||
|
||
if (mag_sq < 1e-30) { // Более строгий порог для double
|
||
phase_table[i] = 0.0f;
|
||
continue;
|
||
}
|
||
|
||
double H_real = (num_real * den_real + num_imag * den_imag) / mag_sq;
|
||
double H_imag = (num_imag * den_real - num_real * den_imag) / mag_sq;
|
||
|
||
double phase_rad = atan2(H_imag, H_real);
|
||
double phase_deg = phase_rad * 180.0 / PI;
|
||
|
||
// Нормализация
|
||
while (phase_deg > 180.0) phase_deg -= 360.0;
|
||
while (phase_deg < -180.0) phase_deg += 360.0;
|
||
|
||
phase_table[i] = (float)phase_deg;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения фильтром
|
||
* @param filter Указатель на структуру фильтра
|
||
* @param input Входное значение в диапазоне [-1.0, 1.0]
|
||
* @return Отфильтрованное значение
|
||
*
|
||
* @details Алгоритм:
|
||
// * 1. Вычисление производной: diff = input - prev_input
|
||
* 2. Полосовая фильтрация diff
|
||
* 3. Обновление состояний
|
||
*/
|
||
float FilterBandPass_Process(FilterBandPass_t* filter,
|
||
float input) {
|
||
check_process_filter(filter);
|
||
|
||
// 1. Дифференциатор (разностный фильтр 1-го порядка)
|
||
//float diff = input - filter->prev_input;
|
||
//filter->prev_input = input;
|
||
float diff = input;
|
||
|
||
// 2. Полосовой фильтр (биквадратный, прямая форма II)
|
||
// y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2]
|
||
float output = filter->b0 * diff +
|
||
filter->b1 * filter->x1 +
|
||
filter->b2 * filter->x2 -
|
||
filter->a1 * filter->y1 -
|
||
filter->a2 * filter->y2;
|
||
|
||
// 3. Обновление состояний
|
||
filter->x2 = filter->x1; // x[n-2] = x[n-1]
|
||
filter->x1 = diff; // x[n-1] = x[n]
|
||
filter->y2 = filter->y1; // y[n-2] = y[n-1]
|
||
filter->y1 = output; // y[n-1] = y[n]
|
||
|
||
filter->last_output = output;
|
||
filter->dataProcessing = 0; // Данные обработаны
|
||
|
||
return output;
|
||
}
|
||
|
||
|
||
#ifdef DSP_FITLERS
|
||
|
||
/**
|
||
* @brief Инициализация биквадратного фильтра с CMSIS-DSP
|
||
*/
|
||
int FilterBiquad_Init(FilterBiquad_t* filter, const float32_t coeffs[5])
|
||
{
|
||
check_init_filter(filter);
|
||
|
||
if (coeffs == NULL) return -1;
|
||
|
||
memcpy(filter->coeffs, coeffs, sizeof(filter->coeffs));
|
||
memset(filter->state_buffer, 0, sizeof(filter->state_buffer));
|
||
|
||
// Инициализация CMSIS-DSP структуры (1 каскад)
|
||
arm_biquad_cascade_df2T_init_f32(&filter->instance,
|
||
1,
|
||
filter->coeffs,
|
||
filter->state_buffer);
|
||
|
||
filter->state = FILTER_READY;
|
||
filter->reset = &FilterBiquad_Init;
|
||
filter->process = &FilterBiquad_Process;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Обработка значения биквадратным фильтром CMSIS-DSP
|
||
*/
|
||
float FilterBiquad_Process(FilterBiquad_t* filter, float input)
|
||
{
|
||
check_process_filter(filter);
|
||
|
||
float32_t in_arr[1] = {input};
|
||
float32_t out_arr[1];
|
||
|
||
arm_biquad_cascade_df2T_f32(&filter->instance, in_arr, out_arr, 1);
|
||
|
||
return out_arr[0];
|
||
}
|
||
#endif
|
||
|
||
#endif // FILTERS_ENABLE
|