Commit | Line | Data |
---|---|---|
4b43521a H |
1 | #include <stdio.h> |
2 | #include <math.h> | |
59934436 H |
3 | #include <getopt.h> |
4 | ||
4b43521a H |
5 | |
6 | /* | |
7 | Usage examples | |
8 | arecord | ./goertzel | |
9 | sox input.mp3 -b 8 -c 1 -r 8000 -t wav - | ./goertzel | |
59934436 H |
10 | |
11 | Arguments for DTMF decoding: | |
12 | -f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10 | |
4b43521a H |
13 | */ |
14 | ||
15 | float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data) | |
16 | { | |
17 | /* | |
18 | On lower samplerates and frame sizes this may perform sub-optimally. Eg.: | |
19 | When set to detect 440Hz (at 8000Hz samplerate and ~4000 samples) | |
20 | it actually detects something around 438,3Hz rather than 400Hz... | |
21 | If you can't increase samplerate way around this is just to increase sensitivity. | |
22 | */ | |
23 | ||
24 | int k,i; | |
25 | float floatnumSamples; | |
26 | float omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag; | |
27 | ||
28 | float scalingFactor = numSamples / 2.0; | |
29 | ||
30 | floatnumSamples = (float) numSamples; | |
31 | k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE)); | |
32 | omega = (2.0 * M_PI * k) / floatnumSamples; | |
33 | sine = sin(omega); | |
34 | cosine = cos(omega); | |
35 | coeff = 2.0 * cosine; | |
36 | q0=0; | |
37 | q1=0; | |
38 | q2=0; | |
39 | ||
40 | for(i=0; i<numSamples; i++) | |
41 | { | |
42 | q0 = coeff * q1 - q2 + data[i]; | |
43 | q2 = q1; | |
44 | q1 = q0; | |
45 | } | |
46 | ||
47 | // calculate the real and imaginary results | |
48 | // scaling appropriately | |
49 | real = (q1 - q2 * cosine) / scalingFactor; | |
50 | imag = (q2 * sine) / scalingFactor; | |
51 | ||
52 | magnitude = sqrtf(real*real + imag*imag); | |
53 | return magnitude; | |
54 | } | |
55 | ||
59934436 H |
56 | void print_help(char ** argv) { |
57 | printf("help me %s\n", argv[0]); | |
58 | } | |
59 | ||
60 | void addfreq(int *freqs, int freq) { | |
61 | int i = 0; | |
62 | while(freqs[i]!=-1) i++; | |
63 | freqs[i]=freq; | |
64 | freqs[i+1]=-1; | |
65 | } | |
66 | ||
67 | int main(int argc, char ** argv) { | |
4b43521a H |
68 | /* |
69 | int samples[] = {0,1,2,3,4,5,6,7,8,9,8,7,6,5,4,3,2,1}; | |
70 | int samplecount = 18; | |
71 | float power = goertzel(samplecount, samples, 1.2, 18); | |
72 | printf("G: %f\n", power); | |
73 | */ | |
74 | ||
75 | int samplerate = 8000; | |
76 | int samplecount = 4000; | |
59934436 H |
77 | int treshold = -1; |
78 | char noreturn = 0; | |
79 | char integers=0; | |
80 | char verbose=1; | |
81 | int freqs[argc+1]; freqs[0]=-1; | |
82 | ||
83 | int opt; | |
84 | while ((opt = getopt(argc, argv, "?r:s:f:t:iqn")) != -1) { | |
85 | switch (opt) { | |
86 | case 'r': | |
87 | samplerate = atoi(optarg); | |
88 | break; | |
89 | case 's': | |
90 | samplecount = atoi(optarg); | |
91 | break; | |
92 | case 'f': | |
93 | addfreq(freqs, atoi(optarg)); | |
94 | break; | |
95 | case 't': | |
96 | treshold = atoi(optarg); | |
97 | break; | |
98 | case 'i': | |
99 | integers = 1; | |
100 | break; | |
101 | case 'n': | |
102 | noreturn = 1; | |
103 | break; | |
104 | case 'q': | |
105 | verbose = 0; | |
106 | break; | |
107 | case '?': | |
108 | print_help(argv); | |
109 | return 0; | |
110 | break; | |
111 | } | |
112 | } | |
113 | ||
114 | if(freqs[0]==-1) addfreq(freqs, 440); | |
4b43521a H |
115 | float samples[samplecount]; |
116 | float position = 0; | |
59934436 H |
117 | |
118 | if(verbose) { | |
119 | fprintf(stderr, | |
120 | "#Detected tone: %d Hz\n" | |
121 | "#Samplerate: %d Hz\n" | |
122 | "#Frame lenght: %d samples\n" | |
123 | "#Treshold: %d\n" | |
124 | "#\n" | |
125 | ,freqs[0],samplerate,samplecount,treshold); | |
126 | fflush(stderr); | |
127 | ||
128 | printf("#Position"); | |
129 | int i; for(i=0;freqs[i]!=-1;i++) { | |
130 | printf("\t%2dHz",freqs[i]); | |
131 | } | |
132 | puts(""); | |
133 | } | |
134 | ||
135 | char print=0, printnow=0, printlast = 0; | |
4b43521a H |
136 | while(!feof(stdin)) { |
137 | int i; | |
59934436 H |
138 | |
139 | //Sample data | |
4b43521a H |
140 | for(i=0;i<samplecount && !feof(stdin);i++) { |
141 | unsigned char sample; | |
142 | fread(&sample,1,1,stdin); | |
143 | samples[i]=sample; | |
144 | //printf("%d\n", sample); | |
145 | } | |
59934436 H |
146 | |
147 | //Apply goertzel | |
148 | float power[argc]; | |
149 | print=0; | |
150 | for(i=0;freqs[i]!=-1;i++) { | |
151 | power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples); | |
152 | ||
153 | //Set print true if over treshold or if changed to false (print for the last time after going under treshold) | |
154 | printnow = power[i] > treshold; | |
155 | print = print || printnow || (printlast && !noreturn); | |
156 | } | |
157 | printlast = printnow; | |
158 | fflush(stdout); | |
159 | ||
160 | //Print data | |
161 | if(print) { | |
162 | printf("%8.2f", position); | |
163 | for(i=0;freqs[i]!=-1;i++) { | |
164 | printf("\t"); | |
165 | if(integers) | |
166 | printf("%d",(int)power[i]); | |
167 | else | |
168 | printf("%.4f",power[i]); | |
169 | } | |
170 | puts(""); | |
171 | fflush(stdout); | |
172 | } | |
173 | ||
174 | //Increase time | |
4b43521a | 175 | position += ((float)samplecount/(float)samplerate); |
4b43521a H |
176 | } |
177 | } |