ffmpeg / libavcodec / iirfilter.c @ fcdf0a43
History  View  Annotate  Download (10.7 KB)
1 
/*


2 
* IIR filter

3 
* Copyright (c) 2008 Konstantin Shishkov

4 
*

5 
* This file is part of FFmpeg.

6 
*

7 
* FFmpeg is free software; you can redistribute it and/or

8 
* modify it under the terms of the GNU Lesser General Public

9 
* License as published by the Free Software Foundation; either

10 
* version 2.1 of the License, or (at your option) any later version.

11 
*

12 
* FFmpeg is distributed in the hope that it will be useful,

13 
* but WITHOUT ANY WARRANTY; without even the implied warranty of

14 
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU

15 
* Lesser General Public License for more details.

16 
*

17 
* You should have received a copy of the GNU Lesser General Public

18 
* License along with FFmpeg; if not, write to the Free Software

19 
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 021101301 USA

20 
*/

21  
22 
/**

23 
* @file

24 
* different IIR filters implementation

25 
*/

26  
27 
#include "iirfilter.h" 
28 
#include <math.h> 
29  
30 
/**

31 
* IIR filter global parameters

32 
*/

33 
typedef struct FFIIRFilterCoeffs{ 
34 
int order;

35 
float gain;

36 
int *cx;

37 
float *cy;

38 
}FFIIRFilterCoeffs; 
39  
40 
/**

41 
* IIR filter state

42 
*/

43 
typedef struct FFIIRFilterState{ 
44 
float x[1]; 
45 
}FFIIRFilterState; 
46  
47 
/// maximum supported filter order

48 
#define MAXORDER 30 
49  
50 
static int butterworth_init_coeffs(void *avc, struct FFIIRFilterCoeffs *c, 
51 
enum IIRFilterMode filt_mode,

52 
int order, float cutoff_ratio, 
53 
float stopband)

54 
{ 
55 
int i, j;

56 
double wa;

57 
double p[MAXORDER + 1][2]; 
58  
59 
if (filt_mode != FF_FILTER_MODE_LOWPASS) {

60 
av_log(avc, AV_LOG_ERROR, "Butterworth filter currently only supports "

61 
"lowpass filter mode\n");

62 
return 1; 
63 
} 
64 
if (order & 1) { 
65 
av_log(avc, AV_LOG_ERROR, "Butterworth filter currently only supports "

66 
"even filter orders\n");

67 
return 1; 
68 
} 
69  
70 
wa = 2 * tan(M_PI * 0.5 * cutoff_ratio); 
71  
72 
c>cx[0] = 1; 
73 
for(i = 1; i < (order >> 1) + 1; i++) 
74 
c>cx[i] = c>cx[i  1] * (order  i + 1LL) / i; 
75  
76 
p[0][0] = 1.0; 
77 
p[0][1] = 0.0; 
78 
for(i = 1; i <= order; i++) 
79 
p[i][0] = p[i][1] = 0.0; 
80 
for(i = 0; i < order; i++){ 
81 
double zp[2]; 
82 
double th = (i + (order >> 1) + 0.5) * M_PI / order; 
83 
double a_re, a_im, c_re, c_im;

84 
zp[0] = cos(th) * wa;

85 
zp[1] = sin(th) * wa;

86 
a_re = zp[0] + 2.0; 
87 
c_re = zp[0]  2.0; 
88 
a_im = 
89 
c_im = zp[1];

90 
zp[0] = (a_re * c_re + a_im * c_im) / (c_re * c_re + c_im * c_im);

91 
zp[1] = (a_im * c_re  a_re * c_im) / (c_re * c_re + c_im * c_im);

92  
93 
for(j = order; j >= 1; j) 
94 
{ 
95 
a_re = p[j][0];

96 
a_im = p[j][1];

97 
p[j][0] = a_re*zp[0]  a_im*zp[1] + p[j1][0]; 
98 
p[j][1] = a_re*zp[1] + a_im*zp[0] + p[j1][1]; 
99 
} 
100 
a_re = p[0][0]*zp[0]  p[0][1]*zp[1]; 
101 
p[0][1] = p[0][0]*zp[1] + p[0][1]*zp[0]; 
102 
p[0][0] = a_re; 
103 
} 
104 
c>gain = p[order][0];

105 
for(i = 0; i < order; i++){ 
106 
c>gain += p[i][0];

107 
c>cy[i] = (p[i][0] * p[order][0] + p[i][1] * p[order][1]) / 
108 
(p[order][0] * p[order][0] + p[order][1] * p[order][1]); 
109 
} 
110 
c>gain /= 1 << order;

111  
112 
return 0; 
113 
} 
114  
115 
static int biquad_init_coeffs(void *avc, struct FFIIRFilterCoeffs *c, 
116 
enum IIRFilterMode filt_mode, int order, 
117 
float cutoff_ratio, float stopband) 
118 
{ 
119 
double cos_w0, sin_w0;

120 
double a0, x0, x1;

121  
122 
if (filt_mode != FF_FILTER_MODE_HIGHPASS &&

123 
filt_mode != FF_FILTER_MODE_LOWPASS) { 
124 
av_log(avc, AV_LOG_ERROR, "Biquad filter currently only supports "

125 
"highpass and lowpass filter modes\n");

126 
return 1; 
127 
} 
128 
if (order != 2) { 
129 
av_log(avc, AV_LOG_ERROR, "Biquad filter must have order of 2\n");

130 
return 1; 
131 
} 
132  
133 
cos_w0 = cos(M_PI * cutoff_ratio); 
134 
sin_w0 = sin(M_PI * cutoff_ratio); 
135  
136 
a0 = 1.0 + (sin_w0 / 2.0); 
137  
138 
if (filt_mode == FF_FILTER_MODE_HIGHPASS) {

139 
c>gain = ((1.0 + cos_w0) / 2.0) / a0; 
140 
x0 = ((1.0 + cos_w0)) / a0; 
141 
x1 = ((1.0 + cos_w0) / 2.0) / a0; 
142 
} else { // FF_FILTER_MODE_LOWPASS 
143 
c>gain = ((1.0  cos_w0) / 2.0) / a0; 
144 
x0 = (1.0  cos_w0) / a0; 
145 
x1 = ((1.0  cos_w0) / 2.0) / a0; 
146 
} 
147 
c>cy[0] = (2.0 * cos_w0) / a0; 
148 
c>cy[1] = (1.0 + (sin_w0 / 2.0)) / a0; 
149  
150 
// divide by gain to make the x coeffs integers.

151 
// during filtering, the delay state will include the gain multiplication

152 
c>cx[0] = lrintf(x0 / c>gain);

153 
c>cx[1] = lrintf(x1 / c>gain);

154 
c>cy[0] /= c>gain;

155 
c>cy[1] /= c>gain;

156  
157 
return 0; 
158 
} 
159  
160 
av_cold struct FFIIRFilterCoeffs* ff_iir_filter_init_coeffs(void *avc, 
161 
enum IIRFilterType filt_type,

162 
enum IIRFilterMode filt_mode,

163 
int order, float cutoff_ratio, 
164 
float stopband, float ripple) 
165 
{ 
166 
FFIIRFilterCoeffs *c; 
167  
168 
if (order <= 0  order > MAXORDER  cutoff_ratio >= 1.0) 
169 
return NULL; 
170  
171 
FF_ALLOCZ_OR_GOTO(avc, c, sizeof(FFIIRFilterCoeffs),

172 
init_fail); 
173 
FF_ALLOC_OR_GOTO (avc, c>cx, sizeof(c>cx[0]) * ((order >> 1) + 1), 
174 
init_fail); 
175 
FF_ALLOC_OR_GOTO (avc, c>cy, sizeof(c>cy[0]) * order, 
176 
init_fail); 
177 
c>order = order; 
178  
179 
if (filt_type == FF_FILTER_TYPE_BUTTERWORTH) {

180 
if (butterworth_init_coeffs(avc, c, filt_mode, order, cutoff_ratio,

181 
stopband)) { 
182 
goto init_fail;

183 
} 
184 
} else if (filt_type == FF_FILTER_TYPE_BIQUAD) { 
185 
if (biquad_init_coeffs(avc, c, filt_mode, order, cutoff_ratio,

186 
stopband)) { 
187 
goto init_fail;

188 
} 
189 
} else {

190 
av_log(avc, AV_LOG_ERROR, "filter type is not currently implemented\n");

191 
goto init_fail;

192 
} 
193  
194 
return c;

195  
196 
init_fail:

197 
ff_iir_filter_free_coeffs(c); 
198 
return NULL; 
199 
} 
200  
201 
av_cold struct FFIIRFilterState* ff_iir_filter_init_state(int order) 
202 
{ 
203 
FFIIRFilterState* s = av_mallocz(sizeof(FFIIRFilterState) + sizeof(s>x[0]) * (order  1)); 
204 
return s;

205 
} 
206  
207 
#define CONV_S16(dest, source) dest = av_clip_int16(lrintf(source));

208  
209 
#define CONV_FLT(dest, source) dest = source;

210  
211 
#define FILTER_BW_O4_1(i0, i1, i2, i3, fmt) \

212 
in = *src0 * c>gain \ 
213 
+ c>cy[0]*s>x[i0] + c>cy[1]*s>x[i1] \ 
214 
+ c>cy[2]*s>x[i2] + c>cy[3]*s>x[i3]; \ 
215 
res = (s>x[i0] + in )*1 \

216 
+ (s>x[i1] + s>x[i3])*4 \

217 
+ s>x[i2] *6; \

218 
CONV_##fmt(*dst0, res) \ 
219 
s>x[i0] = in; \ 
220 
src0 += sstep; \ 
221 
dst0 += dstep; 
222  
223 
#define FILTER_BW_O4(type, fmt) { \

224 
int i; \

225 
const type *src0 = src; \

226 
type *dst0 = dst; \ 
227 
for (i = 0; i < size; i += 4) { \ 
228 
float in, res; \

229 
FILTER_BW_O4_1(0, 1, 2, 3, fmt); \ 
230 
FILTER_BW_O4_1(1, 2, 3, 0, fmt); \ 
231 
FILTER_BW_O4_1(2, 3, 0, 1, fmt); \ 
232 
FILTER_BW_O4_1(3, 0, 1, 2, fmt); \ 
233 
} \ 
234 
} 
235  
236 
#define FILTER_DIRECT_FORM_II(type, fmt) { \

237 
int i; \

238 
const type *src0 = src; \

239 
type *dst0 = dst; \ 
240 
for (i = 0; i < size; i++) { \ 
241 
int j; \

242 
float in, res; \

243 
in = *src0 * c>gain; \ 
244 
for(j = 0; j < c>order; j++) \ 
245 
in += c>cy[j] * s>x[j]; \ 
246 
res = s>x[0] + in + s>x[c>order >> 1] * c>cx[c>order >> 1]; \ 
247 
for(j = 1; j < c>order >> 1; j++) \ 
248 
res += (s>x[j] + s>x[c>order  j]) * c>cx[j]; \ 
249 
for(j = 0; j < c>order  1; j++) \ 
250 
s>x[j] = s>x[j + 1]; \

251 
CONV_##fmt(*dst0, res) \ 
252 
s>x[c>order  1] = in; \

253 
src0 += sstep; \ 
254 
dst0 += dstep; \ 
255 
} \ 
256 
} 
257  
258 
void ff_iir_filter(const struct FFIIRFilterCoeffs *c, 
259 
struct FFIIRFilterState *s, int size, 
260 
const int16_t *src, int sstep, int16_t *dst, int dstep) 
261 
{ 
262 
if (c>order == 4) { 
263 
FILTER_BW_O4(int16_t, S16) 
264 
} else {

265 
FILTER_DIRECT_FORM_II(int16_t, S16) 
266 
} 
267 
} 
268  
269 
void ff_iir_filter_flt(const struct FFIIRFilterCoeffs *c, 
270 
struct FFIIRFilterState *s, int size, 
271 
const float *src, int sstep, void *dst, int dstep) 
272 
{ 
273 
if (c>order == 4) { 
274 
FILTER_BW_O4(float, FLT)

275 
} else {

276 
FILTER_DIRECT_FORM_II(float, FLT)

277 
} 
278 
} 
279  
280 
av_cold void ff_iir_filter_free_state(struct FFIIRFilterState *state) 
281 
{ 
282 
av_free(state); 
283 
} 
284  
285 
av_cold void ff_iir_filter_free_coeffs(struct FFIIRFilterCoeffs *coeffs) 
286 
{ 
287 
if(coeffs){

288 
av_free(coeffs>cx); 
289 
av_free(coeffs>cy); 
290 
} 
291 
av_free(coeffs); 
292 
} 
293  
294 
#ifdef TEST

295 
#define FILT_ORDER 4 
296 
#define SIZE 1024 
297 
int main(void) 
298 
{ 
299 
struct FFIIRFilterCoeffs *fcoeffs = NULL; 
300 
struct FFIIRFilterState *fstate = NULL; 
301 
float cutoff_coeff = 0.4; 
302 
int16_t x[SIZE], y[SIZE]; 
303 
int i;

304 
FILE* fd; 
305  
306 
fcoeffs = ff_iir_filter_init_coeffs(FF_FILTER_TYPE_BUTTERWORTH, 
307 
FF_FILTER_MODE_LOWPASS, FILT_ORDER, 
308 
cutoff_coeff, 0.0, 0.0); 
309 
fstate = ff_iir_filter_init_state(FILT_ORDER); 
310  
311 
for (i = 0; i < SIZE; i++) { 
312 
x[i] = lrint(0.75 * INT16_MAX * sin(0.5*M_PI*i*i/SIZE)); 
313 
} 
314  
315 
ff_iir_filter(fcoeffs, fstate, SIZE, x, 1, y, 1); 
316  
317 
fd = fopen("in.bin", "w"); 
318 
fwrite(x, sizeof(x[0]), SIZE, fd); 
319 
fclose(fd); 
320  
321 
fd = fopen("out.bin", "w"); 
322 
fwrite(y, sizeof(y[0]), SIZE, fd); 
323 
fclose(fd); 
324  
325 
ff_iir_filter_free_coeffs(fcoeffs); 
326 
ff_iir_filter_free_state(fstate); 
327 
return 0; 
328 
} 
329 
#endif /* TEST */ 