1 #ifndef ORFANIDIS_EQ_H_
2 #define ORFANIDIS_EQ_H_
3
4 #include <math.h>
5 #include <vector>
6
7 using namespace std;
8
9 namespace orfanidis_eq {
10
11 //Eq data types.
12 typedef double eq_single_t;
13 typedef double eq_double_t;
14 //NOTE: the default float type usage
15 //can have shortage of precision
16
17 //Eq types
18 typedef enum {
19 none,
20 butterworth,
21 chebyshev1,
22 chebyshev2
23 } filter_type;
24
get_eq_text(filter_type type)25 static const char *get_eq_text(filter_type type) {
26 switch(type) {
27 case none:
28 return "not initialized";
29 case butterworth:
30 return "butterworth";
31 case chebyshev1:
32 return "chebyshev1";
33 case chebyshev2:
34 return "chebyshev2";
35 default:
36 return "none";
37 }
38 }
39
40 //Eq errors
41 typedef enum {
42 no_error,
43 invalid_input_data_error,
44 processing_error
45 } eq_error_t;
46
47 //Constants
48 static const eq_double_t pi = 3.1415926535897932384626433832795;
49 static const unsigned int fo_section_order = 4;
50
51 //Default gains
52 static const int max_base_gain_db = 0;
53 static const int min_base_gain_db = -60;
54 static const int butterworth_band_gain_db = -3;
55 static const int chebyshev1_band_base_gain_db = -6;
56 static const int chebyshev2_band_base_gain_db = -40;
57 static const int eq_min_max_gain_db = 46;
58
59 //Default freq's
60 static const eq_double_t lowest_grid_center_freq_hz = 31.25;
61 static const eq_double_t bands_grid_center_freq_hz = 1000;
62 static const eq_double_t lowest_audio_freq_hz = 20;
63 static const eq_double_t highest_audio_freq_hz = 20000;
64
65 //Eq config constants
66 static const unsigned int default_eq_band_filters_order = 4; //>2
67 static const eq_double_t default_sample_freq_hz = 48000;
68
69 //Precomputed Eq (eq2) config constants
70 static const eq_double_t p_eq_min_max_gain_db = 40;
71 static const eq_double_t p_eq_gain_step_db = 1;
72 static const eq_double_t common_base_gain_db = 3;
73 static const eq_double_t p_eq_default_gain_db = 0;
74
75 //Version
76 static const char* eq_version = "0.01";
77
78
79 //------------ Conversion functions class ------------
80 class conversions
81 {
82 int db_min_max;
83 std::vector<eq_double_t> lin_gains;
84
lin_gains_index(eq_double_t x)85 int lin_gains_index(eq_double_t x) {
86 int int_x = (int)x;
87 if((x >= -db_min_max) && (x < db_min_max - 1))
88 return db_min_max + int_x;
89
90 return db_min_max;
91 }
92
conversions()93 conversions(){}
94
95 public:
conversions(int min_max)96 conversions(int min_max) {
97 db_min_max = min_max;
98 //Update table (vector elements) for fast conversions
99 int step = -min_max;
100 while(step <= min_max)
101 lin_gains.push_back(db_2_lin(step++));
102 }
103
fast_db_2_lin(eq_double_t x)104 inline eq_double_t fast_db_2_lin(eq_double_t x) {
105 int int_part = (int)x;
106 eq_double_t frac_part = x - int_part;
107 return lin_gains[lin_gains_index(int_part)]*(1-frac_part) +
108 (lin_gains[lin_gains_index(int_part + 1)])*frac_part;
109 }
110
fast_lin_2_db(eq_double_t x)111 inline eq_double_t fast_lin_2_db(eq_double_t x) {
112 if((x >= lin_gains[0]) && (x < lin_gains[lin_gains.size() - 1])) {
113 for (unsigned int i = 0; i < lin_gains.size() - 2; i++)
114 if((x >= lin_gains[i]) && (x < lin_gains[i + 1])) {
115 int int_part = i - db_min_max;
116 eq_double_t frac_part = x - (int)(x);
117 return int_part + frac_part;
118 }
119 }
120 return 0;
121 }
122
db_2_lin(eq_double_t x)123 inline static eq_double_t db_2_lin(eq_double_t x) {
124 return pow(10, x/20);
125 }
126
lin_2_db(eq_double_t x)127 inline static eq_double_t lin_2_db(eq_double_t x) {
128 return 20*log10(x);
129 }
130
rad_2_hz(eq_double_t x,eq_double_t fs)131 inline static eq_double_t rad_2_hz(eq_double_t x, eq_double_t fs) {
132 return 2*pi/x*fs;
133 }
134
hz_2_rad(eq_double_t x,eq_double_t fs)135 inline static eq_double_t hz_2_rad(eq_double_t x, eq_double_t fs) {
136 return 2*pi*x/fs;
137 }
138 };
139
140 //------------ Band freq's structure ------------
141 struct band_freqs {
142 private:
143 band_freqs();
144
145 public:
146 eq_double_t min_freq;
147 eq_double_t center_freq;
148 eq_double_t max_freq;
149
band_freqsband_freqs150 band_freqs(eq_double_t f1, eq_double_t f0, eq_double_t f2):
151 min_freq(f1), center_freq(f0), max_freq(f2){}
152
~band_freqsband_freqs153 ~band_freqs(){}
154 };
155
156 //------------ Frequency grid class ------------
157 class freq_grid {
158 private:
159 std::vector<band_freqs> freqs_;
160
161 public:
freq_grid()162 freq_grid(){}
freq_grid(const freq_grid & fg)163 freq_grid(const freq_grid& fg){this->freqs_ = fg.freqs_;}
~freq_grid()164 ~freq_grid(){}
165
set_band(eq_double_t fmin,eq_double_t f0,eq_double_t fmax)166 eq_error_t set_band(eq_double_t fmin, eq_double_t f0, eq_double_t fmax) {
167 freqs_.clear();
168 return add_band(fmin, f0, fmax);
169 }
170
171 //fc, fmin, fmax
add_band(eq_double_t fmin,eq_double_t f0,eq_double_t fmax)172 eq_error_t add_band(eq_double_t fmin, eq_double_t f0, eq_double_t fmax) {
173 if(fmin < f0 && f0 < fmax)
174 freqs_.push_back(band_freqs(fmin, f0, fmax));
175 else
176 return invalid_input_data_error;
177 return no_error;
178 }
179
180 //f0, deltaf = fmax - fmin
add_band(eq_double_t f0,eq_double_t df)181 eq_error_t add_band(eq_double_t f0, eq_double_t df) {
182 if(f0 >= df/2)
183 {
184 eq_double_t fmin = f0 - df/2;
185 eq_double_t fmax = f0 + df/2;
186 freqs_.push_back(band_freqs(fmin, f0, fmax));
187 }
188 else
189 return invalid_input_data_error;
190 return no_error;
191 }
192
193 eq_error_t set_5_bands(eq_double_t center_freq = bands_grid_center_freq_hz) {
194 freqs_.clear();
195 if(lowest_audio_freq_hz < center_freq &&
196 center_freq < highest_audio_freq_hz) {
197
198 //Find lowest center frequency in band
199 eq_double_t lowest_center_freq = center_freq;
200 while(lowest_center_freq > lowest_grid_center_freq_hz)
201 lowest_center_freq/=4.0;
202 if(lowest_center_freq < lowest_grid_center_freq_hz)
203 lowest_center_freq*=4.0;
204
205 //Calculate freq's
206 eq_double_t f0 = lowest_center_freq;
207 for(unsigned int i = 0; i < 5 ; i++) {
208 freqs_.push_back(band_freqs(f0/2, f0, f0*2));
209 f0 *= 4;
210 }
211 }
212 else
213 return invalid_input_data_error;
214 return no_error;
215 }
216
217 eq_error_t set_10_bands(eq_double_t center_freq = bands_grid_center_freq_hz) {
218 freqs_.clear();
219 if(lowest_audio_freq_hz < center_freq &&
220 center_freq < highest_audio_freq_hz) {
221
222 //Find lowest center frequency in band
223 eq_double_t lowest_center_freq = center_freq;
224 while(lowest_center_freq > lowest_grid_center_freq_hz)
225 lowest_center_freq/=2;
226 if(lowest_center_freq < lowest_grid_center_freq_hz)
227 lowest_center_freq*=2;
228
229 //Calculate freq's
230 eq_double_t f0 = lowest_center_freq;
231 for(unsigned int i = 0; i < 10; i++) {
232 freqs_.push_back(band_freqs(f0/pow(2,0.5), f0, f0*pow(2,0.5)));
233 f0 *= 2;
234 }
235 }
236 else
237 return invalid_input_data_error;
238 return no_error;
239 }
240
241 eq_error_t set_20_bands(eq_double_t center_freq = bands_grid_center_freq_hz) {
242 freqs_.clear();
243 if(lowest_audio_freq_hz < center_freq &&
244 center_freq < highest_audio_freq_hz) {
245
246 //Find lowest center frequency in band
247 eq_double_t lowest_center_freq = center_freq;
248 while(lowest_center_freq > lowest_audio_freq_hz)
249 lowest_center_freq/=pow(2,0.5);
250 if(lowest_center_freq < lowest_audio_freq_hz)
251 lowest_center_freq*=pow(2,0.5);
252
253 //Calculate freq's
254 eq_double_t f0 = lowest_center_freq;
255 for(unsigned int i = 0; i < 20; i++) {
256 freqs_.push_back(band_freqs(f0/pow(2,0.25),
257 f0, f0*pow(2,0.25)));
258 f0 *= pow(2,0.5);
259 }
260 }
261 else
262 return invalid_input_data_error;
263 return no_error;
264 }
265
266 eq_error_t set_30_bands(eq_double_t center_freq = bands_grid_center_freq_hz) {
267 freqs_.clear();
268 if(lowest_audio_freq_hz < center_freq &&
269 center_freq < highest_audio_freq_hz) {
270
271 //Find lowest center frequency in band
272 eq_double_t lowest_center_freq = center_freq;
273 while(lowest_center_freq > lowest_audio_freq_hz)
274 lowest_center_freq/=pow(2.0,1.0/3.0);
275 if(lowest_center_freq < lowest_audio_freq_hz)
276 lowest_center_freq*=pow(2.0,1.0/3.0);
277
278 //Calculate freq's
279 eq_double_t f0 = lowest_center_freq;
280 for(unsigned int i = 0; i < 30; i++) {
281 freqs_.push_back(band_freqs(f0/pow(2.0,1.0/6.0),
282 f0, f0*pow(2.0,1.0/6.0)));
283 f0 *= pow(2,1.0/3.0);
284 }
285 }
286 else
287 return invalid_input_data_error;
288 return no_error;
289 }
290
get_number_of_bands()291 unsigned int get_number_of_bands(){return freqs_.size();}
292
get_freqs()293 std::vector<band_freqs> get_freqs(){return freqs_;}
294
get_freq(unsigned int number)295 unsigned int get_freq(unsigned int number) {
296 if(number < freqs_.size())
297 return freqs_[number].center_freq;
298 else
299 return 0;
300 }
301
get_rounded_freq(unsigned int number)302 unsigned int get_rounded_freq(unsigned int number) {
303 if(number < freqs_.size()) {
304 unsigned int freq = freqs_[number].center_freq;
305 if (freq < 100)
306 return freq;
307 else if(freq >= 100 && freq < 1000) {
308 unsigned int rest = freq%10;
309 if(rest < 5)
310 return freq - rest;
311 else
312 return freq - rest + 10;
313 }
314 else if(freq >= 1000 && freq < 10000) {
315 unsigned int rest = freq%100;
316 if(rest < 50)
317 return freq - rest;
318 else
319 return freq - rest + 100;
320 }
321 else if(freq >= 10000) {
322 unsigned int rest = freq%1000;
323 if(rest < 500)
324 return freq - rest;
325 else
326 return freq - rest + 1000;
327 }
328 }
329 return 0;
330 }
331 };
332
333 //------------ Forth order sections ------------
334 class fo_section {
335 protected:
336 eq_single_t b0; eq_single_t b1; eq_single_t b2; eq_single_t b3; eq_single_t b4;
337 eq_single_t a0; eq_single_t a1; eq_single_t a2; eq_single_t a3; eq_single_t a4;
338
339 eq_single_t numBuf[fo_section_order];
340 eq_single_t denumBuf[fo_section_order];
341
df1_fo_process(eq_single_t in)342 inline eq_single_t df1_fo_process(eq_single_t in) {
343 eq_single_t out = 0;
344 out+= b0*in;
345 out+= (b1*numBuf[0] - denumBuf[0]*a1);
346 out+= (b2*numBuf[1] - denumBuf[1]*a2);
347 out+= (b3*numBuf[2] - denumBuf[2]*a3);
348 out+= (b4*numBuf[3] - denumBuf[3]*a4);
349
350 numBuf[3] = numBuf[2];
351 numBuf[2] = numBuf[1];
352 numBuf[1] = numBuf[0];
353 *numBuf = in;
354
355 denumBuf[3] = denumBuf[2];
356 denumBuf[2] = denumBuf[1];
357 denumBuf[1] = denumBuf[0];
358 *denumBuf = out;
359
360 return(out);
361 }
362
363 public:
fo_section()364 fo_section() {
365 b0 = 1; b1 = 0; b2 = 0; b3 = 0; b4 = 0;
366 a0 = 1; a1 = 0; a2 = 0; a3 = 0; a4 = 0;
367
368 for(unsigned int i = 0; i < fo_section_order; i++) {
369 numBuf[i] = 0;
370 denumBuf[i] = 0;
371 }
372 }
373
~fo_section()374 virtual ~fo_section(){}
375
process(eq_single_t in)376 eq_single_t process(eq_single_t in) {
377 return df1_fo_process(in);
378 }
379
get()380 virtual fo_section get() {
381 return *this;
382 }
383 };
384
385 class butterworth_fo_section : public fo_section {
butterworth_fo_section()386 butterworth_fo_section(){}
butterworth_fo_section(butterworth_fo_section &)387 butterworth_fo_section(butterworth_fo_section&){}
388 public:
butterworth_fo_section(eq_double_t beta,eq_double_t s,eq_double_t g,eq_double_t g0,eq_double_t D,eq_double_t c0)389 butterworth_fo_section(eq_double_t beta,
390 eq_double_t s, eq_double_t g, eq_double_t g0,
391 eq_double_t D, eq_double_t c0) {
392 b0 = (g*g*beta*beta + 2*g*g0*s*beta + g0*g0)/D;
393 b1 = -4*c0*(g0*g0 + g*g0*s*beta)/D;
394 b2 = 2*(g0*g0*(1 + 2*c0*c0) - g*g*beta*beta)/D;
395 b3 = -4*c0*(g0*g0 - g*g0*s*beta)/D;
396 b4 = (g*g*beta*beta - 2*g*g0*s*beta + g0*g0)/D;
397
398 a0 = 1;
399 a1 = -4*c0*(1 + s*beta)/D;
400 a2 = 2*(1 + 2*c0*c0 - beta*beta)/D;
401 a3 = -4*c0*(1 - s*beta)/D;
402 a4 = (beta*beta - 2*s*beta + 1)/D;
403 }
404
get()405 fo_section get() {return *this;}
406 };
407
408 class chebyshev_type1_fo_section : public fo_section {
chebyshev_type1_fo_section()409 chebyshev_type1_fo_section(){}
chebyshev_type1_fo_section(chebyshev_type1_fo_section &)410 chebyshev_type1_fo_section(chebyshev_type1_fo_section&){}
411 public:
chebyshev_type1_fo_section(eq_double_t a,eq_double_t c,eq_double_t tetta_b,eq_double_t g0,eq_double_t s,eq_double_t b,eq_double_t D,eq_double_t c0)412 chebyshev_type1_fo_section(eq_double_t a,
413 eq_double_t c, eq_double_t tetta_b,
414 eq_double_t g0, eq_double_t s, eq_double_t b,
415 eq_double_t D, eq_double_t c0) {
416 b0 = ((b*b + g0*g0*c*c)*tetta_b*tetta_b + 2*g0*b*s*tetta_b + g0*g0)/D;
417 b1 = -4*c0*(g0*g0 + g0*b*s*tetta_b)/D;
418 b2 = 2*(g0*g0*(1 + 2*c0*c0) - (b*b + g0*g0*c*c)*tetta_b*tetta_b)/D;
419 b3 = -4*c0*(g0*g0 - g0*b*s*tetta_b)/D;
420 b4 = ((b*b + g0*g0*c*c)*tetta_b*tetta_b - 2*g0*b*s*tetta_b + g0*g0)/D;
421
422 a0 = 1;
423 a1 = -4*c0*(1 + a*s*tetta_b)/D;
424 a2 = 2*(1 + 2*c0*c0 - (a*a + c*c)*tetta_b*tetta_b)/D;
425 a3 = -4*c0*(1 - a*s*tetta_b)/D;
426 a4 = ((a*a + c*c)*tetta_b*tetta_b - 2*a*s*tetta_b + 1)/D;
427 }
428
get()429 fo_section get() {return *this;}
430 };
431
432 class chebyshev_type2_fo_section : public fo_section {
chebyshev_type2_fo_section()433 chebyshev_type2_fo_section(){}
chebyshev_type2_fo_section(chebyshev_type2_fo_section &)434 chebyshev_type2_fo_section(chebyshev_type2_fo_section&){}
435 public:
chebyshev_type2_fo_section(eq_double_t a,eq_double_t c,eq_double_t tetta_b,eq_double_t g,eq_double_t s,eq_double_t b,eq_double_t D,eq_double_t c0)436 chebyshev_type2_fo_section(eq_double_t a,
437 eq_double_t c, eq_double_t tetta_b,
438 eq_double_t g, eq_double_t s, eq_double_t b,
439 eq_double_t D, eq_double_t c0) {
440 b0 = (g*g*tetta_b*tetta_b + 2*g*b*s*tetta_b + b*b + g*g*c*c)/D;
441 b1 = -4*c0*(b*b + g*g*c*c + g*b*s*tetta_b)/D;
442 b2 = 2*((b*b + g*g*c*c)*(1 + 2*c0*c0) - g*g*tetta_b*tetta_b)/D;
443 b3 = -4*c0*(b*b + g*g*c*c - g*b*s*tetta_b)/D;
444 b4 = (g*g*tetta_b*tetta_b - 2*g*b*s*tetta_b + b*b + g*g*c*c)/D;
445
446 a0 = 1;
447 a1 = -4*c0*(a*a + c*c + a*s*tetta_b)/D;
448 a2 = 2*((a*a + c*c)*(1 + 2*c0*c0) - tetta_b*tetta_b)/D;
449 a3 = -4*c0*(a*a + c*c - a*s*tetta_b)/D;
450 a4 = (tetta_b*tetta_b - 2*a*s*tetta_b + a*a + c*c)/D;
451 }
452
get()453 fo_section get() {return *this;}
454 };
455
456 //------------ Bandpass filters ------------
457 class bp_filter {
458 public:
bp_filter()459 bp_filter(){}
~bp_filter()460 virtual ~bp_filter(){}
461
462 virtual eq_single_t process(eq_single_t in) = 0;
463 };
464
465 class butterworth_bp_filter : public bp_filter {
466 private:
467 std::vector<fo_section> sections_;
468
butterworth_bp_filter()469 butterworth_bp_filter(){}
470 public:
butterworth_bp_filter(butterworth_bp_filter & f)471 butterworth_bp_filter(butterworth_bp_filter& f) {
472 this->sections_ = f.sections_;
473 }
474
butterworth_bp_filter(unsigned int N,eq_double_t w0,eq_double_t wb,eq_double_t G,eq_double_t Gb,eq_double_t G0)475 butterworth_bp_filter(unsigned int N,
476 eq_double_t w0, eq_double_t wb,
477 eq_double_t G, eq_double_t Gb, eq_double_t G0) {
478 //Case if G == 0 : allpass
479 if(G == 0 && G0 == 0) {
480 sections_.push_back(fo_section());
481 return;
482 }
483
484 //Get number of analog sections
485 unsigned int r = N%2;
486 unsigned int L = (N - r)/2;
487
488 //Convert gains to linear scale
489 G = conversions::db_2_lin(G);
490 Gb = conversions::db_2_lin(Gb);
491 G0 = conversions::db_2_lin(G0);
492
493 eq_double_t epsilon = pow(((eq_double_t)(G*G - Gb*Gb))/
494 (Gb*Gb - G0*G0),0.5);
495 eq_double_t g = pow(((eq_double_t)G),1.0/((eq_double_t)N));
496 eq_double_t g0 = pow(((eq_double_t)G0),1.0/((eq_double_t)N));
497 eq_double_t beta = pow(((eq_double_t)epsilon), -1.0/((eq_double_t)N))*
498 tan(wb/2.0);
499
500 eq_double_t c0 = cos(w0);
501 if (w0 == 0) c0 = 1;
502 if (w0 == pi/2) c0=0;
503 if (w0 == pi) c0 =- 1;
504
505 //Calculate every section
506 for(unsigned int i = 1; i <= L; i++) {
507 eq_double_t ui = (2.0*i-1)/N;
508 eq_double_t si = sin(pi*ui/2.0);
509
510 eq_double_t Di = beta*beta + 2*si*beta + 1;
511
512 sections_.push_back
513 (butterworth_fo_section(beta, si, g, g0, Di, c0));
514 }
515 }
516
~butterworth_bp_filter()517 ~butterworth_bp_filter(){}
518
compute_bw_gain_db(eq_single_t gain)519 static eq_single_t compute_bw_gain_db(eq_single_t gain) {
520 eq_single_t bw_gain = 0;
521 if(gain <= -6)
522 bw_gain = gain + common_base_gain_db;
523 else if(gain > -6 && gain < 6)
524 bw_gain = gain*0.5;
525 else if(gain >= 6)
526 bw_gain = gain - common_base_gain_db;
527
528 return bw_gain;
529 }
530
process(eq_single_t in)531 virtual eq_single_t process(eq_single_t in) {
532 eq_single_t p0 = in;
533 eq_single_t p1 = 0;
534 //Process FO sections in serial connection
535 for(unsigned int i = 0; i < sections_.size(); i++) {
536 p1 = sections_[i].process(p0);
537 p0 = p1;
538 }
539
540 return p1;
541 }
542 };
543
544 class chebyshev_type1_bp_filter : public bp_filter {
545 private:
546 std::vector<fo_section> sections_;
547
chebyshev_type1_bp_filter()548 chebyshev_type1_bp_filter(){}
549 public:
chebyshev_type1_bp_filter(unsigned int N,eq_double_t w0,eq_double_t wb,eq_double_t G,eq_double_t Gb,eq_double_t G0)550 chebyshev_type1_bp_filter(unsigned int N,
551 eq_double_t w0, eq_double_t wb,
552 eq_double_t G, eq_double_t Gb, eq_double_t G0) {
553 //Case if G == 0 : allpass
554 if(G == 0 && G0 == 0) {
555 sections_.push_back(fo_section());
556 return;
557 }
558
559 //Get number of analog sections
560 unsigned int r = N%2;
561 unsigned int L = (N - r)/2;
562
563 //Convert gains to linear scale
564 G = conversions::db_2_lin(G);
565 Gb = conversions::db_2_lin(Gb);
566 G0 = conversions::db_2_lin(G0);
567
568 eq_double_t epsilon = pow((eq_double_t)(G*G - Gb*Gb)/
569 (Gb*Gb - G0*G0),0.5);
570 eq_double_t g0 = pow((eq_double_t)(G0),1.0/N);
571 eq_double_t alfa =
572 pow(1.0/epsilon + pow(1 + pow(epsilon,-2.0),0.5),1.0/N);
573 eq_double_t beta =
574 pow(G/epsilon + Gb*pow(1 + pow(epsilon,-2.0),0.5),1.0/N);
575 eq_double_t a = 0.5*(alfa - 1.0/alfa);
576 eq_double_t b = 0.5*(beta - g0*g0*(1/beta));
577 eq_double_t tetta_b = tan(wb/2);
578
579 eq_double_t c0 = cos(w0);
580 if (w0 == 0) c0 = 1;
581 if (w0 == pi/2) c0=0;
582 if (w0 == pi) c0 =- 1;
583
584 //Calculate every section
585 for(unsigned int i = 1; i <= L; i++) {
586 eq_double_t ui = (2.0*i-1.0)/N;
587 eq_double_t ci = cos(pi*ui/2.0);
588 eq_double_t si = sin(pi*ui/2.0);
589
590 eq_double_t Di = (a*a + ci*ci)*tetta_b*tetta_b +
591 2.0*a*si*tetta_b + 1;
592 sections_.push_back(
593 chebyshev_type1_fo_section(a, ci, tetta_b, g0, si, b, Di, c0));
594 }
595 }
596
597
~chebyshev_type1_bp_filter()598 ~chebyshev_type1_bp_filter(){}
599
compute_bw_gain_db(eq_single_t gain)600 static eq_single_t compute_bw_gain_db(eq_single_t gain) {
601 eq_single_t bw_gain = 0;
602 if(gain <= -6)
603 bw_gain = gain + 1;
604 else if(gain > -6 && gain < 6)
605 bw_gain = gain*0.9;
606 else if(gain >= 6)
607 bw_gain = gain - 1;
608
609 return bw_gain;
610 }
611
process(eq_single_t in)612 eq_single_t process(eq_single_t in) {
613 eq_single_t p0 = in;
614 eq_single_t p1 = 0;
615 //Process FO sections in serial connection
616 for(unsigned int i = 0; i < sections_.size(); i++) {
617 p1 = sections_[i].process(p0);
618 p0 = p1;
619 }
620
621 return p1;
622 }
623 };
624
625 class chebyshev_type2_bp_filter : public bp_filter {
626 private:
627 std::vector<fo_section> sections_;
628
chebyshev_type2_bp_filter()629 chebyshev_type2_bp_filter(){}
630 public:
chebyshev_type2_bp_filter(unsigned int N,eq_double_t w0,eq_double_t wb,eq_double_t G,eq_double_t Gb,eq_double_t G0)631 chebyshev_type2_bp_filter(unsigned int N,
632 eq_double_t w0, eq_double_t wb,
633 eq_double_t G, eq_double_t Gb, eq_double_t G0) {
634 //Case if G == 0 : allpass
635 if(G == 0 && G0 == 0) {
636 sections_.push_back(fo_section());
637 return;
638 }
639
640 //Get number of analog sections
641 unsigned int r = N%2;
642 unsigned int L = (N - r)/2;
643
644 //Convert gains to linear scale
645 G = conversions::db_2_lin(G);
646 Gb = conversions::db_2_lin(Gb);
647 G0 = conversions::db_2_lin(G0);
648
649 eq_double_t epsilon = pow((eq_double_t)((G*G - Gb*Gb)/
650 (Gb*Gb - G0*G0)),0.5);
651 eq_double_t g = pow((eq_double_t)(G),1.0/N);
652 eq_double_t eu = pow(epsilon + sqrt(1 + epsilon*epsilon), 1.0/N);
653 eq_double_t ew = pow(G0*epsilon + Gb*sqrt(1 + epsilon*epsilon), 1.0/N);
654 eq_double_t a = (eu - 1.0/eu)/2.0;
655 eq_double_t b = (ew - g*g/ew)/2.0;
656 eq_double_t tetta_b = tan(wb/2);
657
658 eq_double_t c0 = cos(w0);
659 if (w0 == 0) c0 = 1;
660 if (w0 == pi/2) c0=0;
661 if (w0 == pi) c0 =- 1;
662
663 //Calculate every section
664 for(unsigned int i = 1; i <= L; i++) {
665 eq_double_t ui = (2.0*i-1.0)/N;
666 eq_double_t ci = cos(pi*ui/2.0);
667 eq_double_t si = sin(pi*ui/2.0);
668 eq_double_t Di = tetta_b*tetta_b + 2*a*si*tetta_b + a*a + ci*ci;
669
670 sections_.push_back(
671 chebyshev_type2_fo_section(a, ci, tetta_b, g, si, b, Di, c0));
672 }
673 }
674
~chebyshev_type2_bp_filter()675 ~chebyshev_type2_bp_filter(){}
676
compute_bw_gain_db(eq_single_t gain)677 static eq_single_t compute_bw_gain_db(eq_single_t gain) {
678 eq_single_t bw_gain = 0;
679 if(gain <= -6)
680 bw_gain = -common_base_gain_db;
681 else if(gain > -6 && gain < 6)
682 bw_gain = gain*0.3;
683 else if(gain >= 6)
684 bw_gain = common_base_gain_db;
685
686 return bw_gain;
687 }
688
process(eq_single_t in)689 eq_single_t process(eq_single_t in) {
690 eq_single_t p0 = in;
691 eq_single_t p1 = 0;
692
693 //Process FO sections in serial connection
694 for(unsigned int i = 0; i < sections_.size(); i++) {
695 p1 = sections_[i].process(p0);
696 p0 = p1;
697 }
698
699 return p1;
700 }
701 };
702
703 // ------------ eq1 ------------
704 // Equalizer with single precomputed filter for every band
705 class eq1 {
706 private:
707 conversions conv_;
708 eq_double_t sampling_frequency_;
709 freq_grid freq_grid_;
710 std::vector<eq_single_t> band_gains_;
711 std::vector<bp_filter*> filters_;
712 filter_type current_eq_type_;
713
eq1()714 eq1():conv_(eq_min_max_gain_db){}
eq1(const eq1 &)715 eq1(const eq1&):conv_(eq_min_max_gain_db){}
716
cleanup_filters_array()717 void cleanup_filters_array() {
718 for(unsigned int j = 0; j < filters_.size(); j++)
719 delete filters_[j];
720 }
721
722 public:
eq1(const freq_grid & fg,filter_type eq_t)723 eq1(const freq_grid &fg, filter_type eq_t) : conv_(eq_min_max_gain_db) {
724 sampling_frequency_ = default_sample_freq_hz;
725 freq_grid_ = fg;
726 current_eq_type_ = eq_t;
727 set_eq(freq_grid_, eq_t);
728 }
~eq1()729 ~eq1(){cleanup_filters_array();}
730
set_eq(freq_grid & fg,filter_type eqt)731 eq_error_t set_eq(freq_grid& fg, filter_type eqt) {
732 band_gains_.clear();
733 cleanup_filters_array();
734 filters_.clear();
735 freq_grid_ = fg;
736
737 for(unsigned int i = 0; i < freq_grid_.get_number_of_bands(); i++) {
738
739 eq_double_t wb = conversions::hz_2_rad(
740 freq_grid_.get_freqs()[i].max_freq -
741 freq_grid_.get_freqs()[i].min_freq,
742 sampling_frequency_);
743
744 eq_double_t w0 = conversions::hz_2_rad(
745 freq_grid_.get_freqs()[i].center_freq,
746 sampling_frequency_);
747
748 switch(eqt) {
749 case (butterworth): {
750 butterworth_bp_filter* bf =
751 new butterworth_bp_filter(
752 default_eq_band_filters_order,
753 w0,
754 wb,
755 max_base_gain_db,
756 butterworth_band_gain_db,
757 min_base_gain_db
758 );
759
760 filters_.push_back(bf);
761 break;
762 }
763
764 case (chebyshev1): {
765 chebyshev_type1_bp_filter* cf1 =
766 new chebyshev_type1_bp_filter(
767 default_eq_band_filters_order,
768 w0,
769 wb,
770 max_base_gain_db,
771 chebyshev1_band_base_gain_db,
772 min_base_gain_db
773 );
774
775 filters_.push_back(cf1);
776 break;
777 }
778
779 case (chebyshev2): {
780 chebyshev_type2_bp_filter* cf2 =
781 new chebyshev_type2_bp_filter(
782 default_eq_band_filters_order,
783 w0,
784 wb,
785 max_base_gain_db,
786 chebyshev2_band_base_gain_db,
787 min_base_gain_db
788 );
789
790 filters_.push_back(cf2);
791 break;
792 }
793
794 default:
795 current_eq_type_ = none;
796 return invalid_input_data_error;
797
798 }
799 band_gains_.push_back(max_base_gain_db);
800 }
801
802 current_eq_type_ = eqt;
803 return no_error;
804 }
805
set_eq(filter_type eqt)806 eq_error_t set_eq(filter_type eqt)
807 {
808 return set_eq(freq_grid_, eqt);
809 }
810
set_sample_rate(eq_double_t sr)811 eq_error_t set_sample_rate(eq_double_t sr) {
812 eq_error_t err = no_error;
813 sampling_frequency_ = sr;
814 err = set_eq(freq_grid_, current_eq_type_);
815
816 return err;
817 }
818
change_gains(std::vector<eq_single_t> band_gains)819 eq_error_t change_gains(std::vector<eq_single_t> band_gains) {
820 if(band_gains_.size() == band_gains.size())
821 band_gains_ = band_gains;
822 else
823 return invalid_input_data_error;
824
825 return no_error;
826 }
827
change_gains_db(std::vector<eq_single_t> band_gains)828 eq_error_t change_gains_db(std::vector<eq_single_t> band_gains) {
829 if(band_gains_.size() == band_gains.size())
830 for(unsigned int j = 0; j < get_number_of_bands(); j++)
831 band_gains_[j] = conv_.fast_db_2_lin(band_gains[j]);
832 else
833 return invalid_input_data_error;
834
835 return no_error;
836 }
837
change_band_gain(unsigned int band_number,eq_single_t band_gain)838 eq_error_t change_band_gain(unsigned int band_number,
839 eq_single_t band_gain) {
840 if(band_number < get_number_of_bands())
841 band_gains_[band_number] = band_gain;
842 else
843 return invalid_input_data_error;
844
845 return no_error;
846 }
847
change_band_gain_db(unsigned int band_number,eq_single_t band_gain)848 eq_error_t change_band_gain_db(unsigned int band_number,
849 eq_single_t band_gain) {
850 if(band_number < get_number_of_bands())
851 band_gains_[band_number] = conv_.fast_db_2_lin(band_gain);
852 else
853 return invalid_input_data_error;
854
855 return no_error;
856 }
857
sbs_process_band(unsigned int band_number,eq_single_t * in,eq_single_t * out)858 eq_error_t sbs_process_band(unsigned int band_number,
859 eq_single_t *in, eq_single_t *out) {
860 if(band_number < get_number_of_bands())
861 *out = band_gains_[band_number]*
862 filters_[band_number]->process(*in);
863 else
864 return invalid_input_data_error;
865
866 return no_error;
867 }
868
sbs_process(eq_single_t * in,eq_single_t * out)869 eq_error_t sbs_process(eq_single_t *in, eq_single_t *out) {
870 eq_error_t err = no_error;
871 eq_single_t acc_out = 0;
872 for(unsigned int j = 0; j < get_number_of_bands(); j++) {
873 eq_single_t band_out = 0;
874 err = sbs_process_band(j, in, &band_out);
875 acc_out += band_out;
876 }
877 *out = acc_out;
878
879 return err;
880 }
881
get_eq_type()882 filter_type get_eq_type(){return current_eq_type_;}
get_string_eq_type()883 const char* get_string_eq_type(){return get_eq_text(current_eq_type_);}
get_number_of_bands()884 unsigned int get_number_of_bands() {
885 return freq_grid_.get_number_of_bands();
886 }
get_version()887 const char* get_version(){return eq_version;}
888 };
889
890 //!!! New functionality
891
892 // ------------ eq_channel ------------
893 // Precomputed equalizer channel,
894 // consists of vector of filters for every gain value
895 class eq_channel
896 {
897 eq_single_t f0_;
898 eq_single_t fb_;
899 eq_single_t sampling_frequency_;
900 eq_single_t min_max_gain_db_;
901 eq_single_t gain_step_db_;
902
903 unsigned int current_filter_index_;
904 eq_single_t current_gain_db_;
905
906 std::vector<bp_filter*> filters_;
907 filter_type current_channel_type_;
908
eq_channel()909 eq_channel(){}
910
get_flt_index(eq_single_t gain_db)911 unsigned int get_flt_index(eq_single_t gain_db) {
912 unsigned int number_of_filters = filters_.size();
913 eq_single_t scale_coef = gain_db/min_max_gain_db_;
914 return (number_of_filters/2) + (number_of_filters/2)*scale_coef;
915 }
916
cleanup_filters_array()917 void cleanup_filters_array() {
918 for(unsigned int j = 0; j < filters_.size(); j++)
919 delete filters_[j];
920 }
921
922 public:
923 eq_channel(filter_type ft,
924 eq_single_t fs, eq_single_t f0, eq_single_t fb,
925 eq_single_t min_max_gain_db = p_eq_min_max_gain_db,
926 eq_single_t step_db = p_eq_gain_step_db) {
927
928 //Init data fields
929 sampling_frequency_ = fs;
930 f0_ = f0;
931 fb_ = fb;
932 min_max_gain_db_ = min_max_gain_db;
933 gain_step_db_ = step_db;
934
935 current_gain_db_ = 0;
936 current_filter_index_ = 0;
937
938 current_channel_type_ = ft;
939
940 set_channel(current_channel_type_, sampling_frequency_);
941 }
942
~eq_channel()943 ~eq_channel(){cleanup_filters_array();}
944
set_channel(filter_type ft,eq_single_t fs)945 eq_error_t set_channel(filter_type ft, eq_single_t fs) {
946
947 eq_double_t wb = conversions::hz_2_rad(fb_, sampling_frequency_);
948 eq_double_t w0 = conversions::hz_2_rad(f0_, sampling_frequency_);
949
950 for(eq_single_t gain = -min_max_gain_db_; gain <= min_max_gain_db_;
951 gain+= gain_step_db_) {
952
953 switch(ft) {
954 case (butterworth): {
955 eq_single_t bw_gain =
956 butterworth_bp_filter::compute_bw_gain_db(gain);
957
958 butterworth_bp_filter* bf =
959 new butterworth_bp_filter(
960 default_eq_band_filters_order,
961 w0,
962 wb,
963 gain,
964 bw_gain,
965 p_eq_default_gain_db
966 );
967
968 filters_.push_back(bf);
969 break;
970 }
971 case (chebyshev1): {
972 eq_single_t bw_gain =
973 chebyshev_type1_bp_filter::compute_bw_gain_db(gain);
974
975 chebyshev_type1_bp_filter* cf1 =
976 new chebyshev_type1_bp_filter(
977 default_eq_band_filters_order,
978 w0,
979 wb,
980 gain,
981 bw_gain,
982 p_eq_default_gain_db
983 );
984
985 filters_.push_back(cf1);
986 break;
987 }
988 case (chebyshev2): {
989 eq_single_t bw_gain =
990 chebyshev_type2_bp_filter::compute_bw_gain_db(gain);
991
992 chebyshev_type2_bp_filter* cf2 =
993 new chebyshev_type2_bp_filter(
994 default_eq_band_filters_order,
995 w0,
996 wb,
997 gain,
998 bw_gain,
999 p_eq_default_gain_db
1000 );
1001
1002 filters_.push_back(cf2);
1003 break;
1004 }
1005 default: {
1006 current_channel_type_ = none;
1007 return invalid_input_data_error;
1008 }
1009 }
1010 }
1011
1012 //Get current filter index
1013 current_gain_db_ = 0;
1014 current_filter_index_ = get_flt_index(current_gain_db_);
1015
1016 return no_error;
1017 }
1018
set_gain_db(eq_single_t db)1019 eq_error_t set_gain_db(eq_single_t db) {
1020 if (db > -min_max_gain_db_ && db < min_max_gain_db_) {
1021 current_gain_db_ = db;
1022 current_filter_index_ = get_flt_index(db);
1023 } else {
1024 return invalid_input_data_error;
1025 }
1026 return no_error;
1027 }
1028
sbs_process(eq_single_t * in,eq_single_t * out)1029 eq_error_t sbs_process(eq_single_t *in, eq_single_t *out) {
1030 *out = filters_[current_filter_index_]->process(*in);
1031 return no_error;
1032 }
1033 };
1034
1035 // ------------ eq2 ------------
1036 // Precomputed equalizer
1037
1038 class eq2
1039 {
1040 conversions conv_;
1041 eq_double_t sampling_frequency_;
1042 freq_grid freq_grid_;
1043 std::vector<eq_channel*> channels_;
1044 filter_type current_eq_type_;
1045
cleanup_channels_array()1046 void cleanup_channels_array() {
1047 for(unsigned int j = 0; j < channels_.size(); j++)
1048 delete channels_[j];
1049 }
1050
1051 public:
eq2(freq_grid & fg,filter_type eq_t)1052 eq2(freq_grid &fg, filter_type eq_t) : conv_(eq_min_max_gain_db) {
1053 sampling_frequency_ = default_sample_freq_hz;
1054 freq_grid_ = fg;
1055 current_eq_type_ = eq_t;
1056 set_eq(freq_grid_, eq_t);
1057 }
~eq2()1058 ~eq2(){cleanup_channels_array();}
1059
set_eq(const freq_grid & fg,filter_type ft)1060 eq_error_t set_eq(const freq_grid& fg, filter_type ft) {
1061 cleanup_channels_array();
1062 channels_.clear();
1063 freq_grid_ = fg;
1064
1065 for(unsigned int i = 0; i < freq_grid_.get_number_of_bands(); i++) {
1066 band_freqs b_fres = freq_grid_.get_freqs()[i];
1067
1068 eq_channel* eq_ch = new eq_channel(ft, sampling_frequency_,
1069 b_fres.center_freq, b_fres.max_freq - b_fres.min_freq);
1070
1071 channels_.push_back(eq_ch);
1072 channels_[i]->set_gain_db(p_eq_default_gain_db);
1073 }
1074
1075 current_eq_type_ = ft;
1076 return no_error;
1077 }
1078
set_eq(filter_type ft)1079 eq_error_t set_eq(filter_type ft) {
1080 eq_error_t err = set_eq(freq_grid_, ft);
1081 return err;
1082 }
1083
set_sample_rate(eq_double_t sr)1084 eq_error_t set_sample_rate(eq_double_t sr) {
1085 sampling_frequency_ = sr;
1086 eq_error_t err = set_eq(current_eq_type_);
1087 return err;
1088 }
1089
change_gains(std::vector<eq_single_t> band_gains)1090 eq_error_t change_gains(std::vector<eq_single_t> band_gains) {
1091 if(channels_.size() == band_gains.size())
1092 for(unsigned int j = 0; j < channels_.size(); j++)
1093 channels_[j]->set_gain_db(conv_.fast_lin_2_db(band_gains[j]));
1094 else
1095 return invalid_input_data_error;
1096
1097 return no_error;
1098 }
1099
change_gains_db(std::vector<eq_single_t> band_gains)1100 eq_error_t change_gains_db(std::vector<eq_single_t> band_gains) {
1101 if(channels_.size() == band_gains.size())
1102 for(unsigned int j = 0; j < channels_.size(); j++)
1103 channels_[j]->set_gain_db(band_gains[j]);
1104 else
1105 return invalid_input_data_error;
1106
1107 return no_error;
1108 }
1109
change_band_gain(unsigned int band_number,eq_single_t band_gain)1110 eq_error_t change_band_gain(unsigned int band_number,
1111 eq_single_t band_gain) {
1112 if(band_number < channels_.size())
1113 channels_[band_number]->set_gain_db(conv_.fast_lin_2_db(band_gain));
1114 else
1115 return invalid_input_data_error;
1116
1117 return no_error;
1118 }
1119
change_band_gain_db(unsigned int band_number,eq_single_t band_gain)1120 eq_error_t change_band_gain_db(unsigned int band_number,
1121 eq_single_t band_gain) {
1122 if(band_number < channels_.size()) {
1123 channels_[band_number]->set_gain_db(band_gain);
1124 } else {
1125 return invalid_input_data_error;
1126 }
1127 return no_error;
1128 }
1129
sbs_process_band(unsigned int band_number,eq_single_t * in,eq_single_t * out)1130 eq_error_t sbs_process_band(unsigned int band_number,
1131 eq_single_t *in, eq_single_t *out) {
1132 if(band_number < get_number_of_bands())
1133 channels_[band_number]->sbs_process(in, out);
1134 else
1135 return invalid_input_data_error;
1136
1137 return no_error;
1138 }
1139
sbs_process(eq_single_t * in,eq_single_t * out)1140 eq_error_t sbs_process(eq_single_t *in, eq_single_t *out) {
1141 eq_error_t err = no_error;
1142 eq_single_t in_out = *in;
1143 for(unsigned int i = 0; i < get_number_of_bands(); i++)
1144 err = sbs_process_band(i,&in_out,&in_out);
1145
1146 *out = in_out;
1147
1148 return err;
1149 }
1150
get_eq_type()1151 filter_type get_eq_type(){return current_eq_type_;}
get_string_eq_type()1152 const char* get_string_eq_type(){return get_eq_text(current_eq_type_);}
get_number_of_bands()1153 unsigned int get_number_of_bands() {
1154 return freq_grid_.get_number_of_bands();
1155 }
get_version()1156 const char* get_version(){return eq_version;}
1157 };
1158
1159 } //namespace orfanidis_eq
1160 #endif //ORFANIDIS_EQ_H_
1161