3
* $Id: iir.c,v 1.21 2001/12/16 17:39:17 mag Exp $
5
* Copyright (C) 2000 Alexander Ehlert
7
* This program is free software; you can redistribute it and/or modify
8
* it under the terms of the GNU General Public License as published by
9
* the Free Software Foundation; either version 2 of the License, or
10
* (at your option) any later version.
12
* This program 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
15
* GNU General Public License for more details.
17
* You should have received a copy of the GNU General Public License
18
* along with this program; if not, write to the Free Software
19
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
28
#include <sys/types.h>
36
PLUGIN_SET(iir, "highpass lowpass bandpass bandpass_a")
38
#define GLAME_IIR_LOWPASS 0
39
#define GLAME_IIR_HIGHPASS 1
40
#define GLAME_IIR_BANDPASS 2
41
#define GLAME_IIR_BANDPASS_A 3
43
/* Just query the name of the plugin we get called from
44
* to identify the right filter mode
45
* returns the filter mode or -1 on error
48
int identify_plugin(filter_t *n){
49
const char *name = plugin_name(n->plugin);
51
DPRINTF("plugin name = %s filter name = %s\n", name, n->name);
56
if (strcmp(name,"lowpass")==0)
57
return GLAME_IIR_LOWPASS;
58
else if (strcmp(name,"highpass")==0)
59
return GLAME_IIR_HIGHPASS;
60
else if (strcmp(name,"bandpass")==0)
61
return GLAME_IIR_BANDPASS;
62
else if (strcmp(name,"bandpass_a")==0)
63
return GLAME_IIR_BANDPASS_A;
68
/* To get better filter accuracy I decided to compute the single
69
* stages of the filter seperatly and apply them one by one
70
* to the sample data. According to the DSPGUIDE chapter 20 pp339
71
* filters are more stable when applied in stages.
72
* Who doesn't like that can still combine
73
* all stages to one stage by just convoluting the single stages.
74
* But in the moment it's up to the user that he knows what he's doing.
75
* float accuracy can't be enough for certain parameters.
79
#define CLAMP(x,mi,ma) ( (x < mi) ? mi : ( (x>ma) ? ma : x))
81
/* (hopefully) generic description of an iir filter */
83
typedef struct glame_iir glame_iir_t;
85
int np; /* Number of poles */
86
int mode; /* Filter mode low/high/bandpass... */
87
int nstages; /* Number of filterstages */
88
int na; /* number of a coefficients per stage */
89
int nb; /* number of b coefficients per stage */
90
gliirt fc; /* cutoff/center frequency */
91
gliirt bw; /* bandwidth for bandpass */
92
gliirt ppr; /* percent of ripple in passband */
93
gliirt spr; /* percent of ripple in stopband */
94
gliirt **coeff; /* Actual filter coefficients */
97
glame_iir_t *init_glame_iir(int mode, int nstages, int na, int nb){
98
glame_iir_t *dum=NULL;
100
if ((dum=ALLOCN(1,glame_iir_t))){
102
dum->nstages=nstages;
105
dum->coeff=(gliirt **)malloc(nstages*sizeof(gliirt *));
106
for(i=0;i<nstages;i++)
107
dum->coeff[i]=(gliirt *)malloc((na+nb)*sizeof(gliirt));
112
glame_iir_t *combine_iir_stages(int mode, glame_iir_t *first, glame_iir_t *second){
113
int stages, i, j, cnt;
116
stages = first->nstages + second->nstages;
118
if ((first->na != second->na) || (first->nb != second->nb))
121
if ((gt = init_glame_iir(mode, stages, first->na, first->nb))==NULL)
124
cnt = first->na + first->nb;
126
/* copy coefficients */
127
for(i=0; i<first->nstages; i++)
129
gt->coeff[i][j]=first->coeff[i][j];
131
for(i=first->nstages; i<stages; i++)
133
gt->coeff[i][j]=second->coeff[i-first->nstages][j];
138
void free_glame_iir(glame_iir_t *gt){
140
for(i=0;i<gt->nstages;i++)
146
/* center: frequency already normalized between 0 and 0.5 of sampling
147
* bandwidth given in octaves between lower and upper -3dB point
150
glame_iir_t *calc_2polebandpass(float center, float bandwidth)
153
double omega, alpha, gain;
156
if ((gt=init_glame_iir(GLAME_IIR_BANDPASS, 1, 3, 2))==NULL)
159
omega = 2.0*M_PI*center;
160
DPRINTF("omega=%f\n", omega);
161
alpha = sin(omega)*sinh(log(2.0)/2.0*bandwidth*omega/sin(omega));
162
/*alpha=sin(omega)/2.0;*/
163
DPRINTF("alpha=%f\n", alpha);
164
gt->coeff[0][0] = alpha;
165
gt->coeff[0][1] = 0.0;
166
gt->coeff[0][2] = -alpha;
167
gt->coeff[0][3] = 2.0 * cos(omega);
168
gt->coeff[0][4] = alpha - 1.0;
171
gt->coeff[0][i]/=gain;
175
/* chebyshev calculates coefficients for a chebyshev filter
177
* n number of poles(2,4,6,...)
178
* m 0..lowpass, 1..highpass
179
* fc cutoff frequency in percent of samplerate
180
* pr percent ripple in passband (0.5 is optimal)
182
* Code from DSPGUIDE Chapter 20, pp341
183
* online version http://www.dspguide.com
186
#define chebtype double
188
int chebyshev_stage(glame_iir_t *gt, int n){
189
chebtype h,rp,ip,es,kx,vx,t,w,m,d,k,gain;
190
chebtype *x=NULL, *y=NULL, *a=NULL, *b=NULL;
195
if (gt->na+gt->nb!=5)
198
if (!(x=ALLOCN(3,chebtype)))
200
if (!(y=ALLOCN(2,chebtype)))
202
if (!(a=ALLOCN(3,chebtype)))
204
if (!(b=ALLOCN(2,chebtype)))
207
h=M_PI/((chebtype)gt->np*2.0)+n*M_PI/(chebtype)gt->np;
211
DPRINTF("rp=%f ip=%f np=%d h=%f ppr=%f\n",rp,ip,gt->np,h,gt->ppr);
214
h=100.0/(100.0-gt->ppr);
217
vx=1.0/(chebtype)gt->np*log(h+sqrt(h*h+1.0));
218
kx=1.0/(chebtype)gt->np*log(h+sqrt(h*h-1.0));
219
kx=(exp(kx)+exp(-kx))/2.0;
221
rp*=(h-1.0/h)*0.5/kx;
222
ip*=(h+1.0/h)*0.5/kx;
225
DPRINTF("rp=%f ip=%f es=%f kx=%f vx=%f h=%f\n",rp,ip,es,kx,vx,h);
230
d=4.0-4.0*rp*t+m*t*t;
234
y[0]=(8.0-2.0*m*t*t)/d;
235
y[1]=(-4.0-4.0*rp*t-m*t*t)/d;
237
DPRINTF("\nt=%f\nw=%f\nm=%f\nd=%f\n",t,w,m,d);
238
DPRINTF("\nx0=%f\nx1=%f\nx2=%f\ny1=%f\ny2=%f\n",x[0],x[1],x[2],y[0],y[1]);
240
if (gt->mode==GLAME_IIR_HIGHPASS)
241
k=-cos(w*0.5+0.5)/cos(w*0.5-0.5);
243
k=sin(0.5-w*0.5)/sin(0.5+w*0.5);
245
a[0]=(x[0]-x[1]*k+x[2]*k*k)/d;
246
a[1]=(-2.0*x[0]*k+x[1]+x[1]*k*k-2.0*x[2]*k)/d;
247
a[2]=(x[0]*k*k-x[1]*k+x[2])/d;
248
b[0]=(2.0*k+y[0]+y[0]*k*k-2.0*y[1]*k)/d;
249
b[1]=(-k*k-y[0]*k+y[1])/d;
250
if(gt->mode==GLAME_IIR_HIGHPASS){
255
DPRINTF("n=%d a0=%e a1=%e a2=%e b1=%e b2=%e\n",n,a[0],a[1],a[2],b[0],b[1]);
257
/* FIXME gain correction for every stage is probably wrong */
259
if(gt->mode==GLAME_IIR_HIGHPASS)
260
gain=(a[0]-a[1]+a[2])/(1.0+b[0]-b[1]);
262
gain=(a[0]+a[1]+a[2])/(1.0-b[0]-b[1]);
263
for(i=0;i<3;i++) a[i]/=gain;
266
DPRINTF("n=%d a0=%e a1=%e a2=%e b1=%e b2=%e gain=%e\n",n,a[0],a[1],a[2],b[0],b[1],gain);
268
gt->coeff[n][0]=(gliirt)(a[0]);
269
gt->coeff[n][1]=(gliirt)(a[1]);
270
gt->coeff[n][2]=(gliirt)(a[2]);
271
gt->coeff[n][3]=(gliirt)(b[0]);
272
gt->coeff[n][4]=(gliirt)(b[1]);
283
glame_iir_t *chebyshev(int n, int mode, float fc, float pr){
288
DPRINTF("number of poles must be even.\n");
292
if ((mode!=0) && (mode!=1)){
293
DPRINTF("Mode %d not supported.\n",mode);
297
if ((fc<0.0) || (fc>0.5)){
298
DPRINTF("Invalid range for cutoff frequency(0-0.5)\n");
302
if ((gt=init_glame_iir(mode,n/2,3,2))){
303
DPRINTF("init_glame_iir done!\n");
308
chebyshev_stage(gt,i);
309
DPRINTF("Filter setup done!\n");
314
static int iir_f(filter_t *n)
323
filter_pipe_t *in, *out;
324
filter_buffer_t *inb;
325
glame_iir_t *gt, *first, *second;
328
int stages, mode, rate;
329
float ripple, fc, ufc, lfc, bw;
331
if (!(in = filterport_get_pipe(filterportdb_get_port(filter_portdb(n), PORTNAME_IN)))
332
|| !(out = filterport_get_pipe(filterportdb_get_port(filter_portdb(n), PORTNAME_OUT))))
333
FILTER_ERROR_RETURN("no in- or output");
336
rate = filterpipe_sample_rate(in);
338
/* identify the filter mode */
340
if ((mode=identify_plugin(n))==-1)
341
FILTER_ERROR_RETURN("Doh! Filter mode not recognized\n");
344
if (mode < GLAME_IIR_BANDPASS) {
345
stages = filterparam_val_int(filterparamdb_get_param(filter_paramdb(n), "stages"));
346
fc = filterparam_val_float(filterparamdb_get_param(filter_paramdb(n), "cutoff"))/(float)rate;
347
fc = CLAMP(fc, 0.0, 0.5);
349
ripple = filterparam_val_float(filterparamdb_get_param(filter_paramdb(n), "ripple"));
351
DPRINTF("poles=%d mode=%d fc=%f ripple=%f\n", stages*2,mode,fc,ripple);
353
if (!(gt=chebyshev(stages*2, mode, fc, ripple)))
354
FILTER_ERROR_RETURN("chebyshev failed");
356
} else if (mode == GLAME_IIR_BANDPASS) {
357
stages = filterparam_val_int(filterparamdb_get_param(filter_paramdb(n), "stages"));
359
fc = filterparam_val_float(filterparamdb_get_param(filter_paramdb(n), "center"))/(float)rate;
360
fc = CLAMP(fc, 0.0, 0.5);
362
bw = filterparam_val_float(filterparamdb_get_param(filter_paramdb(n), "width"))/(float)rate;
363
bw = CLAMP(bw, 0.0, 0.5);
367
ufc = CLAMP(ufc, 0.0, 0.5);
368
lfc = CLAMP(lfc, 0.0, 0.5);
370
ripple=filterparam_val_float(filterparamdb_get_param(filter_paramdb(n), "ripple"));
372
if (!(first=chebyshev(stages*2,GLAME_IIR_LOWPASS,ufc,ripple)))
373
FILTER_ERROR_RETURN("chebyshev failed");
375
if (!(second=chebyshev(stages*2,GLAME_IIR_HIGHPASS,lfc,ripple)))
376
FILTER_ERROR_RETURN("chebyshev failed");
378
if (!(gt = combine_iir_stages(mode, first, second)))
379
FILTER_ERROR_RETURN("combine stages failed");
381
free_glame_iir(first);
382
free_glame_iir(second);
384
} else if (mode == GLAME_IIR_BANDPASS_A) {
386
fc = filterparam_val_float(filterparamdb_get_param(filter_paramdb(n), "center"));
387
/* normalize center frequency */
388
nfc= CLAMP(fc/(float)rate, 0.0, 0.5);
390
bw = filterparam_val_float(filterparamdb_get_param(filter_paramdb(n), "width"));
394
DPRINTF("bandwidth(HZ)=%f\n", bw);
395
/* calculate width in octaves log_2(x) = log(x)/log(2) */
396
bw = log((fc+bw*0.5)/(fc-bw*0.5+1.0))/log(2.0);
398
DPRINTF("center = %f, width in octaves = %f\n", nfc, bw);
400
if (!(gt=calc_2polebandpass(nfc, bw)))
401
FILTER_ERROR_RETURN("bandpass failed");
405
/* here follows the generic code */
408
FILTER_ERROR_RETURN("iir filter needs at least one stage");
410
if (!(iirf=ALLOCN(gt->nstages,iirf_t)))
411
FILTER_ERROR_RETURN("memory allocation error");
413
for(i=0;i<gt->nstages;i++){
414
DPRINTF("Stage %d\n",i);
415
for(j=0;j<gt->na;j++)
416
DPRINTF("a[%d]=%f\n",j,gt->coeff[i][j]);
417
for(;j<gt->na+gt->nb;j++)
418
DPRINTF("b[%d]=%f\n",j-gt->nb,gt->coeff[i][j]);
419
if (!(iirf[i].iring=ALLOCN(gt->na,gliirt)))
420
FILTER_ERROR_CLEANUP("memory allocation error");
421
if (!(iirf[i].oring=ALLOCN(gt->nb+1,gliirt)))
422
FILTER_ERROR_CLEANUP("memory allocation error");
432
/* Yes I know that this code is very ugly and possibly quite slow, but it's my first try :) */
437
while(pos<sbuf_size(inb)){
438
for(i=0;i<gt->nstages;i++){
440
iirf[0].iring[iirf[0].ipos]=sbuf_buf(inb)[pos];
442
iirf[i].iring[iirf[i].ipos]=iirf[i-1].oring[iirf[i-1].opos];
443
iirf[i].oring[iirf[i].opos]=0.0;
444
/* y[n]=a0*x[n]+a1*x[n-1]+... */
446
for(j=0;j<gt->na;j++){
449
iirf[i].oring[iirf[i].opos]+=gt->coeff[i][j]*iirf[i].iring[z--];
451
/* y[n]=y[n]+b1*y[n-1]+b2*y[n-2]+... */
453
for(j=gt->na;j<nt;j++){
456
iirf[i].oring[iirf[i].opos]+=gt->coeff[i][j]*iirf[i].oring[z--];
459
/* At least I process it in place :) */
460
sbuf_buf(inb)[pos++]=(SAMPLE)iirf[gt->nstages-1].oring[iirf[gt->nstages-1].opos];
461
/* Adjust ringbuffers */
462
for(i=0;i<gt->nstages;i++){
464
if (iirf[i].ipos==gt->na)
467
if (iirf[i].opos==nb)
474
inb=sbuf_make_private(inb);
481
FILTER_BEFORE_STOPCLEANUP;
482
FILTER_BEFORE_CLEANUP;
484
for(i=0;i<gt->nstages;i++){
490
/* Later on this should be done by the calling filter */
497
int highpass_register(plugin_t *p)
501
if (!(f = filter_creat(NULL)))
505
filterportdb_add_port(filter_portdb(f), PORTNAME_OUT,
506
FILTER_PORTTYPE_SAMPLE, FILTER_PORTFLAG_OUTPUT,
507
FILTERPORT_DESCRIPTION, "output channel",
509
filterportdb_add_port(filter_portdb(f), PORTNAME_IN,
510
FILTER_PORTTYPE_SAMPLE, FILTER_PORTFLAG_INPUT,
511
FILTERPORT_DESCRIPTION, "input channel",
514
filterparamdb_add_param_int(filter_paramdb(f),"stages",
515
FILTER_PARAMTYPE_INT,1,
516
FILTERPARAM_DESCRIPTION,"number of stages",
519
filterparamdb_add_param_float(filter_paramdb(f),"cutoff",
520
FILTER_PARAMTYPE_FLOAT, 1000,
521
FILTERPARAM_DESCRIPTION,"cutoff frequency",
524
filterparamdb_add_param_float(filter_paramdb(f),"ripple",
525
FILTER_PARAMTYPE_FLOAT,0.5,
526
FILTERPARAM_DESCRIPTION,"percent ripple",
529
plugin_set(p, PLUGIN_DESCRIPTION, "Chebyshev Lowpass");
530
plugin_set(p, PLUGIN_PIXMAP, "iir.png");
531
plugin_set(p, PLUGIN_CATEGORY, "Spectrum");
532
plugin_set(p, PLUGIN_GUI_HELP_PATH, "highpass");
534
filter_register(f, p);
539
int lowpass_register(plugin_t *p)
543
if (!(f = filter_creat(NULL)))
547
filterportdb_add_port(filter_portdb(f), PORTNAME_OUT,
548
FILTER_PORTTYPE_SAMPLE, FILTER_PORTFLAG_OUTPUT,
549
FILTERPORT_DESCRIPTION, "output channel",
551
filterportdb_add_port(filter_portdb(f), PORTNAME_IN,
552
FILTER_PORTTYPE_SAMPLE, FILTER_PORTFLAG_INPUT,
553
FILTERPORT_DESCRIPTION, "input channel",
556
filterparamdb_add_param_int(filter_paramdb(f),"stages",
557
FILTER_PARAMTYPE_INT,1,
558
FILTERPARAM_DESCRIPTION,"number of stages",
561
filterparamdb_add_param_float(filter_paramdb(f),"cutoff",
562
FILTER_PARAMTYPE_FLOAT, 1000,
563
FILTERPARAM_DESCRIPTION,"cutoff frequency",
566
filterparamdb_add_param_float(filter_paramdb(f),"ripple",
567
FILTER_PARAMTYPE_FLOAT,0.5,
568
FILTERPARAM_DESCRIPTION,"percent ripple",
571
plugin_set(p, PLUGIN_DESCRIPTION, "Chebyshev Highpass");
572
plugin_set(p, PLUGIN_PIXMAP, "iir.png");
573
plugin_set(p, PLUGIN_CATEGORY, "Spectrum");
574
plugin_set(p, PLUGIN_GUI_HELP_PATH, "lowpass");
576
filter_register(f, p);
581
int bandpass_register(plugin_t *p)
585
if (!(f = filter_creat(NULL)))
590
filterportdb_add_port(filter_portdb(f), PORTNAME_OUT,
591
FILTER_PORTTYPE_SAMPLE, FILTER_PORTFLAG_OUTPUT,
592
FILTERPORT_DESCRIPTION, "output channel",
594
filterportdb_add_port(filter_portdb(f), PORTNAME_IN,
595
FILTER_PORTTYPE_SAMPLE, FILTER_PORTFLAG_INPUT,
596
FILTERPORT_DESCRIPTION, "input channel",
599
filterparamdb_add_param_int(filter_paramdb(f),"stages",
600
FILTER_PARAMTYPE_INT,1,
601
FILTERPARAM_DESCRIPTION,"number of stages",
604
filterparamdb_add_param_float(filter_paramdb(f),"center",
605
FILTER_PARAMTYPE_FLOAT, 1000,
606
FILTERPARAM_DESCRIPTION,"center frequency",
609
filterparamdb_add_param_float(filter_paramdb(f),"width",
610
FILTER_PARAMTYPE_FLOAT,500,
611
FILTERPARAM_DESCRIPTION,"bandwidth around center",
614
filterparamdb_add_param_float(filter_paramdb(f),"ripple",
615
FILTER_PARAMTYPE_FLOAT,0.5,
616
FILTERPARAM_DESCRIPTION,"percent ripple",
619
plugin_set(p, PLUGIN_DESCRIPTION, "Chebyshev 2-stage Bandpass");
620
plugin_set(p, PLUGIN_PIXMAP, "bandpass.png");
621
plugin_set(p, PLUGIN_CATEGORY, "Spectrum");
622
plugin_set(p, PLUGIN_GUI_HELP_PATH, "bandpass");
624
filter_register(f, p);
629
int bandpass_a_register(plugin_t *p)
633
if (!(f = filter_creat(NULL)))
638
filterportdb_add_port(filter_portdb(f), PORTNAME_OUT,
639
FILTER_PORTTYPE_SAMPLE, FILTER_PORTFLAG_OUTPUT,
640
FILTERPORT_DESCRIPTION, "output channel",
642
filterportdb_add_port(filter_portdb(f), PORTNAME_IN,
643
FILTER_PORTTYPE_SAMPLE, FILTER_PORTFLAG_INPUT,
644
FILTERPORT_DESCRIPTION, "input channel",
647
filterparamdb_add_param_float(filter_paramdb(f),"center",
648
FILTER_PARAMTYPE_FLOAT, 1000,
649
FILTERPARAM_DESCRIPTION,"center frequency",
652
filterparamdb_add_param_float(filter_paramdb(f),"width",
653
FILTER_PARAMTYPE_FLOAT,500,
654
FILTERPARAM_DESCRIPTION,"bandwidth around center",
657
plugin_set(p, PLUGIN_DESCRIPTION, "Biquad Bandpass (analog modelled bandpass)");
658
plugin_set(p, PLUGIN_PIXMAP, "bandpass.png");
659
plugin_set(p, PLUGIN_CATEGORY, "Spectrum");
660
plugin_set(p, PLUGIN_GUI_HELP_PATH, "bandpass_a");
662
filter_register(f, p);
added
removed
src/plugins/iir.c
