47 lines
1.2 KiB
C
47 lines
1.2 KiB
C
#ifndef LOWPASS_H
|
|
#define LOWPASS_H
|
|
|
|
#include <math.h>
|
|
|
|
// 一阶低通滤波器结构体
|
|
typedef struct {
|
|
float b0, b1; // 分子系数
|
|
float a1; // 分母系数 (a0 归一化为 1)
|
|
float x1; // 上一个输入样本
|
|
float y1; // 上一个输出样本
|
|
} LowPassFilter_t;
|
|
|
|
/**
|
|
* 初始化低通滤波器
|
|
* @param f 滤波器结构体指针
|
|
* @param fs 采样频率 (Hz)
|
|
* @param fc 截止频率 (-3dB 频率) (Hz)
|
|
*/
|
|
static inline void LowPassFilter_Init(LowPassFilter_t *f, float fs, float fc) {
|
|
// 预畸变角频率: w = 2 * pi * fc / fs, 然后 tan(w/2)
|
|
float wc = 2.0f * M_PI * fc / fs;
|
|
float tan_wc2 = tanf(wc * 0.5f);
|
|
float den = 1.0f + tan_wc2;
|
|
f->b0 = tan_wc2 / den;
|
|
f->b1 = f->b0;
|
|
f->a1 = (tan_wc2 - 1.0f) / den;
|
|
f->x1 = 0.0f;
|
|
f->y1 = 0.0f;
|
|
}
|
|
|
|
/**
|
|
* 运行低通滤波器
|
|
* @param f 滤波器结构体指针
|
|
* @param x 当前输入样本
|
|
* @return 滤波后的输出
|
|
*/
|
|
static inline float LowPassFilter_Run(LowPassFilter_t *f, float x) {
|
|
// y[n] = b0*x[n] + b1*x[n-1] - a1*y[n-1]
|
|
float y = f->b0 * x + f->b1 * f->x1 - f->a1 * f->y1;
|
|
f->x1 = x;
|
|
f->y1 = y;
|
|
return y;
|
|
}
|
|
|
|
#endif // LOWPASS_H
|