fftv1.cpp

dsp实现的fft定点算法

#include "math.h"
#include "stdio.h"
#include "w.h"

void ChangeOrder (Int16 *changeDataIn, Int16 length, Int16 pow_4)
{
	int i, j, k;
	int temp1, temp2;
	Int16 temp3;
	int Index[10];
	for (i = 1; i < length - 1; i++)
	{
		temp1 = i;
		temp2 = 0;
		for (j = pow_4 - 1; j > 0; j--)
		{
			Index[j] = temp1 / int (pow (4, j) ) ;
			temp1 = temp1 % int (pow (4, j) );
		}
		Index[0] = temp1;
		for (k = 0; k < pow_4; k++)
		{
			temp2 = temp2 + Index[k] * int (pow (4, pow_4 - k - 1));
		}
		if (i < temp2)
		{
			temp3 = changeDataIn[2*i];
			changeDataIn[2*i] = changeDataIn[2*temp2];
			changeDataIn[2*temp2] = temp3;
			temp3 = changeDataIn[2*i+1];
			changeDataIn[2*i+1] = changeDataIn[2*temp2+1];
			changeDataIn[2*temp2+1] = temp3;

		}
	}
}
void DSP_r4fft(int n, Int16  *x,  short  *w)       
  {                                                                       
      int           n1, n2, ie, ia1, ia2, ia3, i0, i1, i2, i3, j, k;    
      Int16           t, r1, r2, s1, s2, co1, co2, co3, si1, si2, si3;    
      Int16		pow_4;
	  pow_4 = (Int16)(log(n) / log(4) + 0.5);                                                                    
      n2 = n;                                                             
      ie = 1;                                                             
      for (k = n; k > 4; k >>= 2)                                         
      {                                                                   
          n1 = n2;                                                        
          n2 >>= 2;                                                       
          ia1 = 0;                                                        
          for (j = 0; j < n2; j++)                                        
          {                                                               
              ia2 = ia1 + ia1;                                            
              ia3 = ia2 + ia1;                                            
              co1 = w[ia1 * 2 + 1];                                       
              si1 = w[ia1 * 2];                                           
              co2 = w[ia2 * 2 + 1];                                       
              si2 = w[ia2 * 2];                                           
              co3 = w[ia3 * 2 + 1];                                       
              si3 = w[ia3 * 2];                                           
              ia1 = ia1 + ie;                                             
              for (i0 = j; i0 < n; i0 += n1)                              
              {                                                           
                  i1 = i0 + n2;                                           
                  i2 = i1 + n2;                                           
                  i3 = i2 + n2;                                           
                  r1 = x[2 * i0] + x[2 * i2];                             
                  r2 = x[2 * i0] - x[2 * i2];                             
                  t = x[2 * i1] + x[2 * i3];                              
                  x[2 * i0] = (r1 + t + 1) >> 1;                                     
                  r1 = r1 - t;                                            
                  s1 = x[2 * i0 + 1] + x[2 * i2 + 1];                     
                  s2 = x[2 * i0 + 1] - x[2 * i2 + 1];                     
                  t = x[2 * i1 + 1] + x[2 * i3 + 1];                      
                  x[2 * i0 + 1] = ( s1 + t + 1) >> 1;                                 
                  s1 = s1 - t;                                            
                  x[2 * i2] = (r1 * co2 + s1 * si2 + 0x00008000) >> 16;                
                  x[2 * i2 + 1] = (s1 * co2-r1 * si2 + 0x00008000) >>16;                
                  t = x[2 * i1 + 1] - x[2 * i3 + 1];                      
                  r1 = r2 + t;                                            
                  r2 = r2 - t;                                            
                  t = x[2 * i1] - x[2 * i3];                              
                  s1 = s2 - t;                                            
                  s2 = s2 + t;                                            
                  x[2 * i1] = (r1 * co1 + s1 * si1 + 0x00008000)  >>16;                
                  x[2 * i1 + 1] = (s1 * co1-r1 * si1 + 0x00008000)>>16;                
                  x[2 * i3] = (r2 * co3 + s2 * si3 + 0x00008000)  >>16;                
                  x[2 * i3 + 1] = (s2 * co3-r2 * si3 + 0x00008000)>>16;                
              }                                                           
          }                                                               
          ie <<= 2;   
      }            
	  for (i0 = 0; i0 < n; i0 += 4)                              
	  {                                                           
          i1 = i0 + 1;                                           
          i2 = i1 + 1;                                           
          i3 = i2 + 1;                                           
          r1 = x[2 * i0] + x[2 * i2];                             
          r2 = x[2 * i0] - x[2 * i2];                             
          t = x[2 * i1] + x[2 * i3];                              
          x[2 * i0] = r1 + t;                                     
          r1 = r1 - t;                                            
          s1 = x[2 * i0 + 1] + x[2 * i2 + 1];                     
          s2 = x[2 * i0 + 1] - x[2 * i2 + 1];                     
          t = x[2 * i1 + 1] + x[2 * i3 + 1];                      
          x[2 * i0 + 1] = s1 + t;                                 
          s1 = s1 - t;                                            
          x[2 * i2] = r1;                
          x[2 * i2 + 1] = s1;                
          t = x[2 * i1 + 1] - x[2 * i3 + 1];                      
          r1 = r2 + t;                                            
          r2 = r2 - t;                                            
          t = x[2 * i1] - x[2 * i3];                              
          s1 = s2 - t;                                            
          s2 = s2 + t;                                            
          x[2 * i1] = r1 ;                
          x[2 * i1 + 1] = s1 ;                
          x[2 * i3] = r2 ;                
          x[2 * i3 + 1] = s2 ;                
       }                                     
	  
	 ChangeOrder (x, n, pow_4);
}            

int main()
{
/*	int i;
	short w[512];
	double PI2 = 3.141592654 * 2;
	for (i = 0; i < 256; i ++)
	{
		w[2*i] = (short)(sin  (i * PI2 / 256) * 32767);
		w[2*i+1] = (short)(cos  (i * PI2 / 256) * 32767);
	}
	FILE *fp;
	fp = fopen ("w.txt","w");
	for (i = 0; i < 512; i++)
	{
		fprintf (fp,"%8d,",w[i]);
		if ( (i+1)%4 == 0)
		{
			fprintf(fp,"\n");
		}
	}
	fclose(fp);
*/
	int i;
	Int16 fftin[512];
	for (i = 0; i < 256; i++)
	{
		fftin[2*i] = i;
		fftin[2*i+1] = 0;
	}
	DSP_r4fft(256,  fftin,  w)  ;
	return(0);
}