From f04ea3143569dbadcdd5ed96e8feed0601464156 Mon Sep 17 00:00:00 2001 From: matteo Date: Mon, 17 Mar 2003 18:11:45 +0000 Subject: lun mar 17 19:11:15 CET 2003 git-svn-id: http://svn.digium.com/svn/zaptel/trunk@155 5390a7c7-147a-4af0-8ec9-7488f05a26cb --- mknotch.cc | 145 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 145 insertions(+) create mode 100755 mknotch.cc (limited to 'mknotch.cc') diff --git a/mknotch.cc b/mknotch.cc new file mode 100755 index 0000000..98a247b --- /dev/null +++ b/mknotch.cc @@ -0,0 +1,145 @@ +/* mknotch -- Make IIR notch filter parameters, based upon mkfilter; + A.J. Fisher, University of York + September 1992 */ + +#include +#include +#include + +#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); +} + + + + -- cgit v1.2.3