Загрузка данных
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];*/
}
}
/* Compute spectrum of estimated echo for use in an echo post-filter */
void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *residual_echo, int len)
{
int i;
spx_word16_t leak2;
int N;
N = st->window_size;
/* Apply hanning window (should pre-compute it)*/
for (i=0;i<N;i++)
st->y[i] = MULT16_16_Q15(st->window[i],st->last_y[i]);
/* Compute power spectrum of the echo */
spx_fft(st->fft_table, st->y, st->Y);
power_spectrum(st->Y, residual_echo, N);
#ifdef FIXED_POINT
if (st->leak_estimate > 16383)
leak2 = 32767;
else
leak2 = SHL16(st->leak_estimate, 1);
#else
if (st->leak_estimate>.5)
leak2 = 1;
else
leak2 = 2*st->leak_estimate;
#endif
/* Estimate residual echo */
for (i=0;i<=st->frame_size;i++)
residual_echo[i] = (spx_int32_t)MULT16_32_Q15(leak2,residual_echo[i]);
}
EXPORT int speex_echo_ctl(SpeexEchoState *st, int request, void *ptr)
{
switch(request)
{
case SPEEX_ECHO_GET_FRAME_SIZE:
(*(int*)ptr) = st->frame_size;
break;
case SPEEX_ECHO_SET_SAMPLING_RATE:
st->sampling_rate = (*(int*)ptr);
st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
#ifdef FIXED_POINT
st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
#else
st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
st->beta_max = (.5f*st->frame_size)/st->sampling_rate;
#endif
if (st->sampling_rate<12000)
st->notch_radius = QCONST16(.9, 15);
else if (st->sampling_rate<24000)
st->notch_radius = QCONST16(.982, 15);
else
st->notch_radius = QCONST16(.992, 15);
break;
case SPEEX_ECHO_GET_SAMPLING_RATE:
(*(int*)ptr) = st->sampling_rate;
break;
case SPEEX_ECHO_GET_IMPULSE_RESPONSE_SIZE:
/*FIXME: Implement this for multiple channels */
*((spx_int32_t *)ptr) = st->M * st->frame_size;
break;
case SPEEX_ECHO_GET_IMPULSE_RESPONSE:
{
int M = st->M, N = st->window_size, n = st->frame_size, i, j;
spx_int32_t *filt = (spx_int32_t *) ptr;
for(j=0;j<M;j++)
{
/*FIXME: Implement this for multiple channels */
#ifdef FIXED_POINT
for (i=0;i<N;i++)
st->wtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],16+NORMALIZE_SCALEDOWN));
spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
#else
spx_ifft(st->fft_table, &st->W[j*N], st->wtmp);
#endif
for(i=0;i<n;i++)
filt[j*n+i] = PSHR32(MULT16_16(32767,st->wtmp[i]), WEIGHT_SHIFT-NORMALIZE_SCALEDOWN);
}
}
break;
default:
speex_warning_int("Unknown speex_echo_ctl request: ", request);
return -1;
}
return 0;
}