ffmpeg / libavcodec / iirfilter.c @ c3897d76
History  View  Annotate  Download (6.28 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 
av_cold struct FFIIRFilterCoeffs* ff_iir_filter_init_coeffs(enum IIRFilterType filt_type, 
51 
enum IIRFilterMode filt_mode,

52 
int order, float cutoff_ratio, 
53 
float stopband, float ripple) 
54 
{ 
55 
int i, j;

56 
FFIIRFilterCoeffs *c; 
57 
double wa;

58 
double p[MAXORDER + 1][2]; 
59  
60 
if(filt_type != FF_FILTER_TYPE_BUTTERWORTH  filt_mode != FF_FILTER_MODE_LOWPASS)

61 
return NULL; 
62 
if(order <= 1  (order & 1)  order > MAXORDER  cutoff_ratio >= 1.0) 
63 
return NULL; 
64  
65 
c = av_malloc(sizeof(FFIIRFilterCoeffs));

66 
c>cx = av_malloc(sizeof(c>cx[0]) * ((order >> 1) + 1)); 
67 
c>cy = av_malloc(sizeof(c>cy[0]) * order); 
68 
c>order = order; 
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 c;

113 
} 
114  
115 
av_cold struct FFIIRFilterState* ff_iir_filter_init_state(int order) 
116 
{ 
117 
FFIIRFilterState* s = av_mallocz(sizeof(FFIIRFilterState) + sizeof(s>x[0]) * (order  1)); 
118 
return s;

119 
} 
120  
121 
#define FILTER(i0, i1, i2, i3) \

122 
in = *src * c>gain \ 
123 
+ c>cy[0]*s>x[i0] + c>cy[1]*s>x[i1] \ 
124 
+ c>cy[2]*s>x[i2] + c>cy[3]*s>x[i3]; \ 
125 
res = (s>x[i0] + in )*1 \

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

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

128 
*dst = av_clip_int16(lrintf(res)); \ 
129 
s>x[i0] = in; \ 
130 
src += sstep; \ 
131 
dst += dstep; \ 
132  
133 
void ff_iir_filter(const struct FFIIRFilterCoeffs *c, struct FFIIRFilterState *s, int size, const int16_t *src, int sstep, int16_t *dst, int dstep) 
134 
{ 
135 
int i;

136  
137 
if(c>order == 4){ 
138 
for(i = 0; i < size; i += 4){ 
139 
float in, res;

140  
141 
FILTER(0, 1, 2, 3); 
142 
FILTER(1, 2, 3, 0); 
143 
FILTER(2, 3, 0, 1); 
144 
FILTER(3, 0, 1, 2); 
145 
} 
146 
}else{

147 
for(i = 0; i < size; i++){ 
148 
int j;

149 
float in, res;

150 
in = *src * c>gain; 
151 
for(j = 0; j < c>order; j++) 
152 
in += c>cy[j] * s>x[j]; 
153 
res = s>x[0] + in + s>x[c>order >> 1] * c>cx[c>order >> 1]; 
154 
for(j = 1; j < c>order >> 1; j++) 
155 
res += (s>x[j] + s>x[c>order  j]) * c>cx[j]; 
156 
for(j = 0; j < c>order  1; j++) 
157 
s>x[j] = s>x[j + 1];

158 
*dst = av_clip_int16(lrintf(res)); 
159 
s>x[c>order  1] = in;

160 
src += sstep; 
161 
dst += dstep; 
162 
} 
163 
} 
164 
} 
165  
166 
av_cold void ff_iir_filter_free_state(struct FFIIRFilterState *state) 
167 
{ 
168 
av_free(state); 
169 
} 
170  
171 
av_cold void ff_iir_filter_free_coeffs(struct FFIIRFilterCoeffs *coeffs) 
172 
{ 
173 
if(coeffs){

174 
av_free(coeffs>cx); 
175 
av_free(coeffs>cy); 
176 
} 
177 
av_free(coeffs); 
178 
} 
179  
180 
#ifdef TEST

181 
#define FILT_ORDER 4 
182 
#define SIZE 1024 
183 
int main(void) 
184 
{ 
185 
struct FFIIRFilterCoeffs *fcoeffs = NULL; 
186 
struct FFIIRFilterState *fstate = NULL; 
187 
float cutoff_coeff = 0.4; 
188 
int16_t x[SIZE], y[SIZE]; 
189 
int i;

190 
FILE* fd; 
191  
192 
fcoeffs = ff_iir_filter_init_coeffs(FF_FILTER_TYPE_BUTTERWORTH, 
193 
FF_FILTER_MODE_LOWPASS, FILT_ORDER, 
194 
cutoff_coeff, 0.0, 0.0); 
195 
fstate = ff_iir_filter_init_state(FILT_ORDER); 
196  
197 
for (i = 0; i < SIZE; i++) { 
198 
x[i] = lrint(0.75 * INT16_MAX * sin(0.5*M_PI*i*i/SIZE)); 
199 
} 
200  
201 
ff_iir_filter(fcoeffs, fstate, SIZE, x, 1, y, 1); 
202  
203 
fd = fopen("in.bin", "w"); 
204 
fwrite(x, sizeof(x[0]), SIZE, fd); 
205 
fclose(fd); 
206  
207 
fd = fopen("out.bin", "w"); 
208 
fwrite(y, sizeof(y[0]), SIZE, fd); 
209 
fclose(fd); 
210  
211 
ff_iir_filter_free_coeffs(fcoeffs); 
212 
ff_iir_filter_free_state(fstate); 
213 
return 0; 
214 
} 
215 
#endif /* TEST */ 