It worked for me. Here is Listing 1:
Listing 1 A Goertzel implementation
#include #include
#define FLOATING float #define SAMPLE unsigned char
#define SAMPLING_RATE 8000.0 //8kHz #define TARGET_FREQUENCY 941.0 //941 Hz #define N 205 //Block size
FLOATING coeff; FLOATING Q1; FLOATING Q2; FLOATING sine; FLOATING cosine;
SAMPLE testData[N];
/* Call this routine before every "block" (size=3DN) of samples. */ void ResetGoertzel(void) { Q2 =3D 0; Q1 =3D 0; }
/* Call this once, to precompute the constants. */ void InitGoertzel(void) { int k; FLOATING floatN; FLOATING omega;
floatN =3D (FLOATING) N; k =3D (int) (0.5 + ((floatN * TARGET_FREQUENCY) / SAMPLING_RATE)); omega =3D (2.0 * PI * k) / floatN; sine =3D sin(omega); cosine =3D cos(omega); coeff =3D 2.0 * cosine;
printf("For SAMPLING_RATE =3D %f", SAMPLING_RATE); printf(" N =3D %d", N); printf(" and FREQUENCY =3D %f,\n", TARGET_FREQUENCY); printf("k =3D %d and coeff =3D %f\n\n", k, coeff);
ResetGoertzel(); }
/* Call this routine for every sample. */ void ProcessSample(SAMPLE sample) { FLOATING Q0; Q0 =3D coeff * Q1 - Q2 + (FLOATING) sample; Q2 =3D Q1; Q1 =3D Q0; }
/* Basic Goertzel */ /* Call this routine after every block to get the complex result. */ void GetRealImag(FLOATING *realPart, FLOATING *imagPart) { *realPart =3D (Q1 - Q2 * cosine); *imagPart =3D (Q2 * sine); }
/* Optimized Goertzel */ /* Call this after every block to get the RELATIVE magnitude squared.
*/ FLOATING GetMagnitudeSquared(void) { FLOATING result;result =3D Q1 * Q1 + Q2 * Q2 - Q1 * Q2 * coeff; return result; }
/*** End of Goertzel-specific code, the remainder is test code. */
/* Synthesize some test data at a given frequency. */ void Generate(FLOATING frequency) { int index; FLOATING step;
step =3D frequency * ((2.0 * PI) / SAMPLING_RATE);
/* Generate the test data */ for (index =3D 0; index < N; index++) { testData[index] =3D (SAMPLE) (100.0 * sin(index * step) + 100.0); } }
/* Demo 1 */ void GenerateAndTest(FLOATING frequency) { int index;
FLOATING magnitudeSquared; FLOATING magnitude; FLOATING real; FLOATING imag;
printf("For test frequency %f:\n", frequency); Generate(frequency);
/* Process the samples */ for (index =3D 0; index < N; index++) { ProcessSample(testData[index]); }
/* Do the "basic Goertzel" processing. */ GetRealImag(&real, &imag);
printf("real =3D %f imag =3D %f\n", real, imag);
magnitudeSquared =3D real*real + imag*imag; printf("Relative magnitude squared =3D %f\n", magnitudeSquared); magnitude =3D sqrt(magnitudeSquared); printf("Relative magnitude =3D %f\n", magnitude);
/* Do the "optimized Goertzel" processing */ magnitudeSquared =3D GetMagnitudeSquared(); printf("Relative magnitude squared =3D %f\n", magnitudeSquared); magnitude =3D sqrt(magnitudeSquared); printf("Relative magnitude =3D %f\n\n", magnitude);
ResetGoertzel(); }
/* Demo 2 */ void GenerateAndTest2(FLOATING frequency) { int index;
FLOATING magnitudeSquared; FLOATING magnitude; FLOATING real; FLOATING imag;
printf("Freq=3D%7.1f ", frequency); Generate(frequency);
/* Process the samples. */ for (index =3D 0; index < N; index++) { ProcessSample(testData[index]); }
/* Do the "standard Goertzel" processing. */ GetRealImag(&real, &imag);
magnitudeSquared =3D real*real + imag*imag; printf("rel mag^2=3D%16.5f ", magnitudeSquared); magnitude =3D sqrt(magnitudeSquared); printf("rel mag=3D%12.5f\n", magnitude);
ResetGoertzel(); }
int main(void) { FLOATING freq;
InitGoertzel();
/* Demo 1 */ GenerateAndTest(TARGET_FREQUENCY - 250); GenerateAndTest(TARGET_FREQUENCY); GenerateAndTest(TARGET_FREQUENCY + 250);
/* Demo 2 */ for (freq =3D TARGET_FREQUENCY - 300; freq