356 lines
12 KiB
C
356 lines
12 KiB
C
/*
|
|
* broadvoice - a library for the BroadVoice 16 and 32 codecs
|
|
*
|
|
* bv16coarse_pitch.c - Coarse pitch search
|
|
*
|
|
* Adapted by Steve Underwood <steveu@coppice.org> from code which is
|
|
* Copyright 2000-2009 Broadcom Corporation
|
|
*
|
|
* All rights reserved.
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU Lesser General Public License version 2.1,
|
|
* as published by the Free Software Foundation.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU Lesser General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public
|
|
* License along with this program; if not, write to the Free Software
|
|
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
*
|
|
* $Id: bv16coarse_pitch.c,v 1.1.1.1 2009/11/19 12:10:48 steveu Exp $
|
|
*/
|
|
|
|
/*! \file */
|
|
|
|
#if defined(HAVE_CONFIG_H)
|
|
#include "config.h"
|
|
#endif
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include "typedef.h"
|
|
#include "bv16cnst.h"
|
|
#include "utility.h"
|
|
#include "bv16externs.h"
|
|
|
|
int coarsepitch(
|
|
Float *xw,
|
|
Float *xwdm,
|
|
Float *dfm, /* (i/o) ellipse low pass filter memory */
|
|
int cpplast)
|
|
{
|
|
Float xwd[LXD];
|
|
Float cor[MAXPPD1];
|
|
Float cor2[MAXPPD1];
|
|
Float energy[MAXPPD1];
|
|
Float cor2i[HMAXPPD];
|
|
Float energyi[HMAXPPD];
|
|
Float mplth;
|
|
Float tmp[DFO + FRSZ];
|
|
Float threshold;
|
|
Float *fp0, *fp1, *fp2, *fp3, s, t, a, b, c, deltae;
|
|
Float cor2max, energymax, cor2m, energym, ci, eni;
|
|
int i, j, k, n, npeaks, imax, im, idx[HMAXPPD], plag[HMAXPPD];
|
|
int cpp, maxdev, flag, mpflag;
|
|
|
|
/* reset local buffers */
|
|
Fzero(cor, MAXPPD1);
|
|
Fzero(energy, MAXPPD1);
|
|
|
|
/* LOWPASS FILTER xw() TO 800 Hz; SHIFT & OUTPUT INTO xwd() */
|
|
|
|
/* copy xwd[] from memory to buffer */
|
|
Fcopy(xwd, xwdm, XDOFF);
|
|
|
|
/* copy memory to temp buffer */
|
|
Fcopy(tmp, dfm, DFO);
|
|
|
|
/* AP and AZ filtering and decimation */
|
|
fp0 = xwd + XDOFF;
|
|
fp1 = tmp + DFO;
|
|
fp3 = xw;
|
|
for (i = 0; i < FRSZD; i++)
|
|
{
|
|
for (k = 0; k < DECF; k++)
|
|
{
|
|
t = *fp3++;
|
|
fp2 = fp1 - 1;
|
|
for (j = 0; j < DFO; j++)
|
|
t -= bv16_adf[j + 1]*(*fp2--);
|
|
*fp1++ = t;
|
|
}
|
|
fp2 = fp1 - 1;
|
|
t = bv16_bdf[0]*(*fp2--);
|
|
for (j = 0; j < DFO; j++)
|
|
t += bv16_bdf[j + 1]*(*fp2--);
|
|
*fp0++ = t;
|
|
}
|
|
|
|
/* copy temp buffer to memory */
|
|
fp1 -= DFO;
|
|
for (i = 0; i < DFO; i++)
|
|
dfm[i] = *fp1++;
|
|
|
|
/* update xwd() memory */
|
|
Fcopy(xwdm, xwd + FRSZD, XDOFF);
|
|
|
|
/* COMPUTE CORRELATION & ENERGY OF PREDICTION BASIS VECTOR */
|
|
fp0 = xwd + MAXPPD1;
|
|
fp1 = xwd + MAXPPD1 - M1;
|
|
s = t = 0.0;
|
|
for (i = 0; i < (LXD - MAXPPD1); i++)
|
|
{
|
|
s += (*fp1)*(*fp1);
|
|
t += (*fp0++)*(*fp1++);
|
|
}
|
|
energy[M1 - 1] = s;
|
|
cor[M1 - 1] = t;
|
|
if (t > 0.0F)
|
|
cor2[M1 - 1] = t*t;
|
|
else
|
|
cor2[M1 - 1] = -t*t;
|
|
|
|
|
|
fp2 = xwd + LXD - M1 - 1;
|
|
fp3 = xwd + MAXPPD1 - M1 - 1;
|
|
|
|
for (i = M1; i < M2; i++)
|
|
{
|
|
fp0 = xwd + MAXPPD1;
|
|
fp1 = xwd + MAXPPD1 - i - 1;
|
|
t = 0.0;
|
|
for (j = 0; j < (LXD - MAXPPD1); j++)
|
|
t += (*fp0++)*(*fp1++);
|
|
cor[i] = t;
|
|
if (t > 0.0F)
|
|
cor2[i] = t*t;
|
|
else
|
|
cor2[i] = -t*t;
|
|
s = s - (*fp2)*(*fp2) + (*fp3)*(*fp3);
|
|
fp2--;
|
|
fp3--;
|
|
energy[i] = s;
|
|
}
|
|
|
|
/* FIND POSITIVE COR*COR/ENERGY PEAKS */
|
|
npeaks = 0;
|
|
n = MINPPD - 1;
|
|
while ((n < MAXPPD) && (npeaks < MAX_NPEAKS))
|
|
{
|
|
if ((cor2[n]*energy[n - 1] > cor2[n - 1]*energy[n])
|
|
&&
|
|
(cor2[n]*energy[n + 1] > cor2[n + 1]*energy[n])
|
|
&&
|
|
(cor2[n] > 0))
|
|
{
|
|
idx[npeaks] = n;
|
|
npeaks++;
|
|
}
|
|
n++;
|
|
}
|
|
|
|
/* RETURN EARLY IF THERE IS NO PEAK OR ONLY ONE PEAK */
|
|
if (npeaks == 0) /* if there are no positive peak, */
|
|
return MINPPD*cpp_scale; /* minimum pitch period in decimated domain */
|
|
if (npeaks == 1) /* if there is exactly one peak, */
|
|
return (idx[0] + 1)*cpp_scale; /* the time lag for this single peak */
|
|
|
|
/* IF PROGRAM PROCEEDS TO HERE, THERE ARE 2 OR MORE PEAKS */
|
|
cor2max = -1e30;
|
|
energymax = 1.0F;
|
|
imax = 0;
|
|
for (i = 0; i < npeaks; i++)
|
|
{
|
|
/* USE QUADRATIC INTERPOLATION TO FIND THE INTERPOLATED cor[] AND
|
|
energy[] CORRESPONDING TO INTERPOLATED PEAK OF cor2[]/energy[] */
|
|
/* first calculate coefficients of y(x)=ax^2+bx+c; */
|
|
n = idx[i];
|
|
a = 0.5F*(cor[n + 1] + cor[n - 1]) - cor[n];
|
|
b = 0.5F*(cor[n + 1] - cor[n - 1]);
|
|
c = cor[n];
|
|
|
|
/* INITIALIZE VARIABLES BEFORE SEARCHING FOR INTERPOLATED PEAK */
|
|
im = 0;
|
|
cor2m = cor2[n];
|
|
energym = energy[n];
|
|
eni = energy[n];
|
|
|
|
/* DERTERMINE WHICH SIDE THE INTERPOLATED PEAK FALLS IN, THEN
|
|
DO THE SEARCH IN THE APPROPRIATE RANGE */
|
|
if (cor2[n + 1]*energy[n - 1] > cor2[n - 1]*energy[n + 1]) /* if right side */
|
|
{
|
|
deltae = (energy[n + 1] - eni)*INVDECF; /*increment for linear interp.*/
|
|
for (k = 0; k < HDECF; k++)
|
|
{
|
|
ci = a*bv16_x2[k] + b*bv16_x[k] + c; /* quadratically interpolated cor[] */
|
|
eni += deltae; /* linearly interpolated energy[] */
|
|
if (ci*ci*energym > cor2m*eni)
|
|
{
|
|
im = k + 1;
|
|
cor2m = ci*ci;
|
|
energym = eni;
|
|
}
|
|
}
|
|
}
|
|
else /* if interpolated peak is on the left side */
|
|
{
|
|
deltae=(energy[n-1] - eni) * INVDECF; /*increment for linear interp.*/
|
|
for (k = 0; k < HDECF; k++)
|
|
{
|
|
ci = a*bv16_x2[k] - b*bv16_x[k] + c;
|
|
eni += deltae;
|
|
if (ci*ci*energym > cor2m*eni)
|
|
{
|
|
im = -k-1;
|
|
cor2m=ci*ci;
|
|
energym=eni;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* SEARCH DONE; ASSIGN cor2[] AND energy[] CORRESPONDING TO
|
|
INTERPOLATED PEAK */
|
|
plag[i] = (idx[i] + 1)*cpp_scale + im; /* lag of interp. peak */
|
|
cor2i[i] = cor2m; /* interpolated cor2[] of i-th interpolated peak */
|
|
energyi[i] = energym; /* interpolated energy[] of i-th interpolated peak */
|
|
|
|
/* SEARCH FOR GLOBAL MAXIMUM OF INTERPOLATED cor2[]/energy[] peak */
|
|
if (cor2m*energymax > cor2max*energym)
|
|
{
|
|
imax = i;
|
|
cor2max = cor2m;
|
|
energymax = energym;
|
|
}
|
|
}
|
|
cpp = plag[imax]; /* first candidate for coarse pitch period */
|
|
mplth = plag[npeaks - 1]*1.0/DECF; /* plag[] is Q2 */
|
|
|
|
/* FIND THE LARGEST PEAK (IF THERE IS ANY) AROUND THE LAST PITCH */
|
|
maxdev = (int) (DEVTH*cpplast); /* maximum deviation from last pitch */
|
|
im = -1;
|
|
cor2m = -1e30;
|
|
energym = 1.0F;
|
|
for (i = 0; i < npeaks; i++) /* loop thru the peaks before the largest peak */
|
|
{
|
|
if (abs(plag[i] - cpplast) <= maxdev)
|
|
{
|
|
if (cor2i[i]*energym > cor2m*energyi[i])
|
|
{
|
|
im = i;
|
|
cor2m = cor2i[i];
|
|
energym = energyi[i];
|
|
}
|
|
}
|
|
} /* if there is no peaks around last pitch, then im is still -1 */
|
|
|
|
/* NOW SEE IF WE SHOULD PICK ANY ALTERNATICE PEAK */
|
|
/* FIRST, SEARCH FIRST HALF OF PITCH RANGE, SEE IF ANY QUALIFIED PEAK
|
|
HAS LARGE ENOUGH PEAKS AT EVERY MULTIPLE OF ITS LAG */
|
|
i = 0;
|
|
while (plag[i] < 0.5*mplth*DECF)
|
|
{
|
|
/* DETERMINE THE APPROPRIATE THRESHOLD FOR THIS PEAK */
|
|
if (i != im) /* if not around last pitch, */
|
|
{
|
|
threshold = TH1; /* use a higher threshold */
|
|
}
|
|
else /* if around last pitch */
|
|
{
|
|
threshold = TH2; /* use a lower threshold */
|
|
}
|
|
|
|
/* IF THRESHOLD EXCEEDED, TEST PEAKS AT MULTIPLES OF THIS LAG */
|
|
if (cor2i[i]*energymax > threshold*cor2max*energyi[i])
|
|
{
|
|
flag = 1;
|
|
j = i + 1;
|
|
k = 0;
|
|
s = 2.0F*plag[i]; /* initialize t to twice the current lag */
|
|
while (s <= mplth*DECF) /* loop thru all multiple lag <= mplth */
|
|
{
|
|
mpflag = 0; /* initialize multiple pitch flag to 0 */
|
|
a = MPR1*s; /* multiple pitch range lower bound */
|
|
b = MPR2*s; /* multiple pitch range upper bound */
|
|
while (j < npeaks) /* loop thru peaks with larger lags */
|
|
{
|
|
if (plag[j] > b) /* if range exceeded, */
|
|
{
|
|
break; /* break the innermost while loop */
|
|
} /* if didn't break, then plag[j] <= b */
|
|
if (plag[j] > a) /* if current peak lag within range, */
|
|
{
|
|
/* then check if peak value large enough */
|
|
if (k < 4)
|
|
{
|
|
c = bv16_MPTH[k];
|
|
}
|
|
else
|
|
{
|
|
c = MPTH4;
|
|
}
|
|
if (cor2i[j]*energymax > c*cor2max*energyi[j])
|
|
{
|
|
mpflag = 1; /* if peak large enough, set mpflag, */
|
|
break; /* and break the innermost while loop */
|
|
}
|
|
}
|
|
j++;
|
|
}
|
|
/* if no qualified peak found at this multiple lag */
|
|
if (mpflag == 0)
|
|
{
|
|
flag = 0; /* disqualify the lag plag[i] */
|
|
break; /* and break the while (s<=mplth) loop */
|
|
}
|
|
k++;
|
|
s += plag[i]; /* update s to the next multiple pitch lag */
|
|
}
|
|
/* if there is a qualified peak at every multiple of plag[i], */
|
|
if (flag == 1)
|
|
return plag[i]; /* and return to calling function */
|
|
}
|
|
i++;
|
|
if (i == npeaks)
|
|
break; /* to avoid out of array bound error */
|
|
}
|
|
|
|
/* IF PROGRAM PROCEEDS TO HERE, NONE OF THE PEAKS WITH LAGS < 0.5*mplth
|
|
QUALIFIES AS THE FINAL PITCH. IN THIS CASE, CHECK IF
|
|
THERE IS ANY PEAK LARGE ENOUGH AROUND LAST PITCH. IF SO, USE ITS
|
|
LAG AS THE FINAL PITCH. */
|
|
if (im != -1) /* if there is at least one peak around last pitch */
|
|
{
|
|
if (im == imax) /* if this peak is also the global maximum, */
|
|
return cpp; /* return first pitch candidate at global max */
|
|
if (im < imax) /* if lag of this peak < lag of global max, */
|
|
{
|
|
if (cor2m*energymax > LPTH2*cor2max*energym)
|
|
{
|
|
if (plag[im] > HMAXPPD*cpp_scale)
|
|
return plag[im];
|
|
for (k = 2; k <= 5; k++) /* check if current candidate pitch */
|
|
{
|
|
s = plag[imax]/(float)(k); /* is a sub-multiple of */
|
|
a = SMDTH1*s; /* the time lag of */
|
|
b = SMDTH2*s; /* the global maximum peak */
|
|
if (plag[im] > a && plag[im] < b) /* if so, */
|
|
return plag[im]; /* and return as pitch */
|
|
}
|
|
}
|
|
}
|
|
else /* if lag of this peak > lag of global max, */
|
|
{
|
|
if (cor2m*energymax > LPTH1*cor2max*energym)
|
|
return plag[im]; /* accept its lag */
|
|
}
|
|
}
|
|
|
|
/* IF PROGRAM PROCEEDS TO HERE, WE HAVE NO CHOICE BUT TO ACCEPT THE
|
|
LAG OF THE GLOBAL MAXIMUM */
|
|
return cpp;
|
|
}
|