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; | |
4b50f692 | 79 | char repeat = 1; |
59934436 H |
80 | char integers=0; |
81 | char verbose=1; | |
82 | int freqs[argc+1]; freqs[0]=-1; | |
83 | ||
84 | int opt; | |
4b50f692 | 85 | while ((opt = getopt(argc, argv, "?r:s:f:t:iqna")) != -1) { |
59934436 H |
86 | switch (opt) { |
87 | case 'r': | |
88 | samplerate = atoi(optarg); | |
89 | break; | |
90 | case 's': | |
91 | samplecount = atoi(optarg); | |
92 | break; | |
93 | case 'f': | |
94 | addfreq(freqs, atoi(optarg)); | |
95 | break; | |
96 | case 't': | |
97 | treshold = atoi(optarg); | |
98 | break; | |
99 | case 'i': | |
100 | integers = 1; | |
101 | break; | |
4b50f692 H |
102 | case 'a': |
103 | repeat = 0; | |
104 | break; | |
59934436 H |
105 | case 'n': |
106 | noreturn = 1; | |
107 | break; | |
108 | case 'q': | |
109 | verbose = 0; | |
110 | break; | |
111 | case '?': | |
112 | print_help(argv); | |
113 | return 0; | |
114 | break; | |
115 | } | |
116 | } | |
117 | ||
118 | if(freqs[0]==-1) addfreq(freqs, 440); | |
4b43521a H |
119 | float samples[samplecount]; |
120 | float position = 0; | |
59934436 H |
121 | |
122 | if(verbose) { | |
123 | fprintf(stderr, | |
124 | "#Detected tone: %d Hz\n" | |
125 | "#Samplerate: %d Hz\n" | |
126 | "#Frame lenght: %d samples\n" | |
127 | "#Treshold: %d\n" | |
128 | "#\n" | |
129 | ,freqs[0],samplerate,samplecount,treshold); | |
130 | fflush(stderr); | |
131 | ||
132 | printf("#Position"); | |
133 | int i; for(i=0;freqs[i]!=-1;i++) { | |
134 | printf("\t%2dHz",freqs[i]); | |
135 | } | |
136 | puts(""); | |
137 | } | |
138 | ||
139 | char print=0, printnow=0, printlast = 0; | |
4b43521a H |
140 | while(!feof(stdin)) { |
141 | int i; | |
59934436 H |
142 | |
143 | //Sample data | |
4b43521a H |
144 | for(i=0;i<samplecount && !feof(stdin);i++) { |
145 | unsigned char sample; | |
146 | fread(&sample,1,1,stdin); | |
147 | samples[i]=sample; | |
148 | //printf("%d\n", sample); | |
149 | } | |
59934436 H |
150 | |
151 | //Apply goertzel | |
152 | float power[argc]; | |
153 | print=0; | |
154 | for(i=0;freqs[i]!=-1;i++) { | |
155 | power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples); | |
156 | ||
157 | //Set print true if over treshold or if changed to false (print for the last time after going under treshold) | |
158 | printnow = power[i] > treshold; | |
4b50f692 | 159 | print = !(!repeat && printlast && !(!printnow)) && (print || printnow || (printlast && !noreturn)); |
59934436 H |
160 | } |
161 | printlast = printnow; | |
162 | fflush(stdout); | |
163 | ||
164 | //Print data | |
165 | if(print) { | |
166 | printf("%8.2f", position); | |
167 | for(i=0;freqs[i]!=-1;i++) { | |
168 | printf("\t"); | |
169 | if(integers) | |
170 | printf("%d",(int)power[i]); | |
171 | else | |
172 | printf("%.4f",power[i]); | |
173 | } | |
174 | puts(""); | |
175 | fflush(stdout); | |
176 | } | |
177 | ||
178 | //Increase time | |
4b43521a | 179 | position += ((float)samplecount/(float)samplerate); |
4b43521a H |
180 | } |
181 | } |