/* * Reset the echo canceller. */ void pjs_echo_canceller::reset() { PPJ_WaitAndLock wl(*lock); while (!pj_list_empty(&lat_buf)) { struct frame *frm; frm = lat_buf.next; pj_list_erase(frm); pj_list_push_back(&lat_free, frm); } lat_ready = PJ_FALSE; speex_echo_state_reset(state); pjmedia_delay_buf_reset(delay_buf); }
t_audio_rx::~t_audio_rx() { struct timespec sleeptimer; if (is_running) { stop_running = true; do { sleeptimer.tv_sec = 0; sleeptimer.tv_nsec = 10000000; nanosleep(&sleeptimer, NULL); } while (is_running); } #ifdef HAVE_SPEEX // cleaning speex preprocessing if (audio_session->get_do_echo_cancellation()) { speex_echo_state_reset(audio_session->get_speex_echo_state()); } speex_preprocess_state_destroy(speex_preprocess_state); #endif MEMMAN_DELETE_ARRAY(input_sample_buf); delete [] input_sample_buf; MEMMAN_DELETE_ARRAY(payload); delete [] payload; MEMMAN_DELETE(audio_encoder); delete audio_encoder; // Clean up resources for 3-way conference calls if (media_3way_peer_tx) { MEMMAN_DELETE(media_3way_peer_tx); delete media_3way_peer_tx; } if (media_3way_peer_rx) { MEMMAN_DELETE(media_3way_peer_rx); delete media_3way_peer_rx; } if (mix_buf_3way) { MEMMAN_DELETE_ARRAY(mix_buf_3way); delete [] mix_buf_3way; } if (dtmf_player) { MEMMAN_DELETE(dtmf_player); delete dtmf_player; } }
/** Performs echo cancellation on a frame */ void speex_echo_cancel(SpeexEchoState *st, short *ref, short *echo, short *out, float *Yout) { int i,j,m; int N,M; float scale; float ESR; float SER; float Sry=0,Srr=0,Syy=0,Sey=0,See=0,Sxx=0; float leak_estimate; leak_estimate = .1+(.9/(1+2*st->sum_adapt)); N = st->window_size; M = st->M; scale = 1.0f/N; st->cancel_count++; /* Copy input data to buffer */ for (i=0; i<st->frame_size; i++) { st->x[i] = st->x[i+st->frame_size]; st->x[i+st->frame_size] = echo[i]; st->d[i] = st->d[i+st->frame_size]; st->d[i+st->frame_size] = ref[i]; } /* Shift memory: this could be optimized eventually*/ for (i=0; i<N*(M-1); i++) st->X[i]=st->X[i+N]; /* Copy new echo frame */ for (i=0; i<N; i++) st->X[(M-1)*N+i]=st->x[i]; /* Convert x (echo input) to frequency domain */ spx_drft_forward(st->fft_lookup, &st->X[(M-1)*N]); /* Compute filter response Y */ for (i=0; i<N; i++) st->Y[i] = 0; for (j=0; j<M; j++) spectral_mul_accum(&st->X[j*N], &st->W[j*N], st->Y, N); /* Convert Y (filter response) to time domain */ for (i=0; i<N; i++) st->y[i] = st->Y[i]; spx_drft_backward(st->fft_lookup, st->y); for (i=0; i<N; i++) st->y[i] *= scale; /* Transform d (reference signal) to frequency domain */ for (i=0; i<N; i++) st->D[i]=st->d[i]; spx_drft_forward(st->fft_lookup, st->D); /* Compute error signal (signal with echo removed) */ for (i=0; i<st->frame_size; i++) { float tmp_out; tmp_out = (float)ref[i] - st->y[i+st->frame_size]; st->E[i] = 0; st->E[i+st->frame_size] = tmp_out; /* Saturation */ if (tmp_out>32767) tmp_out = 32767; else if (tmp_out<-32768) tmp_out = -32768; out[i] = tmp_out; } /* This bit of code provides faster adaptation by doing a projection of the previous gradient on the "MMSE surface" */ if (1) { float Sge, Sgg, Syy; float gain; Syy = inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size); for (i=0; i<N; i++) st->Y2[i] = 0; for (j=0; j<M; j++) spectral_mul_accum(&st->X[j*N], &st->PHI[j*N], st->Y2, N); for (i=0; i<N; i++) st->y2[i] = st->Y2[i]; spx_drft_backward(st->fft_lookup, st->y2); for (i=0; i<N; i++) st->y2[i] *= scale; Sge = inner_prod(st->y2+st->frame_size, st->E+st->frame_size, st->frame_size); Sgg = inner_prod(st->y2+st->frame_size, st->y2+st->frame_size, st->frame_size); /* Compute projection gain */ gain = Sge/(N+.03*Syy+Sgg); if (gain>2) gain = 2; if (gain < -2) gain = -2; /* Apply gain to weights, echo estimates, output */ for (i=0; i<N; i++) st->Y[i] += gain*st->Y2[i]; for (i=0; i<st->frame_size; i++) { st->y[i+st->frame_size] += gain*st->y2[i+st->frame_size]; st->E[i+st->frame_size] -= gain*st->y2[i+st->frame_size]; } for (i=0; i<M*N; i++) st->W[i] += gain*st->PHI[i]; } /* Compute power spectrum of output (D-Y) and filter response (Y) */ for (i=0; i<N; i++) st->D[i] -= st->Y[i]; power_spectrum(st->D, st->Rf, N); power_spectrum(st->Y, st->Yf, N); /* Compute frequency-domain adaptation mask */ for (j=0; j<=st->frame_size; j++) { float r; r = leak_estimate*st->Yf[j] / (1+st->Rf[j]); if (r>1) r = 1; st->fratio[j] = r; } /* Compute a bunch of correlations */ Sry = inner_prod(st->y+st->frame_size, st->d+st->frame_size, st->frame_size); Sey = inner_prod(st->y+st->frame_size, st->E+st->frame_size, st->frame_size); See = inner_prod(st->E+st->frame_size, st->E+st->frame_size, st->frame_size); Syy = inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size); Srr = inner_prod(st->d+st->frame_size, st->d+st->frame_size, st->frame_size); Sxx = inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size); /* Compute smoothed cross-correlation and energy */ st->Sey = .98*st->Sey + .02*Sey; st->Syy = .98*st->Syy + .02*Syy; st->See = .98*st->See + .02*See; /* Check if filter is completely mis-adapted (if so, reset filter) */ if (st->Sey/(1+st->Syy + .01*st->See) < -1) { /*fprintf (stderr, "reset at %d\n", st->cancel_count);*/ speex_echo_state_reset(st); return; } SER = Srr / (1+Sxx); ESR = leak_estimate*Syy / (1+See); if (ESR>1) ESR = 1; #if 1 /* If over-cancellation (creating echo with 180 phase) damp filter */ if (st->Sey/(1+st->Syy) < -.1 && (ESR > .3)) { for (i=0; i<M*N; i++) st->W[i] *= .95; st->Sey *= .5; /*fprintf (stderr, "corrected down\n");*/ } #endif #if 1 /* If under-cancellation (leaving echo with 0 phase) scale filter up */ if (st->Sey/(1+st->Syy) > .1 && (ESR > .1 || SER < 10)) { for (i=0; i<M*N; i++) st->W[i] *= 1.05; st->Sey *= .5; /*fprintf (stderr, "corrected up %d\n", st->cancel_count);*/ } #endif /* We consider that the filter is adapted if the following is true*/ if (ESR>.6 && st->sum_adapt > 1) { /*if (!st->adapted) fprintf(stderr, "Adapted at %d %f\n", st->cancel_count, st->sum_adapt);*/ st->adapted = 1; } /* Update frequency-dependent energy ratio with the total energy ratio */ for (i=0; i<=st->frame_size; i++) { st->fratio[i] = (.2*ESR+.8*min(.005+ESR,st->fratio[i])); } if (st->adapted) { st->adapt_rate = .95f/(2+M); } else { /* Temporary adaption rate if filter is not adapted correctly */ if (SER<.1) st->adapt_rate =.8/(2+M); else if (SER<1) st->adapt_rate =.4/(2+M); else if (SER<10) st->adapt_rate =.2/(2+M); else if (SER<30) st->adapt_rate =.08/(2+M); else st->adapt_rate = 0; } /* How much have we adapted so far? */ st->sum_adapt += st->adapt_rate; /* Compute echo power in each frequency bin */ { float ss = 1.0f/st->cancel_count; if (ss < .3/M) ss=.3/M; power_spectrum(&st->X[(M-1)*N], st->Xf, N); /* Smooth echo energy estimate over time */ for (j=0; j<=st->frame_size; j++) st->power[j] = (1-ss)*st->power[j] + ss*st->Xf[j]; /* Combine adaptation rate to the the inverse energy estimate */ if (st->adapted) { /* If filter is adapted, include the frequency-dependent ratio too */ for (i=0; i<=st->frame_size; i++) st->power_1[i] = st->adapt_rate*st->fratio[i] /(1.f+st->power[i]); } else { for (i=0; i<=st->frame_size; i++) st->power_1[i] = st->adapt_rate/(1.f+st->power[i]); } } /* Convert error to frequency domain */ spx_drft_forward(st->fft_lookup, st->E); /* Do some regularization (prevents problems when system is ill-conditoned) */ for (m=0; m<M; m++) for (i=0; i<N; i++) st->W[m*N+i] *= 1-st->regul[i]*ESR; /* Compute weight gradient */ for (j=0; j<M; j++) { weighted_spectral_mul_conj(st->power_1, &st->X[j*N], st->E, st->PHI+N*j, N); } /* Gradient descent */ for (i=0; i<M*N; i++) st->W[i] += st->PHI[i]; /* AUMDF weight constraint */ for (j=0; j<M; j++) { /* Remove the "if" to make this an MDF filter */ if (st->cancel_count%M == j) { spx_drft_backward(st->fft_lookup, &st->W[j*N]); for (i=0; i<N; i++) st->W[j*N+i]*=scale; for (i=st->frame_size; i<N; i++) { st->W[j*N+i]=0; } spx_drft_forward(st->fft_lookup, &st->W[j*N]); } } /* Compute spectrum of estimated echo for use in an echo post-filter (if necessary)*/ if (Yout) { if (st->adapted) { /* If the filter is adapted, take the filtered echo */ for (i=0; i<st->frame_size; i++) st->last_y[i] = st->last_y[st->frame_size+i]; for (i=0; i<st->frame_size; i++) st->last_y[st->frame_size+i] = st->y[st->frame_size+i]; } else { /* If filter isn't adapted yet, all we can do is take the echo signal directly */ for (i=0; i<N; i++) st->last_y[i] = st->x[i]; } /* Apply hanning window (should pre-compute it)*/ for (i=0; i<N; i++) st->Yps[i] = (.5-.5*cos(2*M_PI*i/N))*st->last_y[i]; /* Compute power spectrum of the echo */ spx_drft_forward(st->fft_lookup, st->Yps); power_spectrum(st->Yps, st->Yps, N); /* Estimate residual echo */ for (i=0; i<=st->frame_size; i++) Yout[i] = 2*leak_estimate*st->Yps[i]; } }
/* * Reset AEC */ PJ_DEF(void) speex_aec_reset(void *state ) { speex_ec *echo = (speex_ec*) state; speex_echo_state_reset(echo->state); }
int main(int argc, char *argv[]) { int sd, rc, n; int i; struct sockaddr_in cliAddr, remoteAddr; char msg[MAX_MSG]; struct hostent *h; int local_port, remote_port; int nfds; struct pollfd *pfds; SpeexPreprocessState *preprocess; AlsaDevice *audio_dev; int tmp; if (argc != 5) { fprintf(stderr, "wrong options\n"); exit(1); } h = gethostbyname(argv[2]); if(h==NULL) { fprintf(stderr, "%s: unknown host '%s' \n", argv[0], argv[1]); exit(1); } local_port = atoi(argv[3]); remote_port = atoi(argv[4]); printf("%s: sending data to '%s' (IP : %s) \n", argv[0], h->h_name, inet_ntoa(*(struct in_addr *)h->h_addr_list[0])); { remoteAddr.sin_family = h->h_addrtype; memcpy((char *) &remoteAddr.sin_addr.s_addr, h->h_addr_list[0], h->h_length); remoteAddr.sin_port = htons(remote_port); } /* socket creation */ sd=socket(AF_INET, SOCK_DGRAM, 0); if(sd<0) { printf("%s: cannot open socket \n",argv[0]); exit(1); } /* bind any port */ cliAddr.sin_family = AF_INET; cliAddr.sin_addr.s_addr = htonl(INADDR_ANY); cliAddr.sin_port = htons(local_port); rc = bind(sd, (struct sockaddr *) &cliAddr, sizeof(cliAddr)); if(rc<0) { printf("%s: cannot bind port\n", argv[0]); exit(1); } /* Setup audio device */ audio_dev = alsa_device_open(argv[1], SAMPLING_RATE, 1, FRAME_SIZE); /* Setup the encoder and decoder in wideband */ void *enc_state, *dec_state; enc_state = speex_encoder_init(&speex_wb_mode); tmp = 8; speex_encoder_ctl(enc_state, SPEEX_SET_QUALITY, &tmp); tmp = 2; speex_encoder_ctl(enc_state, SPEEX_SET_COMPLEXITY, &tmp); dec_state = speex_decoder_init(&speex_wb_mode); tmp = 1; speex_decoder_ctl(dec_state, SPEEX_SET_ENH, &tmp); SpeexBits enc_bits, dec_bits; speex_bits_init(&enc_bits); speex_bits_init(&dec_bits); struct sched_param param; /*param.sched_priority = 40; */ param.sched_priority = sched_get_priority_min(SCHED_FIFO); if (sched_setscheduler(0,SCHED_FIFO,¶m)) perror("sched_setscheduler"); int send_timestamp = 0; int recv_started=0; /* Setup all file descriptors for poll()ing */ nfds = alsa_device_nfds(audio_dev); pfds = malloc(sizeof(*pfds)*(nfds+1)); alsa_device_getfds(audio_dev, pfds, nfds); pfds[nfds].fd = sd; pfds[nfds].events = POLLIN; /* Setup jitter buffer using decoder */ SpeexJitter jitter; speex_jitter_init(&jitter, dec_state, SAMPLING_RATE); /* Echo canceller with 200 ms tail length */ SpeexEchoState *echo_state = speex_echo_state_init(FRAME_SIZE, 10*FRAME_SIZE); tmp = SAMPLING_RATE; speex_echo_ctl(echo_state, SPEEX_ECHO_SET_SAMPLING_RATE, &tmp); /* Setup preprocessor and associate with echo canceller for residual echo suppression */ preprocess = speex_preprocess_state_init(FRAME_SIZE, SAMPLING_RATE); speex_preprocess_ctl(preprocess, SPEEX_PREPROCESS_SET_ECHO_STATE, echo_state); alsa_device_start(audio_dev); /* Infinite loop on capture, playback and receiving packets */ while (1) { /* Wait for either 1) capture 2) playback 3) socket data */ poll(pfds, nfds+1, -1); /* Received packets */ if (pfds[nfds].revents & POLLIN) { /*fprintf (stderr, "x");*/ n = recv(sd, msg, MAX_MSG, 0); int recv_timestamp = ((int*)msg)[1]; int payload = ((int*)msg)[0]; if ((payload & 0x80000000) == 0) { /* Put content of the packet into the jitter buffer, except for the pseudo-header */ speex_jitter_put(&jitter, msg+8, n-8, recv_timestamp); recv_started = 1; } } /* Ready to play a frame (playback) */ if (alsa_device_playback_ready(audio_dev, pfds, nfds)) { short pcm[FRAME_SIZE]; if (recv_started) { /* Get audio from the jitter buffer */ speex_jitter_get(&jitter, pcm, NULL); } else { for (i=0; i<FRAME_SIZE; i++) pcm[i] = 0; } /* Playback the audio and reset the echo canceller if we got an underrun */ if (alsa_device_write(audio_dev, pcm, FRAME_SIZE)) speex_echo_state_reset(echo_state); /* Put frame into playback buffer */ speex_echo_playback(echo_state, pcm); } /* Audio available from the soundcard (capture) */ if (alsa_device_capture_ready(audio_dev, pfds, nfds)) { short pcm[FRAME_SIZE], pcm2[FRAME_SIZE]; char outpacket[MAX_MSG]; /* Get audio from the soundcard */ alsa_device_read(audio_dev, pcm, FRAME_SIZE); /* Perform echo cancellation */ speex_echo_capture(echo_state, pcm, pcm2); for (i=0; i<FRAME_SIZE; i++) pcm[i] = pcm2[i]; speex_bits_reset(&enc_bits); /* Apply noise/echo suppression */ speex_preprocess_run(preprocess, pcm); /* Encode */ speex_encode_int(enc_state, pcm, &enc_bits); int packetSize = speex_bits_write(&enc_bits, outpacket+8, MAX_MSG); /* Pseudo header: four null bytes and a 32-bit timestamp */ ((int*)outpacket)[0] = htonl(0); ((int*)outpacket)[1] = send_timestamp; send_timestamp += FRAME_SIZE; rc = sendto(sd, outpacket, packetSize+8, 0, (struct sockaddr *) &remoteAddr, sizeof(remoteAddr)); if(rc<0) { printf("cannot send audio data\n"); close(sd); exit(1); } } } return 0; }
/** Performs echo cancellation on a frame */ EXPORT void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out) { int i,j, chan, speak; int N,M, C, K; spx_word32_t Syy,See,Sxx,Sdd, Sff; #ifdef TWO_PATH spx_word32_t Dbf; int update_foreground; #endif spx_word32_t Sey; spx_word16_t ss, ss_1; spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE; spx_float_t alpha, alpha_1; spx_word16_t RER; spx_word32_t tmp32; N = st->window_size; M = st->M; C = st->C; K = st->K; st->cancel_count++; #ifdef FIXED_POINT ss=DIV32_16(11469,M); ss_1 = SUB16(32767,ss); #else ss=.35/M; ss_1 = 1-ss; #endif for (chan = 0; chan < C; chan++) { /* Apply a notch filter to make sure DC doesn't end up causing problems */ filter_dc_notch16(in+chan, st->notch_radius, st->input+chan*st->frame_size, st->frame_size, st->notch_mem+2*chan, C); /* Copy input data to buffer and apply pre-emphasis */ /* Copy input data to buffer */ for (i=0;i<st->frame_size;i++) { spx_word32_t tmp32; /* FIXME: This core has changed a bit, need to merge properly */ tmp32 = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD[chan]))); #ifdef FIXED_POINT if (tmp32 > 32767) { tmp32 = 32767; if (st->saturated == 0) st->saturated = 1; } if (tmp32 < -32767) { tmp32 = -32767; if (st->saturated == 0) st->saturated = 1; } #endif st->memD[chan] = st->input[chan*st->frame_size+i]; st->input[chan*st->frame_size+i] = EXTRACT16(tmp32); } } for (speak = 0; speak < K; speak++) { for (i=0;i<st->frame_size;i++) { spx_word32_t tmp32; st->x[speak*N+i] = st->x[speak*N+i+st->frame_size]; tmp32 = SUB32(EXTEND32(far_end[i*K+speak]), EXTEND32(MULT16_16_P15(st->preemph, st->memX[speak]))); #ifdef FIXED_POINT /*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */ if (tmp32 > 32767) { tmp32 = 32767; st->saturated = M+1; } if (tmp32 < -32767) { tmp32 = -32767; st->saturated = M+1; } #endif st->x[speak*N+i+st->frame_size] = EXTRACT16(tmp32); st->memX[speak] = far_end[i*K+speak]; } } for (speak = 0; speak < K; speak++) { /* Shift memory: this could be optimized eventually*/ for (j=M-1;j>=0;j--) { for (i=0;i<N;i++) st->X[(j+1)*N*K+speak*N+i] = st->X[j*N*K+speak*N+i]; } /* Convert x (echo input) to frequency domain */ spx_fft(st->fft_table, st->x+speak*N, &st->X[speak*N]); } Sxx = 0; for (speak = 0; speak < K; speak++) { Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size); power_spectrum_accum(st->X+speak*N, st->Xf, N); } Sff = 0; for (chan = 0; chan < C; chan++) { #ifdef TWO_PATH /* Compute foreground filter */ spectral_mul_accum16(st->X, st->foreground+chan*N*K*M, st->Y+chan*N, N, M*K); spx_ifft(st->fft_table, st->Y+chan*N, st->e+chan*N); for (i=0;i<st->frame_size;i++) st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->e[chan*N+i+st->frame_size]); Sff += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size); #endif } /* Adjust proportional adaption rate */ /* FIXME: Adjust that for C, K*/ if (st->adapted) mdf_adjust_prop (st->W, N, M, C*K, st->prop); /* Compute weight gradient */ if (st->saturated == 0) { for (chan = 0; chan < C; chan++) { for (speak = 0; speak < K; speak++) { for (j=M-1;j>=0;j--) { weighted_spectral_mul_conj(st->power_1, FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), &st->X[(j+1)*N*K+speak*N], st->E+chan*N, st->PHI, N); for (i=0;i<N;i++) st->W[chan*N*K*M + j*N*K + speak*N + i] += st->PHI[i]; } } } } else { st->saturated--; } /* FIXME: MC conversion required */ /* Update weight to prevent circular convolution (MDF / AUMDF) */ for (chan = 0; chan < C; chan++) { for (speak = 0; speak < K; speak++) { for (j=0;j<M;j++) { /* This is a variant of the Alternatively Updated MDF (AUMDF) */ /* Remove the "if" to make this an MDF filter */ if (j==0 || st->cancel_count%(M-1) == j-1) { #ifdef FIXED_POINT for (i=0;i<N;i++) st->wtmp2[i] = EXTRACT16(PSHR32(st->W[chan*N*K*M + j*N*K + speak*N + i],NORMALIZE_SCALEDOWN+16)); spx_ifft(st->fft_table, st->wtmp2, st->wtmp); for (i=0;i<st->frame_size;i++) { st->wtmp[i]=0; } for (i=st->frame_size;i<N;i++) { st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP); } spx_fft(st->fft_table, st->wtmp, st->wtmp2); /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */ for (i=0;i<N;i++) st->W[chan*N*K*M + j*N*K + speak*N + i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1); #else spx_ifft(st->fft_table, &st->W[chan*N*K*M + j*N*K + speak*N], st->wtmp); for (i=st->frame_size;i<N;i++) { st->wtmp[i]=0; } spx_fft(st->fft_table, st->wtmp, &st->W[chan*N*K*M + j*N*K + speak*N]); #endif } } } } /* So we can use power_spectrum_accum */ for (i=0;i<=st->frame_size;i++) st->Rf[i] = st->Yf[i] = st->Xf[i] = 0; Dbf = 0; See = 0; #ifdef TWO_PATH /* Difference in response, this is used to estimate the variance of our residual power estimate */ for (chan = 0; chan < C; chan++) { spectral_mul_accum(st->X, st->W+chan*N*K*M, st->Y+chan*N, N, M*K); spx_ifft(st->fft_table, st->Y+chan*N, st->y+chan*N); for (i=0;i<st->frame_size;i++) st->e[chan*N+i] = SUB16(st->e[chan*N+i+st->frame_size], st->y[chan*N+i+st->frame_size]); Dbf += 10+mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size); for (i=0;i<st->frame_size;i++) st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]); See += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size); } #endif #ifndef TWO_PATH Sff = See; #endif #ifdef TWO_PATH /* Logic for updating the foreground filter */ /* For two time windows, compute the mean of the energy difference, as well as the variance */ st->Davg1 = ADD32(MULT16_32_Q15(QCONST16(.6f,15),st->Davg1), MULT16_32_Q15(QCONST16(.4f,15),SUB32(Sff,See))); st->Davg2 = ADD32(MULT16_32_Q15(QCONST16(.85f,15),st->Davg2), MULT16_32_Q15(QCONST16(.15f,15),SUB32(Sff,See))); st->Dvar1 = FLOAT_ADD(FLOAT_MULT(VAR1_SMOOTH, st->Dvar1), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.4f,15),Sff), MULT16_32_Q15(QCONST16(.4f,15),Dbf))); st->Dvar2 = FLOAT_ADD(FLOAT_MULT(VAR2_SMOOTH, st->Dvar2), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.15f,15),Sff), MULT16_32_Q15(QCONST16(.15f,15),Dbf))); /* Equivalent float code: st->Davg1 = .6*st->Davg1 + .4*(Sff-See); st->Davg2 = .85*st->Davg2 + .15*(Sff-See); st->Dvar1 = .36*st->Dvar1 + .16*Sff*Dbf; st->Dvar2 = .7225*st->Dvar2 + .0225*Sff*Dbf; */ update_foreground = 0; /* Check if we have a statistically significant reduction in the residual echo */ /* Note that this is *not* Gaussian, so we need to be careful about the longer tail */ if (FLOAT_GT(FLOAT_MUL32U(SUB32(Sff,See),ABS32(SUB32(Sff,See))), FLOAT_MUL32U(Sff,Dbf))) update_foreground = 1; else if (FLOAT_GT(FLOAT_MUL32U(st->Davg1, ABS32(st->Davg1)), FLOAT_MULT(VAR1_UPDATE,(st->Dvar1)))) update_foreground = 1; else if (FLOAT_GT(FLOAT_MUL32U(st->Davg2, ABS32(st->Davg2)), FLOAT_MULT(VAR2_UPDATE,(st->Dvar2)))) update_foreground = 1; /* Do we update? */ if (update_foreground) { st->Davg1 = st->Davg2 = 0; st->Dvar1 = st->Dvar2 = FLOAT_ZERO; /* Copy background filter to foreground filter */ for (i=0;i<N*M*C*K;i++) st->foreground[i] = EXTRACT16(PSHR32(st->W[i],16)); /* Apply a smooth transition so as to not introduce blocking artifacts */ for (chan = 0; chan < C; chan++) for (i=0;i<st->frame_size;i++) st->e[chan*N+i+st->frame_size] = MULT16_16_Q15(st->window[i+st->frame_size],st->e[chan*N+i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[chan*N+i+st->frame_size]); } else { int reset_background=0; /* Otherwise, check if the background filter is significantly worse */ if (FLOAT_GT(FLOAT_MUL32U(NEG32(SUB32(Sff,See)),ABS32(SUB32(Sff,See))), FLOAT_MULT(VAR_BACKTRACK,FLOAT_MUL32U(Sff,Dbf)))) reset_background = 1; if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg1), ABS32(st->Davg1)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar1))) reset_background = 1; if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg2), ABS32(st->Davg2)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar2))) reset_background = 1; if (reset_background) { /* Copy foreground filter to background filter */ for (i=0;i<N*M*C*K;i++) st->W[i] = SHL32(EXTEND32(st->foreground[i]),16); /* We also need to copy the output so as to get correct adaptation */ for (chan = 0; chan < C; chan++) { for (i=0;i<st->frame_size;i++) st->y[chan*N+i+st->frame_size] = st->e[chan*N+i+st->frame_size]; for (i=0;i<st->frame_size;i++) st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]); } See = Sff; st->Davg1 = st->Davg2 = 0; st->Dvar1 = st->Dvar2 = FLOAT_ZERO; } } #endif Sey = Syy = Sdd = 0; for (chan = 0; chan < C; chan++) { /* Compute error signal (for the output with de-emphasis) */ for (i=0;i<st->frame_size;i++) { spx_word32_t tmp_out; #ifdef TWO_PATH tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->e[chan*N+i+st->frame_size])); #else tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->y[chan*N+i+st->frame_size])); #endif tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE[chan]))); /* This is an arbitrary test for saturation in the microphone signal */ if (in[i*C+chan] <= -32000 || in[i*C+chan] >= 32000) { if (st->saturated == 0) st->saturated = 1; } out[i*C+chan] = WORD2INT(tmp_out); st->memE[chan] = tmp_out; } #ifdef DUMP_ECHO_CANCEL_DATA dump_audio(in, far_end, out, st->frame_size); #endif /* Compute error signal (filter update version) */ for (i=0;i<st->frame_size;i++) { st->e[chan*N+i+st->frame_size] = st->e[chan*N+i]; st->e[chan*N+i] = 0; } /* Compute a bunch of correlations */ /* FIXME: bad merge */ Sey += mdf_inner_prod(st->e+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size); Syy += mdf_inner_prod(st->y+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size); Sdd += mdf_inner_prod(st->input+chan*st->frame_size, st->input+chan*st->frame_size, st->frame_size); /* Convert error to frequency domain */ spx_fft(st->fft_table, st->e+chan*N, st->E+chan*N); for (i=0;i<st->frame_size;i++) st->y[i+chan*N] = 0; spx_fft(st->fft_table, st->y+chan*N, st->Y+chan*N); /* Compute power spectrum of echo (X), error (E) and filter response (Y) */ power_spectrum_accum(st->E+chan*N, st->Rf, N); power_spectrum_accum(st->Y+chan*N, st->Yf, N); } /*printf ("%f %f %f %f\n", Sff, See, Syy, Sdd, st->update_cond);*/ /* Do some sanity check */ if (!(Syy>=0 && Sxx>=0 && See >= 0) #ifndef FIXED_POINT || !(Sff < N*1e9 && Syy < N*1e9 && Sxx < N*1e9) #endif ) { /* Things have gone really bad */ st->screwed_up += 50; for (i=0;i<st->frame_size*C;i++) out[i] = 0; } else if (SHR32(Sff, 2) > ADD32(Sdd, SHR32(MULT16_16(N, 10000),6))) { /* AEC seems to add lots of echo instead of removing it, let's see if it will improve */ st->screwed_up++; } else { /* Everything's fine */ st->screwed_up=0; } if (st->screwed_up>=50) { speex_warning("The echo canceller started acting funny and got slapped (reset). It swears it will behave now."); speex_echo_state_reset(st); return; } /* Add a small noise floor to make sure not to have problems when dividing */ See = MAX32(See, SHR32(MULT16_16(N, 100),6)); for (speak = 0; speak < K; speak++) { Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size); power_spectrum_accum(st->X+speak*N, st->Xf, N); } /* Smooth far end energy estimate over time */ for (j=0;j<=st->frame_size;j++) st->power[j] = MULT16_32_Q15(ss_1,st->power[j]) + 1 + MULT16_32_Q15(ss,st->Xf[j]); /* Compute filtered spectra and (cross-)correlations */ for (j=st->frame_size;j>=0;j--) { spx_float_t Eh, Yh; Eh = PSEUDOFLOAT(st->Rf[j] - st->Eh[j]); Yh = PSEUDOFLOAT(st->Yf[j] - st->Yh[j]); Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh)); Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh)); #ifdef FIXED_POINT st->Eh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Eh[j]), st->spec_average, st->Rf[j]); st->Yh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Yh[j]), st->spec_average, st->Yf[j]); #else st->Eh[j] = (1-st->spec_average)*st->Eh[j] + st->spec_average*st->Rf[j]; st->Yh[j] = (1-st->spec_average)*st->Yh[j] + st->spec_average*st->Yf[j]; #endif } Pyy = FLOAT_SQRT(Pyy); Pey = FLOAT_DIVU(Pey,Pyy); /* Compute correlation updatete rate */ tmp32 = MULT16_32_Q15(st->beta0,Syy); if (tmp32 > MULT16_32_Q15(st->beta_max,See)) tmp32 = MULT16_32_Q15(st->beta_max,See); alpha = FLOAT_DIV32(tmp32, See); alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha); /* Update correlations (recursive average) */ st->Pey = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pey) , FLOAT_MULT(alpha,Pey)); st->Pyy = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pyy) , FLOAT_MULT(alpha,Pyy)); if (FLOAT_LT(st->Pyy, FLOAT_ONE)) st->Pyy = FLOAT_ONE; /* We don't really hope to get better than 33 dB (MIN_LEAK-3dB) attenuation anyway */ if (FLOAT_LT(st->Pey, FLOAT_MULT(MIN_LEAK,st->Pyy))) st->Pey = FLOAT_MULT(MIN_LEAK,st->Pyy); if (FLOAT_GT(st->Pey, st->Pyy)) st->Pey = st->Pyy; /* leak_estimate is the linear regression result */ st->leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14)); /* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */ if (st->leak_estimate > 16383) st->leak_estimate = 32767; else st->leak_estimate = SHL16(st->leak_estimate,1); /*printf ("%f\n", st->leak_estimate);*/ /* Compute Residual to Error Ratio */ #ifdef FIXED_POINT tmp32 = MULT16_32_Q15(st->leak_estimate,Syy); tmp32 = ADD32(SHR32(Sxx,13), ADD32(tmp32, SHL32(tmp32,1))); /* Check for y in e (lower bound on RER) */ { spx_float_t bound = PSEUDOFLOAT(Sey); bound = FLOAT_DIVU(FLOAT_MULT(bound, bound), PSEUDOFLOAT(ADD32(1,Syy))); if (FLOAT_GT(bound, PSEUDOFLOAT(See))) tmp32 = See; else if (tmp32 < FLOAT_EXTRACT32(bound)) tmp32 = FLOAT_EXTRACT32(bound); } if (tmp32 > SHR32(See,1)) tmp32 = SHR32(See,1); RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15)); #else RER = (.0001*Sxx + 3.*MULT16_32_Q15(st->leak_estimate,Syy)) / See; /* Check for y in e (lower bound on RER) */ if (RER < Sey*Sey/(1+See*Syy)) RER = Sey*Sey/(1+See*Syy); if (RER > .5) RER = .5; #endif /* We consider that the filter has had minimal adaptation if the following is true*/ if (!st->adapted && st->sum_adapt > SHL32(EXTEND32(M),15) && MULT16_32_Q15(st->leak_estimate,Syy) > MULT16_32_Q15(QCONST16(.03f,15),Syy)) { st->adapted = 1; } if (st->adapted) { /* Normal learning rate calculation once we're past the minimal adaptation phase */ for (i=0;i<=st->frame_size;i++) { spx_word32_t r, e; /* Compute frequency-domain adaptation mask */ r = MULT16_32_Q15(st->leak_estimate,SHL32(st->Yf[i],3)); e = SHL32(st->Rf[i],3)+1; #ifdef FIXED_POINT if (r>SHR32(e,1)) r = SHR32(e,1); #else if (r>.5*e) r = .5*e; #endif r = MULT16_32_Q15(QCONST16(.7,15),r) + MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e))); /*st->power_1[i] = adapt_rate*r/(e*(1+st->power[i]));*/ st->power_1[i] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,st->power[i]+10)),WEIGHT_SHIFT+16); } } else { /* Temporary adaption rate if filter is not yet adapted enough */ spx_word16_t adapt_rate=0; if (Sxx > SHR32(MULT16_16(N, 1000),6)) { tmp32 = MULT16_32_Q15(QCONST16(.25f, 15), Sxx); #ifdef FIXED_POINT if (tmp32 > SHR32(See,2)) tmp32 = SHR32(See,2); #else if (tmp32 > .25*See) tmp32 = .25*See; #endif adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32, See),15)); } for (i=0;i<=st->frame_size;i++) st->power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(st->power[i],10)),WEIGHT_SHIFT+1); /* How much have we adapted so far? */ st->sum_adapt = ADD32(st->sum_adapt,adapt_rate); } /* FIXME: MC conversion required */ for (i=0;i<st->frame_size;i++) st->last_y[i] = st->last_y[st->frame_size+i]; if (st->adapted) { /* If the filter is adapted, take the filtered echo */ for (i=0;i<st->frame_size;i++) st->last_y[st->frame_size+i] = in[i]-out[i]; } else { /* If filter isn't adapted yet, all we can do is take the far end signal directly */ /* moved earlier: for (i=0;i<N;i++) st->last_y[i] = st->x[i];*/ } }
/** * * @param aqi * @param sampleRate * @param sampleSizeInBits * @param channels * @param buffer * @param length the length of <tt>buffer</tt> in bytes */ static void AudioQualityImprovement_resampleInPlay (AudioQualityImprovement *aqi, double sampleRate, unsigned long sampleSizeInBits, int channels, void *buffer, unsigned long length) { spx_uint32_t playSize; spx_uint32_t playCapacity; spx_uint32_t playLength;; spx_int16_t *play; if (sampleRate == aqi->sampleRate) playSize = length; else if (length * aqi->sampleRate == aqi->frameSize * sampleRate) { if (aqi->resampler) { speex_resampler_set_rate( aqi->resampler, (spx_uint32_t) sampleRate, (spx_uint32_t) (aqi->sampleRate)); playSize = aqi->frameSize; } else { aqi->resampler = speex_resampler_init( channels, (spx_uint32_t) sampleRate, (spx_uint32_t) (aqi->sampleRate), SPEEX_RESAMPLER_QUALITY_VOIP, NULL); if (aqi->resampler) playSize = aqi->frameSize; else { aqi->playIsDelaying = JNI_TRUE; aqi->playLength = 0; return; } } } else { /* * The specified buffer neither is in the format of the audio capture * nor can be resampled to it. */ aqi->playIsDelaying = JNI_TRUE; aqi->playLength = 0; return; } /* Ensure that play exists and is large enough. */ playCapacity = ((1 + aqi->playDelay) + 1) * (aqi->frameSize / sizeof(spx_int16_t)); playLength = playSize / sizeof(spx_int16_t); if (playCapacity < playLength) playCapacity = playLength; if (!(aqi->play) || (aqi->playCapacity < playCapacity)) { spx_int16_t *newPlay; newPlay = realloc(aqi->play, playCapacity * sizeof(spx_int16_t)); if (newPlay) { if (!(aqi->play)) { aqi->playIsDelaying = JNI_TRUE; aqi->playLength = 0; } aqi->play = newPlay; aqi->playCapacity = playCapacity; } else { aqi->playIsDelaying = JNI_TRUE; aqi->playLength = 0; return; } } /* Ensure that there is room for buffer in play. */ if (aqi->playLength + playLength > aqi->playCapacity) { aqi->playIsDelaying = JNI_TRUE; aqi->playLength = 0; /* * We don't have enough room in play for buffer which means that we'll * have to throw some samples away. But it'll effectively mean that * we'll enlarge the drift which will disrupt the echo cancellation. So * it seems the least of two evils to just reset the echo cancellation. */ speex_echo_state_reset(aqi->echo); } /* Place buffer in play. */ play = aqi->play + aqi->playLength; if (length == aqi->frameSize) memcpy(play, buffer, playSize); else { unsigned long sampleSizeInBytes = sampleSizeInBits / 8; spx_uint32_t bufferSampleCount = length / sampleSizeInBytes; speex_resampler_process_interleaved_int( aqi->resampler, buffer, &bufferSampleCount, play, &playLength); } aqi->playLength += playLength; /* Take into account the latency. */ if (aqi->playIsDelaying == JNI_TRUE) AudioQualityImprovement_updatePlayIsDelaying(aqi); }