| 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 - 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(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 | } |