1 | /* Simple implementation of Biquad filters -- Tom St Denis
|
2 | *
|
3 | * Based on the work
|
4 |
|
5 | Cookbook formulae for audio EQ biquad filter coefficients
|
6 | ---------------------------------------------------------
|
7 | by Robert Bristow-Johnson, pbjrbj@viconet.com a.k.a. robert@audioheads.com
|
8 |
|
9 | * Available on the web at
|
10 |
|
11 | http://www.smartelectronix.com/musicdsp/text/filters005.txt
|
12 |
|
13 | * Enjoy.
|
14 | *
|
15 | * This work is hereby placed in the public domain for all purposes, whether
|
16 | * commercial, free [as in speech] or educational, etc. Use the code and please
|
17 | * give me credit if you wish.
|
18 | *
|
19 | * Tom St Denis -- http://tomstdenis.home.dhs.org
|
20 | */
|
21 |
|
22 | /* this would be biquad.h */
|
23 | #include <math.h>
|
24 | #include <stdlib.h>
|
25 |
|
26 | #ifndef M_LN2
|
27 | #define M_LN2 0.69314718055994530942
|
28 | #endif
|
29 |
|
30 | #ifndef M_PI
|
31 | #define M_PI3.14159265358979323846
|
32 | #endif
|
33 |
|
34 | /* whatever sample type you want */
|
35 | typedef double smp_type;
|
36 |
|
37 | /* this holds the data required to update samples thru a filter */
|
38 | typedef struct {
|
39 | smp_type a0, a1, a2, a3, a4;
|
40 | smp_type x1, x2, y1, y2;
|
41 | }
|
42 | biquad;
|
43 |
|
44 | extern smp_type BiQuad(smp_type sample, biquad * b);
|
45 | extern biquad *BiQuad_new(int type, smp_type dbGain, /* gain of filter */
|
46 | smp_type freq, /* center frequency */
|
47 | smp_type srate, /* sampling rate */
|
48 | smp_type bandwidth); /* bandwidth in octaves */
|
49 |
|
50 | /* filter types */
|
51 | enum {
|
52 | LPF, /* low pass filter */
|
53 | HPF, /* High pass filter */
|
54 | BPF, /* band pass filter */
|
55 | NOTCH, /* Notch Filter */
|
56 | PEQ, /* Peaking band EQ filter */
|
57 | LSH, /* Low shelf filter */
|
58 | HSH /* High shelf filter */
|
59 | };
|
60 |
|
61 | /* Below this would be biquad.c */
|
62 | /* Computes a BiQuad filter on a sample */
|
63 | smp_type BiQuad(smp_type sample, biquad * b)
|
64 | {
|
65 | smp_type result;
|
66 |
|
67 | /* compute result */
|
68 | result = b->a0 * sample + b->a1 * b->x1 + b->a2 * b->x2 -
|
69 | b->a3 * b->y1 - b->a4 * b->y2;
|
70 |
|
71 | /* shift x1 to x2, sample to x1 */
|
72 | b->x2 = b->x1;
|
73 | b->x1 = sample;
|
74 |
|
75 | /* shift y1 to y2, result to y1 */
|
76 | b->y2 = b->y1;
|
77 | b->y1 = result;
|
78 |
|
79 | return result;
|
80 | }
|
81 |
|
82 | /* sets up a BiQuad Filter */
|
83 | biquad *BiQuad_new(int type, smp_type dbGain, smp_type freq,
|
84 | smp_type srate, smp_type bandwidth)
|
85 | {
|
86 | biquad *b;
|
87 | smp_type A, omega, sn, cs, alpha, beta;
|
88 | smp_type a0, a1, a2, b0, b1, b2;
|
89 |
|
90 | b = malloc(sizeof(biquad));
|
91 | if (b == NULL)
|
92 | return NULL;
|
93 |
|
94 | /* setup variables */
|
95 | A = pow(10, dbGain /40);
|
96 | omega = 2 * M_PI * freq /srate;
|
97 | sn = sin(omega);
|
98 | cs = cos(omega);
|
99 | alpha = sn * sinh(M_LN2 /2 * bandwidth * omega /sn);
|
100 | beta = sqrt(A + A);
|
101 |
|
102 | switch (type) {
|
103 | case LPF:
|
104 | b0 = (1 - cs) /2;
|
105 | b1 = 1 - cs;
|
106 | b2 = (1 - cs) /2;
|
107 | a0 = 1 + alpha;
|
108 | a1 = -2 * cs;
|
109 | a2 = 1 - alpha;
|
110 | break;
|
111 | case HPF:
|
112 | b0 = (1 + cs) /2;
|
113 | b1 = -(1 + cs);
|
114 | b2 = (1 + cs) /2;
|
115 | a0 = 1 + alpha;
|
116 | a1 = -2 * cs;
|
117 | a2 = 1 - alpha;
|
118 | break;
|
119 | case BPF:
|
120 | b0 = alpha;
|
121 | b1 = 0;
|
122 | b2 = -alpha;
|
123 | a0 = 1 + alpha;
|
124 | a1 = -2 * cs;
|
125 | a2 = 1 - alpha;
|
126 | break;
|
127 | case NOTCH:
|
128 | b0 = 1;
|
129 | b1 = -2 * cs;
|
130 | b2 = 1;
|
131 | a0 = 1 + alpha;
|
132 | a1 = -2 * cs;
|
133 | a2 = 1 - alpha;
|
134 | break;
|
135 | case PEQ:
|
136 | b0 = 1 + (alpha * A);
|
137 | b1 = -2 * cs;
|
138 | b2 = 1 - (alpha * A);
|
139 | a0 = 1 + (alpha /A);
|
140 | a1 = -2 * cs;
|
141 | a2 = 1 - (alpha /A);
|
142 | break;
|
143 | case LSH:
|
144 | b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
|
145 | b1 = 2 * A * ((A - 1) - (A + 1) * cs);
|
146 | b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);
|
147 | a0 = (A + 1) + (A - 1) * cs + beta * sn;
|
148 | a1 = -2 * ((A - 1) + (A + 1) * cs);
|
149 | a2 = (A + 1) + (A - 1) * cs - beta * sn;
|
150 | break;
|
151 | case HSH:
|
152 | b0 = A * ((A + 1) + (A - 1) * cs + beta * sn);
|
153 | b1 = -2 * A * ((A - 1) + (A + 1) * cs);
|
154 | b2 = A * ((A + 1) + (A - 1) * cs - beta * sn);
|
155 | a0 = (A + 1) - (A - 1) * cs + beta * sn;
|
156 | a1 = 2 * ((A - 1) - (A + 1) * cs);
|
157 | a2 = (A + 1) - (A - 1) * cs - beta * sn;
|
158 | break;
|
159 | default:
|
160 | free(b);
|
161 | return NULL;
|
162 | }
|
163 |
|
164 | /* precompute the coefficients */
|
165 | b->a0 = b0 /a0;
|
166 | b->a1 = b1 /a0;
|
167 | b->a2 = b2 /a0;
|
168 | b->a3 = a1 /a0;
|
169 | b->a4 = a2 /a0;
|
170 |
|
171 | /* zero initial samples */
|
172 | b->x1 = b->x2 = 0;
|
173 | b->y1 = b->y2 = 0;
|
174 |
|
175 | return b;
|
176 | }
|
177 | /* crc==3062280887, version==4, Sat Jul 7 00:03:23 2001 */
|
