Listing 6

#include "biquad.h"

/* Computes a BiQuad filter on a sample */
smp_type BiQuad(smp_type sample, biquad * b)
{
    smp_type result;
    /* compute result */
    result = b->a0 * sample + b->a1 * b->x1 + b->a2 * b->x2 -
        b->a3 * b->y1 - b->a4 * b->y2;
    /* shift x1 to x2, sample to x1 */
    b->x2 = b->x1;
    b->x1 = sample;

    /* shift y1 to y2, result to y1 */
    b->y2 = b->y1;
    b->y1 = result;
    return result;
}
/* sets up a BiQuad Filter */
biquad *BiQuad_new(int type, smp_type dbGain, smp_type freq,
smp_type srate, smp_type BW_Q_SH, smp_type S)
{
    biquad *b;
    smp_type A, omega, sn, cs, alpha, beta;
    smp_type a0, a1, a2, b0, b1, b2;

    b = (biquad *)malloc(sizeof(biquad));
    if (b == NULL)
        return NULL;
    /* setup variables */
    A = pow(10, dbGain/40);
    omega = 2 * M_PI * freq/srate;
    sn = sin(omega);
    cs = cos(omega);
    switch (type) {
    case LPF:
        alpha = sn/(2.0*BW_Q_SH);
        b0 = (1 - cs)/2;
        b1 = 1 - cs; 
        b2 = (1 - cs)/2;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case HPF:
        alpha = sn/(2.0*BW_Q_SH);
        b0 = (1 + cs)/2;
        b1 = -(1 + cs);
        b2 = (1 + cs)/2;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case BPFA:
        alpha = sn/(2.0*BW_Q_SH);
        b0 = sn/2;
        b1 = 0;
        b2 = -sn/2;

        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case BPFB:
        alpha = sn/(2.0*BW_Q_SH);
        b0 = alpha;
        b1 = 0;
        b2 = -alpha;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case NOTCH:
        alpha = sn/(2.0*BW_Q_SH);
        b0 = 1;
        b1 = -2 * cs;
        b2 = 1;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case APF:
        alpha = sn/(2.0*BW_Q_SH);
        b0 = 1 - alpha;
        b1 = -2 * cs;
        b2 = 1 + alpha;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case PEQ:
        alpha = sn * sinh(M_LN2 / 2 * BW_Q_SH * omega / sn);
        b0 = 1 + (alpha * A);
        b1 = -2 * cs;
        b2 = 1 - (alpha * A);
        a0 = 1 + (alpha / A);
        a1 = -2 * cs;
        a2 = 1 - (alpha / A);
        break;
    case LSH:
        alpha = sn/2*sqrt((A + 1/A)*(1/S - 1) + 2);
        beta = 2 * sqrt(A) * alpha;
        b0 = A * ((A + 1) - (A - 1) * cs + beta);
        b1 = 2 * A * ((A - 1) - (A + 1) * cs);
        b2 = A * ((A + 1) - (A - 1) * cs - beta);
        a0 = (A + 1) + (A - 1) * cs + beta;
        a1 = -2 * ((A - 1) + (A + 1) * cs);
        a2 = (A + 1) + (A - 1) * cs - beta;
        break;
    case HSH:
        alpha = sn/2 * sqrt((A + 1/A)*(1/S - 1) + 2);
        beta = 2 * sqrt(A) * alpha;
        b0 = A * ((A + 1) + (A - 1) * cs + beta);
        b1 = -2 * A * ((A - 1) + (A + 1) * cs);
        b2 = A * ((A + 1) + (A - 1) * cs - beta);
        a0 = (A + 1) - (A - 1) * cs + beta;
        a1 = 2 * ((A - 1) - (A + 1) * cs);
        a2 = (A + 1) - (A - 1) * cs - beta;
        break;
    default:
        free(b);
        return NULL;
    }
    /* precompute the coefficients */
    b->a0 = b0/a0;
    b->a1 = b1/a0;
    b->a2 = b2/a0;
    b->a3 = a1/a0;
    b->a4 = a2/a0;

    /* zero initial samples */
    b->x1 = b->x2 = 0;
    b->y1 = b->y2 = 0;

    return b;
}
float forder(smp_type Apass, smp_type Astop, smp_type Fpass, 
                                         smp_type Fstop, smp_type Fsamp)
{
    smp_type stop_power, pass_power, frequency_pass, 
                                           frequency_stop, temp0, temp1;
    float N;
    stop_power = pow(10.0,Astop/20.0)-1;
    pass_power = pow(10.0,Apass/20.0)-1;
    frequency_pass = tan(M_PI*Fpass/Fsamp);
    frequency_stop = tan(M_PI*Fstop/Fsamp);
    
    temp0 = log(frequency_stop/frequency_pass);
    temp1 = log(sqrt(stop_power/pass_power));
    
    N= log(sqrt(stop_power/pass_power))/log(frequency_stop/frequency_pass);
    return ceil(N); 
}