Mercurial > hg > audiostuff
comparison spandsp-0.0.6pre17/src/echo.c @ 4:26cd8f1ef0b1
import spandsp-0.0.6pre17
author | Peter Meerwald <pmeerw@cosy.sbg.ac.at> |
---|---|
date | Fri, 25 Jun 2010 15:50:58 +0200 |
parents | |
children |
comparison
equal
deleted
inserted
replaced
3:c6c5a16ce2f2 | 4:26cd8f1ef0b1 |
---|---|
1 /* | |
2 * SpanDSP - a series of DSP components for telephony | |
3 * | |
4 * echo.c - An echo cancellor, suitable for electrical and acoustic | |
5 * cancellation. This code does not currently comply with | |
6 * any relevant standards (e.g. G.164/5/7/8). One day.... | |
7 * | |
8 * Written by Steve Underwood <steveu@coppice.org> | |
9 * | |
10 * Copyright (C) 2001, 2003 Steve Underwood | |
11 * | |
12 * Based on a bit from here, a bit from there, eye of toad, | |
13 * ear of bat, etc - plus, of course, my own 2 cents. | |
14 * | |
15 * All rights reserved. | |
16 * | |
17 * This program is free software; you can redistribute it and/or modify | |
18 * it under the terms of the GNU Lesser General Public License version 2.1, | |
19 * as published by the Free Software Foundation. | |
20 * | |
21 * This program is distributed in the hope that it will be useful, | |
22 * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
23 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
24 * GNU Lesser General Public License for more details. | |
25 * | |
26 * You should have received a copy of the GNU Lesser General Public | |
27 * License along with this program; if not, write to the Free Software | |
28 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. | |
29 * | |
30 * $Id: echo.c,v 1.33 2009/09/22 13:11:04 steveu Exp $ | |
31 */ | |
32 | |
33 /*! \file */ | |
34 | |
35 /* TODO: | |
36 Finish the echo suppressor option, however nasty suppression may be. | |
37 Add an option to reintroduce side tone at -24dB under appropriate conditions. | |
38 Improve double talk detector (iterative!) | |
39 */ | |
40 | |
41 /* We need to differentiate between transmitted energy which will train the echo | |
42 canceller well (voice, white noise, and other broadband sources) and energy | |
43 which will train it badly (supervisory tones, DTMF, whistles, and other | |
44 narrowband sources). There are many ways this might be done. This canceller uses | |
45 a method based on the autocorrelation qualities of the transmitted signal. A rather | |
46 peaky autocorrelation function is a clear sign of a narrowband signal. We only need | |
47 perform the autocorrelation at well spaced intervals, so the compute load is not too | |
48 great. Multiple successive autocorrelation functions with a similar peaky shape are a | |
49 clear indication of a stationary narrowband signal. Using TKEO, it should be possible to | |
50 greatly reduce the compute requirement for narrowband detection. */ | |
51 | |
52 /* The FIR taps must be adapted as 32 bit values, to get the necessary finesse | |
53 in the adaption process. However, they are applied as 16 bit values (bits 30-15 | |
54 of the 32 bit values) in the FIR. For the working 16 bit values, we need 4 sets. | |
55 | |
56 3 of the 16 bit sets are used on a rotating basis. Normally the canceller steps | |
57 round these 3 sets at regular intervals. Any time we detect double talk, we can go | |
58 back to the set from two steps ago with reasonable assurance it is a well adapted | |
59 set. We cannot just go back one step, as we may have rotated the sets just before | |
60 double talk or tone was detected, and that set may already be somewhat corrupted. | |
61 | |
62 When narrowband energy is detected we need to continue adapting to it, to echo | |
63 cancel it. However, the adaption will almost certainly be going astray. Broadband | |
64 (or even complex sequences of narrowband) energy will normally lead to a well | |
65 trained cancellor, with taps matching the impulse response of the channel. | |
66 For stationary narrowband energy, there is usually has an infinite number of | |
67 alternative tap sets which will cancel it well. A previously well trained set of | |
68 taps will tend to drift amongst the alternatives. When broadband energy resumes, the | |
69 taps may be a total mismatch for the signal, and could even amplify rather than | |
70 attenuate the echo. The solution is to use a fourth set of 16 bit taps. When we first | |
71 detect the narrowband energy we save the oldest of the group of three sets, but do | |
72 not change back to an older set. We let the canceller cancel, and it adaption drift | |
73 while the narrowband energy is present. When we detect the narrowband energy has ceased, | |
74 we switch to using the fourth set of taps which was saved. | |
75 | |
76 When we revert to an older set of taps, we must replace both the 16 bit and 32 bit | |
77 working tap sets. The saved 16 bit values are good enough to also be used as a replacement | |
78 for the 32 bit values. We loose the fractions, but they should soon settle down in a | |
79 reasonable way. */ | |
80 | |
81 #if defined(HAVE_CONFIG_H) | |
82 #include "config.h" | |
83 #endif | |
84 | |
85 #include <inttypes.h> | |
86 #include <stdlib.h> | |
87 #if defined(HAVE_TGMATH_H) | |
88 #include <tgmath.h> | |
89 #endif | |
90 #if defined(HAVE_MATH_H) | |
91 #include <math.h> | |
92 #endif | |
93 #include "floating_fudge.h" | |
94 #include <string.h> | |
95 #include <stdio.h> | |
96 | |
97 #include "spandsp/telephony.h" | |
98 #include "spandsp/fast_convert.h" | |
99 #include "spandsp/logging.h" | |
100 #include "spandsp/saturated.h" | |
101 #include "spandsp/dc_restore.h" | |
102 #include "spandsp/bit_operations.h" | |
103 #include "spandsp/echo.h" | |
104 | |
105 #include "spandsp/private/echo.h" | |
106 | |
107 #if !defined(NULL) | |
108 #define NULL (void *) 0 | |
109 #endif | |
110 #if !defined(FALSE) | |
111 #define FALSE 0 | |
112 #endif | |
113 #if !defined(TRUE) | |
114 #define TRUE (!FALSE) | |
115 #endif | |
116 | |
117 #define NONUPDATE_DWELL_TIME 600 /* 600 samples, or 75ms */ | |
118 | |
119 #define MIN_TX_POWER_FOR_ADAPTION 64*64 | |
120 #define MIN_RX_POWER_FOR_ADAPTION 64*64 | |
121 | |
122 static int narrowband_detect(echo_can_state_t *ec) | |
123 { | |
124 int k; | |
125 int i; | |
126 float temp; | |
127 float scale; | |
128 float sf[128]; | |
129 float f_acf[128]; | |
130 int32_t acf[28]; | |
131 int score; | |
132 int len = 32; | |
133 int alen = 9; | |
134 | |
135 k = ec->curr_pos; | |
136 for (i = 0; i < len; i++) | |
137 { | |
138 sf[i] = ec->fir_state.history[k++]; | |
139 if (k >= 256) | |
140 k = 0; | |
141 } | |
142 for (k = 0; k < alen; k++) | |
143 { | |
144 temp = 0; | |
145 for (i = k; i < len; i++) | |
146 temp += sf[i]*sf[i - k]; | |
147 f_acf[k] = temp; | |
148 } | |
149 scale = 0x1FFFFFFF/f_acf[0]; | |
150 for (k = 0; k < alen; k++) | |
151 acf[k] = (int32_t) (f_acf[k]*scale); | |
152 score = 0; | |
153 for (i = 0; i < 9; i++) | |
154 { | |
155 if (ec->last_acf[i] >= 0 && acf[i] >= 0) | |
156 { | |
157 if ((ec->last_acf[i] >> 1) < acf[i] && acf[i] < (ec->last_acf[i] << 1)) | |
158 score++; | |
159 } | |
160 else if (ec->last_acf[i] < 0 && acf[i] < 0) | |
161 { | |
162 if ((ec->last_acf[i] >> 1) > acf[i] && acf[i] > (ec->last_acf[i] << 1)) | |
163 score++; | |
164 } | |
165 } | |
166 memcpy(ec->last_acf, acf, alen*sizeof(ec->last_acf[0])); | |
167 return score; | |
168 } | |
169 | |
170 static __inline__ void lms_adapt(echo_can_state_t *ec, int factor) | |
171 { | |
172 int i; | |
173 | |
174 #if 0 | |
175 mmx_t *mmx_taps; | |
176 mmx_t *mmx_coeffs; | |
177 mmx_t *mmx_hist; | |
178 mmx_t mmx; | |
179 | |
180 mmx.w[0] = | |
181 mmx.w[1] = | |
182 mmx.w[2] = | |
183 mmx.w[3] = factor; | |
184 mmx_hist = (mmx_t *) &fir->history[fir->curr_pos]; | |
185 mmx_taps = (mmx_t *) &fir->taps; | |
186 mmx_coeffs = (mmx_t *) fir->coeffs; | |
187 i = fir->taps; | |
188 movq_m2r(mmx, mm0); | |
189 while (i > 0) | |
190 { | |
191 movq_m2r(mmx_hist[0], mm1); | |
192 movq_m2r(mmx_taps[0], mm0); | |
193 movq_m2r(mmx_taps[1], mm1); | |
194 movq_r2r(mm1, mm2); | |
195 pmulhw(mm0, mm1); | |
196 pmullw(mm0, mm2); | |
197 | |
198 pmaddwd_r2r(mm1, mm0); | |
199 pmaddwd_r2r(mm3, mm2); | |
200 paddd_r2r(mm0, mm4); | |
201 paddd_r2r(mm2, mm4); | |
202 movq_r2m(mm0, mmx_taps[0]); | |
203 movq_r2m(mm1, mmx_taps[0]); | |
204 movq_r2m(mm2, mmx_coeffs[0]); | |
205 mmx_taps += 2; | |
206 mmx_coeffs += 1; | |
207 mmx_hist += 1; | |
208 i -= 4; | |
209 ) | |
210 emms(); | |
211 #elif 0 | |
212 /* Update the FIR taps */ | |
213 for (i = ec->taps - 1; i >= 0; i--) | |
214 { | |
215 /* Leak to avoid the coefficients drifting beyond the ability of the | |
216 adaption process to bring them back under control. */ | |
217 ec->fir_taps32[i] -= (ec->fir_taps32[i] >> 23); | |
218 ec->fir_taps32[i] += (ec->fir_state.history[i + ec->curr_pos]*factor); | |
219 ec->latest_correction = (ec->fir_state.history[i + ec->curr_pos]*factor); | |
220 ec->fir_taps16[ec->tap_set][i] = ec->fir_taps32[i] >> 15; | |
221 } | |
222 #else | |
223 int offset1; | |
224 int offset2; | |
225 | |
226 /* Update the FIR taps */ | |
227 offset2 = ec->curr_pos; | |
228 offset1 = ec->taps - offset2; | |
229 for (i = ec->taps - 1; i >= offset1; i--) | |
230 { | |
231 ec->fir_taps32[i] += (ec->fir_state.history[i - offset1]*factor); | |
232 ec->fir_taps16[ec->tap_set][i] = (int16_t) (ec->fir_taps32[i] >> 15); | |
233 } | |
234 for ( ; i >= 0; i--) | |
235 { | |
236 ec->fir_taps32[i] += (ec->fir_state.history[i + offset2]*factor); | |
237 ec->fir_taps16[ec->tap_set][i] = (int16_t) (ec->fir_taps32[i] >> 15); | |
238 } | |
239 #endif | |
240 } | |
241 /*- End of function --------------------------------------------------------*/ | |
242 | |
243 SPAN_DECLARE(echo_can_state_t *) echo_can_init(int len, int adaption_mode) | |
244 { | |
245 echo_can_state_t *ec; | |
246 int i; | |
247 int j; | |
248 | |
249 if ((ec = (echo_can_state_t *) malloc(sizeof(*ec))) == NULL) | |
250 return NULL; | |
251 memset(ec, 0, sizeof(*ec)); | |
252 ec->taps = len; | |
253 ec->curr_pos = ec->taps - 1; | |
254 ec->tap_mask = ec->taps - 1; | |
255 if ((ec->fir_taps32 = (int32_t *) malloc(ec->taps*sizeof(int32_t))) == NULL) | |
256 { | |
257 free(ec); | |
258 return NULL; | |
259 } | |
260 memset(ec->fir_taps32, 0, ec->taps*sizeof(int32_t)); | |
261 for (i = 0; i < 4; i++) | |
262 { | |
263 if ((ec->fir_taps16[i] = (int16_t *) malloc(ec->taps*sizeof(int16_t))) == NULL) | |
264 { | |
265 for (j = 0; j < i; j++) | |
266 free(ec->fir_taps16[j]); | |
267 free(ec->fir_taps32); | |
268 free(ec); | |
269 return NULL; | |
270 } | |
271 memset(ec->fir_taps16[i], 0, ec->taps*sizeof(int16_t)); | |
272 } | |
273 fir16_create(&ec->fir_state, | |
274 ec->fir_taps16[0], | |
275 ec->taps); | |
276 ec->rx_power_threshold = 10000000; | |
277 ec->geigel_max = 0; | |
278 ec->geigel_lag = 0; | |
279 ec->dtd_onset = FALSE; | |
280 ec->tap_set = 0; | |
281 ec->tap_rotate_counter = 1600; | |
282 ec->cng_level = 1000; | |
283 echo_can_adaption_mode(ec, adaption_mode); | |
284 return ec; | |
285 } | |
286 /*- End of function --------------------------------------------------------*/ | |
287 | |
288 SPAN_DECLARE(int) echo_can_release(echo_can_state_t *ec) | |
289 { | |
290 return 0; | |
291 } | |
292 /*- End of function --------------------------------------------------------*/ | |
293 | |
294 SPAN_DECLARE(int) echo_can_free(echo_can_state_t *ec) | |
295 { | |
296 int i; | |
297 | |
298 fir16_free(&ec->fir_state); | |
299 free(ec->fir_taps32); | |
300 for (i = 0; i < 4; i++) | |
301 free(ec->fir_taps16[i]); | |
302 free(ec); | |
303 return 0; | |
304 } | |
305 /*- End of function --------------------------------------------------------*/ | |
306 | |
307 SPAN_DECLARE(void) echo_can_adaption_mode(echo_can_state_t *ec, int adaption_mode) | |
308 { | |
309 ec->adaption_mode = adaption_mode; | |
310 } | |
311 /*- End of function --------------------------------------------------------*/ | |
312 | |
313 SPAN_DECLARE(void) echo_can_flush(echo_can_state_t *ec) | |
314 { | |
315 int i; | |
316 | |
317 for (i = 0; i < 4; i++) | |
318 ec->tx_power[i] = 0; | |
319 for (i = 0; i < 3; i++) | |
320 ec->rx_power[i] = 0; | |
321 ec->clean_rx_power = 0; | |
322 ec->nonupdate_dwell = 0; | |
323 | |
324 fir16_flush(&ec->fir_state); | |
325 ec->fir_state.curr_pos = ec->taps - 1; | |
326 memset(ec->fir_taps32, 0, ec->taps*sizeof(int32_t)); | |
327 for (i = 0; i < 4; i++) | |
328 memset(ec->fir_taps16[i], 0, ec->taps*sizeof(int16_t)); | |
329 | |
330 ec->curr_pos = ec->taps - 1; | |
331 | |
332 ec->supp_test1 = 0; | |
333 ec->supp_test2 = 0; | |
334 ec->supp1 = 0; | |
335 ec->supp2 = 0; | |
336 ec->vad = 0; | |
337 ec->cng_level = 1000; | |
338 ec->cng_filter = 0; | |
339 | |
340 ec->geigel_max = 0; | |
341 ec->geigel_lag = 0; | |
342 ec->dtd_onset = FALSE; | |
343 ec->tap_set = 0; | |
344 ec->tap_rotate_counter = 1600; | |
345 | |
346 ec->latest_correction = 0; | |
347 | |
348 memset(ec->last_acf, 0, sizeof(ec->last_acf)); | |
349 ec->narrowband_count = 0; | |
350 ec->narrowband_score = 0; | |
351 } | |
352 /*- End of function --------------------------------------------------------*/ | |
353 | |
354 int sample_no = 0; | |
355 | |
356 SPAN_DECLARE(void) echo_can_snapshot(echo_can_state_t *ec) | |
357 { | |
358 memcpy(ec->snapshot, ec->fir_taps16[0], ec->taps*sizeof(int16_t)); | |
359 } | |
360 /*- End of function --------------------------------------------------------*/ | |
361 | |
362 static __inline__ int16_t echo_can_hpf(int32_t coeff[2], int16_t amp) | |
363 { | |
364 int32_t z; | |
365 | |
366 /* | |
367 Filter DC, 3dB point is 160Hz (I think), note 32 bit precision required | |
368 otherwise values do not track down to 0. Zero at DC, Pole at (1-Beta) | |
369 only real axis. Some chip sets (like Si labs) don't need | |
370 this, but something like a $10 X100P card does. Any DC really slows | |
371 down convergence. | |
372 | |
373 Note: removes some low frequency from the signal, this reduces | |
374 the speech quality when listening to samples through headphones | |
375 but may not be obvious through a telephone handset. | |
376 | |
377 Note that the 3dB frequency in radians is approx Beta, e.g. for | |
378 Beta = 2^(-3) = 0.125, 3dB freq is 0.125 rads = 159Hz. | |
379 | |
380 This is one of the classic DC removal filters, adjusted to provide sufficient | |
381 bass rolloff to meet the above requirement to protect hybrids from things that | |
382 upset them. The difference between successive samples produces a lousy HPF, and | |
383 then a suitably placed pole flattens things out. The final result is a nicely | |
384 rolled off bass end. The filtering is implemented with extended fractional | |
385 precision, which noise shapes things, giving very clean DC removal. | |
386 | |
387 Make sure the gain of the HPF is 1.0. The first can still saturate a little under | |
388 impulse conditions, and it might roll to 32768 and need clipping on sustained peak | |
389 level signals. However, the scale of such clipping is small, and the error due to | |
390 any saturation should not markedly affect the downstream processing. */ | |
391 z = amp << 15; | |
392 z -= (z >> 4); | |
393 coeff[0] += z - (coeff[0] >> 3) - coeff[1]; | |
394 coeff[1] = z; | |
395 z = coeff[0] >> 15; | |
396 | |
397 return saturate(z); | |
398 } | |
399 /*- End of function --------------------------------------------------------*/ | |
400 | |
401 SPAN_DECLARE(int16_t) echo_can_update(echo_can_state_t *ec, int16_t tx, int16_t rx) | |
402 { | |
403 int32_t echo_value; | |
404 int clean_rx; | |
405 int nsuppr; | |
406 int score; | |
407 int i; | |
408 | |
409 sample_no++; | |
410 if (ec->adaption_mode & ECHO_CAN_USE_RX_HPF) | |
411 rx = echo_can_hpf(ec->rx_hpf, rx); | |
412 | |
413 ec->latest_correction = 0; | |
414 /* Evaluate the echo - i.e. apply the FIR filter */ | |
415 /* Assume the gain of the FIR does not exceed unity. Exceeding unity | |
416 would seem like a rather poor thing for an echo cancellor to do :) | |
417 This means we can compute the result with a total disregard for | |
418 overflows. 16bits x 16bits -> 31bits, so no overflow can occur in | |
419 any multiply. While accumulating we may overflow and underflow the | |
420 32 bit scale often. However, if the gain does not exceed unity, | |
421 everything should work itself out, and the final result will be | |
422 OK, without any saturation logic. */ | |
423 /* Overflow is very much possible here, and we do nothing about it because | |
424 of the compute costs */ | |
425 /* 16 bit coeffs for the LMS give lousy results (maths good, actual sound | |
426 bad!), but 32 bit coeffs require some shifting. On balance 32 bit seems | |
427 best */ | |
428 echo_value = fir16(&ec->fir_state, tx); | |
429 | |
430 /* And the answer is..... */ | |
431 clean_rx = rx - echo_value; | |
432 printf("echo is %" PRId32 "\n", echo_value); | |
433 /* That was the easy part. Now we need to adapt! */ | |
434 if (ec->nonupdate_dwell > 0) | |
435 ec->nonupdate_dwell--; | |
436 | |
437 /* Calculate short term power levels using very simple single pole IIRs */ | |
438 /* TODO: Is the nasty modulus approach the fastest, or would a real | |
439 tx*tx power calculation actually be faster? Using the squares | |
440 makes the numbers grow a lot! */ | |
441 ec->tx_power[3] += ((abs(tx) - ec->tx_power[3]) >> 5); | |
442 ec->tx_power[2] += ((tx*tx - ec->tx_power[2]) >> 8); | |
443 ec->tx_power[1] += ((tx*tx - ec->tx_power[1]) >> 5); | |
444 ec->tx_power[0] += ((tx*tx - ec->tx_power[0]) >> 3); | |
445 ec->rx_power[1] += ((rx*rx - ec->rx_power[1]) >> 6); | |
446 ec->rx_power[0] += ((rx*rx - ec->rx_power[0]) >> 3); | |
447 ec->clean_rx_power += ((clean_rx*clean_rx - ec->clean_rx_power) >> 6); | |
448 | |
449 score = 0; | |
450 /* If there is very little being transmitted, any attempt to train is | |
451 futile. We would either be training on the far end's noise or signal, | |
452 the channel's own noise, or our noise. Either way, this is hardly good | |
453 training, so don't do it (avoid trouble). */ | |
454 if (ec->tx_power[0] > MIN_TX_POWER_FOR_ADAPTION) | |
455 { | |
456 /* If the received power is very low, either we are sending very little or | |
457 we are already well adapted. There is little point in trying to improve | |
458 the adaption under these circumstances, so don't do it (reduce the | |
459 compute load). */ | |
460 if (ec->tx_power[1] > ec->rx_power[0]) | |
461 { | |
462 /* There is no (or little) far-end speech. */ | |
463 if (ec->nonupdate_dwell == 0) | |
464 { | |
465 if (++ec->narrowband_count >= 160) | |
466 { | |
467 ec->narrowband_count = 0; | |
468 score = narrowband_detect(ec); | |
469 printf("Do the narrowband test %d at %d\n", score, ec->curr_pos); | |
470 if (score > 6) | |
471 { | |
472 if (ec->narrowband_score == 0) | |
473 memcpy(ec->fir_taps16[3], ec->fir_taps16[(ec->tap_set + 1)%3], ec->taps*sizeof(int16_t)); | |
474 ec->narrowband_score += score; | |
475 } | |
476 else | |
477 { | |
478 if (ec->narrowband_score > 200) | |
479 { | |
480 printf("Revert to %d at %d\n", (ec->tap_set + 1)%3, sample_no); | |
481 memcpy(ec->fir_taps16[ec->tap_set], ec->fir_taps16[3], ec->taps*sizeof(int16_t)); | |
482 memcpy(ec->fir_taps16[(ec->tap_set - 1)%3], ec->fir_taps16[3], ec->taps*sizeof(int16_t)); | |
483 for (i = 0; i < ec->taps; i++) | |
484 ec->fir_taps32[i] = ec->fir_taps16[3][i] << 15; | |
485 ec->tap_rotate_counter = 1600; | |
486 } | |
487 ec->narrowband_score = 0; | |
488 } | |
489 } | |
490 ec->dtd_onset = FALSE; | |
491 if (--ec->tap_rotate_counter <= 0) | |
492 { | |
493 printf("Rotate to %d at %d\n", ec->tap_set, sample_no); | |
494 ec->tap_rotate_counter = 1600; | |
495 ec->tap_set++; | |
496 if (ec->tap_set > 2) | |
497 ec->tap_set = 0; | |
498 ec->fir_state.coeffs = ec->fir_taps16[ec->tap_set]; | |
499 } | |
500 /* ... and we are not in the dwell time from previous speech. */ | |
501 if ((ec->adaption_mode & ECHO_CAN_USE_ADAPTION) && ec->narrowband_score == 0) | |
502 { | |
503 //nsuppr = saturate((clean_rx << 16)/ec->tx_power[1]); | |
504 //nsuppr = clean_rx/ec->tx_power[1]; | |
505 /* If a sudden surge in signal level (e.g. the onset of a tone | |
506 burst) cause an abnormally high instantaneous to average | |
507 signal power ratio, we could kick the adaption badly in the | |
508 wrong direction. This is because the tx_power takes too long | |
509 to react and rise. We need to stop too rapid adaption to the | |
510 new signal. We normalise to a value derived from the | |
511 instantaneous signal if it exceeds the peak by too much. */ | |
512 nsuppr = clean_rx; | |
513 /* Divide isn't very quick, but the "where is the top bit" and shift | |
514 instructions are single cycle. */ | |
515 if (tx > 4*ec->tx_power[3]) | |
516 i = top_bit(tx) - 8; | |
517 else | |
518 i = top_bit(ec->tx_power[3]) - 8; | |
519 if (i > 0) | |
520 nsuppr >>= i; | |
521 lms_adapt(ec, nsuppr); | |
522 } | |
523 } | |
524 //printf("%10d %10d %10d %10d %10d\n", rx, clean_rx, nsuppr, ec->tx_power[1], ec->rx_power[1]); | |
525 //printf("%.4f\n", (float) ec->rx_power[1]/(float) ec->clean_rx_power); | |
526 } | |
527 else | |
528 { | |
529 if (!ec->dtd_onset) | |
530 { | |
531 printf("Revert to %d at %d\n", (ec->tap_set + 1)%3, sample_no); | |
532 memcpy(ec->fir_taps16[ec->tap_set], ec->fir_taps16[(ec->tap_set + 1)%3], ec->taps*sizeof(int16_t)); | |
533 memcpy(ec->fir_taps16[(ec->tap_set - 1)%3], ec->fir_taps16[(ec->tap_set + 1)%3], ec->taps*sizeof(int16_t)); | |
534 for (i = 0; i < ec->taps; i++) | |
535 ec->fir_taps32[i] = ec->fir_taps16[(ec->tap_set + 1)%3][i] << 15; | |
536 ec->tap_rotate_counter = 1600; | |
537 ec->dtd_onset = TRUE; | |
538 } | |
539 ec->nonupdate_dwell = NONUPDATE_DWELL_TIME; | |
540 } | |
541 } | |
542 | |
543 if (ec->rx_power[1]) | |
544 ec->vad = (8000*ec->clean_rx_power)/ec->rx_power[1]; | |
545 else | |
546 ec->vad = 0; | |
547 if (ec->rx_power[1] > 2048*2048 && ec->clean_rx_power > 4*ec->rx_power[1]) | |
548 { | |
549 /* The EC seems to be making things worse, instead of better. Zap it! */ | |
550 memset(ec->fir_taps32, 0, ec->taps*sizeof(int32_t)); | |
551 for (i = 0; i < 4; i++) | |
552 memset(ec->fir_taps16[i], 0, ec->taps*sizeof(int16_t)); | |
553 } | |
554 | |
555 #if defined(XYZZY) | |
556 if ((ec->adaption_mode & ECHO_CAN_USE_SUPPRESSOR)) | |
557 { | |
558 ec->supp_test1 += (ec->fir_state.history[ec->curr_pos] - ec->fir_state.history[(ec->curr_pos - 7) & ec->tap_mask]); | |
559 ec->supp_test2 += (ec->fir_state.history[(ec->curr_pos - 24) & ec->tap_mask] - ec->fir_state.history[(ec->curr_pos - 31) & ec->tap_mask]); | |
560 if (ec->supp_test1 > 42 && ec->supp_test2 > 42) | |
561 supp_change = 25; | |
562 else | |
563 supp_change = 50; | |
564 supp = supp_change + k1*ec->supp1 + k2*ec->supp2; | |
565 ec->supp2 = ec->supp1; | |
566 ec->supp1 = supp; | |
567 clean_rx *= (1 - supp); | |
568 } | |
569 #endif | |
570 | |
571 if ((ec->adaption_mode & ECHO_CAN_USE_NLP)) | |
572 { | |
573 /* Non-linear processor - a fancy way to say "zap small signals, to avoid | |
574 residual echo due to (uLaw/ALaw) non-linearity in the channel.". */ | |
575 if (ec->rx_power[1] < 30000000) | |
576 { | |
577 if (!ec->cng) | |
578 { | |
579 ec->cng_level = ec->clean_rx_power; | |
580 ec->cng = TRUE; | |
581 } | |
582 if ((ec->adaption_mode & ECHO_CAN_USE_CNG)) | |
583 { | |
584 /* Very elementary comfort noise generation */ | |
585 /* Just random numbers rolled off very vaguely Hoth-like */ | |
586 ec->cng_rndnum = 1664525U*ec->cng_rndnum + 1013904223U; | |
587 ec->cng_filter = ((ec->cng_rndnum & 0xFFFF) - 32768 + 5*ec->cng_filter) >> 3; | |
588 clean_rx = (ec->cng_filter*ec->cng_level) >> 17; | |
589 /* TODO: A better CNG, with more accurate (tracking) spectral shaping! */ | |
590 } | |
591 else | |
592 { | |
593 clean_rx = 0; | |
594 } | |
595 //clean_rx = -16000; | |
596 } | |
597 else | |
598 { | |
599 ec->cng = FALSE; | |
600 } | |
601 } | |
602 else | |
603 { | |
604 ec->cng = FALSE; | |
605 } | |
606 | |
607 printf("Narrowband score %4d %5d at %d\n", ec->narrowband_score, score, sample_no); | |
608 /* Roll around the rolling buffer */ | |
609 if (ec->curr_pos <= 0) | |
610 ec->curr_pos = ec->taps; | |
611 ec->curr_pos--; | |
612 return (int16_t) clean_rx; | |
613 } | |
614 /*- End of function --------------------------------------------------------*/ | |
615 | |
616 SPAN_DECLARE(int16_t) echo_can_hpf_tx(echo_can_state_t *ec, int16_t tx) | |
617 { | |
618 if (ec->adaption_mode & ECHO_CAN_USE_TX_HPF) | |
619 tx = echo_can_hpf(ec->tx_hpf, tx); | |
620 return tx; | |
621 } | |
622 /*- End of function --------------------------------------------------------*/ | |
623 /*- End of file ------------------------------------------------------------*/ |