Commit | Line | Data |
---|---|---|
4b43521a H |
1 | #include <stdio.h> |
2 | #include <math.h> | |
59934436 H |
3 | #include <getopt.h> |
4 | ||
9bd0f299 | 5 | float goertzel_mag(int numSamples,float TARGET_FREQUENCY,int SAMPLING_RATE, float* data) |
4b43521a | 6 | { |
4b43521a H |
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; | |
9bd0f299 | 14 | k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / (float)SAMPLING_RATE)); |
4b43521a H |
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 | |
227018fa | 32 | real = (q1 * cosine - q2) / scalingFactor; |
599af17a | 33 | imag = (q1 * sine) / scalingFactor; |
4b43521a H |
34 | |
35 | magnitude = sqrtf(real*real + imag*imag); | |
21e17c18 | 36 | //phase = atan(imag/real) |
4b43521a H |
37 | return magnitude; |
38 | } | |
39 | ||
59934436 | 40 | void print_help(char ** argv) { |
8f751717 H |
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 | ); | |
84329d34 H |
60 | |
61 | printf( | |
62 | "Arguments:\n" | |
84329d34 H |
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" | |
7d11ab97 | 69 | "\t-d <divisor>\tFrame size ( count = samplerate/divisor ) (default 2)\n" |
84329d34 | 70 | "\n" |
7d11ab97 TM |
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" | |
84329d34 | 85 | "\n" |
84329d34 H |
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 | ); | |
59934436 H |
105 | } |
106 | ||
9bd0f299 | 107 | void addfreq(float *freqs, float freq) { |
59934436 H |
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) { | |
4b43521a H |
115 | int samplerate = 8000; |
116 | int samplecount = 4000; | |
7d11ab97 | 117 | |
59934436 | 118 | int treshold = -1; |
19a509d1 | 119 | char filter = 0; |
7d11ab97 TM |
120 | char under = 0; |
121 | ||
c9be90d5 | 122 | char format=0; |
59934436 | 123 | char verbose=1; |
7d11ab97 | 124 | |
9bd0f299 | 125 | float freqs[argc+1]; freqs[0]=-1; |
59934436 | 126 | |
9bd0f299 H |
127 | |
128 | float floatarg; | |
59934436 | 129 | int opt; |
7d11ab97 | 130 | while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:l:uq")) != -1) { |
59934436 | 131 | switch (opt) { |
c7611c89 H |
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; | |
59934436 H |
141 | case 'r': |
142 | samplerate = atoi(optarg); | |
143 | break; | |
c7611c89 | 144 | case 'c': |
59934436 H |
145 | samplecount = atoi(optarg); |
146 | break; | |
f5c8b03d H |
147 | case 'd': |
148 | samplecount = samplerate/atoi(optarg); | |
149 | break; | |
59934436 | 150 | case 'f': |
9bd0f299 H |
151 | sscanf(optarg,"%f",&floatarg); |
152 | addfreq(freqs, floatarg); | |
59934436 H |
153 | break; |
154 | case 't': | |
155 | treshold = atoi(optarg); | |
156 | break; | |
c7611c89 | 157 | case 'n': |
c9be90d5 | 158 | format = optarg[0]; |
59934436 | 159 | break; |
c7611c89 | 160 | case 'l': |
19a509d1 | 161 | filter = optarg[0]; |
59934436 | 162 | break; |
7d11ab97 TM |
163 | case 'u': |
164 | under = 1; | |
165 | break; | |
59934436 H |
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); | |
4b43521a H |
177 | float samples[samplecount]; |
178 | float position = 0; | |
59934436 H |
179 | |
180 | if(verbose) { | |
181 | fprintf(stderr, | |
9bd0f299 | 182 | "#Detected tone: %.2f Hz\n" |
59934436 H |
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++) { | |
9bd0f299 | 192 | printf("\t%2.0fHz",freqs[i]); //TODO: print decimal places |
59934436 H |
193 | } |
194 | puts(""); | |
195 | } | |
196 | ||
19a509d1 TM |
197 | int i; |
198 | char print=0, printnow=0; | |
199 | char laststate[argc]; for(i=0;freqs[i]!=-1;i++) laststate[i]=-1; | |
4b43521a | 200 | while(!feof(stdin)) { |
59934436 H |
201 | |
202 | //Sample data | |
4b43521a H |
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 | } | |
59934436 H |
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 | ||
19a509d1 | 216 | //Decide if we will print |
7d11ab97 | 217 | printnow = under ? power[i] < treshold : power[i] > treshold; //Is over/under treshold? |
19a509d1 TM |
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 | |
59934436 | 229 | } |
59934436 H |
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"); | |
c9be90d5 TM |
237 | switch(format) { |
238 | case 'i': | |
7d11ab97 | 239 | printf("%d",(int)round(power[i])); |
c9be90d5 TM |
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: | |
9bd0f299 | 250 | printf("%7.5f",power[i]); |
c9be90d5 | 251 | } |
59934436 H |
252 | } |
253 | puts(""); | |
254 | fflush(stdout); | |
255 | } | |
256 | ||
257 | //Increase time | |
4b43521a | 258 | position += ((float)samplecount/(float)samplerate); |
4b43521a H |
259 | } |
260 | } |