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 repeat = 1;
  80         char integers=0;
  81         char verbose=1;
  82         int freqs[argc+1]; freqs[0]=-1;
  83
  84         int opt;
  85         while ((opt = getopt(argc, argv, "?r:s:f:t:iqna")) != -1) {
  86                 switch (opt) {
  87                         case 'r':
  88                                 samplerate = atoi(optarg);
  89                                 break;
  90                         case 's':
  91                                 samplecount = atoi(optarg);
  92                                 break;
  93                         case 'f':
  94                                 addfreq(freqs, atoi(optarg));
  95                                 break;
  96                         case 't':
  97                                 treshold = atoi(optarg);
  98                                 break;
  99                         case 'i':
 100                                 integers = 1;
 101                                 break;
 102                         case 'a':
 103                                 repeat = 0;
 104                                 break;
 105                         case 'n':
 106                                 noreturn = 1;
 107                                 break;
 108                         case 'q':
 109                                 verbose = 0;
 110                                 break;
 111                         case '?':
 112                                 print_help(argv);
 113                                 return 0;
 114                                 break;
 115                 }
 116         }
 117
 118         if(freqs[0]==-1) addfreq(freqs, 440);
 119         float samples[samplecount];
 120         float position = 0;
 121
 122         if(verbose) {
 123                 fprintf(stderr,
 124                         "#Detected tone: %d Hz\n"
 125                         "#Samplerate: %d Hz\n"
 126                         "#Frame lenght: %d samples\n"
 127                         "#Treshold: %d\n"
 128                         "#\n"
 129                         ,freqs[0],samplerate,samplecount,treshold);
 130                 fflush(stderr);
 131
 132                 printf("#Position");
 133                 int i; for(i=0;freqs[i]!=-1;i++) {
 134                         printf("\t%2dHz",freqs[i]);
 135                 }
 136                 puts("");
 137         }
 138
 139         char print=0, printnow=0, printlast = 0;
 140         while(!feof(stdin)) {
 141                 int i;
 142
 143                 //Sample data
 144                 for(i=0;i<samplecount && !feof(stdin);i++) {
 145                         unsigned char sample;
 146                         fread(&sample,1,1,stdin);
 147                         samples[i]=sample;
 148                         //printf("%d\n", sample);
 149                 }
 150
 151                 //Apply goertzel
 152                 float power[argc];
 153                 print=0;
 154                 for(i=0;freqs[i]!=-1;i++) {
 155                         power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
 156
 157                         //Set print true if over treshold or if changed to false (print for the last time after going under treshold)
 158                         printnow = power[i] > treshold;
 159                         print = !(!repeat && printlast && !(!printnow)) && (print || printnow || (printlast && !noreturn));
 160                 }
 161                 printlast = printnow;
 162                 fflush(stdout);
 163
 164                 //Print data
 165                 if(print) {
 166                         printf("%8.2f", position);
 167                         for(i=0;freqs[i]!=-1;i++) {
 168                                 printf("\t");
 169                                 if(integers)
 170                                         printf("%d",(int)power[i]);
 171                                 else
 172                                         printf("%.4f",power[i]);
 173                         }
 174                         puts("");
 175                         fflush(stdout);
 176                 }
 177
 178                 //Increase time
 179                 position += ((float)samplecount/(float)samplerate);
 180         }
 181 }