process_data.c

noise cancelation useing DSP BF537

///////////////////////////////////////////////////////////////////////////////
//
// 	
//  Experiment 4.5_BF537 Adaptive line enhancer (ALE) with BF537 EZ-KIT
//  FILE name: Process_data.c
//
//  Description: Perform real-time ALE to remove noisy signal.
//		 
//  
//  For the book "Embedded Signal Processing with the Micro Signal Architecture"
//		  By Woon-Seng Gan and Sen M. Kuo
//		  Publisher: John Wiley and Sons, Inc.
//
//  Tools used: VisualDSP++ v4.0 (running on BF537 EZ-KIT)
//
///////////////////////////////////////////////////////////////////////////////

#include "ALE.h"


fract16 q1[100*INPUT_SIZE];
fract16 q2[100*INPUT_SIZE];
double z=90*INPUT_SIZE;
int i=0;
int j=0;
float aux=0.5;
float aux1=0;
//int mod;
//int test=0;
fract16 out[INPUT_SIZE];
int count=0;


// LMS filter function prototype
void LMS_filter(fract16[], fract16[], fract16[], fract16[], fract16[], int, int, int, fir_state_fr16*);
fract16 Calc_mu(fract16[], int);

void Process_Data(void)
{
	int k;
	
	
	
	
	
		i++;
	if (i<z)
	if (j==0)
	{	
		if (i<z-1)
		{
			fract16	sample;
	
		for (k=0; k<INPUT_SIZE; k++)
		{
			sample = sCh0LeftIn[k];
		//	sCh0RightOut[k] = sCh0RightIn[k];
			q1[i++]= sample ;
			out[k] = q1[i+10*INPUT_SIZE] + q1[i];
		}
//			fract16	sample=sCh0LeftIn;
//			q1[i]= sample;
//			out = q1[i+1]*aux+q1[i];
//			if (test == 0)
//			calc_var();
			//if (ucMode == OUT1)
			//{
			//out = out - q1[i+1]*aux1;
			//iChannel0LeftOut = ((( (int)out)<<8)>>0)>>8 ;	
			//iChannel0RightOut = ((( (int)out)<<8)>>0)>>8 ;
			//}
			//if (ucMode == OUT2)
			//{
			//iChannel0LeftOut = ((( (int)out)<<8)>>0)>>8 ;	
			//iChannel0RightOut = ((( (int)out)<<8)>>0)>>8 ;
			//}
		}
	/*	if (i==z-1)
		{
			int	sample=((iChannel0LeftIn<<8)>>0)>>8;
			q1[i]= sample ;
			out = q2[1]*aux + q1[i];
			iChannel0LeftOut = (((  (int)out)<<8)>>0)>>8 ;
			iChannel0RightOut = ((( (int)out)<<8)>>0)>>8 ;	
		}*/
	}
	
	
	if (j==1)
	{	
		if (i<z-1)
		{
		fract16	sample;
	
		for (k=0; k<INPUT_SIZE; k++)
		{
			sample = sCh0LeftIn[k];
		//	sCh0RightOut[k] = sCh0RightIn[k];
			q2[i++]= sample ;
			out[k] = q2[i+1+256] + q2[i];
		}
			//if (ucMode == OUT1)
			//{
			//out = out - q2[i+1]*aux1;
			//iChannel0LeftOut = ((( (int)out)<<8)>>0)>>8 ;	
			//iChannel0RightOut = ((( (int)out)<<8)>>0)>>8 ;
			//}
			//if (ucMode == OUT2)
			//{
			//	iChannel0LeftOut = ((( (int)out)<<8)>>0)>>8 ;	
			//	iChannel0RightOut = ((( (int)out)<<8)>>0)>>8 ;
			//}
		}
	/*	if (i==z-1)
		{
			int	sample=((iChannel0LeftIn<<8)>>0)>>8;
			q2[i]= sample ;
			out = q1[1]*aux + q2[i];
		    iChannel0LeftOut = ((( (int)out)<<8)>>0)>>8 ;	
			iChannel0RightOut = ((( (int)out)<<8)>>0)>>8 ;
		}*/
	
	}
	
	
	
	
	if (i >= z+1)
		{
		j=2-(j+1);
		i=-1;
		count++;
		}	

	
	
	
	
	
	
	if (ucMode == OUT1) // Passthrough mode
	{
		for (k=0; k<INPUT_SIZE; k++)
		{
		//	sCh0LeftOut[k] = sCh0LeftIn[k];
		//	sCh0RightOut[k] = sCh0RightIn[k];
			sCh0LeftOut[k] = out[k];
			sCh0RightOut[k] = out[k];
		
		}
		
	}
	else
	{	
		LMS_filter(out,LMSOutPredL,LMSOutErrL,lmsdelayL,coefL,TAPS,INPUT_SIZE,DELAY_SIZE,&firstateL);
		LMS_filter(out,LMSOutPredR,LMSOutErrR,lmsdelayR,coefR,TAPS,INPUT_SIZE,DELAY_SIZE,&firstateR);
		
		if (ucMode == OUT2) // To listen to prediction (adaptive filter output)
		{
			for (k=0; k<INPUT_SIZE; k++)
			{
				sCh0LeftOut[k] = LMSOutPredL[k];
				sCh0RightOut[k] = LMSOutPredR[k];
			}
		}
		else	// To listen to error signal (input - prediction)
		{
			for (k=0; k<INPUT_SIZE; k++)
			{
				sCh0LeftOut[k] = LMSOutErrL[k];
				sCh0RightOut[k] = LMSOutErrR[k];
			}
		}
	}
}

// LMS_filter function definition
// Input parameters sequence:
//        input,prediction,error,delay buff,coeff,no.of tap,block size,no.of delay,fir filter state
    		
void LMS_filter(fract16 in[],fract16 out1[],fract16 out2[],fract16 d[],fract16 c[],int taps,int size,int delaysize, fir_state_fr16* s)
{	
	fract16 acc = 0, err = 0, mu, mu_err;
	int i, j;
	int subs = size+delaysize-1;
	
	for (i=0; i<size; i++)
	{
		d[size-i-1] = in[i];
		
		// call fir filter built-in function
		fir_fr16(&d[subs-i], &acc, 1, s);
		
		err = in[i] - acc;
		//mu = Calc_mu(&d[subs-i], taps); // compute step size based on power of input signal(NLMS)
		mu = 0x200;						  // fixed-size step size: 0x01, 0x10,0x20,0x100
		mu_err = multr_fr1x16(mu, err);
		
		for (j=0; j<taps; j++)
		{	
			c[j] = c[j] + multr_fr1x16(mu_err, d[(subs-i)+j]);	
		}
		
		out1[i] = acc; // prediction/output of adaptive filter
		out2[i] = err; // diff between input and prediction
	}
	
	// linear buffering is used (updating of delay buffer is done
	//							after processing the whole block)
	for (i=0; i<taps+delaysize-1; i++)
	{
		d[size+i] = d[i];
	}	
}

fract16 Calc_mu(fract16 current[], int n)
{	
	static fract16 est_power = 0; // also used to store previous estimation of power
	
	// P(n) = (1-Beta) * P(n-1) + Beta * (x(n)^2)
	est_power = multr_fr1x16
				(
					BETA1,
					est_power
				)
				+
				multr_fr1x16
				(
					BETA,
					multr_fr1x16
					(
						current[0],
						current[0]
					)
				);
				
	// The following computes: (alpha/N)/Px
	// Precomputed constant: 0.125/32  (or 0x80 in fract16)	
	return div_s(0x0080, est_power);	
}