Добавлен RMS фильтр
This commit is contained in:
@@ -288,6 +288,70 @@ float FilterLUT_Process(FilterLUT_t* filter, float input) {
|
||||
|
||||
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
|
||||
@@ -311,6 +375,7 @@ int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size, int32_t init_v
|
||||
for (int i = 0; i < size; i++)
|
||||
{
|
||||
filter->buffer[i] = init_val;
|
||||
filter->sorted[i] = init_val;
|
||||
}
|
||||
|
||||
filter->index = 0;
|
||||
@@ -329,19 +394,69 @@ int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size, int32_t init_v
|
||||
* @return Отфильтрованное значение
|
||||
*/
|
||||
int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) {
|
||||
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];
|
||||
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];
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -578,6 +693,70 @@ int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) {
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @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)sqrt((double)mean_square);
|
||||
}
|
||||
|
||||
return filter->last_rms;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef DSP_FITLERS
|
||||
|
||||
/**
|
||||
|
||||
Reference in New Issue
Block a user