Добавлен RMS фильтр
This commit is contained in:
parent
c5a01c56ac
commit
1f7384c5ed
@ -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);
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
/**
|
||||
|
||||
Loading…
Reference in New Issue
Block a user