c/goertzel/goertzel.c

   1 #include <stdio.h>
   2 #include <math.h>
   3 #include <getopt.h>
   4
   5 float goertzel_mag(int numSamples,float 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) / (float)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 * cosine - q2) / scalingFactor;
  33     imag = (q1 * 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 <divisor>\tFrame size ( count = samplerate/divisor ) (default 2)\n"
  69                 "\n"
  70                 "\t-f <freq>\tAdd frequency in Hz to detect (use multiple times, default 440 Hz)\n"
  71                 "\n"
  72                 "\t-n <format>\tSet number output format\n"
  73                 "\t\tf: float\t23.4223 (default)\n"
  74                 "\t\ti: integer\t23\n"
  75                 "\t\tb: binary\t(0|1)\n"
  76                 "\t\tB: Boolean\t(false|true)\n"
  77                 "\n"
  78                 "\t-t <treshold>\tSet treshold (used in filter, see -l) (defaults -1)\n"
  79                 "\t-l <filter>\tSet line filter\n"
  80                 "\t\tf: Falldown:\tprint only when over treshold or just crossed (default)\n"
  81                 "\t\tt: Treshold:\tprint only when over treshold\n"
  82                 "\t\tc: Crossed:\tprint only when treshold crossed\n"
  83                 "\t-u\t\tInvert\ttreshold (values under treshold will be displayed)\n"
  84                 "\n"
  85                 "\t-q\t\tQuiet mode: print only values\n"
  86                 "\n"
  87                 "\t-?\t\tPrint help\n"
  88                 "\n"
  89         );
  90
  91         printf(
  92                 "Usage examples:\n"
  93                 "\tarecord | %s\n"
  94                 "\tsox input.mp3 -b 8 -c 1 -r 8000 -t wav - | %s\n"
  95                 "\t%s -n -q -l -r 8000 -d 20 -t $tresh -f 697 [-f 770 ...]\n"
  96                 "\n"
  97                 ,argv[0],argv[0],argv[0]
  98         );
  99
 100         printf(
 101                 "Frequencies for DTMF decoding:\n"
 102                 "\t-f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10\n"
 103         );
 104 }
 105
 106 void addfreq(float *freqs, float freq) {
 107         int i = 0;
 108         while(freqs[i]!=-1) i++;
 109         freqs[i]=freq;
 110         freqs[i+1]=-1;
 111 }
 112
 113 int main(int argc, char ** argv) {
 114         int samplerate = 8000;
 115         int samplecount = 4000;
 116
 117         int treshold = -1;
 118         char filter = 0;
 119         char under = 0;
 120
 121         char format=0;
 122         char verbose=1;
 123
 124         float freqs[argc+1]; freqs[0]=-1;
 125
 126
 127         float floatarg;
 128         int opt;
 129         while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:l:uq")) != -1) {
 130                 switch (opt) {
 131                         case 'i':
 132                                 freopen(optarg, "r", stdin);
 133                                 break;
 134                         case 'o':
 135                                 freopen(optarg, "w", stdout);
 136                                 break;
 137                         case 'a':
 138                                 freopen(optarg, "a", stdout);
 139                                 break;
 140                         case 'r':
 141                                 samplerate = atoi(optarg);
 142                                 break;
 143                         case 'c':
 144                                 samplecount = atoi(optarg);
 145                                 break;
 146                         case 'd':
 147                                 samplecount = samplerate/atoi(optarg);
 148                                 break;
 149                         case 'f':
 150                                 sscanf(optarg,"%f",&floatarg);
 151                                 addfreq(freqs, floatarg);
 152                                 break;
 153                         case 't':
 154                                 treshold = atoi(optarg);
 155                                 break;
 156                         case 'n':
 157                                 format = optarg[0];
 158                                 break;
 159                         case 'l':
 160                                 filter = optarg[0];
 161                                 break;
 162                         case 'u':
 163                                 under = 1;
 164                                 break;
 165                         case 'q':
 166                                 verbose = 0;
 167                                 break;
 168                         case '?':
 169                                 print_help(argv);
 170                                 return 0;
 171                                 break;
 172                 }
 173         }
 174
 175         if(freqs[0]==-1) addfreq(freqs, 440);
 176         float samples[samplecount];
 177         float position = 0;
 178
 179         if(verbose) {
 180                 fprintf(stderr,
 181                         "#Detected tone: %.2f Hz\n"
 182                         "#Samplerate: %d Hz\n"
 183                         "#Frame lenght: %d samples\n"
 184                         "#Treshold: %d\n"
 185                         "#\n"
 186                         ,freqs[0],samplerate,samplecount,treshold);
 187                 fflush(stderr);
 188
 189                 printf("#Position");
 190                 int i; for(i=0;freqs[i]!=-1;i++) {
 191                         printf("\t%2.0fHz",freqs[i]); //TODO: print decimal places
 192                 }
 193                 puts("");
 194         }
 195
 196         int i;
 197         char print=0, printnow=0;
 198         char laststate[argc]; for(i=0;freqs[i]!=-1;i++) laststate[i]=-1;
 199         while(!feof(stdin)) {
 200
 201                 //Sample data
 202                 for(i=0;i<samplecount && !feof(stdin);i++) {
 203                         unsigned char sample;
 204                         fread(&sample,1,1,stdin);
 205                         samples[i]=sample;
 206                         //printf("%d\n", sample);
 207                 }
 208
 209                 //Apply goertzel
 210                 float power[argc];
 211                 print=0;
 212                 for(i=0;freqs[i]!=-1;i++) {
 213                         power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
 214
 215                         //Decide if we will print
 216                         printnow = under ? power[i] < treshold : power[i] > treshold; //Is over/under treshold?
 217                         switch(filter) {
 218                                 case 'c': //Print if treshold crossed
 219                                         print = print || (laststate[i] != printnow);
 220                                         break;
 221                                 default:
 222                                 case 'f': //Print if over treshold or falled down
 223                                         print = print || (laststate[i] != printnow);
 224                                 case 't': //Print if over treshold
 225                                         print = print || printnow;
 226                         }
 227                         laststate[i] = printnow; //Store last state
 228                 }
 229                 fflush(stdout);
 230
 231                 //Print data
 232                 if(print) {
 233                         printf("%8.2f", position);
 234                         for(i=0;freqs[i]!=-1;i++) {
 235                                 printf("\t");
 236                                 switch(format) {
 237                                         case 'i':
 238                                                 printf("%d",(int)round(power[i]));
 239                                                 break;
 240                                         case 'b':
 241                                                 printf("%d",power[i]>treshold);
 242                                                 break;
 243                                         case 'B':
 244                                                 if(power[i]>treshold) printf("true");
 245                                                         else printf("false");
 246                                                 break;
 247                                         case 'f':
 248                                         default:
 249                                                 printf("%7.5f",power[i]);
 250                                 }
 251                         }
 252                         puts("");
 253                         fflush(stdout);
 254                 }
 255
 256                 //Increase time
 257                 position += ((float)samplecount/(float)samplerate);
 258         }
 259 }