1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107 | /** Tone detect by Goertzel algorithm
*
* This program basically searches for tones (sines) in a sample and reports the different dB it finds for
* different frequencies. Can easily be extended with some thresholding to report true/false on detection.
* I'm far from certain goertzel it implemented 100% correct, but it works :)
*
* Hint, the SAMPLERATE, BUFFERSIZE, FREQUENCY, NOISE and SIGNALVOLUME all affects the outcome of the reported dB. Tweak
* em to find the settings best for your application. Also, seems to be pretty sensitive to noise (whitenoise anyway) which
* is a bit sad. Also I don't know if the goertzel really likes float values for the frequency ... And using 44100 as
* samplerate for detecting 6000 Hz tone is kinda silly I know :)
*
* Written by: Espen Riskedal, espenr@ii.uib.no, july-2002
*/
#include <iostream>
#include <cmath>
#include <cstdlib>
using std::rand;
// math stuff
using std::cos;
using std::abs;
using std::exp;
using std::log10;
// iostream stuff
using std::cout;
using std::endl;
#define PI 3.14159265358979323844
// change the defines if you want to
#define SAMPLERATE 44100
#define BUFFERSIZE 8820
#define FREQUENCY 6000
#define NOISE 0.05
#define SIGNALVOLUME 0.8
/** The Goertzel algorithm computes the k-th DFT coefficient of the input signal using a second-order filter.
* http://ptolemy.eecs.berkeley.edu/papers/96/dtmf_ict/www/node3.html.
* Basiclly it just does a DFT of the frequency we want to check, and none of the others (FFT calculates for all frequencies).
*/
float goertzel(float *x, int N, float frequency, int samplerate) {
float Skn, Skn1, Skn2;
Skn = Skn1 = Skn2 = 0;
for (int i=0; i<N; i++) {
Skn2 = Skn1;
Skn1 = Skn;
Skn = 2*cos(2*PI*frequency/samplerate)*Skn1 - Skn2 + x[i];
}
float WNk = exp(-2*PI*frequency/samplerate); // this one ignores complex stuff
//float WNk = exp(-2*j*PI*k/N);
return (Skn - WNk*Skn1);
}
/** Generates a tone of the specified frequency
* Gotten from: http://groups.google.com/groups?hl=en&lr=&ie=UTF-8&oe=UTF-8&safe=off&selm=3c641e%243jn%40uicsl.csl.uiuc.edu
*/
float *makeTone(int samplerate, float frequency, int length, float gain=1.0) {
//y(n) = 2 * cos(A) * y(n-1) - y(n-2)
//A= (frequency of interest) * 2 * PI / (sampling frequency)
//A is in radians.
// frequency of interest MUST be <= 1/2 the sampling frequency.
float *tone = new float[length];
float A = frequency*2*PI/samplerate;
for (int i=0; i<length; i++) {
if (i > 1) tone[i]= 2*cos(A)*tone[i-1] - tone[i-2];
else if (i > 0) tone[i] = 2*cos(A)*tone[i-1] - (cos(A));
else tone[i] = 2*cos(A)*cos(A) - cos(2*A);
}
for (int i=0; i<length; i++) tone[i] = tone[i]*gain;
return tone;
}
/** adds whitenoise to a sample */
void *addNoise(float *sample, int length, float gain=1.0) {
for (int i=0; i<length; i++) sample[i] += (2*(rand()/(float)RAND_MAX)-1)*gain;
}
/** returns the signal power/dB */
float power(float value) {
return 20*log10(abs(value));
}
int main(int argc, const char* argv) {
cout << "Samplerate: " << SAMPLERATE << "Hz\n";
cout << "Buffersize: " << BUFFERSIZE << " samples\n";
cout << "Correct frequency is: " << FREQUENCY << "Hz\n";
cout << " - signal volume: " << SIGNALVOLUME*100 << "%\n";
cout << " - white noise: " << NOISE*100 << "%\n";
float *tone = makeTone(SAMPLERATE, FREQUENCY, BUFFERSIZE, SIGNALVOLUME);
addNoise(tone, BUFFERSIZE,NOISE);
int stepsize = FREQUENCY/5;
for (int i=0; i<10; i++) {
int freq = stepsize*i;
cout << "Trying freq: " << freq << "Hz -> dB: " << power(goertzel(tone, BUFFERSIZE, freq, SAMPLERATE)) << endl;
}
delete tone;
return 0;
}
|