1 /*
2 * Copyright (C) 2002-2003 Fhg Fokus
3 *
4 * This file is part of SEMS, a free SIP media server.
5 *
6 * SEMS is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * For a license to use the sems software under conditions
12 * other than those described here, or to purchase support for this
13 * software, please contact iptel.org by e-mail at the following addresses:
14 * info@iptel.org
15 *
16 * SEMS is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU General Public License for more details.
20 *
21 * You should have received a copy of the GNU General Public License
22 * along with this program; if not, write to the Free Software
23 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
24 */
25
26 #include "LowcFE.h"
27
28 #include <math.h>
29 #include <stdio.h>
30 #include <stdlib.h>
31
convertsf(short * f,Float * t,int cnt)32 void LowcFE::convertsf(short *f, Float *t, int cnt)
33 {
34 for (int i = 0; i < cnt; i++)
35 t[i] = (Float)f[i];
36 }
37
convertfs(Float * f,short * t,int cnt)38 void LowcFE::convertfs(Float *f, short *t, int cnt)
39 {
40 for (int i = 0; i < cnt; i++){
41 t[i] = (short)f[i];
42 }
43 }
44
copyf(Float * f,Float * t,int cnt)45 void LowcFE::copyf(Float *f, Float *t, int cnt)
46 {
47 for (int i = 0; i < cnt; i++)
48 t[i] = f[i];
49 }
50
copys(short * f,short * t,int cnt)51 void LowcFE::copys(short *f, short *t, int cnt)
52 {
53 for (int i = 0; i < cnt; i++)
54 t[i] = f[i];
55 }
56
zeros(short * s,int cnt)57 void LowcFE::zeros(short *s, int cnt)
58 {
59 for (int i = 0; i < cnt; i++)
60 s[i] = 0;
61 }
62
LowcFE(unsigned int sample_rate)63 LowcFE::LowcFE(unsigned int sample_rate)
64 : sample_rate(sample_rate), erasecnt(0), pitchbufend(0)
65 {
66 pitchbuf = new Float[HISTORYLEN];
67 lastq = new Float[POVERLAPMAX];
68 history = new short[HISTORYLEN];
69 memset(pitchbuf, 0, sizeof(Float) * HISTORYLEN);
70 memset(lastq, 0, sizeof(Float) * POVERLAPMAX);
71 zeros(history, HISTORYLEN);
72 }
73
~LowcFE()74 LowcFE::~LowcFE()
75 {
76 delete[] history;
77 delete[] lastq;
78 delete[] pitchbuf;
79 }
80
81 /*
82 * Save a frames worth of new speech in the history buffer.
83 * Return the output speech delayed by POVERLAPMAX.
84 */
savespeech(short * s)85 void LowcFE::savespeech(short *s)
86 {
87 /* make room for new signal */
88 copys(&history[FRAMESZ], history, HISTORYLEN - FRAMESZ);
89 /* copy in the new frame */
90 copys(s, &history[HISTORYLEN - FRAMESZ], FRAMESZ);
91 /* copy out the delayed frame */
92 copys(&history[HISTORYLEN - FRAMESZ - POVERLAPMAX], s, FRAMESZ);
93 }
94
95 /*
96 * A good frame was received and decoded.
97 * If right after an erasure, do an overlap add with the synthetic signal.
98 * Add the frame to history buffer.
99 */
addtohistory(short * s)100 void LowcFE::addtohistory(short *s)
101 {
102 if (erasecnt) {
103 short overlapbuf[FRAMESZ];
104 /*
105 * longer erasures require longer overlaps
106 * to smooth the transition between the synthetic
107 * and real signal.
108 */
109 unsigned int olen = poverlap + (erasecnt - 1) * EOVERLAPINCR;
110 if (olen > FRAMESZ)
111 olen = FRAMESZ;
112 getfespeech(overlapbuf, olen);
113 overlapaddatend(s, overlapbuf, olen);
114 erasecnt = 0;
115 }
116 savespeech(s);
117 }
118
119 /*
120 * Generate the synthetic signal.
121 * At the beginning of an erasure determine the pitch, and extract
122 * one pitch period from the tail of the signal. Do an OLA for 1/4
123 * of the pitch to smooth the signal. Then repeat the extracted signal
124 * for the length of the erasure. If the erasure continues for more than
125 * 10 ms, increase the number of periods in the pitchbuffer. At the end
126 * of an erasure, do an OLA with the start of the first good frame.
127 * The gain decays as the erasure gets longer.
128 */
dofe(short * out)129 void LowcFE::dofe(short *out)
130 {
131 pitchbufend = &pitchbuf[HISTORYLEN];
132
133 if (erasecnt == 0) {
134 convertsf(history, pitchbuf, HISTORYLEN); /* get history */
135 pitch = findpitch(); /* find pitch */
136 poverlap = pitch >> 2; /* OLA 1/4 wavelength */
137 /* save original last poverlap samples */
138 copyf(pitchbufend - poverlap, lastq, poverlap);
139 poffset = 0; /* create pitch buffer with 1 period */
140 pitchblen = pitch;
141 pitchbufstart = pitchbufend - pitchblen;
142 overlapadd(lastq, pitchbufstart - poverlap,
143 pitchbufend - poverlap, poverlap);
144 /* update last 1/4 wavelength in history buffer */
145 convertfs(pitchbufend - poverlap, &history[HISTORYLEN-poverlap],
146 poverlap);
147 getfespeech(out, FRAMESZ); /* get synthesized speech */
148 } else if (erasecnt == 1 || erasecnt == 2) {
149 /* tail of previous pitch estimate */
150 short tmp[POVERLAPMAX];
151 int saveoffset = poffset; /* save offset for OLA */
152 getfespeech(tmp, poverlap); /* continue with old pitchbuf */
153 /* add periods to the pitch buffer */
154 poffset = saveoffset;
155 while (poffset > pitch)
156 poffset -= pitch;
157 pitchblen += pitch; /* add a period */
158 pitchbufstart = pitchbufend - pitchblen;
159 overlapadd(lastq, pitchbufstart - poverlap,
160 pitchbufend - poverlap, poverlap);
161 /* overlap add old pitchbuffer with new */
162 getfespeech(out, FRAMESZ);
163 overlapadd(tmp, out, out, poverlap);
164 scalespeech(out);
165 } else if (erasecnt > 5) {
166 zeros(out, FRAMESZ);
167 } else {
168 getfespeech(out, FRAMESZ);
169 scalespeech(out);
170 }
171 erasecnt++;
172 savespeech(out);
173 }
174
175 /*
176 * Estimate the pitch.
177 * l - pointer to first sample in last 20 msec of speech.
178 * r - points to the sample PITCH_MAX before l
179 */
findpitch()180 int LowcFE::findpitch()
181 {
182 int i, j, k;
183 int bestmatch;
184 Float bestcorr;
185 Float corr; /* correlation */
186 Float energy; /* running energy */
187 Float scale; /* scale correlation by average power */
188 Float *rp; /* segment to match */
189 Float *l = pitchbufend - CORRLEN;
190 Float *r = pitchbufend - CORRBUFLEN;
191
192 /* coarse search */
193 rp = r;
194 energy = 0.f;
195 corr = 0.f;
196 for (i = 0; i < (int)(CORRLEN); i += NDEC) {
197 energy += rp[i] * rp[i];
198 corr += rp[i] * l[i];
199 }
200 scale = energy;
201 if (scale < CORRMINPOWER){
202 scale = CORRMINPOWER;
203 }
204 corr = corr / (Float)sqrt(scale);
205 bestcorr = corr;
206 bestmatch = 0;
207 for (j = NDEC; j <= (int)(PITCHDIFF); j += NDEC) {
208 energy -= rp[0] * rp[0];
209 energy += rp[CORRLEN] * rp[CORRLEN];
210 rp += NDEC;
211 corr = 0.f;
212 for (i = 0; i < (int)(CORRLEN); i += NDEC)
213 corr += rp[i] * l[i];
214 scale = energy;
215 if (scale < CORRMINPOWER)
216 scale = CORRMINPOWER;
217 corr /= (Float)sqrt(scale);
218 if (corr >= bestcorr) {
219 bestcorr = corr;
220 bestmatch = j;
221 }
222 }
223 /* fine search */
224 j = bestmatch - (NDEC - 1);
225 if (j < 0)
226 j = 0;
227 k = bestmatch + (NDEC - 1);
228 if (k > (int)(PITCHDIFF))
229 k = PITCHDIFF;
230 rp = &r[j];
231 energy = 0.f;
232 corr = 0.f;
233 for (i = 0; i < (int)(CORRLEN); i++) {
234 energy += rp[i] * rp[i];
235 corr += rp[i] * l[i];
236 }
237 scale = energy;
238 if (scale < CORRMINPOWER)
239 scale = CORRMINPOWER;
240
241 corr = corr / (Float)sqrt(scale);
242 bestcorr = corr;
243 bestmatch = j;
244 for (j++; j <= k; j++) {
245 energy -= rp[0] * rp[0];
246 energy += rp[CORRLEN] * rp[CORRLEN];
247 rp++;
248 corr = 0.f;
249 for (i = 0; i < (int)(CORRLEN); i++)
250 corr += rp[i] * l[i];
251 scale = energy;
252 if (scale < CORRMINPOWER)
253 scale = CORRMINPOWER;
254 corr = corr / (Float)sqrt(scale);
255 if (corr > bestcorr) {
256 bestcorr = corr;
257 bestmatch = j;
258 }
259 }
260 return PITCH_MAX - bestmatch;
261 }
262
263 /*
264 * Get samples from the circular pitch buffer. Update poffset so
265 * when subsequent frames are erased the signal continues.
266 */
getfespeech(short * out,int sz)267 void LowcFE::getfespeech(short *out, int sz)
268 {
269 while (sz) {
270 int cnt = pitchblen - poffset;
271 if (cnt > sz)
272 cnt = sz;
273 convertfs(&pitchbufstart[poffset], out, cnt);
274 poffset += cnt;
275 if (poffset == pitchblen)
276 poffset = 0;
277 out += cnt;
278 sz -= cnt;
279 }
280 }
281
scalespeech(short * out)282 void LowcFE::scalespeech(short *out)
283 {
284 Float g = (Float)1. - (erasecnt - 1) * ATTENFAC;
285 for (unsigned int i = 0; i < FRAMESZ; i++) {
286 out[i] = (short)(out[i] * g);
287 g -= ATTENINCR;
288 }
289 }
290
291 /*
292 * Overlap add left and right sides
293 */
overlapadd(Float * l,Float * r,Float * o,int cnt)294 void LowcFE::overlapadd(Float *l, Float *r, Float *o, int cnt)
295 {
296 Float incr = (Float)1. / cnt;
297 Float lw = (Float)1. - incr;
298 Float rw = incr;
299 for (int i = 0; i < cnt; i++) {
300 Float t = lw * l[i] + rw * r[i];
301 if (t > 32767.)
302 t = 32767.;
303 else if (t < -32768.)
304 t = -32768.;
305 o[i] = t;
306 lw -= incr;
307 rw += incr;
308 }
309 }
310
overlapadd(short * l,short * r,short * o,int cnt)311 void LowcFE::overlapadd(short *l, short *r, short *o, int cnt)
312 {
313 Float incr = (Float)1. / cnt;
314 Float lw = (Float)1. - incr;
315 Float rw = incr;
316 for (int i = 0; i < cnt; i++) {
317 Float t = lw * l[i] + rw * r[i];
318 if (t > 32767.)
319 t = 32767.;
320 else if (t < -32768.)
321 t = -32768.;
322 o[i] = (short)t;
323 lw -= incr;
324 rw += incr;
325 }
326 }
327
328 /*
329 * Overlap add the erasure tail with the start of the first good frame
330 * Scale the synthetic speech by the gain factor before the OLA.
331 */
overlapaddatend(short * s,short * f,int cnt)332 void LowcFE::overlapaddatend(short *s, short *f, int cnt)
333 {
334 Float incr = (Float)1. / cnt;
335 Float gain = (Float)1. - (erasecnt - 1) * ATTENFAC;
336 if (gain < 0.)
337 gain = (Float)0.;
338 Float incrg = incr * gain;
339 Float lw = ((Float)1. - incr) * gain;
340 Float rw = incr;
341 for (int i = 0; i < cnt; i++) {
342 Float t = lw * f[i] + rw * s[i];
343 if (t > 32767.)
344 t = 32767.;
345 else if (t < -32768.)
346 t = -32768.;
347 s[i] = (short)t;
348 lw -= incrg;
349 rw += incr;
350 }
351 }
352
353
354