c/goertzel/goertzel.c

   1 #include <stdio.h>
   2 #include <math.h>
   3 #include <getopt.h>
   4
   5 float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
   6 {
   7     int     k,i;
   8     float   floatnumSamples;
   9     float   omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;
  10
  11     float   scalingFactor = numSamples / 2.0;
  12
  13     floatnumSamples = (float) numSamples;
  14     k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
  15     omega = (2.0 * M_PI * k) / floatnumSamples;
  16     sine = sin(omega);
  17     cosine = cos(omega);
  18     coeff = 2.0 * cosine;
  19     q0=0;
  20     q1=0;
  21     q2=0;
  22
  23     for(i=0; i<numSamples; i++)
  24     {
  25         q0 = coeff * q1 - q2 + data[i];
  26         q2 = q1;
  27         q1 = q0;
  28     }
  29
  30     // calculate the real and imaginary results
  31     // scaling appropriately
  32     real = (q1 - q2 * cosine) / scalingFactor;
  33     imag = (q2 * sine) / scalingFactor;
  34
  35     magnitude = sqrtf(real*real + imag*imag);
  36     return magnitude;
  37 }
  38
  39 void print_help(char ** argv) {
  40         printf(
  41                 "%s takes raw (wav) audio stream and computes power (or magnitude)\n"
  42                 "of desired frequencies using Goertzel algorithm for time frames\n"
  43                 "of fixed length (specified in samples or relative to sample rate).\n"
  44                 "This can be used in various frequency detection applications\n"
  45                 "like guitar tuning, DTMF decoding and many others...\n"
  46                 "\n"
  47                 "http://en.wikipedia.org/wiki/Goertzel_algorithm\n"
  48                 "\n"
  49                 "Curently only raw unsigned 8bit (u8) mono audio is supported, but\n"
  50                 "samplerate may vary. You can convert other formats before processing.\n"
  51                 "\n"
  52                 "On lower samplerates and frame sizes this may perform sub-optimally. Eg.:\n"
  53                 "When set to detect 440Hz (at 8000Hz samplerate and ~4000 samples)\n"
  54                 "it actually detects something around 438,3Hz rather than 400Hz...\n"
  55                 "If you can't increase samplerate way around this is just to increase sensitivity.\n"
  56                 "\n"
  57                 ,argv[0]
  58         );
  59
  60         printf(
  61                 "Arguments:\n"
  62                 "\t-i <file>\tInput from file (default STDIN)\n"
  63                 "\t-o <file>\tOutput to file (default STDOUT)\n"
  64                 "\t-a <file>\tOutput to file (append) (default STDOUT)\n"
  65                 "\n"
  66                 "\t-r <samplerate>\tInput samplerate (deault 8000 Hz)\n"
  67                 "\t-c <count>\tFrame size in samples (default 4000 Samples)\n"
  68                 "\t-d <ratio>\tFrame size (default 2) (samplerate will be divided by this number to get frame size same as -c)\n"
  69                 "\n"
  70                 "\t-f <freq>\tAdd frequency in Hz to detect (use multiple times, if no added 440 Hz will be...)\n"
  71                 "\n"
  72                 "\t-t <treshold>\tSet treshold (used to hide magnitudes lower than treshold) (defaults -1)\n"
  73                 "\t-n\t\tPrint integers rather than floats\n"
  74                 "\t-l\t\tDo not repeat values while still over treshold\n"
  75                 "\t-b\t\tDo not print first value that will fall under treshold\n"
  76                 "\t-q\t\tQuiet mode: print only values\n"
  77                 "\n"
  78                 "\t-?\t\tPrint help\n"
  79                 "\n"
  80         );
  81
  82         printf(
  83                 "Usage examples:\n"
  84                 "\tarecord | %s\n"
  85                 "\tsox input.mp3 -b 8 -c 1 -r 8000 -t wav - | %s\n"
  86                 "\t%s -n -q -l -r 8000 -d 20 -t $tresh -f 697 [-f 770 ...]\n"
  87                 "\n"
  88                 ,argv[0],argv[0],argv[0]
  89         );
  90
  91         printf(
  92                 "Frequencies for DTMF decoding:\n"
  93                 "\t-f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10\n"
  94         );
  95 }
  96
  97 void addfreq(int *freqs, int freq) {
  98         int i = 0;
  99         while(freqs[i]!=-1) i++;
 100         freqs[i]=freq;
 101         freqs[i+1]=-1;
 102 }
 103
 104 int main(int argc, char ** argv) {
 105         int samplerate = 8000;
 106         int samplecount = 4000;
 107         int treshold = -1;
 108         char noreturn = 0;
 109         char repeat = 1;
 110         char integers=0;
 111         char verbose=1;
 112         int freqs[argc+1]; freqs[0]=-1;
 113
 114         int opt;
 115         while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:nlbq")) != -1) {
 116                 switch (opt) {
 117                         case 'i':
 118                                 freopen(optarg, "r", stdin);
 119                                 break;
 120                         case 'o':
 121                                 freopen(optarg, "w", stdout);
 122                                 break;
 123                         case 'a':
 124                                 freopen(optarg, "a", stdout);
 125                                 break;
 126                         case 'r':
 127                                 samplerate = atoi(optarg);
 128                                 break;
 129                         case 'c':
 130                                 samplecount = atoi(optarg);
 131                                 break;
 132                         case 'd':
 133                                 samplecount = samplerate/atoi(optarg);
 134                                 break;
 135                         case 'f':
 136                                 addfreq(freqs, atoi(optarg));
 137                                 break;
 138                         case 't':
 139                                 treshold = atoi(optarg);
 140                                 break;
 141                         case 'n':
 142                                 integers = 1;
 143                                 break;
 144                         case 'l':
 145                                 repeat = 0;
 146                                 break;
 147                         case 'b':
 148                                 noreturn = 1;
 149                                 break;
 150                         case 'q':
 151                                 verbose = 0;
 152                                 break;
 153                         case '?':
 154                                 print_help(argv);
 155                                 return 0;
 156                                 break;
 157                 }
 158         }
 159
 160         if(freqs[0]==-1) addfreq(freqs, 440);
 161         float samples[samplecount];
 162         float position = 0;
 163
 164         if(verbose) {
 165                 fprintf(stderr,
 166                         "#Detected tone: %d Hz\n"
 167                         "#Samplerate: %d Hz\n"
 168                         "#Frame lenght: %d samples\n"
 169                         "#Treshold: %d\n"
 170                         "#\n"
 171                         ,freqs[0],samplerate,samplecount,treshold);
 172                 fflush(stderr);
 173
 174                 printf("#Position");
 175                 int i; for(i=0;freqs[i]!=-1;i++) {
 176                         printf("\t%2dHz",freqs[i]);
 177                 }
 178                 puts("");
 179         }
 180
 181         char print=0, printnow=0, printlast = 0;
 182         while(!feof(stdin)) {
 183                 int i;
 184
 185                 //Sample data
 186                 for(i=0;i<samplecount && !feof(stdin);i++) {
 187                         unsigned char sample;
 188                         fread(&sample,1,1,stdin);
 189                         samples[i]=sample;
 190                         //printf("%d\n", sample);
 191                 }
 192
 193                 //Apply goertzel
 194                 float power[argc];
 195                 print=0;
 196                 for(i=0;freqs[i]!=-1;i++) {
 197                         power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
 198
 199                         //Set print true if over treshold or if changed to false (print for the last time after going under treshold)
 200                         printnow = power[i] > treshold;
 201                         print = !(!repeat && printlast && !(!printnow)) && (print || printnow || (printlast && !noreturn));
 202                 }
 203                 printlast = printnow;
 204                 fflush(stdout);
 205
 206                 //Print data
 207                 if(print) {
 208                         printf("%8.2f", position);
 209                         for(i=0;freqs[i]!=-1;i++) {
 210                                 printf("\t");
 211                                 if(integers)
 212                                         printf("%d",(int)power[i]);
 213                                 else
 214                                         printf("%.4f",power[i]);
 215                         }
 216                         puts("");
 217                         fflush(stdout);
 218                 }
 219
 220                 //Increase time
 221                 position += ((float)samplecount/(float)samplerate);
 222         }
 223 }