Add DC normalizing filter to the beginning of the echo canceler (Thanks SteveU!)

git-svn-id: http://svn.digium.com/svn/zaptel/trunk@968 5390a7c7-147a-4af0-8ec9-7488f05a26cb

1 files changed, 11 insertions, 1 deletions

diff --git a/mg2ec.h b/mg2ec.h
index 334b8a1..8ad9a92 100644
--- a/mg2ec.h
+++ b/mg2ec.h
@@ -155,6 +155,7 @@ struct echo_can_state {
 	short lastsig[256];
 	int lastpos;
 	int backup;
+	int dc_estimate;
 
 };
 
@@ -257,9 +258,16 @@ static inline void init_cc(struct echo_can_state *ec, int N, int maxy, int maxu)
 
 static inline void echo_can_free(struct echo_can_state *ec)
 {
+	printk("EC: DC bias calculated: %d V\n", ec->dc_estimate >> 15);
 	FREE(ec);
 }
 
+short inline dc_removal(int *dc_estimate, short samp)
+{
+	*dc_estimate += ((((int)samp << 15) - *dc_estimate) >> 9);
+	return samp - (*dc_estimate >> 15);
+}
+
 static inline short echo_can_update(struct echo_can_state *ec, short iref, short isig) 
 {
 
@@ -274,6 +282,8 @@ static inline short echo_can_update(struct echo_can_state *ec, short iref, short
 	int Py_i;
 	/* ... */
 	int two_beta_i;
+
+	isig = dc_removal(&ec->dc_estimate, isig);
 	
 	/* flow A on pg. 428 */
 	/* eq. (16): high-pass filter the input to generate the next value;
@@ -646,7 +656,7 @@ static inline struct echo_can_state *echo_can_create(int len, int adaption_mode)
 									2 * sizeof(short) * (1 << DEFAULT_ALPHA_ST_I) + /* s_s */
 									2 * sizeof(short) * (maxu) +		/* u_s */
 									2 * sizeof(short) * len);			/* y_tilde_s */
-	  init_cc(ec, len, maxy, maxu);
+		init_cc(ec, len, maxy, maxu);
 	}
 	return ec;
 }