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 <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(int *freqs, int 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         int freqs[argc+1]; freqs[0]=-1;
 125
 126         int opt;
 127         while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:l:uq")) != -1) {
 128                 switch (opt) {
 129                         case 'i':
 130                                 freopen(optarg, "r", stdin);
 131                                 break;
 132                         case 'o':
 133                                 freopen(optarg, "w", stdout);
 134                                 break;
 135                         case 'a':
 136                                 freopen(optarg, "a", stdout);
 137                                 break;
 138                         case 'r':
 139                                 samplerate = atoi(optarg);
 140                                 break;
 141                         case 'c':
 142                                 samplecount = atoi(optarg);
 143                                 break;
 144                         case 'd':
 145                                 samplecount = samplerate/atoi(optarg);
 146                                 break;
 147                         case 'f':
 148                                 addfreq(freqs, atoi(optarg));
 149                                 break;
 150                         case 't':
 151                                 treshold = atoi(optarg);
 152                                 break;
 153                         case 'n':
 154                                 format = optarg[0];
 155                                 break;
 156                         case 'l':
 157                                 filter = optarg[0];
 158                                 break;
 159                         case 'u':
 160                                 under = 1;
 161                                 break;
 162                         case 'q':
 163                                 verbose = 0;
 164                                 break;
 165                         case '?':
 166                                 print_help(argv);
 167                                 return 0;
 168                                 break;
 169                 }
 170         }
 171
 172         if(freqs[0]==-1) addfreq(freqs, 440);
 173         float samples[samplecount];
 174         float position = 0;
 175
 176         if(verbose) {
 177                 fprintf(stderr,
 178                         "#Detected tone: %d Hz\n"
 179                         "#Samplerate: %d Hz\n"
 180                         "#Frame lenght: %d samples\n"
 181                         "#Treshold: %d\n"
 182                         "#\n"
 183                         ,freqs[0],samplerate,samplecount,treshold);
 184                 fflush(stderr);
 185
 186                 printf("#Position");
 187                 int i; for(i=0;freqs[i]!=-1;i++) {
 188                         printf("\t%2dHz",freqs[i]);
 189                 }
 190                 puts("");
 191         }
 192
 193         int i;
 194         char print=0, printnow=0;
 195         char laststate[argc]; for(i=0;freqs[i]!=-1;i++) laststate[i]=-1;
 196         while(!feof(stdin)) {
 197
 198                 //Sample data
 199                 for(i=0;i<samplecount && !feof(stdin);i++) {
 200                         unsigned char sample;
 201                         fread(&sample,1,1,stdin);
 202                         samples[i]=sample;
 203                         //printf("%d\n", sample);
 204                 }
 205
 206                 //Apply goertzel
 207                 float power[argc];
 208                 print=0;
 209                 for(i=0;freqs[i]!=-1;i++) {
 210                         power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
 211
 212                         //Decide if we will print
 213                         printnow = under ? power[i] < treshold : power[i] > treshold; //Is over/under treshold?
 214                         switch(filter) {
 215                                 case 'c': //Print if treshold crossed
 216                                         print = print || (laststate[i] != printnow);
 217                                         break;
 218                                 default:
 219                                 case 'f': //Print if over treshold or falled down
 220                                         print = print || (laststate[i] != printnow);
 221                                 case 't': //Print if over treshold
 222                                         print = print || printnow;
 223                         }
 224                         laststate[i] = printnow; //Store last state
 225                 }
 226                 fflush(stdout);
 227
 228                 //Print data
 229                 if(print) {
 230                         printf("%8.2f", position);
 231                         for(i=0;freqs[i]!=-1;i++) {
 232                                 printf("\t");
 233                                 switch(format) {
 234                                         case 'i':
 235                                                 printf("%d",(int)round(power[i]));
 236                                                 break;
 237                                         case 'b':
 238                                                 printf("%d",power[i]>treshold);
 239                                                 break;
 240                                         case 'B':
 241                                                 if(power[i]>treshold) printf("true");
 242                                                         else printf("false");
 243                                                 break;
 244                                         case 'f':
 245                                         default:
 246                                                 printf("%.4f",power[i]);
 247                                 }
 248                         }
 249                         puts("");
 250                         fflush(stdout);
 251                 }
 252
 253                 //Increase time
 254                 position += ((float)samplecount/(float)samplerate);
 255         }
 256 }