ksrfir.c

dsp AD公司ADSP21的代码,里面有FFT FIR IIR EQULIZER G722_21F 等可以在项目中直接应用的代码.此代码的来源是ADI公司自己出版的书籍,此书在美国购得

/* Linear phase FIR filter coefficient computation using the Kaiser window
design method.  Filter length is odd. */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "rtdspc.h"

double get_float(char *title_string,double low_limit,double up_limit);
void filter_length(double att,double deltaf,int *nfilt,int *npair,double *beta);
double izero(double y);

void main()
{
    static float h[500], w[500], x[500];
    int eflag, filt_cat, npair, nfilt, n;
    double att, fa, fp, fa1, fa2, fp1, fp2, deltaf, d1, d2, fl, fu, beta;
    double fc, fm, pifc, tpifm, i, y, valizb;
    char ft_s[128];

    char  fp_s[] = "Passband edge frequency Fp";
    char  fa_s[] = "Stopband edge frequency Fa";
    char fp1_s[] = "Lower passband edge frequency Fp1";
    char fp2_s[] = "Upper passband edge frequency Fp2";
    char fa1_s[] = "Lower stopband edge frequency Fa1";
    char fa2_s[] = "Upper stopband edge frequency Fa2";

    printf("\nFilter type (lp, hp, bp, bs) ? ");
    gets(ft_s);
    strupr( ft_s );
    att = get_float("Desired stopband attenuation (dB)", 10, 200);
    filt_cat = 0;
    if( strcmp( ft_s, "LP" ) == 0 ) filt_cat = 1;
    if( strcmp( ft_s, "HP" ) == 0 ) filt_cat = 2;
    if( strcmp( ft_s, "BP" ) == 0 ) filt_cat = 3;
    if( strcmp( ft_s, "BS" ) == 0 ) filt_cat = 4;
    if(!filt_cat) exit(0);

    switch ( filt_cat ){
        case 1: case 2:
            switch ( filt_cat ){
                case 1:
                    fp = get_float( fp_s, 0,  0.5 );
                    fa = get_float( fa_s, fp, 0.5 ); break;
                case 2:
                    fa = get_float( fa_s, 0,  0.5 );
                    fp = get_float( fp_s, fa, 0.5 );
            }
            deltaf = (fa-fp) ; if(filt_cat == 2) deltaf = -deltaf;
            filter_length( att, deltaf, &nfilt, &npair, &beta );
            if( npair > 500 ){
                printf("\n*** Filter length %d is too large.\n", nfilt );
                exit(0);
            }
            printf("\n...filter length: %d ...beta: %f", nfilt, beta );
            fc = (fp + fa); h[npair] = fc;
            if ( filt_cat == 2 ) h[npair] = 1 - fc;
            pifc = PI * fc;
            for ( n=0; n < npair; n++){
                i = (npair - n);
                h[n] = sin(i * pifc) / (i * PI);
                if( filt_cat == 2 ) h[n] = - h[n];
            }
            break;
        case 3: case 4:
            printf("\n----> Transition bands must be equal <----");
            do {
                eflag = 0;
                switch (filt_cat){
                    case 3:
                        fa1 = get_float( fa1_s,   0, 0.5);
                        fp1 = get_float( fp1_s, fa1, 0.5);
                        fp2 = get_float( fp2_s, fp1, 0.5);
                        fa2 = get_float( fa2_s, fp2, 0.5); break;
                    case 4:
                        fp1 = get_float( fp1_s,   0, 0.5);
                        fa1 = get_float( fa1_s, fp1, 0.5);
                        fa2 = get_float( fa2_s, fa1, 0.5);
                        fp2 = get_float( fp2_s, fa2, 0.5);
                }
                d1 = fp1 - fa1; d2 = fa2 - fp2;
                if ( fabs(d1 - d2) > 1E-5 ){
                    printf( "\n...error...transition bands not equal\n");
                    eflag = -1;
                }
            } while (eflag);
            deltaf = d1; if(filt_cat == 4) deltaf = -deltaf;
            filter_length( att, deltaf, &nfilt, &npair, &beta);
            if( npair > 500 ){
                printf("\n*** Filter length %d is too large.\n", nfilt );
                exit(0);
            }
            printf( "\n..filter length: %d ...beta: %f", nfilt, beta);
            fl = (fa1 + fp1) / 2; fu = (fa2 + fp2) / 2;
            fc = (fu - fl); fm = (fu + fl) / 2;
            h[npair] = 2 * fc; if( filt_cat == 4 ) h[npair] = 1 - 2 * fc;
            pifc = PI * fc; tpifm = 2 * PI * fm;
            for (n = 0; n < npair; n++){
                i = (npair - n);
                h[n] = 2 * sin(i * pifc) * cos(i * tpifm) / (i * PI);
            if( filt_cat == 4) h[n] = -h[n];
            } break;
        default:  printf( "\n## error\n" ); exit(0);
    }

/* Compute Kaiser window sample values */
    y = beta; valizb = izero(y);
    for (n = 0; n <= npair; n++){
        i = (n - npair);
        y = beta * sqrt(1 - (i / npair) * (i / npair));
        w[n] = izero(y) / valizb;
    }

/* first half of response */
    for(n = 0; n <= npair; n++) x[n] = w[n] * h[n];

    printf("\n---First half of coefficient set...remainder by symmetry---");
    printf("\n  #      ideal        window     actual    ");
    printf("\n         coeff        value    filter coeff");
    for(n=0; n <= npair; n++){
        printf("\n %4d   %9.6f   %9.6f   %9.6f",n, h[n], w[n], x[n]);
    }
}

/* Use att to get beta (for Kaiser window function) and nfilt (always odd
    valued and = 2*npair +1) using Kaiser's empirical formulas */
void filter_length(double att,double deltaf,int *nfilt,int *npair,double *beta)
{
    *beta = 0;      /* value of beta if att < 21 */
    if(att >= 50) *beta = .1102 * (att - 8.71);
    if (att < 50 & att >= 21)
        *beta = .5842 * pow( (att-21), 0.4) + .07886 * (att - 21);
    *npair = (int)( (att - 8) / (29 * deltaf) );
    *nfilt = 2 * *npair +1;
}

/* Compute Bessel function Izero(y) using a series approximation */
double izero(double y){
    double s=1, ds=1, d=0;
    do{
        d = d + 2; ds = ds * (y*y)/(d*d);
        s = s + ds;
    } while( ds > 1E-7 * s);
    return(s);
}

/* FLOAT INPUT */
double get_float(char *title_string,double low_limit,double up_limit)
{
    double i;
    int error_flag;
    char *endcp;
    char ctemp[128];
    if(low_limit > up_limit){
        printf("\nLimit error lower > upper\n");
        exit(1);}
    do {
        printf("\n%s [%G to %G] ? ", title_string, low_limit, up_limit);
        gets(ctemp);
        i = strtod(ctemp, &endcp);
        error_flag = (ctemp == endcp) || (*endcp != '\0');
    } while (i < low_limit || i > up_limit || error_flag);
    return(i);
}