/*
* FFT of the length 15 * (2^N)
*/
static void fft_calc(CeltIMDCTContext *s, FFTComplex *out, const FFTComplex *in,
int N, ptrdiff_t stride)
{
if (N) {
const FFTComplex *exptab = s->exptab[N];
const int len2 = 15 * (1 << (N - 1));
int k;
fft_calc(s, out, in, N - 1, stride * 2);
fft_calc(s, out + len2, in + stride, N - 1, stride * 2);
for (k = 0; k < len2; k++) {
FFTComplex t;
CMUL(t, out[len2 + k], exptab[k]);
out[len2 + k].re = out[k].re - t.re;
out[len2 + k].im = out[k].im - t.im;
out[k].re += t.re;
out[k].im += t.im;
}
} else
fft15(s, out, in, stride);
}
static void celt_imdct_half(CeltIMDCTContext *s, float *dst, const float *src,
ptrdiff_t stride, float scale)
{
FFTComplex *z = (FFTComplex *)dst;
const int len8 = s->len4 / 2;
const float *in1 = src;
const float *in2 = src + (s->len2 - 1) * stride;
int i;
for (i = 0; i < s->len4; i++) {
FFTComplex tmp = { *in2, *in1 };
CMUL(s->tmp[i], tmp, s->twiddle_exptab[i]);
in1 += 2 * stride;
in2 -= 2 * stride;
}
fft_calc(s, z, s->tmp, s->fft_n, 1);
for (i = 0; i < len8; i++) {
float r0, i0, r1, i1;
CMUL3(r0, i1, z[len8 - i - 1].im, z[len8 - i - 1].re, s->twiddle_exptab[len8 - i - 1].im, s->twiddle_exptab[len8 - i - 1].re);
CMUL3(r1, i0, z[len8 + i].im, z[len8 + i].re, s->twiddle_exptab[len8 + i].im, s->twiddle_exptab[len8 + i].re);
z[len8 - i - 1].re = scale * r0;
z[len8 - i - 1].im = scale * i0;
z[len8 + i].re = scale * r1;
z[len8 + i].im = scale * i1;
}
}
void fft_calc(const int N,const double *x,complex double *X,complex double *P,const int step,const complex double *twids)
{
complex double *S = P + N/2;
if (N == 1){
X[0] = x[0];
return;
}
fft_calc(N/2, x, S, X,2*step, twids);
fft_calc(N/2, x+step, P, X,2*step, twids);
int k;
for (k=0;k<N/2;k++){
P[k] = P[k]*twids[k*step];
X[k] = S[k] + P[k];
X[k+N/2] = S[k] - P[k];
}
}
void fft_calc(const int N, const double *x, Complex *X, Complex *P, const int step, const Complex *twids){
int k;
Complex *S = P + N/2;
if (N == 1){
X[0].re = x[0];
X[0].im = 0;
return;
}
fft_calc(N/2, x, S, X,2*step, twids);
fft_calc(N/2, x+step, P, X,2*step, twids);
for (k=0;k<N/2;k++){
P[k] = mult_complex(P[k],twids[k*step]);
X[k] = add_complex(S[k],P[k]);
X[k+N/2] = sub_complex(S[k],P[k]);
}
}
int fft(const double *x, const int N, Complex *X){
Complex *twiddle_factors = (Complex*)malloc(sizeof(Complex)*(N/2));
Complex *Xt = (Complex*)malloc(sizeof(Complex)*(N));
int k;
for (k=0;k<N/2;k++){
twiddle_factors[k] = polar_to_complex(1.0, 2.0*PI*k/N);
}
fft_calc(N, x, X, Xt, 1, twiddle_factors);
free(twiddle_factors);
free(Xt);
return 0;
}
/**
* Compute inverse MDCT of size N = 2^nbits
* @param output N samples
* @param input N/2 samples
* @param tmp N/2 samples
*/
void ff_imdct_calc(MDCTContext *s, FFTSample *output,
const FFTSample *input, FFTSample *tmp)
{
int k, n8, n4, n2, n, j;
const uint16_t *revtab = s->fft.revtab;
const FFTSample *tcos = s->tcos;
const FFTSample *tsin = s->tsin;
const FFTSample *in1, *in2;
FFTComplex *z = (FFTComplex *)tmp;
n = 1 << s->nbits;
n2 = n >> 1;
n4 = n >> 2;
n8 = n >> 3;
/* pre rotation */
in1 = input;
in2 = input + n2 - 1;
for(k = 0; k < n4; k++) {
j=revtab[k];
CMUL(z[j].re, z[j].im, *in2, *in1, tcos[k], tsin[k]);
in1 += 2;
in2 -= 2;
}
fft_calc(&s->fft, z);
/* post rotation + reordering */
/* XXX: optimize */
for(k = 0; k < n4; k++) {
CMUL(z[k].re, z[k].im, z[k].re, z[k].im, tcos[k], tsin[k]);
}
for(k = 0; k < n8; k++) {
output[2*k] = -z[n8 + k].im;
output[n2-1-2*k] = z[n8 + k].im;
output[2*k+1] = z[n8-1-k].re;
output[n2-1-2*k-1] = -z[n8-1-k].re;
output[n2 + 2*k]=-z[k+n8].re;
output[n-1- 2*k]=-z[k+n8].re;
output[n2 + 2*k+1]=z[n8-k-1].im;
output[n-2 - 2 * k] = z[n8-k-1].im;
}
}
int fft(double *x, int N, complex double *X)
{
complex double *twiddle_factors = (complex double*)malloc(sizeof(complex double)*(N/2));
complex double *Xt = (complex double*)malloc(sizeof(complex double)*N);
int k;
for (k=0;k<N/2;k++){
twiddle_factors[k] = polar_to_complex(1.0, 2.0*PI*k/N);
}
fft_calc(N, x, X, Xt, 1, twiddle_factors);
free(twiddle_factors);
free(Xt);
return 0;
}
/**
* Compute MDCT of size N = 2^nbits
* @param input N samples
* @param out N/2 samples
* @param tmp temporary storage of N/2 samples
*/
void ff_mdct_calc(MDCTContext *s, FFTSample *out,
const FFTSample *input, FFTSample *tmp)
{
int i, j, n, n8, n4, n2, n3;
FFTSample re, im, re1, im1;
const uint16_t *revtab = s->fft.revtab;
const FFTSample *tcos = s->tcos;
const FFTSample *tsin = s->tsin;
FFTComplex *x = (FFTComplex *)tmp;
n = 1 << s->nbits;
n2 = n >> 1;
n4 = n >> 2;
n8 = n >> 3;
n3 = 3 * n4;
/* pre rotation */
for(i=0;i<n8;i++) {
re = -input[2*i+3*n4] - input[n3-1-2*i];
im = -input[n4+2*i] + input[n4-1-2*i];
j = revtab[i];
CMUL(x[j].re, x[j].im, re, im, -tcos[i], tsin[i]);
re = input[2*i] - input[n2-1-2*i];
im = -(input[n2+2*i] + input[n-1-2*i]);
j = revtab[n8 + i];
CMUL(x[j].re, x[j].im, re, im, -tcos[n8 + i], tsin[n8 + i]);
}
fft_calc(&s->fft, x);
/* post rotation */
for(i=0;i<n4;i++) {
re = x[i].re;
im = x[i].im;
CMUL(re1, im1, re, im, -tsin[i], -tcos[i]);
out[2*i] = im1;
out[n2-1-2*i] = re1;
}
}
//--------------------------------------------------------------
// start vom Oszi (Endlosloop)
//--------------------------------------------------------------
void oszi_start(void)
{
MENU_Status_t status;
p_oszi_draw_background();
UB_Graphic2D_Copy2DMA(Menu.akt_transparenz);
while(1) {
//---------------------------------------------
// warten bis GUI-Timer abgelaufen ist
//---------------------------------------------
if(GUI_Timer_ms==0) {
GUI_Timer_ms=GUI_INTERVALL_MS;
//--------------------------------------
// User-Button einlesen (fuer RUN/STOP)
//--------------------------------------
if(UB_Button_OnClick(BTN_USER)==true) {
status=MENU_NO_CHANGE;
if(Menu.trigger.mode==2) { // "single"
if(Menu.trigger.single==4) {
Menu.trigger.single=5; // von "Ready" auf "Stop"
status=MENU_CHANGE_NORMAL;
}
}
else { // "normal" oder "auto"
if(Menu.trigger.single==0) {
Menu.trigger.single=1; // von "Run" auf "Stop"
status=MENU_CHANGE_NORMAL;
}
else if(Menu.trigger.single==1) {
Menu.trigger.single=2; // von "Stop" auf "Weiter"
status=MENU_CHANGE_NORMAL;
}
}
}
else {
//--------------------------------------
// Test ob Touch betaetigt
//--------------------------------------
status=menu_check_touch();
}
if(status!=MENU_NO_CHANGE) {
p_oszi_draw_background();
if(status==MENU_CHANGE_FRQ) ADC_change_Frq(Menu.timebase.value);
if(status==MENU_CHANGE_MODE) {
ADC_change_Mode(Menu.trigger.mode);
if(Menu.trigger.mode!=2) {
Menu.trigger.single=0;
}
else {
Menu.trigger.single=3;
}
p_oszi_draw_background(); // nochmal zeichnen, zum update
ADC_UB.status=ADC_VORLAUF;
TIM_Cmd(TIM2, ENABLE);
}
if(status==MENU_SEND_DATA) {
p_oszi_draw_background(); // nochmal zeichnen, zum update
p_oszi_draw_adc();
// gesendet wird am Ende
}
}
if(Menu.trigger.mode==1) {
//--------------------------------------
// Trigger-Mode = "AUTO"
// Screnn immer neu zeichnen
//--------------------------------------
if(Menu.trigger.single==0) {
if ((ADC1_DMA_STREAM->CR & DMA_SxCR_CT) == 0) {
ADC_UB.trigger_pos=SCALE_X_MITTE;
ADC_UB.trigger_quarter=2;
}
else {
ADC_UB.trigger_pos=SCALE_X_MITTE;
ADC_UB.trigger_quarter=4;
}
p_oszi_sort_adc();
p_oszi_fill_fft();
if(Menu.fft.mode!=0) fft_calc();
p_oszi_draw_adc();
ADC_UB.status=ADC_VORLAUF;
UB_Led_Toggle(LED_RED);
}
else if(Menu.trigger.single==1) {
// Button "STOP" wurde gedrückt
// Timer analten
TIM_Cmd(TIM2, DISABLE);
if(status!=MENU_NO_CHANGE) p_oszi_draw_adc();
}
else if(Menu.trigger.single==2) {
// Button "START" wurde gedrückt
Menu.trigger.single=0;
ADC_UB.status=ADC_VORLAUF;
TIM_Cmd(TIM2, ENABLE);
if(status!=MENU_NO_CHANGE) p_oszi_draw_adc();
}
}
else if(Menu.trigger.mode==0) {
//--------------------------------------
// Trigger-Mode = "NORMAL"
// Screnn nur zeichnen, nach Triggerevent
//--------------------------------------
if(Menu.trigger.single==0) {
if(ADC_UB.status==ADC_READY) {
UB_Led_Toggle(LED_RED);
p_oszi_sort_adc();
p_oszi_fill_fft();
if(Menu.fft.mode!=0) fft_calc();
p_oszi_draw_adc();
ADC_UB.status=ADC_VORLAUF;
TIM_Cmd(TIM2, ENABLE);
}
else {
// oder wenn Menu verändert wurde
if(status!=MENU_NO_CHANGE) p_oszi_draw_adc();
}
}
else if(Menu.trigger.single==1) {
// Button "STOP" wurde gedrückt
// Timer analten
TIM_Cmd(TIM2, DISABLE);
if(status!=MENU_NO_CHANGE) p_oszi_draw_adc();
}
else if(Menu.trigger.single==2) {
// Button "START" wurde gedrückt
Menu.trigger.single=0;
ADC_UB.status=ADC_VORLAUF;
TIM_Cmd(TIM2, ENABLE);
if(status!=MENU_NO_CHANGE) p_oszi_draw_adc();
}
}
else {
//--------------------------------------
// Trigger-Mode = "SINGLE"
// Screnn nur einmal zeichnen, nach Triggerevent
//--------------------------------------
if(Menu.trigger.single==3) {
// warten auf Trigger-Event
if(ADC_UB.status==ADC_READY) {
Menu.trigger.single=4;
UB_Led_Toggle(LED_RED);
p_oszi_sort_adc();
p_oszi_fill_fft();
if(Menu.fft.mode!=0) fft_calc();
p_oszi_draw_adc();
}
else {
// oder wenn Menu verändert wurde
if(status!=MENU_NO_CHANGE) p_oszi_draw_adc();
}
}
else if(Menu.trigger.single==5) {
// Button "Reset" wurde gedrückt
Menu.trigger.single=3;
p_oszi_draw_adc();
ADC_UB.status=ADC_VORLAUF;
TIM_Cmd(TIM2, ENABLE);
}
else {
// oder wenn Menu verändert wurde
if(status!=MENU_NO_CHANGE) p_oszi_draw_adc();
}
}
if(GUI.gui_xpos==GUI_XPOS_OFF) {
// ohne GUI => ohne Transparenz zeichnen
UB_Graphic2D_Copy1DMA();
}
else {
// mit GUI => mit Transparenz zeichnen
UB_Graphic2D_Copy2DMA(Menu.akt_transparenz);
}
// Refreh vom LCD
UB_LCD_Refresh();
// event. Daten senden
if(Menu.send.data!=0) {
p_oszi_send_data();
Menu.send.data=0;
}
}
}
}
double Equalizer::filter( QByteArray &data, bool flush )
{
if ( canFilter )
{
mutex.lock();
if ( !flush )
{
float *samples = ( float * )data.data();
const int size = data.size() / sizeof( float );
for ( int c = 0 ; c < chn ; ++c ) //Buforowanie danych
for ( int i = 0 ; i < size ; i += chn )
input[ c ].append( samples[ c+i ] );
}
else for ( int c = 0 ; c < chn ; ++c ) //Dokładanie ciszy
input[ c ].resize( FFT_SIZE );
data.clear();
const int chunks = input.at( 0 ).size() / FFT_SIZE_2 - 1;
if ( chunks > 0 ) //Jeżeli jest wystarczająca ilość danych
{
data.resize( chn * sizeof( float ) * FFT_SIZE_2 * chunks );
float *samples = ( float * )data.data();
for ( int c = 0 ; c < chn ; ++c )
{
int pos = c;
while ( input.at( c ).size() >= FFT_SIZE )
{
for ( int i = 0 ; i < FFT_SIZE ; ++i )
complex[ i ] = ( FFTComplex ){ input.at( c ).at( i ), 0.0f };
if ( !flush )
input[ c ].remove( 0, FFT_SIZE_2 );
else
input[ c ].clear();
fft_calc( fftIn, complex );
for ( int i = 0 ; i < FFT_SIZE_2 ; ++i )
{
complex[ i ].re *= f.at( i );
complex[ i ].im *= f.at( i );
complex[ FFT_SIZE-1-i ].re *= f.at( i );
complex[ FFT_SIZE-1-i ].im *= f.at( i );
}
fft_calc( fftOut, complex );
if ( last_samples.at( c ).isEmpty() )
{
for ( int i = 0 ; i < FFT_SIZE_2 ; ++i, pos += chn )
samples[ pos ] = complex[ i ].re / FFT_SIZE;
last_samples[ c ].resize( FFT_SIZE_2 );
}
else for ( int i = 0 ; i < FFT_SIZE_2 ; ++i, pos += chn )
samples[ pos ] = ( complex[ i ].re / FFT_SIZE ) * wind_f.at( i ) + last_samples.at( c ).at( i );
for ( int i = FFT_SIZE_2 ; i < FFT_SIZE ; ++i )
last_samples[ c ][ i-FFT_SIZE_2 ] = ( complex[ i ].re / FFT_SIZE ) * wind_f.at( i );
}
}
}
mutex.unlock();
return FFT_SIZE / ( double )srate;
}
return 0.0;
}
int main(int argc, char **argv)
{
FFTComplex *tab, *tab1, *tab_ref;
FFTSample *tabtmp, *tab2;
int it, i, c;
int do_speed = 0;
int do_mdct = 0;
int do_inverse = 0;
FFTContext s1, *s = &s1;
MDCTContext m1, *m = &m1;
int fft_nbits, fft_size;
mm_flags = 0;
fft_nbits = 9;
for(;;) {
c = getopt(argc, argv, "hsimn:");
if (c == -1)
break;
switch(c) {
case 'h':
help();
break;
case 's':
do_speed = 1;
break;
case 'i':
do_inverse = 1;
break;
case 'm':
do_mdct = 1;
break;
case 'n':
fft_nbits = atoi(optarg);
break;
}
}
fft_size = 1 << fft_nbits;
tab = av_malloc(fft_size * sizeof(FFTComplex));
tab1 = av_malloc(fft_size * sizeof(FFTComplex));
tab_ref = av_malloc(fft_size * sizeof(FFTComplex));
tabtmp = av_malloc(fft_size / 2 * sizeof(FFTSample));
tab2 = av_malloc(fft_size * sizeof(FFTSample));
if (do_mdct) {
if (do_inverse)
printf("IMDCT");
else
printf("MDCT");
ff_mdct_init(m, fft_nbits, do_inverse);
} else {
if (do_inverse)
printf("IFFT");
else
printf("FFT");
fft_init(s, fft_nbits, do_inverse);
fft_ref_init(fft_nbits, do_inverse);
}
printf(" %d test\n", fft_size);
/* generate random data */
for(i=0;i<fft_size;i++) {
tab1[i].re = frandom();
tab1[i].im = frandom();
}
/* checking result */
printf("Checking...\n");
if (do_mdct) {
if (do_inverse) {
imdct_ref((float *)tab_ref, (float *)tab1, fft_size);
ff_imdct_calc(m, tab2, (float *)tab1, tabtmp);
check_diff((float *)tab_ref, tab2, fft_size);
} else {
mdct_ref((float *)tab_ref, (float *)tab1, fft_size);
ff_mdct_calc(m, tab2, (float *)tab1, tabtmp);
check_diff((float *)tab_ref, tab2, fft_size / 2);
}
} else {
memcpy(tab, tab1, fft_size * sizeof(FFTComplex));
fft_permute(s, tab);
fft_calc(s, tab);
fft_ref(tab_ref, tab1, fft_nbits);
check_diff((float *)tab_ref, (float *)tab, fft_size * 2);
}
/* do a speed test */
if (do_speed) {
int64_t time_start, duration;
int nb_its;
printf("Speed test...\n");
/* we measure during about 1 seconds */
nb_its = 1;
for(;;) {
time_start = gettime();
for(it=0;it<nb_its;it++) {
if (do_mdct) {
if (do_inverse) {
ff_imdct_calc(m, (float *)tab, (float *)tab1, tabtmp);
} else {
ff_mdct_calc(m, (float *)tab, (float *)tab1, tabtmp);
}
} else {
memcpy(tab, tab1, fft_size * sizeof(FFTComplex));
fft_calc(s, tab);
}
}
duration = gettime() - time_start;
if (duration >= 1000000)
break;
nb_its *= 2;
}
printf("time: %0.1f us/transform [total time=%0.2f s its=%d]\n",
(double)duration / nb_its,
(double)duration / 1000000.0,
nb_its);
}
if (do_mdct) {
ff_mdct_end(m);
} else {
fft_end(s);
}
return 0;
}
int main(int argc, char **argv)
{
FFTComplex *tab, *tab1, *tab_ref;
FFTSample *tab2;
enum tf_transform transform = TRANSFORM_FFT;
FFTContext *m, *s;
#if FFT_FLOAT
RDFTContext *r;
DCTContext *d;
#endif /* FFT_FLOAT */
int it, i, err = 1;
int do_speed = 0, do_inverse = 0;
int fft_nbits = 9, fft_size;
double scale = 1.0;
AVLFG prng;
#if !AVFFT
s = av_mallocz(sizeof(*s));
m = av_mallocz(sizeof(*m));
#endif
#if !AVFFT && FFT_FLOAT
r = av_mallocz(sizeof(*r));
d = av_mallocz(sizeof(*d));
#endif
av_lfg_init(&prng, 1);
for (;;) {
int c = getopt(argc, argv, "hsimrdn:f:c:");
if (c == -1)
break;
switch (c) {
case 'h':
help();
return 1;
case 's':
do_speed = 1;
break;
case 'i':
do_inverse = 1;
break;
case 'm':
transform = TRANSFORM_MDCT;
break;
case 'r':
transform = TRANSFORM_RDFT;
break;
case 'd':
transform = TRANSFORM_DCT;
break;
case 'n':
fft_nbits = atoi(optarg);
break;
case 'f':
scale = atof(optarg);
break;
case 'c':
{
unsigned cpuflags = av_get_cpu_flags();
if (av_parse_cpu_caps(&cpuflags, optarg) < 0)
return 1;
av_force_cpu_flags(cpuflags);
break;
}
}
}
fft_size = 1 << fft_nbits;
tab = av_malloc_array(fft_size, sizeof(FFTComplex));
tab1 = av_malloc_array(fft_size, sizeof(FFTComplex));
tab_ref = av_malloc_array(fft_size, sizeof(FFTComplex));
tab2 = av_malloc_array(fft_size, sizeof(FFTSample));
if (!(tab && tab1 && tab_ref && tab2))
goto cleanup;
switch (transform) {
#if CONFIG_MDCT
case TRANSFORM_MDCT:
av_log(NULL, AV_LOG_INFO, "Scale factor is set to %f\n", scale);
if (do_inverse)
av_log(NULL, AV_LOG_INFO, "IMDCT");
else
av_log(NULL, AV_LOG_INFO, "MDCT");
mdct_init(&m, fft_nbits, do_inverse, scale);
break;
#endif /* CONFIG_MDCT */
case TRANSFORM_FFT:
if (do_inverse)
av_log(NULL, AV_LOG_INFO, "IFFT");
else
av_log(NULL, AV_LOG_INFO, "FFT");
fft_init(&s, fft_nbits, do_inverse);
if ((err = fft_ref_init(fft_nbits, do_inverse)) < 0)
goto cleanup;
break;
#if FFT_FLOAT
# if CONFIG_RDFT
case TRANSFORM_RDFT:
if (do_inverse)
av_log(NULL, AV_LOG_INFO, "IDFT_C2R");
else
av_log(NULL, AV_LOG_INFO, "DFT_R2C");
rdft_init(&r, fft_nbits, do_inverse ? IDFT_C2R : DFT_R2C);
if ((err = fft_ref_init(fft_nbits, do_inverse)) < 0)
goto cleanup;
break;
# endif /* CONFIG_RDFT */
# if CONFIG_DCT
case TRANSFORM_DCT:
if (do_inverse)
av_log(NULL, AV_LOG_INFO, "DCT_III");
else
av_log(NULL, AV_LOG_INFO, "DCT_II");
dct_init(&d, fft_nbits, do_inverse ? DCT_III : DCT_II);
break;
# endif /* CONFIG_DCT */
#endif /* FFT_FLOAT */
default:
av_log(NULL, AV_LOG_ERROR, "Requested transform not supported\n");
goto cleanup;
}
av_log(NULL, AV_LOG_INFO, " %d test\n", fft_size);
/* generate random data */
for (i = 0; i < fft_size; i++) {
tab1[i].re = frandom(&prng);
tab1[i].im = frandom(&prng);
}
/* checking result */
av_log(NULL, AV_LOG_INFO, "Checking...\n");
switch (transform) {
#if CONFIG_MDCT
case TRANSFORM_MDCT:
if (do_inverse) {
imdct_ref(&tab_ref->re, &tab1->re, fft_nbits);
imdct_calc(m, tab2, &tab1->re);
err = check_diff(&tab_ref->re, tab2, fft_size, scale);
} else {
mdct_ref(&tab_ref->re, &tab1->re, fft_nbits);
mdct_calc(m, tab2, &tab1->re);
err = check_diff(&tab_ref->re, tab2, fft_size / 2, scale);
}
break;
#endif /* CONFIG_MDCT */
case TRANSFORM_FFT:
memcpy(tab, tab1, fft_size * sizeof(FFTComplex));
fft_permute(s, tab);
fft_calc(s, tab);
fft_ref(tab_ref, tab1, fft_nbits);
err = check_diff(&tab_ref->re, &tab->re, fft_size * 2, 1.0);
break;
#if FFT_FLOAT
#if CONFIG_RDFT
case TRANSFORM_RDFT:
{
int fft_size_2 = fft_size >> 1;
if (do_inverse) {
tab1[0].im = 0;
tab1[fft_size_2].im = 0;
for (i = 1; i < fft_size_2; i++) {
tab1[fft_size_2 + i].re = tab1[fft_size_2 - i].re;
tab1[fft_size_2 + i].im = -tab1[fft_size_2 - i].im;
}
memcpy(tab2, tab1, fft_size * sizeof(FFTSample));
tab2[1] = tab1[fft_size_2].re;
rdft_calc(r, tab2);
fft_ref(tab_ref, tab1, fft_nbits);
for (i = 0; i < fft_size; i++) {
tab[i].re = tab2[i];
tab[i].im = 0;
}
err = check_diff(&tab_ref->re, &tab->re, fft_size * 2, 0.5);
} else {
for (i = 0; i < fft_size; i++) {
tab2[i] = tab1[i].re;
tab1[i].im = 0;
}
rdft_calc(r, tab2);
fft_ref(tab_ref, tab1, fft_nbits);
tab_ref[0].im = tab_ref[fft_size_2].re;
err = check_diff(&tab_ref->re, tab2, fft_size, 1.0);
}
break;
}
#endif /* CONFIG_RDFT */
#if CONFIG_DCT
case TRANSFORM_DCT:
memcpy(tab, tab1, fft_size * sizeof(FFTComplex));
dct_calc(d, &tab->re);
if (do_inverse)
idct_ref(&tab_ref->re, &tab1->re, fft_nbits);
else
dct_ref(&tab_ref->re, &tab1->re, fft_nbits);
err = check_diff(&tab_ref->re, &tab->re, fft_size, 1.0);
break;
#endif /* CONFIG_DCT */
#endif /* FFT_FLOAT */
}
/* do a speed test */
if (do_speed) {
int64_t time_start, duration;
int nb_its;
av_log(NULL, AV_LOG_INFO, "Speed test...\n");
/* we measure during about 1 seconds */
nb_its = 1;
for (;;) {
time_start = av_gettime_relative();
for (it = 0; it < nb_its; it++) {
switch (transform) {
case TRANSFORM_MDCT:
if (do_inverse)
imdct_calc(m, &tab->re, &tab1->re);
else
mdct_calc(m, &tab->re, &tab1->re);
break;
case TRANSFORM_FFT:
memcpy(tab, tab1, fft_size * sizeof(FFTComplex));
fft_calc(s, tab);
break;
#if FFT_FLOAT
case TRANSFORM_RDFT:
memcpy(tab2, tab1, fft_size * sizeof(FFTSample));
rdft_calc(r, tab2);
break;
case TRANSFORM_DCT:
memcpy(tab2, tab1, fft_size * sizeof(FFTSample));
dct_calc(d, tab2);
break;
#endif /* FFT_FLOAT */
}
}
duration = av_gettime_relative() - time_start;
if (duration >= 1000000)
break;
nb_its *= 2;
}
av_log(NULL, AV_LOG_INFO,
"time: %0.1f us/transform [total time=%0.2f s its=%d]\n",
(double) duration / nb_its,
(double) duration / 1000000.0,
nb_its);
}
switch (transform) {
#if CONFIG_MDCT
case TRANSFORM_MDCT:
mdct_end(m);
break;
#endif /* CONFIG_MDCT */
case TRANSFORM_FFT:
fft_end(s);
break;
#if FFT_FLOAT
# if CONFIG_RDFT
case TRANSFORM_RDFT:
rdft_end(r);
break;
# endif /* CONFIG_RDFT */
# if CONFIG_DCT
case TRANSFORM_DCT:
dct_end(d);
break;
# endif /* CONFIG_DCT */
#endif /* FFT_FLOAT */
}
cleanup:
av_free(tab);
av_free(tab1);
av_free(tab2);
av_free(tab_ref);
av_free(exptab);
#if !AVFFT
av_free(s);
av_free(m);
#endif
#if !AVFFT && FFT_FLOAT
av_free(r);
av_free(d);
#endif
if (err)
printf("Error: %d.\n", err);
return !!err;
}
AUD_Int32s denoise_aud( AUD_Int16s *pInBuf, AUD_Int16s *pOutBuf, AUD_Int32s inLen )
{
Fft_16s *hFft = NULL;
Ifft_16s *hIfft = NULL;
AUD_Window16s *hWin = NULL;
AUD_Int32s frameSize = 512;
AUD_Int32s frameStride = 256;
AUD_Int32s frameOverlap = 256;
AUD_Int32s nFFT = frameSize;
AUD_Int32s nSpecLen = nFFT / 2 + 1;
AUD_Int32s nNoiseFrame = 6; // (AUD_Int32s)( ( 0.25 * SAMPLE_RATE - frameSize ) / frameStride + 1 );
AUD_Int32s i, j, k, m, n, ret;
AUD_Int32s cleanLen = 0;
// pre-emphasis
// sig_preemphasis( pInBuf, pInBuf, inLen );
// init hamming module
win16s_init( &hWin, AUD_WIN_HAMM, frameSize, 14 );
AUD_ASSERT( hWin );
// init fft handle
fft_init( &hFft, nFFT, 15 );
AUD_ASSERT( hFft );
// init ifft handle
ifft_init( &hIfft, nFFT, 15 );
AUD_ASSERT( hIfft );
AUD_Int16s *pFrame = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pFrame );
// FFT
AUD_Int32s *pFFTMag = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTMag );
AUD_Int32s *pFFTRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTRe );
AUD_Int32s *pFFTIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTIm );
AUD_Int32s *pFFTCleanRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanRe );
AUD_Int32s *pFFTCleanIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanIm );
// noise spectrum
AUD_Double *pNoiseEn = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pNoiseEn );
AUD_Double *pNoiseB = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pNoiseB );
AUD_Double *pXPrev = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pXPrev );
AUD_Double *pAb = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pAb );
AUD_Double *pH = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pH );
AUD_Double *pGammak = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pGammak );
AUD_Double *pKsi = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pKsi );
AUD_Double *pLogSigmak = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pLogSigmak );
AUD_Double *pAlpha = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pAlpha );
AUD_Int32s *pLinToBark = (AUD_Int32s*)calloc( nSpecLen * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pLinToBark );
AUD_Int16s *pxOld = (AUD_Int16s*)calloc( frameOverlap * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxOld );
AUD_Int16s *pxClean = (AUD_Int16s*)calloc( nFFT * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxClean );
/*
AUD_Int32s critBandEnds[22] = { 0, 100, 200, 300, 400, 510, 630, 770, 920, 1080, 1270,
1480, 1720, 2000, 2320, 2700, 3150, 3700, 4400, 5300, 6400, 7700 };
*/
AUD_Int32s critFFTEnds[CRITICAL_BAND_NUM + 1] = { 0, 4, 7, 10, 13, 17, 21, 25, 30, 35, 41, 48, 56, 64,
75, 87, 101, 119, 141, 170, 205, 247, 257 };
// generate linear->bark transform mapping
k = 0;
for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
{
while ( k >= critFFTEnds[i] && k < critFFTEnds[i + 1] )
{
pLinToBark[k] = i;
k++;
}
}
AUD_Double absThr[CRITICAL_BAND_NUM] = { 38, 31, 22, 18.5, 15.5, 13, 11, 9.5, 8.75, 7.25, 4.75, 2.75,
1.5, 0.5, 0, 0, 0, 0, 2, 7, 12, 15.5 };
AUD_Double dbOffset[CRITICAL_BAND_NUM];
AUD_Double sumn[CRITICAL_BAND_NUM];
AUD_Double spread[CRITICAL_BAND_NUM];
for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
{
absThr[i] = pow( 10., absThr[i] / 10. ) / nFFT / ( 65535. * 65535. );
dbOffset[i] = 10. + i;
sumn[i] = 0.474 + i;
spread[i] = pow( 10., ( 15.81 + 7.5 * sumn[i] - 17.5 * sqrt( 1. + sumn[i] * sumn[i] ) ) / 10. );
}
AUD_Double dcGain[CRITICAL_BAND_NUM];
for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
{
dcGain[i] = 0.;
for ( j = 0; j < CRITICAL_BAND_NUM; j++ )
{
dcGain[i] += spread[MABS( i - j )];
}
}
AUD_Matrix exPatMatrix;
exPatMatrix.rows = CRITICAL_BAND_NUM;
exPatMatrix.cols = nSpecLen;
exPatMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &exPatMatrix );
AUD_ASSERT( ret == 0 );
// excitation pattern
AUD_Int32s index = 0;
for ( i = 0; i < exPatMatrix.rows; i++ )
{
AUD_Double *pExpatRow = exPatMatrix.pDouble + i * exPatMatrix.cols;
for ( j = 0; j < exPatMatrix.cols; j++ )
{
index = MABS( i - pLinToBark[j] );
pExpatRow[j] = spread[index];
}
}
AUD_Int32s frameNum = (inLen - frameSize) / frameStride + 1;
AUD_ASSERT( frameNum > nNoiseFrame );
// compute noise mean
for ( i = 0; i < nNoiseFrame; i++ )
{
win16s_calc( hWin, pInBuf + i * frameSize, pFrame );
fft_mag( hFft, pFrame, frameSize, pFFTMag );
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseEn[j] += pFFTMag[j] / 32768. * pFFTMag[j] / 32768.;
}
}
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseEn[j] /= nNoiseFrame;
}
// get cirtical band mean filtered noise power
AUD_Int32s k1 = 0, k2 = 0;
for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
{
k1 = k2;
AUD_Double segSum = 0.;
while ( k2 >= critFFTEnds[i] && k2 < critFFTEnds[i + 1] )
{
segSum += pNoiseEn[k2];
k2++;
}
segSum /= ( k2 - k1 );
for ( m = k1; m < k2; m++ )
{
pNoiseB[m] = segSum;
}
}
#if 0
AUDLOG( "noise band spectrum:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pNoiseB[j] );
}
AUDLOG( "\n" );
#endif
AUD_Matrix frameMatrix;
frameMatrix.rows = nSpecLen;
frameMatrix.cols = 1;
frameMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &frameMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pFrameEn = frameMatrix.pDouble;
AUD_Matrix xMatrix;
xMatrix.rows = nSpecLen;
xMatrix.cols = 1;
xMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &xMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pX = xMatrix.pDouble;
AUD_Matrix cMatrix;
cMatrix.rows = CRITICAL_BAND_NUM;
cMatrix.cols = 1;
cMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &cMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pC = cMatrix.pDouble;
AUD_Matrix tMatrix;
tMatrix.rows = 1;
tMatrix.cols = CRITICAL_BAND_NUM;
tMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &tMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pT = tMatrix.pDouble;
AUD_Matrix tkMatrix;
tkMatrix.rows = 1;
tkMatrix.cols = nSpecLen;
tkMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &tkMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pTk = tkMatrix.pDouble;
AUD_Double dB0[CRITICAL_BAND_NUM];
AUD_Double epsilon = pow( 2, -52 );
#define ESTIMATE_MASKTHRESH( sigMatrix, tkMatrix )\
do {\
AUD_Double *pSig = sigMatrix.pDouble; \
for ( m = 0; m < exPatMatrix.rows; m++ ) \
{ \
AUD_Double suma = 0.; \
AUD_Double *pExpatRow = exPatMatrix.pDouble + m * exPatMatrix.cols; \
for ( n = 0; n < exPatMatrix.cols; n++ ) \
{ \
suma += pExpatRow[n] * pSig[n]; \
} \
pC[m] = suma; \
} \
AUD_Double product = 1.; \
AUD_Double sum = 0.; \
for ( m = 0; m < sigMatrix.rows; m++ ) \
{ \
product *= pSig[m]; \
sum += pSig[m]; \
} \
AUD_Double power = 1. / sigMatrix.rows;\
AUD_Double sfmDB = 10. * log10( pow( product, power ) / sum / sigMatrix.rows + epsilon ); \
AUD_Double alpha = AUD_MIN( 1., sfmDB / (-60.) ); \
for ( m = 0; m < tMatrix.cols; m++ ) \
{ \
dB0[m] = dbOffset[m] * alpha + 5.5; \
pT[m] = pC[m] / pow( 10., dB0[m] / 10. ) / dcGain[m]; \
pT[m] = AUD_MAX( pT[m], absThr[m] ); \
} \
for ( m = 0; m < tkMatrix.cols; m++ ) \
{ \
pTk[m] = pT[pLinToBark[m]]; \
} \
} while ( 0 )
AUD_Double aa = 0.98;
AUD_Double mu = 0.98;
AUD_Double eta = 0.15;
AUD_Double vadDecision;
k = 0;
// start processing
for ( i = 0; i < frameNum; i++ )
{
win16s_calc( hWin, pInBuf + i * frameStride, pFrame );
fft_calc( hFft, pFrame, frameSize, pFFTRe, pFFTIm );
// compute SNR
vadDecision = 0.;
for ( j = 0; j < nSpecLen; j++ )
{
pFrameEn[j] = pFFTRe[j] / 32768. * pFFTRe[j] / 32768. + pFFTIm[j] / 32768. * pFFTIm[j] / 32768.;
pGammak[j] = AUD_MIN( pFrameEn[j] / pNoiseEn[j], 40. );
if ( i > 0 )
{
pKsi[j] = aa * pXPrev[j] / pNoiseEn[j] + ( 1 - aa ) * AUD_MAX( pGammak[j] - 1., 0. );
}
else
{
pKsi[j] = aa + ( 1. - aa ) * AUD_MAX( pGammak[j] - 1., 0. );
}
pLogSigmak[j] = pGammak[j] * pKsi[j] / ( 1. + pKsi[j] ) - log( 1. + pKsi[j] );
vadDecision += ( j > 0 ? 2 : 1 ) * pLogSigmak[j];
}
vadDecision /= nFFT;
#if 0
AUDLOG( "X prev:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pXPrev[j] );
}
AUDLOG( "\n" );
#endif
#if 0
AUDLOG( "gamma k:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pGammak[j] );
}
AUDLOG( "\n" );
#endif
#if 0
AUDLOG( "ksi:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pKsi[j] );
}
AUDLOG( "\n" );
#endif
#if 0
AUDLOG( "log sigma k:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pLogSigmak[j] );
}
AUDLOG( "\n" );
#endif
// AUDLOG( "vadDecision: %.2f\n", vadDecision );
// re-estimate noise
if ( vadDecision < eta )
{
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseEn[j] = mu * pNoiseEn[j] + ( 1. - mu ) * pFrameEn[j];
}
// re-estimate crital band based noise
AUD_Int32s k1 = 0, k2 = 0;
for ( int band = 0; band < CRITICAL_BAND_NUM; band++ )
{
k1 = k2;
AUD_Double segSum = 0.;
while ( k2 >= critFFTEnds[band] && k2 < critFFTEnds[band + 1] )
{
segSum += pNoiseEn[k2];
k2++;
}
segSum /= ( k2 - k1 );
for ( m = k1; m < k2; m++ )
{
pNoiseB[m] = segSum;
}
}
#if 0
AUDLOG( "noise band spectrum:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pNoiseB[j] );
}
AUDLOG( "\n" );
#endif
}
for ( j = 0; j < nSpecLen; j++ )
{
pX[j] = AUD_MAX( pFrameEn[j] - pNoiseEn[j], 0.001 );
pXPrev[j] = pFrameEn[j];
}
ESTIMATE_MASKTHRESH( xMatrix, tkMatrix );
for ( int iter = 0; iter < 2; iter++ )
{
for ( j = 0; j < nSpecLen; j++ )
{
pAb[j] = pNoiseB[j] + pNoiseB[j] * pNoiseB[j] / pTk[j];
pFrameEn[j] = pFrameEn[j] * pFrameEn[j] / ( pFrameEn[j] + pAb[j] );
ESTIMATE_MASKTHRESH( frameMatrix, tkMatrix );
#if 0
showMatrix( &tMatrix );
#endif
}
}
#if 0
AUDLOG( "tk:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pTk[j] );
}
AUDLOG( "\n" );
#endif
pAlpha[0] = ( pNoiseB[0] + pTk[0] ) * ( pNoiseB[0] / pTk[0] );
pH[0] = pFrameEn[0] / ( pFrameEn[0] + pAlpha[0] );
pXPrev[0] *= pH[0] * pH[0];
pFFTCleanRe[0] = 0;
pFFTCleanIm[0] = 0;
for ( j = 1; j < nSpecLen; j++ )
{
pAlpha[j] = ( pNoiseB[j] + pTk[j] ) * ( pNoiseB[j] / pTk[j] );
pH[j] = pFrameEn[j] / ( pFrameEn[j] + pAlpha[j] );
pFFTCleanRe[j] = pFFTCleanRe[nFFT - j] = (AUD_Int32s)round( pH[j] * pFFTRe[j] );
pFFTCleanIm[j] = (AUD_Int32s)round( pH[j] * pFFTIm[j] );
pFFTCleanIm[nFFT - j] = -pFFTCleanIm[j];
pXPrev[j] *= pH[j] * pH[j];
}
#if 0
AUDLOG( "denoise transfer function:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pH[j] );
}
AUDLOG( "\n" );
#endif
#if 0
AUDLOG( "clean FFT with phase:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d + j%d, ", pFFTCleanRe[j], pFFTCleanIm[j] );
}
AUDLOG( "\n" );
#endif
ifft_real( hIfft, pFFTCleanRe, pFFTCleanIm, nFFT, pxClean );
#if 0
AUDLOG( "clean speech:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d, ", pxClean[j] );
}
AUDLOG( "\n" );
#endif
for ( j = 0; j < frameStride; j++ )
{
if ( j < frameOverlap )
{
pOutBuf[k + j] = pxOld[j] + pxClean[j];
pxOld[j] = pxClean[frameStride + j];
}
else
{
pOutBuf[k + j] = pxClean[j];
}
}
k += frameStride;
cleanLen += frameStride;
}
// de-emphasis
// sig_deemphasis( pOutBuf, pOutBuf, cleanLen );
win16s_free( &hWin );
fft_free( &hFft );
ifft_free( &hIfft );
free( pFrame );
free( pNoiseEn );
free( pNoiseB );
free( pFFTMag );
free( pFFTRe );
free( pFFTIm );
free( pXPrev );
free( pAb );
free( pH );
free( pFFTCleanRe );
free( pFFTCleanIm );
free( pxOld );
free( pxClean );
free( pGammak );
free( pKsi );
free( pLogSigmak );
free( pAlpha );
free( pLinToBark );
ret = createMatrix( &xMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &exPatMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &frameMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &cMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &tMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &tkMatrix );
AUD_ASSERT( ret == 0 );
return cleanLen;
}
AUD_Int32s denoise_mmse( AUD_Int16s *pInBuf, AUD_Int16s *pOutBuf, AUD_Int32s inLen )
{
Fft_16s *hFft = NULL;
Ifft_16s *hIfft = NULL;
AUD_Window16s *hWin = NULL;
AUD_Int32s frameSize = 512;
AUD_Int32s frameStride = 256;
AUD_Int32s frameOverlap = 256;
AUD_Int32s nFFT = frameSize;
AUD_Int32s nSpecLen = nFFT / 2 + 1;
AUD_Int32s nNoiseFrame = (AUD_Int32s)( ( 0.25 * SAMPLE_RATE - frameSize ) / frameStride + 1 );;
AUD_Int32s i, j, k;
AUD_Int32s cleanLen = 0;
// pre-emphasis
// sig_preemphasis( pInBuf, pInBuf, inLen );
// init hamming module
win16s_init( &hWin, AUD_WIN_HAMM, frameSize, 14 );
AUD_ASSERT( hWin );
// init fft handle
fft_init( &hFft, nFFT, 15 );
AUD_ASSERT( hFft );
// init ifft handle
ifft_init( &hIfft, nFFT, 15 );
AUD_ASSERT( hIfft );
AUD_Int16s *pFrame = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pFrame );
// FFT
AUD_Int32s *pFFTMag = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTMag );
AUD_Int32s *pFFTRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTRe );
AUD_Int32s *pFFTIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTIm );
AUD_Int32s *pFFTCleanRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanRe );
AUD_Int32s *pFFTCleanIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanIm );
AUD_Double *pNoiseMean = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pNoiseMean );
AUD_Int32s *pFFTCleanMag = (AUD_Int32s*)calloc( nSpecLen * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanMag );
AUD_Int16s *pxOld = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxOld );
AUD_Int16s *pxClean = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxClean );
for ( i = 0; (i < nNoiseFrame) && ( (i * frameStride + frameSize) < inLen ); i++ )
{
win16s_calc( hWin, pInBuf + i * frameSize, pFrame );
fft_mag( hFft, pFrame, frameSize, pFFTMag );
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] += (AUD_Double)pFFTMag[j];
}
}
// compute noise mean
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] /= nNoiseFrame;
}
AUD_Double thres = 3.;
AUD_Double alpha = 2.;
AUD_Double flr = 0.002;
AUD_Double G = 0.9;
AUD_Double snr, beta;
AUD_Double tmp;
AUD_Double sigEnergy, noiseEnergy;
k = 0;
// start processing
for ( i = 0; (i * frameStride + frameSize) < inLen; i++ )
{
win16s_calc( hWin, pInBuf + i * frameStride, pFrame );
fft_calc( hFft, pFrame, frameSize, pFFTRe, pFFTIm );
// compute SNR
sigEnergy = 0.;
noiseEnergy = 0.;
for ( j = 0; j < nSpecLen; j++ )
{
tmp = pFFTRe[j] / 32768. * pFFTRe[j] / 32768. + pFFTIm[j] / 32768. * pFFTIm[j] / 32768.;
sigEnergy += tmp;
noiseEnergy += pNoiseMean[j] / 32768. * pNoiseMean[j] / 32768.;
tmp = sqrt( tmp ) * 32768.;
if ( tmp > MAX_INT32S )
{
AUD_ECHO
pFFTMag[j] = MAX_INT32S;
}
else
{
pFFTMag[j] = (AUD_Int32s)round( tmp );
}
}
snr = 10. * log10( sigEnergy / noiseEnergy );
beta = berouti( snr );
// AUDLOG( "signal energy: %.2f, noise energy: %.2f, snr: %.2f, beta: %.2f\n", sigEnergy, noiseEnergy, snr, beta );
#if 0
AUDLOG( "noisy FFT:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%d, ", pFFTMag[j] );
}
AUDLOG( "\n" );
#endif
tmp = 0.;
for ( j = 0; j < nSpecLen; j++ )
{
tmp = AUD_MAX( pow( pFFTMag[j], alpha ) - beta * pow( pNoiseMean[j], alpha ), flr * pow( pNoiseMean[j], alpha ) );
pFFTCleanMag[j] = (AUD_Int32s)round( pow( tmp, 1. / alpha ) );
}
// re-estimate noise
if ( snr < thres )
{
AUD_Double tmpNoise;
for ( j = 0; j < nSpecLen; j++ )
{
tmpNoise = G * pow( pNoiseMean[j], alpha ) + ( 1 - G ) * pow( pFFTMag[j], alpha );
pNoiseMean[j] = pow( tmpNoise, 1. / alpha );
}
}
#if 0
AUDLOG( "clean FFT:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%d, ", pFFTCleanMag[j] );
}
AUDLOG( "\n" );
#endif
pFFTCleanRe[0] = pFFTCleanMag[0];
pFFTCleanIm[0] = 0;
AUD_Double costheta, sintheta;
for ( j = 1; j < nSpecLen; j++ )
{
if ( pFFTMag[j] != 0 )
{
costheta = (AUD_Double)pFFTRe[j] / (AUD_Double)pFFTMag[j];
sintheta = (AUD_Double)pFFTIm[j] / (AUD_Double)pFFTMag[j];
pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = (AUD_Int32s)round( costheta * pFFTCleanMag[j] );
pFFTCleanIm[j] = (AUD_Int32s)round( sintheta * pFFTCleanMag[j] );
pFFTCleanIm[nFFT - j] = -pFFTCleanIm[j];
}
else
{
pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = pFFTCleanMag[j];
pFFTCleanIm[nFFT - j] = pFFTCleanIm[j] = 0;
}
}
#if 0
AUDLOG( "clean FFT with phase:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d + j%d, ", pFFTCleanRe[j], pFFTCleanIm[j] );
}
AUDLOG( "\n" );
#endif
ifft_real( hIfft, pFFTCleanRe, pFFTCleanIm, nFFT, pxClean );
#if 0
AUDLOG( "clean speech:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d, ", pxClean[j] );
}
AUDLOG( "\n" );
#endif
for ( j = 0; j < frameStride; j++ )
{
pOutBuf[k + j] = pxOld[j] + pxClean[j];
pxOld[j] = pxClean[frameOverlap + j];
}
k += frameStride;
cleanLen += frameStride;
}
// de-emphasis
// sig_deemphasis( pOutBuf, pOutBuf, cleanLen );
win16s_free( &hWin );
fft_free( &hFft );
ifft_free( &hIfft );
free( pFrame );
free( pNoiseMean );
free( pFFTMag );
free( pFFTRe );
free( pFFTIm );
free( pFFTCleanMag );
free( pFFTCleanRe );
free( pFFTCleanIm );
free( pxOld );
free( pxClean );
return cleanLen;
}
AUD_Int32s denoise_wiener( AUD_Int16s *pInBuf, AUD_Int16s *pOutBuf, AUD_Int32s inLen )
{
Fft_16s *hFft = NULL;
Ifft_16s *hIfft = NULL;
AUD_Window16s *hWin = NULL;
_VAD_Nest vadState;
AUD_Int32s frameSize = 512;
AUD_Int32s frameStride = 256;
AUD_Int32s nFFT = frameSize;
AUD_Int32s nSpecLen = nFFT / 2 + 1;
AUD_Int32s nNoiseFrame = (AUD_Int32s)( ( 0.25 * SAMPLE_RATE - frameSize ) / frameStride + 1 );
AUD_Int32s i, j, k;
AUD_Int32s cleanLen = 0;
vadState.meanEn = 280;
vadState.nbSpeechFrame = 0;
vadState.hangOver = 0;
AUD_Int16s *pFrame = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pFrame );
AUD_Double *pNoiseMean = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pNoiseMean );
AUD_Double *pLamdaD = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pLamdaD );
AUD_Double *pXi = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pXi );
AUD_Double *pGamma = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pGamma );
AUD_Double *pG = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pG );
AUD_Double *pGammaNew = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pGammaNew );
for ( j = 0; j < nSpecLen; j++ )
{
pG[j] = 1.;
pGamma[j] = 1.;
}
// FFT
AUD_Int32s *pFFTMag = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTMag );
AUD_Int32s *pFFTRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTRe );
AUD_Int32s *pFFTIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTIm );
AUD_Double *pFFTCleanMag = (AUD_Double*)calloc( nFFT * sizeof(AUD_Double), 1 );
AUD_ASSERT( pFFTCleanMag );
AUD_Int32s *pFFTCleanRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanRe );
AUD_Int32s *pFFTCleanIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanIm );
AUD_Int16s *pxClean = (AUD_Int16s*)calloc( nFFT * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxClean );
memset( pOutBuf, 0, inLen * sizeof(AUD_Int16s) );
// init hamming module
win16s_init( &hWin, AUD_WIN_HAMM, frameSize, 14 );
AUD_ASSERT( hWin );
// init fft handle
fft_init( &hFft, nFFT, 15 );
AUD_ASSERT( hFft );
// init ifft handle
ifft_init( &hIfft, nFFT, 15 );
AUD_ASSERT( hIfft );
// noise manipulation
for ( i = 0; (i < nNoiseFrame) && ( (i * frameStride + frameSize) < inLen ); i++ )
{
win16s_calc( hWin, pInBuf + i * frameStride, pFrame );
fft_mag( hFft, pFrame, frameSize, pFFTMag );
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] += (AUD_Double)pFFTMag[j];
pLamdaD[j] += (AUD_Double)pFFTMag[j] * (AUD_Double)pFFTMag[j];
}
}
// compute noise mean
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] /= nNoiseFrame;
pLamdaD[j] /= nNoiseFrame;
}
AUD_Int32s vadFlag = 0;
AUD_Double noiseLen = 9.;
AUD_Double alpha = 0.99;
k = 0;
for ( i = 0; (i * frameStride + frameSize) < inLen; i++ )
{
win16s_calc( hWin, pInBuf + i * frameStride, pFrame );
fft_calc( hFft, pFrame, frameSize, pFFTRe, pFFTIm );
for ( j = 0; j < nSpecLen; j++ )
{
pFFTMag[j] = (AUD_Int32s)round( sqrt( (AUD_Double)pFFTRe[j] * pFFTRe[j] + (AUD_Double)pFFTIm[j] * pFFTIm[j] ) );
}
#if 0
AUDLOG( "noisy FFT:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%d, ", pFFTMag[j] );
}
AUDLOG( "\n" );
#endif
vadFlag = vad_nest( &vadState, pFrame, frameSize );
if ( vadFlag == 0 )
{
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] = ( noiseLen * pNoiseMean[j] + (AUD_Double)pFFTMag[j] ) / ( noiseLen + 1. );
pLamdaD[j] = ( noiseLen * pLamdaD[j] + (AUD_Double)pFFTMag[j] * pFFTMag[j] ) / ( noiseLen + 1. );
}
}
for ( j = 0; j < nSpecLen; j++ )
{
pGammaNew[j] = (AUD_Double)pFFTMag[j] * pFFTMag[j] / pLamdaD[j];
pXi[j] = alpha * pG[j] * pG[j] * pGamma[j] + ( 1. - alpha ) * AUD_MAX( pGammaNew[j] - 1., 0. );
pGamma[j] = pGammaNew[j];
pG[j] = pXi[j] / ( pXi[j] + 1. );
pFFTCleanMag[j] = pG[j] * pFFTMag[j];
}
#if 0
AUDLOG( "clean FFT:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pFFTCleanMag[j] );
}
AUDLOG( "\n" );
#endif
// combine to real/im part of IFFT
pFFTCleanRe[0] = pFFTCleanMag[0];
pFFTCleanIm[0] = 0;
AUD_Double costheta, sintheta;
for ( j = 1; j < nSpecLen; j++ )
{
if ( pFFTMag[j] != 0 )
{
costheta = (AUD_Double)pFFTRe[j] / (AUD_Double)pFFTMag[j];
sintheta = (AUD_Double)pFFTIm[j] / (AUD_Double)pFFTMag[j];
pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = (AUD_Int32s)round( costheta * pFFTCleanMag[j] );
pFFTCleanIm[j] = (AUD_Int32s)round( sintheta * pFFTCleanMag[j] );
pFFTCleanIm[nFFT - j] = -pFFTCleanIm[j];
}
else
{
pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = pFFTCleanMag[j];
pFFTCleanIm[nFFT - j] = pFFTCleanIm[j] = 0;
}
}
ifft_real( hIfft, pFFTCleanRe, pFFTCleanIm, nFFT, pxClean );
#if 0
AUDLOG( "clean FFT with phase:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d + j%d, ", pFFTCleanRe[j], pFFTCleanIm[j] );
}
AUDLOG( "\n" );
#endif
for ( j = 0; j < frameSize; j++ )
{
pOutBuf[k + j] = pOutBuf[k + j] + pxClean[j];
}
k += frameStride;
cleanLen += frameStride;
}
win16s_free( &hWin );
fft_free( &hFft );
ifft_free( &hIfft );
free( pFrame );
free( pNoiseMean );
free( pLamdaD );
free( pXi );
free( pGamma );
free( pG );
free( pGammaNew );
free( pFFTMag );
free( pFFTRe );
free( pFFTIm );
free( pFFTCleanMag );
free( pFFTCleanRe );
free( pFFTCleanIm );
free( pxClean );
return cleanLen;
}
