rfast21.c

dsp AD公司ADSP21的代码,里面有FFT FIR IIR EQULIZER G722_21F 等可以在项目中直接应用的代码.此代码的来源是ADI公司自己出版的书籍,此书在美国购得

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "rtdspc.h"
#include "filter.h"

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

RFAST.C - Realtime fast convolution using the FFT

This program performs fast convolution using the FFT.  It performs
the convolution required to implement a 35 point FIR filter
(stored in variable fir_lpf35) on an
arbitrary length realtime input.  The filter is
a LPF with 40 dB out of band rejection.  The 3 dB point is at a
relative frequency of approximately .25*fs.

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

/* FFT length must be a power of 2 */
#define FFT_LENGTH 1024
#define M 10            /* must be log2(FFT_LENGTH) */
#define FILTER_LENGTH 35

/* set to 1 to bypass the filter */
    int bypass = 0;

void main()
{
  int          i, j;
  float        tempflt,inv_len;
  COMPLEX      *samp, *filt;
  static float input_save[FILTER_LENGTH];

/* power of 2 length of FFT and complex allocation */
  samp = (COMPLEX *) calloc(FFT_LENGTH, sizeof(COMPLEX));
  if(!samp){
    exit(1);
  }

/* Zero fill the filter to the sequence length */
  filt = (COMPLEX *) calloc(FFT_LENGTH, sizeof(COMPLEX));
  if(!filt){
    exit(1);
  }

/* copy the filter into complex array and scale by 1/N for inverse FFT */
  inv_len = 1.0/FFT_LENGTH;  
  for(i = 0 ; i < FILTER_LENGTH ; i++)
      filt[i].real = inv_len*fir_lpf35[i];

/* FFT the zero filled filter impulse response */
  fft(filt,M);

/* read in one FFT worth of samples to start, imag already zero */
  for(i = 0 ; i < FFT_LENGTH-FILTER_LENGTH ; i++)
    samp[i].real = getinput();

/* save the last FILTER_LENGTH points for next time */
  for(j = 0 ; j < FILTER_LENGTH ; j++, i++)
    input_save[j] = samp[i].real = getinput();

 while(1) {

/* do FFT of samples */
  fft(samp,M);

/* Multiply the two transformed sequences */
/* swap the real and imag outputs so we can do a forward FFT instead of
inverse FFT */
  if(!bypass) {
    for(i = 0 ; i < FFT_LENGTH ; i++) {
      tempflt = samp[i].real * filt[i].real
                             - samp[i].imag * filt[i].imag;
      samp[i].real = samp[i].real * filt[i].imag
                             + samp[i].imag * filt[i].real;
      samp[i].imag = tempflt;
    }
  }
  else {
    for(i = 0 ; i < FFT_LENGTH ; i++) {
      tempflt = inv_len*samp[i].real;
      samp[i].real = inv_len*samp[i].imag;
      samp[i].imag = tempflt;
    }
  }

/* Inverse fft the multiplied sequences */
  fft(samp,M);

/* Write the result out to a dsp data file */
/* because we used a forward FFT for the inverse FFT,
the output is in the imag part */
  for(i = FILTER_LENGTH ; i < FFT_LENGTH ; i++) sendout(samp[i].imag);

/* overlap the last FILTER_LENGTH-1 input data points in the next FFT */
  for(i = 0; i < FILTER_LENGTH ; i++) {
    samp[i].real = input_save[i];
    samp[i].imag = 0.0;
  }

  for( ; i < FFT_LENGTH-FILTER_LENGTH ; i++) {
    samp[i].real = getinput();
    samp[i].imag = 0.0;
  }

/* save the last FILTER_LENGTH points for next time */
  for(j = 0 ; j < FILTER_LENGTH ; j++, i++) {
    input_save[j] = samp[i].real = getinput();
    samp[i].imag = 0.0;
  }
 
 }

}

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

fft - In-place radix 2 decimation in time FFT

Requires pointer to complex array, x and power of 2 size of FFT, m
(size of FFT = 2**m).  Places FFT output on top of input COMPLEX array.

void fft(COMPLEX *x, int m)

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

void fft(COMPLEX *x,int m)
{
    static COMPLEX *w;           /* used to store the w complex array */
    static int mstore = 0;       /* stores m for future reference */
    static int n = 1;            /* length of fft stored for future */

    COMPLEX u,temp,tm;
    COMPLEX *xi,*xip,*xj,*wptr;

    int i,j,k,l,le,windex;

    double arg,w_real,w_imag,wrecur_real,wrecur_imag,wtemp_real;

    if(m != mstore) {

/* free previously allocated storage and set new m */

        if(mstore != 0) free(w);
        mstore = m;
        if(m == 0) return;       /* if m=0 then done */

/* n = 2**m = fft length */

        n = 1 << m;
        le = n/2;

/* allocate the storage for w */

        w = (COMPLEX *) calloc(le-1,sizeof(COMPLEX));
        if(!w) {
            exit(1);
        }

/* calculate the w values recursively */

        arg = 4.0*atan(1.0)/le;         /* PI/le calculation */
        wrecur_real = w_real = cos(arg);
        wrecur_imag = w_imag = -sin(arg);
        xj = w;
        for (j = 1 ; j < le ; j++) {
            xj->real = (float)wrecur_real;
            xj->imag = (float)wrecur_imag;
            xj++;
            wtemp_real = wrecur_real*w_real - wrecur_imag*w_imag;
            wrecur_imag = wrecur_real*w_imag + wrecur_imag*w_real;
            wrecur_real = wtemp_real;
        }
    }

/* start fft */

    le = n;
    windex = 1;
    for (l = 0 ; l < m ; l++) {
        le = le/2;

/* first iteration with no multiplies */

        for(i = 0 ; i < n ; i = i + 2*le) {
            xi = x + i;
            xip = xi + le;
            temp.real = xi->real + xip->real;
            temp.imag = xi->imag + xip->imag;
            xip->real = xi->real - xip->real;
            xip->imag = xi->imag - xip->imag;
            *xi = temp;
        }

/* remaining iterations use stored w */

        wptr = w + windex - 1;
        for (j = 1 ; j < le ; j++) {
            u = *wptr;
            for (i = j ; i < n ; i = i + 2*le) {
                xi = x + i;
                xip = xi + le;
                temp.real = xi->real + xip->real;
                temp.imag = xi->imag + xip->imag;
                tm.real = xi->real - xip->real;
                tm.imag = xi->imag - xip->imag;             
                xip->real = tm.real*u.real - tm.imag*u.imag;
                xip->imag = tm.real*u.imag + tm.imag*u.real;
                *xi = temp;
            }
            wptr = wptr + windex;
        }
        windex = 2*windex;
    }            

/* rearrange data by bit reversing */

    j = 0;
    for (i = 1 ; i < (n-1) ; i++) {
        k = n/2;
        while(k <= j) {
            j = j - k;
            k = k/2;
        }
        j = j + k;
        if (i < j) {
            xi = x + i;
            xj = x + j;
            temp = *xj;
            *xj = *xi;
            *xi = temp;
        }
    }
}