Logo Search packages:      
Sourcecode: zaptel version File versions

mknotch.cc

/* 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);
}





Generated by  Doxygen 1.6.0   Back to index