pshift.c

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

#include <math.h>
#include "rtdspc.h"

/* Kaiser Window Pitch Shift Algorithm */

/* set interpolation ratio */
    int ratio = 60;
/* passband specified, larger makes longer filters */
    float percent_pass = 80.0;
/* minimum attenuation in stopbands (dB), larger make long filters */
    float att = 50.0;
/* key value to shift by (semi-tones up or down) */
/* 12 is one octave */
    int key = -6;
    int lsize;
    int count;

void main()
{
    int i,j;
    int nfilt,npair,n,k;
    float fa,fp,deltaf,beta,valizb,alpha;
    float w,ck,y,npair_inv,pi_ratio;
    float signal_in,phase,dec;
    int old_key = 0;            /* remember last key value */
    float **h;

    static float hist[100];     /* lsize can not get bigger than 100 */

    long int filter_length();
    float izero();

    float fir_filter_no_update();
    void fir_update_history();

    fp = percent_pass/(200.0*ratio);
    fa = (200.0 - percent_pass)/(200.0*ratio);
    deltaf = fa-fp;

    nfilt = filter_length( att, deltaf, &beta );

    lsize = nfilt/ratio;

    nfilt = (long)lsize*ratio + 1;
    npair = (nfilt - 1)/2;

    h = (float **) calloc(ratio,sizeof(float *));
    if(!h) exit(1);
    for(i = 0 ; i < ratio ; i++) {
        h[i] = (float *) calloc(lsize,sizeof(float));
        if(!h[i]) exit(1);
    }

    /* Compute Kaiser window sample values */
    i = 0;
    j = 0;
    valizb = 1.0 / izero(beta);
    npair_inv = 1.0/npair;
    pi_ratio = PI/ratio;
    h[i++][j] = 0.0;        /* n = 0 case */
    for (n = 1 ; n < npair ; n++) {
        k = npair - n;
        alpha = k * npair_inv;
        y = beta * sqrt(1.0 - (alpha * alpha));
        w = valizb * izero(y);
        ck = ratio*sin(k*pi_ratio)/(k*PI);
        h[i++][j] = w * ck;
        if(i == ratio) {
            i = 0;
            j++;
        }
    }
/* force the pass through point */
    h[i][lsize/2] = 1.0;

/* second half of response */
    for(n = 1; n < npair; n++) {
        i = npair - n;          /* "from" location */
        k = npair + n;          /* "to" location */
        h[k%ratio][k/ratio] = h[i%ratio][i/ratio];
    }

/* interpolate the data by calls to fir_filter_no_update,
decimate the interpolated samples by only generating the samples
required */
    phase = 0.0;
    dec = (float)ratio;
    for(count = 0 ; ; count++) {

/* decimation ratio for key semitones shift */
/* allow real-time updates */
        if(key != old_key) {
            dec = ratio*pow(2.0,0.0833333333*key);
            old_key = key;
        }

        signal_in = getinput();
        while(phase < (float)ratio) {
            k = (int)phase;    /* pointer to poly phase values */
            sendout(fir_filter_no_update(signal_in,h[k],lsize,hist));
            phase += dec;
        }
        phase -= ratio;
        fir_update_history(signal_in,lsize,hist);

/* change key a little bit every 32K samples */
        if((count & 0x7FFF) == 0) {
            key++;
            if(key > 12) key = -6;
        }
    }
}

/* Use att to get beta (for Kaiser window function) and nfilt (always odd
    valued and = 2*npair +1) using Kaiser's empirical formulas.           */
long int filter_length(att,deltaf,beta)
    float att,deltaf,*beta;
{
    long int npair;
    *beta = 0.0;      /* value of beta if att < 21 */
    if(att >= 50.0) *beta = .1102 * (att - 8.71);
    if (att < 50.0 & att >= 21.0)
        *beta = .5842 * pow( (att-21.0), 0.4) + .07886 * (att - 21.0);
    npair = (long int)( (att - 8.0) / (28.72 * deltaf) );
    return(2*npair + 1);
}

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

/* run the fir filter using floating point numbers and
 do not update the history array */

float fir_filter_no_update(input,coef,ni,history)
    float input,*coef;
    int ni;
    float *history;
{
    register int i;
    register int n = ni;
    register float *hist_ptr,*coef_ptr;
    register float output;

    hist_ptr = history;
    coef_ptr = coef + n - 1;          /* point to last coef */

/* form output accumulation */
    output = *hist_ptr++ * (*coef_ptr--);
    for(i = 2 ; i < n ; i++) {
        output += (*hist_ptr++) * (*coef_ptr--);
    }
    output += input * (*coef_ptr);           /* input tap */

    return(output);
}

/* update the fir_filter history array */

void fir_update_history(input,ni,history)
    float input;
    int ni;
    float *history;
{
    register int i;
    register int n = ni;
    register float *hist_ptr,*hist1_ptr;

    hist_ptr = history;
    hist1_ptr = hist_ptr;                   /* use for history update */
    hist_ptr++;

    for(i = 2 ; i < n ; i++) {
        *hist1_ptr++ = *hist_ptr++;          /* update history array */
    }
    *hist1_ptr = input;                      /* last history */
}