blob: ae33a4c96ea37bc7196487d0f154fc2f745ef5bc (
plain) (
blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
|
//#include "biquad_filter.h"
typedef struct biquad_filter_t {
// Filter coefficients
double b0, b1, b2; // Feedforward coefficients
double a1, a2; // Feedback coefficients
// Delay buffers (history of input and output)
double x1, x2; // Previous input samples
double y1, y2; // Previous output samples
} biquad_filter_t;
void
biquad_calculate_coefficients(biquad_filter_t* filter, double freq, double Q, double sampleRate, char type)
{
double omega = 2.0 * M_PI * freq / sampleRate;
double alpha = sin(omega) / (2.0 * Q);
double cos_omega = cos(omega);
// Initialize coefficients based on filter type
switch (type) {
case 'l': // Low-pass filter
filter->b0 = (1.0 - cos_omega) / 2.0;
filter->b1 = 1.0 - cos_omega;
filter->b2 = filter->b0;
filter->a1 = -2.0 * cos_omega;
filter->a2 = 1.0 - alpha;
break;
case 'h': // High-pass filter
filter->b0 = (1.0 + cos_omega) / 2.0;
filter->b1 = -(1.0 + cos_omega);
filter->b2 = filter->b0;
filter->a1 = -2.0 * cos_omega;
filter->a2 = 1.0 - alpha;
break;
case 'b': // Band-pass filter
filter->b0 = alpha;
filter->b1 = 0.0;
filter->b2 = -alpha;
filter->a1 = -2.0 * cos_omega;
filter->a2 = 1.0 - alpha;
break;
default:
exit(EXIT_FAILURE); // Unsupported filter type
}
// Normalize coefficients
double a0 = 1.0 + alpha;
filter->b0 /= a0;
filter->b1 /= a0;
filter->b2 /= a0;
filter->a1 /= a0;
filter->a2 /= a0;
}
double
biquad_process(biquad_filter_t* filter, double input)
{
double output = filter->b0 * input +
filter->b1 * filter->x1 +
filter->b2 * filter->x2 -
filter->a1 * filter->y1 -
filter->a2 * filter->y2;
filter->x2 = filter->x1;
filter->x1 = input;
filter->y2 = filter->y1;
filter->y1 = output;
return output;
}
|
