Fix for fixed-point preprocessor bug reported by Peter Rowling
[speexdsp.git] / libspeex / preprocess.c
index e531746..bbdb19f 100644 (file)
 #define M_PI 3.14159263
 #endif
 
-#define LOUDNESS_EXP 2.5
-
+#define LOUDNESS_EXP 5.f
+#define AMP_SCALE .001f
+#define AMP_SCALE_1 1000.f
+      
 #define NB_BANDS 24
 
 #define SPEECH_PROB_START_DEFAULT       QCONST16(0.35f,15)
 #define SPEECH_PROB_CONTINUE_DEFAULT    QCONST16(0.20f,15)
-#define NOISE_SUPPRESS_DEFAULT       -25
-#define ECHO_SUPPRESS_DEFAULT        -45
+#define NOISE_SUPPRESS_DEFAULT       -15
+#define ECHO_SUPPRESS_DEFAULT        -40
 #define ECHO_SUPPRESS_ACTIVE_DEFAULT -15
 
 #ifndef NULL
@@ -185,8 +187,6 @@ struct SpeexPreprocessState_ {
    
    /* Parameters */
    int    denoise_enabled;
-   int    agc_enabled;
-   float  agc_level;
    int    vad_enabled;
    int    dereverb_enabled;
    spx_word16_t  reverb_decay;
@@ -215,7 +215,7 @@ struct SpeexPreprocessState_ {
    spx_word32_t *S;          /**< Smoothed power spectrum */
    spx_word32_t *Smin;       /**< See Cohen paper */
    spx_word32_t *Stmp;       /**< See Cohen paper */
-   int *update_prob;       /**< Propability of speech presence for noise update */
+   int *update_prob;         /**< Probability of speech presence for noise update */
 
    spx_word16_t *zeta;       /**< Smoothed a priori SNR */
    spx_word32_t *echo_noise;
@@ -225,11 +225,20 @@ struct SpeexPreprocessState_ {
    spx_word16_t *inbuf;      /**< Input buffer (overlapped analysis) */
    spx_word16_t *outbuf;     /**< Output buffer (for overlap and add) */
 
+   /* AGC stuff, only for floating point for now */
 #ifndef FIXED_POINT
+   int    agc_enabled;
+   float  agc_level;
+   float  loudness_accum;
    float *loudness_weight;   /**< Perceptual loudness curve */
-   float  loudness;          /**< loudness estimate */
-   float  loudness2;         /**< loudness estimate */
+   float  loudness;          /**< Loudness estimate */
+   float  agc_gain;          /**< Current AGC gain */
    int    nb_loudness_adapt; /**< Number of frames used for loudness adaptation so far */
+   float  max_gain;          /**< Maximum gain allowed */
+   float  max_increase_step; /**< Maximum increase in gain from one frame to another */
+   float  max_decrease_step; /**< Maximum decrease in gain from one frame to another */
+   float  prev_loudness;     /**< Loudness of previous frame */
+   float  init_max;          /**< Current gain limit during initialisation */
 #endif
    int    nb_adapt;          /**< Number of frames used for adaptation so far */
    int    was_speech;
@@ -247,7 +256,11 @@ static void conj_window(spx_word16_t *w, int len)
    for (i=0;i<len;i++)
    {
       spx_word16_t tmp;
+#ifdef FIXED_POINT
+      spx_word16_t x = DIV32_16(MULT16_16(32767,i),len);
+#else      
       spx_word16_t x = DIV32_16(MULT16_16(QCONST16(4.f,13),i),len);
+#endif
       int inv=0;
       if (x<QCONST16(1.f,13))
       {
@@ -413,8 +426,6 @@ SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_r
    
    st->sampling_rate = sampling_rate;
    st->denoise_enabled = 1;
-   st->agc_enabled = 0;
-   st->agc_level = 8000;
    st->vad_enabled = 0;
    st->dereverb_enabled = 0;
    st->reverb_decay = 0;
@@ -487,6 +498,8 @@ SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_r
       st->outbuf[i]=0;
    }
 #ifndef FIXED_POINT
+   st->agc_enabled = 0;
+   st->agc_level = 8000;
    st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
    for (i=0;i<N;i++)
    {
@@ -497,9 +510,15 @@ SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_r
          st->loudness_weight[i]=.01f;
       st->loudness_weight[i] *= st->loudness_weight[i];
    }
-   st->loudness = pow(6000,LOUDNESS_EXP);
-   st->loudness2 = 6000;
+   /*st->loudness = pow(AMP_SCALE*st->agc_level,LOUDNESS_EXP);*/
+   st->loudness = 1e-15;
+   st->agc_gain = 1;
    st->nb_loudness_adapt = 0;
+   st->max_gain = 30;
+   st->max_increase_step = exp(0.11513f * 12.*st->frame_size / st->sampling_rate);
+   st->max_decrease_step = exp(-0.11513f * 40.*st->frame_size / st->sampling_rate);
+   st->prev_loudness = 1;
+   st->init_max = 1;
 #endif
    st->was_speech = 0;
 
@@ -550,40 +569,49 @@ static void speex_compute_agc(SpeexPreprocessState *st, spx_word16_t Pframe, spx
 {
    int i;
    int N = st->ps_size;
-   float agc_gain;
-
-   float loudness=0.f;
-   float rate, rate2=.05f;
+   float target_gain;
+   float loudness=1.f;
+   float rate;
    
    for (i=2;i<N;i++)
    {
-      loudness += 2.f*N*SQR16(st->ft[i])* st->loudness_weight[i];
+      loudness += 2.f*N*st->ps[i]* st->loudness_weight[i];
    }
    loudness=sqrt(loudness);
       /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
    loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
-   if (Pframe>.5 && st->nb_adapt>50)
+   if (Pframe>.3f)
    {
       st->nb_loudness_adapt++;
-      rate=2.0f/(1+st->nb_loudness_adapt);
-      st->loudness = (1-rate)*st->loudness + (rate)*pow(loudness, LOUDNESS_EXP);
-      
-      st->loudness2 = (1-rate2)*st->loudness2 + rate2*pow(st->loudness, 1.0f/LOUDNESS_EXP);
+      /*rate=2.0f*Pframe*Pframe/(1+st->nb_loudness_adapt);*/
+      rate = .03*Pframe*Pframe;
+      st->loudness = (1-rate)*st->loudness + (rate)*pow(AMP_SCALE*loudness, LOUDNESS_EXP);
+      st->loudness_accum = (1-rate)*st->loudness_accum + rate;
+      if (st->init_max < st->max_gain && st->nb_adapt > 20)
+         st->init_max *= 1.f + .1f*Pframe*Pframe;
    }
-   printf ("%f %f %f %f\n", Pframe, loudness, pow(st->loudness, 1.0f/LOUDNESS_EXP), st->loudness2);
+   /*printf ("%f %f %f %f\n", Pframe, loudness, pow(st->loudness, 1.0f/LOUDNESS_EXP), st->loudness2);*/
    
-   loudness = pow(st->loudness, 1.0f/LOUDNESS_EXP);
-   
-   /*fprintf (stderr, "%f %f %f\n", loudness, st->loudness2, rate);*/
-   
-   agc_gain = st->agc_level/st->loudness2;
-   /*fprintf (stderr, "%f %f %f %f\n", active_bands, st->loudness, st->loudness2, agc_gain);*/
-   if (agc_gain>30)
-      agc_gain = 30;
+   target_gain = AMP_SCALE*st->agc_level*pow(st->loudness/(1e-4+st->loudness_accum), -1.0f/LOUDNESS_EXP);
+
+   if ((Pframe>.5  && st->nb_adapt > 20) || target_gain < st->agc_gain)
+   {
+      if (target_gain > st->max_increase_step*st->agc_gain)
+         target_gain = st->max_increase_step*st->agc_gain;
+      if (target_gain < st->max_decrease_step*st->agc_gain && loudness < 10*st->prev_loudness)
+         target_gain = st->max_decrease_step*st->agc_gain;
+      if (target_gain > st->max_gain)
+         target_gain = st->max_gain;
+      if (target_gain > st->init_max)
+         target_gain = st->init_max;
    
+      st->agc_gain = target_gain;
+   }
+   /*fprintf (stderr, "%f %f %f\n", loudness, (float)AMP_SCALE_1*pow(st->loudness, 1.0f/LOUDNESS_EXP), st->agc_gain);*/
+      
    for (i=0;i<2*N;i++)
-      ft[i] *= agc_gain;
-   
+      ft[i] *= st->agc_gain;
+   st->prev_loudness = loudness;
 }
 #endif
 
@@ -709,6 +737,8 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
    spx_word16_t effective_echo_suppress;
    
    st->nb_adapt++;
+   if (st->nb_adapt>20000)
+      st->nb_adapt = 20000;
    st->min_count++;
    
    beta = MAX16(QCONST16(.03,15),DIV32_16(Q15_ONE,st->nb_adapt));
@@ -1045,6 +1075,24 @@ int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
       (*(float*)ptr) = st->agc_level;
       break;
+   case SPEEX_PREPROCESS_SET_AGC_INCREMENT:
+      st->max_increase_step = exp(0.11513f * (*(spx_int32_t*)ptr)*st->frame_size / st->sampling_rate);
+      break;
+   case SPEEX_PREPROCESS_GET_AGC_INCREMENT:
+      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_increase_step)*st->sampling_rate/st->frame_size);
+      break;
+   case SPEEX_PREPROCESS_SET_AGC_DECREMENT:
+      st->max_decrease_step = exp(0.11513f * (*(spx_int32_t*)ptr)*st->frame_size / st->sampling_rate);
+      break;
+   case SPEEX_PREPROCESS_GET_AGC_DECREMENT:
+      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_decrease_step)*st->sampling_rate/st->frame_size);
+      break;
+   case SPEEX_PREPROCESS_SET_AGC_MAX_GAIN:
+      st->max_gain = exp(0.11513f * (*(spx_int32_t*)ptr));
+      break;
+   case SPEEX_PREPROCESS_GET_AGC_MAX_GAIN:
+      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_gain));
+      break;
 #endif
    case SPEEX_PREPROCESS_SET_VAD:
       speex_warning("The VAD has been replaced by a hack pending a complete rewrite");