More echo canceller updates for MG2. Thanks mgernoth, you rock! (#5520)

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

1 files changed, 24 insertions, 12 deletions

diff --git a/mg2ec.h b/mg2ec.h
index 03bb56b..334b8a1 100644
--- a/mg2ec.h
+++ b/mg2ec.h
@@ -462,8 +462,12 @@ static inline short echo_can_update(struct echo_can_state *ec, short iref, short
 		!(ec->i_d % DEFAULT_M)) {		/* we only update on every DEFAULM_M'th sample from the stream */
   			if (ec->Lu_i > MIN_UPDATE_THRESH_I) {	/* there is sufficient energy above the noise floor to contain meaningful data */
   							/* so loop over all the filter coefficients */
-#ifdef FILTER_PERCENT
-				int max = 0, max2 = 0;
+#ifdef USED_COEFFS
+				int max_coeffs[USED_COEFFS];
+				int *pos;
+
+				if (ec->N_d > USED_COEFFS)
+					memset(max_coeffs, 0, USED_COEFFS*sizeof(int));
 #endif
 #ifdef MEC2_STATS_DETAILED
 				printk( KERN_INFO "updating coefficients with: ec->Lu_i %9d\n", ec->Lu_i);
@@ -482,21 +486,29 @@ static inline short echo_can_update(struct echo_can_state *ec, short iref, short
 					ec->a_i[k] += grad2 / two_beta_i;
 					ec->a_s[k] = ec->a_i[k] >> 16;
 
-#ifdef FILTER_PERCENT
-					/* Find the peaks */
-					if (abs(ec->a_i[k]) > max)
-					{
-						max2 = max;
-						max = abs(ec->a_i[k]);
+#ifdef USED_COEFFS
+					if (ec->N_d > USED_COEFFS) {
+						if (abs(ec->a_i[k]) > max_coeffs[USED_COEFFS-1]) {
+							/* More or less insertion-sort... */
+							pos = max_coeffs;
+							while (*pos > abs(ec->a_i[k]))
+								pos++;
+
+							if (*pos > max_coeffs[USED_COEFFS-1])
+								memmove(pos+1, pos, (USED_COEFFS-(pos-max_coeffs)-1)*sizeof(int));
+
+							*pos = abs(ec->a_i[k]);
+						}
 					}
 #endif
 				}
 
-#ifdef FILTER_PERCENT
+#ifdef USED_COEFFS
 				/* Filter out irrelevant coefficients */
-				for (k=0; k < ec->N_d; k++)
-					if (abs(ec->a_i[k]) < (max2*FILTER_PERCENT)/100)
-						ec->a_i[k] = ec->a_s[k] = 0;
+				if (ec->N_d > USED_COEFFS)
+					for (k=0; k < ec->N_d; k++)
+						if (abs(ec->a_i[k]) < max_coeffs[USED_COEFFS-1])
+							ec->a_i[k] = ec->a_s[k] = 0;
 #endif
 		 	 } else { 
 #ifdef MEC2_STATS_DETAILED