Templates/ExtendedLibs
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 2 Wiki Activity

release 0.3

Browse Source 
- Добавлены фильтры
- Добавлена функция для измерения периода в bench time
- Добавлена функция для получения n-го бита в bit_access
- Дефайн функции заменены на _STATIC_INLINE
This commit is contained in:
Razvalyaev
2025-12-28 15:36:25 +03:00
parent eff64709bc
commit 795ebbd220
9 changed files with 1573 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
MyLibs/Inc/bench_time.h
View File

@@ -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 Получение минимального времени измерения
*/

9
MyLibs/Inc/bit_access.h
View File

@@ -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

526
MyLibs/Inc/filters.h Normal file
View File

@@ -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 <stdint.h>
#include <stdlib.h>
#include <string.h>
#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_

56
MyLibs/Inc/mylibs_defs.h
View File

@@ -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
* @}
*/

904
MyLibs/Src/filters.c Normal file
View File

@@ -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

29
__mylibs_config.h
View File

@@ -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
* @}

58
__mylibs_include.h
View File

@@ -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"