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