Adaptive IIR filter using fixed point integer arithmetic

March 23, 2013 Coded in C
typedef signed long s32;

#define ABS(x)                 ((x) < 0 ? -(x) : (x))
#define CLAMP(x,min,max)       {if ((x) <= (min)) (x) = (min); else if ((x) >= (max)) (x) = (max);}

#define KLPF_MAX_ADAPT			232L
#define KLPF_MIN_ADAPT			 0L
#define KLPF_DEFAULT_ADAPT		160L

#define ACCEL_MIN_ADAPT     5
#define ACCEL_MAX_ADAPT     15

s32 gnKLpf          = KLPF_DEFAULT_ADAPT;   // IIR filter coefficient, 0 to 255
s32 gnKLpfMax       = KLPF_MAX_ADAPT;  
s32 gnAccel = 0;   // rate of change of climbrate
s32 gnCpsx256 = 0; // climbrate in centimeters per second, with 8 bit fractional precision
s32 gnCps; // climbrate in centimeters per second

// IIR low pass filter using fixed point integer arithmetic. 8bits of fractional precision for IIR coefficient K.
// So the actual floating point coefficient is k = K/256, 0.0 <= k < 1.0. The filter computes
// climbRateFiltered = climbRateFiltered*k + newClimbRate*(1.0 - k). So K values close to 256 will result in 
// heavy damping, K values close to 0 will result in almost no damping.  The IIR low pass filtered output gnCps 
// is the rounded up integer value, the 8bit fraction precision value gnCpsx256 is a global variable, so is
// maintained between calls to the filter. No integer divides required. 

void sns_LPFClimbRate(void)   {
   	s32 tmp;
   	tmp = gnCpsx256*gnKLpf + (gnCps<<8)*(256L-gnKLpf);
   	gnCpsx256 = (tmp >= 0 ? ((tmp + 128L)>>8) : ((tmp - 128L)>>8));
   	gnCps = (gnCpsx256>>8);
   	}  

// Adapt the IIR filter coeffient to the rate of change. If samples are not changing much, increase the damping.
// If the samples are changing by a large amount, reduce the damping. This is done to reduce the response delay to a
// a step change, while still being able to heavily damp out jitter in steady state noisy samples.
// This function basically linearly interpolates KLpf between KLPF_MAX and KLPF_MIN based on the acceleration.
// Values of acceleration outside experimentally determined limits are clamped to the limits, depends on the 
// application. A variable is used for KLpfMax to allow user to adjust the maximum allowed damping.

s32 sns_AdaptKLpf(void) {
	s32 klpf,nAccel;
	nAccel = ABS(gnAccel);
	CLAMP(nAccel, ACCEL_MIN_ADAPT, ACCEL_MAX_ADAPT);
	klpf = gnKLpfMax + ((KLPF_MIN_ADAPT-gnKLpfMax)*(nAccel - ACCEL_MIN_ADAPT))/(ACCEL_MAX_ADAPT-ACCEL_MIN_ADAPT);
	CLAMP(klpf, KLPF_MIN_ADAPT, KLPF_MAX_ADAPT);
	return klpf;
	}

// usage : 
// For each new data sample that arrives
//   1. calculate new unfiltered climbrate nCps from sample data buffer	
//	 2. gnAccel = (nCps - gnCps);
//	 3. (optionally low pass filter gnAccel using same type of IIR filter, if required)
//   4. gnCps = nCps;
//	 5. gnKLpf = sns_AdaptKLpf();
//	 6. sns_LPFClimbRate();

Tremolo audio effect (amplitude modulation)

Gabriel Rivas November 29, 20112 comments Coded in C
/************************Tremolo.c*************************/

#include "Tremolo.h"

static double dep;
static short counter_limit;
static short control;
static short mod;
static double offset;

void Tremolo_init(short effect_rate,double depth) {
    dep     = depth; 
    control = 1;
    mod     = 0;
    counter_limit = effect_rate;
    offset  = 1 - dep;
}

double Tremolo_process(double xin) {
    double yout;
    double m;

    m = (double)mod*dep/counter_limit;
    yout = (m + offset)*xin;
    return yout;
}

void Tremolo_sweep(void) {

	    mod += control;
  
	    if (mod > counter_limit) {
	        control = -1;
            } 
            else if(!mod) {
	        control = 1;
            }
}

/********************Tremolo.h****************************/
#ifndef __TREMOLO_H__
#define __TREMOLO_H__

extern void Tremolo_init(short effect_rate,double depth);
extern double Tremolo_process(double xin);
extern void Tremolo_sweep(void);

#endif

/*Usage example*/
#include "Tremolo.h"

void main(void) {
    double xin;
    double yout;
    Tremolo_init(4000,1);

    while(1) {
        if (new_sample_flag()) {
            /*When there's new sample at your ADC or CODEC input*/
            /*Read the sample*/
            xin = read_sample();
            /*Apply the Tremolo_process function to the sample*/
            yout = Tremolo_process(0.7*temp);

            /*Send the output value to your DAC or codec output*/
            write_output(yout);
            /*Makes LFO vary*/
            Tremolo_sweep();
        }                              
    }
}

Auto Wah and Envelope follower audio effect

Gabriel Rivas November 29, 2011 Coded in C
#include "bp_iir.h"
#include "AutoWah.h"

static short center_freq;
static short samp_freq;
static short counter;
static short counter_limit;
static short control;
static short max_freq;
static short min_freq;
static short f_step;
static struct bp_filter H;
static double a[3];
static double b[3];
double x[3],y[3];

void AutoWah_init(short effect_rate,short sampling,short maxf,short minf,short Q,double gainfactor,short freq_step) {
	double C;

	//Process variables
	center_freq = 0;
	samp_freq = sampling;
	counter = effect_rate;
	control = 0;

	//User Parametters
	counter_limit = effect_rate;
	
	//Convert frequencies to index ranges
	min_freq = 0;
	max_freq = (maxf - minf)/freq_step;

    bp_iir_init(sampling,gainfactor,Q,freq_step,minf);
	f_step = freq_step; 
        
	//Lowpass filter parameters	
	/*
	   C = 1/TAN(PI*Fc/Fs)
	*/
	C = 1018.59;

	b[0] = 1/(1+2*0.7071*C+pow(C,2));
	b[1] = 2*b[0];
	b[2] = b[0];

	a[1] = 2*b[0]*(1-pow(C,2));
	a[2] = b[0]*(1-2*0.7071*C+pow(C,2));
}

double AutoWah_process(double xin) {
	double yout;

	yout = bp_iir_filter(xin,&H);
	#ifdef INPUT_UNSIGNED
		yout += 32767;
	#endif
	
	return yout;
}

void AutoWah_sweep(double xin) {
	unsigned int filter_index;
	double yout;
        double detect;

	/*The input is 16 bit unsigned so it
	has to be centered to 0*/
	detect = (xin - 32767.0); 
	x[0] = x[1]; 
	x[1] = x[2]; 
	/*Here the input to the filter
	is half rectified*/
	x[2] = (detect > 0)?detect:0;
		
	y[0] = y[1]; 
	y[1] = y[2]; 
	
	y[2] =    b[0]*x[2];
	y[2] +=   b[1]*x[1];
	y[2] +=   b[2]*x[0];
	y[2] -=   a[1]*y[1];
	y[2] -=   a[2]*y[0];

	if (!--counter) {
		/*The output of the LPF (y[2]) is scaled by 0.1
		in order to be used as a LFO to control the band pass filter
		*/
	    filter_index = (double)min_freq + y[2]*0.1;
                        
            /*The scaling value is determined as a value
            that would keep the filter index within the
            range of max_freq
	    */
            if(filter_index > max_freq) {
                filter_index = max_freq;
            } 
            if(filter_index < 0) {
                filter_index = 0;
            }

	    bp_iir_setup(&H,filter_index);

	    counter = counter_limit; 
	}
}

Auto Wah audio effect (LFO control)

Gabriel Rivas September 24, 2011 Coded in C
/*************** AutoWah.c ********************************/

#include "bp_iir.h"
#include "AutoWah.h"

static short center_freq;
static short samp_freq;
static short counter;
static short counter_limit;
static short control;
static short max_freq;
static short min_freq;
static short f_step;
static struct bp_filter H;

/*
This is the auto wah effect initialization function. 
This initializes the band pass filter and the effect control variables
*/
void AutoWah_init(short effect_rate,short sampling,short maxf,short minf,short Q,double gainfactor,short freq_step) {
	double C;

	/*Process variables*/
	center_freq = 0;
	samp_freq = sampling;
	counter = effect_rate;
	control = 0;

	/*User Parametters*/
	counter_limit = effect_rate;
	
	/*Convert frequencies to index ranges*/
	min_freq = 0;
	max_freq = (maxf - minf)/freq_step;

        bp_iir_init(sampling,gainfactor,Q,freq_step,minf);
	f_step = freq_step; 
}

/*
This function generates the current output value
Note that if the input and output signal are integer
unsigned types, we need to add a half scale offset
*/
double AutoWah_process(int xin) {
	double yout;

	yout = bp_iir_filter(xin,&H);
	#ifdef INPUT_UNSIGNED
		yout += 32767;
	#endif
	
	return yout;
}

/*
This function will emulate a LFO that will vary according
to the effect_rate parameter set in the AutoWah_init function.
*/
void AutoWah_sweep(void) {
	double yout;
     
	if (!--counter) {
		if (!control) {
			bp_iir_setup(&H,(center_freq+=f_step));
			if (center_freq > max_freq) {
				control = 1;
			}
		}
		else if (control) {
			bp_iir_setup(&H,(center_freq-=f_step));
			if (center_freq == min_freq) {
				control = 0;
			}
		}

		counter = counter_limit; 
	}
}

/*************** AutoWah.h ****************************/

#ifndef __AUTOWAH_H__
#define __AUTOWAH_H__

extern void AutoWah_init(short effect_rate,short sampling,short maxf,short minf,short Q,double gainfactor,short freq_step);
extern double AutoWah_process(int xin);
extern void AutoWah_sweep(void);

#endif

/************** Usage Example **************************/

#include "AutoWah.h"

void main(void) {
    short xin;
    short yout;
    AutoWah_init(2000,  /*Effect rate 2000*/
                 16000, /*Sampling Frequency*/
                 1000,  /*Maximum frequency*/
                 500,   /*Minimum frequency*/
                 4,     /*Q*/
                 0.707, /*Gain factor*/
                 10     /*Frequency increment*/
                 );   

    while(1) {
        if (new_sample_flag()) {
            /*When there's new sample at your ADC or CODEC input*/
            /*Read the sample*/
            xin = read_sample();
            /*Apply AutoWah_process function to the sample*/
            yout =AutoWah_process(xin);

            /*Send the output value to your DAC or codec output*/
            write_output(yout);
            /*Makes the LFO vary*/
            AutoWah_sweep();
        }                              
    }
}

Farrow resampler implementation

Markus Nentwig August 13, 20111 comment Coded in C
/* ****************************************************
 * Farrow resampler (with optional bank switching)
 * M. Nentwig, 2011
 * Input stream is taken from stdin
 * Output stream goes to stdout
 * Main target was readability, is not optimized for efficiency.
 * ****************************************************/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>

#if 1
/* **************************************************************
 * example coefficients. 
 * Each column [c0; c1; c2; ...] describes a polynomial for one tap coefficent in fractional time ft [0, 1]:
 * tapCoeff = c0 + c1 * ft + c2 * ft ^ 2 + ...
 * Each column corresponds to one tap. 
 * The example filters is based on a 6th order Chebyshev Laplace-domain prototype.
 * This version uses three sub-segments per tap (NBANKS = 3)
 * **************************************************************/
const double cMatrix[] = {
  2.87810386e-4, 4.70096244e-3, 7.93412570e-2, 4.39824536e-1, 1.31192924e+000, 2.67892232e+000, 4.16465421e+000, 5.16499621e+000, 5.15592605e+000, 3.99000369e+000, 2.00785470e+000, -7.42377060e-2, -1.52569354e+000, -1.94402804e+000, -1.40915797e+000, -3.86484652e-1, 5.44712939e-1, 9.77559688e-1, 8.32191447e-1, 3.22691788e-1, -2.13133045e-1, -5.08501962e-1, -4.82928807e-1, -2.36313854e-1, 4.76034568e-2, 2.16891966e-1, 2.20894063e-1, 1.08361553e-1, -2.63421832e-2, -1.06276015e-1, -1.07491548e-1, -5.53793711e-2, 4.86314061e-3, 3.94357182e-2, 4.06217506e-2, 2.17199064e-2, 1.60318761e-3, -8.40370106e-3, -8.10525279e-3, -3.62112499e-3, -4.13413072e-4, 2.33101911e-4, 
  -3.26760325e-3, -6.46028234e-3, 1.46793247e-1, 5.90235537e-1, 1.18931309e+000, 1.57853546e+000, 1.40402774e+000, 5.76506323e-1, -6.33522788e-1, -1.74564700e+000, -2.24153717e+000, -1.91309453e+000, -9.55568978e-1, 1.58239169e-1, 9.36193787e-1, 1.10969783e+000, 7.33284446e-1, 1.06542194e-1, -4.15412084e-1, -6.06616434e-1, -4.54898908e-1, -1.20841199e-1, 1.82941623e-1, 3.12543429e-1, 2.49935829e-1, 8.05376898e-2, -7.83213666e-2, -1.47769751e-1, -1.18735248e-1, -3.70656555e-2, 3.72608374e-2, 6.71425397e-2, 5.17812605e-2, 1.55564930e-2, -1.40896327e-2, -2.35058137e-2, -1.59635057e-2, -3.44701792e-3, 4.14108065e-3, 4.56234829e-3, 1.59503132e-3, -3.17301882e-4,
  5.64310141e-3, 7.74786707e-2, 2.11791763e-1, 2.84703201e-1, 1.85158633e-1, -8.41118142e-2, -3.98497442e-1, -5.86821615e-1, -5.40397941e-1, -2.47558080e-1, 1.50864737e-1, 4.59312895e-1, 5.41539400e-1, 3.84673917e-1, 9.39576331e-2, -1.74932542e-1, -3.01635463e-1, -2.56239225e-1, -9.87146864e-2, 6.82216764e-2, 1.59795852e-1, 1.48668245e-1, 6.62563431e-2, -2.71234898e-2, -8.07045577e-2, -7.76841351e-2, -3.55333136e-2, 1.23206602e-2, 3.88535040e-2, 3.64199073e-2, 1.54608563e-2, -6.59814558e-3, -1.72735099e-2, -1.46307777e-2, -5.04363288e-3, 3.31049461e-3, 6.01267607e-3, 3.83904192e-3, 3.92549958e-4, -1.36315264e-3, -9.76017430e-4, 7.46699178e-5};
#define NTAPS (14)
#define NBANKS (3)
#define ORDER (2)
#else
/* Alternative example
 * Similar impulse response as above
 * A conventional Farrow design (no subdivisions => NBANKS = 1), order 3
 */ 
const double cMatrix[] = {
  -8.57738278e-3, 7.82989032e-1, 7.19303539e+000, 6.90955718e+000, -2.62377450e+000, -6.85327127e-1, 1.44681608e+000, -8.79147907e-1, 7.82633997e-2, 1.91318985e-1, -1.88573400e-1, 6.91790782e-2, 3.07723786e-3, -6.74800912e-3,
  2.32448021e-1, 2.52624309e+000, 7.67543936e+000, -8.83951796e+000, -5.49838636e+000, 6.07298348e+000, -2.16053205e+000, -7.59142947e-1, 1.41269409e+000, -8.17735712e-1, 1.98119464e-1, 9.15904145e-2, -9.18092030e-2, 2.74136108e-2,
  -1.14183319e+000, 6.86126458e+000, -6.86015957e+000, -6.35135894e+000, 1.10745051e+001, -3.34847578e+000, -2.22405694e+000, 3.14374725e+000, -1.68249886e+000, 2.54083065e-1, 3.22275037e-1, -3.04794927e-1, 1.29393976e-1, -3.32026332e-2,
  1.67363115e+000, -2.93090391e+000, -1.13549165e+000, 5.65274939e+000, -3.60291782e+000, -6.20715544e-1, 2.06619782e+000, -1.42159644e+000, 3.75075865e-1, 1.88433333e-1, -2.64135123e-1, 1.47117661e-1, -4.71871047e-2, 1.24921920e-2};
#define NTAPS (14)
#define NBANKS (1)
#define ORDER (3)
#endif

/* Set here the ratio between output and input sample rate.
 * It could be changed even during runtime! */
#define INCR (1.0 / 6.28 / 3)

/* delay line storage */
double delayLine[NTAPS];

/* Coefficient lookup "table" */
static double c(int ixTap, int ixBank, int ixPower){
  return cMatrix[ixPower * (NTAPS * NBANKS) + ixTap * NBANKS + ixBank];
}

int main (void){

  /* clear delay line */
  int ix; 
  for (ix = 0; ix < NTAPS; ++ix)
    delayLine[ix] = 0;
  
  /* Index of last input sample that was read 
   * First valid sample starts at 0 */
  int sampleIndexInput = -1;
  
  /* Position of next output sample in input stream */
  double sampleIndexOutput = 0.0;

  /* loop forever. Terminate, once out of input data. */
  while (1){
    /* Split output sample location into integer and fractional part:
     * Integer part corresponds to sample index in input stream
     * fractional part [0, 1[ spans one tap (covering NBANKS segments) */
    int sio_int = floor(sampleIndexOutput);
    double sio_fracInTap = sampleIndexOutput - (double)sio_int;
    assert((sio_fracInTap >= 0) && (sio_fracInTap < 1));
    
    /* Further split the fractional part into 
     * - bank index
     * - fractional part within the bank */
    int sio_intBank = floor(sio_fracInTap * (double) NBANKS);
    double sio_fracInBank = sio_fracInTap * (double) NBANKS - (double)sio_intBank;
    assert((sio_fracInBank >= 0) && (sio_fracInBank < 1));
    
    /* ****************************************************
     * load new samples into the delay line, until the last 
     * processed input sample (sampleIndexInput) catches
     * up with the integer part of the output stream position (sio_int)
     * ***************************************************/
    while (sampleIndexInput < sio_int){
      /* Advance the delay line one step */
      ++sampleIndexInput;
      for (ix = NTAPS-1; ix > 0; --ix)
	delayLine[ix] = delayLine[ix-1];      
      
      /* Read one input sample */    
      int flag = scanf("%lf", &delayLine[0]);
      if (flag != 1) 
	goto done; /* there's nothing wrong with "goto" as "break" for multiple loops ... */
      
    } /* while delay line behind output data */

    /* ****************************************************
     * Calculate one output sample:
     * "out" sums up the contribution of each tap
     * ***************************************************/
    double out = 0;
    int ixTap; 

    for (ixTap = 0; ixTap < NTAPS; ++ixTap){
      /* ****************************************************
       * Contribution of tap "ixTap" to output: 
       * ***************************************************/
      /* Evaluate polynomial in sio_fracInBank:
       * c(ixTap, sio_intBank, 0) is the constant coefficient 
       * c(ixTap, sio_intBank, 1) is the linear coefficient etc
       */
      double hornerSum = c(ixTap, sio_intBank, ORDER);
      int ixPower;
      for (ixPower = ORDER-1; ixPower >= 0; --ixPower){
	hornerSum *= sio_fracInBank;
	hornerSum += c(ixTap, sio_intBank, ixPower);
      } /* for ixPower */

      /* ****************************************************
       * Weigh the delay line sample of this tap with the 
       * polynomial result and add to output
       * ***************************************************/
      out += hornerSum * delayLine[ixTap];
    } /* for ixTap */
    
    /* ****************************************************
     * Generate output sample and advance the position of
     * the next output sample in the input data stream 
     * ***************************************************/
    printf("%1.12le\n", out);
    sampleIndexOutput += INCR;
  } /* loop until out of input data */
  
 done: /* out of input data => break loops and continue here */
  return 0;
} /* main */

Fast MDCT/IMDCT Based on Forward FFT

August 6, 2011 Coded in C
/******** begin of mdct.h ******** */
#ifndef __MDCT_H
#define __MDCT_H

#include <fftw3.h>

#ifdef __cplusplus
extern "C" {
#endif

#ifdef SINGLE_PRECISION
typedef float         FLOAT;
typedef fftwf_complex FFTW_COMPLEX;
typedef fftwf_plan    FFTW_PLAN;
#else // DOUBLE_PRECISION
typedef double        FLOAT;
typedef fftw_complex  FFTW_COMPLEX;
typedef fftw_plan     FFTW_PLAN;
#endif  // SINGLE_PRECISION

typedef struct {
    int           M;            // MDCT spectrum size (number of bins)
    FLOAT*        twiddle;      // twiddle factor
    FFTW_COMPLEX* fft_in;       // fft workspace, input
    FFTW_COMPLEX* fft_out;      // fft workspace, output
    FFTW_PLAN     fft_plan;     // fft configuration
} mdct_plan; 

mdct_plan* mdct_init(int M);    // MDCT spectrum size (number of bins)

void mdct_free(mdct_plan* m_plan);

void mdct(FLOAT* mdct_line, FLOAT* time_signal, mdct_plan* m_plan);

void imdct(FLOAT* time_signal, FLOAT* mdct_line, mdct_plan* m_plan);

#ifdef __cplusplus
}
#endif

#endif // __MDCT_H
/******** end of mdct.h ******** */

/******** begin of mdct.c ******** */
#ifdef SINGLE_PRECISION

#define FFTW_MALLOC   fftwf_malloc
#define FFTW_FREE     fftwf_free
#define FFTW_PLAN_1D  fftwf_plan_dft_1d
#define FFTW_DESTROY  fftwf_destroy_plan 
#define FFTW_EXECUTE  fftwf_execute

#else // DOUBLE_PRECISION

#define FFTW_MALLOC   fftw_malloc
#define FFTW_FREE     fftw_free
#define FFTW_PLAN_1D  fftw_plan_dft_1d
#define FFTW_DESTROY  fftw_destroy_plan 
#define FFTW_EXECUTE  fftw_execute

#endif // SINGLE_PRECISION

void mdct_free(mdct_plan* m_plan)
{
    if(m_plan) 
    {
        FFTW_DESTROY(m_plan->fft_plan);
        FFTW_FREE(m_plan->fft_in);
        FFTW_FREE(m_plan->fft_out);

        if(m_plan->twiddle)
            free(m_plan->twiddle);

        free(m_plan);
    }
}

#define MDCT_CLEAUP(msg, ...) \
    {fprintf(stderr, msg", %s(), %s:%d \n", \
            __VA_ARGS__, __func__, __FILE__, __LINE__); \
     mdct_free(m_plan); return NULL;}

mdct_plan* mdct_init(int M)
{
    int        n;
    FLOAT      alpha, omega, scale;
    mdct_plan* m_plan = NULL;

    if(0x00 != (M & 0x01)) 
        MDCT_CLEAUP(" Expect an even number of MDCT coeffs, but meet %d", M);

    m_plan = (mdct_plan*) malloc(sizeof(mdct_plan));
    if(NULL == m_plan)
        MDCT_CLEAUP(" malloc error: %s", "m_plan");
    memset(m_plan, 0, sizeof(m_plan[0]));

    m_plan->M = M;

    m_plan->twiddle = (FLOAT*) malloc(sizeof(FLOAT) * M);
    if(NULL == m_plan->twiddle)
        MDCT_CLEAUP(" malloc error: %s", "m_plan->twiddle");
    alpha = M_PI / (8.f * M);
    omega = M_PI / M;
    scale = sqrt(sqrt(2.f / M));    
    for(n = 0; n < (M >> 1); n++)
    {    
        m_plan->twiddle[2*n+0] = (FLOAT) (scale * cos(omega * n + alpha));
        m_plan->twiddle[2*n+1] = (FLOAT) (scale * sin(omega * n + alpha));
    }    
	
    m_plan->fft_in   
        = (FFTW_COMPLEX*) FFTW_MALLOC(sizeof(FFTW_COMPLEX) * M >> 1);    
    if(NULL == m_plan->fft_in)
        MDCT_CLEAUP(" malloc error: %s", "m_plan->fft_in");

    m_plan->fft_out  
        = (FFTW_COMPLEX*) FFTW_MALLOC(sizeof(FFTW_COMPLEX) * M >> 1);    
    if(NULL == m_plan->fft_out)
        MDCT_CLEAUP(" malloc error: %s", "m_plan->fft_out");

    m_plan->fft_plan = FFTW_PLAN_1D(M >> 1, 
                                    m_plan->fft_in, 
                                    m_plan->fft_out,    
                                    FFTW_FORWARD,
                                    FFTW_MEASURE);
    if(NULL == m_plan->fft_plan)
        MDCT_CLEAUP(" malloc error: %s", "m_plan->fft_plan");

    return m_plan;
}

void mdct(FLOAT* mdct_line, FLOAT* time_signal, mdct_plan* m_plan)
{
    FLOAT *xr, *xi, r0, i0;
    FLOAT *cos_tw, *sin_tw, c, s;
    int    M, M2, M32, M52, n;

    M   = m_plan->M;
    M2  = M >> 1;
    M32 = 3 * M2;
    M52 = 5 * M2;

    cos_tw = m_plan->twiddle;
    sin_tw = cos_tw + 1; 
    
    /* odd/even folding and pre-twiddle */
    xr = (FLOAT*) m_plan->fft_in;
    xi = xr + 1;
    for(n = 0; n < M2; n += 2) 
    {
        r0 = time_signal[M32-1-n] + time_signal[M32+n];    
        i0 = time_signal[M2+n]    - time_signal[M2-1-n];    
        
        c = cos_tw[n];
        s = sin_tw[n];

        xr[n] = r0 * c + i0 * s;
        xi[n] = i0 * c - r0 * s;
    }

    for(; n < M; n += 2) 
    {
        r0 = time_signal[M32-1-n] - time_signal[-M2+n];    
        i0 = time_signal[M2+n]    + time_signal[M52-1-n];    
        
        c = cos_tw[n];
        s = sin_tw[n];

        xr[n] = r0 * c + i0 * s;
        xi[n] = i0 * c - r0 * s;
    }

    /* complex FFT of size M/2 */
    FFTW_EXECUTE(m_plan->fft_plan);

    /* post-twiddle */
    xr = (FLOAT*) m_plan->fft_out;
    xi = xr + 1;
    for(n = 0; n < M; n += 2)
    {
        r0 = xr[n];
        i0 = xi[n];
        
        c = cos_tw[n];
        s = sin_tw[n];    

        mdct_line[n]     = - r0 * c - i0 * s;
        mdct_line[M-1-n] = - r0 * s + i0 * c;
    }
}

void imdct(FLOAT* time_signal, FLOAT* mdct_line, mdct_plan* m_plan)
{
    FLOAT *xr, *xi, r0, i0, r1, i1;
    FLOAT *cos_tw, *sin_tw, c, s;
    int    M, M2, M32, M52, n;

    M   = m_plan->M;
    M2  = M >> 1;
    M32 = 3 * M2;
    M52 = 5 * M2;

    cos_tw = m_plan->twiddle;
    sin_tw = cos_tw + 1; 
    
    /* pre-twiddle */
    xr = (FLOAT*) m_plan->fft_in;
    xi = xr + 1;
    for(n = 0; n < M; n += 2)
    {
        r0 =  mdct_line[n];	
        i0 =  mdct_line[M-1-n];
        
        c = cos_tw[n];
        s = sin_tw[n];    
        
        xr[n] = -i0 * s - r0 * c;
        xi[n] = -i0 * c + r0 * s;
    } 
    
    /* complex FFT of size M/2 */
    FFTW_EXECUTE(m_plan->fft_plan);

    /* odd/even expanding and post-twiddle */
    xr = (FLOAT*) m_plan->fft_out;
    xi = xr + 1;
    for(n = 0; n < M2; n += 2) 
    {
        r0 = xr[n]; 
        i0 = xi[n];    
        
        c = cos_tw[n]; 
        s = sin_tw[n];

        r1 = r0 * c + i0 * s;
        i1 = r0 * s - i0 * c;

        time_signal[M32-1-n] =  r1;
        time_signal[M32+n]   =  r1;
        time_signal[M2+n]    =  i1;
        time_signal[M2-1-n]  = -i1;
    }

    for(; n < M; n += 2) 
    {
        r0 = xr[n]; 
        i0 = xi[n];    
        
        c = cos_tw[n]; 
        s = sin_tw[n];
        
        r1 = r0 * c + i0 * s;
        i1 = r0 * s - i0 * c;
        
        time_signal[M32-1-n] =  r1;
        time_signal[-M2+n]   = -r1;
        time_signal[M2+n]    =  i1;
        time_signal[M52-1-n] =  i1;
    }
}
/******** end of mdct.c ******** */

/******** begin of mdct_test.c ******** */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/time.h>
#include <time.h>
#include "mdct.h"

int main(int argc, char* argv[])
{
    int        M, r, i;
    FLOAT*     time = NULL;
    FLOAT*     freq = NULL;
    mdct_plan* m_plan = NULL;
    char*      precision = NULL;
    struct timeval t0, t1;
    long long elps;

    if(3 != argc)
    {
        fprintf(stderr, " Usage: %s <MDCT_SPECTRUM_SIZE> <run_times> \n", argv[0]);
        return -1;
    }

    sscanf(argv[1], "%d", &M);
    sscanf(argv[2], "%d", &r);    
    if(NULL == (m_plan = mdct_init(M)))
        return -1;
    if(NULL == (time = (FLOAT*) malloc(2 * M * sizeof(FLOAT))))
        return -1;
    if(NULL == (freq = (FLOAT*) malloc(M * sizeof(FLOAT))))
        return -1;

    for(i = 0; i < 2 * M; i++)
        time[i] = 2.f * rand() / RAND_MAX - 1.f;        
    for(i = 0; i < M; i++)
        freq[i] = 2.f * rand() / RAND_MAX - 1.f;        

    precision = (sizeof(float) == sizeof(FLOAT))? 
                "single precision" : "double precision";

#if 1
    gettimeofday(&t0, NULL);
    for(i = 0; i < r; i++)
        mdct(freq, time, m_plan);
    gettimeofday(&t1, NULL);

    elps = (t1.tv_sec - t0.tv_sec) * 1000000 + (t1.tv_usec - t0.tv_usec);    
    fprintf(stdout, "MDCT size of %d, %s, running %d times, average %.3f ms\n", 
            M, precision, r, (FLOAT) elps / r / 1000.f); 
#endif // 0

#if 1    
    gettimeofday(&t0, NULL);
    for(i = 0; i < r; i++)
        imdct(time, freq, m_plan);
    gettimeofday(&t1, NULL);

    elps = (t1.tv_sec - t0.tv_sec) * 1000000 + (t1.tv_usec - t0.tv_usec);    
    fprintf(stdout, "IMDCT size of %d, %s, running %d times, average %.3f ms\n", 
            M, precision, r, (FLOAT) elps / r / 1000.f); 
#endif //0

#if 0
    for(i = 0; i < 2 * M; i++)
        fprintf(stdout, "%f    ", time[i]);
    fprintf(stdout, "\n");

    for(i = 0; i < M; i++)
        fprintf(stdout, "%f    ", freq[i]);
    fprintf(stdout, "\n");
#endif // 0

    free(time);
    free(freq);
    mdct_free(m_plan);    

    return 0;
}
/******** end of mdct_test.c ******** */

Phaser audio effect

Gabriel Rivas July 4, 20112 comments Coded in C
/*
Phaser audio effect:

Xin                                                                       Yout
-------------------------------[dir_mix]--------------------->[+]--------->
      |                                                        ^  
      |                                                        |
      |-->[VNS1]-->[VNS2]-->[VNS3]...-->[VNSN]-->[pha_mix]------      

            ^        ^        ^           ^ 
            |        |        |           |
            |--------|--------|---...------
            |
          [LFO]         

VNS = Variable notch stage

*/

#include "br_iir.h"
#include "Phaser.h"

/*This defines the phaser stages
that is the number of variable notch blocks
*/
#define PH_STAGES 20

static short center_freq;    /*Center frequency counter*/
static short samp_freq;      /*Sampling frequency*/
static short counter;        /*Smaple counter*/
static short counter_limit;  /*Smaple counter limit*/
static short control;        /*LFO Control*/
static short max_freq;       /*Maximum notch center frequency*/
static short min_freq;       /*Minimum notch center frequency*/ 
static double pha_mix;       /*Filtered signal mix*/
static short f_step;         /*Sweep frequency step*/ 
static double dir_mix;       /*Direct signal mix*/
static struct br_filter H[PH_STAGES]; /*Array of notch filters stages*/

/*
This funtion initializes the phaser control variables
and the variable notch filter coefficients array
*/
void Phaser_init(short effect_rate,short sampling,short maxf,short minf,short Q,double gainfactor,double pha_mixume,short freq_step, double dmix) {
    /*Initialize notch filter coefficients set array*/
    br_iir_init(sampling,gainfactor,Q,freq_step, minf);
   
    /*Initializes the phaser control variables*/
    center_freq = 0;
    samp_freq = sampling;
    counter = effect_rate;
    control = 0;
    counter_limit = effect_rate;

    /*Convert frequencies to integer indexes*/
    min_freq = 0;
    max_freq = (maxf - minf)/freq_step;

    pha_mix = pha_mixume;
    f_step = freq_step;
    dir_mix = dmix;
}

/*
This function does the actual phasing processing
1. It takes the input sample and pass it trough the
cascaded notch filter stages
2. It takes tha output of the cascaded notch filters
and scales it, scales the input sample and generate
the output effect sample.
*/
double Phaser_process(double xin) {
    double yout;
    int i;

    yout = br_iir_filter(xin,&H[0]);

    for(i = 1; i < PH_STAGES; i++) {
        yout = br_iir_filter(yout,&H[i]);
    }
     
    yout = dir_mix*xin + pha_mix*yout;
      
    return yout;
}

/*
This function makes vary the center notch frequency
in all the cascaded notch filter stages by a simulated
triangle wave LFO that goes up and down
*/
void Phaser_sweep(void) {
    int i;

    if (!--counter) {
        if (!control) {
            center_freq+=f_step;

            if (center_freq > max_freq) {
                control = 1;
            }
        }
        else if (control) {
            center_freq-=f_step;

            if (center_freq == min_freq) {               
                control = 0;
            }
        }
        for(i = 0; i < PH_STAGES; i++) {
            br_iir_setup(&H[i],center_freq);
        }       
        counter = counter_limit;
    }
}

/************

Phaser.h

***********/

#ifndef __PHASER_H__
#define __PHASER_H__

extern void Phaser_init(short effect_rate,short sampling,short maxf,short minf,short Q,double gainfactor,double mix_volume,short freq_step, double dmix);
extern double Phaser_process(double xin);
extern void Phaser_sweep(void);

#endif

Variable 2nd order band pass IIR filter

Gabriel Rivas July 4, 20112 comments Coded in C
#include "bp_iir.h"
#include <math.h>
#define BP_MAX_COEFS 120
#define PI 3.1415926

/*This is an array of the filter parameters object
defined in the br_iir.h file*/
static struct bp_coeffs bp_coeff_arr[BP_MAX_COEFS];

/*This initialization function will create the
band pass filter coefficients array, you have to specify:
fsfilt = Sampling Frequency
gb     = Gain at cut frequencies
Q      = Q factor, Higher Q gives narrower band
fstep  = Frequency step to increase center frequencies
in the array
fmin   = Minimum frequency for the range of center   frequencies
*/
void bp_iir_init(double fsfilt,double gb,double Q,short fstep, short fmin) {
      int i;
      double damp;
      double wo;
      
       damp = gb/sqrt(1 - pow(gb,2));

	for (i=0;i<BP_MAX_COEFS;i++) {
            wo = 2*PI*(fstep*i + fmin)/fsfilt;
		bp_coeff_arr[i].e = 1/(1 + damp*tan(wo/(Q*2)));
		bp_coeff_arr[i].p = cos(wo);
		bp_coeff_arr[i].d[0] = (1-bp_coeff_arr[i].e);
		bp_coeff_arr[i].d[1] = 2*bp_coeff_arr[i].e*bp_coeff_arr[i].p;
		bp_coeff_arr[i].d[2] = (2*bp_coeff_arr[i].e-1);
	}
}

/*This function loads a given set of band pass filter coefficients acording to a center frequency index
into a band pass filter object
H = filter object
ind = index of the array mapped to a center frequency
*/
void bp_iir_setup(struct bp_filter * H,int ind) {
	H->e = bp_coeff_arr[ind].e;
	H->p = bp_coeff_arr[ind].p;
	H->d[0] = bp_coeff_arr[ind].d[0];
	H->d[1] = bp_coeff_arr[ind].d[1];
	H->d[2] = bp_coeff_arr[ind].d[2];
}

/*This function loads a given set of band pass filter coefficients acording to a center frequency index
into a band pass filter object
H = filter object
ind = index of the array mapped to a center frequency
*/
double bp_iir_filter(double yin,struct bp_filter * H) {
	double yout;

	H->x[0] =  H->x[1]; 
	H->x[1] =  H->x[2]; 
	H->x[2] = yin;
	
	H->y[0] =  H->y[1]; 
	H->y[1] =  H->y[2]; 

	H->y[2] = H->d[0]* H->x[2] - H->d[0]* H->x[0] + (H->d[1]* H->y[1]) - H->d[2]* H->y[0];
	
	yout =  H->y[2];

	return yout;
}

/******************
bp_iir.h
**********************/

#ifndef __BP_IIR_H__
#define __BP_IIR_H__

struct bp_coeffs{
	double e;
	double p;
	double d[3];
};

struct bp_filter{
	double e;
	double p;
	double d[3];
	double x[3];
	double y[3];
};

extern void bp_iir_init(double fsfilt,double gb,double Q,short fstep, short fmin);
extern void bp_iir_setup(struct bp_filter * H,int index);
extern double bp_iir_filter(double yin,struct bp_filter * H);

#endif

Variable 2nd order Notch IIR filter

Gabriel Rivas June 21, 20112 comments Coded in C
#include "br_iir.h"
#include <math.h>

#define BR_MAX_COEFS 120
#define PI 3.1415926

/*This is an array of the filter parameters object
defined in the br_iir.h file*/
static struct br_coeffs br_coeff_arr[BR_MAX_COEFS];

/*This initialization function will create the
notch filter coefficients array, you have to specify:
fsfilt = Sampling Frequency
gb     = Gain at cut frequencies
Q      = Q factor, Higher Q gives narrower band
fstep  = Frequency step to increase center frequencies
in the array
fmin   = Minimum frequency for the range of center   frequencies
*/
void br_iir_init(double fsfilt,double gb,double Q,double fstep, short fmin) {
	int i;
      double damp;
      double wo;
      
      damp = sqrt(1 - pow(gb,2))/gb;

	for (i=0;i<BR_MAX_COEFS;i++) {
                wo = 2*PI*(fstep*i + fmin)/fsfilt;
		br_coeff_arr[i].e = 1/(1 + damp*tan(wo/(Q*2)));
		br_coeff_arr[i].p = cos(wo);
		br_coeff_arr[i].d[0] = br_coeff_arr[i].e;
		br_coeff_arr[i].d[1] = 2*br_coeff_arr[i].e*br_coeff_arr[i].p;
		br_coeff_arr[i].d[2] = (2*br_coeff_arr[i].e-1);
	}
}

/*This function loads a given set of notch filter coefficients acording to a center frequency index
into a notch filter object
H = filter object
ind = index of the array mapped to a center frequency
*/
void br_iir_setup(struct br_filter * H,int ind) {
	H->e = br_coeff_arr[ind].e;
	H->p = br_coeff_arr[ind].p;
	H->d[0] = br_coeff_arr[ind].d[0];
	H->d[1] = br_coeff_arr[ind].d[1];
	H->d[2] = br_coeff_arr[ind].d[2];
}

/*
This function does the actual notch filter processing
yin = input sample
H   = filter object 
*/
double br_iir_filter(double yin,struct br_filter * H) {
	double yout;

	H->x[0] = H->x[1]; 
	H->x[1] = H->x[2]; 
	H->x[2] = yin;
	
	H->y[0] = H->y[1]; 
	H->y[1] = H->y[2]; 

	H->y[2] = H->d[0]*H->x[2] - H->d[1]*H->x[1] + H->d[0]*H->x[0] + H->d[1]*H->y[1] - H->d[2]*H->y[0];
	
	yout = H->y[2];
	return yout;
}

/******************/
#ifndef __BR_IIR_H__
#define __BR_IIR_H__

/*
Notch filter coefficients object
*/
struct br_coeffs {
	double e;
	double p;
	double d[3];
};

/*
Notch filter object
*/
struct br_filter {
	double e;
	double p;
	double d[3];
	double x[3];
	double y[3];
};

extern void br_iir_init(double fsfilt,double gb,double Q,double fstep, short fmin);
extern void br_iir_setup(struct br_filter * H,int index);
extern double br_iir_filter(double yin,struct br_filter * H);

#endif

T1 F281x Mcbsp Transmitter Configuration

Emmanuel May 13, 2011 Coded in C for the TI F28x
/**********************************************************************/
#include "DSP281x_Device.h"
/**********************************************************************
Parameter					Condition		Register		Value

Delay between data frames:	1 bit			XCR2.XDATDLY	0b01
Word Length:				16 bits			XCR1.XDLEN1		0b010
Words per frame:			1 word			XCR1.XFRLEN1	0
sample rate:				150 kHz			SRGR1.CLKGDV	0xC7
															(LSPCLK=30 MHz)

************************************************************************/
void InitMcbsp(void)
{

/*** Mcbsp	Reset	*/

	McbspaRegs.SPCR2.bit.XRST= 0;
	McbspaRegs.SPCR2.bit.GRST= 0;
	McbspaRegs.SPCR2.bit.FRST= 0;
	
/*** SPCR2 Register Configuration	*/

	McbspaRegs.SPCR2.all = 0x0000;
	
// bit 15-10 	0's:	Reserved
// bit 9 		0:		FREE Free running mode
// bit 8 		0:		SOFT Soft bit
// bit 7 		0:		FRST Frame sync generator reset
// bit 6 		0:		GRST Sample rate generator reset
// bit 5-4 		00:		XINTM Transmit interrupt mode
// bit 3 		0:		XSYNCERR Transmit synchronization error
// bit 2 		0:		XEMPTY Transmit shift register (XSR) empty
// bit 1 		0:		XRDY Transmitter ready
// bit 0 		0:		XRST Transmitter reset
	
/*** SPCR1 Register Configuration 		*/	

	McbspaRegs.SPCR1.all = 0x0000;

// bit 15 		0:		DLB Digital Loop back mode
// bit 14-13 	0:		RJUST Receive sign extension and justification mode
// bit 12-11 	00:		CLKSTP Clock Stop Mode
// bit 10-8 	0's:	Reserved
// bit 7 		0:		DXENA DX enabler. 
// bit 6 		0:		ABIS ABIS mode
// bit 5-4 		00:		RINTM Receive Interrupt Mode
// bit 3 		0		RSYNCERR Receive Synchronization error
// bit 2 		0		RFULL Receive shift register full
// bit 1 		0		RRDY Receiver ready
// bit 0 		0		RRST Receiver reset

/*** XCR2 Register Configuration 	*/	

	McbspaRegs.XCR2.all = 0x0001;

// bit 15 		0:		XPHASE Transmit phase
// bit 14-8 	0's;	XRFRLEN2 Transmit frame length 2
// bit 7-5 		000:	XWDLEN2 Transmit word length 2
// bit 4-3 		00:		XCOMPAND Transmit companding mode
// bit 2 		0:		XFIG Transmite frame ignore
// bit 1-0 		01:		XDATDLY Data Delay (Delay in bits between two frames)
	
/*** XCR1 Register Configuration 	*/	

	McbspaRegs.XCR1.all = 0x0040;
	
// bit 15 		0		Reserved
// bit 14-8 	0's:	XFRLEN1 Frame Length 1 (Words per frame)
// bit 7-5 		010:	XWDLEN1 Word Length 1  (bits per word)
// bit 4 		0:		Reserved
// bit 3-0 		0's:	Reserved

/*** SRGR2 Register Configuration 	*/

	McbspaRegs.SRGR2.all = 0x2011;

// bit 15 		0;		GSYNC Sample rate generator clock synchronization
// bit 14 		0:		Reserved
// bit 13 		1:		CLKSM Mcbsp sample rate generator clock mode: internal clock
// bit 12 		0:		FSGM Transmit frame synchronization mode
// bit 11-0 	x011:	FPER Frame Period (number of CLKG cycles between sync signals): 18 cycles

/*** SRGR1 Register Configuration 	*/	

	McbspaRegs.SRGR1.all = 0x00E8;

// bit 15-8 	x00:	FWID Frame Width
// bit 7-0 		xC7		CLKGDV Sample rate generator clock divider

/*** MCR2 Register Configuration 	*/	

	McbspaRegs.MCR2.all = 0x0200;
	
// bit 15-10 	0's:	Reserved
// bit 9 		1:		XMCME Enhanced transmit multichannel selection enable
// bit 8-7 		00:		XPBBLK Receive/transmit partition B block
// bit 6-5 		00:		XPABLK Receive/transmit partition A block
// bit 4-2 		000:	XCBLK Receive/transmit current block
// bit 1-0 		00:		XMCM Transmit multichannel selection enable

/*** MCR1 Register Configuration  */	

	McbspaRegs.MCR1.all = 0x0201;
	
// bit 15-10 	0's:	Reserved
// bit 9 		1		RMCME Enhanced receive multichannel selection enable
// bit 8-7 		00		RPBBLK Receive/transmit partition B block
// bit 6-5 		00		RPABLK Receive/transmit partition A block
// bit 4-2 		000		RCBLK Receive transmit current block
// bit 1 		0		Reserved
// bit 0 		1		RMCM Receive multichannel selection enable

/*** PCR Register Configuration */	

	McbspaRegs.PCR.all = 0x0A00;

// bit 15-12 	0's:	Reserved
// bit 11 		1:		FSXM Transmit frame synchronization mode
// bit 10 		0:		FSRM Receive frame synchronization mode
// bit 9 		1:		CLKXM Transmit frame synchronization mode (internal, FSR as output)
// bit 8 		0:		CLKRM Receiver clock mode
// bit 7 		0:		SCLKME Sample clock mode selection bit
// bit 6 		0:		CLKS_STAT Reserved 
// bit 5 		0:		DX_STAT DX pin status
// bit 4 		0:		DR_STAT DR pin status
// bit 3 		0:		FSXP Transmit frame synchronization polarity
// bit 2 		0:		FSRP Receive frame synchronization polarity
// bit 1 		0:		CLKXP Tramsmit clock polarity
// bit 0 		0:		CLKRP Receive clock polarity
	

	
/*** Mcbsp Start-up		*/

	McbspaRegs.SPCR2.bit.XRST= 1;
	McbspaRegs.SPCR2.bit.GRST= 1;
	McbspaRegs.SPCR2.bit.FRST= 1;
	

/*** Interruption Configuration  */

	McbspaRegs.MFFINT.bit.XINT= 1;

	PieCtrlRegs.PIEIER6.bit.INTx6= 1;	
    IER |= 0x0020;   
    
    

}

/**End of function*/