7568
|
1 /*=============================================================================
|
|
2 //
|
|
3 // This software has been released under the terms of the GNU Public
|
|
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 */
|
|
14 #include <math.h>
|
|
15 #include "dsp.h"
|
|
16
|
|
17 /* C implementation of FIR filter y=w*x
|
|
18
|
|
19 n number of filter taps, where mod(n,4)==0
|
|
20 w filter taps
|
|
21 x input signal must be a circular buffer which is indexed backwards
|
|
22 */
|
|
23 inline _ftype_t fir(register unsigned int n, _ftype_t* w, _ftype_t* x)
|
|
24 {
|
|
25 register _ftype_t y; // Output
|
|
26 y = 0.0;
|
|
27 do{
|
|
28 n--;
|
|
29 y+=w[n]*x[n];
|
|
30 }while(n != 0);
|
|
31 return y;
|
|
32 }
|
|
33
|
|
34 /* C implementation of parallel FIR filter y(k)=w(k) * x(k) (where * denotes convolution)
|
|
35
|
|
36 n number of filter taps, where mod(n,4)==0
|
|
37 d number of filters
|
|
38 xi current index in xq
|
|
39 w filter taps k by n big
|
|
40 x input signal must be a circular buffers which are indexed backwards
|
|
41 y output buffer
|
|
42 s output buffer stride
|
|
43 */
|
|
44 inline _ftype_t* pfir(unsigned int n, unsigned int d, unsigned int xi, _ftype_t** w, _ftype_t** x, _ftype_t* y, unsigned int s)
|
|
45 {
|
|
46 register _ftype_t* xt = *x + xi;
|
|
47 register _ftype_t* wt = *w;
|
|
48 register int nt = 2*n;
|
|
49 while(d-- > 0){
|
|
50 *y = fir(n,wt,xt);
|
|
51 wt+=n;
|
|
52 xt+=nt;
|
|
53 y+=s;
|
|
54 }
|
|
55 return y;
|
|
56 }
|
|
57
|
|
58 /* Add new data to circular queue designed to be used with a parallel
|
|
59 FIR filter, with d filters. xq is the circular queue, in pointing
|
|
60 at the new samples, xi current index in xq and n the length of the
|
|
61 filter. xq must be n*2 by k big, s is the index for in.
|
|
62 */
|
|
63 inline int updatepq(unsigned int n, unsigned int d, unsigned int xi, _ftype_t** xq, _ftype_t* in, unsigned int s)
|
|
64 {
|
|
65 register _ftype_t* txq = *xq + xi;
|
|
66 register int nt = n*2;
|
|
67
|
|
68 while(d-- >0){
|
|
69 *txq= *(txq+n) = *in;
|
|
70 txq+=nt;
|
|
71 in+=s;
|
|
72 }
|
|
73 return (++xi)&(n-1);
|
|
74 }
|
|
75
|
|
76
|
|
77 /* Design FIR filter using the Window method
|
|
78
|
|
79 n filter length must be odd for HP and BS filters
|
|
80 w buffer for the filter taps (must be n long)
|
|
81 fc cutoff frequencies (1 for LP and HP, 2 for BP and BS)
|
|
82 0 < fc < 1 where 1 <=> Fs/2
|
|
83 flags window and filter type as defined in filter.h
|
|
84 variables are ored together: i.e. LP|HAMMING will give a
|
|
85 low pass filter designed using a hamming window
|
|
86 opt beta constant used only when designing using kaiser windows
|
|
87
|
|
88 returns 0 if OK, -1 if fail
|
|
89 */
|
|
90 int design_fir(unsigned int n, _ftype_t* w, _ftype_t* fc, unsigned int flags, _ftype_t opt)
|
|
91 {
|
|
92 unsigned int o = n & 1; // Indicator for odd filter length
|
|
93 unsigned int end = ((n + 1) >> 1) - o; // Loop end
|
|
94 unsigned int i; // Loop index
|
|
95
|
|
96 _ftype_t k1 = 2 * M_PI; // 2*pi*fc1
|
|
97 _ftype_t k2 = 0.5 * (_ftype_t)(1 - o);// Constant used if the filter has even length
|
|
98 _ftype_t k3; // 2*pi*fc2 Constant used in BP and BS design
|
|
99 _ftype_t g = 0.0; // Gain
|
|
100 _ftype_t t1,t2,t3; // Temporary variables
|
|
101 _ftype_t fc1,fc2; // Cutoff frequencies
|
|
102
|
|
103 // Sanity check
|
|
104 if(!w || (n == 0)) return -1;
|
|
105
|
|
106 // Get window coefficients
|
|
107 switch(flags & WINDOW_MASK){
|
|
108 case(BOXCAR):
|
|
109 boxcar(n,w); break;
|
|
110 case(TRIANG):
|
|
111 triang(n,w); break;
|
|
112 case(HAMMING):
|
|
113 hamming(n,w); break;
|
|
114 case(HANNING):
|
|
115 hanning(n,w); break;
|
|
116 case(BLACKMAN):
|
|
117 blackman(n,w); break;
|
|
118 case(FLATTOP):
|
|
119 flattop(n,w); break;
|
|
120 case(KAISER):
|
|
121 kaiser(n,w,opt); break;
|
|
122 default:
|
|
123 return -1;
|
|
124 }
|
|
125
|
|
126 if(flags & (LP | HP)){
|
|
127 fc1=*fc;
|
|
128 // Cutoff frequency must be < 0.5 where 0.5 <=> Fs/2
|
|
129 fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25;
|
|
130 k1 *= fc1;
|
|
131
|
|
132 if(flags & LP){ // Low pass filter
|
|
133
|
|
134 // If the filter length is odd, there is one point which is exactly
|
|
135 // in the middle. The value at this point is 2*fCutoff*sin(x)/x,
|
|
136 // where x is zero. To make sure nothing strange happens, we set this
|
|
137 // value separately.
|
|
138 if (o){
|
|
139 w[end] = fc1 * w[end] * 2.0;
|
|
140 g=w[end];
|
|
141 }
|
|
142
|
|
143 // Create filter
|
|
144 for (i=0 ; i<end ; i++){
|
|
145 t1 = (_ftype_t)(i+1) - k2;
|
|
146 w[end-i-1] = w[n-end+i] = w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc
|
|
147 g += 2*w[end-i-1]; // Total gain in filter
|
|
148 }
|
|
149 }
|
|
150 else{ // High pass filter
|
|
151 if (!o) // High pass filters must have odd length
|
|
152 return -1;
|
|
153 w[end] = 1.0 - (fc1 * w[end] * 2.0);
|
|
154 g= w[end];
|
|
155
|
|
156 // Create filter
|
|
157 for (i=0 ; i<end ; i++){
|
|
158 t1 = (_ftype_t)(i+1);
|
|
159 w[end-i-1] = w[n-end+i] = -1 * w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc
|
|
160 g += ((i&1) ? (2*w[end-i-1]) : (-2*w[end-i-1])); // Total gain in filter
|
|
161 }
|
|
162 }
|
|
163 }
|
|
164
|
|
165 if(flags & (BP | BS)){
|
|
166 fc1=fc[0];
|
|
167 fc2=fc[1];
|
|
168 // Cutoff frequencies must be < 1.0 where 1.0 <=> Fs/2
|
|
169 fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25;
|
|
170 fc2 = ((fc2 <= 1.0) && (fc2 > 0.0)) ? fc2/2 : 0.25;
|
|
171 k3 = k1 * fc2; // 2*pi*fc2
|
|
172 k1 *= fc1; // 2*pi*fc1
|
|
173
|
|
174 if(flags & BP){ // Band pass
|
|
175 // Calculate center tap
|
|
176 if (o){
|
|
177 g=w[end]*(fc1+fc2);
|
|
178 w[end] = (fc2 - fc1) * w[end] * 2.0;
|
|
179 }
|
|
180
|
|
181 // Create filter
|
|
182 for (i=0 ; i<end ; i++){
|
|
183 t1 = (_ftype_t)(i+1) - k2;
|
|
184 t2 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2
|
|
185 t3 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1
|
|
186 g += w[end-i-1] * (t3 + t2); // Total gain in filter
|
|
187 w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3);
|
|
188 }
|
|
189 }
|
|
190 else{ // Band stop
|
|
191 if (!o) // Band stop filters must have odd length
|
|
192 return -1;
|
|
193 w[end] = 1.0 - (fc2 - fc1) * w[end] * 2.0;
|
|
194 g= w[end];
|
|
195
|
|
196 // Create filter
|
|
197 for (i=0 ; i<end ; i++){
|
|
198 t1 = (_ftype_t)(i+1);
|
|
199 t2 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1
|
|
200 t3 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2
|
|
201 w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3);
|
|
202 g += 2*w[end-i-1]; // Total gain in filter
|
|
203 }
|
|
204 }
|
|
205 }
|
|
206
|
|
207 // Normalize gain
|
|
208 g=1/g;
|
|
209 for (i=0; i<n; i++)
|
|
210 w[i] *= g;
|
|
211
|
|
212 return 0;
|
|
213 }
|
|
214
|
|
215 /* Design polyphase FIR filter from prototype filter
|
|
216
|
|
217 n length of prototype filter
|
|
218 k number of polyphase components
|
|
219 w prototype filter taps
|
|
220 pw Parallel FIR filter
|
|
221 g Filter gain
|
|
222 flags FWD forward indexing
|
|
223 REW reverse indexing
|
|
224 ODD multiply every 2nd filter tap by -1 => HP filter
|
|
225
|
|
226 returns 0 if OK, -1 if fail
|
|
227 */
|
|
228 int design_pfir(unsigned int n, unsigned int k, _ftype_t* w, _ftype_t** pw, _ftype_t g, unsigned int flags)
|
|
229 {
|
|
230 int l = (int)n/k; // Length of individual FIR filters
|
|
231 int i; // Counters
|
|
232 int j;
|
|
233 _ftype_t t; // g * w[i]
|
|
234
|
|
235 // Sanity check
|
|
236 if(l<1 || k<1 || !w || !pw)
|
|
237 return -1;
|
|
238
|
|
239 // Do the stuff
|
|
240 if(flags&REW){
|
|
241 for(j=l-1;j>-1;j--){//Columns
|
|
242 for(i=0;i<(int)k;i++){//Rows
|
|
243 t=g * *w++;
|
|
244 pw[i][j]=t * ((flags & ODD) ? ((j & 1) ? -1 : 1) : 1);
|
|
245 }
|
|
246 }
|
|
247 }
|
|
248 else{
|
|
249 for(j=0;j<l;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 return -1;
|
|
257 }
|