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 ------------------------------------------------------------*/ |
