数字语音的AEC源代码

#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") float dotp(float a[], float b[], int len ) { float sum = 0.0; float temp; int j; for (j = 0; j < len; j ++) { temp = mpysp(a[j], b[j]); sum = addsp(temp, sum);//a[j] * b[j]; } return sum; } 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.0f; AEC_setambient( aec, NoiseFloor); aec->dfast = aec->dslow = M75dB_PCM; aec->xfast = aec->xslow = M80dB_PCM; aec->gain = 1.0f; } // 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") float AEC_dtd(AECSTATDEF *aec, float d, float x) { float stepsize; float ratio; float M; float t1,t2,t3,t4,t5; // fast near-end and far-end average aec->dfast += mpysp(ALPHAFAST , (subsp(fabs(d) , aec->dfast))); aec->xfast += mpysp(ALPHAFAST , (subsp(fabs(x) , aec->xfast))); // slow near-end and far-end average aec->dslow += mpysp(ALPHASLOW , (subsp(fabs(d) , aec->dslow))); aec->xslow += mpysp(ALPHASLOW , (subsp(fabs(x) , aec->xslow))); if (aec->xfast < M70dB_PCM) { return 0.0; // no Spk signal } if (aec->dfast < M70dB_PCM) { return 0.0; // no Mic signal } // ratio of NFRs t1= mpysp(aec->dfast , aec->xslow); t2 = mpysp(aec->dslow , aec->xfast); ratio = divsp(t1, t2); //ratio = (aec->dfast * aec->xslow) / (aec->dslow * aec->xfast); // begrenzte lineare Kennlinie M = divsp(subsp(STEPY2 , STEPY1) , subsp(STEPX2 , STEPX1)); if (ratio < STEPX1) { stepsize = STEPY1; } else if (ratio > STEPX2) { stepsize = STEPY2; } else { // Punktrichtungsform einer Geraden stepsize = addsp(mpysp(M , subsp(ratio , STEPX1)) , STEPY1); } 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) { // 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") float AEC_nlms_pw(AECSTATDEF *aec, float d, float x_, float stepsize) { float e; float ef; float t1,t2,t3; float 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 = mpysp(aec->xf[aec->j] , aec->xf[aec->j]); t2 = mpysp(aec->xf[aec->j + aec->NLMS_LEN - 1] , aec->xf[aec->j + aec->NLMS_LEN - 1]); t3 = subsp(t1, t2); aec->dotp_xf_xf = addsp(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.0) { // calculate variable step size float temp; int i; mikro_ef = (divsp(mpysp(stepsize , ef) , aec->dotp_xf_xf))*2; //加快处理速度 // update tap weights (filter learning) for (i = 0; i < aec->NLMS_LEN; i ++) { temp = mpysp(mikro_ef , aec->xf[i + aec->j]); aec->w[i] = addsp(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(float)); memmove(aec->xf + aec->j + 1, aec->xf, (aec->NLMS_LEN - 1) * sizeof(float)); } // 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) { float d = (float) d_; float x = (float) 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; } float AEC_getambient(AECSTATDEF *aec) { return aec->dfast; } float AEC_getstepsize(AECSTATDEF *aec) { return aec->stepsize; } void AEC_setambient(AECSTATDEF *aec, float Min_xf) { float temp; aec->dotp_xf_xf = subsp(aec->dotp_xf_xf, aec->delta); // subtract old delta temp = mpysp(Min_xf , Min_xf); aec->delta = (aec->NLMS_LEN-1) * temp; aec->dotp_xf_xf = addsp(aec->dotp_xf_xf, aec->delta); // add new delta } void AEC_setgain(AECSTATDEF *aec, float gain_) { aec->gain = gain_; } void AEC_setSamplFreqType(AECSTATDEF *aec, int sft) { aec->samplFreqType = sft; }