Mercurial > mplayer.hg
annotate libaf/filter.c @ 20085:6b5e20b52e7a
add gcc-4.x to the list of compilers displayed
when configure detects an unsupported compiler
author | gpoirier |
---|---|
date | Fri, 06 Oct 2006 13:19:06 +0000 |
parents | 36db63e8e5d7 |
children | a54a25221b79 |
rev | line source |
---|---|
7568 | 1 /*============================================================================= |
2 // | |
13602
14090f7300a8
The full name of the GPL is GNU General Public License.
diego
parents:
11000
diff
changeset
|
3 // This software has been released under the terms of the GNU General Public |
7568 | 4 // license. See http://www.gnu.org/copyleft/gpl.html for details. |
5 // | |
6 // Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au | |
7 // | |
8 //============================================================================= | |
9 */ | |
10 | |
11 /* Design and implementation of different types of digital filters | |
12 | |
13 */ | |
9217
420e2b2f8e5a
compiler warning fixes patch by Dominik Mierzejewski <dominik@rangers.eu.org>
arpi
parents:
8832
diff
changeset
|
14 #include <string.h> |
7568 | 15 #include <math.h> |
16 #include "dsp.h" | |
17 | |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
18 /****************************************************************************** |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
19 * FIR filter implementations |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
20 ******************************************************************************/ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
21 |
7568 | 22 /* C implementation of FIR filter y=w*x |
23 | |
24 n number of filter taps, where mod(n,4)==0 | |
25 w filter taps | |
26 x input signal must be a circular buffer which is indexed backwards | |
27 */ | |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
28 inline _ftype_t af_filter_fir(register unsigned int n, _ftype_t* w, _ftype_t* x) |
7568 | 29 { |
30 register _ftype_t y; // Output | |
31 y = 0.0; | |
32 do{ | |
33 n--; | |
34 y+=w[n]*x[n]; | |
35 }while(n != 0); | |
36 return y; | |
37 } | |
38 | |
39 /* C implementation of parallel FIR filter y(k)=w(k) * x(k) (where * denotes convolution) | |
40 | |
41 n number of filter taps, where mod(n,4)==0 | |
42 d number of filters | |
43 xi current index in xq | |
44 w filter taps k by n big | |
45 x input signal must be a circular buffers which are indexed backwards | |
46 y output buffer | |
47 s output buffer stride | |
48 */ | |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
49 inline _ftype_t* af_filter_pfir(unsigned int n, unsigned int d, unsigned int xi, _ftype_t** w, _ftype_t** x, _ftype_t* y, unsigned int s) |
7568 | 50 { |
51 register _ftype_t* xt = *x + xi; | |
52 register _ftype_t* wt = *w; | |
53 register int nt = 2*n; | |
54 while(d-- > 0){ | |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
55 *y = af_filter_fir(n,wt,xt); |
7568 | 56 wt+=n; |
57 xt+=nt; | |
58 y+=s; | |
59 } | |
60 return y; | |
61 } | |
62 | |
63 /* Add new data to circular queue designed to be used with a parallel | |
64 FIR filter, with d filters. xq is the circular queue, in pointing | |
65 at the new samples, xi current index in xq and n the length of the | |
66 filter. xq must be n*2 by k big, s is the index for in. | |
67 */ | |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
68 inline int af_filter_updatepq(unsigned int n, unsigned int d, unsigned int xi, _ftype_t** xq, _ftype_t* in, unsigned int s) |
7568 | 69 { |
70 register _ftype_t* txq = *xq + xi; | |
71 register int nt = n*2; | |
72 | |
73 while(d-- >0){ | |
74 *txq= *(txq+n) = *in; | |
75 txq+=nt; | |
76 in+=s; | |
77 } | |
78 return (++xi)&(n-1); | |
79 } | |
80 | |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
81 /****************************************************************************** |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
82 * FIR filter design |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
83 ******************************************************************************/ |
7568 | 84 |
85 /* Design FIR filter using the Window method | |
86 | |
87 n filter length must be odd for HP and BS filters | |
88 w buffer for the filter taps (must be n long) | |
89 fc cutoff frequencies (1 for LP and HP, 2 for BP and BS) | |
90 0 < fc < 1 where 1 <=> Fs/2 | |
91 flags window and filter type as defined in filter.h | |
92 variables are ored together: i.e. LP|HAMMING will give a | |
93 low pass filter designed using a hamming window | |
94 opt beta constant used only when designing using kaiser windows | |
95 | |
96 returns 0 if OK, -1 if fail | |
97 */ | |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
98 int af_filter_design_fir(unsigned int n, _ftype_t* w, _ftype_t* fc, unsigned int flags, _ftype_t opt) |
7568 | 99 { |
100 unsigned int o = n & 1; // Indicator for odd filter length | |
101 unsigned int end = ((n + 1) >> 1) - o; // Loop end | |
102 unsigned int i; // Loop index | |
103 | |
104 _ftype_t k1 = 2 * M_PI; // 2*pi*fc1 | |
105 _ftype_t k2 = 0.5 * (_ftype_t)(1 - o);// Constant used if the filter has even length | |
106 _ftype_t k3; // 2*pi*fc2 Constant used in BP and BS design | |
107 _ftype_t g = 0.0; // Gain | |
108 _ftype_t t1,t2,t3; // Temporary variables | |
109 _ftype_t fc1,fc2; // Cutoff frequencies | |
110 | |
111 // Sanity check | |
112 if(!w || (n == 0)) return -1; | |
113 | |
114 // Get window coefficients | |
115 switch(flags & WINDOW_MASK){ | |
116 case(BOXCAR): | |
14274 | 117 af_window_boxcar(n,w); break; |
7568 | 118 case(TRIANG): |
14274 | 119 af_window_triang(n,w); break; |
7568 | 120 case(HAMMING): |
14274 | 121 af_window_hamming(n,w); break; |
7568 | 122 case(HANNING): |
14274 | 123 af_window_hanning(n,w); break; |
7568 | 124 case(BLACKMAN): |
14274 | 125 af_window_blackman(n,w); break; |
7568 | 126 case(FLATTOP): |
14274 | 127 af_window_flattop(n,w); break; |
7568 | 128 case(KAISER): |
14274 | 129 af_window_kaiser(n,w,opt); break; |
7568 | 130 default: |
131 return -1; | |
132 } | |
133 | |
134 if(flags & (LP | HP)){ | |
135 fc1=*fc; | |
136 // Cutoff frequency must be < 0.5 where 0.5 <=> Fs/2 | |
137 fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25; | |
138 k1 *= fc1; | |
139 | |
140 if(flags & LP){ // Low pass filter | |
141 | |
142 // If the filter length is odd, there is one point which is exactly | |
143 // in the middle. The value at this point is 2*fCutoff*sin(x)/x, | |
144 // where x is zero. To make sure nothing strange happens, we set this | |
145 // value separately. | |
146 if (o){ | |
147 w[end] = fc1 * w[end] * 2.0; | |
148 g=w[end]; | |
149 } | |
150 | |
151 // Create filter | |
152 for (i=0 ; i<end ; i++){ | |
153 t1 = (_ftype_t)(i+1) - k2; | |
154 w[end-i-1] = w[n-end+i] = w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc | |
155 g += 2*w[end-i-1]; // Total gain in filter | |
156 } | |
157 } | |
158 else{ // High pass filter | |
159 if (!o) // High pass filters must have odd length | |
160 return -1; | |
161 w[end] = 1.0 - (fc1 * w[end] * 2.0); | |
162 g= w[end]; | |
163 | |
164 // Create filter | |
165 for (i=0 ; i<end ; i++){ | |
166 t1 = (_ftype_t)(i+1); | |
167 w[end-i-1] = w[n-end+i] = -1 * w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc | |
168 g += ((i&1) ? (2*w[end-i-1]) : (-2*w[end-i-1])); // Total gain in filter | |
169 } | |
170 } | |
171 } | |
172 | |
173 if(flags & (BP | BS)){ | |
174 fc1=fc[0]; | |
175 fc2=fc[1]; | |
176 // Cutoff frequencies must be < 1.0 where 1.0 <=> Fs/2 | |
177 fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25; | |
178 fc2 = ((fc2 <= 1.0) && (fc2 > 0.0)) ? fc2/2 : 0.25; | |
179 k3 = k1 * fc2; // 2*pi*fc2 | |
180 k1 *= fc1; // 2*pi*fc1 | |
181 | |
182 if(flags & BP){ // Band pass | |
183 // Calculate center tap | |
184 if (o){ | |
185 g=w[end]*(fc1+fc2); | |
186 w[end] = (fc2 - fc1) * w[end] * 2.0; | |
187 } | |
188 | |
189 // Create filter | |
190 for (i=0 ; i<end ; i++){ | |
191 t1 = (_ftype_t)(i+1) - k2; | |
192 t2 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2 | |
193 t3 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1 | |
194 g += w[end-i-1] * (t3 + t2); // Total gain in filter | |
195 w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3); | |
196 } | |
197 } | |
198 else{ // Band stop | |
199 if (!o) // Band stop filters must have odd length | |
200 return -1; | |
201 w[end] = 1.0 - (fc2 - fc1) * w[end] * 2.0; | |
202 g= w[end]; | |
203 | |
204 // Create filter | |
205 for (i=0 ; i<end ; i++){ | |
206 t1 = (_ftype_t)(i+1); | |
207 t2 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1 | |
208 t3 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2 | |
209 w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3); | |
210 g += 2*w[end-i-1]; // Total gain in filter | |
211 } | |
212 } | |
213 } | |
214 | |
215 // Normalize gain | |
216 g=1/g; | |
217 for (i=0; i<n; i++) | |
218 w[i] *= g; | |
219 | |
220 return 0; | |
221 } | |
222 | |
223 /* Design polyphase FIR filter from prototype filter | |
224 | |
225 n length of prototype filter | |
226 k number of polyphase components | |
227 w prototype filter taps | |
228 pw Parallel FIR filter | |
229 g Filter gain | |
230 flags FWD forward indexing | |
231 REW reverse indexing | |
232 ODD multiply every 2nd filter tap by -1 => HP filter | |
233 | |
234 returns 0 if OK, -1 if fail | |
235 */ | |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
236 int af_filter_design_pfir(unsigned int n, unsigned int k, _ftype_t* w, _ftype_t** pw, _ftype_t g, unsigned int flags) |
7568 | 237 { |
238 int l = (int)n/k; // Length of individual FIR filters | |
239 int i; // Counters | |
240 int j; | |
241 _ftype_t t; // g * w[i] | |
242 | |
243 // Sanity check | |
244 if(l<1 || k<1 || !w || !pw) | |
245 return -1; | |
246 | |
247 // Do the stuff | |
248 if(flags&REW){ | |
249 for(j=l-1;j>-1;j--){//Columns | |
250 for(i=0;i<(int)k;i++){//Rows | |
251 t=g * *w++; | |
252 pw[i][j]=t * ((flags & ODD) ? ((j & 1) ? -1 : 1) : 1); | |
253 } | |
254 } | |
255 } | |
256 else{ | |
257 for(j=0;j<l;j++){//Columns | |
258 for(i=0;i<(int)k;i++){//Rows | |
259 t=g * *w++; | |
260 pw[i][j]=t * ((flags & ODD) ? ((j & 1) ? 1 : -1) : 1); | |
261 } | |
262 } | |
263 } | |
264 return -1; | |
265 } | |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
266 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
267 /****************************************************************************** |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
268 * IIR filter design |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
269 ******************************************************************************/ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
270 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
271 /* Helper functions for the bilinear transform */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
272 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
273 /* Pre-warp the coefficients of a numerator or denominator. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
274 Note that a0 is assumed to be 1, so there is no wrapping |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
275 of it. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
276 */ |
18967
36db63e8e5d7
makes several libaf functions static coz they are not used outside their source files. Patch by Stefan Huehner, stefan AT huehner-org
reynaldo
parents:
14275
diff
changeset
|
277 static void af_filter_prewarp(_ftype_t* a, _ftype_t fc, _ftype_t fs) |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
278 { |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
279 _ftype_t wp; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
280 wp = 2.0 * fs * tan(M_PI * fc / fs); |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
281 a[2] = a[2]/(wp * wp); |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
282 a[1] = a[1]/wp; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
283 } |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
284 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
285 /* Transform the numerator and denominator coefficients of s-domain |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
286 biquad section into corresponding z-domain coefficients. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
287 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
288 The transfer function for z-domain is: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
289 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
290 1 + alpha1 * z^(-1) + alpha2 * z^(-2) |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
291 H(z) = ------------------------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
292 1 + beta1 * z^(-1) + beta2 * z^(-2) |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
293 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
294 Store the 4 IIR coefficients in array pointed by coef in following |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
295 order: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
296 beta1, beta2 (denominator) |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
297 alpha1, alpha2 (numerator) |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
298 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
299 Arguments: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
300 a - s-domain numerator coefficients |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
301 b - s-domain denominator coefficients |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
302 k - filter gain factor. Initially set to 1 and modified by each |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
303 biquad section in such a way, as to make it the |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
304 coefficient by which to multiply the overall filter gain |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
305 in order to achieve a desired overall filter gain, |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
306 specified in initial value of k. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
307 fs - sampling rate (Hz) |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
308 coef - array of z-domain coefficients to be filled in. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
309 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
310 Return: On return, set coef z-domain coefficients and k to the gain |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
311 required to maintain overall gain = 1.0; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
312 */ |
18967
36db63e8e5d7
makes several libaf functions static coz they are not used outside their source files. Patch by Stefan Huehner, stefan AT huehner-org
reynaldo
parents:
14275
diff
changeset
|
313 static void af_filter_bilinear(_ftype_t* a, _ftype_t* b, _ftype_t* k, _ftype_t fs, _ftype_t *coef) |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
314 { |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
315 _ftype_t ad, bd; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
316 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
317 /* alpha (Numerator in s-domain) */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
318 ad = 4. * a[2] * fs * fs + 2. * a[1] * fs + a[0]; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
319 /* beta (Denominator in s-domain) */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
320 bd = 4. * b[2] * fs * fs + 2. * b[1] * fs + b[0]; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
321 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
322 /* Update gain constant for this section */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
323 *k *= ad/bd; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
324 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
325 /* Denominator */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
326 *coef++ = (2. * b[0] - 8. * b[2] * fs * fs)/bd; /* beta1 */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
327 *coef++ = (4. * b[2] * fs * fs - 2. * b[1] * fs + b[0])/bd; /* beta2 */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
328 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
329 /* Numerator */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
330 *coef++ = (2. * a[0] - 8. * a[2] * fs * fs)/ad; /* alpha1 */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
331 *coef = (4. * a[2] * fs * fs - 2. * a[1] * fs + a[0])/ad; /* alpha2 */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
332 } |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
333 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
334 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
335 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
336 /* IIR filter design using bilinear transform and prewarp. Transforms |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
337 2nd order s domain analog filter into a digital IIR biquad link. To |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
338 create a filter fill in a, b, Q and fs and make space for coef and k. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
339 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
340 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
341 Example Butterworth design: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
342 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
343 Below are Butterworth polynomials, arranged as a series of 2nd |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
344 order sections: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
345 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
346 Note: n is filter order. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
347 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
348 n Polynomials |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
349 ------------------------------------------------------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
350 2 s^2 + 1.4142s + 1 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
351 4 (s^2 + 0.765367s + 1) * (s^2 + 1.847759s + 1) |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
352 6 (s^2 + 0.5176387s + 1) * (s^2 + 1.414214 + 1) * (s^2 + 1.931852s + 1) |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
353 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
354 For n=4 we have following equation for the filter transfer function: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
355 1 1 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
356 T(s) = --------------------------- * ---------------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
357 s^2 + (1/Q) * 0.765367s + 1 s^2 + (1/Q) * 1.847759s + 1 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
358 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
359 The filter consists of two 2nd order sections since highest s power |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
360 is 2. Now we can take the coefficients, or the numbers by which s |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
361 is multiplied and plug them into a standard formula to be used by |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
362 bilinear transform. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
363 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
364 Our standard form for each 2nd order section is: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
365 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
366 a2 * s^2 + a1 * s + a0 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
367 H(s) = ---------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
368 b2 * s^2 + b1 * s + b0 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
369 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
370 Note that Butterworth numerator is 1 for all filter sections, which |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
371 means s^2 = 0 and s^1 = 0 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
372 |
11000 | 373 Let's convert standard Butterworth polynomials into this form: |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
374 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
375 0 + 0 + 1 0 + 0 + 1 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
376 --------------------------- * -------------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
377 1 + ((1/Q) * 0.765367) + 1 1 + ((1/Q) * 1.847759) + 1 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
378 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
379 Section 1: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
380 a2 = 0; a1 = 0; a0 = 1; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
381 b2 = 1; b1 = 0.765367; b0 = 1; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
382 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
383 Section 2: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
384 a2 = 0; a1 = 0; a0 = 1; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
385 b2 = 1; b1 = 1.847759; b0 = 1; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
386 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
387 Q is filter quality factor or resonance, in the range of 1 to |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
388 1000. The overall filter Q is a product of all 2nd order stages. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
389 For example, the 6th order filter (3 stages, or biquads) with |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
390 individual Q of 2 will have filter Q = 2 * 2 * 2 = 8. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
391 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
392 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
393 Arguments: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
394 a - s-domain numerator coefficients, a[1] is always assumed to be 1.0 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
395 b - s-domain denominator coefficients |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
396 Q - Q value for the filter |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
397 k - filter gain factor. Initially set to 1 and modified by each |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
398 biquad section in such a way, as to make it the |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
399 coefficient by which to multiply the overall filter gain |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
400 in order to achieve a desired overall filter gain, |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
401 specified in initial value of k. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
402 fs - sampling rate (Hz) |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
403 coef - array of z-domain coefficients to be filled in. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
404 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
405 Note: Upon return from each call, the k argument will be set to a |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
406 value, by which to multiply our actual signal in order for the gain |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
407 to be one. On second call to szxform() we provide k that was |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
408 changed by the previous section. During actual audio filtering |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
409 k can be used for gain compensation. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
410 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
411 return -1 if fail 0 if success. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
412 */ |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
413 int af_filter_szxform(_ftype_t* a, _ftype_t* b, _ftype_t Q, _ftype_t fc, _ftype_t fs, _ftype_t *k, _ftype_t *coef) |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
414 { |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
415 _ftype_t at[3]; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
416 _ftype_t bt[3]; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
417 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
418 if(!a || !b || !k || !coef || (Q>1000.0 || Q< 1.0)) |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
419 return -1; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
420 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
421 memcpy(at,a,3*sizeof(_ftype_t)); |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
422 memcpy(bt,b,3*sizeof(_ftype_t)); |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
423 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
424 bt[1]/=Q; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
425 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
426 /* Calculate a and b and overwrite the original values */ |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
427 af_filter_prewarp(at, fc, fs); |
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
428 af_filter_prewarp(bt, fc, fs); |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
429 /* Execute bilinear transform */ |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
430 af_filter_bilinear(at, bt, k, fs, coef); |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
431 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
432 return 0; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
433 } |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
434 |