aec.cxx

MiniSip Client with DomainKeys Authentication, Sip, Audio communications, Echo Cancel

/* aec.cxx
 * Acoustic Echo Cancellation NLMS-pw algorithm
 * Author: Andre Adrian, DFS Deutsche Flugsicherung
 * <[e-mail]>
 *
 * Version 1.1
 */

#include<config.h>

#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include<libminisip/aec/aec.h>

#ifdef _WIN32_WCE
# define M_PI           3.14159265358979323846  /* pi */
#endif

/* ================================================================ */
/* Exponential Smoothing or IIR Infinite Impulse Response Filter */

IIR6::IIR6()
{
  memset(this, 0, sizeof(IIR6));
}

float IIR6::lowpass(float in)
{
  const float AlphaLp = 0.15f;	/* controls Transfer Frequence */

  /* Lowpass = Exponential Smoothing */
  lowpassf[0] = in;
  int i;
  for (i = 0; i < 2*POL; ++i) {
    lowpassf[i+1] += AlphaLp*(lowpassf[i] - lowpassf[i+1]);
  }
  return lowpassf[2*POL];   
}

/* ================================================================ */

/*
 * Algorithm:  Recursive single pole FIR high-pass filter
 *
 * Reference: The Scientist and Engineer's Guide to Digital Processing
 */

FIR1::FIR1()
{
  float x = (float) exp(-2.0 * M_PI * PreWhiteTransferFreq/8000.0f);
  
  a0 = (1.0f + x) / 2.0f;
  a1 = -(1.0f + x) / 2.0f;
  b1 = x;
  last_in = 0.0f;
  last_out = 0.0f;
}

/* ================================================================ */

/* Vector Dot Product */
float dotp(float a[], float b[]) {
  float sum0 = 0.0, sum1 = 0.0;
  int j;
  
  for (j = 0; j < NLMS_LEN; j+= 2) {
    // optimize: partial loop unrolling
    sum0 += a[j] * b[j];
    sum1 += a[j+1] * b[j+1];
  }
  return sum0+sum1;
}

AEC::AEC()
{
  max_max_x = 0.0f;
  hangover = 0;
  memset(max_x, 0, sizeof(max_x));
  dtdCnt = dtdNdx = 0;
  
  memset(x, 0, sizeof(x));
  memset(xf, 0, sizeof(xf));
  memset(w, 0, sizeof(w));
  j = NLMS_EXT;
  lastupdate = 0;
  dotp_xf_xf = 0.0f;
}

/* Normalized Least Mean Square Algorithm pre-whitening (NLMS-pw)
 * The LMS algorithm was developed by Bernard Widrow
 * book: Widrow/Stearns, Adaptive Signal Processing, Prentice-Hall, 1985
 *
 * in mic: microphone sample (PCM as floating point value)
 * in spk: loudspeaker sample (PCM as floating point value)
 * in update: 0 for convolve only, 1 for convolve and update 
 * return: echo cancelled microphone sample
 */
float AEC::nlms_pw(float mic, float spk, int update)
{
  float d = mic;      	      	  // desired signal
  x[j] = spk;
  xf[j] = Fx.highpass(spk);       // pre-whitening of x

  // calculate error value (mic signal - estimated mic signal from spk signal)
  float e = d - dotp(w, x + j);
  
  if (update) {    
    float ef = Fe.highpass(e);    // pre-whitening of e
    
    if (lastupdate) {
      // optimize: iterative dotp(xf, xf)
      dotp_xf_xf += (xf[j]*xf[j] - xf[j+NLMS_LEN-1]*xf[j+NLMS_LEN-1]);
    } else {
      dotp_xf_xf = dotp(xf+j, xf+j);
    } 
      
    // calculate variable step size
    float mikro_ef = 0.5f/dotp_xf_xf * ef;
    
    // update tap weights (filter learning)
    int i;
    for (i = 0; i < NLMS_LEN; i += 2) {
      // optimize: partial loop unrolling
      w[i] += mikro_ef*xf[i+j];
      w[i+1] += mikro_ef*xf[i+j+1];
    }
  }
  lastupdate = update;
  
  if (--j < 0) {
    // optimize: decrease number of memory copies
    j = NLMS_EXT;
    memmove(x+j+1, x, (NLMS_LEN-1)*sizeof(float));    
    memmove(xf+j+1, xf, (NLMS_LEN-1)*sizeof(float));    
  }
  
  return e;
}


/* Geigel Double-Talk Detector
 *
 * in d: microphone sample (PCM as floating point value)
 * in x: loudspeaker sample (PCM as floating point value)
 * return: 0 for no talking, 1 for talking
 */
int AEC::dtd(float d, float x)
{
  // optimized implementation of max(|x[0]|, |x[1]|, .., |x[L-1]|):
  // calculate max of block (DTD_LEN values)
  x = fabsf(x);
  if (x > max_x[dtdNdx]) {
    max_x[dtdNdx] = x;
    if (x > max_max_x) {
      max_max_x = x;
    }
  }
  if (++dtdCnt >= DTD_LEN) {
    dtdCnt = 0;
    // calculate max of max
    max_max_x = 0.0f;
    for (int i = 0; i < NLMS_LEN/DTD_LEN; ++i) {
      if (max_x[i] > max_max_x) {
        max_max_x = max_x[i];
      }
    }
    // rotate Ndx
    if (++dtdNdx >= NLMS_LEN/DTD_LEN) dtdNdx = 0;
    max_x[dtdNdx] = 0.0f;
  }

  // The Geigel DTD algorithm with Hangover timer Thold
  if (fabsf(d) >= GeigelThreshold * max_max_x) {
    hangover = Thold;
  }
    
  if (hangover) --hangover;
  
  if (max_max_x < UpdateThreshold) {
    // avoid update with silence
    return 1;
  } else {
    return (hangover > 0);
  }
}


int AEC::doAEC(int d, int x) 
{
  float s0 = (float)d;
  float s1 = (float)x;
  
  // Mic and Spk signal remove DC (IIR highpass filter)
  s0 = dc0.highpass(s0);
  s1 = dc1.highpass(s1);

  // Mic Highpass Filter - telephone users are used to 300Hz cut-off
  s0 = hp0.highpass(s0);

  // Double Talk Detector
  int update = !dtd(s0, s1);

  // Acoustic Echo Cancellation
  s0 = nlms_pw(s0, s1, update);

  // Acoustic Echo Suppression
  if (update) {
    // Non Linear Processor (NLP): attenuate low volumes
    s0 *= NLPAttenuation;
  }
  
  // Saturation
  if (s0 > MAXPCM) {
    return (int)MAXPCM;
  } else if (s0 < -MAXPCM) {
    return (int)-MAXPCM;
  } else {
    return (int)floorf( ( float)(s0+0.5) );
  }
}