Mercurial > mplayer.hg
annotate libaf/filter.c @ 27319:09cf111f68b8
Revert to previous dependency checking behavior.
Take included header files into account when generating dependency files.
This has problems when header files are removed or renamed, but does not
silently miscompile files.
author | diego |
---|---|
date | Sat, 26 Jul 2008 18:36:48 +0000 |
parents | 82fd0e4c93c9 |
children | 72d0b1444141 |
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 */ | |
27258 | 28 inline FLOAT_TYPE af_filter_fir(register unsigned int n, const FLOAT_TYPE* w, |
29 const FLOAT_TYPE* x) | |
7568 | 30 { |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
31 register FLOAT_TYPE y; // Output |
7568 | 32 y = 0.0; |
33 do{ | |
34 n--; | |
35 y+=w[n]*x[n]; | |
36 }while(n != 0); | |
37 return y; | |
38 } | |
39 | |
40 /* C implementation of parallel FIR filter y(k)=w(k) * x(k) (where * denotes convolution) | |
41 | |
42 n number of filter taps, where mod(n,4)==0 | |
43 d number of filters | |
44 xi current index in xq | |
45 w filter taps k by n big | |
46 x input signal must be a circular buffers which are indexed backwards | |
47 y output buffer | |
48 s output buffer stride | |
49 */ | |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
50 FLOAT_TYPE* af_filter_pfir(unsigned int n, unsigned int d, unsigned int xi, |
27258 | 51 const FLOAT_TYPE** w, const FLOAT_TYPE** x, FLOAT_TYPE* y, |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
52 unsigned int s) |
7568 | 53 { |
27258 | 54 register const FLOAT_TYPE* xt = *x + xi; |
55 register const FLOAT_TYPE* wt = *w; | |
7568 | 56 register int nt = 2*n; |
57 while(d-- > 0){ | |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
58 *y = af_filter_fir(n,wt,xt); |
7568 | 59 wt+=n; |
60 xt+=nt; | |
61 y+=s; | |
62 } | |
63 return y; | |
64 } | |
65 | |
66 /* Add new data to circular queue designed to be used with a parallel | |
67 FIR filter, with d filters. xq is the circular queue, in pointing | |
68 at the new samples, xi current index in xq and n the length of the | |
69 filter. xq must be n*2 by k big, s is the index for in. | |
70 */ | |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
71 int af_filter_updatepq(unsigned int n, unsigned int d, unsigned int xi, |
27258 | 72 FLOAT_TYPE** xq, const FLOAT_TYPE* in, unsigned int s) |
7568 | 73 { |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
74 register FLOAT_TYPE* txq = *xq + xi; |
7568 | 75 register int nt = n*2; |
76 | |
77 while(d-- >0){ | |
78 *txq= *(txq+n) = *in; | |
79 txq+=nt; | |
80 in+=s; | |
81 } | |
82 return (++xi)&(n-1); | |
83 } | |
84 | |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
85 /****************************************************************************** |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
86 * 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
|
87 ******************************************************************************/ |
7568 | 88 |
89 /* Design FIR filter using the Window method | |
90 | |
91 n filter length must be odd for HP and BS filters | |
92 w buffer for the filter taps (must be n long) | |
93 fc cutoff frequencies (1 for LP and HP, 2 for BP and BS) | |
94 0 < fc < 1 where 1 <=> Fs/2 | |
95 flags window and filter type as defined in filter.h | |
96 variables are ored together: i.e. LP|HAMMING will give a | |
97 low pass filter designed using a hamming window | |
98 opt beta constant used only when designing using kaiser windows | |
99 | |
100 returns 0 if OK, -1 if fail | |
101 */ | |
27258 | 102 int af_filter_design_fir(unsigned int n, FLOAT_TYPE* w, const FLOAT_TYPE* fc, |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
103 unsigned int flags, FLOAT_TYPE opt) |
7568 | 104 { |
105 unsigned int o = n & 1; // Indicator for odd filter length | |
106 unsigned int end = ((n + 1) >> 1) - o; // Loop end | |
107 unsigned int i; // Loop index | |
108 | |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
109 FLOAT_TYPE k1 = 2 * M_PI; // 2*pi*fc1 |
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
110 FLOAT_TYPE k2 = 0.5 * (FLOAT_TYPE)(1 - o);// Constant used if the filter has even length |
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
111 FLOAT_TYPE k3; // 2*pi*fc2 Constant used in BP and BS design |
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
112 FLOAT_TYPE g = 0.0; // Gain |
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
113 FLOAT_TYPE t1,t2,t3; // Temporary variables |
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
114 FLOAT_TYPE fc1,fc2; // Cutoff frequencies |
7568 | 115 |
116 // Sanity check | |
117 if(!w || (n == 0)) return -1; | |
118 | |
119 // Get window coefficients | |
120 switch(flags & WINDOW_MASK){ | |
121 case(BOXCAR): | |
14274 | 122 af_window_boxcar(n,w); break; |
7568 | 123 case(TRIANG): |
14274 | 124 af_window_triang(n,w); break; |
7568 | 125 case(HAMMING): |
14274 | 126 af_window_hamming(n,w); break; |
7568 | 127 case(HANNING): |
14274 | 128 af_window_hanning(n,w); break; |
7568 | 129 case(BLACKMAN): |
14274 | 130 af_window_blackman(n,w); break; |
7568 | 131 case(FLATTOP): |
14274 | 132 af_window_flattop(n,w); break; |
7568 | 133 case(KAISER): |
14274 | 134 af_window_kaiser(n,w,opt); break; |
7568 | 135 default: |
136 return -1; | |
137 } | |
138 | |
139 if(flags & (LP | HP)){ | |
140 fc1=*fc; | |
141 // Cutoff frequency must be < 0.5 where 0.5 <=> Fs/2 | |
142 fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25; | |
143 k1 *= fc1; | |
144 | |
145 if(flags & LP){ // Low pass filter | |
146 | |
147 // If the filter length is odd, there is one point which is exactly | |
148 // in the middle. The value at this point is 2*fCutoff*sin(x)/x, | |
149 // where x is zero. To make sure nothing strange happens, we set this | |
150 // value separately. | |
151 if (o){ | |
152 w[end] = fc1 * w[end] * 2.0; | |
153 g=w[end]; | |
154 } | |
155 | |
156 // Create filter | |
157 for (i=0 ; i<end ; i++){ | |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
158 t1 = (FLOAT_TYPE)(i+1) - k2; |
7568 | 159 w[end-i-1] = w[n-end+i] = w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc |
160 g += 2*w[end-i-1]; // Total gain in filter | |
161 } | |
162 } | |
163 else{ // High pass filter | |
164 if (!o) // High pass filters must have odd length | |
165 return -1; | |
166 w[end] = 1.0 - (fc1 * w[end] * 2.0); | |
167 g= w[end]; | |
168 | |
169 // Create filter | |
170 for (i=0 ; i<end ; i++){ | |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
171 t1 = (FLOAT_TYPE)(i+1); |
7568 | 172 w[end-i-1] = w[n-end+i] = -1 * w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc |
173 g += ((i&1) ? (2*w[end-i-1]) : (-2*w[end-i-1])); // Total gain in filter | |
174 } | |
175 } | |
176 } | |
177 | |
178 if(flags & (BP | BS)){ | |
179 fc1=fc[0]; | |
180 fc2=fc[1]; | |
181 // Cutoff frequencies must be < 1.0 where 1.0 <=> Fs/2 | |
182 fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25; | |
183 fc2 = ((fc2 <= 1.0) && (fc2 > 0.0)) ? fc2/2 : 0.25; | |
184 k3 = k1 * fc2; // 2*pi*fc2 | |
185 k1 *= fc1; // 2*pi*fc1 | |
186 | |
187 if(flags & BP){ // Band pass | |
188 // Calculate center tap | |
189 if (o){ | |
190 g=w[end]*(fc1+fc2); | |
191 w[end] = (fc2 - fc1) * w[end] * 2.0; | |
192 } | |
193 | |
194 // Create filter | |
195 for (i=0 ; i<end ; i++){ | |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
196 t1 = (FLOAT_TYPE)(i+1) - k2; |
7568 | 197 t2 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2 |
198 t3 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1 | |
199 g += w[end-i-1] * (t3 + t2); // Total gain in filter | |
200 w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3); | |
201 } | |
202 } | |
203 else{ // Band stop | |
204 if (!o) // Band stop filters must have odd length | |
205 return -1; | |
206 w[end] = 1.0 - (fc2 - fc1) * w[end] * 2.0; | |
207 g= w[end]; | |
208 | |
209 // Create filter | |
210 for (i=0 ; i<end ; i++){ | |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
211 t1 = (FLOAT_TYPE)(i+1); |
7568 | 212 t2 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1 |
213 t3 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2 | |
214 w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3); | |
215 g += 2*w[end-i-1]; // Total gain in filter | |
216 } | |
217 } | |
218 } | |
219 | |
220 // Normalize gain | |
221 g=1/g; | |
222 for (i=0; i<n; i++) | |
223 w[i] *= g; | |
224 | |
225 return 0; | |
226 } | |
227 | |
228 /* Design polyphase FIR filter from prototype filter | |
229 | |
230 n length of prototype filter | |
231 k number of polyphase components | |
232 w prototype filter taps | |
233 pw Parallel FIR filter | |
234 g Filter gain | |
235 flags FWD forward indexing | |
236 REW reverse indexing | |
237 ODD multiply every 2nd filter tap by -1 => HP filter | |
238 | |
239 returns 0 if OK, -1 if fail | |
240 */ | |
27258 | 241 int af_filter_design_pfir(unsigned int n, unsigned int k, const FLOAT_TYPE* w, |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
242 FLOAT_TYPE** pw, FLOAT_TYPE g, unsigned int flags) |
7568 | 243 { |
244 int l = (int)n/k; // Length of individual FIR filters | |
245 int i; // Counters | |
246 int j; | |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
247 FLOAT_TYPE t; // g * w[i] |
7568 | 248 |
249 // Sanity check | |
250 if(l<1 || k<1 || !w || !pw) | |
251 return -1; | |
252 | |
253 // Do the stuff | |
254 if(flags&REW){ | |
255 for(j=l-1;j>-1;j--){//Columns | |
256 for(i=0;i<(int)k;i++){//Rows | |
257 t=g * *w++; | |
258 pw[i][j]=t * ((flags & ODD) ? ((j & 1) ? -1 : 1) : 1); | |
259 } | |
260 } | |
261 } | |
262 else{ | |
263 for(j=0;j<l;j++){//Columns | |
264 for(i=0;i<(int)k;i++){//Rows | |
265 t=g * *w++; | |
266 pw[i][j]=t * ((flags & ODD) ? ((j & 1) ? 1 : -1) : 1); | |
267 } | |
268 } | |
269 } | |
270 return -1; | |
271 } | |
8832
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 /****************************************************************************** |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
274 * 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
|
275 ******************************************************************************/ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
276 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
277 /* 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
|
278 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
279 /* 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
|
280 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
|
281 of it. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
282 */ |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
283 static void af_filter_prewarp(FLOAT_TYPE* a, FLOAT_TYPE fc, FLOAT_TYPE fs) |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
284 { |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
285 FLOAT_TYPE wp; |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
286 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
|
287 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
|
288 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
|
289 } |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
290 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
291 /* 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
|
292 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
|
293 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
294 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
|
295 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
296 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
|
297 H(z) = ------------------------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
298 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
|
299 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
300 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
|
301 order: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
302 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
|
303 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
|
304 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
305 Arguments: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
306 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
|
307 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
|
308 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
|
309 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
|
310 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
|
311 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
|
312 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
|
313 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
|
314 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
|
315 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
316 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
|
317 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
|
318 */ |
27258 | 319 static void af_filter_bilinear(const FLOAT_TYPE* a, const FLOAT_TYPE* b, FLOAT_TYPE* k, |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
320 FLOAT_TYPE fs, FLOAT_TYPE *coef) |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
321 { |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
322 FLOAT_TYPE ad, bd; |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
323 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
324 /* 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
|
325 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
|
326 /* 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
|
327 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
|
328 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
329 /* 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
|
330 *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
|
331 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
332 /* Denominator */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
333 *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
|
334 *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
|
335 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
336 /* Numerator */ |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
337 *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
|
338 *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
|
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 |
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 /* 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
|
344 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
|
345 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
|
346 |
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 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
|
349 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
350 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
|
351 order sections: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
352 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
353 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
|
354 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
355 n Polynomials |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
356 ------------------------------------------------------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
357 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
|
358 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
|
359 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
|
360 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
361 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
|
362 1 1 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
363 T(s) = --------------------------- * ---------------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
364 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
|
365 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
366 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
|
367 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
|
368 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
|
369 bilinear transform. |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
370 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
371 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
|
372 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
373 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
|
374 H(s) = ---------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
375 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
|
376 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
377 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
|
378 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
|
379 |
11000 | 380 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
|
381 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
382 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
|
383 --------------------------- * -------------------------- |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
384 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
|
385 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
386 Section 1: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
387 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
|
388 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
|
389 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
390 Section 2: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
391 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
|
392 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
|
393 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
394 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
|
395 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
|
396 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
|
397 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
|
398 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
399 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
400 Arguments: |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
401 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
|
402 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
|
403 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
|
404 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
|
405 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
|
406 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
|
407 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
|
408 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
|
409 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
|
410 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
|
411 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
412 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
|
413 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
|
414 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
|
415 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
|
416 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
|
417 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
418 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
|
419 */ |
27258 | 420 int af_filter_szxform(const FLOAT_TYPE* a, const FLOAT_TYPE* b, FLOAT_TYPE Q, FLOAT_TYPE fc, |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
421 FLOAT_TYPE fs, FLOAT_TYPE *k, FLOAT_TYPE *coef) |
8832
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
422 { |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
423 FLOAT_TYPE at[3]; |
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
424 FLOAT_TYPE bt[3]; |
8832
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 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
|
427 return -1; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
428 |
26350
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
429 memcpy(at,a,3*sizeof(FLOAT_TYPE)); |
07abe94a9cc4
Fix illegal identifier: Rename _ftype_t macro to FLOAT_TYPE.
diego
parents:
24890
diff
changeset
|
430 memcpy(bt,b,3*sizeof(FLOAT_TYPE)); |
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 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
|
433 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
434 /* 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
|
435 af_filter_prewarp(at, fc, fs); |
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
436 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
|
437 /* Execute bilinear transform */ |
14275
de13fd557440
less namespace pollution #2 (prefixed globals in filter.c with af_filter_)
alex
parents:
14274
diff
changeset
|
438 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
|
439 |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
440 return 0; |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
441 } |
a1578b329cc0
Adding sub-woofer filter, use this filter to add a sub channel to the audio stream
anders
parents:
7568
diff
changeset
|
442 |