From 1f7384c5edc30469217ca49c6489bdab13460320 Mon Sep 17 00:00:00 2001 From: Razvalyaev Date: Tue, 2 Dec 2025 18:39:52 +0300 Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=20RMS=20=D1=84=D0=B8=D0=BB=D1=8C=D1=82=D1=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MyLibs/Inc/filters.h | 43 ++++++++- MyLibs/Src/filters.c | 205 ++++++++++++++++++++++++++++++++++++++++--- 2 files changed, 234 insertions(+), 14 deletions(-) diff --git a/MyLibs/Inc/filters.h b/MyLibs/Inc/filters.h index 58c81c8..f21e451 100644 --- a/MyLibs/Inc/filters.h +++ b/MyLibs/Inc/filters.h @@ -94,7 +94,7 @@ int32_t process_value_int(int32_t raw_adc_quant) { #ifndef FILTERS_DISABLE_MOVING_AVERAGE #define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна усредняющего фильтра #else -#define FILTER_AVERAGE_MAX_SIZE 100000 ///< Размер окна усредняющего фильтра без буфера +#define FILTER_AVERAGE_MAX_SIZE 65535 ///< Размер окна усредняющего фильтра без буфера #endif #endif @@ -106,6 +106,9 @@ int32_t process_value_int(int32_t raw_adc_quant) { #define FILTER_POLY_MAX_ORDER 4 ///< Максимальный порядок полинома #endif +#ifndef FILTER_RMS_MAX_SIZE +#define FILTER_RMS_MAX_SIZE 128 ///< Максимальный размер окна (рекомендуется степень 2) +#endif #define check_proccess_func(_ptr_) \ ((_fltr_)->process != NULL) 1 : \ @@ -259,6 +262,22 @@ typedef struct _FilterLUT_t{ float (*process)(struct _FilterLUT_t *filter, float input); } FilterLUT_t; +/** + * @brief Структура True RMS фильтра (float) + */ +typedef struct _FilterRMS_t { + FilterState_t state; ///< Состояние фильтра + float buffer_sq[FILTER_RMS_MAX_SIZE]; ///< Буфер квадратов значений + float sum_squares; ///< Текущая сумма квадратов + float last_rms; ///< Последнее рассчитанное RMS значение + uint32_t window_size; ///< Размер окна усреднения + uint32_t index; ///< Текущий индекс в буфере + uint32_t count; ///< Количество накопленных значений + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterRMS_t *filter, uint32_t window_size); + float (*process)(struct _FilterRMS_t *filter, float input); +} FilterRMS_t; // Float версии функций int FilterMedian_Init(FilterMedian_t* filter, uint8_t size, float init_val); @@ -271,6 +290,8 @@ int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order); float FilterPoly_Process(FilterPoly_t* filter, float input); int 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); +int FilterRMS_Init(FilterRMS_t* filter, uint32_t window_size); +float FilterRMS_Process(FilterRMS_t* filter, float input); /** @@ -328,6 +349,7 @@ float FilterLUT_Process(FilterLUT_t* filter, float input); typedef struct _FilterMedianInt_t{ FilterState_t state; ///< Состояние фильтра int32_t buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений + int32_t sorted[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер отсортированных значений uint8_t index; ///< Текущий индекс uint8_t size; ///< Фактический размер фильтра uint8_t dataProcessing; ///< Флаг - данные в обработке @@ -402,6 +424,23 @@ typedef struct _FilterLUTInt_t{ int32_t (*process)(struct _FilterLUTInt_t *filter, int32_t input); } FilterLUTInt_t; +/** + * @brief Структура True RMS фильтра (int32_t) + */ +typedef struct _FilterRMSInt_t { + FilterState_t state; ///< Состояние фильтра + int64_t buffer_sq[FILTER_RMS_MAX_SIZE]; ///< Буфер квадратов значений + int64_t sum_squares; ///< Текущая сумма квадратов + int32_t last_rms; ///< Последнее рассчитанное RMS значение + uint32_t window_size; ///< Размер окна усреднения + uint32_t index; ///< Текущий индекс в буфере + uint32_t count; ///< Количество накопленных значений + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterRMSInt_t *filter, uint32_t window_size); + int32_t (*process)(struct _FilterRMSInt_t *filter, int32_t input); +} FilterRMSInt_t; + // Int32_t версии функций int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size, int32_t init_val); int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input); @@ -413,6 +452,8 @@ int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input); int 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); +int FilterRMSInt_Init(FilterRMSInt_t* filter, uint32_t window_size); +int32_t FilterRMSInt_Process(FilterRMSInt_t* filter, int32_t input); diff --git a/MyLibs/Src/filters.c b/MyLibs/Src/filters.c index cf1ac47..ed4ab61 100644 --- a/MyLibs/Src/filters.c +++ b/MyLibs/Src/filters.c @@ -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 /**