1 // ----------------------------------------------------------------------------
2 //
3 // Copyright (C) 2013-2018 Fons Adriaensen <fons@linuxaudio.org>
4 //
5 // This program is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU General Public License as published by
7 // the Free Software Foundation; either version 3 of the License, or
8 // (at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU General Public License for more details.
14 //
15 // You should have received a copy of the GNU General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 //
18 // ----------------------------------------------------------------------------
19
20
21 #include <string.h>
22 #include "bp6filter.h"
23
24
25 static const float EPS = 1e-40f;
26
27
Bp6filter(void)28 Bp6filter::Bp6filter (void):
29 _param (0)
30 {
31 reset ();
32 }
33
34
~Bp6filter(void)35 Bp6filter::~Bp6filter (void)
36 {
37 }
38
39
reset(void)40 void Bp6filter::reset (void)
41 {
42 memset (_z, 0, 6 * sizeof (double));
43 }
44
45
setparam(const Bp6param * P)46 void Bp6filter::setparam (const Bp6param *P)
47 {
48 if (_param != P)
49 {
50 _param = P;
51 reset ();
52 }
53 }
54
55
process(int nsamp,float * inp,float * out)56 void Bp6filter::process (int nsamp, float *inp, float *out)
57 {
58 int i;
59 double z0, z1, z2, z3, z4, z5;
60 double c0, c1, c2, c3, c4, c5;
61 double g, h, x, y;
62
63 if (!_param)
64 {
65 memset (out, 0, nsamp * sizeof (float));
66 return;
67 }
68
69 z0 = _z [0];
70 z1 = _z [1];
71 z2 = _z [2];
72 z3 = _z [3];
73 g = _param->_gain;
74 c0 = _param->_coeff [0];
75 c1 = _param->_coeff [1];
76 c2 = _param->_coeff [2];
77 c3 = _param->_coeff [3];
78
79 if (_param->_mode == Bp6param::HH)
80 {
81 for (i = 0; i < nsamp; i++)
82 {
83 x = inp [i];
84 x -= c0 * z0 + c1 * z1 + EPS;
85 z1 += z0;
86 z0 += x;
87 x -= c2 * z2 + c3 * z3 + EPS;
88 z3 += z2;
89 z2 += x;
90 out [i] = g * x;
91 }
92 }
93 else
94 {
95 z4 = _z [4];
96 z5 = _z [5];
97 c4 = _param->_coeff [4];
98 c5 = _param->_coeff [5];
99 h = (_param->_mode == Bp6param::BBH) ? 0.0 : 2.0;
100
101 for (i = 0; i < nsamp; i++)
102 {
103 x = inp [i];
104 x -= c0 * z0 + c1 * z1 + EPS;
105 y = x + 2 * z0;
106 z1 += z0;
107 z0 += x;
108 y -= c2 * z2 + c3 * z3 + EPS;
109 x = y + 2 * z2;
110 z3 += z2;
111 z2 += y;
112 x -= c4 * z4 + c5 * z5 + EPS;
113 y = x + h * z4;
114 z5 += z4;
115 z4 += x;
116 out [i] = g * y;
117 }
118 _z [4] = z4;
119 _z [5] = z5;
120 }
121 _z [0] = z0;
122 _z [1] = z1;
123 _z [2] = z2;
124 _z [3] = z3;
125 }
126
127
128