c/goertzel/goertzel.c

   1 #include <stdio.h>
   2 #include <math.h>
   3
   4 /*
   5         Usage examples
   6         arecord | ./goertzel
   7         sox input.mp3 -b 8 -c 1 -r 8000 -t wav - | ./goertzel
   8 */
   9
  10 float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
  11 {
  12         /*
  13                 On lower samplerates and frame sizes this may perform sub-optimally. Eg.:
  14                 When set to detect 440Hz (at 8000Hz samplerate and ~4000 samples)
  15                 it actually detects something around 438,3Hz rather than 400Hz...
  16                 If you can't increase samplerate way around this is just to increase sensitivity.
  17         */
  18
  19     int     k,i;
  20     float   floatnumSamples;
  21     float   omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;
  22
  23     float   scalingFactor = numSamples / 2.0;
  24
  25     floatnumSamples = (float) numSamples;
  26     k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
  27     omega = (2.0 * M_PI * k) / floatnumSamples;
  28     sine = sin(omega);
  29     cosine = cos(omega);
  30     coeff = 2.0 * cosine;
  31     q0=0;
  32     q1=0;
  33     q2=0;
  34
  35     for(i=0; i<numSamples; i++)
  36     {
  37         q0 = coeff * q1 - q2 + data[i];
  38         q2 = q1;
  39         q1 = q0;
  40     }
  41
  42     // calculate the real and imaginary results
  43     // scaling appropriately
  44     real = (q1 - q2 * cosine) / scalingFactor;
  45     imag = (q2 * sine) / scalingFactor;
  46
  47     magnitude = sqrtf(real*real + imag*imag);
  48     return magnitude;
  49 }
  50
  51 int main() {
  52         /*
  53         int samples[] = {0,1,2,3,4,5,6,7,8,9,8,7,6,5,4,3,2,1};
  54         int samplecount = 18;
  55         float power = goertzel(samplecount, samples, 1.2, 18);
  56         printf("G: %f\n", power);
  57         */
  58
  59         int samplerate = 8000;
  60         int samplecount = 4000;
  61         float samples[samplecount];
  62         float position = 0;
  63         fprintf(stderr,"Position (Secs)\tMagnitude\n");
  64         while(!feof(stdin)) {
  65                 int i;
  66                 for(i=0;i<samplecount && !feof(stdin);i++) {
  67                         unsigned char sample;
  68                         fread(&sample,1,1,stdin);
  69                         samples[i]=sample;
  70                         //printf("%d\n", sample);
  71                 }
  72                 position += ((float)samplecount/(float)samplerate);
  73                 float power = goertzel_mag(samplecount, 440, samplerate, samples);
  74                 printf("%f\t%f\n", position, power);
  75         }
  76 }