Добавлен RMS фильтр

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);
 
 
 

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);
+    check_process_filter(filter);
     
-  // Добавляем значение в буфер
+    register int32_t old_value = filter->buffer[filter->index];
-  filter->buffer[filter->index] = input;
+    register uint8_t size = filter->size;
-  filter->index = (filter->index + 1) % filter->size;
+    register uint8_t idx = filter->index;
+    int32_t* sorted = filter->sorted;
     
-  // Копируем буфер для сортировки
+    // Обновляем circular buffer
-  int32_t sort_buffer[FILTER_MEDIAN_MAX_SIZE];
+    filter->buffer[idx] = input;
-  memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer));
+    idx++;
+    if (idx >= size) idx = 0;
+    filter->index = idx;
     
-  // Сортируем и возвращаем медиану
+    // Одновременно ищем позицию для удаления и вставки
-  qsort(sort_buffer, filter->size, sizeof(int32_t), Filter_int32_compare);
+    uint8_t remove_pos = 0;
-  return sort_buffer[filter->size / 2];
+    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
 
 /**