三阶陷波滤波器系数计算函数 Alg_TrpFlt_calcSet

void Alg_TrpFlt_calcSet(struct ST_trapper *p, float fFliterFeq, float fCtrlFeq)
{
    // 声明并初始化变量
    float32_t a, b, c;
    float32_t f1Divfw;
    float32_t ftmp;
    // 计算滤波器的截止频率
    p->fw = fFliterFeq * 6.28318f * 2.0f;  // fw为截止频率，fFliterFeq为输入的截止频率
    // 计算控制周期
    p->fT = 1.0f / fCtrlFeq;  // fT为控制周期，fCtrlFeq为输入的控制频率
    // 计算1/fw
    f1Divfw = 1.0f / p->fw;
    // 计算a、b、c的值
    a = (f1Divfw) * (f1Divfw);
    b = 10.0F * f1Divfw;
    c = 0.00000001f * f1Divfw;
    // 计算中间结果
    ftmp = 1.0f / (4.0f * a + 2.0f * b * p->fT + p->fT * p->fT);
    // 计算滤波器系数
    p->fk_0 = ftmp * (4.0f * a + 2.0f * c * p->fT + p->fT * p->fT);
    p->fk_1 = ftmp * (-8.0f * a + 2.0f * p->fT * p->fT);
    p->fk_2 = ftmp * (4.0f * a + p->fT * p->fT - 2.0f * c * p->fT);
    p->fk_3 = ftmp * (4.0f * a + p->fT * p->fT - 2.0f * b * p->fT);
}

这段代码是一个函数，用于计算三阶陷波滤波器的系数。

函数的输入参数为结构体指针p，以及滤波器的截止频率fFliterFeq和控制周期fCtrlFeq。

在函数中，首先声明并初始化了三个变量a、b、c，以及中间结果变量ftmp。

接着，计算了滤波器的截止频率fw，以及控制周期fT。

接下来，计算了1/fw的值，以及a、b、c的值。

然后，通过ftmp计算出了中间结果。

最后，根据中间结果计算了滤波器系数fk_0、fk_1、fk_2、fk_3。

函数的作用是计算出三阶陷波滤波器的系数，用于滤除噪声。