diff --git a/MyLibs/Inc/bench_time.h b/MyLibs/Inc/bench_time.h index 731ccc3..178f1f1 100644 --- a/MyLibs/Inc/bench_time.h +++ b/MyLibs/Inc/bench_time.h @@ -182,6 +182,61 @@ static inline uint32_t BenchTime_End(uint8_t channel, uint32_t ticks) { return elapsed_ticks; } + +/** + * @brief Измерение периода на указанном канале + * @param channel Номер канала (0..BENCH_TIME_MAX_CHANNELS-1) + * @param ticks Источник тиков (например: HAL_GetTick(), TIM2->CNT, DWT->CYCCNT) + * @param tick_period Период тиков для переполнения + * @return Измеренное время в тиках, 0 - в случае первого запуска или ошибки + */ +static inline uint32_t BenchTime_Period(uint8_t channel, uint32_t ticks, uint32_t tick_period) { + if (channel >= BENCH_TIME_MAX_CHANNELS) return 0; + if (!hbt.channels[channel].is_running) + { + hbt.channels[channel].start_tick = ticks; + hbt.channels[channel].tick_period = tick_period; + hbt.channels[channel].is_running = 1; + return 0; + }; + + uint32_t end_tick = ticks; + uint32_t start_tick = hbt.channels[channel].start_tick; + uint32_t elapsed_ticks; + + // Инит для следующего измерения + hbt.channels[channel].start_tick = end_tick; + hbt.channels[channel].tick_period = tick_period; + + if (end_tick >= start_tick) { + elapsed_ticks = end_tick - start_tick; + } else { + elapsed_ticks = (tick_period - start_tick) + end_tick + 1; + } + + if (elapsed_ticks > tick_period) { + elapsed_ticks = tick_period; + } + + // Обновление статистики + BenchTimeStats_t* stats = &hbt.channels[channel].stats; + stats->last_ticks = elapsed_ticks; + + if (elapsed_ticks < stats->min_ticks) { + stats->min_ticks = elapsed_ticks; + } + + if (elapsed_ticks > stats->max_ticks) { + stats->max_ticks = elapsed_ticks; + } + + stats->total_ticks += elapsed_ticks; + stats->count++; + + return elapsed_ticks; +} + + /** * @brief Получение минимального времени измерения */ @@ -254,4 +309,4 @@ static inline void BenchTime_ResetStats(uint8_t channel) { /** BENCH_TIME * @} - */ \ No newline at end of file + */ diff --git a/MyLibs/Inc/bit_access.h b/MyLibs/Inc/bit_access.h index 52a6d60..f390c77 100644 --- a/MyLibs/Inc/bit_access.h +++ b/MyLibs/Inc/bit_access.h @@ -119,6 +119,15 @@ typedef union }uint64_BitTypeDef; +/** + * @brief Получить n-й бит из любого числа + * @param _uint8_ Переменная типа uint8_t + * @param _bit_ Константный номер бита (0..7) + * @return Значение выбранного бита (0 или 1) + * @note Индекс бита вычисляется программно, а не на этапе компиляции + */ +#define read_bit(_val_, _bit_) ((_val_) & ((uint64_t)1 << _bit_)) + /** * @brief Получить n-й бит из uint8_t * @param _uint8_ Переменная типа uint8_t @@ -156,4 +165,4 @@ typedef union /** BIT_ACCESS_DEFINES * @} - */ \ No newline at end of file + */ diff --git a/MyLibs/Inc/filters.h b/MyLibs/Inc/filters.h new file mode 100644 index 0000000..6c5957f --- /dev/null +++ b/MyLibs/Inc/filters.h @@ -0,0 +1,526 @@ +/** +****************************************************************************** +* @file filters.h +* @brief Заголовочный файл библиотеки фильтров +****************************************************************************** +* @addtogroup FILTERS Filters Library +* @brief Библиотека математических фильтров и коррекций +* @details +Поддерживает: +- Медианную фильтрацию (float и int32_t) +- Экспоненциальное скользящее среднее (float и int32_t) +- Скользящее среднее арифметическое (float и int32_t) +- Полиномиальную коррекцию (float и int32_t) +- Табличный фильтр LUT (Look-Up Table) (float и int32_t) + +Параметры для конфигурации: +- @ref FILTERS_ENABLE - Включить библиотеку фильтров +- @ref FILTER_MEDIAN_MAX_SIZE - Размер окна медианного фильтра (по умолчанию 5) +- @ref FILTER_AVERAGE_MAX_SIZE - Размер окна усредняющего фильтра (по умолчанию 8) +- @ref FILTER_POLY_MAX_ORDER - Максимальный порядок полинома (по умолчанию 4) + +@par Пример использования: +@code +#include "filters.h" + +// Фильтры для float +FilterMedian_t median_f; +FilterExp_t exp_f; +FilterAverage_t avg_f; +FilterPoly_t poly_f; + +// Фильтры для int32_t +FilterMedianInt_t median_i; +FilterExpInt_t exp_i; +FilterAverageInt_t avg_i; +FilterPolyInt_t poly_i; + +// Коэффициенты полинома +float poly_coeffs[3] = {0.0f, 1.1f, -0.05f}; +int32_t poly_coeffs_int[3] = {0, 110, -5}; // 1.1 и -0.05 с масштабом 100 + +void filters_init(void) { + // Float версии + FilterMedian_Init(&median_f); + FilterExp_Init(&exp_f, 0.1f); + FilterAverage_Init(&avg_f); + FilterPoly_Init(&poly_f, poly_coeffs, 3); + + // Int версии + FilterMedianInt_Init(&median_i); + FilterExpInt_Init(&exp_i, 10); // alpha = 0.1 с масштабом 100 + FilterAverageInt_Init(&avg_i); + FilterPolyInt_Init(&poly_i, poly_coeffs_int, 3, 100); // масштаб 100 +} + +// Обработка float значений +float process_value_float(float raw) { + float filtered; + filtered = FilterMedian_Process(&median_f, raw); + filtered = FilterExp_Process(&exp_f, filtered); + filtered = FilterAverage_Process(&avg_f, filtered); + filtered = FilterPoly_Process(&poly_f, filtered); + return filtered; +} + +// Обработка int32_t значений (квантов АЦП) +int32_t process_value_int(int32_t raw_adc_quant) { + int32_t filtered; + filtered = FilterMedianInt_Process(&median_i, raw_adc_quant); + filtered = FilterExpInt_Process(&exp_i, filtered); + filtered = FilterAverageInt_Process(&avg_i, filtered); + filtered = FilterPolyInt_Process(&poly_i, filtered); + return filtered; +} +@endcode +* @{ +*****************************************************************************/ +#ifndef __FILTERS_H_ +#define __FILTERS_H_ +#include "mylibs_defs.h" +#include +#include +#include + +#ifdef FILTERS_ENABLE + +#ifdef ARM_MATH_CM4 + #include "arm_math.h" + #define DSP_FITLERS 1 +#else + #include "math.h" +#endif + + +#ifndef FILTER_AVERAGE_MAX_SIZE +#ifndef FILTERS_DISABLE_MOVING_AVERAGE +#define FILTER_AVERAGE_MAX_SIZE 100 ///< Размер окна усредняющего фильтра +#else +#define FILTER_AVERAGE_MAX_SIZE 65535 ///< Размер окна усредняющего фильтра без буфера +#endif +#endif + +#ifndef FILTER_MEDIAN_MAX_SIZE +#define FILTER_MEDIAN_MAX_SIZE 10 ///< Размер окна медианного фильтра +#endif + +#ifndef FILTER_POLY_MAX_ORDER +#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 : \ +((_fltr_)->process == &FilterMedian_Init) 0 : \ +((_fltr_)->process == &FilterExp_Init) 0 : \ +((_fltr_)->process == &FilterAverage_Init) 0 : \ +((_fltr_)->process == &FilterPoly_Init) 0 : \ +((_fltr_)->process == &FilterLUT_Init) 0 : 1 + + +#define check_init_func(_ptr_) \ +((_fltr_)->process != NULL) 1 : \ +((_fltr_)->process == &FilterMedian_Process) 0 : \ +((_fltr_)->process == &FilterExp_Process) 0 : \ +((_fltr_)->process == &FilterExp_Process) 0 : \ +((_fltr_)->process == &FilterPoly_Process) 0 : \ +((_fltr_)->process == &FilterLUT_Process) 0 : 1 + +/** + * @brief Сброс фильтра (повторная инициализация) + * @param filter Указатель на структуру фильтра + * @details Запускает функцию инициализации, если указатель инициализирован + */ +#define Filter_ReInit(_fltr_, ...) \ +((_fltr_)->reset != NULL) ? (_fltr_)->reset(_fltr_, __VA_ARGS__): -1 + + +/** + * @brief Обработать число фильтром + * @param filter Указатель на структуру фильтра + * @details Запускает функцию фильтрации, если указатель инициализирован + */ +#define Filter_Process(_fltr_, _input_) \ +((_fltr_)->process != NULL) ? (_fltr_)->process(_fltr_, _input_): 0 + +/** + * @brief Запуск фильтра + * @param filter Указатель на структуру фильтра + * @details Запускает фильтр только если он в состоянии готовности. + * Если он не инициализирован или уже запущен - ничего не делается + */ +#define Filter_Start(_fltr_) \ +do{ if(Filter_isReady(_fltr_)) (_fltr_)->state = FILTER_ENABLE; }while(0) + +/** + * @brief Остановка работы фильтра + * @param filter Указатель на структуру фильтра + * @details Останавливет фильтр только если он запущен @ref Filter_Start. + * Если он не инициализирован или уже остановлен - ничего не делается + */ +#define Filter_Stop(_fltr_) \ +do{ if(Filter_isEnable(_fltr_)) (_fltr_)->state = FILTER_READY; }while(0) + +#define Filter_isDataReady(_fltr_) ((_fltr_)->dataProcessing == 0) + +#define Filter_GetState(_fltr_) (_fltr_)->state +#define Filter_isInit(_fltr_) !(Filter_GetState(_fltr_) == FILTER_NOT_INIT) +#define Filter_isReady(_fltr_) (Filter_GetState(_fltr_) == FILTER_READY) +#define Filter_isEnable(_fltr_) (Filter_GetState(_fltr_) == FILTER_ENABLE) + + +typedef enum +{ + FILTER_NOT_INIT, + FILTER_READY, + FILTER_ENABLE +}FilterState_t; + +typedef enum +{ + FILTER_MODE_DEFAULT = 0, + FILTER_MODE_MOVING, +} FilterMode_t; + + +// ==================== FLOAT ВЕРСИИ ==================== + +/** + * @brief Структура медианного фильтра (float) + */ +typedef struct _FilterMedian_t{ + FilterState_t state; ///< Состояние фильтра + float buffer[FILTER_MEDIAN_MAX_SIZE]; ///< Буфер значений + uint8_t index; ///< Текущий индекс + uint8_t size; ///< Фактический размер фильтра + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterMedian_t *filter, uint8_t size, float init_val); + float (*process)(struct _FilterMedian_t *filter, float input); +} FilterMedian_t; + +/** + * @brief Структура экспоненциального фильтра (float) + */ +typedef struct _FilterExp_t { + FilterState_t state; ///< Состояние фильтра + float alpha; ///< Коэффициент сглаживания (0..1) + float value; ///< Текущее значение + uint8_t initialized; ///< Флаг инициализации + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterExp_t *filter, float alpha); + float (*process)(struct _FilterExp_t *filter, float input); +} FilterExp_t; + +/** + * @brief Структура фильтра скользящего среднего (float) + */ +typedef struct _FilterAverage_t{ + FilterState_t state; ///< Состояние фильтра + FilterMode_t mode; ///< Режим фильтра +#ifndef FILTERS_DISABLE_MOVING_AVERAGE + float buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений +#endif + uint32_t size; ///< Фактический размер фильтра + float sum; ///< Сумма значений + uint32_t index; ///< Текущий индекс + uint32_t count; ///< Количество элементов + float lastValue; ///< Последнее измеренное значение + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterAverage_t *filter, uint32_t size, FilterMode_t mode); + float (*process)(struct _FilterAverage_t *filter, float input); +} FilterAverage_t; + +/** + * @brief Структура полиномиальной коррекции (float) + */ +typedef struct _FilterPoly_t{ + FilterState_t state; ///< Состояние фильтра + float coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома + uint8_t order; ///< Порядок полинома + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterPoly_t *filter, float* coeffs, uint8_t order); + float (*process)(struct _FilterPoly_t *filter, float input); +} FilterPoly_t; + +/** + * @brief Структура табличного фильтра (float) + */ +typedef struct _FilterLUT_t{ + FilterState_t state; ///< Состояние фильтра + float* input_values; // Массив входных значений + float* output_values; // Массив выходных значений + uint16_t size; // Размер таблицы + uint8_t interpolation; // Флаг интерполяции (0 - отключена, 1 - линейная) + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterLUT_t *filter, float* input_arr, float* output_arr, uint16_t size, uint8_t interpolation); + 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); +float FilterMedian_Process(FilterMedian_t* filter, float input); +int FilterExp_Init(FilterExp_t* filter, float alpha); +float FilterExp_Process(FilterExp_t* filter, float input); +int FilterAverage_Init(FilterAverage_t* filter, uint32_t size, FilterMode_t mode); +float FilterAverage_Process(FilterAverage_t* filter, float input); +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); + + +/** + * @brief Расчет коэфициента альфа + * @param tau Постоянная времени (время нарастания до 63% от уровня сигнала) + * @param TsUs Период вызова фильтра в микросекундах + * @return Коэфициент альфа + */ +#define FilterExp_CalcAlpha(tau, TsUs) (((float)TsUs/1000000) / (((float)TsUs/1000000) + (tau))) + +/** + * @brief Расчет alpha для времени нарастания до 63% + * @param rise_time Требуемое время нарастания до 63% + * @param TsUs Период вызова фильтра в микросекундах + * @return Коэфициент альфа + */ +#define FilterExp_CalcAlpha63(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 1.0f, TsUs) + +/** + * @brief Расчет alpha для времени нарастания до 86% + * @param rise_time Требуемое время нарастания до 86% + * @param TsUs Период вызова фильтра в микросекундах + * @return Коэфициент альфа + */ +#define FilterExp_CalcAlpha86(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 2.0f, TsUs) + +/** + * @brief Расчет alpha для времени нарастания до 95% + * @param rise_time Требуемое время нарастания до 95% + * @param TsUs Период вызова фильтра в микросекундах + * @return Коэфициент альфа + */ +#define FilterExp_CalcAlpha95(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 3.0f, TsUs) + +/** + * @brief Расчет alpha для времени нарастания до 98% + * @param rise_time Требуемое время нарастания до 98% + * @param TsUs Период вызова фильтра в микросекундах + * @return Коэфициент альфа + */ +#define FilterExp_CalcAlpha98(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 4.0f, TsUs) + +/** + * @brief Расчет alpha для времени нарастания до 99% + * @param rise_time Требуемое время нарастания до 99% + * @param TsUs Период вызова фильтра в микросекундах + * @return Коэфициент альфа + */ +#define FilterExp_CalcAlpha99(rise_time, TsUs) FilterExp_CalcAlpha((rise_time) / 5.0f, TsUs) +// ==================== INT32_T ВЕРСИИ ==================== + +/** + * @brief Структура медианного фильтра (int32_t) + */ +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; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterMedianInt_t *filter, uint8_t size, int32_t init_val); + int32_t (*process)(struct _FilterMedianInt_t *filter, int32_t input); +} FilterMedianInt_t; + +/** + * @brief Структура экспоненциального фильтра (int32_t) + */ +typedef struct _FilterExpInt_t{ + FilterState_t state; ///< Состояние фильтра + int32_t alpha; ///< Коэффициент сглаживания (в масштабе scale) + int32_t value; ///< Текущее значение + uint8_t initialized; ///< Флаг инициализации + int32_t scale; ///< Масштаб коэффициента (например 100 для 0.01) + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterExpInt_t *filter, int32_t alpha, int32_t scale); + int32_t (*process)(struct _FilterExpInt_t *filter, int32_t input); +} FilterExpInt_t; + +/** + * @brief Структура фильтра скользящего среднего (int32_t) + */ +typedef struct _FilterAverageInt_t{ + FilterState_t state; ///< Состояние фильтра + FilterMode_t mode; ///< Режим фильтра +#ifndef FILTERS_DISABLE_MOVING_AVERAGE + int32_t buffer[FILTER_AVERAGE_MAX_SIZE]; ///< Буфер значений +#endif + uint32_t size; ///< Фактический размер фильтра + int64_t sum; ///< Сумма значений + uint32_t index; ///< Текущий индекс + uint32_t count; ///< Количество элементов + uint32_t lastValue; ///< Последнее измеренное значение + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterAverageInt_t *filter, uint32_t size, FilterMode_t mode); + int32_t (*process)(struct _FilterAverageInt_t *filter, int32_t input); +} FilterAverageInt_t; + + + +/** + * @brief Структура полиномиальной коррекции (int32_t) + */ +typedef struct _FilterPolyInt_t{ + FilterState_t state; ///< Состояние фильтра + int32_t coefficients[FILTER_POLY_MAX_ORDER + 1]; ///< Коэффициенты полинома + uint8_t order; ///< Порядок полинома + int32_t scale; ///< Масштаб коэффициентов + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterPolyInt_t *filter, int32_t* coeffs, uint8_t order, int32_t scale); + int32_t (*process)(struct _FilterPolyInt_t *filter, int32_t input); +} FilterPolyInt_t; + +/** + * @brief Структура табличного фильтра (int32_t) + */ +typedef struct _FilterLUTInt_t{ + FilterState_t state; ///< Состояние фильтра + int32_t* input_values; ///< Массив входных значений + int32_t* output_values; ///< Массив выходных значений + uint16_t size; ///< Размер таблицы + uint8_t interpolation; ///< Флаг интерполяции + uint8_t dataProcessing; ///< Флаг - данные в обработке + + int (*reset)(struct _FilterLUTInt_t *filter, int32_t* input_arr, int32_t* output_arr, uint16_t size, uint8_t interpolation); + 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); +int FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale); +int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input); +int FilterAverageInt_Init(FilterAverageInt_t* filter, uint32_t size, FilterMode_t mode); +int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input); +int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, int32_t scale); +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); + + + +// ==================== ДРУГИЕ ФИЛЬТРЫ ==================== + + +/** + * @brief Структура полосового фильтра с дифференциатором + * @details Комбинация дифференциатора и полосового фильтра 2-го порядка. + * Используется для выделения сетевой частоты и подготовки к детектированию нуля. + */ +typedef struct _FilterBandPassDerivative_t { + FilterState_t state; ///< Состояние фильтра + + // Дифференциатор + float prev_input; ///< Предыдущее входное значение + + // Полосовой фильтр (биквадратный, прямая форма II) + float b0, b1, b2; ///< Коэффициенты числителя + float a1, a2; ///< Коэффициенты знаменателя + + // Состояния фильтра + float x1, x2; ///< Состояния входа + float y1, y2; ///< Состояния выхода + + float last_output; ///< Последнее выходное значение + uint8_t dataProcessing; ///< Флаг обработки данных + + // Указатели на функции + int (*reset)(struct _FilterBandPassDerivative_t *filter, + float center_freq_ratio, float bandwidth_ratio); + float (*process)(struct _FilterBandPassDerivative_t *filter, float input); +} FilterBandPassDerivative_t; + +int FilterBandPassDerivative_Init(FilterBandPassDerivative_t* filter, + float center_freq_ratio, + float bandwidth_ratio); +float FilterBandPassDerivative_Process(FilterBandPassDerivative_t* filter, + float input); + +#ifdef DSP_FITLERS +/** + * @brief Структура биквадратного фильтра с CMSIS-DSP + */ +typedef struct _FilterBiquad_t{ + FilterState_t state; ///< Состояние фильтра + arm_biquad_cascade_df2T_instance_f32 instance; ///< CMSIS-DSP instance + float32_t coeffs[5]; ///< Коэффициенты [b0, b1, b2, a1, a2] + float32_t state_buffer[4]; ///< Буфер состояний (2 состояния на каскад) + + int (*reset)(struct _FilterBiquad_t *filter, const float32_t coeffs[5]); + float (*process)(struct _FilterBiquad_t *filter, float input); +} FilterBiquad_t; + + +// CMSIS версии функций +int FilterBiquad_Init(FilterBiquad_t* filter, const float32_t coeffs[5]); +float FilterBiquad_Process(FilterBiquad_t* filter, float input); +#endif //DSP_FITLERS + + + + +#else // FILTERS_ENABLE + +#endif // FILTERS_ENABLE + +#endif // __FILTERS_H_ diff --git a/MyLibs/Inc/mylibs_defs.h b/MyLibs/Inc/mylibs_defs.h index 8b32702..fa60061 100644 --- a/MyLibs/Inc/mylibs_defs.h +++ b/MyLibs/Inc/mylibs_defs.h @@ -79,8 +79,8 @@ extern void Error_Handler(void); * Этот блок содержит макросы для реализации задержек с использованием HAL или FreeRTOS: * - @ref msDelay — простая задержка заданной длительности; * - @ref msDelayStart — сохранение текущего времени начала задержки; - * - @ref msDelayWhileActive — проверка, активна ли задержка; - * - @ref msDelayWaitDone — проверка, завершена ли задержка. + * - @ref msDelayActive — проверка, активна ли задержка; + * - @ref msDelayDone — проверка, завершена ли задержка. * Эти макросы удобны для реализации неблокирующих задержек. * @{ */ @@ -92,13 +92,18 @@ extern void Error_Handler(void); * @note Использует задержку через @ref local_time или osDelay в зависимости от @ref FREERTOS_DELAY. */ #ifdef FREERTOS_DELAY - #define msDelay(_ms_) osDelay(_ms_) +__STATIC_INLINE void msDelay(uint32_t _ms_) +{ + osDelay(_ms_); +} #else - #define msDelay(_ms_) \ - do { \ - uint32_t _start_ = local_time(); \ - while (local_time() - _start_ < (_ms_)) {} \ - } while(0) +__STATIC_INLINE void msDelay(uint32_t _ms_) +{ + volatile uint32_t _start_ = local_time(); + while ((local_time() - _start_) < (_ms_)) + { + } +} #endif @@ -110,7 +115,10 @@ extern void Error_Handler(void); * * Используется для реализации неблокирующих задержек. */ -#define msDelayStart(_pvar_) *(_pvar_) = local_time() +__STATIC_INLINE void msDelayStart(uint32_t *_pvar_) +{ + *(_pvar_) = local_time(); +} /** * @brief Проверяет, активна ли задержка. @@ -119,15 +127,18 @@ extern void Error_Handler(void); * @retval 1 Задержка еще активна. * @retval 0 Задержка завершена. * @details - * Возвращает true, пока время задержки не истекло. Используется в проверках, + * Возвращает true, пока задержка активна. Используется в проверках, * когда нужно **действовать, пока задержка выполняется**. Пример: * @code - * while(msDelayWhileActive(1000, &tick)) { + * while(msDelayActive(1000, &tick)) { * // выполняем другие задачи, задержка не блокирует поток * } * @endcode */ -#define msDelayWhileActive(_ms_, _pvar_) (local_time() - *(_pvar_) < _ms_) +__STATIC_INLINE int msDelayActive(uint32_t _ms_, uint32_t *_pvar_) +{ + return (local_time() - *(_pvar_) < _ms_); +} /** * @brief Проверяет, завершилась ли задержка. @@ -136,15 +147,18 @@ extern void Error_Handler(void); * @retval 1 Задержка завершена. * @retval 0 Задержка еще активна. * @details - * Возвращает true, когда задержка уже завершена. Используется в проверках, + * Возвращает true, когда задержка закончилась. Используется в проверках, * когда нужно **выполнить действие только после окончания задержки**. Пример: * @code - * if(msDelayWaitDone(1000, &tick)) { + * if(msDelayDone(1000, &tick)) { * // выполняем действие после завершения задержки * } * @endcode */ -#define msDelayWaitDone(_ms_, _pvar_) (local_time() - *(_pvar_) >= _ms_) +__STATIC_INLINE int msDelayDone(uint32_t _ms_, uint32_t *_pvar_) +{ + return (local_time() - *(_pvar_) >= _ms_); +} /** DELAYS_DEFINES * @} @@ -160,6 +174,12 @@ extern void Error_Handler(void); * @{ */ + /** + * @brief Размер структуры в её элементах. + * @param _struct_ Структура, размер которой надо вычислить. + */ +#define numbof(_arr_) (sizeof(_arr_)/sizeof(_arr_[0])) + /** * @brief Обнуление структуры. * @param _struct_ Структура, которую нужно обнулить. @@ -195,6 +215,12 @@ extern void Error_Handler(void); */ #define ABS(x) ( ((x) > 0)? (x) : -(x)) +/** + * @brief Константа Пи + */ +#ifndef PI +#define PI 3.14159265f +#endif /** UTILS_DEFINES * @} */ @@ -218,4 +244,4 @@ do{ \ /** @endcond */ -#endif //__MYLIBS_TOOLS_H_ \ No newline at end of file +#endif //__MYLIBS_TOOLS_H_ diff --git a/MyLibs/Inc/trace.h b/MyLibs/Inc/trace.h index 976b8d1..fde2312 100644 --- a/MyLibs/Inc/trace.h +++ b/MyLibs/Inc/trace.h @@ -587,4 +587,4 @@ __STATIC_FORCEINLINE void HF_HandleFault(void) -#endif //__TRACE_H_ \ No newline at end of file +#endif //__TRACE_H_ diff --git a/MyLibs/Inc/trackers.h b/MyLibs/Inc/trackers.h index 458dd00..089ecdb 100644 --- a/MyLibs/Inc/trackers.h +++ b/MyLibs/Inc/trackers.h @@ -159,4 +159,4 @@ #endif //TRACKERS_ENABLE -#endif //__TRACKERS_H_ \ No newline at end of file +#endif //__TRACKERS_H_ diff --git a/MyLibs/Src/filters.c b/MyLibs/Src/filters.c new file mode 100644 index 0000000..3a0edf9 --- /dev/null +++ b/MyLibs/Src/filters.c @@ -0,0 +1,904 @@ +/** +****************************************************************************** +* @file filters.c +* @brief Реализация библиотеки фильтров +****************************************************************************** +*/ +#include "filters.h" + +#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 +static int Filter_float_compare(const void *a, const void *b) { + float fa = *(const float*)a; + float fb = *(const float*)b; + if (fa < fb) return -1; + if (fa > fb) return 1; + return 0; +} + +/** + * @brief Инициализация медианного фильтра (float) + * @param filter Указатель на структуру фильтра + * @return 0 - успех, -1 - ошибка + */ +int FilterMedian_Init(FilterMedian_t* filter, uint8_t size, float init_val) { + check_init_filter(filter); + if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1; + + for (int i = 0; i < size; i++) + { + filter->buffer[i] = init_val; + } + filter->index = 0; + filter->size = size; + + filter->state = FILTER_READY; + filter->reset = &FilterMedian_Init; + filter->process = &FilterMedian_Process; + return 0; +} + +/** + * @brief Обработка значения медианным фильтром (float) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Отфильтрованное значение + */ +float FilterMedian_Process(FilterMedian_t* filter, float input) { + 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 - ошибка + */ +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; + filter->reset = &FilterExp_Init; + filter->process = &FilterExp_Process; + return 0; +} + +/** + * @brief Обработка значения экспоненциальным фильтром (float) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Отфильтрованное значение + */ +float FilterExp_Process(FilterExp_t* filter, float input) { + 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 - ошибка + */ +int FilterAverage_Init(FilterAverage_t* filter, uint32_t size, FilterMode_t mode) { + check_init_filter(filter); + if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return -1; + +#ifndef FILTERS_DISABLE_MOVING_AVERAGE + memset(filter->buffer, 0, sizeof(filter->buffer)); +#endif + filter->size = size; + filter->sum = 0.0f; + filter->index = 0; + filter->count = 0; + filter->mode = mode; + + filter->state = FILTER_READY; + filter->reset = &FilterAverage_Init; + filter->process = &FilterAverage_Process; + return 0; +} + +/** + * @brief Обработка значения фильтром скользящего среднего (float) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Отфильтрованное значение + */ +float FilterAverage_Process(FilterAverage_t* filter, float input) { + check_process_filter(filter); + + // Общая логика для обоих режимов + filter->sum += input; + filter->count++; + filter->dataProcessing = 1; + // Логика скользящего среднего + if (filter->mode == FILTER_MODE_MOVING) { +#ifndef FILTERS_DISABLE_MOVING_AVERAGE + if (filter->count > filter->size) { + filter->sum -= filter->buffer[filter->index]; + filter->count = filter->size; // Поддерживаем фиксированный размер окна + } + filter->buffer[filter->index] = input; + filter->index = (filter->index + 1) % filter->size; + filter->lastValue = filter->sum / filter->count; + filter->dataProcessing = 0; +#endif + } + else + { + if (filter->count >= filter->size) + { + filter->lastValue = filter->sum / filter->count; + filter->count = 0; + filter->sum = 0; + filter->dataProcessing = 0; + } + } + + return filter->lastValue; +} + +/** + * @brief Инициализация полиномиальной коррекции (float) + * @param filter Указатель на структуру коррекции + * @param coeffs Массив коэффициентов полинома + * @param order Порядок полинома + * @return 0 - успех, -1 - ошибка + */ +int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) { + 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; + filter->reset = &FilterPoly_Init; + filter->process = &FilterPoly_Process; + return 0; +} + +/** + * @brief Применение полиномиальной коррекции к значению (float) + * @param filter Указатель на структуру коррекции + * @param input Входное значение + * @return Скорректированное значение + */ +float FilterPoly_Process(FilterPoly_t* filter, float input) { + 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; +} + +/** + * @brief Инициализация табличного фильтра (float) + * @param filter Указатель на структуру фильтра + * @param input_arr Массив входных значений (должен быть отсортирован по возрастанию) + * @param output_arr Массив выходных значений + * @param size Размер таблицы + * @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция) + * @return 0 - успех, -1 - ошибка + */ +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; + filter->reset = &FilterLUT_Init; + filter->process = &FilterLUT_Process; + return 0; +} + +/** + * @brief Обработка значения табличным фильтром (float) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Выходное значение по таблице + */ +float FilterLUT_Process(FilterLUT_t* filter, float 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; + } + } + + // Без интерполяции - возвращаем значение левой границы + 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); +} + +/** + * @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 +//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; +//} + +/** + * @brief Инициализация медианного фильтра (int32_t) + * @param filter Указатель на структуру фильтра + * @return 0 - успех, -1 - ошибка + */ +int FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size, int32_t init_val) { + check_init_filter(filter); + if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return -1; + + for (int i = 0; i < size; i++) + { + filter->buffer[i] = init_val; + filter->sorted[i] = init_val; + } + + filter->index = 0; + filter->size = size; + + filter->state = FILTER_READY; + filter->reset = &FilterMedianInt_Init; + filter->process = &FilterMedianInt_Process; + return 0; +} + +/** + * @brief Обработка значения медианным фильтром (int32_t) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Отфильтрованное значение + */ +int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) { + 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]; +} + +/** + * @brief Инициализация экспоненциального фильтра (int32_t) + * @param filter Указатель на структуру фильтра + * @param alpha Коэффициент сглаживания в масштабированном виде + * @param scale Масштаб коэффициента (например 100 для работы с процентами) + * @return 0 - успех, -1 - ошибка + */ +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; + filter->reset = &FilterExpInt_Init; + filter->process = &FilterExpInt_Process; + return 0; +} + +/** + * @brief Обработка значения экспоненциальным фильтром (int32_t) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Отфильтрованное значение + */ +int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input) { + 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 - ошибка + */ +int FilterAverageInt_Init(FilterAverageInt_t* filter, uint32_t size, FilterMode_t mode) { + check_init_filter(filter); + if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return - 1; +#ifndef FILTERS_DISABLE_MOVING_AVERAGE + memset(filter->buffer, 0, sizeof(filter->buffer)); +#endif + filter->size = size; + filter->sum = 0; + filter->index = 0; + filter->count = 0; + filter->mode = mode; + + filter->state = FILTER_READY; + filter->reset = &FilterAverageInt_Init; + filter->process = &FilterAverageInt_Process; + return 0; +} + +/** + * @brief Обработка значения фильтром скользящего среднего (int32_t) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Отфильтрованное значение + */ +int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) { + check_process_filter(filter); + + // Общая логика для обоих режимов + filter->sum += input; + filter->count++; + + // Логика скользящего среднего + if (filter->mode == FILTER_MODE_MOVING) { +#ifndef FILTERS_DISABLE_MOVING_AVERAGE + if (filter->count > filter->size) { + filter->sum -= filter->buffer[filter->index]; + filter->count = filter->size; // Поддерживаем фиксированный размер окна + } + filter->buffer[filter->index] = input; + filter->index = (filter->index + 1) % filter->size; + filter->lastValue = filter->sum / filter->count; +#endif + } + else + { + if (filter->count >= filter->size) + { + filter->lastValue = filter->sum / filter->count; + filter->count = 0; + filter->sum = 0; + } + } + + return filter->lastValue; +} + +/** + * @brief Инициализация полиномиальной коррекции (int32_t) + * @param filter Указатель на структуру коррекции + * @param coeffs Массив коэффициентов полинома в масштабированном виде + * @param order Порядок полинома + * @param scale Масштаб коэффициентов + * @return 0 - успех, -1 - ошибка + */ +int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, int32_t scale) { + 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; + filter->reset = &FilterPolyInt_Init; + filter->process = &FilterPolyInt_Process; + return 0; +} + +/** + * @brief Применение полиномиальной коррекции к значению (int32_t) + * @param filter Указатель на структуру коррекции + * @param input Входное значение + * @return Скорректированное значение + */ +int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) { + 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); +} + +/** + * @brief Инициализация табличного фильтра (int32_t) + * @param filter Указатель на структуру фильтра + * @param input_arr Массив входных значений (должен быть отсортирован по возрастанию) + * @param output_arr Массив выходных значений + * @param size Размер таблицы + * @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция) + * @return 0 - успех, -1 - ошибка + */ +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; + filter->reset = &FilterLUTInt_Init; + filter->process = &FilterLUTInt_Process; + return 0; +} + +/** + * @brief Обработка значения табличным фильтром (int32_t) + * @param filter Указатель на структуру фильтра + * @param input Входное значение + * @return Выходное значение по таблице + */ +int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t 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; + } + } + + // Без интерполяции - возвращаем значение левой границы + 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; +} + + + + +/** + * @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)_sqrtf((float)mean_square); + } + + return filter->last_rms; +} + + + +// ==================== ДРУГИЕ ФИЛЬТРЫ ==================== + +/** + * @brief Инициализация полосового фильтра с дифференциатором + * @param filter Указатель на структуру фильтра + * @param center_freq_ratio Отношение центральной частоты к частоте дискретизации (0..0.5) + * Например: 50 Гц / 1000 Гц = 0.05 + * @param bandwidth_ratio Относительная ширина полосы (0..1) + * Например: 0.1 = полоса 10% от центральной частоты + * @return 0 - успех, -1 - ошибка + */ +int FilterBandPassDerivative_Init(FilterBandPassDerivative_t* filter, + float center_freq_ratio, + float bandwidth_ratio) { + check_init_filter(filter); + + // Проверка параметров + if (center_freq_ratio <= 0.0f || center_freq_ratio >= 0.5f) return -1; + if (bandwidth_ratio <= 0.0f || bandwidth_ratio > 1.0f) return -1; + + // 1. Расчет параметров полосового фильтра + float w0 = 2.0f * PI * center_freq_ratio; // Нормированная угловая частота + float Q = 1.0f / bandwidth_ratio; // Добротность + float alpha = sinf(w0) / (2.0f * Q); + float cos_w0 = cosf(w0); + + // Коэффициенты биквадратного полосового фильтра + // H_bp(z) = (alpha - alpha*z^-2) / (1 + a1*z^-1 + a2*z^-2) + float b0_bp = alpha; + float b1_bp = 0.0f; + float b2_bp = -alpha; + float a0_bp = 1.0f + alpha; + float a1_bp = -2.0f * cos_w0; + float a2_bp = 1.0f - alpha; + + // Нормализация (a0 = 1) + filter->b0 = b0_bp / a0_bp; + filter->b1 = b1_bp / a0_bp; + filter->b2 = b2_bp / a0_bp; + filter->a1 = a1_bp / a0_bp; + filter->a2 = a2_bp / a0_bp; + + // 2. Дифференциатор реализуется отдельно в process() + filter->prev_input = 0.0f; + + // 3. Инициализация состояний фильтра + filter->x1 = 0.0f; + filter->x2 = 0.0f; + filter->y1 = 0.0f; + filter->y2 = 0.0f; + filter->last_output = 0.0f; + filter->dataProcessing = 0; + + filter->state = FILTER_READY; + filter->reset = &FilterBandPassDerivative_Init; + filter->process = &FilterBandPassDerivative_Process; + + return 0; +} + +/** + * @brief Обработка значения фильтром + * @param filter Указатель на структуру фильтра + * @param input Входное значение в диапазоне [-1.0, 1.0] + * @return Отфильтрованное значение + * + * @details Алгоритм: + * 1. Вычисление производной: diff = input - prev_input + * 2. Полосовая фильтрация diff + * 3. Обновление состояний + */ +float FilterBandPassDerivative_Process(FilterBandPassDerivative_t* filter, + float input) { + check_process_filter(filter); + + // 1. Дифференциатор (разностный фильтр 1-го порядка) + //float diff = input - filter->prev_input; + //filter->prev_input = input; + float diff = input; + + // 2. Полосовой фильтр (биквадратный, прямая форма II) + // y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2] + float output = filter->b0 * diff + + filter->b1 * filter->x1 + + filter->b2 * filter->x2 - + filter->a1 * filter->y1 - + filter->a2 * filter->y2; + + // 3. Обновление состояний + filter->x2 = filter->x1; // x[n-2] = x[n-1] + filter->x1 = diff; // x[n-1] = x[n] + filter->y2 = filter->y1; // y[n-2] = y[n-1] + filter->y1 = output; // y[n-1] = y[n] + + filter->last_output = output; + filter->dataProcessing = 0; // Данные обработаны + + return output; +} + + + + +#ifdef DSP_FITLERS + +/** + * @brief Инициализация биквадратного фильтра с CMSIS-DSP + */ +int FilterBiquad_Init(FilterBiquad_t* filter, const float32_t coeffs[5]) +{ + check_init_filter(filter); + + if (coeffs == NULL) return -1; + + memcpy(filter->coeffs, coeffs, sizeof(filter->coeffs)); + memset(filter->state_buffer, 0, sizeof(filter->state_buffer)); + + // Инициализация CMSIS-DSP структуры (1 каскад) + arm_biquad_cascade_df2T_init_f32(&filter->instance, + 1, + filter->coeffs, + filter->state_buffer); + + filter->state = FILTER_READY; + filter->reset = &FilterBiquad_Init; + filter->process = &FilterBiquad_Process; + return 0; +} + +/** + * @brief Обработка значения биквадратным фильтром CMSIS-DSP + */ +float FilterBiquad_Process(FilterBiquad_t* filter, float input) +{ + check_process_filter(filter); + + float32_t in_arr[1] = {input}; + float32_t out_arr[1]; + + arm_biquad_cascade_df2T_f32(&filter->instance, in_arr, out_arr, 1); + + return out_arr[0]; +} +#endif + +#endif // FILTERS_ENABLE diff --git a/__mylibs_config.h b/__mylibs_config.h index 5e56f3d..22387d0 100644 --- a/__mylibs_config.h +++ b/__mylibs_config.h @@ -22,8 +22,8 @@ * @{ */ -#define TRACKERS_ENABLE ///< Включить трекеры -#define SERIAL_TRACE_ENABLE ///< Включить serial трассировку +//#define TRACKERS_ENABLE ///< Включить трекеры +//#define SERIAL_TRACE_ENABLE ///< Включить serial трассировку #define RTT_TRACE_ENABLE ///< Включить serial трассировку через RTT #define SWO_TRACE_ENABLE ///< Включить serial трассировку через SWO /** @@ -54,6 +54,25 @@ */ + +/** + * @addtogroup FILTER_CONFIG Filter configs + * @ingroup MYLIBS_CONFIG + * @brief Конфигурация фильтров + * @{ + */ + + +//#define FILTERS_ENABLE ///< Включить библиотеку фильтров +//#define FILTER_MEDIAN_MAX_SIZE ///< Максимальный размер окна медианного фильтра (по умолчанию 5) +//#define FILTER_AVERAGE_MAX_SIZE ///< Максимальный размер окна усредняющего фильтра (по умолчанию 8) +//#define FILTER_POLY_MAX_ORDER ///< Максимальный порядок полинома (по умолчанию 4) +#define FILTERS_DISABLE_MOVING_AVERAGE +/** GEN_CONFIG + * @} + */ + + /** * @addtogroup GEN_CONFIG Genetic configs * @ingroup MYLIBS_CONFIG @@ -61,7 +80,7 @@ * @{ */ -#define GEN_OPTIMIZATION_ENABLE ///< Включить оптимизацию параметров +//#define GEN_OPTIMIZATION_ENABLE ///< Включить оптимизацию параметров #define GEN_MAX_PARAMS 20 ///< Максимальное количество параметров #define GEN_MAX_CANDIDATES 100 ///< Максимальное количество кандидатов для обучения @@ -78,7 +97,7 @@ */ -#define BENCH_TIME_ENABLE ///< Включить бенч времени +//#define BENCH_TIME_ENABLE ///< Включить бенч времени #define BENCH_TIME_MAX_CHANNELS 16 ///< Максимальное количество каналов измерения /** GEN_CONFIG @@ -101,7 +120,7 @@ #define INCLUDE_TRACKERS_LIB ///< Подключить библиотеку с трекерами #define INCLUDE_TRACE_LIB ///< Подключить библиотеку с трейсами #define INCLUDE_GENERAL_PERIPH_LIBS ///< Подключить библиотеку с периферией -#define FREERTOS_DELAY ///< Использовать FreeRTOS задержку, вместо HAL +//#define FREERTOS_DELAY ///< Использовать FreeRTOS задержку, вместо HAL /** LIBS_CONFIG * @} @@ -110,4 +129,4 @@ /** MYLIBS_CONFIG * @} */ -#endif //__MYLIBS_CONFIG_H_ \ No newline at end of file +#endif //__MYLIBS_CONFIG_H_ diff --git a/__mylibs_include.h b/__mylibs_include.h index 27fa86c..fa445c8 100644 --- a/__mylibs_include.h +++ b/__mylibs_include.h @@ -37,58 +37,16 @@ #ifdef INCLUDE_TRACKERS_LIB #include "trackers.h" #else - #define TrackerTypeDef(num_user_vars) void * - #define num_of_usercnts(_user_) 0 - #define assert_tracecnt(_cntstruct_, _uservarnumb_) 0 - #define if_assert_usertracker(_cntstruct_, _uservarnumb_) if(0) - #define tern_assert_usertracker(_cntstruct_, _uservarnumb_) 0 - #define TrackerGet_Ok(_cntstruct_) dummy - #define TrackerGet_Err(_cntstruct_) dummy - #define TrackerGet_Warn(_cntstruct_) dummy - #define TrackerGet_User(_cntstruct_, _uservarnumb_) dummy - #define TrackerCnt_Ok(_cntstruct_) - #define TrackerCnt_Err(_cntstruct_) - #define TrackerCnt_Warn(_cntstruct_) - #define TrackerCnt_User(_cntstruct_, _uservarnumb_) - #define TrackerWrite_User(_cntstruct_, _uservarnumb_, _val_) - #define TrackerClear_All(_cntstruct_) - #define TrackerClear_Ok(_cntstruct_) - #define TrackerClear_Err(_cntstruct_) - #define TrackerClear_Warn(_cntstruct_) - #define TrackerClear_User(_cntstruct_) - #define TrackerClear_UserAll(_cntstruct_) #endif #ifdef INCLUDE_TRACE_LIB #include "trace.h" #else -#define my_printf(...) -#define log_printf(TAG, fmt, ...) -#define TRACE_GPIO_SET(_gpio_,_pin_) -#define TRACE_GPIO_RESET(_gpio_,_pin_) -#define RTT_FlashPrepare(...) -#define RTT_EraseFlash(...) 0 -#define RTT_SaveToFlash(...) 0 -#define RTT_ReadFromFlash(...) 0 -#define HF_CheckRecovered(...) 0 -#define HF_HandleFault(...) #endif #ifdef INCLUDE_GEN_OPTIMIZER #include "gen_optimizer.h" #else -typedef struct { - uint16_t n_params; - uint16_t n_cand; - uint16_t n_best; - uint16_t iq_mutation; - int32_t loss[0]; - int32_t candidates[0][0]; -} GenOptimizer_t; -#define GenOptimizer_Init(opt, n_params, n_cand, n_best, iq_mutation, start_params) -#define GenOptimizer_Step(opt, params, LossFunc) -#define PARAM_SCALE_Q16(x, min_val, max_val) (x) -#define PARAM_UNSCALE_Q16(q16_val, min_val, max_val) (q16_val) #endif @@ -96,17 +54,15 @@ typedef struct { #ifdef INCLUDE_BENCH_TEST #include "bench_time.h" #else //BENCH_TIME_ENABLE -#define BenchTime_Init() -#define BenchTime_Start(channel, ticks, tick_period) 0 -#define BenchTime_End(channel, ticks) 0 -#define BenchTime_GetMin(channel) 0 -#define BenchTime_GetMax(channel) 0 -#define BenchTime_GetAverage(channel) 0 -#define BenchTime_GetCount(channel) 0 -#define BenchTime_GetLast(channel) 0 -#define BenchTime_ResetStats(channel) #endif //BENCH_TIME_ENABLE + +#ifdef INCLUDE_FILTERS +#include "filters.h" +#else //INCLUDE_FILTERS +#endif //INCLUDE_FILTERS + + #ifdef INCLUDE_GENERAL_PERIPH_LIBS #include "__general_flash.h" @@ -133,4 +89,4 @@ typedef struct { /////////////////////////---USER SETTINGS---///////////////////////// -#endif // __MYLIBS_INCLUDE_H_ \ No newline at end of file +#endif // __MYLIBS_INCLUDE_H_