| /* |
| * 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. |
| */ |
| |
| #include "pitch_estimator.h" |
| #include "os_specific_inline.h" |
| |
| #include <stdlib.h> |
| #include <memory.h> |
| #include <math.h> |
| |
| static const double kDampFilter[PITCH_DAMPORDER] = {-0.07, 0.25, 0.64, 0.25, -0.07}; |
| |
| /* interpolation coefficients; generated by design_pitch_filter.m */ |
| static const double kIntrpCoef[PITCH_FRACS][PITCH_FRACORDER] = { |
| {-0.02239172458614, 0.06653315052934, -0.16515880017569, 0.60701333734125, 0.64671399919202, -0.20249000396417, 0.09926548334755, -0.04765933793109, 0.01754159521746}, |
| {-0.01985640750434, 0.05816126837866, -0.13991265473714, 0.44560418147643, 0.79117042386876, -0.20266133815188, 0.09585268418555, -0.04533310458084, 0.01654127246314}, |
| {-0.01463300534216, 0.04229888475060, -0.09897034715253, 0.28284326017787, 0.90385267956632, -0.16976950138649, 0.07704272393639, -0.03584218578311, 0.01295781500709}, |
| {-0.00764851320885, 0.02184035544377, -0.04985561057281, 0.13083306574393, 0.97545011664662, -0.10177807997561, 0.04400901776474, -0.02010737175166, 0.00719783432422}, |
| {-0.00000000000000, 0.00000000000000, -0.00000000000001, 0.00000000000001, 0.99999999999999, 0.00000000000001, -0.00000000000001, 0.00000000000000, -0.00000000000000}, |
| { 0.00719783432422, -0.02010737175166, 0.04400901776474, -0.10177807997562, 0.97545011664663, 0.13083306574393, -0.04985561057280, 0.02184035544377, -0.00764851320885}, |
| { 0.01295781500710, -0.03584218578312, 0.07704272393640, -0.16976950138650, 0.90385267956634, 0.28284326017785, -0.09897034715252, 0.04229888475059, -0.01463300534216}, |
| { 0.01654127246315, -0.04533310458085, 0.09585268418557, -0.20266133815190, 0.79117042386878, 0.44560418147640, -0.13991265473712, 0.05816126837865, -0.01985640750433} |
| }; |
| |
| |
| void WebRtcIsac_PitchfilterPre(double *indat, |
| double *outdat, |
| PitchFiltstr *pfp, |
| double *lags, |
| double *gains) |
| { |
| |
| double ubuf[PITCH_INTBUFFSIZE]; |
| const double *fracoeff = NULL; |
| double curgain, curlag, gaindelta, lagdelta; |
| double sum, inystate[PITCH_DAMPORDER]; |
| double ftmp, oldlag, oldgain; |
| int k, n, m, pos, ind, pos2, Li, frc; |
| |
| Li = 0; |
| /* Set up buffer and states */ |
| memcpy(ubuf, pfp->ubuf, sizeof(double) * PITCH_BUFFSIZE); |
| memcpy(inystate, pfp->ystate, sizeof(double) * PITCH_DAMPORDER); |
| |
| oldlag = *pfp->oldlagp; |
| oldgain = *pfp->oldgainp; |
| |
| /* No interpolation if pitch lag step is big */ |
| if ((lags[0] > (PITCH_UPSTEP * oldlag)) || (lags[0] < (PITCH_DOWNSTEP * oldlag))) { |
| oldlag = lags[0]; |
| oldgain = gains[0]; |
| } |
| |
| ind=0; |
| for (k=0;k<PITCH_SUBFRAMES;k++) { |
| |
| /* Calculate interpolation steps */ |
| lagdelta=(lags[k]-oldlag) / PITCH_GRAN_PER_SUBFRAME; |
| curlag=oldlag ; |
| gaindelta=(gains[k]-oldgain) / PITCH_GRAN_PER_SUBFRAME; |
| curgain=oldgain ; |
| oldlag=lags[k]; |
| oldgain=gains[k]; |
| |
| for (n=0;n<PITCH_SUBFRAME_LEN;n++) { |
| if ((ind % PITCH_UPDATE) == 0) { /* Update parameters */ |
| curgain += gaindelta; |
| curlag += lagdelta; |
| Li = WebRtcIsac_lrint(curlag+PITCH_FILTDELAY + 0.5); |
| ftmp = Li - (curlag+PITCH_FILTDELAY); |
| frc = WebRtcIsac_lrint(PITCH_FRACS * ftmp - 0.5); |
| fracoeff = kIntrpCoef[frc]; |
| } |
| |
| /* shift low pass filter state */ |
| for (m=PITCH_DAMPORDER-1;m>0;m--) |
| inystate[m] = inystate[m-1]; |
| |
| /* Filter to get fractional pitch */ |
| pos = ind + PITCH_BUFFSIZE; |
| pos2 = pos - Li; |
| sum=0; |
| for (m=0;m<PITCH_FRACORDER;m++) |
| sum += ubuf[pos2+m] * fracoeff[m]; |
| inystate[0] = curgain * sum; /* Multiply with gain */ |
| |
| /* Low pass filter */ |
| sum=0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| sum += inystate[m] * kDampFilter[m]; |
| |
| /* Subtract from input and update buffer */ |
| outdat[ind] = indat[ind] - sum; |
| ubuf[pos] = indat[ind] + outdat[ind]; |
| ind++; |
| } |
| } |
| |
| /* Export buffer and states */ |
| memcpy(pfp->ubuf, ubuf+PITCH_FRAME_LEN, sizeof(double) * PITCH_BUFFSIZE); |
| memcpy(pfp->ystate, inystate, sizeof(double) * PITCH_DAMPORDER); |
| |
| *pfp->oldlagp = oldlag; |
| *pfp->oldgainp = oldgain; |
| |
| } |
| |
| |
| void WebRtcIsac_PitchfilterPre_la(double *indat, |
| double *outdat, |
| PitchFiltstr *pfp, |
| double *lags, |
| double *gains) |
| { |
| double ubuf[PITCH_INTBUFFSIZE+QLOOKAHEAD]; |
| const double *fracoeff = NULL; |
| double curgain, curlag, gaindelta, lagdelta; |
| double sum, inystate[PITCH_DAMPORDER]; |
| double ftmp; |
| double oldlag, oldgain; |
| int k, n, m, pos, ind, pos2, Li, frc; |
| |
| Li = 0; |
| /* Set up buffer and states */ |
| memcpy(ubuf, pfp->ubuf, sizeof(double) * PITCH_BUFFSIZE); |
| memcpy(inystate, pfp->ystate, sizeof(double) * PITCH_DAMPORDER); |
| |
| oldlag = *pfp->oldlagp; |
| oldgain = *pfp->oldgainp; |
| |
| /* No interpolation if pitch lag step is big */ |
| if ((lags[0] > (PITCH_UPSTEP * oldlag)) || (lags[0] < (PITCH_DOWNSTEP * oldlag))) { |
| oldlag = lags[0]; |
| oldgain = gains[0]; |
| } |
| |
| |
| ind=0; |
| for (k=0;k<PITCH_SUBFRAMES;k++) { |
| |
| /* Calculate interpolation steps */ |
| lagdelta=(lags[k]-oldlag) / PITCH_GRAN_PER_SUBFRAME; |
| curlag=oldlag ; |
| gaindelta=(gains[k]-oldgain) / PITCH_GRAN_PER_SUBFRAME; |
| curgain=oldgain ; |
| oldlag=lags[k]; |
| oldgain=gains[k]; |
| |
| for (n=0;n<PITCH_SUBFRAME_LEN;n++) { |
| if ((ind % PITCH_UPDATE) == 0) { /* Update parameters */ |
| curgain += gaindelta; |
| curlag += lagdelta; |
| Li = WebRtcIsac_lrint(curlag+PITCH_FILTDELAY + 0.5); |
| ftmp = Li - (curlag+PITCH_FILTDELAY); |
| frc = WebRtcIsac_lrint(PITCH_FRACS * ftmp - 0.5); |
| fracoeff = kIntrpCoef[frc]; |
| } |
| |
| /* shift low pass filter state */ |
| for (m=PITCH_DAMPORDER-1;m>0;m--) |
| inystate[m] = inystate[m-1]; |
| |
| /* Filter to get fractional pitch */ |
| pos = ind + PITCH_BUFFSIZE; |
| pos2 = pos - Li; |
| sum=0.0; |
| for (m=0;m<PITCH_FRACORDER;m++) |
| sum += ubuf[pos2+m] * fracoeff[m]; |
| inystate[0] = curgain * sum; /* Multiply with gain */ |
| |
| /* Low pass filter */ |
| sum=0.0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| sum += inystate[m] * kDampFilter[m]; |
| |
| /* Subtract from input and update buffer */ |
| outdat[ind] = indat[ind] - sum; |
| ubuf[pos] = indat[ind] + outdat[ind]; |
| ind++; |
| } |
| } |
| |
| /* Export buffer and states */ |
| memcpy(pfp->ubuf, ubuf+PITCH_FRAME_LEN, sizeof(double) * PITCH_BUFFSIZE); |
| memcpy(pfp->ystate, inystate, sizeof(double) * PITCH_DAMPORDER); |
| |
| *pfp->oldlagp = oldlag; |
| *pfp->oldgainp = oldgain; |
| |
| |
| /* Filter look-ahead segment */ |
| for (n=0;n<QLOOKAHEAD;n++) { |
| /* shift low pass filter state */ |
| for (m=PITCH_DAMPORDER-1;m>0;m--) |
| inystate[m] = inystate[m-1]; |
| |
| /* Filter to get fractional pitch */ |
| pos = ind + PITCH_BUFFSIZE; |
| pos2 = pos - Li; |
| sum=0.0; |
| for (m=0;m<PITCH_FRACORDER;m++) |
| sum += ubuf[pos2+m] * fracoeff[m]; |
| inystate[0] = curgain * sum; /* Multiply with gain */ |
| |
| /* Low pass filter */ |
| sum=0.0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| sum += inystate[m] * kDampFilter[m]; |
| |
| /* Subtract from input and update buffer */ |
| outdat[ind] = indat[ind] - sum; |
| ubuf[pos] = indat[ind] + outdat[ind]; |
| ind++; |
| } |
| } |
| |
| |
| void WebRtcIsac_PitchfilterPre_gains(double *indat, |
| double *outdat, |
| double out_dG[][PITCH_FRAME_LEN + QLOOKAHEAD], |
| PitchFiltstr *pfp, |
| double *lags, |
| double *gains) |
| { |
| double ubuf[PITCH_INTBUFFSIZE+QLOOKAHEAD]; |
| double inystate_dG[4][PITCH_DAMPORDER]; |
| double gain_mult[4]; |
| const double *fracoeff = NULL; |
| double curgain, curlag, gaindelta, lagdelta; |
| double sum, sum2, inystate[PITCH_DAMPORDER]; |
| double ftmp, oldlag, oldgain; |
| int k, n, m, m_tmp, j, pos, ind, pos2, Li, frc; |
| |
| Li = 0; |
| |
| /* Set up buffer and states */ |
| memcpy(ubuf, pfp->ubuf, sizeof(double) * PITCH_BUFFSIZE); |
| memcpy(inystate, pfp->ystate, sizeof(double) * PITCH_DAMPORDER); |
| |
| /* clear some buffers */ |
| for (k = 0; k < 4; k++) { |
| gain_mult[k] = 0.0; |
| for (n = 0; n < PITCH_DAMPORDER; n++) |
| inystate_dG[k][n] = 0.0; |
| } |
| |
| oldlag = *pfp->oldlagp; |
| oldgain = *pfp->oldgainp; |
| |
| /* No interpolation if pitch lag step is big */ |
| if ((lags[0] > (PITCH_UPSTEP * oldlag)) || (lags[0] < (PITCH_DOWNSTEP * oldlag))) { |
| oldlag = lags[0]; |
| oldgain = gains[0]; |
| gain_mult[0] = 1.0; |
| } |
| |
| |
| ind=0; |
| for (k=0;k<PITCH_SUBFRAMES;k++) { |
| |
| /* Calculate interpolation steps */ |
| lagdelta=(lags[k]-oldlag) / PITCH_GRAN_PER_SUBFRAME; |
| curlag=oldlag ; |
| gaindelta=(gains[k]-oldgain) / PITCH_GRAN_PER_SUBFRAME; |
| curgain=oldgain ; |
| oldlag=lags[k]; |
| oldgain=gains[k]; |
| |
| for (n=0;n<PITCH_SUBFRAME_LEN;n++) { |
| if ((ind % PITCH_UPDATE) == 0) { /* Update parameters */ |
| curgain += gaindelta; |
| curlag += lagdelta; |
| Li = WebRtcIsac_lrint(curlag+PITCH_FILTDELAY + 0.5); |
| ftmp = Li - (curlag+PITCH_FILTDELAY); |
| frc = WebRtcIsac_lrint(PITCH_FRACS * ftmp - 0.5); |
| fracoeff = kIntrpCoef[frc]; |
| gain_mult[k] += 0.2; |
| if (gain_mult[k] > 1.0) gain_mult[k] = 1.0; |
| if (k > 0) gain_mult[k-1] -= 0.2; |
| } |
| |
| /* shift low pass filter states */ |
| for (m=PITCH_DAMPORDER-1;m>0;m--) { |
| inystate[m] = inystate[m-1]; |
| for (j = 0; j < 4; j++) |
| inystate_dG[j][m] = inystate_dG[j][m-1]; |
| } |
| |
| pos = ind + PITCH_BUFFSIZE; |
| pos2 = pos - Li; |
| |
| /* Filter to get fractional pitch */ |
| sum=0.0; |
| for (m=0;m<PITCH_FRACORDER;m++) |
| sum += ubuf[pos2+m] * fracoeff[m]; |
| inystate[0] = curgain * sum; /* Multiply with gain */ |
| m_tmp = (Li-ind > 0) ? Li-ind : 0; |
| for (j = 0; j < k+1; j++) { |
| /* filter */ |
| sum2 = 0.0; |
| for (m = PITCH_FRACORDER-1; m >= m_tmp; m--) |
| sum2 += out_dG[j][ind-Li + m] * fracoeff[m]; |
| inystate_dG[j][0] = gain_mult[j] * sum + curgain * sum2; |
| } |
| |
| /* Low pass filter */ |
| sum=0.0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| sum += inystate[m] * kDampFilter[m]; |
| |
| /* Subtract from input and update buffer */ |
| outdat[ind] = indat[ind] - sum; |
| ubuf[pos] = indat[ind] + outdat[ind]; |
| |
| for (j = 0; j < k+1; j++) { |
| sum = 0.0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| sum -= inystate_dG[j][m] * kDampFilter[m]; |
| out_dG[j][ind] = sum; |
| } |
| for (j = k+1; j < 4; j++) |
| out_dG[j][ind] = 0.0; |
| |
| |
| ind++; |
| } |
| } |
| |
| /* Filter look-ahead segment */ |
| for (n=0;n<QLOOKAHEAD;n++) { |
| /* shift low pass filter states */ |
| for (m=PITCH_DAMPORDER-1;m>0;m--) { |
| inystate[m] = inystate[m-1]; |
| for (j = 0; j < 4; j++) |
| inystate_dG[j][m] = inystate_dG[j][m-1]; |
| } |
| |
| pos = ind + PITCH_BUFFSIZE; |
| pos2 = pos - Li; |
| |
| /* Filter to get fractional pitch */ |
| sum=0.0; |
| for (m=0;m<PITCH_FRACORDER;m++) |
| sum += ubuf[pos2+m] * fracoeff[m]; |
| inystate[0] = curgain * sum; /* Multiply with gain */ |
| m_tmp = (Li-ind > 0) ? Li-ind : 0; |
| for (j = 0; (j<k+1)&&(j<4); j++) { |
| /* filter */ |
| sum2 = 0.0; |
| for (m = PITCH_FRACORDER-1; m >= m_tmp; m--) |
| sum2 += out_dG[j][ind-Li + m] * fracoeff[m]; |
| inystate_dG[j][0] = gain_mult[j] * sum + curgain * sum2; |
| } |
| |
| /* Low pass filter */ |
| sum=0.0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| sum += inystate[m] * kDampFilter[m]; |
| |
| /* Subtract from input and update buffer */ |
| outdat[ind] = indat[ind] - sum; |
| ubuf[pos] = indat[ind] + outdat[ind]; |
| |
| for (j = 0; (j<k+1)&&(j<4); j++) { |
| sum = 0.0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| sum -= inystate_dG[j][m] * kDampFilter[m]; |
| out_dG[j][ind] = sum; |
| } |
| |
| ind++; |
| } |
| } |
| |
| |
| void WebRtcIsac_PitchfilterPost(double *indat, |
| double *outdat, |
| PitchFiltstr *pfp, |
| double *lags, |
| double *gains) |
| { |
| |
| double ubuf[PITCH_INTBUFFSIZE]; |
| const double *fracoeff = NULL; |
| double curgain, curlag, gaindelta, lagdelta; |
| double sum, inystate[PITCH_DAMPORDER]; |
| double ftmp, oldlag, oldgain; |
| int k, n, m, pos, ind, pos2, Li, frc; |
| |
| Li = 0; |
| |
| /* Set up buffer and states */ |
| memcpy(ubuf, pfp->ubuf, sizeof(double) * PITCH_BUFFSIZE); |
| memcpy(inystate, pfp->ystate, sizeof(double) * PITCH_DAMPORDER); |
| |
| oldlag = *pfp->oldlagp; |
| oldgain = *pfp->oldgainp; |
| |
| /* make output more periodic */ |
| for (k=0;k<PITCH_SUBFRAMES;k++) |
| gains[k] *= 1.3; |
| |
| /* No interpolation if pitch lag step is big */ |
| if ((lags[0] > (PITCH_UPSTEP * oldlag)) || (lags[0] < (PITCH_DOWNSTEP * oldlag))) { |
| oldlag = lags[0]; |
| oldgain = gains[0]; |
| } |
| |
| |
| ind=0; |
| for (k=0;k<PITCH_SUBFRAMES;k++) { |
| |
| /* Calculate interpolation steps */ |
| lagdelta=(lags[k]-oldlag) / PITCH_GRAN_PER_SUBFRAME; |
| curlag=oldlag ; |
| gaindelta=(gains[k]-oldgain) / PITCH_GRAN_PER_SUBFRAME; |
| curgain=oldgain ; |
| oldlag=lags[k]; |
| oldgain=gains[k]; |
| |
| for (n=0;n<PITCH_SUBFRAME_LEN;n++) { |
| if ((ind % PITCH_UPDATE) == 0) { /* Update parameters */ |
| curgain += gaindelta; |
| curlag += lagdelta; |
| Li = WebRtcIsac_lrint(curlag+PITCH_FILTDELAY + 0.5); |
| ftmp = Li - (curlag+PITCH_FILTDELAY); |
| frc = WebRtcIsac_lrint(PITCH_FRACS * ftmp - 0.5); |
| fracoeff = kIntrpCoef[frc]; |
| } |
| |
| /* shift low pass filter state */ |
| for (m=PITCH_DAMPORDER-1;m>0;m--) |
| inystate[m] = inystate[m-1]; |
| |
| /* Filter to get fractional pitch */ |
| pos = ind + PITCH_BUFFSIZE; |
| pos2 = pos - Li; |
| sum=0.0; |
| for (m=0;m<PITCH_FRACORDER;m++) |
| sum += ubuf[pos2+m] * fracoeff[m]; |
| inystate[0] = curgain * sum; /* Multiply with gain */ |
| |
| /* Low pass filter */ |
| sum=0.0; |
| for (m=0;m<PITCH_DAMPORDER;m++) |
| sum += inystate[m] * kDampFilter[m]; |
| |
| /* Add to input and update buffer */ |
| outdat[ind] = indat[ind] + sum; |
| ubuf[pos] = indat[ind] + outdat[ind]; |
| ind++; |
| } |
| } |
| |
| /* Export buffer and states */ |
| memcpy(pfp->ubuf, ubuf+PITCH_FRAME_LEN, sizeof(double) * PITCH_BUFFSIZE); |
| memcpy(pfp->ystate, inystate, sizeof(double) * PITCH_DAMPORDER); |
| |
| *pfp->oldlagp = oldlag; |
| *pfp->oldgainp = oldgain; |
| |
| } |
