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
|
/* mknotch -- Make IIR notch filter parameters, based upon mkfilter;
A.J. Fisher, University of York <fisher@minster.york.ac.uk>
September 1992 */
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "mkfilter.h"
#include "complex.h"
#define opt_be 0x00001 /* -Be Bessel characteristic */
#define opt_bu 0x00002 /* -Bu Butterworth characteristic */
#define opt_ch 0x00004 /* -Ch Chebyshev characteristic */
#define opt_re 0x00008 /* -Re Resonator */
#define opt_pi 0x00010 /* -Pi proportional-integral */
#define opt_lp 0x00020 /* -Lp lowpass */
#define opt_hp 0x00040 /* -Hp highpass */
#define opt_bp 0x00080 /* -Bp bandpass */
#define opt_bs 0x00100 /* -Bs bandstop */
#define opt_ap 0x00200 /* -Ap allpass */
#define opt_a 0x00400 /* -a alpha value */
#define opt_l 0x00800 /* -l just list filter parameters */
#define opt_o 0x01000 /* -o order of filter */
#define opt_p 0x02000 /* -p specified poles only */
#define opt_w 0x04000 /* -w don't pre-warp */
#define opt_z 0x08000 /* -z use matched z-transform */
#define opt_Z 0x10000 /* -Z additional zero */
struct pzrep
{ complex poles[MAXPZ], zeros[MAXPZ];
int numpoles, numzeros;
};
static pzrep splane, zplane;
static double raw_alpha1, raw_alpha2;
static complex dc_gain, fc_gain, hf_gain;
static uint options;
static double qfactor;
static bool infq;
static uint polemask;
static double xcoeffs[MAXPZ+1], ycoeffs[MAXPZ+1];
static void compute_notch();
static void expandpoly(), expand(complex[], int, complex[]), multin(complex, int, complex[]);
static void compute_bpres()
{ /* compute Z-plane pole & zero positions for bandpass resonator */
zplane.numpoles = zplane.numzeros = 2;
zplane.zeros[0] = 1.0; zplane.zeros[1] = -1.0;
double theta = TWOPI * raw_alpha1; /* where we want the peak to be */
if (infq)
{ /* oscillator */
complex zp = expj(theta);
zplane.poles[0] = zp; zplane.poles[1] = cconj(zp);
}
else
{ /* must iterate to find exact pole positions */
complex topcoeffs[MAXPZ+1]; expand(zplane.zeros, zplane.numzeros, topcoeffs);
double r = exp(-theta / (2.0 * qfactor));
double thm = theta, th1 = 0.0, th2 = PI;
bool cvg = false;
for (int i=0; i < 50 && !cvg; i++)
{ complex zp = r * expj(thm);
zplane.poles[0] = zp; zplane.poles[1] = cconj(zp);
complex botcoeffs[MAXPZ+1]; expand(zplane.poles, zplane.numpoles, botcoeffs);
complex g = evaluate(topcoeffs, zplane.numzeros, botcoeffs, zplane.numpoles, expj(theta));
double phi = g.im / g.re; /* approx to atan2 */
if (phi > 0.0) th2 = thm; else th1 = thm;
if (fabs(phi) < EPS) cvg = true;
thm = 0.5 * (th1+th2);
}
unless (cvg) fprintf(stderr, "mkfilter: warning: failed to converge\n");
}
}
static void compute_notch()
{ /* compute Z-plane pole & zero positions for bandstop resonator (notch filter) */
compute_bpres(); /* iterate to place poles */
double theta = TWOPI * raw_alpha1;
complex zz = expj(theta); /* place zeros exactly */
zplane.zeros[0] = zz; zplane.zeros[1] = cconj(zz);
}
static void expandpoly() /* given Z-plane poles & zeros, compute top & bot polynomials in Z, and then recurrence relation */
{ complex topcoeffs[MAXPZ+1], botcoeffs[MAXPZ+1]; int i;
expand(zplane.zeros, zplane.numzeros, topcoeffs);
expand(zplane.poles, zplane.numpoles, botcoeffs);
dc_gain = evaluate(topcoeffs, zplane.numzeros, botcoeffs, zplane.numpoles, 1.0);
double theta = TWOPI * 0.5 * (raw_alpha1 + raw_alpha2); /* "jwT" for centre freq. */
fc_gain = evaluate(topcoeffs, zplane.numzeros, botcoeffs, zplane.numpoles, expj(theta));
hf_gain = evaluate(topcoeffs, zplane.numzeros, botcoeffs, zplane.numpoles, -1.0);
for (i = 0; i <= zplane.numzeros; i++) xcoeffs[i] = +(topcoeffs[i].re / botcoeffs[zplane.numpoles].re);
for (i = 0; i <= zplane.numpoles; i++) ycoeffs[i] = -(botcoeffs[i].re / botcoeffs[zplane.numpoles].re);
}
static void expand(complex pz[], int npz, complex coeffs[])
{ /* compute product of poles or zeros as a polynomial of z */
int i;
coeffs[0] = 1.0;
for (i=0; i < npz; i++) coeffs[i+1] = 0.0;
for (i=0; i < npz; i++) multin(pz[i], npz, coeffs);
/* check computed coeffs of z^k are all real */
for (i=0; i < npz+1; i++)
{ if (fabs(coeffs[i].im) > EPS)
{ fprintf(stderr, "mkfilter: coeff of z^%d is not real; poles/zeros are not complex conjugates\n", i);
exit(1);
}
}
}
static void multin(complex w, int npz, complex coeffs[])
{ /* multiply factor (z-w) into coeffs */
complex nw = -w;
for (int i = npz; i >= 1; i--) coeffs[i] = (nw * coeffs[i]) + coeffs[i-1];
coeffs[0] = nw * coeffs[0];
}
extern "C" void mknotch(float freq,float bw,long *p1, long *p2, long *p3);
void mknotch(float freq,float bw,long *p1, long *p2, long *p3)
{
#define NB 14
options = opt_re;
qfactor = freq / bw;
infq = false;
raw_alpha1 = freq / 8000.0;
polemask = ~0;
compute_notch();
expandpoly();
float fsh = (float) (1 << NB);
*p1 = (long)(xcoeffs[1] * fsh);
*p2 = (long)(ycoeffs[0] * fsh);
*p3 = (long)(ycoeffs[1] * fsh);
}
|
