gaecrcvs.c

TI公司DSP/tms320c55x/tms320c54x系列的声学回声消除代码

/*-------------------------------------------------------------------------*
 *                                                                         *
 *   THIS IS AN UNPUBLISHED WORK CONTAINING CONFIDENTIAL AND PROPRIETARY   *
 *   INFORMATION.  IF PUBLICATION OCCURS, THE FOLLOWING NOTICE APPLIES:    *
 *      "COPYRIGHT 2002 MIKET DSP SOLUTIONS, ALL RIGHTS RESERVED"          *
 *                                                                         *
 *-------------------------------------------------------------------------*/

#include "gaeci.h"

/*--------------------- local defs ----------------------------------------*/
#define _SJ0_MIN (2048/16)
/*--------------------- public vars ---------------------------------------*/
/*--------------------- local vars ----------------------------------------*/
/*--------------------- local functions -----------------------------------*/

static S16 _abs16(S16 x)
{
    if (x > 0) return (S16)(x); else return (S16)(-x);
}

/*-------------------------------------------------------------------------*/

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
S16                            gaec_rcv_durbin
#else
S16                            gaec_rcv_durbinC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tScRcv *pScr
)
{
    S16 k;
    S16 i;
    S16 sK;
    S32 ac0;
    S16 sJ0;

    for (k = 0; k < 6; k++)
    {
        pScr->asF[k] = 0;
        pScr->asK[k] = 0;
        pScr->aslG[k] = 0;
    }

    /* durbin iterations */

    /* iteration 0 */
    pScr->asF[0] = 32767;
    pScr->asK[0] = 32767;
    sJ0 = 32767;

    /* iteration 1 */
    {
        S16 sK = pScr->asRk[1];
        ac0 = (32767L<<15) - (sK * (S32) sK);
        sJ0 = _Srnd(ac0, 0);
        pScr->asF[1] = -sK;
        pScr->asK[1] = sK;

        if (sJ0 < _SJ0_MIN) return sJ0;
    }

    for (i = 2; i < 6; i++)
    {
        ac0  = ((S32)pScr->asRk[i])<<15;
        for (k = i-1; k > 0; k--)
        {
        	ac0 += pScr->asRk[k] * (S32)pScr->asF[i-k];
        }
        ac0 += 0x4000;
        ac0 >>= 15;
        if (_abs16((S16)ac0) >= sJ0) return 0;

        sK = (ac0 * 0x8000L)/sJ0;
        pScr->asK[i] = sK;
        
        ac0 = ((32767L<<15) - sK * (S32) sK + 0x4000)>>15;
        sJ0 = (ac0 * sJ0 + 0x4000)>>15;

        for(k = 1; k < i; k++) 
            pScr->aslG[k] = pScr->asF[k] * (S32)sK;
        pScr->asF[i]  = -sK;
        for(k = 1; k < i; k++) 
        {
            ac0 = ((S32)pScr->asF[i-k])<<15;
            ac0 -= pScr->aslG[k];
            pScr->asF[i-k] = _Srnd(ac0, 0);
        }

		if (sJ0 < _SJ0_MIN) return sJ0;
    }
    return sJ0;
}


/*-------------------------------------------------------------------------*/
#if ! defined(_dsp)
void                    gaec_rcv_normalise_rk
#else
void                    gaec_rcv_normalise_rkC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tScRcv *pScr,
S16 sShift,
S16 sAdd
)
{
    S32 ac0;
    S16 exp;
    S16 sInvR0;
    S16 k;

    /* regularization */    
    ac0 = pScr->aslRk[0];
    ac0 += sAdd;
    ac0 += ac0>>(-sShift);

    /* normalization */

    for (exp = 0; exp < 32;)
    {
        if (ac0 & 0x40000000L) break;
        ac0 <<= 1;
        exp++;
    }
	pScr->asRk[0] = (S16)(ac0>>16);
    sInvR0 = (S16)((0x3fff*0x8000L)/pScr->asRk[0]);
    pScr->asRk[0] = 32767;
    
    for (k = 1; k< 6; k++) 
    {
        ac0 = (pScr->aslRk[k]<<exp)>>16;
	    ac0 = ac0 * sInvR0;
       	pScr->asRk[k] = (S16)(ac0>>14);
    }
}


/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                    gaec_rcv_moments
#else
void                    gaec_rcv_momentsC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tScRcv *pScr,
S16 *psRcv
)
{
	S16 k;

// shift old data
	for (k = 0; k < 5; k++) 
    {
        pScr->asTdBuf[k] = pDb->asTdSav[k];
    }
// add new data
	for (k = 0; k < GAEC_FR_SZ; k++) 
    {
        pScr->asTdBuf[k+5] = psRcv[k] >> 2;
    }
// save new data
	for (k = 0; k < 5; k++) 
    {
        pDb->asTdSav[k] = pScr->asTdBuf[k+GAEC_FR_SZ];
    }
    /* compute moments for the current frame */
    for (k = 0; k < 6; k++)
    {
        S32 ac0 = 0;
        S16 i;

	    for (i = 0; i < GAEC_FR_SZ; i++)
	    {
		    ac0 += pScr->asTdBuf[i+5] * (S32) pScr->asTdBuf[i+5-k];
	    }
        pScr->aslRkTx[k] = ac0;
    }
#if 0
#endif    
}