1 /*
2 * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
3 *
4 * Use of this source code is governed by a BSD-style license
5 * that can be found in the LICENSE file in the root of the source
6 * tree. An additional intellectual property rights grant can be found
7 * in the file PATENTS. All contributing project authors may
8 * be found in the AUTHORS file in the root of the source tree.
9 */
10
11 #include "pitch_estimator.h"
12 #include "os_specific_inline.h"
13
14 #include <stdlib.h>
15 #include <memory.h>
16 #include <math.h>
17
18 static const double kDampFilter[PITCH_DAMPORDER] = {-0.07, 0.25, 0.64, 0.25, -0.07};
19
20 /* interpolation coefficients; generated by design_pitch_filter.m */
21 static const double kIntrpCoef[PITCH_FRACS][PITCH_FRACORDER] = {
22 {-0.02239172458614, 0.06653315052934, -0.16515880017569, 0.60701333734125, 0.64671399919202, -0.20249000396417, 0.09926548334755, -0.04765933793109, 0.01754159521746},
23 {-0.01985640750434, 0.05816126837866, -0.13991265473714, 0.44560418147643, 0.79117042386876, -0.20266133815188, 0.09585268418555, -0.04533310458084, 0.01654127246314},
24 {-0.01463300534216, 0.04229888475060, -0.09897034715253, 0.28284326017787, 0.90385267956632, -0.16976950138649, 0.07704272393639, -0.03584218578311, 0.01295781500709},
25 {-0.00764851320885, 0.02184035544377, -0.04985561057281, 0.13083306574393, 0.97545011664662, -0.10177807997561, 0.04400901776474, -0.02010737175166, 0.00719783432422},
26 {-0.00000000000000, 0.00000000000000, -0.00000000000001, 0.00000000000001, 0.99999999999999, 0.00000000000001, -0.00000000000001, 0.00000000000000, -0.00000000000000},
27 { 0.00719783432422, -0.02010737175166, 0.04400901776474, -0.10177807997562, 0.97545011664663, 0.13083306574393, -0.04985561057280, 0.02184035544377, -0.00764851320885},
28 { 0.01295781500710, -0.03584218578312, 0.07704272393640, -0.16976950138650, 0.90385267956634, 0.28284326017785, -0.09897034715252, 0.04229888475059, -0.01463300534216},
29 { 0.01654127246315, -0.04533310458085, 0.09585268418557, -0.20266133815190, 0.79117042386878, 0.44560418147640, -0.13991265473712, 0.05816126837865, -0.01985640750433}
30 };
31
32
WebRtcIsac_PitchfilterPre(double * indat,double * outdat,PitchFiltstr * pfp,double * lags,double * gains)33 void WebRtcIsac_PitchfilterPre(double *indat,
34 double *outdat,
35 PitchFiltstr *pfp,
36 double *lags,
37 double *gains)
38 {
39
40 double ubuf[PITCH_INTBUFFSIZE];
41 const double *fracoeff = NULL;
42 double curgain, curlag, gaindelta, lagdelta;
43 double sum, inystate[PITCH_DAMPORDER];
44 double ftmp, oldlag, oldgain;
45 int k, n, m, pos, ind, pos2, Li, frc;
46
47 Li = 0;
48 /* Set up buffer and states */
49 memcpy(ubuf, pfp->ubuf, sizeof(double) * PITCH_BUFFSIZE);
50 memcpy(inystate, pfp->ystate, sizeof(double) * PITCH_DAMPORDER);
51
52 oldlag = *pfp->oldlagp;
53 oldgain = *pfp->oldgainp;
54
55 /* No interpolation if pitch lag step is big */
56 if ((lags[0] > (PITCH_UPSTEP * oldlag)) || (lags[0] < (PITCH_DOWNSTEP * oldlag))) {
57 oldlag = lags[0];
58 oldgain = gains[0];
59 }
60
61 ind=0;
62 for (k=0;k<PITCH_SUBFRAMES;k++) {
63
64 /* Calculate interpolation steps */
65 lagdelta=(lags[k]-oldlag) / PITCH_GRAN_PER_SUBFRAME;
66 curlag=oldlag ;
67 gaindelta=(gains[k]-oldgain) / PITCH_GRAN_PER_SUBFRAME;
68 curgain=oldgain ;
69 oldlag=lags[k];
70 oldgain=gains[k];
71
72 for (n=0;n<PITCH_SUBFRAME_LEN;n++) {
73 if ((ind % PITCH_UPDATE) == 0) { /* Update parameters */
74 curgain += gaindelta;
75 curlag += lagdelta;
76 Li = WebRtcIsac_lrint(curlag+PITCH_FILTDELAY + 0.5);
77 ftmp = Li - (curlag+PITCH_FILTDELAY);
78 frc = WebRtcIsac_lrint(PITCH_FRACS * ftmp - 0.5);
79 fracoeff = kIntrpCoef[frc];
80 }
81
82 /* shift low pass filter state */
83 for (m=PITCH_DAMPORDER-1;m>0;m--)
84 inystate[m] = inystate[m-1];
85
86 /* Filter to get fractional pitch */
87 pos = ind + PITCH_BUFFSIZE;
88 pos2 = pos - Li;
89 sum=0;
90 for (m=0;m<PITCH_FRACORDER;m++)
91 sum += ubuf[pos2+m] * fracoeff[m];
92 inystate[0] = curgain * sum; /* Multiply with gain */
93
94 /* Low pass filter */
95 sum=0;
96 for (m=0;m<PITCH_DAMPORDER;m++)
97 sum += inystate[m] * kDampFilter[m];
98
99 /* Subtract from input and update buffer */
100 outdat[ind] = indat[ind] - sum;
101 ubuf[pos] = indat[ind] + outdat[ind];
102 ind++;
103 }
104 }
105
106 /* Export buffer and states */
107 memcpy(pfp->ubuf, ubuf+PITCH_FRAME_LEN, sizeof(double) * PITCH_BUFFSIZE);
108 memcpy(pfp->ystate, inystate, sizeof(double) * PITCH_DAMPORDER);
109
110 *pfp->oldlagp = oldlag;
111 *pfp->oldgainp = oldgain;
112
113 }
114
115
WebRtcIsac_PitchfilterPre_la(double * indat,double * outdat,PitchFiltstr * pfp,double * lags,double * gains)116 void WebRtcIsac_PitchfilterPre_la(double *indat,
117 double *outdat,
118 PitchFiltstr *pfp,
119 double *lags,
120 double *gains)
121 {
122 double ubuf[PITCH_INTBUFFSIZE+QLOOKAHEAD];
123 const double *fracoeff = NULL;
124 double curgain, curlag, gaindelta, lagdelta;
125 double sum, inystate[PITCH_DAMPORDER];
126 double ftmp;
127 double oldlag, oldgain;
128 int k, n, m, pos, ind, pos2, Li, frc;
129
130 Li = 0;
131 /* Set up buffer and states */
132 memcpy(ubuf, pfp->ubuf, sizeof(double) * PITCH_BUFFSIZE);
133 memcpy(inystate, pfp->ystate, sizeof(double) * PITCH_DAMPORDER);
134
135 oldlag = *pfp->oldlagp;
136 oldgain = *pfp->oldgainp;
137
138 /* No interpolation if pitch lag step is big */
139 if ((lags[0] > (PITCH_UPSTEP * oldlag)) || (lags[0] < (PITCH_DOWNSTEP * oldlag))) {
140 oldlag = lags[0];
141 oldgain = gains[0];
142 }
143
144
145 ind=0;
146 for (k=0;k<PITCH_SUBFRAMES;k++) {
147
148 /* Calculate interpolation steps */
149 lagdelta=(lags[k]-oldlag) / PITCH_GRAN_PER_SUBFRAME;
150 curlag=oldlag ;
151 gaindelta=(gains[k]-oldgain) / PITCH_GRAN_PER_SUBFRAME;
152 curgain=oldgain ;
153 oldlag=lags[k];
154 oldgain=gains[k];
155
156 for (n=0;n<PITCH_SUBFRAME_LEN;n++) {
157 if ((ind % PITCH_UPDATE) == 0) { /* Update parameters */
158 curgain += gaindelta;
159 curlag += lagdelta;
160 Li = WebRtcIsac_lrint(curlag+PITCH_FILTDELAY + 0.5);
161 ftmp = Li - (curlag+PITCH_FILTDELAY);
162 frc = WebRtcIsac_lrint(PITCH_FRACS * ftmp - 0.5);
163 fracoeff = kIntrpCoef[frc];
164 }
165
166 /* shift low pass filter state */
167 for (m=PITCH_DAMPORDER-1;m>0;m--)
168 inystate[m] = inystate[m-1];
169
170 /* Filter to get fractional pitch */
171 pos = ind + PITCH_BUFFSIZE;
172 pos2 = pos - Li;
173 sum=0.0;
174 for (m=0;m<PITCH_FRACORDER;m++)
175 sum += ubuf[pos2+m] * fracoeff[m];
176 inystate[0] = curgain * sum; /* Multiply with gain */
177
178 /* Low pass filter */
179 sum=0.0;
180 for (m=0;m<PITCH_DAMPORDER;m++)
181 sum += inystate[m] * kDampFilter[m];
182
183 /* Subtract from input and update buffer */
184 outdat[ind] = indat[ind] - sum;
185 ubuf[pos] = indat[ind] + outdat[ind];
186 ind++;
187 }
188 }
189
190 /* Export buffer and states */
191 memcpy(pfp->ubuf, ubuf+PITCH_FRAME_LEN, sizeof(double) * PITCH_BUFFSIZE);
192 memcpy(pfp->ystate, inystate, sizeof(double) * PITCH_DAMPORDER);
193
194 *pfp->oldlagp = oldlag;
195 *pfp->oldgainp = oldgain;
196
197
198 /* Filter look-ahead segment */
199 for (n=0;n<QLOOKAHEAD;n++) {
200 /* shift low pass filter state */
201 for (m=PITCH_DAMPORDER-1;m>0;m--)
202 inystate[m] = inystate[m-1];
203
204 /* Filter to get fractional pitch */
205 pos = ind + PITCH_BUFFSIZE;
206 pos2 = pos - Li;
207 sum=0.0;
208 for (m=0;m<PITCH_FRACORDER;m++)
209 sum += ubuf[pos2+m] * fracoeff[m];
210 inystate[0] = curgain * sum; /* Multiply with gain */
211
212 /* Low pass filter */
213 sum=0.0;
214 for (m=0;m<PITCH_DAMPORDER;m++)
215 sum += inystate[m] * kDampFilter[m];
216
217 /* Subtract from input and update buffer */
218 outdat[ind] = indat[ind] - sum;
219 ubuf[pos] = indat[ind] + outdat[ind];
220 ind++;
221 }
222 }
223
224
WebRtcIsac_PitchfilterPre_gains(double * indat,double * outdat,double out_dG[][PITCH_FRAME_LEN+QLOOKAHEAD],PitchFiltstr * pfp,double * lags,double * gains)225 void WebRtcIsac_PitchfilterPre_gains(double *indat,
226 double *outdat,
227 double out_dG[][PITCH_FRAME_LEN + QLOOKAHEAD],
228 PitchFiltstr *pfp,
229 double *lags,
230 double *gains)
231 {
232 double ubuf[PITCH_INTBUFFSIZE+QLOOKAHEAD];
233 double inystate_dG[4][PITCH_DAMPORDER];
234 double gain_mult[4];
235 const double *fracoeff = NULL;
236 double curgain, curlag, gaindelta, lagdelta;
237 double sum, sum2, inystate[PITCH_DAMPORDER];
238 double ftmp, oldlag, oldgain;
239 int k, n, m, m_tmp, j, pos, ind, pos2, Li, frc;
240
241 Li = 0;
242
243 /* Set up buffer and states */
244 memcpy(ubuf, pfp->ubuf, sizeof(double) * PITCH_BUFFSIZE);
245 memcpy(inystate, pfp->ystate, sizeof(double) * PITCH_DAMPORDER);
246
247 /* clear some buffers */
248 for (k = 0; k < 4; k++) {
249 gain_mult[k] = 0.0;
250 for (n = 0; n < PITCH_DAMPORDER; n++)
251 inystate_dG[k][n] = 0.0;
252 }
253
254 oldlag = *pfp->oldlagp;
255 oldgain = *pfp->oldgainp;
256
257 /* No interpolation if pitch lag step is big */
258 if ((lags[0] > (PITCH_UPSTEP * oldlag)) || (lags[0] < (PITCH_DOWNSTEP * oldlag))) {
259 oldlag = lags[0];
260 oldgain = gains[0];
261 gain_mult[0] = 1.0;
262 }
263
264
265 ind=0;
266 for (k=0;k<PITCH_SUBFRAMES;k++) {
267
268 /* Calculate interpolation steps */
269 lagdelta=(lags[k]-oldlag) / PITCH_GRAN_PER_SUBFRAME;
270 curlag=oldlag ;
271 gaindelta=(gains[k]-oldgain) / PITCH_GRAN_PER_SUBFRAME;
272 curgain=oldgain ;
273 oldlag=lags[k];
274 oldgain=gains[k];
275
276 for (n=0;n<PITCH_SUBFRAME_LEN;n++) {
277 if ((ind % PITCH_UPDATE) == 0) { /* Update parameters */
278 curgain += gaindelta;
279 curlag += lagdelta;
280 Li = WebRtcIsac_lrint(curlag+PITCH_FILTDELAY + 0.5);
281 ftmp = Li - (curlag+PITCH_FILTDELAY);
282 frc = WebRtcIsac_lrint(PITCH_FRACS * ftmp - 0.5);
283 fracoeff = kIntrpCoef[frc];
284 gain_mult[k] += 0.2;
285 if (gain_mult[k] > 1.0) gain_mult[k] = 1.0;
286 if (k > 0) gain_mult[k-1] -= 0.2;
287 }
288
289 /* shift low pass filter states */
290 for (m=PITCH_DAMPORDER-1;m>0;m--) {
291 inystate[m] = inystate[m-1];
292 for (j = 0; j < 4; j++)
293 inystate_dG[j][m] = inystate_dG[j][m-1];
294 }
295
296 pos = ind + PITCH_BUFFSIZE;
297 pos2 = pos - Li;
298
299 /* Filter to get fractional pitch */
300 sum=0.0;
301 for (m=0;m<PITCH_FRACORDER;m++)
302 sum += ubuf[pos2+m] * fracoeff[m];
303 inystate[0] = curgain * sum; /* Multiply with gain */
304 m_tmp = (Li-ind > 0) ? Li-ind : 0;
305 for (j = 0; j < k+1; j++) {
306 /* filter */
307 sum2 = 0.0;
308 for (m = PITCH_FRACORDER-1; m >= m_tmp; m--)
309 sum2 += out_dG[j][ind-Li + m] * fracoeff[m];
310 inystate_dG[j][0] = gain_mult[j] * sum + curgain * sum2;
311 }
312
313 /* Low pass filter */
314 sum=0.0;
315 for (m=0;m<PITCH_DAMPORDER;m++)
316 sum += inystate[m] * kDampFilter[m];
317
318 /* Subtract from input and update buffer */
319 outdat[ind] = indat[ind] - sum;
320 ubuf[pos] = indat[ind] + outdat[ind];
321
322 for (j = 0; j < k+1; j++) {
323 sum = 0.0;
324 for (m=0;m<PITCH_DAMPORDER;m++)
325 sum -= inystate_dG[j][m] * kDampFilter[m];
326 out_dG[j][ind] = sum;
327 }
328 for (j = k+1; j < 4; j++)
329 out_dG[j][ind] = 0.0;
330
331
332 ind++;
333 }
334 }
335
336 /* Filter look-ahead segment */
337 for (n=0;n<QLOOKAHEAD;n++) {
338 /* shift low pass filter states */
339 for (m=PITCH_DAMPORDER-1;m>0;m--) {
340 inystate[m] = inystate[m-1];
341 for (j = 0; j < 4; j++)
342 inystate_dG[j][m] = inystate_dG[j][m-1];
343 }
344
345 pos = ind + PITCH_BUFFSIZE;
346 pos2 = pos - Li;
347
348 /* Filter to get fractional pitch */
349 sum=0.0;
350 for (m=0;m<PITCH_FRACORDER;m++)
351 sum += ubuf[pos2+m] * fracoeff[m];
352 inystate[0] = curgain * sum; /* Multiply with gain */
353 m_tmp = (Li-ind > 0) ? Li-ind : 0;
354 for (j = 0; (j<k+1)&&(j<4); j++) {
355 /* filter */
356 sum2 = 0.0;
357 for (m = PITCH_FRACORDER-1; m >= m_tmp; m--)
358 sum2 += out_dG[j][ind-Li + m] * fracoeff[m];
359 inystate_dG[j][0] = gain_mult[j] * sum + curgain * sum2;
360 }
361
362 /* Low pass filter */
363 sum=0.0;
364 for (m=0;m<PITCH_DAMPORDER;m++)
365 sum += inystate[m] * kDampFilter[m];
366
367 /* Subtract from input and update buffer */
368 outdat[ind] = indat[ind] - sum;
369 ubuf[pos] = indat[ind] + outdat[ind];
370
371 for (j = 0; (j<k+1)&&(j<4); j++) {
372 sum = 0.0;
373 for (m=0;m<PITCH_DAMPORDER;m++)
374 sum -= inystate_dG[j][m] * kDampFilter[m];
375 out_dG[j][ind] = sum;
376 }
377
378 ind++;
379 }
380 }
381
382
WebRtcIsac_PitchfilterPost(double * indat,double * outdat,PitchFiltstr * pfp,double * lags,double * gains)383 void WebRtcIsac_PitchfilterPost(double *indat,
384 double *outdat,
385 PitchFiltstr *pfp,
386 double *lags,
387 double *gains)
388 {
389
390 double ubuf[PITCH_INTBUFFSIZE];
391 const double *fracoeff = NULL;
392 double curgain, curlag, gaindelta, lagdelta;
393 double sum, inystate[PITCH_DAMPORDER];
394 double ftmp, oldlag, oldgain;
395 int k, n, m, pos, ind, pos2, Li, frc;
396
397 Li = 0;
398
399 /* Set up buffer and states */
400 memcpy(ubuf, pfp->ubuf, sizeof(double) * PITCH_BUFFSIZE);
401 memcpy(inystate, pfp->ystate, sizeof(double) * PITCH_DAMPORDER);
402
403 oldlag = *pfp->oldlagp;
404 oldgain = *pfp->oldgainp;
405
406 /* make output more periodic */
407 for (k=0;k<PITCH_SUBFRAMES;k++)
408 gains[k] *= 1.3;
409
410 /* No interpolation if pitch lag step is big */
411 if ((lags[0] > (PITCH_UPSTEP * oldlag)) || (lags[0] < (PITCH_DOWNSTEP * oldlag))) {
412 oldlag = lags[0];
413 oldgain = gains[0];
414 }
415
416
417 ind=0;
418 for (k=0;k<PITCH_SUBFRAMES;k++) {
419
420 /* Calculate interpolation steps */
421 lagdelta=(lags[k]-oldlag) / PITCH_GRAN_PER_SUBFRAME;
422 curlag=oldlag ;
423 gaindelta=(gains[k]-oldgain) / PITCH_GRAN_PER_SUBFRAME;
424 curgain=oldgain ;
425 oldlag=lags[k];
426 oldgain=gains[k];
427
428 for (n=0;n<PITCH_SUBFRAME_LEN;n++) {
429 if ((ind % PITCH_UPDATE) == 0) { /* Update parameters */
430 curgain += gaindelta;
431 curlag += lagdelta;
432 Li = WebRtcIsac_lrint(curlag+PITCH_FILTDELAY + 0.5);
433 ftmp = Li - (curlag+PITCH_FILTDELAY);
434 frc = WebRtcIsac_lrint(PITCH_FRACS * ftmp - 0.5);
435 fracoeff = kIntrpCoef[frc];
436 }
437
438 /* shift low pass filter state */
439 for (m=PITCH_DAMPORDER-1;m>0;m--)
440 inystate[m] = inystate[m-1];
441
442 /* Filter to get fractional pitch */
443 pos = ind + PITCH_BUFFSIZE;
444 pos2 = pos - Li;
445 sum=0.0;
446 for (m=0;m<PITCH_FRACORDER;m++)
447 sum += ubuf[pos2+m] * fracoeff[m];
448 inystate[0] = curgain * sum; /* Multiply with gain */
449
450 /* Low pass filter */
451 sum=0.0;
452 for (m=0;m<PITCH_DAMPORDER;m++)
453 sum += inystate[m] * kDampFilter[m];
454
455 /* Add to input and update buffer */
456 outdat[ind] = indat[ind] + sum;
457 ubuf[pos] = indat[ind] + outdat[ind];
458 ind++;
459 }
460 }
461
462 /* Export buffer and states */
463 memcpy(pfp->ubuf, ubuf+PITCH_FRAME_LEN, sizeof(double) * PITCH_BUFFSIZE);
464 memcpy(pfp->ystate, inystate, sizeof(double) * PITCH_DAMPORDER);
465
466 *pfp->oldlagp = oldlag;
467 *pfp->oldgainp = oldgain;
468
469 }
470