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 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 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 */ 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 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 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 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
