www.pudn.com > aec_fix_zhang.rar > aec.c, change:2008-11-05,size:6720b


#include <stdio.h> 
#include <stdlib.h> 
#include <math.h> 
#include <string.h> 
 
 
//#include "recip.h" 
//#include <fastrts62x64x.h> 
 
 
#include "aec.h" 
 
#define M_PI        3.14159265358979323846f 
 
AECSTATDEF aecdata; 
/* 
#pragma DATA_SECTION(a, "EXTERNALHEAP") 
#pragma DATA_ALIGN(a, 128) 
float a[MAX_NLMS_LEN + NLMS_EXT]; 
 
#pragma DATA_SECTION(b, "EXTERNALHEAP") 
#pragma DATA_ALIGN(b, 128) 
float b[MAX_NLMS_LEN + NLMS_EXT]; 
 
#pragma DATA_SECTION(c, "EXTERNALHEAP") 
#pragma DATA_ALIGN(c, 128) 
float c[MAX_NLMS_LEN]; 
*/ 
/* Vector Dot Product */ 
#pragma CODE_SECTION(dotp, ".AEC:dotp") 
int dotp(int a[], int b[], int len ) 
{ 
	__int64 sum = 0; 
	__int64 temp; 
	int j; 
	 
	for (j = 0; j < len; j ++)  
	{ 
		temp = a[j] * b[j]; 
		sum = temp + sum;//a[j] * b[j]; 
	} 
	return sum >> 16; 
} 
 
 
void AEC_init(AECSTATDEF *aec, int sft) 
{ 
	AEC_setSamplFreqType( aec, sft); 
	aec->RATE = aec->samplFreqType * 8000; 
 
	// NLMS filter length in taps (samples). A longer filter length gives 
	// better Echo Cancellation, but maybe slower convergence speed and 
	// needs more CPU power (Order of NLMS is linear)  
	aec->NLMS_LEN = 100 * aec->samplFreqType * 8;	// 在此设置时间长度 
 
	// Vector w visualization length in taps (samples). 
	// Must match argv value for wdisplay.tcl  
	aec->DUMP_LEN = 40 * aec->samplFreqType * 8; 
 
	// Leaky hangover in taps 
	aec->Thold = 60 * aec->samplFreqType * 8; 
 
	aec->hangover = 0; 
	memset(aec->x, 0, sizeof(aec->x)); 
	memset(aec->xf, 0, sizeof(aec->xf)); 
	memset(aec->w, 0, sizeof(aec->w)); 
	aec->j = NLMS_EXT; 
	aec->delta = 0; 
	AEC_setambient( aec, NoiseFloor); 
	aec->dfast = aec->dslow = M75dB_PCM << 16; 
	aec->xfast = aec->xslow = M80dB_PCM << 16; 
	aec->gain = 1; 
 
} 
 
// soft decision DTD 
// (Dual Average Near-End to Far-End signal Ratio DTD) 
// This algorithm uses exponential smoothing with differnt 
// ageing parameters to get fast and slow near-end and far-end 
// signal averages. The ratio of NFRs term 
// (dfast / xfast) / (dslow / xslow) is used to compute the stepsize 
// A ratio value of 2.5 is mapped to stepsize 0, a ratio of 0 is 
// mapped to 1.0 with a limited linear function. 
#pragma CODE_SECTION(AEC_dtd, ".AEC:AEC_dtd") 
int AEC_dtd(AECSTATDEF *aec, int d, int x) 
{ 
	int stepsize; 
	int ratio; 
	int M; 
	int t1,t2,t3,t4,t5; 
 
	// fast near-end and far-end average 
	aec->dfast += ALPHAFAST * (abs(d) - (aec->dfast >> 16)); 
	aec->xfast += ALPHAFAST * (abs(x) - (aec->xfast >> 16)); 
	 
	// slow near-end and far-end average 
	aec->dslow += ALPHASLOW * (abs(d) - (aec->dslow >> 16)); 
	aec->xslow += ALPHASLOW * (abs(x) - (aec->xslow >> 16)); 
	 
	if (aec->xfast < (M70dB_PCM << 16))  
	{ 
		return 0;   // no Spk signal 
	} 
     
	if (aec->dfast < (M70dB_PCM << 16))  
	{ 
		return 0;   // no Mic signal 
	} 
   	 
	// ratio of NFRs 
	t1 = (aec->dfast << 8) / aec->dslow; 
	t2 = (aec->xslow << 8) / aec->xfast; 
	ratio = t1 * t2; 
	//ratio = (aec->dfast * aec->xslow) / (aec->dslow * aec->xfast); 
	 
	// begrenzte lineare Kennlinie 
	M = (STEPY2 - STEPY1) / (STEPX2 - STEPX1); 
	if (ratio < (STEPX1 << 16))  
	{ 
		stepsize = STEPY1 << 16; 
	}  
	else if (ratio > (STEPX2 << 16))  
	{ 
		stepsize = STEPY2 << 16; 
	} 
	else  
	{ 
		// Punktrichtungsform einer Geraden 
		stepsize = M * (ratio - (STEPX1 << 16)) + (STEPY1 << 16); 
	} 
	 
	return stepsize; 
} 
 
 
// The xfast signal is used to charge the hangover timer to Thold. 
// When hangover expires (no Spk signal for some time) the vector w 
// is erased. This is my implementation of Leaky NLMS. 
#pragma CODE_SECTION(AEC_leaky, ".AEC:AEC_leaky") 
void AEC_leaky(AECSTATDEF *aec) 
{ 
	if (aec->xfast >= (M70dB_PCM << 16))  
	{ 
		// vector w is valid for hangover Thold time 
		aec->hangover = aec->Thold; 
	}  
	else  
	{ 
		if (aec->hangover > 1)  
		{ 
			--aec->hangover; 
		}  
		else if (1 == aec->hangover)  
		{ 
			--aec->hangover; 
			// My Leaky NLMS is to erase vector w when hangover expires 
			memset(aec->w, 0, sizeof(aec->w)); 
		} 
	}      
} 
 
#pragma CODE_SECTION(AEC_nlms_pw, ".AEC:AEC_nlms_pw") 
int AEC_nlms_pw(AECSTATDEF *aec, int d, int x_, int stepsize) 
{ 
	int e; 
	int ef; 
	int t1,t2,t3; 
	int mikro_ef; 
 
	aec->x[aec->j] = x_; 
	aec->xf[aec->j] = x_; //IIR1_HP_apply(&aec->Fx, x_);    // pre-whitening of x 
 
	// calculate error value  
	// (mic signal - estimated mic signal from spk signal) 
	e = d; 
	if (aec->hangover > 0)  
	{ 
		e -= dotp(aec->w, aec->x + aec->j, aec->NLMS_LEN); 
	} 
	ef = e; //IIR1_HP_apply(&aec->Fe, e);     // pre-whitening of e 
	 
	// optimize: iterative dotp(xf, xf) 
	t1 = aec->xf[aec->j] * aec->xf[aec->j]; 
	t2 = aec->xf[aec->j + aec->NLMS_LEN - 1] * aec->xf[aec->j + aec->NLMS_LEN - 1]; 
	t3 = t1 - t2; 
	aec->dotp_xf_xf = aec->dotp_xf_xf + t3; 
	//aec->dotp_xf_xf += (aec->xf[aec->j] * aec->xf[aec->j] - aec->xf[aec->j + aec->NLMS_LEN - 1] * aec->xf[aec->j + aec->NLMS_LEN - 1]); 
	 
	if (stepsize > 0)  
	{ 
		// calculate variable step size 
		int temp; 
		int i; 
		mikro_ef = (((__int64)stepsize * ef) << 11) / aec->dotp_xf_xf; //加快处理速度		 
		 
		// update tap weights (filter learning)		 
		for (i = 0; i < aec->NLMS_LEN; i ++)  
		{ 
			temp = (mikro_ef * aec->xf[i + aec->j]) >> 10; 
			aec->w[i] = aec->w[i] + temp; 
		} 
	} 
	 
	if (--aec->j < 0) 
	{ 
		// optimize: decrease number of memory copies 
		aec->j = NLMS_EXT; 
		memmove(aec->x + aec->j + 1, aec->x, (aec->NLMS_LEN - 1) * sizeof(int)); 
		memmove(aec->xf + aec->j + 1, aec->xf, (aec->NLMS_LEN - 1) * sizeof(int)); 
	} 
	 
	// Saturation 
	if (e > MAXPCM)  
	{ 
		return MAXPCM; 
	}  
	else if (e < -MAXPCM)  
	{ 
		return -MAXPCM; 
	}  
	else  
	{ 
		return e; 
	} 
} 
 
#pragma CODE_SECTION(AEC_doAEC, ".AEC:AEC_doAEC") 
int AEC_doAEC(AECSTATDEF *aec, int d_, int x_, int sft) 
{ 
	int d = d_; 
	int x = x_; 
	 
	// Amplify, for e.g. Soundcards with -6dB max. volume 
	d *= aec->gain; 
	 
	// Double Talk Detector 
	aec->stepsize = AEC_dtd(aec, d, x); 
	 
	// Leaky (ageing of vector w) 
	AEC_leaky(aec); 
	 
	// Acoustic Echo Cancellation 
	d = AEC_nlms_pw(aec, d, x, aec->stepsize); 
	 
	return (int) d; 
} 
 
int AEC_getambient(AECSTATDEF *aec)  
{ 
	return aec->dfast  >> 16; 
} 
 
int AEC_getstepsize(AECSTATDEF *aec)  
{ 
	return aec->stepsize; 
} 
 
void AEC_setambient(AECSTATDEF *aec, int Min_xf)  
{ 
	int temp; 
	aec->dotp_xf_xf = aec->dotp_xf_xf - aec->delta;  // subtract old delta 
	temp = Min_xf * Min_xf; 
	aec->delta = (aec->NLMS_LEN-1) * temp; 
	aec->dotp_xf_xf = aec->dotp_xf_xf + aec->delta;  // add new delta 
} 
 
void AEC_setgain(AECSTATDEF *aec, int gain_)  
{ 
	aec->gain = gain_; 
} 
 
void AEC_setSamplFreqType(AECSTATDEF *aec, int sft) 
{ 
	aec->samplFreqType = sft; 
}