C语言陷阱滤波器系数计算函数 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 = fFliterFeq6.28318f2.0f; // 计算控制周期 p->fT = 1.0f/fCtrlFeq; // 计算1/fw f1Divfw =1.0f/p->fw; // 计算a、b、c的值 a=(f1Divfw)(f1Divfw); b= 10.0Ff1Divfw; c= 0.00000001ff1Divfw; // 计算中间结果 ftmp =1.0f/(4.0fa + 2.0fbp->fT +p->fTp->fT); // 计算滤波器系数 p->fk_0 = ftmp*(4.0fa + 2.0fcp->fT +p->fTp->fT); p->fk_1 = ftmp*(-8.0fa+2.0fp->fTp->fT); p->fk_2 = ftmp(4.0fa +p->fTp->fT - 2.0fcp->fT); p->fk_3 = ftmp*(4.0fa +p->fTp->fT - 2.0fbp->fT); }