Templates/ExtendedLibs
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 2 Wiki Activity

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

Browse Source 
- универсализированы проверки состояния инициализации фильтров
- avg сделан скользящий и обычный
- плюс еще что-то наверное
This commit is contained in:
Razvalyaev
2025-11-11 12:45:27 +03:00
parent 3b162c9f8c
commit 9b9969dd7c
2 changed files with 399 additions and 320 deletions
Download Patch File Download Diff File Expand all files Collapse all files

605
MyLibs/Src/filters.c
View File

@@ -8,6 +8,14 @@
#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
@@ -22,15 +30,18 @@ static int Filter_float_compare(const void *a, const void *b) {
/**
* @brief Инициализация медианного фильтра (float)
* @param filter Указатель на структуру фильтра
* @return 0 - успех, -1 - ошибка
*/
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 = size;
int FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
check_init_filter(filter);
if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1;
memset(filter->buffer, 0, sizeof(filter->buffer));
filter->index = 0;
filter->size = size;
filter->state = FILTER_READY;
return 0;
}
/**
@@ -40,32 +51,36 @@ void FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
* @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_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];
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 - ошибка
*/
void FilterExp_Init(FilterExp_t* filter, float alpha) {
if (filter == NULL) return;
filter->alpha = alpha;
filter->value = 0.0f;
filter->initialized = 0;
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;
return 0;
}
/**
@@ -75,31 +90,36 @@ void FilterExp_Init(FilterExp_t* filter, float alpha) {
* @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;
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 - ошибка
*/
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;
int FilterAverage_Init(FilterAverage_t* filter, uint8_t size, FilterMode_t mode) {
check_init_filter(filter);
if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return -1;
memset(filter->buffer, 0, sizeof(filter->buffer));
filter->size = size;
filter->sum = 0.0f;
filter->index = 0;
filter->count = 0;
filter->mode = mode;
filter->state = FILTER_READY;
return 0;
}
/**
@@ -109,21 +129,33 @@ void FilterAverage_Init(FilterAverage_t* filter, uint8_t size) {
* @return Отфильтрованное значение
*/
float FilterAverage_Process(FilterAverage_t* filter, float input) {
if (filter == NULL) return input;
// Вычитаем старое значение из суммы
if (filter->count == FILTER_AVERAGE_MAX_SIZE) {
filter->sum -= filter->buffer[filter->index];
} else {
filter->count++;
check_process_filter(filter);
// Общая логика для обоих режимов
filter->sum += input;
filter->count++;
// Логика скользящего среднего
if (filter->mode == FILTER_MODE_MOVING) {
if (filter->count > filter->size) {
filter->sum -= filter->buffer[filter->index];
filter->count = filter->size; // Поддерживаем фиксированный размер окна
}
// Добавляем новое значение
filter->buffer[filter->index] = input;
filter->sum += input;
filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE;
return filter->sum / filter->count;
filter->index = (filter->index + 1) % filter->size;
filter->lastValue = filter->sum / filter->count;
}
else
{
if (filter->count > filter->size)
{
filter->lastValue = filter->sum / filter->count;
filter->count = 0;
filter->sum = 0;
}
}
return filter->lastValue;
}
/**
@@ -134,12 +166,14 @@ float FilterAverage_Process(FilterAverage_t* filter, float input) {
* @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;
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;
return 0;
}
/**
@@ -149,17 +183,17 @@ int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) {
* @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;
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;
}
/**
@@ -169,14 +203,19 @@ float FilterPoly_Process(FilterPoly_t* filter, float input) {
* @param output_arr Массив выходных значений
* @param size Размер таблицы
* @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция)
* @return 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;
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;
return 0;
}
/**
@@ -186,71 +225,76 @@ void FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, ui
* @return Выходное значение по таблице
*/
float FilterLUT_Process(FilterLUT_t* filter, float input) {
if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) {
return 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;
}
// Поиск ближайших значений в таблице
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);
}
// Без интерполяции - возвращаем значение левой границы
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
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;
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 - ошибка
*/
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 = size;
int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
check_init_filter(filter);
if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1;
memset(filter->buffer, 0, sizeof(filter->buffer));
filter->index = 0;
filter->size = size;
filter->state = FILTER_READY;
return 0;
}
/**
@@ -260,19 +304,19 @@ void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
* @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_MAX_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];
check_process_filter(filter);
// Добавляем значение в буфер
filter->buffer[filter->index] = input;
filter->index = (filter->index + 1) % filter->size;
// Копируем буфер для сортировки
int32_t sort_buffer[FILTER_MEDIAN_MAX_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];
}
/**
@@ -280,14 +324,18 @@ int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) {
* @param filter Указатель на структуру фильтра
* @param alpha Коэффициент сглаживания в масштабированном виде
* @param scale Масштаб коэффициента (например 100 для работы с процентами)
* @return 0 - успех, -1 - ошибка
*/
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;
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;
return 0;
}
/**
@@ -297,35 +345,40 @@ void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) {
* @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;
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 - ошибка
*/
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;
int FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size, FilterMode_t mode) {
check_init_filter(filter);
if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return - 1;
memset(filter->buffer, 0, sizeof(filter->buffer));
filter->size = size;
filter->sum = 0;
filter->index = 0;
filter->count = 0;
filter->mode = mode;
filter->state = FILTER_READY;
return 0;
}
/**
@@ -335,21 +388,33 @@ void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size) {
* @return Отфильтрованное значение
*/
int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) {
if (filter == NULL) return input;
// Вычитаем старое значение из суммы
if (filter->count == FILTER_AVERAGE_MAX_SIZE) {
filter->sum -= filter->buffer[filter->index];
} else {
filter->count++;
check_process_filter(filter);
// Общая логика для обоих режимов
filter->sum += input;
filter->count++;
// Логика скользящего среднего
if (filter->mode == FILTER_MODE_MOVING) {
if (filter->count > filter->size) {
filter->sum -= filter->buffer[filter->index];
filter->count = filter->size; // Поддерживаем фиксированный размер окна
}
// Добавляем новое значение
filter->buffer[filter->index] = input;
filter->sum += input;
filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE;
return (int32_t)(filter->sum / filter->count);
filter->index = (filter->index + 1) % filter->size;
filter->lastValue = filter->sum / filter->count;
}
else
{
if (filter->count == filter->size)
{
filter->lastValue = filter->sum / filter->count;
filter->count = 0;
filter->sum = 0;
}
}
return filter->lastValue;
}
/**
@@ -361,13 +426,15 @@ int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) {
* @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;
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;
return 0;
}
/**
@@ -377,24 +444,24 @@ int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order,
* @return Скорректированное значение
*/
int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) {
if (filter == NULL) return input;
// 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);
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);
}
/**
@@ -404,14 +471,19 @@ int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) {
* @param output_arr Массив выходных значений
* @param size Размер таблицы
* @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция)
* @return 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;
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;
return 0;
}
/**
@@ -421,48 +493,49 @@ void FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* outp
* @return Выходное значение по таблице
*/
int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) {
if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) {
return 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;
}
// Поиск ближайших значений в таблице
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;
}
// Без интерполяции - возвращаем значение левой границы
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