c/goertzel/goertzel.c

   1 #include <stdio.h>
   2 #include <math.h>
   3 #include <getopt.h>
   4
   5
   6 /*
   7         Usage examples
   8         arecord | ./goertzel
   9         sox input.mp3 -b 8 -c 1 -r 8000 -t wav - | ./goertzel
  10
  11         Arguments for DTMF decoding:
  12         -f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10
  13 */
  14
  15 float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
  16 {
  17         /*
  18                 On lower samplerates and frame sizes this may perform sub-optimally. Eg.:
  19                 When set to detect 440Hz (at 8000Hz samplerate and ~4000 samples)
  20                 it actually detects something around 438,3Hz rather than 400Hz...
  21                 If you can't increase samplerate way around this is just to increase sensitivity.
  22         */
  23
  24     int     k,i;
  25     float   floatnumSamples;
  26     float   omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;
  27
  28     float   scalingFactor = numSamples / 2.0;
  29
  30     floatnumSamples = (float) numSamples;
  31     k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
  32     omega = (2.0 * M_PI * k) / floatnumSamples;
  33     sine = sin(omega);
  34     cosine = cos(omega);
  35     coeff = 2.0 * cosine;
  36     q0=0;
  37     q1=0;
  38     q2=0;
  39
  40     for(i=0; i<numSamples; i++)
  41     {
  42         q0 = coeff * q1 - q2 + data[i];
  43         q2 = q1;
  44         q1 = q0;
  45     }
  46
  47     // calculate the real and imaginary results
  48     // scaling appropriately
  49     real = (q1 - q2 * cosine) / scalingFactor;
  50     imag = (q2 * sine) / scalingFactor;
  51
  52     magnitude = sqrtf(real*real + imag*imag);
  53     return magnitude;
  54 }
  55
  56 void print_help(char ** argv) {
  57         printf("help me %s\n", argv[0]);
  58 }
  59
  60 void addfreq(int *freqs, int freq) {
  61         int i = 0;
  62         while(freqs[i]!=-1) i++;
  63         freqs[i]=freq;
  64         freqs[i+1]=-1;
  65 }
  66
  67 int main(int argc, char ** argv) {
  68         /*
  69         int samples[] = {0,1,2,3,4,5,6,7,8,9,8,7,6,5,4,3,2,1};
  70         int samplecount = 18;
  71         float power = goertzel(samplecount, samples, 1.2, 18);
  72         printf("G: %f\n", power);
  73         */
  74
  75         int samplerate = 8000;
  76         int samplecount = 4000;
  77         int treshold = -1;
  78         char noreturn = 0;
  79         char integers=0;
  80         char verbose=1;
  81         int freqs[argc+1]; freqs[0]=-1;
  82
  83         int opt;
  84         while ((opt = getopt(argc, argv, "?r:s:f:t:iqn")) != -1) {
  85                 switch (opt) {
  86                         case 'r':
  87                                 samplerate = atoi(optarg);
  88                                 break;
  89                         case 's':
  90                                 samplecount = atoi(optarg);
  91                                 break;
  92                         case 'f':
  93                                 addfreq(freqs, atoi(optarg));
  94                                 break;
  95                         case 't':
  96                                 treshold = atoi(optarg);
  97                                 break;
  98                         case 'i':
  99                                 integers = 1;
 100                                 break;
 101                         case 'n':
 102                                 noreturn = 1;
 103                                 break;
 104                         case 'q':
 105                                 verbose = 0;
 106                                 break;
 107                         case '?':
 108                                 print_help(argv);
 109                                 return 0;
 110                                 break;
 111                 }
 112         }
 113
 114         if(freqs[0]==-1) addfreq(freqs, 440);
 115         float samples[samplecount];
 116         float position = 0;
 117
 118         if(verbose) {
 119                 fprintf(stderr,
 120                         "#Detected tone: %d Hz\n"
 121                         "#Samplerate: %d Hz\n"
 122                         "#Frame lenght: %d samples\n"
 123                         "#Treshold: %d\n"
 124                         "#\n"
 125                         ,freqs[0],samplerate,samplecount,treshold);
 126                 fflush(stderr);
 127
 128                 printf("#Position");
 129                 int i; for(i=0;freqs[i]!=-1;i++) {
 130                         printf("\t%2dHz",freqs[i]);
 131                 }
 132                 puts("");
 133         }
 134
 135         char print=0, printnow=0, printlast = 0;
 136         while(!feof(stdin)) {
 137                 int i;
 138
 139                 //Sample data
 140                 for(i=0;i<samplecount && !feof(stdin);i++) {
 141                         unsigned char sample;
 142                         fread(&sample,1,1,stdin);
 143                         samples[i]=sample;
 144                         //printf("%d\n", sample);
 145                 }
 146
 147                 //Apply goertzel
 148                 float power[argc];
 149                 print=0;
 150                 for(i=0;freqs[i]!=-1;i++) {
 151                         power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
 152
 153                         //Set print true if over treshold or if changed to false (print for the last time after going under treshold)
 154                         printnow = power[i] > treshold;
 155                         print = print || printnow || (printlast && !noreturn);
 156                 }
 157                 printlast = printnow;
 158                 fflush(stdout);
 159
 160                 //Print data
 161                 if(print) {
 162                         printf("%8.2f", position);
 163                         for(i=0;freqs[i]!=-1;i++) {
 164                                 printf("\t");
 165                                 if(integers)
 166                                         printf("%d",(int)power[i]);
 167                                 else
 168                                         printf("%.4f",power[i]);
 169                         }
 170                         puts("");
 171                         fflush(stdout);
 172                 }
 173
 174                 //Increase time
 175                 position += ((float)samplecount/(float)samplerate);
 176         }
 177 }