echo_cancel.c

00001 /* -*- Mode: C -*-
00002  * Worldvisions Weaver Software:
00003  *   Copyright (C) 1997-2003 Net Integration Technologies, Inc.
00004  *
00005  * NLMS echo canceller
00006  *
00007  * This is all implemented in plain old C and fixed-point arithmetic so that
00008  * it cooperates will with kernel drivers.
00009  */
00010 
00011 #include "wvtelephony.h"
00012 #include <stdio.h>
00013 
00014 #define SQ(x) ((x)*(x))
00015 #define ROUND_DIV(a,b) (((a) + ((b)/2))/(b))
00016  
00106 void echo_cancel(const short *out_buf,
00107         const short *in,
00108         size_t block_size,
00109         int *filter_q16,
00110         size_t num_taps,
00111         short *echo_cancelled_in)
00112 {
00113     const short *cur_out = &out_buf[block_size-1];
00114     const short *cur_in = &in[block_size-1];
00115     short *cur_in_ec = &echo_cancelled_in[block_size-1];
00116     
00117     const short *out_p;
00118     int out_norm_sq_p16; /* The value is * 2^16 */
00119 
00120     /* Calculate initial norm squared of output vector
00121      */
00122     out_norm_sq_p16 = 0;
00123     for (out_p = &out_buf[block_size+num_taps-1];
00124             out_p != &out_buf[block_size-1];
00125             --out_p)
00126     {
00127         out_norm_sq_p16 += SQ(ROUND_DIV((int) *out_p, 1<<8));
00128     }
00129 
00130     do
00131     {
00132         int *filter_q16_p;
00133         int echo_est_q16;
00134         short echo_est;
00135         
00136         /* Caculate echo estimate for this sample using output data
00137          */
00138         echo_est_q16 = 0;
00139         for (out_p = &cur_out[num_taps-1],
00140                 filter_q16_p = &filter_q16[num_taps-1];
00141                 out_p != cur_out;
00142                 --out_p, --filter_q16_p)
00143         {
00144             echo_est_q16 += (int) *out_p * *filter_q16_p;
00145         }
00146         echo_est = ROUND_DIV(echo_est_q16, 1<<16);
00147         
00148         /* Echo cancelled input is simply input minus echo estimate
00149          * Round echo_est_q16 to nearest int when convering to short
00150          * This can also be interpreted as the error term, which
00151          * is used for the NLMS correction below
00152          */
00153         *cur_in_ec = *cur_in - echo_est;
00154 
00155         /* Update norm squared of output vector
00156          */
00157         out_norm_sq_p16 += SQ(ROUND_DIV((int) cur_out[0], 1<<8))
00158             - SQ(ROUND_DIV((int) cur_out[num_taps], 1<<8));
00159 
00160         /* Update filter tap weights using NLMS correction
00161          */
00162         if (out_norm_sq_p16 != 0)
00163         {
00164             for (out_p = &cur_out[num_taps-1],
00165                     filter_q16_p = &filter_q16[num_taps-1];
00166                     out_p != cur_out;
00167                     --out_p, --filter_q16_p)
00168             {
00169                 *filter_q16_p += 
00170                     ROUND_DIV((int) *cur_in_ec * (int) *out_p, out_norm_sq_p16);
00171             }
00172         }
00173     } while (cur_out--, cur_in_ec--, cur_in-- != in);
00174 }
00175 

Generated on Mon Feb 5 10:54:28 2007 for WvStreams by  doxygen 1.5.1