-
Notifications
You must be signed in to change notification settings - Fork 2
/
Utilities.cpp
159 lines (130 loc) · 4.88 KB
/
Utilities.cpp
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
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
//
// Utilities.cpp
// DSPLibrary
//
// Created by Mayank on 9/11/12.
// Copyright (c) 2012 Mayank Sanganeria. All rights reserved.
//
#include "Utilities.h"
#include <math.h>
void bilinearTransform(double acoefs[], double dcoefs[], double withFs)
{
float b0, b1, b2, a0, a1, a2; //storage for continuous-time filter coefs
float bz0, bz1, bz2, az0, az1, az2; // coefs for discrete-time filter.
b0 = acoefs[0]; b1 = acoefs[1]; b2 = acoefs[2]; // unpack coefficients
a0 = acoefs[3]; a1 = acoefs[4]; a2 = acoefs[5];
// H(s) = 1
// -------------------------------
// s^2*(1/wc^2) + s*(1/(Q*wc)) + 1
// Bilinear transform, H(z) = H(s(z))
// s = 2 * 1 - z^-1
// - --------
// T 1 + z^-1
double T = 1.0 / withFs; // the period
bz2 = 4 * b2 - 2 * b1 * T + b0 * T * T;
bz1 = -8 * b2 + 2 * b0 * T * T;
bz0 = 4 * b2 + 2 * b1 * T + b0 * T * T;
az2 = 4.0 * a2 - 2.0 * T * a1 + T * T * a0;
az1 = -8.0 * a2 + 2.0 * T * T * a0;
az0 = 4.0 * a2 + 2.0 * T * a1 + T * T* a0;
bz2 = bz2 / az0; // normalizing by az0
bz1 = bz1 / az0;
bz0 = bz0 / az0;
az2 = az2 / az0;
az1 = az1 / az0;
az0 = az0 / az0;
dcoefs[0] = bz0; dcoefs[1] = bz1; dcoefs[2] = bz2; // return coefficients to the output
dcoefs[3] = az1; dcoefs[4] = az2;
}
//------------------------------------------------------------------------------
void designParametric(double* dcoefs, double center, double gain, double qval, double withFs)
// design parametric filter based on input center frequency, gain, Q and sampling rate
{
double b0, b1, b2, a0, a1, a2; //storage for continuous-time filter coefs
double acoefs[6];
//Design parametric filter here. Filter should be of the form
//
// 2
// b2s + b1s + b0
// ---------------
// 2
// a2s + a1s + a0
//
// Parameters are center frequency in Hz, gain in dB, and Q.
// design analog filter based on input gain, center frequency and Q
if (gain > 0.0) { // boost case
b0 = 1;
b1 = gain / (qval * center * 2 * M_PI);
b2 = 1 / (center * center * 4 * M_PI * M_PI);
a0 = 1;
a1 = 1 / (qval * center * 2 * M_PI);
a2 = 1 / (center * center * 4 * M_PI * M_PI);
} else { // cut case
b0 = 1;
b1 = 1 / (qval * center * 2 * M_PI);
b2 = 1 / (center * center * 4 * M_PI * M_PI);
a0 = 1;
a1 = 1 / (qval * gain * center * 2 * M_PI);
a2 = 1 / (center * center * 4 * M_PI * M_PI);
}
acoefs[0] = b0; acoefs[1] = b1; acoefs[2] = b2; // pack the analog coeffs into an array
acoefs[3] = a0; acoefs[4] = a1; acoefs[5] = a2;
// and apply the bilinear tranform
bilinearTransform(acoefs, dcoefs, withFs); // inputs the 6 analog coeffs, output the 5 digital coeffs
}
//------------------------------------------------------------------------------
void designFirstOrderLowpass(double* dcoefs, double cutoff, double withFs)
// design parametric filter based on input center frequency, gain, Q and sampling rate
{
double b0, b1, b2, a0, a1, a2; //storage for continuous-time filter coefs
double acoefs[6];
//Design lowpass filter here. Filter should be of the form
//
// 2
// b2s + b1s + b0
// ---------------
// 2
// a2s + a1s + a0
//
// where b2 and a2 are zero (1st order, just keeM_PIng them so the same BLT function can be used)
//
// Parameters are cutoff frequency in Hz
b0 = 1; // design analog filter based on cutoff frequency
b1 = 0;
b2 = 0;
a0 = 1;
a1 = 1 / cutoff;
a2 = 0;
acoefs[0] = b0; acoefs[1] = b1; acoefs[2] = b2; // pack the analog coeffs into an array
acoefs[3] = a0; acoefs[4] = a1; acoefs[5] = a2;
// and apply the bilinear tranform
bilinearTransform(acoefs, dcoefs, withFs); // inputs the 6 analog coeffs, output the 5 digital coeffs
}
//------------------------------------------------------------------------------
void designFirstOrderHighpass(double* dcoefs, double cutoff, double withFs)
// design parametric filter based on input center frequency, gain, Q and sampling rate
{
double b0, b1, b2, a0, a1, a2; //storage for continuous-time filter coefs
double acoefs[6];
//Design highpass filter here. Filter should be of the form
//
// 2
// b2s + b1s + b0
// ---------------
// 2
// a2s + a1s + a0
//
// where b2 and a2 are zero (1st order, just keeM_PIng them so the same BLT function can be used)
//
// Parameters are cutoff frequency in Hz
b0 = 0; // design analog filter based on cutoff frequency
b1 = 1 / cutoff;
b2 = 0;
a0 = 1;
a1 = 1 / cutoff;
a2 = 0;
acoefs[0] = b0; acoefs[1] = b1; acoefs[2] = b2; // pack the analog coeffs into an array
acoefs[3] = a0; acoefs[4] = a1; acoefs[5] = a2;
// and apply the bilinear tranform
bilinearTransform(acoefs, dcoefs, withFs); // inputs the 6 analog coeffs, output the 5 digital coeffs
}