| /* |
| * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved. |
| * |
| * Use of this source code is governed by a BSD-style license |
| * that can be found in the LICENSE file in the root of the source |
| * tree. An additional intellectual property rights grant can be found |
| * in the file PATENTS. All contributing project authors may |
| * be found in the AUTHORS file in the root of the source tree. |
| */ |
| |
| /* |
| * pitch_estimatorfilter.c |
| * |
| * Pitch filter functions |
| * |
| */ |
| |
| #include <string.h> |
| |
| #include "pitch_estimator.h" |
| |
| |
| /* Filter coefficicients in Q15 */ |
| static const WebRtc_Word16 kDampFilter[PITCH_DAMPORDER] = { |
| -2294, 8192, 20972, 8192, -2294 |
| }; |
| |
| /* Interpolation coefficients; generated by design_pitch_filter.m. |
| * Coefficients are stored in Q14. |
| */ |
| static const WebRtc_Word16 kIntrpCoef[PITCH_FRACS][PITCH_FRACORDER] = { |
| {-367, 1090, -2706, 9945, 10596, -3318, 1626, -781, 287}, |
| {-325, 953, -2292, 7301, 12963, -3320, 1570, -743, 271}, |
| {-240, 693, -1622, 4634, 14809, -2782, 1262, -587, 212}, |
| {-125, 358, -817, 2144, 15982, -1668, 721, -329, 118}, |
| { 0, 0, -1, 1, 16380, 1, -1, 0, 0}, |
| { 118, -329, 721, -1668, 15982, 2144, -817, 358, -125}, |
| { 212, -587, 1262, -2782, 14809, 4634, -1622, 693, -240}, |
| { 271, -743, 1570, -3320, 12963, 7301, -2292, 953, -325} |
| }; |
| |
| |
| |
| |
| static __inline WebRtc_Word32 CalcLrIntQ(WebRtc_Word32 fixVal, WebRtc_Word16 qDomain) { |
| WebRtc_Word32 intgr; |
| WebRtc_Word32 roundVal; |
| |
| roundVal = WEBRTC_SPL_LSHIFT_W32((WebRtc_Word32)1, qDomain-1); |
| intgr = WEBRTC_SPL_RSHIFT_W32(fixVal+roundVal, qDomain); |
| |
| return intgr; |
| } |
| |
| void WebRtcIsacfix_PitchFilter(WebRtc_Word16 *indatQQ, /* Q10 if type is 1 or 4, Q0 if type is 2 */ |
| WebRtc_Word16 *outdatQQ, |
| PitchFiltstr *pfp, |
| WebRtc_Word16 *lagsQ7, |
| WebRtc_Word16 *gainsQ12, |
| WebRtc_Word16 type) |
| { |
| int k, n, m, ind; |
| WebRtc_Word16 sign = 1; |
| WebRtc_Word16 inystateQQ[PITCH_DAMPORDER]; |
| WebRtc_Word16 ubufQQ[PITCH_INTBUFFSIZE+QLOOKAHEAD]; |
| WebRtc_Word16 Gain = 21299; /* 1.3 in Q14 */ |
| WebRtc_Word16 DivFactor = 6553; /* 0.2 in Q15 */ |
| WebRtc_Word16 oldLagQ7, oldGainQ12, |
| lagdeltaQ7, curLagQ7, |
| gaindeltaQ12, curGainQ12; |
| WebRtc_Word16 tmpW16, indW16=0, frcQQ, cnt=0, pos, pos2; |
| const WebRtc_Word16 *fracoeffQQ=NULL; |
| WebRtc_Word32 tmpW32; |
| |
| if (type==4) |
| sign = -1; |
| |
| /* Set up buffer and states */ |
| memcpy(ubufQQ, pfp->ubufQQ, WEBRTC_SPL_MUL_16_16(sizeof(WebRtc_Word16), PITCH_BUFFSIZE)); |
| memcpy(inystateQQ, pfp->ystateQQ, WEBRTC_SPL_MUL_16_16(sizeof(WebRtc_Word16), PITCH_DAMPORDER)); |
| |
| /* Get old lag and gain value from memory */ |
| oldLagQ7 = pfp->oldlagQ7; |
| oldGainQ12 = pfp->oldgainQ12; |
| |
| if (type==4) { |
| /* make output more periodic */ |
| /* Fixed 1.3 = 21299 in Q14 */ |
| for (k=0;k<PITCH_SUBFRAMES;k++) |
| gainsQ12[k] = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT(gainsQ12[k], Gain, 14); |
| } |
| |
| /* No interpolation if pitch lag step is big */ |
| if ((WEBRTC_SPL_RSHIFT_W16(WEBRTC_SPL_MUL_16_16(lagsQ7[0], 3), 1) < oldLagQ7) || |
| (lagsQ7[0] > WEBRTC_SPL_RSHIFT_W16(WEBRTC_SPL_MUL_16_16(oldLagQ7, 3), 1))) { |
| oldLagQ7 = lagsQ7[0]; |
| oldGainQ12 = gainsQ12[0]; |
| } |
| |
| ind=0; |
| for (k=0;k<PITCH_SUBFRAMES;k++) { |
| |
| /* Calculate interpolation steps */ |
| lagdeltaQ7 = lagsQ7[k]-oldLagQ7; |
| lagdeltaQ7 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(lagdeltaQ7,DivFactor,15); |
| curLagQ7 = oldLagQ7; |
| gaindeltaQ12 = gainsQ12[k]-oldGainQ12; |
| gaindeltaQ12 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT(gaindeltaQ12,DivFactor,15); |
| |
| curGainQ12 = oldGainQ12; |
| oldLagQ7 = lagsQ7[k]; |
| oldGainQ12 = gainsQ12[k]; |
| |
| |
| for (n=0;n<PITCH_SUBFRAME_LEN;n++) { |
| |
| if (cnt == 0) { /* Update parameters */ |
| |
| curGainQ12 += gaindeltaQ12; |
| curLagQ7 += lagdeltaQ7; |
| indW16 = (WebRtc_Word16)CalcLrIntQ(curLagQ7,7); |
| tmpW16 = WEBRTC_SPL_LSHIFT_W16(indW16,7); |
| tmpW16 -= curLagQ7; |
| frcQQ = WEBRTC_SPL_RSHIFT_W16(tmpW16,4); |
| frcQQ += 4; |
| |
| if(frcQQ==PITCH_FRACS) |
| frcQQ=0; |
| fracoeffQQ = kIntrpCoef[frcQQ]; |
| |
| cnt=12; |
| } |
| |
| /* shift low pass filter state */ |
| for (m=PITCH_DAMPORDER-1;m>0;m--) |
| inystateQQ[m] = inystateQQ[m-1]; |
| |
| /* Filter to get fractional pitch */ |
| pos = ind + PITCH_BUFFSIZE; |
| pos2 = pos - (indW16 + 2); |
| |
| tmpW32=0; |
| for (m=0;m<PITCH_FRACORDER;m++) |
| tmpW32 += WEBRTC_SPL_MUL_16_16(ubufQQ[pos2+m], fracoeffQQ[m]); |
| |
| /* Saturate to avoid overflow in tmpW16 */ |
| tmpW32 = WEBRTC_SPL_SAT(536862719, tmpW32, -536879104); |
| tmpW32 += 8192; |
| tmpW16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmpW32,14); |
| |
| inystateQQ[0] = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(curGainQ12, tmpW16,12); /* Multiply with gain */ |
| |
| /* Low pass filter */ |
| tmpW32=0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| tmpW32 += WEBRTC_SPL_MUL_16_16(inystateQQ[m], kDampFilter[m]); |
| |
| /* Saturate to avoid overflow in tmpW16 */ |
| tmpW32 = WEBRTC_SPL_SAT(1073725439, tmpW32, -1073758208); |
| tmpW32 += 16384; |
| tmpW16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmpW32,15); |
| |
| /* Subtract from input and update buffer */ |
| tmpW32 = indatQQ[ind] - WEBRTC_SPL_MUL_16_16(sign, tmpW16); |
| outdatQQ[ind] = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmpW32); |
| tmpW32 = indatQQ[ind] + (WebRtc_Word32)outdatQQ[ind]; |
| ubufQQ[pos] = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmpW32); |
| |
| ind++; |
| cnt--; |
| } |
| } |
| |
| |
| /* Export buffer and states */ |
| memcpy(pfp->ubufQQ, ubufQQ+PITCH_FRAME_LEN, WEBRTC_SPL_MUL_16_16(sizeof(WebRtc_Word16), PITCH_BUFFSIZE)); |
| memcpy(pfp->ystateQQ, inystateQQ, WEBRTC_SPL_MUL_16_16(sizeof(WebRtc_Word16), PITCH_DAMPORDER)); |
| |
| pfp->oldlagQ7 = oldLagQ7; |
| pfp->oldgainQ12 = oldGainQ12; |
| |
| if (type==2) { |
| /* Filter look-ahead segment */ |
| for (n=0;n<QLOOKAHEAD;n++) { |
| /* shift low pass filter state */ |
| for (m=PITCH_DAMPORDER-1;m>0;m--) |
| inystateQQ[m] = inystateQQ[m-1]; |
| |
| /* Filter to get fractional pitch */ |
| pos = ind + PITCH_BUFFSIZE; |
| pos2= pos - (indW16 + 2); |
| |
| tmpW32=0; |
| for (m=0;m<PITCH_FRACORDER;m++) |
| tmpW32 += WEBRTC_SPL_MUL_16_16(ubufQQ[pos2+m], fracoeffQQ[m]); |
| |
| if (tmpW32<536862720) {//536870912) |
| tmpW32 += 8192; |
| tmpW16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmpW32,14); |
| } else |
| tmpW16= 32767; |
| inystateQQ[0] = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(curGainQ12, tmpW16,12); /* Multiply with gain */ |
| |
| /* Low pass filter */ |
| tmpW32=0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| tmpW32 += WEBRTC_SPL_MUL_16_16(inystateQQ[m], kDampFilter[m]); |
| if (tmpW32<1073725440) { //1073741824) |
| tmpW32 += 16384; |
| tmpW16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmpW32,15); |
| } else |
| tmpW16 = 32767; |
| |
| /* Subtract from input and update buffer */ |
| tmpW32 = indatQQ[ind] - (WebRtc_Word32)tmpW16; |
| outdatQQ[ind] = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmpW32); |
| tmpW32 = indatQQ[ind] + (WebRtc_Word32)outdatQQ[ind]; |
| ubufQQ[pos] = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmpW32); |
| |
| ind++; |
| } |
| |
| } |
| |
| |
| } |
| |
| |
| void WebRtcIsacfix_PitchFilterGains(const WebRtc_Word16 *indatQ0, |
| PitchFiltstr *pfp, |
| WebRtc_Word16 *lagsQ7, |
| WebRtc_Word16 *gainsQ12) |
| { |
| int k, n, m, ind; |
| |
| WebRtc_Word16 ubufQQ[PITCH_INTBUFFSIZE]; |
| WebRtc_Word16 oldLagQ7,lagdeltaQ7, curLagQ7; |
| WebRtc_Word16 DivFactor = 6553; |
| const WebRtc_Word16 *fracoeffQQ = NULL; |
| WebRtc_Word16 scale; |
| WebRtc_Word16 cnt=0, pos, pos3QQ, frcQQ, indW16 = 0, tmpW16; |
| WebRtc_Word32 tmpW32, tmp2W32, csum1QQ, esumxQQ; |
| |
| /* Set up buffer and states */ |
| memcpy(ubufQQ, pfp->ubufQQ, WEBRTC_SPL_MUL_16_16(sizeof(WebRtc_Word16), PITCH_BUFFSIZE)); |
| oldLagQ7 = pfp->oldlagQ7; |
| |
| /* No interpolation if pitch lag step is big */ |
| if ((WEBRTC_SPL_RSHIFT_W16(WEBRTC_SPL_MUL_16_16(lagsQ7[0], 3), 1) < oldLagQ7) || |
| (lagsQ7[0] > WEBRTC_SPL_RSHIFT_W16(WEBRTC_SPL_MUL_16_16(oldLagQ7, 3), 1))) { |
| oldLagQ7 = lagsQ7[0]; |
| } |
| |
| ind=0; |
| scale=0; |
| for (k=0;k<PITCH_SUBFRAMES;k++) { |
| |
| /* Calculate interpolation steps */ |
| lagdeltaQ7 = lagsQ7[k]-oldLagQ7; |
| lagdeltaQ7 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(lagdeltaQ7,DivFactor,15); |
| curLagQ7 = oldLagQ7; |
| oldLagQ7 = lagsQ7[k]; |
| |
| csum1QQ=1; |
| esumxQQ=1; |
| |
| for (n=0;n<PITCH_SUBFRAME_LEN;n++) { |
| |
| if (cnt == 0) { /* Update parameters */ |
| curLagQ7 += lagdeltaQ7; |
| indW16 = (WebRtc_Word16)CalcLrIntQ(curLagQ7,7); |
| tmpW16 = WEBRTC_SPL_LSHIFT_W16(indW16,7); |
| tmpW16 -= curLagQ7; |
| frcQQ = WEBRTC_SPL_RSHIFT_W16(tmpW16,4); |
| frcQQ += 4; |
| |
| if(frcQQ==PITCH_FRACS) |
| frcQQ=0; |
| fracoeffQQ = kIntrpCoef[frcQQ]; |
| |
| cnt=12; |
| } |
| |
| /* Filter to get fractional pitch */ |
| pos = ind + PITCH_BUFFSIZE; |
| pos3QQ = pos - (indW16 + 4); |
| |
| tmpW32=0; |
| for (m=0;m<PITCH_FRACORDER;m++){ |
| tmpW32 += WEBRTC_SPL_MUL_16_16(ubufQQ[pos3QQ+m], fracoeffQQ[m]); |
| } |
| |
| /* Subtract from input and update buffer */ |
| ubufQQ[pos] = indatQ0[ind]; |
| |
| tmp2W32 = WEBRTC_SPL_MUL_16_32_RSFT14(indatQ0[ind], tmpW32); |
| tmpW32 += 8192; |
| tmpW16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmpW32,14); |
| tmpW32 = WEBRTC_SPL_MUL_16_16(tmpW16, tmpW16); |
| |
| if ((tmp2W32>1073700000) || (csum1QQ>1073700000) || (tmpW32>1073700000) || (esumxQQ>1073700000)) {//2^30 |
| scale++; |
| csum1QQ = WEBRTC_SPL_RSHIFT_W32(csum1QQ,1); |
| esumxQQ = WEBRTC_SPL_RSHIFT_W32(esumxQQ,1); |
| } |
| tmp2W32 = WEBRTC_SPL_RSHIFT_W32(tmp2W32,scale); |
| csum1QQ += tmp2W32; |
| tmpW32 = WEBRTC_SPL_RSHIFT_W32(tmpW32,scale); |
| esumxQQ += tmpW32; |
| |
| ind++; |
| cnt--; |
| } |
| |
| if (csum1QQ<esumxQQ) { |
| tmp2W32=WebRtcSpl_DivResultInQ31(csum1QQ,esumxQQ); |
| |
| /* Gain should be half the correlation */ |
| tmpW32=WEBRTC_SPL_RSHIFT_W32(tmp2W32,20); |
| } else |
| tmpW32=4096; |
| gainsQ12[k]=(WebRtc_Word16)WEBRTC_SPL_SAT(PITCH_MAX_GAIN_Q12, tmpW32, 0); |
| |
| } |
| |
| /* Export buffer and states */ |
| memcpy(pfp->ubufQQ, ubufQQ+PITCH_FRAME_LEN, WEBRTC_SPL_MUL_16_16(sizeof(WebRtc_Word16), PITCH_BUFFSIZE)); |
| pfp->oldlagQ7 = lagsQ7[PITCH_SUBFRAMES-1]; |
| pfp->oldgainQ12 = gainsQ12[PITCH_SUBFRAMES-1]; |
| |
| } |
