Mercurial > hg > sv-dependency-builds

annotate src/portaudio/qa/loopback/src/biquad_filter.c @ 95:89f5e221ed7b

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Add FFTW3
author Chris Cannam <cannam@all-day-breakfast.com>
date Wed, 20 Mar 2013 15:35:50 +0000
parents 8a15ff55d9af
children
rev   line source
cannam@89 1 #include <math.h>
cannam@89 2 #include <string.h>
cannam@89 3
cannam@89 4 #include "biquad_filter.h"
cannam@89 5
cannam@89 6 /**
cannam@89 7 * Unit_BiquadFilter implements a second order IIR filter.
cannam@89 8 Here is the equation that we use for this filter:
cannam@89 9 y(n) = a0*x(n) + a1*x(n-1) + a2*x(n-2) - b1*y(n-1) - b2*y(n-2)
cannam@89 10 *
cannam@89 11 * @author (C) 2002 Phil Burk, SoftSynth.com, All Rights Reserved
cannam@89 12 */
cannam@89 13
cannam@89 14 #define FILTER_PI (3.141592653589793238462643)
cannam@89 15 /***********************************************************
cannam@89 16 ** Calculate coefficients common to many parametric biquad filters.
cannam@89 17 */
cannam@89 18 static void BiquadFilter_CalculateCommon( BiquadFilter *filter, double ratio, double Q )
cannam@89 19 {
cannam@89 20 double omega;
cannam@89 21
cannam@89 22 memset( filter, 0, sizeof(BiquadFilter) );
cannam@89 23
cannam@89 24 /* Don't let frequency get too close to Nyquist or filter will blow up. */
cannam@89 25 if( ratio >= 0.499 ) ratio = 0.499;
cannam@89 26 omega = 2.0 * (double)FILTER_PI * ratio;
cannam@89 27
cannam@89 28 filter->cos_omega = (double) cos( omega );
cannam@89 29 filter->sin_omega = (double) sin( omega );
cannam@89 30 filter->alpha = filter->sin_omega / (2.0 * Q);
cannam@89 31 }
cannam@89 32
cannam@89 33 /*********************************************************************************
cannam@89 34 ** Calculate coefficients for Highpass filter.
cannam@89 35 */
cannam@89 36 void BiquadFilter_SetupHighPass( BiquadFilter *filter, double ratio, double Q )
cannam@89 37 {
cannam@89 38 double scalar, opc;
cannam@89 39
cannam@89 40 if( ratio < BIQUAD_MIN_RATIO ) ratio = BIQUAD_MIN_RATIO;
cannam@89 41 if( Q < BIQUAD_MIN_Q ) Q = BIQUAD_MIN_Q;
cannam@89 42
cannam@89 43 BiquadFilter_CalculateCommon( filter, ratio, Q );
cannam@89 44
cannam@89 45 scalar = 1.0 / (1.0 + filter->alpha);
cannam@89 46 opc = (1.0 + filter->cos_omega);
cannam@89 47
cannam@89 48 filter->a0 = opc * 0.5 * scalar;
cannam@89 49 filter->a1 = - opc * scalar;
cannam@89 50 filter->a2 = filter->a0;
cannam@89 51 filter->b1 = -2.0 * filter->cos_omega * scalar;
cannam@89 52 filter->b2 = (1.0 - filter->alpha) * scalar;
cannam@89 53 }
cannam@89 54
cannam@89 55
cannam@89 56 /*********************************************************************************
cannam@89 57 ** Calculate coefficients for Notch filter.
cannam@89 58 */
cannam@89 59 void BiquadFilter_SetupNotch( BiquadFilter *filter, double ratio, double Q )
cannam@89 60 {
cannam@89 61 double scalar, opc;
cannam@89 62
cannam@89 63 if( ratio < BIQUAD_MIN_RATIO ) ratio = BIQUAD_MIN_RATIO;
cannam@89 64 if( Q < BIQUAD_MIN_Q ) Q = BIQUAD_MIN_Q;
cannam@89 65
cannam@89 66 BiquadFilter_CalculateCommon( filter, ratio, Q );
cannam@89 67
cannam@89 68 scalar = 1.0 / (1.0 + filter->alpha);
cannam@89 69 opc = (1.0 + filter->cos_omega);
cannam@89 70
cannam@89 71 filter->a0 = scalar;
cannam@89 72 filter->a1 = -2.0 * filter->cos_omega * scalar;
cannam@89 73 filter->a2 = filter->a0;
cannam@89 74 filter->b1 = filter->a1;
cannam@89 75 filter->b2 = (1.0 - filter->alpha) * scalar;
cannam@89 76 }
cannam@89 77
cannam@89 78 /*****************************************************************
cannam@89 79 ** Perform core IIR filter calculation without permutation.
cannam@89 80 */
cannam@89 81 void BiquadFilter_Filter( BiquadFilter *filter, float *inputs, float *outputs, int numSamples )
cannam@89 82 {
cannam@89 83 int i;
cannam@89 84 double xn, yn;
cannam@89 85 // Pull values from structure to speed up the calculation.
cannam@89 86 double a0 = filter->a0;
cannam@89 87 double a1 = filter->a1;
cannam@89 88 double a2 = filter->a2;
cannam@89 89 double b1 = filter->b1;
cannam@89 90 double b2 = filter->b2;
cannam@89 91 double xn1 = filter->xn1;
cannam@89 92 double xn2 = filter->xn2;
cannam@89 93 double yn1 = filter->yn1;
cannam@89 94 double yn2 = filter->yn2;
cannam@89 95
cannam@89 96 for( i=0; i<numSamples; i++)
cannam@89 97 {
cannam@89 98 // Generate outputs by filtering inputs.
cannam@89 99 xn = inputs[i];
cannam@89 100 yn = (a0 * xn) + (a1 * xn1) + (a2 * xn2) - (b1 * yn1) - (b2 * yn2);
cannam@89 101 outputs[i] = yn;
cannam@89 102
cannam@89 103 // Delay input and output values.
cannam@89 104 xn2 = xn1;
cannam@89 105 xn1 = xn;
cannam@89 106 yn2 = yn1;
cannam@89 107 yn1 = yn;
cannam@89 108
cannam@89 109 if( (i & 7) == 0 )
cannam@89 110 {
cannam@89 111 // Apply a small bipolar impulse to filter to prevent arithmetic underflow.
cannam@89 112 // Underflows can cause the FPU to interrupt the CPU.
cannam@89 113 yn1 += (double) 1.0E-26;
cannam@89 114 yn2 -= (double) 1.0E-26;
cannam@89 115 }
cannam@89 116 }
cannam@89 117
cannam@89 118 filter->xn1 = xn1;
cannam@89 119 filter->xn2 = xn2;
cannam@89 120 filter->yn1 = yn1;
cannam@89 121 filter->yn2 = yn2;
cannam@89 122 }