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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
|
#include <math.h>
#include <string.h>
#include "biquad_filter.h"
/**
* Unit_BiquadFilter implements a second order IIR filter.
*
* Here is the equation that we use for this filter:
* y(n) = a0*x(n) + a1*x(n-1) + a2*x(n-2) - b1*y(n-1) - b2*y(n-2)
*
* @author (C) 2002 Phil Burk, SoftSynth.com, All Rights Reserved
*/
#define FILTER_PI (3.141592653589793238462643)
/***********************************************************
** Calculate coefficients common to many parametric biquad filters.
*/
static void BiquadFilter_CalculateCommon( BiquadFilter *filter, double ratio, double Q )
{
double omega;
memset( filter, 0, sizeof(BiquadFilter) );
/* Don't let frequency get too close to Nyquist or filter will blow up. */
if( ratio >= 0.499 ) ratio = 0.499;
omega = 2.0 * (double)FILTER_PI * ratio;
filter->cos_omega = (double) cos( omega );
filter->sin_omega = (double) sin( omega );
filter->alpha = filter->sin_omega / (2.0 * Q);
}
/*********************************************************************************
** Calculate coefficients for Highpass filter.
*/
void BiquadFilter_SetupHighPass( BiquadFilter *filter, double ratio, double Q )
{
double scalar, opc;
if( ratio < BIQUAD_MIN_RATIO ) ratio = BIQUAD_MIN_RATIO;
if( Q < BIQUAD_MIN_Q ) Q = BIQUAD_MIN_Q;
BiquadFilter_CalculateCommon( filter, ratio, Q );
scalar = 1.0 / (1.0 + filter->alpha);
opc = (1.0 + filter->cos_omega);
filter->a0 = opc * 0.5 * scalar;
filter->a1 = - opc * scalar;
filter->a2 = filter->a0;
filter->b1 = -2.0 * filter->cos_omega * scalar;
filter->b2 = (1.0 - filter->alpha) * scalar;
}
/*********************************************************************************
** Calculate coefficients for Notch filter.
*/
void BiquadFilter_SetupNotch( BiquadFilter *filter, double ratio, double Q )
{
double scalar, opc;
if( ratio < BIQUAD_MIN_RATIO ) ratio = BIQUAD_MIN_RATIO;
if( Q < BIQUAD_MIN_Q ) Q = BIQUAD_MIN_Q;
BiquadFilter_CalculateCommon( filter, ratio, Q );
scalar = 1.0 / (1.0 + filter->alpha);
opc = (1.0 + filter->cos_omega);
filter->a0 = scalar;
filter->a1 = -2.0 * filter->cos_omega * scalar;
filter->a2 = filter->a0;
filter->b1 = filter->a1;
filter->b2 = (1.0 - filter->alpha) * scalar;
}
/*****************************************************************
** Perform core IIR filter calculation without permutation.
*/
void BiquadFilter_Filter( BiquadFilter *filter, float *inputs, float *outputs, int numSamples )
{
int i;
double xn, yn;
// Pull values from structure to speed up the calculation.
double a0 = filter->a0;
double a1 = filter->a1;
double a2 = filter->a2;
double b1 = filter->b1;
double b2 = filter->b2;
double xn1 = filter->xn1;
double xn2 = filter->xn2;
double yn1 = filter->yn1;
double yn2 = filter->yn2;
for( i=0; i<numSamples; i++)
{
// Generate outputs by filtering inputs.
xn = inputs[i];
yn = (a0 * xn) + (a1 * xn1) + (a2 * xn2) - (b1 * yn1) - (b2 * yn2);
outputs[i] = yn;
// Delay input and output values.
xn2 = xn1;
xn1 = xn;
yn2 = yn1;
yn1 = yn;
if( (i & 7) == 0 )
{
// Apply a small bipolar impulse to filter to prevent arithmetic underflow.
// Underflows can cause the FPU to interrupt the CPU.
yn1 += (double) 1.0E-26;
yn2 -= (double) 1.0E-26;
}
}
filter->xn1 = xn1;
filter->xn2 = xn2;
filter->yn1 = yn1;
filter->yn2 = yn2;
}
|
