1 /***************************************************************************
2 * Copyright (C) 2011 by Pere Ràfols Soler *
3 * sapista2@gmail.com *
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 2 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, write to the *
17 * Free Software Foundation, Inc., *
18 * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19 ***************************************************************************/
20
21 /***************************************************************************
22 This file contains the filter definitions
23 ****************************************************************************/
24
25 #ifndef FILTER_H
26 #define FILTER_H
27
28 #include <math.h>
29 #include "param_interpolator.h"
30
31 //#include <stdio.h>
32
33 //Constants definitions
34 #ifndef PI
35 #define PI 3.1416
36 #endif
37
38 #define F_NOT_SET 0
39 #define F_LPF_ORDER_1 1
40 #define F_LPF_ORDER_2 2
41 #define F_LPF_ORDER_3 3
42 #define F_LPF_ORDER_4 4
43 #define F_HPF_ORDER_1 5
44 #define F_HPF_ORDER_2 6
45 #define F_HPF_ORDER_3 7
46 #define F_HPF_ORDER_4 8
47 #define F_LOW_SHELF 9
48 #define F_HIGH_SHELF 10
49 #define F_PEAK 11
50 #define F_NOTCH 12
51
52 typedef struct
53 {
54 double b0, b1, b2, a1, a2; //Second Order coeficients
55 double b1_0, b1_1, b1_2, a1_1, a1_2; //Second Order extra coeficients
56 int filter_order; //filter order
57 double fs; //sample rate
58 float gain, freq, q;
59 double enable;
60 int iType; //Filter type
61
62 //Interpolation K
63 float InterK;
64 float useInterpolation;
65
66 }Filter;
67
68 typedef struct
69 {
70 double buf_0;
71 double buf_1;
72 double buf_2;
73 double buf_e0;
74 double buf_e1;
75 double buf_e2;
76 }Buffers;
77
78 //Initialize filter instance
79 Filter *FilterInit(double rate);
80
81 //Destroy a filter instance
82 void FilterClean(Filter *f);
83
84 //Clean buffers
85 void flushBuffers(Buffers *buf);
86
87 //Compute filter coeficients
calcCoefs(Filter * filter,float fGain,float fFreq,float fQ,int iType,float iEnabled)88 static inline void calcCoefs(Filter *filter, float fGain, float fFreq, float fQ, int iType, float iEnabled) //p2 = GAIN p3 = Q
89 {
90 double alpha, A, b0, b1, b2, a0, a1, a2, b1_0, b1_1, b1_2, a1_0, a1_1, a1_2;
91 alpha = A = b0 = b1 = b2 = a0 = a1 = a2 = b1_0 = b1_1 = b1_2 = a1_0 = a1_1 = a1_2 = 1.0;
92 filter->filter_order = 0;
93
94 //Param Interpolation
95 filter->freq = computeParamInterpolation(fFreq, filter->freq, filter->InterK, filter->useInterpolation);
96 filter->gain = computeParamInterpolation(fGain, filter->gain, filter->InterK, filter->useInterpolation);
97 filter->q = computeParamInterpolation(fQ, filter->q, filter->InterK, filter->useInterpolation);
98 filter->enable = (double)computeParamInterpolation(iEnabled, (float)filter->enable, filter->InterK, filter->useInterpolation);
99
100 double w0=2*PI*(filter->freq/filter->fs);
101 filter->iType = iType;
102
103 switch(iType){
104
105 case F_HPF_ORDER_1:
106 {
107 w0 = tanf(w0/2.0);
108 b0 = 1.0;
109 b1 = -1.0;
110 b2 = 0.0;
111 a0 = w0+1.0;
112 a1 = w0-1.0;
113 a2 = 0.0;
114 }
115 break;
116
117 case F_HPF_ORDER_4:
118 {
119 filter->filter_order = 1;
120 case F_HPF_ORDER_2:
121 alpha = sinf(w0)/(2*filter->q);
122 b1_0 = b0 = (1 + cosf(w0))/2; //b0
123 b1_1 = b1 = -(1 + cosf(w0)); //b1
124 b1_2 = b2 = (1 + cosf(w0))/2; //b2
125 a1_0 = a0 = 1 + alpha; //a0
126 a1_1 = a1 = -2*cosf(w0); //a1
127 a1_2 = a2 = 1 - alpha; //a2
128 }
129 break;
130
131 case F_HPF_ORDER_3:
132 {
133 filter->filter_order = 1;
134 alpha = sinf(w0)/(2*filter->q);
135 b0 = (1 + cosf(w0))/2; //b0
136 b1 = -(1 + cosf(w0)); //b1
137 b2 = (1 + cosf(w0))/2; //b2
138 a0 = 1 + alpha; //a0
139 a1 = -2*cosf(w0); //a1
140 a2 = 1 - alpha; //a2
141 w0 = tanf(w0/2.0);
142 b1_0 = 1.0;
143 b1_1 = -1.0;
144 b1_2 = 0.0;
145 a1_0 = w0+1.0;
146 a1_1 = w0-1.0;
147 a1_2 = 0.0;
148 }
149 break;
150
151 case F_LPF_ORDER_1:
152 {
153 w0 = tanf(w0/2.0);
154 b0 = w0;
155 b1 = w0;
156 b2 = 0.0;
157 a0 = w0+1.0;
158 a1 = w0-1.0;
159 a2 = 0.0;
160 }
161 break;
162
163 case F_LPF_ORDER_4:
164 {
165 filter->filter_order = 1;
166 case F_LPF_ORDER_2:
167 alpha = sinf(w0)/(2*filter->q);
168 b1_0 = b0 = (1 - cosf(w0))/2; //b0
169 b1_1 = b1 = 1 - cosf(w0); //b1
170 b1_2 = b2 = (1 - cosf(w0))/2; //b2
171 a1_0 = a0 = 1 + alpha; //a0
172 a1_1 = a1 = -2*cosf(w0); //a1
173 a1_2 = a2 = 1 - alpha; //a2
174 }
175 break;
176
177 case F_LPF_ORDER_3:
178 {
179 filter->filter_order = 1;
180 alpha = sinf(w0)/(2*filter->q);
181 b0 = (1 - cosf(w0))/2; //b0
182 b1 = 1 - cosf(w0); //b1
183 b2 = (1 - cosf(w0))/2; //b2
184 a0 = 1 + alpha; //a0
185 a1 = -2*cosf(w0); //a1
186 a2 = 1 - alpha; //a2
187 w0 = tanf(w0/2.0);
188 b1_0 = w0;
189 b1_1 = w0;
190 b1_2 = 0.0;
191 a1_0 = w0+1.0;
192 a1_1 = w0-1.0;
193 a1_2 = 0.0;
194 }
195 break;
196
197 case F_LOW_SHELF:
198 {
199 A = sqrtf((filter->gain));
200 alpha =sinf(w0)/2 * (1/filter->q);
201 b0 = A*((A+1)-(A-1)*cosf(w0)+2*sqrtf(A)*alpha); //b0
202 b1 = 2*A*((A-1)-(A+1)*cosf(w0)); //b1
203 b2 = A*((A+1)-(A-1)*cosf(w0)-2*sqrtf(A)*alpha); //b2
204 a0 = (A+1) + (A-1)*cosf(w0) + 2*sqrtf(A)*alpha; //a0
205 a1 = -2*((A-1) + (A+1)*cosf(w0)); //a1
206 a2 = (A+1) + (A-1)*cosf(w0) - 2*sqrtf(A)*alpha; //a2
207 }
208 break;
209
210 case F_HIGH_SHELF:
211 {
212 A = sqrtf((filter->gain));
213 alpha =sinf(w0)/2 * (1/filter->q);
214 b0 = A*( (A+1) + (A-1)*cosf(w0) + 2*sqrtf(A)*alpha ); //b0
215 b1 = -2*A*( (A-1) + (A+1)*cosf(w0)); //b1
216 b2 = A*( (A+1) + (A-1)*cosf(w0) - 2*sqrtf(A)*alpha ); //b2
217 a0 = (A+1) - (A-1)*cosf(w0) + 2*sqrtf(A)*alpha; //a0
218 a1 = 2*( (A-1) - (A+1)*cosf(w0)); //a1
219 a2 = (A+1) - (A-1)*cosf(w0) - 2*sqrtf(A)*alpha; //a2
220 }
221 break;
222
223 case F_PEAK:
224 {
225 A = sqrtf(filter->gain);
226 double A2 = A*A;
227 double PI2 = PI*PI;
228 double Q2 = filter->q*filter->q;
229 double w02 = w0 * w0;
230 double w02_PI22 = (w02 - PI2)*(w02 - PI2);
231
232 //Equivalent analog filter and analog gains
233 double G1 = sqrtf((w02_PI22 + (A2*w02*PI2)/Q2)/(w02_PI22 + (w02*PI2)/(Q2*A2)));
234 double GB = sqrt(G1*filter->gain);
235 double GB2 = GB * GB;
236 double G2 = filter->gain * filter->gain;
237 double G12 = G1 * G1;
238
239 //Digital filter
240 double F = fabs(G2 - GB2);// + 0.00000001f; ///TODO akest petit num sumat en teoria no hi va pero he detectat div by 0
241 double G00 = fabs(G2 - 1.0);// + 0.00000001f; ///TODO akest petit num sumat en teoria no hi va pero he detectat div by 0
242 double F00 = fabs(GB2 - 1.0);// + 0.00000001f; ///TODO akest petit num sumat en teoria no hi va pero he detectat div by 0
243 double G01 = fabs(G2 - G1);// + 0.00000001f; ///TODO akest petit num sumat en teoria no hi va pero he detectat div by 0
244 double G11 = fabs(G2 - G12);// + 0.00000001f; ///TODO akest petit num sumat en teoria no hi va pero he detectat div by 0
245 double F01 = fabs(GB2 - G1);// + 0.00000001f; ///TODO akest petit num sumat en teoria no hi va pero he detectat div by 0
246 double F11 = fabs(GB2 - G12);// + 0.00000001f; ///TODO akest petit num sumat en teoria no hi va pero he detectat div by 0
247 double W2 = sqrtf(G11 / G00) * tanf(w0/2.0) * tanf(w0/2.0);
248
249 //Bandwidth condition
250 double Aw = (w0/(A*filter->q))*sqrtf((GB2-A2 * A2)/(1.0 - GB2)); //Analog filter bandwidth at GB
251 double DW = (1.0 + sqrtf(F00 / F11) * W2) * tanf(Aw/2.0); //Prewarped digital bandwidth
252
253 //printf("G1=%f Aw=%f DW=%f F11=%f GB2=%f G12=%f\r\n",G1,Aw,DW,F11,GB2,G12);
254
255 //Digital coefs
256 double C = F11 * DW * DW - 2.0 * W2 * (F01 - sqrtf(F00 * F11));
257 double D = 2.0 * W2 * (G01 - sqrtf(G00 * G11));
258 double A = sqrtf((C + D) / F);
259 double B = sqrtf((G2 * C + GB2 * D) / F);
260
261 //printf("A=%f B=%f C=%f D=%f W2=%f F=%f G2=%f GB2=%f\r\n", A, B, C, D, W2, F, G2, GB2 );
262
263 if( filter->gain > 1.01 || filter->gain < 0.98 )
264 {
265 b0 = G1 + W2 + B;
266 b1 = -2.0*(G1 - W2);
267 b2 = G1 - B + W2;
268 a0 = 1.0 + W2 + A;
269 a1 = -2.0*(1.0 - W2);
270 a2 = 1.0 + W2 - A;
271 }
272 else
273 {
274 b0 = 1.0;
275 b1 = 0.0;
276 b2 = 0.0;
277 a0 = 1.0;
278 a1 = 0.0;
279 a2 = 0.0;
280 }
281 }
282 break;
283
284 case F_NOTCH:
285 {
286 alpha = sinf(w0)/(2*filter->q);
287
288 b0 = 1; //b0
289 b1 = -2*cosf(w0); //b1
290 b2 = 1; //b2
291 a0 = 1 + alpha; //a0
292 a1 = -2*cosf(w0); //a1
293 a2 = 1 - alpha; //a2
294 }
295 break;
296 } //End of switch
297
298 //Normalice coeficients to a0=1
299 filter->b0 = (b0/a0); //b0
300 filter->b1 = (b1/a0); //b1
301 filter->b2 = (b2/a0); //b2
302 filter->a1 = (a1/a0); //a1
303 filter->a2 = (a2/a0); //a2
304 filter->b1_0 = (b1_0/a1_0);
305 filter->b1_1 = (b1_1/a1_0);
306 filter->b1_2 = (b1_2/a1_0);
307 filter->a1_1 = (a1_1/a1_0);
308 filter->a1_2 = (a1_2/a1_0);
309
310 //Print coefs
311 //printf("Coefs b0=%f b1=%f b2=%f a1=%f a2=%f\r\n",filter->b0,filter->b1,filter->b2,filter->a1,filter->a2);
312 //printf("Gain = %f Freq = %f Q = %f\r\n", filter->gain, filter->freq, filter->q);
313 }
314
315 #define DENORMAL_TO_ZERO(x) if (fabs(x) < (1e-300)) x = 0.0; //Min float is 1.1754943e-38 (Min double is 2.23×10−308)
316
317 //Compute filter
computeFilter(Filter * filter,Buffers * buf,double * inputSample)318 static inline void computeFilter(Filter *filter, Buffers *buf, double *inputSample)
319 {
320 //Process 1, 2 orders
321 //w(n)=x(n)-a1*w(n-1)-a2*w(n-2)
322 buf->buf_0 = (*inputSample)-filter->a1*buf->buf_1-filter->a2*buf->buf_2;
323
324
325 /*
326 //Denomar hard TEST
327 static unsigned int den_counter = 0;
328 if (fabs(buf->buf_0) < (1e-30))
329 {
330 den_counter++;
331 printf("#DENORMAL# %d\r\n",den_counter);
332 }
333 */
334
335
336 DENORMAL_TO_ZERO(buf->buf_0);
337 //y(n)=bo*w(n)+b1*w(n-1)+b2*w(n-2)
338 *inputSample = (filter->b0*buf->buf_0 + filter->b1*buf->buf_1+ filter->b2*buf->buf_2) * filter->enable + (*inputSample)*(1.0 - filter->enable);
339
340 buf->buf_2 = buf->buf_1;
341 buf->buf_1 = buf->buf_0;
342
343 //Process 3,4 orders if apply
344 if(filter->filter_order)
345 {
346 //w(n)=x(n)-a1*w(n-1)-a2*w(n-2)
347 buf->buf_e0 = (*inputSample)-filter->a1_1*buf->buf_e1-filter->a1_2*buf->buf_e2;
348 //y(n)=bo*w(n)+b1*w(n-1)+b2*w(n-2)
349 DENORMAL_TO_ZERO(buf->buf_e0);
350 *inputSample = (filter->b1_0*buf->buf_e0 + filter->b1_1*buf->buf_e1+ filter->b1_2*buf->buf_e2) * filter->enable + (*inputSample)*(1.0 - filter->enable);
351
352 buf->buf_e2 = buf->buf_e1;
353 buf->buf_e1 = buf->buf_e0;
354 }
355 }
356 #endif