RuhNetConsulting
/
bcg729
mirror of https://github.com/BelledonneCommunications/bcg729
1
0
Fork 0
Code Issues 0 Projects 0 Releases 8 Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
150 Commits
4 Branches
4.5 MiB
 
 
 
 
 
Tree: b9d2ad4c5f
Branches Tags
${ item.name }
Create branch ${ searchTerm }
from 'b9d2ad4c5f'
${ noResults }
bcg729/src/fixedCodebookSearch.c

398 lines
20 KiB
Raw Blame History

/*
* Copyright (c) 2011-2019 Belledonne Communications SARL.
*
* This file is part of bcg729.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "typedef.h"
#include "codecParameters.h"
#include "basicOperationsMacros.h"
#include "utils.h"
#include <stdlib.h>
#include "fixedCodebookSearch.h"
/*** local functions ***/
void computeImpulseResponseCorrelationMatrix(word16_t impulseResponse[], word16_t correlationSignal[], int correlationSignalSign[], word32_t Phi[L_SUBFRAME][L_SUBFRAME]);
void computePhiDiagonal(int j, word16_t impulseResponse[], word32_t Phi[L_SUBFRAME][L_SUBFRAME], uint16_t PhiScaling);
/*****************************************************************************/
/* fixedCodebookSearch: compute fixed codebook parameters (codeword and sign)*/
/* compute also fixed codebook vector as in spec 3.8.1 */
/* parameters: */
/* -(i) targetSignal: 40 values as in spec A.3.6 in Q0 */
/* -(i) impulseResponse: 40 values as in spec A.3.5 in Q12 */
/* -(i) intPitchDelay: current integer pitch delay */
/* -(i) lastQuantizedAdaptativeCodebookGain: previous subframe pitch */
/* gain quantized in Q14 */
/* -(i) filteredAdaptativeCodebookVector : 40 values in Q0 */
/* -(i) adaptativeCodebookGain : in Q14 */
/* -(o) fixedCodebookParameter */
/* -(o) fixedCodebookPulsesSigns */
/* -(o) fixedCodebookVector : 40 values as in spec 3.8, eq45 in Q13 */
/* -(o) fixedCodebookVectorConvolved : 40 values as in spec 3.9, eq64 */
/* in Q12. */
/* */
/*****************************************************************************/
void fixedCodebookSearch(word16_t targetSignal[], word16_t impulseResponse[], int16_t intPitchDelay, word16_t lastQuantizedAdaptativeCodebookGain, word16_t filteredAdaptativeCodebookVector[], word16_t adaptativeCodebookGain,
uint16_t *fixedCodebookParameter, uint16_t *fixedCodebookPulsesSigns, word16_t fixedCodebookVector[], word16_t fixedCodebookVectorConvolved[])
{
int i,j,n;
word16_t fixedCodebookTargetSignal[L_SUBFRAME];
word32_t correlationSignal32[L_SUBFRAME]; /* on 32 bits in Q12 */
word16_t correlationSignal[L_SUBFRAME]; /* normalised to fit on 13 bits */
word32_t correlationSignalMax = 0;
word32_t abscCrrelationSignal32;
uint16_t correlationSignalMaxNorm;
int correlationSignalSign[L_SUBFRAME]; /* to store the sign of each correlationSignal element */
/* build the matrix Ф' : impulseResponse correlation matrix spec 3.8.1 eq51, eq56 and eq57 */
/* correlationSignal turns to absolute values and sign of elements is stored in correlationSignalSign */
word32_t Phi[L_SUBFRAME][L_SUBFRAME] = {{0}};
int m3Base;
int i0=0, i1=0, i2=0, i3=0;
word32_t correlationSquareMax = -1;
word32_t energyMax = 1;
int m0=0, m1=0, m2=0, m3=0;
int mSwitch[2][4] = {{2,3,0,1},{3,0,1,2}};
int mIndex;
int jx = 0;
/* compute the target signal for fixed codebook spec 3.8.1 eq50 : fixedCodebookTargetSignal[i] = targetSignal[i] - (adaptativeCodebookGain * filteredAdaptativeCodebookVector[i]) */
for (i=0; i<L_SUBFRAME; i++) {
fixedCodebookTargetSignal[i] = MSU16_16_Q14(targetSignal[i], filteredAdaptativeCodebookVector[i], adaptativeCodebookGain); /* adaptativeCodebookGain in Q14, other values in Q0 */
}
/* update impulse vector as in spec 3.8 eq49 */
for (i=intPitchDelay; i<L_SUBFRAME; i++) {
impulseResponse[i] = MAC16_16_Q14(impulseResponse[i], impulseResponse[i-intPitchDelay], lastQuantizedAdaptativeCodebookGain); /* h[n] = h[n] + β*h[n-T], impulseResponse in Q12, lastQuantizedAdaptativeCodebookGain in Q14 */
}
/* compute the correlation signal as in spec 3.8.1 eq52 */
/* compute on 32 bits and get the maximum */
for (n=0; n<L_SUBFRAME; n++) {
correlationSignal32[n] = 0;
for (i=n; i<L_SUBFRAME; i++) {
correlationSignal32[n] = MAC16_16(correlationSignal32[n], fixedCodebookTargetSignal[i], impulseResponse[i-n]);
}
abscCrrelationSignal32 = correlationSignal32[n]>=0?correlationSignal32[n]:-correlationSignal32[n];
if (abscCrrelationSignal32>correlationSignalMax) {
correlationSignalMax = abscCrrelationSignal32;
}
}
/* normalise on 13 bits */
correlationSignalMaxNorm = countLeadingZeros(correlationSignalMax);
if (correlationSignalMaxNorm<18) { /* if it doesn't already fit on 13 bits */
for (i=0; i<L_SUBFRAME; i++) {
correlationSignal[i] = (word16_t)(SHR(correlationSignal32[i], 18-correlationSignalMaxNorm));
}
} else { /* it fits on 13 bits, just copy it to the 16 bits buffer */
for (i=0; i<L_SUBFRAME; i++) {
correlationSignal[i] = (word16_t)correlationSignal32[i];
}
}
computeImpulseResponseCorrelationMatrix(impulseResponse, correlationSignal, correlationSignalSign, Phi);
/* search for impulses leading to a max in C^2/E : spec 3.8.1 eq53 */
/* algorithm, not described in spec, retrieved from ITU code */
/* by tracks are intended series of index m0 track is 0,5,10,...35. m1 is 1,6,11,..,36. m2 is 2,7,12,..,37. m3 is 3,8,13,..,38. m4 is 4,9,14,..,39 */
/* note index m3 will follow track m3 and m4 */
/* The following search is performed twice: first with m3 index following m3 track, and then m3 index follow m4 track */
/* The following operation are performed twice, first using indexes m2, m3, m0, m1 and then m3, m0, m1, m2 as following */
/* description is made for the first run, for the second one, juste substitute m2 by m3, m3 by m0, m0 by m1 and m1 by m2 */
/* - search in m2 track two maxima for the correlation Signal. For each of this maximum : */
/* -- compute for the whole m3 track (8 values) the values C^2 and E (see eq58 and 59) and keep the one giving the best ratio */
/* - compute for the whole tracks m0 and m1 (64 values) the values C^2 and E (keeping the m2 and m3 previously computed) and save the one giving the best ratio */
for (m3Base=3; m3Base<5; m3Base++) {
for(mIndex=0; mIndex<2; mIndex++) {
/* define for this loop on m3 track the Correlation and Energy giving the maximum of eq53 */
word32_t m3TrackCorrelationSquare = -1;
word32_t m3TrackEnergy = 1;
/* Loop on the two maxima of correlation in the m2 index */
int firstM2 = 0; /* save the first maximum index to not select it again */
word16_t correlationM2M3Max = 0; /* stores the contribution of m2 and m3 impulses to the correlation for the maximum selected */
word32_t energyM2M3Max = 0; /* same thing but for the energy */
for (i=0; i<2; i++) {
word16_t correlationM2 = -1;
int currentM2=0;
word32_t energyM2;
for (j=mSwitch[mIndex][0]; j<L_SUBFRAME; j+=5) { /* in the m2 range, find the correlation Max -> select m2 */
if (correlationSignal[j]>correlationM2 && j!=firstM2) {
currentM2 = j;
correlationM2=correlationSignal[j];
}
}
firstM2 = currentM2; /* to avoid selecting the same maximum at next iteration */
energyM2 = Phi[currentM2][currentM2]; /* compute the energy with terms of eq55 using m2 only: Phi'(m2,m2) */
/* with selected m2, test the 8 m3 possibilities for the current m3 track */
for (j=mSwitch[mIndex][1]; j<L_SUBFRAME; j+=5) {
word16_t correlationM2M3 = ADD16(correlationM2, correlationSignal[j]); /* compute the correlation sum due to m2 and m3 pulses */
word32_t energyM2M3 = ADD32(energyM2, ADD32(Phi[currentM2][j], Phi[j][j])); /* compute the energy if eq55 using term including m2 and m3: Phi'(m2,m2) is already in energyM2 + Phi'(m2,m3) + Phi'(m3,m3) */
word32_t correlationM2M3Square = MULT16_16(correlationM2M3, correlationM2M3);
/* check if the current correlation/energy couple gives better results than the stored one : maximise C^2/E -> C^2/E > C^2max/Emax => Emax*C^2 > C^2max*E */
if (MULT32_32(m3TrackEnergy,correlationM2M3Square) > MULT32_32(energyM2M3, m3TrackCorrelationSquare)) {
m3TrackCorrelationSquare = correlationM2M3Square;
m3TrackEnergy = energyM2M3;
correlationM2M3Max = correlationM2M3;
m3 = j;
m2 = currentM2;
}
}
}
energyM2M3Max = m3TrackEnergy;
/* reset the current m3 track correlationSquare and energy */
m3TrackCorrelationSquare = -1;
m3TrackEnergy = 1;
for (i=mSwitch[mIndex][2]; i<L_SUBFRAME; i+=5) { /* test the 8 possibilities for m0 track */
word16_t correlationM2M3M0 = ADD16(correlationM2M3Max, correlationSignal[i]); /* compute correlation with current m0 taking in account the previously selected m2 and m3 */
word32_t energyM2M3M0 = ADD32(energyM2M3Max, ADD32(Phi[i][i], ADD32(Phi[i][m2], Phi[i][m3]))); /* add to the previously computed energy the terms of eq59 we can compute with the selected m0: Phi'(m0,m0) + Phi'(m0,m2) + Phi'(m0,m3) */
for (j=mSwitch[mIndex][3]; j<L_SUBFRAME; j+=5) { /* test the 8 possibilities for m1 track */
word16_t correlationM2M3M0M1 = ADD16(correlationM2M3M0, correlationSignal[j]); /* compute correlation with current m1 taking in account the previously selected m2, m3 and m0 */
word32_t energyM2M3M0M1 = ADD32(energyM2M3M0, ADD32(Phi[j][i], ADD32(Phi[j][j], ADD32(Phi[j][m2], Phi[j][m3])))); /* add to the previously computed energy the terms of eq59 we can compute with the selected m1: Phi'(m1,m0) + Phi'(m1,m1) + Phi'(m1,m2) + Phi'(m1,m3) */
word32_t correlationM2M3M0M1Square = MULT16_16(correlationM2M3M0M1, correlationM2M3M0M1);
/* check if the current correlation/energy couple gives better results than the stored one : maximise C^2/E -> C^2/E > C^2max/Emax => Emax*C^2 > C^2max*E */
if (MULT32_32(m3TrackEnergy,correlationM2M3M0M1Square) > MULT32_32(energyM2M3M0M1, m3TrackCorrelationSquare)) {
m3TrackCorrelationSquare = correlationM2M3M0M1Square;
m3TrackEnergy = energyM2M3M0M1;
m1 = j;
m0 = i;
}
}
}
/* check with currently selected indexes if this one is better */
if (MULT32_32(energyMax,m3TrackCorrelationSquare) > MULT32_32(m3TrackEnergy, correlationSquareMax)) {
correlationSquareMax = m3TrackCorrelationSquare;
energyMax = m3TrackEnergy;
if (mIndex==0) {
i0 = m0;
i1 = m1;
i2 = m2;
i3 = m3;
} else {
i0 = m3;
i1 = m0;
i2 = m1;
i3 = m2;
}
jx = m3Base - 3; /* needed for parameter computation apec 3.8.2 eq62 */
}
}
mSwitch[0][1]++; mSwitch[1][0]++; /*increment the m3Base into the mSwitch */
}
/* compute the fixedCodebookVector */
for (i=0; i<L_SUBFRAME; i++) {
fixedCodebookVector[i] = 0; /* reset the vector */
}
/* set the four pulses, in Q13 */
fixedCodebookVector[i0] = SSHL((word16_t)correlationSignalSign[i0], 13);
fixedCodebookVector[i1] = SSHL((word16_t)correlationSignalSign[i1], 13);
fixedCodebookVector[i2] = SSHL((word16_t)correlationSignalSign[i2], 13);
fixedCodebookVector[i3] = SSHL((word16_t)correlationSignalSign[i3], 13);
/* adapt it according to eq48 */
for (i=intPitchDelay; i<L_SUBFRAME; i++) {
fixedCodebookVector[i] = MAC16_16_Q14(fixedCodebookVector[i], fixedCodebookVector[i-intPitchDelay], lastQuantizedAdaptativeCodebookGain); /* h[n] = h[n] + β*h[n-T], fixedCodebookVector in Q13, lastQuantizedAdaptativeCodebookGain in Q14 */
}
/* compute the parameters */
*fixedCodebookParameter = (uint16_t)( MULT16_16_Q15((word16_t)i0, O2_IN_Q15)
+ ((MULT16_16_Q15((word16_t)i1, O2_IN_Q15))<<3)
+ ((MULT16_16_Q15((word16_t)i2, O2_IN_Q15))<<6)
+ ((((MULT16_16_Q15((word16_t)i3, O2_IN_Q15))<<1)+jx)<<9)
);
*fixedCodebookPulsesSigns = (uint16_t)((correlationSignalSign[i0]+1)>>1) | /* as in spec 3.8.2 eq61 */
(uint16_t)(((correlationSignalSign[i1]+1)>>1)<<1) |
(uint16_t)(((correlationSignalSign[i2]+1)>>1)<<2) |
(uint16_t)(((correlationSignalSign[i3]+1)>>1)<<3);
/* compute the fixedCodebook vector convolved with impulse response spec 3.9 eq64 */
/* this vector is used in gain quantization but computed here because it's faster doing it having directly the impulses positions */
/* eq64 make use of fixedCodebook vector adapted by eq48, using the impulse position(and thus fixed codebook vector before the adaptation) but */
/* the impulse response adapted as in eq49 gives the same output */
/* reset the vector */
for (i=0; i<i0; i++) fixedCodebookVectorConvolved[i] = 0;
if(correlationSignalSign[i0] > 0) {
for(i=i0, j=0; i<L_SUBFRAME; i++, j++) {
fixedCodebookVectorConvolved[i] = impulseResponse[j];
}
} else {
for(i=i0, j=0; i<L_SUBFRAME; i++, j++) {
fixedCodebookVectorConvolved[i] = -impulseResponse[j];
}
}
if(correlationSignalSign[i1] > 0) {
for(i=i1, j=0; i<L_SUBFRAME; i++, j++) {
fixedCodebookVectorConvolved[i] = ADD16(fixedCodebookVectorConvolved[i], impulseResponse[j]);
}
} else {
for(i=i1, j=0; i<L_SUBFRAME; i++, j++) {
fixedCodebookVectorConvolved[i] = SUB16(fixedCodebookVectorConvolved[i], impulseResponse[j]);
}
}
if(correlationSignalSign[i2] > 0) {
for(i=i2, j=0; i<L_SUBFRAME; i++, j++) {
fixedCodebookVectorConvolved[i] = ADD16(fixedCodebookVectorConvolved[i], impulseResponse[j]);
}
} else {
for(i=i2, j=0; i<L_SUBFRAME; i++, j++) {
fixedCodebookVectorConvolved[i] = SUB16(fixedCodebookVectorConvolved[i], impulseResponse[j]);
}
}
if(correlationSignalSign[i3] > 0) {
for(i=i3, j=0; i<L_SUBFRAME; i++, j++) {
fixedCodebookVectorConvolved[i] = ADD16(fixedCodebookVectorConvolved[i], impulseResponse[j]);
}
} else {
for(i=i3, j=0; i<L_SUBFRAME; i++, j++) {
fixedCodebookVectorConvolved[i] = SUB16(fixedCodebookVectorConvolved[i], impulseResponse[j]);
}
}
return;
}
/*****************************************************************************/
/* computeImpulseResponseCorrelationMatrix: as in spec 3.8.1 eq51, eq56, eq57*/
/* parameters: */
/* -(i) impulseResponse : 40 values in Q12 */
/* -(i/o) correlationSignal: 40 values in Q12 get absolute value of */
/* input as output as specified in spec 3.8.1 */
/* -(o) correlationSignalSign : 40 values of -1 or 1 : the sign of */
/* the input correlationSignal elements */
/* */
/* return value: a triangular matrix composed of Phi(i,j) in Q24 */
/* where Phi(i,j) = ∑h(n-i)*h(n-j) n in i..39 */
/* The matrix is then modified as decribed in eq56 and eq 57 */
/* */
/* Algorithm : the matrix elements are computed for j<=i */
/* due to matrix element definition we have */
/* Phi(i,j) = Phi(i+1,j+1) + h(39-i)*h(39-j) */
/* - The matrix is computed starting from element Phi(39,x)=h(0)*h(39-x) */
/* and adding terms h(39-i)*h(39-j) give all the element of the diagonal */
/* up to Phi(39-x,0) */
/* - Some diagonals are not computed because not needed : */
/* Phi(39,34), Phi(39,29), Phi(39,24), .. Phi(39,4) */
/* - The matrix elements signs are then adjusted according to eq56 */
/* - The correlationSignal is modified to get absolute values of each */
/* element in it. */
/* - Matrix elements are then duplicated to make access to them easier */
/* */
/*****************************************************************************/
void computeImpulseResponseCorrelationMatrix(word16_t impulseResponse[], word16_t correlationSignal[], int correlationSignalSign[], word32_t Phi[L_SUBFRAME][L_SUBFRAME])
{
int i,j,iComp;
word32_t acc = 0;
uint16_t PhiScaling = 0;
int correlationSignalSignInv[L_SUBFRAME];
/* first compute the diagonal Phi(x,x) : Phi(39,39) = h[0]^2 # Phi(38,38) = Phi(39,39)+h[1]^2 */
/* this diagonal must be divided by 2 according to spec 3.8.1 eq57 */
for (i=0, iComp=L_SUBFRAME-1; i<L_SUBFRAME; i++, iComp--) { /* i in [0..39], iComp in [39..0] */
acc = MAC16_16(acc, impulseResponse[i], impulseResponse[i]); /* impulseResponse in Q12 -> acc in Q24 */
Phi[iComp][iComp] = SHR(acc,1); /* divide by 2: eq57*/
}
/* check for possible overflow: Phi will be summed 10 times, so max Phi (by construction Phi[0][0]*2 is the max of Phi-> 2*Phi[0][0]*10 must be < 0x7fff ffff -> Phi[0][0]< 0x06666666 - otherwise scale Phi)*/
if (Phi[0][0]>0x6666666) {
PhiScaling = 3 - countLeadingZeros((Phi[0][0]<<1) + 0x3333333); /* complement 0xccccccc adding 0x3333333 to shift by one when max(2*Phi[0][0]) is in 0x0fffffff < max < 0xcccccc */
for (i=0; i<L_SUBFRAME; i++) {
Phi[i][i] = SHR(Phi[i][i],PhiScaling);
}
}
/* Compute all diagonals but the 34, 29, 24, 19, 14, 9 and 4*/
for (i=0; i<8; i++) {
for (j=0; j<4; j++) {
computePhiDiagonal(5*i+j, impulseResponse, Phi, PhiScaling);
}
}
/* correlationSignal -> absolute value and get sign (and his inverse in an array) */
for (i=0; i<L_SUBFRAME; i++) {
if (correlationSignal[i] >= 0) {
correlationSignalSign[i] = 1;
correlationSignalSignInv[i] = -1;
} else { /* correlationSignal < 0 */
correlationSignalSign[i] = -1;
correlationSignalSignInv[i] = 1;
correlationSignal[i] = -correlationSignal[i];
}
}
/* modify the signs according to eq56 */
for (i=0; i<L_SUBFRAME; i++) {
int *signOfCorrelationSignalJ;
if (correlationSignalSign[i]>0) { /* is sign(correlationSignal[i]) is positive, use the correlationSignalSign otherwise the inverted one */
signOfCorrelationSignalJ = correlationSignalSign;
} else {
signOfCorrelationSignalJ = correlationSignalSignInv;
}
for (j=0; j<=i; j++) { /* multiply by the selected sign the matrix element */
/* Note : even the not needed and thus not computed elements are multiplicated... might found other way to do this sign stuff to be more efficient */
Phi[i][j] = Phi[i][j] * signOfCorrelationSignalJ[j];
}
}
/* duplicate the usefull values to their symetric part to get easier acces to the matrix elements */
for (i=0; i<8; i++) {
for (j=0; j<4; j++) {
int k;
int startIndex = 5*i+j;
for(k=0; k<=startIndex; k++) {
Phi[startIndex-k][L_SUBFRAME-1-k] = Phi[L_SUBFRAME-1-k][startIndex-k];
}
}
}
return;
}
/* compute a diagonal of Phi values: start from Phi(39,j) and step Phi(38, j-1) down to Phi(39-j, 0) */
/* Phi(i,j) = Phi(i+1,j+1) + h(39-i)*h(39-j) */
void computePhiDiagonal(int j, word16_t impulseResponse[], word32_t Phi[L_SUBFRAME][L_SUBFRAME], uint16_t PhiScaling)
{
word32_t acc = 0;
int i=L_SUBFRAME -1;
int iComp = 0;
int jComp = L_SUBFRAME - 1 - j;
if (PhiScaling == 0) {
for (; j>=0; j--, i--, jComp++, iComp++) {
acc = MAC16_16(acc, impulseResponse[iComp], impulseResponse[jComp]);
Phi[i][j] = acc;
}
} else {
for (; j>=0; j--, i--, jComp++, iComp++) {
acc = MAC16_16(acc, impulseResponse[iComp], impulseResponse[jComp]);
Phi[i][j] = SHR(acc,PhiScaling);
}
}
}