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