lun mar 17 19:11:15 CET 2003

git-svn-id: http://svn.digium.com/svn/zaptel/trunk@155 5390a7c7-147a-4af0-8ec9-7488f05a26cb

1 files changed, 145 insertions, 0 deletions

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   <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);
+}
+
+
+
+