-
Notifications
You must be signed in to change notification settings - Fork 1
/
dsp.cpp
307 lines (274 loc) · 9.74 KB
/
dsp.cpp
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
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
#include "dsp.hpp"
#include <vector>
#define _USE_MATH_DEFINES //for msvc++ to recognize M_PI
#include <cmath>
#include <iostream>
#include <iomanip> // for tweaked printing
#ifdef PLATFORM_WINDOWS
#include <windows.h> //for rand
#else
#include <stdlib.h> //for rand
#endif
#define WINDOW_SIZE (0.1) // sliding window size
#define BARTLETT_WINDOWS (10) // num windows in bartlett's method
#define SCALING_FACTOR (1.02) // freq bin scaling factor for freq response
#define LOWEST_FREQ (15000) // lowest freq for spectral analysis
#define HIGHEST_FREQ (30000) // highest freq for spectral analysis
#define TEST_DURATION (10) // for freq_response
using namespace std;
AudioBuf tone( duration_t duration, frequency freq, duration_t delay,
unsigned int fade_samples ){
// create empty buffer
AudioBuf buf = AudioBuf( duration );
unsigned int i, end_i = buf.get_num_samples();
for( i=0; i < end_i; i++ ){
buf[i] = sin( 2*M_PI * freq * i / SAMPLE_RATE );
}
// fade in and fade out to prevent 'click' sound
for( i=0; i<fade_samples; i++ ){
float attenuation = 1.0*i/fade_samples;
buf[i] = attenuation * buf[i];
buf[end_i-1-i] = attenuation * buf[end_i-1-i];
}
return buf;
}
AudioBuf gaussian_white_noise( duration_t duration ){
AudioBuf buf = AudioBuf( duration ); // create empty buffer
unsigned int i, end_i = buf.get_num_samples();
/* inner loop taken from http://www.musicdsp.org/showone.php?id=113 */
for( i=0; i < end_i; i++ ){
float R1 = (float) rand() / (float) RAND_MAX;
float R2 = (float) rand() / (float) RAND_MAX;
buf[i] = (float) sqrt( -2.0f * log( R1 )) * cos( 2.0f * M_PI * R2 );
}
return buf;
}
AudioBuf ultrasonic_noise( duration_t duration ){
AudioBuf buf = tone( duration, 0 ); // create empty buffer;
// below, casts are needed to prevent ambiguity for msvc++
unsigned int num_tones = (log((float)HIGHEST_FREQ)-log((float)LOWEST_FREQ))
/ log((float)SCALING_FACTOR);
frequency f;
for( f = LOWEST_FREQ; f < HIGHEST_FREQ; f *= SCALING_FACTOR ){
buf.mix( tone( duration, f ) * (1.0/num_tones) );
}
return buf;
}
AudioBuf high_pass( const AudioBuf & buf, frequency half_power_freq ){
AudioBuf filtered = AudioBuf( buf.get_length() );
double rc = 1.0 / (2*M_PI*half_power_freq);
double dt = 1.0 / SAMPLE_RATE;
double alpha = rc / (rc + dt);
cout << "rc="<<rc<<" dt="<<dt<<" alpha="<<alpha<<endl;
unsigned int i;
filtered[0] = buf[0];
for( i=1; i<filtered.get_num_samples(); i++ ){
filtered[i] = alpha * (filtered[i-1] + buf[i] - buf[i-1]);
}
return filtered;
}
/** statistical mean */
template<class precision> precision mean( const vector<precision> & arr ){
unsigned int i;
precision mean=0;
for( i=0; i<arr.size(); i++ )
mean += arr[i];
return mean / arr.size();
}
/** statistical variance */
template<class precision> precision variance( const vector<precision> & arr ){
unsigned int i;
precision mu = mean( arr );
precision var = 0;
for( i=0; i<arr.size(); i++ )
var += ( arr[i] - mu ) * ( arr[i] - mu );
return var;
}
/** average inter-sample absolute difference */
template<class precision> precision delta( const vector<precision> & arr ){
unsigned int i;
precision delta=0;
for( i=1; i<arr.size(); i++ )
delta += abs( arr[i] - arr[i-1] );
return delta / arr.size();
}
ostream& operator<<(ostream& os, const Statistics& s){
os << scientific << setprecision(3) << s.mean << " " << s.delta;
/*
os<< "stat m:" <<s.mean<< "\tv:" <<s.variance<< "\td:" <<s.delta
<< "\tf:" << FEATURE(s);
*/
return os;
}
ostream& operator<< (ostream& os, Statistics& s){
os << scientific << setprecision(3) << s.mean << " " << s.delta;
/*
os<< "stat m:" <<s.mean<< "\tv:" <<s.variance<< "\td:" <<s.delta
<< "\tf:" << FEATURE(s);
*/
return os;
}
/** Geortzel's algorithm for finding the energy of a signal at a certain
frequency. Note that end_index is one past the last valid value.
Normalized frequency is measured in cycles per sample: (F/sample_rate)*/
template<class indexable,class precision>
precision goertzel( const indexable & arr, unsigned int start_index,
unsigned int end_index, precision norm_freq ){
precision s_prev = 0;
precision s_prev2 = 0;
precision coeff = 2 * cos( 2 * M_PI * norm_freq );
unsigned int i;
for( i=start_index; i<end_index; i++ ){
precision s = arr[i] + coeff*s_prev - s_prev2;
s_prev2 = s_prev;
s_prev = s;
}
// return the energy (power)
return s_prev2*s_prev2 + s_prev*s_prev - coeff*s_prev2*s_prev;
}
#ifdef GCC
//16 byte vector of 4 floats
typedef float v4sf __attribute__ ((vector_size(16)));
//16 byte vector of 4 uints
typedef unsigned int v4sd __attribute__ ((vector_size(16)));
typedef union f4vector { // wrapper for two alternative vector representations
v4sf v;
float f[4];
} f4v;
typedef union d4vector { // wrapper for two alternative vector representations
v4sd v;
unsigned int f[4];
} d4v;
/** vectorized version of goertzel that operates on four consecutive windows
simultaneously. ie, operates on arr in the index range:
[ start_index, start_index + 4*window_size ) */
template<class indexable>
f4v quad_goertzel( const indexable & arr, unsigned int start_index,
unsigned int window_size, float norm_freq ){
const v4sd one = {1, 1, 1, 1};
const v4sf zero = {0.0, 0.0, 0.0, 0.0};
f4v ret, s_prev, s_prev2;
s_prev.v = s_prev2.v = zero;
f4v coeff;
coeff.f[0]=coeff.f[1]=coeff.f[2]=coeff.f[3]= 2 * cos( 2 * M_PI * norm_freq );
d4v i; // set starting indices
i.f[0] = start_index + 0*window_size;
i.f[1] = start_index + 1*window_size;
i.f[2] = start_index + 2*window_size;
i.f[3] = start_index + 3*window_size;
for(; i.f[0]<start_index+window_size; i.v = i.v+one ){
f4v s,a;
a.f[0] = arr[i.f[0]];
a.f[1] = arr[i.f[1]];
a.f[2] = arr[i.f[2]];
a.f[3] = arr[i.f[3]];
s.v = a.v + coeff.v * s_prev.v - s_prev2.v;
s_prev2 = s_prev;
s_prev = s;
}
// return the energy (power)
ret.v = s_prev2.v * s_prev2.v + s_prev.v * s_prev.v
- coeff.v * s_prev2.v * s_prev.v;
return ret;
}
#endif
/** Bartlett's method is the mean of several runs of Goertzel's algorithm in
consecutive windows. */
template<class indexable,class precision>
precision bartlett( const indexable & arr, unsigned int start_index,
unsigned int end_index, precision norm_freq,
unsigned int num_windows ){
vector<float> energies;
unsigned int i, window_size = (end_index-start_index)/num_windows;
// for each window
for( i=0; i<num_windows; i++ ){
/*
if( i > num_windows-4 ){ // TODO: decide which is more efficient
*/
// if less than four windows left, do one at a time.
// be careful not to go beyond end_index
unsigned int j = (end_index<(i+1)*window_size)?
end_index: start_index+(i+1)*window_size;
energies.push_back( goertzel( arr, start_index + i*window_size,
j, norm_freq) );
/*
}else{
// otherwise do four at once.
f4v res = quad_goertzel( arr, start_index + i*window_size,
window_size, norm_freq );
energies.push_back( res.f[0] );
energies.push_back( res.f[1] );
energies.push_back( res.f[2] );
energies.push_back( res.f[3] );
i+=3; //skip ahead
}
*/
}
return mean( energies );
}
template<class precision>
precision bartlett( const AudioBuf & arr, precision norm_freq ){
return bartlett(arr, 0, arr.get_num_samples(), norm_freq, BARTLETT_WINDOWS);
}
Statistics measure_stats( const AudioBuf & buf, frequency freq ){
vector<float> energies;
unsigned int start, N = (unsigned int)ceil(WINDOW_SIZE*SAMPLE_RATE);
// Calculate echo intensities: use a sliding non-overlapping window
// TODO: parallelize window computations using SIMD instructions.
for( start=0; start + N < buf.get_num_samples(); start += N ){
energies.push_back( bartlett(buf,start,start+N,
(float)freq/SAMPLE_RATE,BARTLETT_WINDOWS) );
}
// calculate statistics
Statistics ret;
ret.mean = mean( energies );
ret.variance = variance( energies );
ret.delta = delta( energies );
return ret;
}
ostream& operator<<(ostream& os, const freq_response& f){
os << "freq_response{" <<endl;
unsigned int i;
for( i=0; i<f.size(); i++ ){
os << f[i].first <<"Hz \t"<< f[i].second <<endl;
}
os << '}' <<endl;
return os;
}
freq_response operator/(freq_response& a, freq_response& b){
freq_response ret;
unsigned int i;
for( i=0; i<a.size(); i++ ){
float quotient = a[i].second / b[i].second;
ret.push_back( make_pair( a[i].first, quotient) );
}
return ret;
}
freq_response test_freq_response( AudioDev & audio, AudioBuf & stimulus ){
AudioBuf rec = audio.recordback( stimulus );
return spectrum( rec );
}
pair<freq_response,freq_response> test_ultrasonic_response( AudioDev & audio ){
cout << "Please wait while the system is calibrated."<<endl
<< "This will take approximately "<<TEST_DURATION*2<<" seconds."<<endl
<< "During this time you may hear some noise."<<endl;
// record silence (as a reference point)
AudioBuf silence = tone( TEST_DURATION, 0 );
freq_response silence_response = test_freq_response( audio, silence );
// record white noise
AudioBuf noise = ultrasonic_noise(TEST_DURATION);
freq_response noise_response = test_freq_response( audio, noise );
// now print ratio of stimulated to silent energy for this range of frequencies
freq_response gain = noise_response / silence_response;
cout << gain <<endl;
return make_pair( noise_response, silence_response );
}
freq_response spectrum( AudioBuf & buf ){
freq_response response;
frequency freq;
for( freq = LOWEST_FREQ; freq <= HIGHEST_FREQ; freq *= SCALING_FACTOR ){
float energy = measure_stats(buf,freq).mean;
response.push_back( make_pair( freq, energy ) );
}
return response;
}