Mercurial > hg > audiostuff

comparison intercom/aec.cpp @ 2:13be24d74cd2

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
import intercom-0.4.1
author Peter Meerwald <pmeerw@cosy.sbg.ac.at>
date Fri, 25 Jun 2010 09:57:52 +0200
parents
children
comparison
equal deleted inserted replaced
1:9cadc470e3da 2:13be24d74cd2
1 /* aec.cpp
2 *
3 * Copyright (C) DFS Deutsche Flugsicherung (2004, 2005).
4 * All Rights Reserved.
5 *
6 * Acoustic Echo Cancellation NLMS-pw algorithm
7 *
8 * Version 0.3 filter created with www.dsptutor.freeuk.com
9 * Version 0.3.1 Allow change of stability parameter delta
10 * Version 0.4 Leaky Normalized LMS - pre whitening algorithm
11 */
12
13 #include <stdio.h>
14 #include <stdlib.h>
15 #include <math.h>
16 #include <string.h>
17 #include <unistd.h>
18
19 #include "oss.h"
20 #include "aec.h"
21 #include "intercomd.h"
22
23 #include "tcp.h"
24
25 /* Vector Dot Product */
26 REAL dotp(REAL a[], REAL b[])
27 {
28 REAL sum0 = 0.0, sum1 = 0.0;
29 int j;
30
31 for (j = 0; j < NLMS_LEN; j += 2) {
32 // optimize: partial loop unrolling
33 sum0 += a[j] * b[j];
34 sum1 += a[j + 1] * b[j + 1];
35 }
36 return sum0 + sum1;
37 }
38
39
40 AEC::AEC()
41 {
42 hangover = 0;
43 memset(x, 0, sizeof(x));
44 memset(xf, 0, sizeof(xf));
45 memset(w, 0, sizeof(w));
46 j = NLMS_EXT;
47 delta = 0.0f;
48 setambient(NoiseFloor);
49 dfast = dslow = M75dB_PCM;
50 xfast = xslow = M80dB_PCM;
51 gain = 1.0f;
52 Fx.init(2000.0f/RATE);
53 Fe.init(2000.0f/RATE);
54
55 aes_y2 = M0dB;
56
57 fdwdisplay = -1;
58 dumpcnt = 0;
59 memset(ws, 0, sizeof(ws));
60 }
61
62 // Adrian soft decision DTD
63 // (Dual Average Near-End to Far-End signal Ratio DTD)
64 // This algorithm uses exponential smoothing with differnt
65 // ageing parameters to get fast and slow near-end and far-end
66 // signal averages. The ratio of NFRs term
67 // (dfast / xfast) / (dslow / xslow) is used to compute the stepsize
68 // A ratio value of 2.5 is mapped to stepsize 0, a ratio of 0 is
69 // mapped to 1.0 with a limited linear function.
70 inline float AEC::dtd(REAL d, REAL x)
71 {
72 float stepsize;
73
74 // fast near-end and far-end average
75 dfast += ALPHAFAST * (fabsf(d) - dfast);
76 xfast += ALPHAFAST * (fabsf(x) - xfast);
77
78 // slow near-end and far-end average
79 dslow += ALPHASLOW * (fabsf(d) - dslow);
80 xslow += ALPHASLOW * (fabsf(x) - xslow);
81
82 if (xfast < M70dB_PCM) {
83 return 0.0; // no Spk signal
84 }
85
86 if (dfast < M70dB_PCM) {
87 return 0.0; // no Mic signal
88 }
89
90 // ratio of NFRs
91 float ratio = (dfast * xslow) / (dslow * xfast);
92
93 // begrenzte lineare Kennlinie
94 const float M = (STEPY2 - STEPY1) / (STEPX2 - STEPX1);
95 if (ratio < STEPX1) {
96 stepsize = STEPY1;
97 } else if (ratio > STEPX2) {
98 stepsize = STEPY2;
99 } else {
100 // Punktrichtungsform einer Geraden
101 stepsize = M * (ratio - STEPX1) + STEPY1;
102 }
103
104 return stepsize;
105 }
106
107
108 inline void AEC::leaky()
109 // The xfast signal is used to charge the hangover timer to Thold.
110 // When hangover expires (no Spk signal for some time) the vector w
111 // is erased. This is my implementation of Leaky NLMS.
112 {
113 if (xfast >= M70dB_PCM) {
114 // vector w is valid for hangover Thold time
115 hangover = Thold;
116 } else {
117 if (hangover > 1) {
118 --hangover;
119 } else if (1 == hangover) {
120 --hangover;
121 // My Leaky NLMS is to erase vector w when hangover expires
122 memset(w, 0, sizeof(w));
123 }
124 }
125 }
126
127
128 void AEC::openwdisplay() {
129 // open TCP connection to program wdisplay.tcl
130 fdwdisplay = socket_async("127.0.0.1", 50999);
131 };
132
133
134 inline REAL AEC::nlms_pw(REAL d, REAL x_, float stepsize)
135 {
136 x[j] = x_;
137 xf[j] = Fx.highpass(x_); // pre-whitening of x
138
139 // calculate error value
140 // (mic signal - estimated mic signal from spk signal)
141 REAL e = d;
142 if (hangover > 0) {
143 e -= dotp(w, x + j);
144 }
145 REAL ef = Fe.highpass(e); // pre-whitening of e
146
147 // optimize: iterative dotp(xf, xf)
148 dotp_xf_xf += (xf[j] * xf[j] - xf[j + NLMS_LEN - 1] * xf[j + NLMS_LEN - 1]);
149
150 if (stepsize > 0.0) {
151 // calculate variable step size
152 REAL mikro_ef = stepsize * ef / dotp_xf_xf;
153
154 // update tap weights (filter learning)
155 int i;
156 for (i = 0; i < NLMS_LEN; i += 2) {
157 // optimize: partial loop unrolling
158 w[i] += mikro_ef * xf[i + j];
159 w[i + 1] += mikro_ef * xf[i + j + 1];
160 }
161 }
162
163 if (--j < 0) {
164 // optimize: decrease number of memory copies
165 j = NLMS_EXT;
166 memmove(x + j + 1, x, (NLMS_LEN - 1) * sizeof(REAL));
167 memmove(xf + j + 1, xf, (NLMS_LEN - 1) * sizeof(REAL));
168 }
169
170 // Saturation
171 if (e > MAXPCM) {
172 return MAXPCM;
173 } else if (e < -MAXPCM) {
174 return -MAXPCM;
175 } else {
176 return e;
177 }
178 }
179
180
181 int AEC::doAEC(int d_, int x_)
182 {
183 REAL d = (REAL) d_;
184 REAL x = (REAL) x_;
185
186 // Mic Highpass Filter - to remove DC
187 d = acMic.highpass(d);
188
189 // Mic Highpass Filter - cut-off below 300Hz
190 d = cutoff.highpass(d);
191
192 // Amplify, for e.g. Soundcards with -6dB max. volume
193 d *= gain;
194
195 // Spk Highpass Filter - to remove DC
196 x = acSpk.highpass(x);
197
198 // Double Talk Detector
199 stepsize = dtd(d, x);
200
201 // Leaky (ageing of vector w)
202 leaky();
203
204 // Acoustic Echo Cancellation
205 d = nlms_pw(d, x, stepsize);
206
207 if (fdwdisplay >= 0) {
208 if (++dumpcnt >= (WIDEB*8000/10)) {
209 // wdisplay creates 10 dumps per seconds = large CPU load!
210 dumpcnt = 0;
211 write(fdwdisplay, ws, DUMP_LEN*sizeof(float));
212 // we don't check return value. This is not production quality!!!
213 memset(ws, 0, sizeof(ws));
214 } else {
215 int i;
216 for (i = 0; i < DUMP_LEN; i += 2) {
217 // optimize: partial loop unrolling
218 ws[i] += w[i];
219 ws[i + 1] += w[i + 1];
220 }
221 }
222 }
223
224 return (int) d;
225 }

Repositories maintained by Peter Meerwald, pmeerw@pmeerw.net.