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