滤波器-二阶低通滤波器

滤波器-二阶低通滤波

BetaFilght 二阶低通

/*低通滤波参数*/
#define GYRO_LPF_CUTOFF_FREQ  120
#define ACCEL_LPF_CUTOFF_FREQ 30

typedef struct 
{
	float a1;
	float a2;
	float b0;
	float b1;
	float b2;
	float delay_element_1;
	float delay_element_2;
} lpf2pData;

/******************************************************************************/

const _iq M_PI_IQ = _IQ(3.14159265);

#define M_PI_F (float)3.14159265
#define M_PI_FLOAT 3.14159265358979323846f

// #define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/

/**********************************二阶滤波 - 浮点数版本****************************************/

lpf2pData accLpf[2];
static void lpf2pSetCutoffFreq(lpf2pData* lpfData, float sample_freq, float cutoff_freq);

/**
 * 二阶低通滤波
 */

/// @brief 二阶低通滤波器初始化
/// @param lpfData 滤波器结构体指针，用于存放滤波器参数和数据
/// @param sample_freq 采样率
/// @param cutoff_freq 截止频率
void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq)
{
	if (lpfData == NULL || cutoff_freq <= 0.0f)
	{
		return;
	}

	lpf2pSetCutoffFreq(lpfData, sample_freq, cutoff_freq);
}

/**
 * 设置二阶低通滤波截至频率
 */
static void lpf2pSetCutoffFreq(lpf2pData* lpfData, float sample_freq, float cutoff_freq)
{
	float fr = sample_freq/cutoff_freq;
	float ohm = tanf(M_PI_F/fr);
	float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm;
	lpfData->b0 = ohm*ohm/c;
	lpfData->b1 = 2.0f*lpfData->b0;
	lpfData->b2 = lpfData->b0;
	lpfData->a1 = 2.0f*(ohm*ohm-1.0f)/c;
	lpfData->a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
	lpfData->delay_element_1 = 0.0f;
	lpfData->delay_element_2 = 0.0f;
}

float lpf2pApply(lpf2pData* lpfData, float sample)
{
	float delay_element_0 , output;
	delay_element_0 = sample - lpfData->delay_element_1 * lpfData->a1 - lpfData->delay_element_2 * lpfData->a2;
	if (!isfinite(delay_element_0))
	{
		// don't allow bad values to propigate via the filter
		delay_element_0 = sample;
	}

	output = delay_element_0 * lpfData->b0 + lpfData->delay_element_1 * lpfData->b1 + lpfData->delay_element_2 * lpfData->b2;

	lpfData->delay_element_2 = lpfData->delay_element_1;
	lpfData->delay_element_1 = delay_element_0;
	return output;
}