GearedMotors700W/UserSource/lowpass.c

/*
 * lowpass.c
 *
 *  Created on: 2023Äê4ÔÂ3ÈÕ
 *      Author: wangd
 */
#include "lowpass.h"
// cutoff  khz ,Ts ms
long a1,a2,a3,a4;
long lowpassfilter(long sample_1,long sample,long cutoff,long Ts)
{
    //1.0f/(M_2PI*cutoff)  rc =  1.0f/(M_2PI*cutoff)
    //alpha = Ts/(Ts + rc) = 1/(1 + Ts*2*pi*f)
    long t = Ts*cutoff>>12;
    long rcf = (25736*t)>>12;// M_2PI 25736

    long Hg = _IQ12div(4096,rcf);
    long alpha = _IQ12div(4096,4096+Hg);

    long result = (4096-alpha)*sample_1+alpha*sample;
    return result>>12;
}