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     //phase = atan(imag/real)
  37     return magnitude;
  38 }
  39
  40 void print_help(char ** argv) {
  41         printf(
  42                 "%s takes raw (wav) audio stream and computes power (or magnitude)\n"
  43                 "of desired frequencies using Goertzel algorithm for time frames\n"
  44                 "of fixed length (specified in samples or relative to sample rate).\n"
  45                 "This can be used in various frequency detection applications\n"
  46                 "like guitar tuning, DTMF decoding and many others...\n"
  47                 "\n"
  48                 "http://en.wikipedia.org/wiki/Goertzel_algorithm\n"
  49                 "\n"
  50                 "Curently only raw unsigned 8bit (u8) mono audio is supported, but\n"
  51                 "samplerate may vary. You can convert other formats before processing.\n"
  52                 "\n"
  53                 "On lower samplerates and frame sizes this may perform sub-optimally. Eg.:\n"
  54                 "When set to detect 440Hz (at 8000Hz samplerate and ~4000 samples)\n"
  55                 "it actually detects something around 438,3Hz rather than 400Hz...\n"
  56                 "If you can't increase samplerate way around this is just to increase sensitivity.\n"
  57                 "\n"
  58                 ,argv[0]
  59         );
  60
  61         printf(
  62                 "Arguments:\n"
  63                 "\t-i <file>\tInput from file (default STDIN)\n"
  64                 "\t-o <file>\tOutput to file (default STDOUT)\n"
  65                 "\t-a <file>\tOutput to file (append) (default STDOUT)\n"
  66                 "\n"
  67                 "\t-r <samplerate>\tInput samplerate (deault 8000 Hz)\n"
  68                 "\t-c <count>\tFrame size in samples (default 4000 Samples)\n"
  69                 "\t-d <divisor>\tFrame size ( count = samplerate/divisor ) (default 2)\n"
  70                 "\n"
  71                 "\t-f <freq>\tAdd frequency in Hz to detect (use multiple times, default 440 Hz)\n"
  72                 "\n"
  73                 "\t-n <format>\tSet number output format\n"
  74                 "\t\tf: float\t23.4223 (default)\n"
  75                 "\t\ti: integer\t23\n"
  76                 "\t\tb: binary\t(0|1)\n"
  77                 "\t\tB: Boolean\t(false|true)\n"
  78                 "\n"
  79                 "\t-t <treshold>\tSet treshold (used in filter, see -l) (defaults -1)\n"
  80                 "\t-l <filter>\tSet line filter\n"
  81                 "\t\tf: Falldown:\tprint only when over treshold or just crossed (default)\n"
  82                 "\t\tt: Treshold:\tprint only when over treshold\n"
  83                 "\t\tc: Crossed:\tprint only when treshold crossed\n"
  84                 "\t-u\t\tInvert\ttreshold (values under treshold will be displayed)\n"
  85                 "\n"
  86                 "\t-q\t\tQuiet mode: print only values\n"
  87                 "\n"
  88                 "\t-?\t\tPrint help\n"
  89                 "\n"
  90         );
  91
  92         printf(
  93                 "Usage examples:\n"
  94                 "\tarecord | %s\n"
  95                 "\tsox input.mp3 -b 8 -c 1 -r 8000 -t wav - | %s\n"
  96                 "\t%s -n -q -l -r 8000 -d 20 -t $tresh -f 697 [-f 770 ...]\n"
  97                 "\n"
  98                 ,argv[0],argv[0],argv[0]
  99         );
 100
 101         printf(
 102                 "Frequencies for DTMF decoding:\n"
 103                 "\t-f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10\n"
 104         );
 105 }
 106
 107 void addfreq(float *freqs, float freq) {
 108         int i = 0;
 109         while(freqs[i]!=-1) i++;
 110         freqs[i]=freq;
 111         freqs[i+1]=-1;
 112 }
 113
 114 int main(int argc, char ** argv) {
 115         int samplerate = 8000;
 116         int samplecount = 4000;
 117
 118         int treshold = -1;
 119         char filter = 0;
 120         char under = 0;
 121
 122         char format=0;
 123         char verbose=1;
 124
 125         float freqs[argc+1]; freqs[0]=-1;
 126
 127
 128         float floatarg;
 129         int opt;
 130         while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:l:uq")) != -1) {
 131                 switch (opt) {
 132                         case 'i':
 133                                 freopen(optarg, "r", stdin);
 134                                 break;
 135                         case 'o':
 136                                 freopen(optarg, "w", stdout);
 137                                 break;
 138                         case 'a':
 139                                 freopen(optarg, "a", stdout);
 140                                 break;
 141                         case 'r':
 142                                 samplerate = atoi(optarg);
 143                                 break;
 144                         case 'c':
 145                                 samplecount = atoi(optarg);
 146                                 break;
 147                         case 'd':
 148                                 samplecount = samplerate/atoi(optarg);
 149                                 break;
 150                         case 'f':
 151                                 sscanf(optarg,"%f",&floatarg);
 152                                 addfreq(freqs, floatarg);
 153                                 break;
 154                         case 't':
 155                                 treshold = atoi(optarg);
 156                                 break;
 157                         case 'n':
 158                                 format = optarg[0];
 159                                 break;
 160                         case 'l':
 161                                 filter = optarg[0];
 162                                 break;
 163                         case 'u':
 164                                 under = 1;
 165                                 break;
 166                         case 'q':
 167                                 verbose = 0;
 168                                 break;
 169                         case '?':
 170                                 print_help(argv);
 171                                 return 0;
 172                                 break;
 173                 }
 174         }
 175
 176         if(freqs[0]==-1) addfreq(freqs, 440);
 177         float samples[samplecount];
 178         float position = 0;
 179
 180         if(verbose) {
 181                 fprintf(stderr,
 182                         "#Detected tone: %.2f Hz\n"
 183                         "#Samplerate: %d Hz\n"
 184                         "#Frame lenght: %d samples\n"
 185                         "#Treshold: %d\n"
 186                         "#\n"
 187                         ,freqs[0],samplerate,samplecount,treshold);
 188                 fflush(stderr);
 189
 190                 printf("#Position");
 191                 int i; for(i=0;freqs[i]!=-1;i++) {
 192                         printf("\t%2.0fHz",freqs[i]); //TODO: print decimal places
 193                 }
 194                 puts("");
 195         }
 196
 197         int i;
 198         char print=0, printnow=0;
 199         char laststate[argc]; for(i=0;freqs[i]!=-1;i++) laststate[i]=-1;
 200         while(!feof(stdin)) {
 201
 202                 //Sample data
 203                 for(i=0;i<samplecount && !feof(stdin);i++) {
 204                         unsigned char sample;
 205                         fread(&sample,1,1,stdin);
 206                         samples[i]=sample;
 207                         //printf("%d\n", sample);
 208                 }
 209
 210                 //Apply goertzel
 211                 float power[argc];
 212                 print=0;
 213                 for(i=0;freqs[i]!=-1;i++) {
 214                         power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
 215
 216                         //Decide if we will print
 217                         printnow = under ? power[i] < treshold : power[i] > treshold; //Is over/under treshold?
 218                         switch(filter) {
 219                                 case 'c': //Print if treshold crossed
 220                                         print = print || (laststate[i] != printnow);
 221                                         break;
 222                                 default:
 223                                 case 'f': //Print if over treshold or falled down
 224                                         print = print || (laststate[i] != printnow);
 225                                 case 't': //Print if over treshold
 226                                         print = print || printnow;
 227                         }
 228                         laststate[i] = printnow; //Store last state
 229                 }
 230                 fflush(stdout);
 231
 232                 //Print data
 233                 if(print) {
 234                         printf("%8.2f", position);
 235                         for(i=0;freqs[i]!=-1;i++) {
 236                                 printf("\t");
 237                                 switch(format) {
 238                                         case 'i':
 239                                                 printf("%d",(int)round(power[i]));
 240                                                 break;
 241                                         case 'b':
 242                                                 printf("%d",power[i]>treshold);
 243                                                 break;
 244                                         case 'B':
 245                                                 if(power[i]>treshold) printf("true");
 246                                                         else printf("false");
 247                                                 break;
 248                                         case 'f':
 249                                         default:
 250                                                 printf("%7.5f",power[i]);
 251                                 }
 252                         }
 253                         puts("");
 254                         fflush(stdout);
 255                 }
 256
 257                 //Increase time
 258                 position += ((float)samplecount/(float)samplerate);
 259         }
 260 }