iir_i2.c

来自「TI的DSP C55X的应用程序」· C语言 代码 · 共 37 行

/*
	iir_i2.c - IIR direct form II biquads filter
                   in Fixed-point implementation
*/

#include <intrindefs.h>

#pragma CODE_SECTION(iir, "iir_code");

#define SCALE 1   

void iir(int *x, unsigned int Nx, int *y, int *coef, unsigned int Ns, int *w)
{      
    unsigned int i,j,n,m,k,l;
    int temp;
    long w_0;
        
    m=Ns*5;             /* Setup for circular buffer coef[] */
    k=(Ns<<1)-1;        /* Setup for circular buffer d[] */

    for (j=0,l=0,n=Nx; n>0; n--)           /* IIR filter */ 
    {
        w_0 = (long)*x <<(15-SCALE); *x++; /* Q14 input (scaled) */ 
        for (i=Ns; i>0; i--)
        {    
            w_0 = _smas(w_0,*(w+l),*(coef+j)); j++; l=(l+Ns)&k; 
            w_0 = _smas(w_0,*(w+l),*(coef+j)); j++;	
            temp = *(w+l);
            *(w+l) = w_0>>15;                  
            w_0 = _lsmpy(     temp,*(coef+j)); j++;	 
            w_0 = _smac(w_0,*(w+l),*(coef+j)); j++; l=(l+Ns)&k;  
            w_0 = _smac(w_0,*(w+l),*(coef+j)); j=(j+1)%m; l=(l+1)&k;
        }
        *y++ = w_0>>(15-SCALE);             /* Q15 format output */
    }
}