|
2
|
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 }
|
