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 "webrtc/modules/audio_coding/codecs/isac/fix/source/pitch_estimator.h"
12 #include "webrtc/common_audio/signal_processing/include/signal_processing_library.h"
13 #include "webrtc/system_wrappers/include/compile_assert_c.h"
14
15 /* log2[0.2, 0.5, 0.98] in Q8 */
16 static const int16_t kLogLagWinQ8[3] = {
17 -594, -256, -7
18 };
19
20 /* [1 -0.75 0.25] in Q12 */
21 static const int16_t kACoefQ12[3] = {
22 4096, -3072, 1024
23 };
24
WebRtcIsacfix_Log2Q8(uint32_t x)25 int32_t WebRtcIsacfix_Log2Q8(uint32_t x) {
26 int32_t zeros;
27 int16_t frac;
28
29 zeros=WebRtcSpl_NormU32(x);
30 frac = (int16_t)(((x << zeros) & 0x7FFFFFFF) >> 23);
31 /* log2(magn(i)) */
32
33 return ((31 - zeros) << 8) + frac;
34 }
35
Exp2Q10(int16_t x)36 static __inline int16_t Exp2Q10(int16_t x) { // Both in and out in Q10
37
38 int16_t tmp16_1, tmp16_2;
39
40 tmp16_2=(int16_t)(0x0400|(x&0x03FF));
41 tmp16_1 = -(x >> 10);
42 if(tmp16_1>0)
43 return tmp16_2 >> tmp16_1;
44 else
45 return tmp16_2 << -tmp16_1;
46
47 }
48
49
50
51 /* 1D parabolic interpolation . All input and output values are in Q8 */
Intrp1DQ8(int32_t * x,int32_t * fx,int32_t * y,int32_t * fy)52 static __inline void Intrp1DQ8(int32_t *x, int32_t *fx, int32_t *y, int32_t *fy) {
53
54 int16_t sign1=1, sign2=1;
55 int32_t r32, q32, t32, nom32, den32;
56 int16_t t16, tmp16, tmp16_1;
57
58 if ((fx[0]>0) && (fx[2]>0)) {
59 r32=fx[1]-fx[2];
60 q32=fx[0]-fx[1];
61 nom32=q32+r32;
62 den32 = (q32 - r32) * 2;
63 if (nom32<0)
64 sign1=-1;
65 if (den32<0)
66 sign2=-1;
67
68 /* t = (q32+r32)/(2*(q32-r32)) = (fx[0]-fx[1] + fx[1]-fx[2])/(2 * fx[0]-fx[1] - (fx[1]-fx[2]))*/
69 /* (Signs are removed because WebRtcSpl_DivResultInQ31 can't handle negative numbers) */
70 /* t in Q31, without signs */
71 t32 = WebRtcSpl_DivResultInQ31(nom32 * sign1, den32 * sign2);
72
73 t16 = (int16_t)(t32 >> 23); /* Q8 */
74 t16=t16*sign1*sign2; /* t in Q8 with signs */
75
76 *y = x[0]+t16; /* Q8 */
77 // *y = x[1]+t16; /* Q8 */
78
79 /* The following code calculates fy in three steps */
80 /* fy = 0.5 * t * (t-1) * fx[0] + (1-t*t) * fx[1] + 0.5 * t * (t+1) * fx[2]; */
81
82 /* Part I: 0.5 * t * (t-1) * fx[0] */
83 tmp16_1 = (int16_t)(t16 * t16); /* Q8*Q8=Q16 */
84 tmp16_1 >>= 2; /* Q16>>2 = Q14 */
85 t16 <<= 6; /* Q8<<6 = Q14 */
86 tmp16 = tmp16_1-t16;
87 *fy = WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[0]); /* (Q14 * Q8 >>15)/2 = Q8 */
88
89 /* Part II: (1-t*t) * fx[1] */
90 tmp16 = 16384-tmp16_1; /* 1 in Q14 - Q14 */
91 *fy += WEBRTC_SPL_MUL_16_32_RSFT14(tmp16, fx[1]);/* Q14 * Q8 >> 14 = Q8 */
92
93 /* Part III: 0.5 * t * (t+1) * fx[2] */
94 tmp16 = tmp16_1+t16;
95 *fy += WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[2]);/* (Q14 * Q8 >>15)/2 = Q8 */
96 } else {
97 *y = x[0];
98 *fy= fx[1];
99 }
100 }
101
102
FindFour32(int32_t * in,int16_t length,int16_t * bestind)103 static void FindFour32(int32_t *in, int16_t length, int16_t *bestind)
104 {
105 int32_t best[4]= {-100, -100, -100, -100};
106 int16_t k;
107
108 for (k=0; k<length; k++) {
109 if (in[k] > best[3]) {
110 if (in[k] > best[2]) {
111 if (in[k] > best[1]) {
112 if (in[k] > best[0]) { // The Best
113 best[3] = best[2];
114 bestind[3] = bestind[2];
115 best[2] = best[1];
116 bestind[2] = bestind[1];
117 best[1] = best[0];
118 bestind[1] = bestind[0];
119 best[0] = in[k];
120 bestind[0] = k;
121 } else { // 2nd best
122 best[3] = best[2];
123 bestind[3] = bestind[2];
124 best[2] = best[1];
125 bestind[2] = bestind[1];
126 best[1] = in[k];
127 bestind[1] = k;
128 }
129 } else { // 3rd best
130 best[3] = best[2];
131 bestind[3] = bestind[2];
132 best[2] = in[k];
133 bestind[2] = k;
134 }
135 } else { // 4th best
136 best[3] = in[k];
137 bestind[3] = k;
138 }
139 }
140 }
141 }
142
143
144
145
146
147 extern void WebRtcIsacfix_PCorr2Q32(const int16_t *in, int32_t *logcorQ8);
148
149
150
WebRtcIsacfix_InitialPitch(const int16_t * in,PitchAnalysisStruct * State,int16_t * lagsQ7)151 void WebRtcIsacfix_InitialPitch(const int16_t *in, /* Q0 */
152 PitchAnalysisStruct *State,
153 int16_t *lagsQ7 /* Q7 */
154 )
155 {
156 int16_t buf_dec16[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2];
157 int32_t *crrvecQ8_1,*crrvecQ8_2;
158 int32_t cv1q[PITCH_LAG_SPAN2+2],cv2q[PITCH_LAG_SPAN2+2], peakvq[PITCH_LAG_SPAN2+2];
159 int k;
160 int16_t peaks_indq;
161 int16_t peakiq[PITCH_LAG_SPAN2];
162 int32_t corr;
163 int32_t corr32, corr_max32, corr_max_o32;
164 int16_t npkq;
165 int16_t best4q[4]={0,0,0,0};
166 int32_t xq[3],yq[1],fyq[1];
167 int32_t *fxq;
168 int32_t best_lag1q, best_lag2q;
169 int32_t tmp32a,tmp32b,lag32,ratq;
170 int16_t start;
171 int16_t oldgQ12, tmp16a, tmp16b, gain_bias16,tmp16c, tmp16d, bias16;
172 int32_t tmp32c,tmp32d, tmp32e;
173 int16_t old_lagQ;
174 int32_t old_lagQ8;
175 int32_t lagsQ8[4];
176
177 old_lagQ = State->PFstr_wght.oldlagQ7; // Q7
178 old_lagQ8 = old_lagQ << 1; // Q8
179
180 oldgQ12= State->PFstr_wght.oldgainQ12;
181
182 crrvecQ8_1=&cv1q[1];
183 crrvecQ8_2=&cv2q[1];
184
185
186 /* copy old values from state buffer */
187 memcpy(buf_dec16, State->dec_buffer16, sizeof(State->dec_buffer16));
188
189 /* decimation; put result after the old values */
190 WebRtcIsacfix_DecimateAllpass32(in, State->decimator_state32, PITCH_FRAME_LEN,
191 &buf_dec16[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2]);
192
193 /* low-pass filtering */
194 start= PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2;
195 WebRtcSpl_FilterARFastQ12(&buf_dec16[start],&buf_dec16[start],(int16_t*)kACoefQ12,3, PITCH_FRAME_LEN/2);
196
197 /* copy end part back into state buffer */
198 for (k = 0; k < (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2); k++)
199 State->dec_buffer16[k] = buf_dec16[k+PITCH_FRAME_LEN/2];
200
201
202 /* compute correlation for first and second half of the frame */
203 WebRtcIsacfix_PCorr2Q32(buf_dec16, crrvecQ8_1);
204 WebRtcIsacfix_PCorr2Q32(buf_dec16 + PITCH_CORR_STEP2, crrvecQ8_2);
205
206
207 /* bias towards pitch lag of previous frame */
208 tmp32a = WebRtcIsacfix_Log2Q8((uint32_t) old_lagQ8) - 2304;
209 // log2(0.5*oldlag) in Q8
210 tmp32b = oldgQ12 * oldgQ12 >> 10; // Q12 & * 4.0;
211 gain_bias16 = (int16_t) tmp32b; //Q12
212 if (gain_bias16 > 3276) gain_bias16 = 3276; // 0.8 in Q12
213
214
215 for (k = 0; k < PITCH_LAG_SPAN2; k++)
216 {
217 if (crrvecQ8_1[k]>0) {
218 tmp32b = WebRtcIsacfix_Log2Q8((uint32_t) (k + (PITCH_MIN_LAG/2-2)));
219 tmp16a = (int16_t) (tmp32b - tmp32a); // Q8 & fabs(ratio)<4
220 tmp32c = tmp16a * tmp16a >> 6; // Q10
221 tmp16b = (int16_t) tmp32c; // Q10 & <8
222 tmp32d = tmp16b * 177 >> 8; // mult with ln2 in Q8
223 tmp16c = (int16_t) tmp32d; // Q10 & <4
224 tmp16d = Exp2Q10((int16_t) -tmp16c); //Q10
225 tmp32c = gain_bias16 * tmp16d >> 13; // Q10 & * 0.5
226 bias16 = (int16_t) (1024 + tmp32c); // Q10
227 tmp32b = WebRtcIsacfix_Log2Q8((uint32_t)bias16) - 2560;
228 // Q10 in -> Q8 out with 10*2^8 offset
229 crrvecQ8_1[k] += tmp32b ; // -10*2^8 offset
230 }
231 }
232
233 /* taper correlation functions */
234 for (k = 0; k < 3; k++) {
235 crrvecQ8_1[k] += kLogLagWinQ8[k];
236 crrvecQ8_2[k] += kLogLagWinQ8[k];
237
238 crrvecQ8_1[PITCH_LAG_SPAN2-1-k] += kLogLagWinQ8[k];
239 crrvecQ8_2[PITCH_LAG_SPAN2-1-k] += kLogLagWinQ8[k];
240 }
241
242
243 /* Make zeropadded corr vectors */
244 cv1q[0]=0;
245 cv2q[0]=0;
246 cv1q[PITCH_LAG_SPAN2+1]=0;
247 cv2q[PITCH_LAG_SPAN2+1]=0;
248 corr_max32 = 0;
249
250 for (k = 1; k <= PITCH_LAG_SPAN2; k++)
251 {
252
253
254 corr32=crrvecQ8_1[k-1];
255 if (corr32 > corr_max32)
256 corr_max32 = corr32;
257
258 corr32=crrvecQ8_2[k-1];
259 corr32 += -4; // Compensate for later (log2(0.99))
260
261 if (corr32 > corr_max32)
262 corr_max32 = corr32;
263
264 }
265
266 /* threshold value to qualify as a peak */
267 // corr_max32 += -726; // log(0.14)/log(2.0) in Q8
268 corr_max32 += -1000; // log(0.14)/log(2.0) in Q8
269 corr_max_o32 = corr_max32;
270
271
272 /* find peaks in corr1 */
273 peaks_indq = 0;
274 for (k = 1; k <= PITCH_LAG_SPAN2; k++)
275 {
276 corr32=cv1q[k];
277 if (corr32>corr_max32) { // Disregard small peaks
278 if ((corr32>=cv1q[k-1]) && (corr32>cv1q[k+1])) { // Peak?
279 peakvq[peaks_indq] = corr32;
280 peakiq[peaks_indq++] = k;
281 }
282 }
283 }
284
285
286 /* find highest interpolated peak */
287 corr_max32=0;
288 best_lag1q =0;
289 if (peaks_indq > 0) {
290 FindFour32(peakvq, (int16_t) peaks_indq, best4q);
291 npkq = WEBRTC_SPL_MIN(peaks_indq, 4);
292
293 for (k=0;k<npkq;k++) {
294
295 lag32 = peakiq[best4q[k]];
296 fxq = &cv1q[peakiq[best4q[k]]-1];
297 xq[0]= lag32;
298 xq[0] <<= 8;
299 Intrp1DQ8(xq, fxq, yq, fyq);
300
301 tmp32a= WebRtcIsacfix_Log2Q8((uint32_t) *yq) - 2048; // offset 8*2^8
302 /* Bias towards short lags */
303 /* log(pow(0.8, log(2.0 * *y )))/log(2.0) */
304 tmp32b = (int16_t)tmp32a * -42 >> 8;
305 tmp32c= tmp32b + 256;
306 *fyq += tmp32c;
307 if (*fyq > corr_max32) {
308 corr_max32 = *fyq;
309 best_lag1q = *yq;
310 }
311 }
312 tmp32b = (best_lag1q - OFFSET_Q8) * 2;
313 lagsQ8[0] = tmp32b + PITCH_MIN_LAG_Q8;
314 lagsQ8[1] = lagsQ8[0];
315 } else {
316 lagsQ8[0] = old_lagQ8;
317 lagsQ8[1] = lagsQ8[0];
318 }
319
320 /* Bias towards constant pitch */
321 tmp32a = lagsQ8[0] - PITCH_MIN_LAG_Q8;
322 ratq = (tmp32a >> 1) + OFFSET_Q8;
323
324 for (k = 1; k <= PITCH_LAG_SPAN2; k++)
325 {
326 tmp32a = k << 7; // 0.5*k Q8
327 tmp32b = tmp32a * 2 - ratq; // Q8
328 tmp32c = (int16_t)tmp32b * (int16_t)tmp32b >> 8; // Q8
329
330 tmp32b = tmp32c + (ratq >> 1);
331 // (k-r)^2 + 0.5 * r Q8
332 tmp32c = WebRtcIsacfix_Log2Q8((uint32_t)tmp32a) - 2048;
333 // offset 8*2^8 , log2(0.5*k) Q8
334 tmp32d = WebRtcIsacfix_Log2Q8((uint32_t)tmp32b) - 2048;
335 // offset 8*2^8 , log2(0.5*k) Q8
336 tmp32e = tmp32c - tmp32d;
337
338 cv2q[k] += tmp32e >> 1;
339
340 }
341
342 /* find peaks in corr2 */
343 corr_max32 = corr_max_o32;
344 peaks_indq = 0;
345
346 for (k = 1; k <= PITCH_LAG_SPAN2; k++)
347 {
348 corr=cv2q[k];
349 if (corr>corr_max32) { // Disregard small peaks
350 if ((corr>=cv2q[k-1]) && (corr>cv2q[k+1])) { // Peak?
351 peakvq[peaks_indq] = corr;
352 peakiq[peaks_indq++] = k;
353 }
354 }
355 }
356
357
358
359 /* find highest interpolated peak */
360 corr_max32 = 0;
361 best_lag2q =0;
362 if (peaks_indq > 0) {
363
364 FindFour32(peakvq, (int16_t) peaks_indq, best4q);
365 npkq = WEBRTC_SPL_MIN(peaks_indq, 4);
366 for (k=0;k<npkq;k++) {
367
368 lag32 = peakiq[best4q[k]];
369 fxq = &cv2q[peakiq[best4q[k]]-1];
370
371 xq[0]= lag32;
372 xq[0] <<= 8;
373 Intrp1DQ8(xq, fxq, yq, fyq);
374
375 /* Bias towards short lags */
376 /* log(pow(0.8, log(2.0f * *y )))/log(2.0f) */
377 tmp32a= WebRtcIsacfix_Log2Q8((uint32_t) *yq) - 2048; // offset 8*2^8
378 tmp32b = (int16_t)tmp32a * -82 >> 8;
379 tmp32c= tmp32b + 256;
380 *fyq += tmp32c;
381 if (*fyq > corr_max32) {
382 corr_max32 = *fyq;
383 best_lag2q = *yq;
384 }
385 }
386
387 tmp32b = (best_lag2q - OFFSET_Q8) * 2;
388 lagsQ8[2] = tmp32b + PITCH_MIN_LAG_Q8;
389 lagsQ8[3] = lagsQ8[2];
390 } else {
391 lagsQ8[2] = lagsQ8[0];
392 lagsQ8[3] = lagsQ8[0];
393 }
394
395 lagsQ7[0] = (int16_t)(lagsQ8[0] >> 1);
396 lagsQ7[1] = (int16_t)(lagsQ8[1] >> 1);
397 lagsQ7[2] = (int16_t)(lagsQ8[2] >> 1);
398 lagsQ7[3] = (int16_t)(lagsQ8[3] >> 1);
399 }
400
401
402
WebRtcIsacfix_PitchAnalysis(const int16_t * inn,int16_t * outQ0,PitchAnalysisStruct * State,int16_t * PitchLags_Q7,int16_t * PitchGains_Q12)403 void WebRtcIsacfix_PitchAnalysis(const int16_t *inn, /* PITCH_FRAME_LEN samples */
404 int16_t *outQ0, /* PITCH_FRAME_LEN+QLOOKAHEAD samples */
405 PitchAnalysisStruct *State,
406 int16_t *PitchLags_Q7,
407 int16_t *PitchGains_Q12)
408 {
409 int16_t inbufQ0[PITCH_FRAME_LEN + QLOOKAHEAD];
410 int16_t k;
411
412 /* inital pitch estimate */
413 WebRtcIsacfix_InitialPitch(inn, State, PitchLags_Q7);
414
415
416 /* Calculate gain */
417 WebRtcIsacfix_PitchFilterGains(inn, &(State->PFstr_wght), PitchLags_Q7, PitchGains_Q12);
418
419 /* concatenate previous input's end and current input */
420 for (k = 0; k < QLOOKAHEAD; k++) {
421 inbufQ0[k] = State->inbuf[k];
422 }
423 for (k = 0; k < PITCH_FRAME_LEN; k++) {
424 inbufQ0[k+QLOOKAHEAD] = (int16_t) inn[k];
425 }
426
427 /* lookahead pitch filtering for masking analysis */
428 WebRtcIsacfix_PitchFilter(inbufQ0, outQ0, &(State->PFstr), PitchLags_Q7,PitchGains_Q12, 2);
429
430
431 /* store last part of input */
432 for (k = 0; k < QLOOKAHEAD; k++) {
433 State->inbuf[k] = inbufQ0[k + PITCH_FRAME_LEN];
434 }
435 }
436