goertzel_algorithm.c

TMS320C5416算法程序包。其中包括FFT

/*****************************************************************************/
/*                                                                           */
/* FILENAME                                                                  */
/*   goertzel_algorithm.c                                                    */
/*                                                                           */
/* DESCRIPTION                                                               */
/*   Contains multiple versions of Goertzel algorithms for 4 x 3 matrix      */
/*                                                                           */
/* REVISION                                                                  */
/*   Revision: 1.00	                                                         */
/*   Author  : Richard Sikora                                                */
/*---------------------------------------------------------------------------*/
/*                                                                           */
/* HISTORY                                                                   */
/*   Revision: 1.00                                                          */
/*   5th December 2002. Created by Richard Sikora.                           */
/*                                                                           */
/*****************************************************************************/

#include <stdio.h>

#include "goertzel.h"


/*****************************************************************************/
/* goertzel_filter()                                                         */
/*---------------------------------------------------------------------------*/
/*                                                                           */
/* Calculate the Goertzel value for a single frequency.                      */
/*                                                                           */
/* INPUTS:  Parameter 1. Input / output. Array containing delay elements.    */
/*          Parameter 2. Input to Goertzel algorithm.                        */
/*          Parameter 3. Coefficient for required frequency.                 */
/*                                                                           */
/* RETURNS: Nothing                                                          */
/*                                                                           */
/*****************************************************************************/

void goertzel_filter ( short int * delay, short int input, short int coefficient )
{
	long product;
      
    product = (long)(delay[1] * coefficient ) >> 14;

    /* The input is divided by 128 to prevent overload */  

	delay[0] = (short int)( (input >> 7) + (short int) product - delay[2] );
	
    /* Shuffle delay elements along one place */

	delay[2] = delay[1];
	delay[1] = delay[0]; 

} 

/*****************************************************************************/
/* calculate_goertzel_output()                                               */
/*---------------------------------------------------------------------------*/
/*                                                                           */
/* Calculate the Goertzel output power for a single frequency.               */
/*                                                                           */
/* INPUTS:  Parameter 1. Address of delay array.                             */
/*          Parameter 2. Coefficient for required frequency.                 */
/*                                                                           */
/* RETURNS: Goertzel value.                                                  */
/*                                                                           */
/*****************************************************************************/

short int calculate_goertzel_output( short int * delay, short int coefficient)
{
  long product1, product2, product3;
  signed int goertzel_power;
 
  product1 = (long)(delay[1] * delay[1]);  /* Magnitude of delay[1] */
	
  product2 = (long)(delay[2] * delay[2]);  /* Magnitude of delay[2] */ 
		 
  product3 = ( (long)( delay[1] * coefficient ) >> 14);

  product3 =  ((int) product3 * delay[2]); /* (int) saves call to library function */
		 
  goertzel_power = (short int) ((product1 + product2 - product3) >> 14 );
 
  /* Re-initialise delay. Also useful in event of instability */

  delay[1] = 0;
  delay[2] = 0;
		 
  return ( goertzel_power );
}

/*****************************************************************************/
/* goertzel_value()                                                          */
/*---------------------------------------------------------------------------*/
/*                                                                           */
/* Calculate the Goertzel value for a single frequency. Used for testing the */
/* accuracy of each Goertzel frequency and checks for overrange products.    */
/*                                                                           */
/* INPUTS:  Parameter 1. Input to Goertzel algorithm.                        */
/*          Parameter 2. Coefficient for required frequency.                 */
/*                                                                           */
/* RETURNS: Goertzel value.                                                  */
/*                                                                           */
/*****************************************************************************/

short int goertzel_value ( short int input, short int coefficient)
{
	static short int delay[3] = { 0, 0, 0 }; 
	static int N = 0;
    static short int goertzel_power =  0 ;
	long product1, product2, product3, temp;
    short int output;
  
    product1 = (long)(delay[1] * coefficient ) >> 14;

    if ( product1 > 32767)
      {
        product1 = 32767;
      }
    else if ( product1 < -32767)
      {
        product1 = -32767;
      }  

    /* The input is divided by 128 to prevent overload */  

	temp = (input >> 7) + (short int) product1 - delay[2];
	
	/* Range check for overload */

    if ( temp > 32767 )
	 {
	   temp = 32767;
	 }  
	else if ( temp < -32767)
	 {
	   temp = -32767;
	 }	

	delay[0] =  (short int) temp;

    /* Shuffle delay elements along one place */

	delay[2] = delay[1];
	delay[1] = delay[0];
	N++;

    /* Timer has expired. Calculate output */

	if ( 206 == N)
	   {
	     product1 = (long)(delay[1] * delay[1]);  /* Magnitude of delay[1] */
	
		 product2 = (long)(delay[2] * delay[2]);  /* Magnitude of delay[2] */ 
		 
		 product3 = ( (long)( delay[1] * coefficient ) >> 14);

		 product3 =  ((int) product3 * delay[2]); /* (int) saves call to library function */
		 
		 temp = ((product1 + product2 - product3) >> 14 );

         /* Range check for overload */

         if ( temp > 32767 )
		 {
		   temp = 32767;
		 }  
		 else if ( temp < -32767)
		 {
		   temp = -32767;
		 }		  

		 goertzel_power = (short int) temp;

         /* Re-initialise delay. Also useful in event of instability */

		 N = 0;
	     delay[1] = 0;
		 delay[2] = 0;
	}	 

   /* Calculate output. Divide by 32767 and add a small amount of gain */

   output = (short int) ( (long)(input * goertzel_power) >> (15 - 2)) ;

   return(output);
}


/*****************************************************************************/
/* End of goertzel_algorithm.c                                               */
/*****************************************************************************/