gaecdtds.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 _DTSSC              (0)

#define _ERL_CRIT_THR       GAEC_DB(3.0)
#define _DTD_CRIT_THR       GAEC_DB(6.0)

#define _DTD_NSE_THR        GAEC_DB(8.0)

/*--------------------- public vars ---------------------------------------*/
/*--------------------- local vars ----------------------------------------*/
/*--------------------- local functions -----------------------------------*/
/*-------------------------------------------------------------------------*/

/*-------------------------------------------------------------------------*/
void                        _adj_nse
/*-------------------------------------------------------------------------*/
(
S16 *psExpectedErrEn,
S16 sNse
)
{
    S16 sDlt = *psExpectedErrEn - sNse;

    if (sDlt < _DTD_NSE_THR)
    {
        *psExpectedErrEn -= sDlt>>1;
        *psExpectedErrEn += _DTD_NSE_THR>>1; 
    }
    if (sDlt < (-_DTD_NSE_THR))
    {
        *psExpectedErrEn -= sDlt>>1;
        *psExpectedErrEn -= _DTD_NSE_THR>>1; 
    }
}

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_dtd_exp_err
#else
void                        gaec_dtd_exp_errC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb, 
GAEC_tSc *pSc
)
{
    S16 sMax;
    S16 sNew = pSc->sRoutEn;
    S16 sErle;
    int k;


    sMax = sNew;
    for (k = 0; k < GAEC_ADF_FRS; k++)
    {
        if (sMax < pDb->asRoutEnHst[k])
            sMax = pDb->asRoutEnHst[k];
    }

    sErle = pDb->sErleAv;
    if (sErle > pSc->sErleMax)
        sErle = pSc->sErleMax;

    sMax -= (pDb->sErlAv + sErle);

    if (pDb->sExpectedErrEn < sMax)
    {
        pDb->sExpectedErrEn = sMax;
    }
    else
    {
        pDb->sExpectedErrEn -= GAEC_DB(0.6);
    }

    for (k = 0; k < GAEC_ADF_FRS-1; k++)
    {
        pDb->asRoutEnHst[k] = pDb->asRoutEnHst[k+1];
    }
    pDb->asRoutEnHst[GAEC_ADF_FRS-1] = sNew;
}
/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_dtd_exp_err_sb
#else
void                        gaec_dtd_exp_err_sbC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb, 
GAEC_tSc *pSc 
)
{
    int band;

    for (band = 0; band < GAEC_BANDS+1; band++)
    {
        S16 sNew = pSc->asRoutEn[band];
        S16 sMax = sNew;
        S16 sErle;
        int k;

        for (k = 0; k < GAEC_ADF_FRS; k++)
        {
            if (sMax < pDb->aasRoutEnHst[band][k])
                sMax = pDb->aasRoutEnHst[band][k];
        }

        sErle = pDb->asErleAv[band];
        if (sErle > pSc->sErleMax)
            sErle = pSc->sErleMax;

        sMax -= (pDb->asErlAv[band] + sErle);

        if (pDb->asExpectedErrEn[band] < sMax)
        {
            pDb->asExpectedErrEn[band] = sMax;
        }
        else
        {
            pDb->asExpectedErrEn[band] -= GAEC_DB(0.6);
        }

        for (k = 0; k < GAEC_ADF_FRS-1; k++)
        {
            pDb->aasRoutEnHst[band][k] = pDb->aasRoutEnHst[band][k+1];
        }
        pDb->aasRoutEnHst[band][GAEC_ADF_FRS-1] = sNew;
    }
}

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_dtd_vad_err
#else
void                        gaec_dtd_vad_errC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc
)
{
    S16 sDlt;
    S16 sErrEn;

    sErrEn = pSc->sErrEn;
    // ignore total silence - mute
    if ((sErrEn > GAEC_VAD_NSE_MIN) &&
        (pSc->sSinEn > GAEC_VAD_NSE_MIN))
    {
        sDlt = sErrEn - pDb->sVadErrNse;

        pDb->sVadErrCrit += (sDlt - GAEC_VAD_CRIT_THR);

        if (pDb->sVadErrCrit > GAEC_CRIT_MAX)
            pDb->sVadErrCrit = GAEC_CRIT_MAX;
        if (pDb->sVadErrCrit < GAEC_CRIT_MIN)
            pDb->sVadErrCrit = GAEC_CRIT_MIN;
    
        if (pDb->sVadErrCrit < GAEC_VAD_CRIT_THR)
        {
            if (pDb->uAdfMode < 3)
                pDb->sVadErrNse += sDlt >> 3;
            else
                pDb->sVadErrNse += sDlt >> 5;

            if (pDb->sVadErrNse > GAEC_VAD_NSE_MAX)
                pDb->sVadErrNse = GAEC_VAD_NSE_MAX;
            if (pDb->sVadErrNse < GAEC_VAD_NSE_MIN)
                pDb->sVadErrNse = GAEC_VAD_NSE_MIN;
        }
        else
        {
            if (sErrEn < GAEC_VAD_NSE_MAX)
            {
                if (pDb->uAdfMode < 3)
                    pDb->sVadErrNse += GAEC_DB(1./17.);
                else
                    pDb->sVadErrNse += 1;

            }
        }
    }
    else
    {
        pDb->sVadErrCrit -= GAEC_VAD_CRIT_THR;
        if (pDb->sVadErrCrit < GAEC_CRIT_MIN)
            pDb->sVadErrCrit = GAEC_CRIT_MIN;

    }
}


/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_dtd_vad_err_sb
#else
void                        gaec_dtd_vad_err_sbC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc
)
{
    S16 sDlt;
    S16 sErrEn;
    S16 sMaxEn;
    int band;

    sMaxEn = -32768;
    for (band = 0; band < (GAEC_BANDS+1); band++)
    {
        if (sMaxEn < pSc->asErrEn[band])
            sMaxEn = pSc->asErrEn[band];
    }

    for (band = 0; band < (GAEC_BANDS+1); band++)
    {
        sErrEn = pSc->asErrEn[band];
        // ignore total silence - mute
        if ((sErrEn > GAEC_VAD_NSE_MIN) &&
            (pSc->asSinEn[band] > GAEC_VAD_NSE_MIN))
        {
            sDlt = sErrEn - pDb->asVadErrNse[band];

            pDb->asVadErrCrit[band] += (sDlt - GAEC_VAD_CRIT_THR);

            if (pDb->asVadErrCrit[band] > GAEC_CRIT_MAX)
                pDb->asVadErrCrit[band] = GAEC_CRIT_MAX;
            if (pDb->asVadErrCrit[band] < GAEC_CRIT_MIN)
                pDb->asVadErrCrit[band] = GAEC_CRIT_MIN;
        
            if (pDb->asVadErrCrit[band] < 0)
//            if (pDb->asVadErrCrit[band] < GAEC_VAD_CRIT_THR)
            {
                if (pDb->uAdfMode < 3)
                    pDb->asVadErrNse[band] += sDlt >> 3;
                else
                    pDb->asVadErrNse[band] += sDlt >> 5;

                if (pDb->asVadErrNse[band] > GAEC_VAD_NSE_MAX)
                    pDb->asVadErrNse[band] = GAEC_VAD_NSE_MAX;
                if (pDb->asVadErrNse[band] < GAEC_VAD_NSE_MIN)
                    pDb->asVadErrNse[band] = GAEC_VAD_NSE_MIN;
            }
            else
            {
                if ((sErrEn < GAEC_VAD_NSE_MAX) &&
                    (sMaxEn < GAEC_VAD_NSE_MAX))
                {
                    if (pDb->uAdfMode < 3)
                        pDb->asVadErrNse[band] += GAEC_DB(1./17.);
                    else
                        pDb->asVadErrNse[band] += 1;

                }
            }
        } 
        else // nse too low
        {
            pDb->asVadErrCrit[band] -= GAEC_VAD_CRIT_THR;
            if (pDb->asVadErrCrit[band] < GAEC_CRIT_MIN)
                pDb->asVadErrCrit[band] = GAEC_CRIT_MIN;

        }
    } // bands
}


/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_dtd_criteria
#else
void                        gaec_dtd_criteriaC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb, 
GAEC_tSc *pSc 
)
{
    S16 sErleLoss;
    S16 sExpectedErrEn;

    sExpectedErrEn = pDb->sExpectedErrEn;

// account for noise floor
    _adj_nse(&sExpectedErrEn, pDb->sVadErrNse);

// get erle loss, correct it + update dt criteria
    sErleLoss = pSc->sErrEn - sExpectedErrEn;
    sErleLoss -= 1*pDb->Cfg.sDtThr;

    if (sErleLoss < (GAEC_CRIT_MIN>>3))
        sErleLoss = (GAEC_CRIT_MIN>>3);

    pDb->sErlCrit += (sErleLoss - 1*pDb->Cfg.sDtThr);

    if (pDb->sErlCrit > GAEC_CRIT_MAX)
        pDb->sErlCrit = GAEC_CRIT_MAX;

    if (pDb->sErlCrit < GAEC_CRIT_MIN)
        pDb->sErlCrit = GAEC_CRIT_MIN;

    if (sErleLoss > 2*_DTD_CRIT_THR)
        sErleLoss = 2*_DTD_CRIT_THR;

    pDb->sDtCrit += sErleLoss;

    if (pDb->sDtCrit > GAEC_CRIT_MAX)
        pDb->sDtCrit = GAEC_CRIT_MAX;

    if (pDb->sDtCrit < GAEC_CRIT_MIN)
        pDb->sDtCrit = GAEC_CRIT_MIN;
        
}

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_dtd_criteria_sb
#else
void                        gaec_dtd_criteria_sbC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb, 
GAEC_tSc *pSc 
)
{
    S16 band;
    S16 sMax = pDb->asExpectedErrEn[0];


    for (band = 1; band < GAEC_BANDS+1; band++)
    {
        if (sMax < pDb->asExpectedErrEn[band])
            sMax = pDb->asExpectedErrEn[band];
    }

//	pSc->asTmp[0] = sMax;

    for (band = 0; band < (GAEC_BANDS+1); band++)
    {
        S16 sErleLoss;
        S16 sExpectedErrEn = pDb->asExpectedErrEn[band]; 

        // account for cross-band-leaks, 
        // which increase with the signal
        sExpectedErrEn += (sMax - sExpectedErrEn) >> 2;
        if (sExpectedErrEn < (sMax - pSc->sErleMax))
        {
            sExpectedErrEn = sMax - pSc->sErleMax;
        }
        // add more for bands 0,1
        if (band < 2)
            sExpectedErrEn += GAEC_DB(2.0);
        if (band < 1)
            sExpectedErrEn += GAEC_DB(2.0);
//        pSc->asTmp[3*band+1] = sExpectedErrEn;

        // account for noise floor
        _adj_nse(&sExpectedErrEn, pDb->asVadErrNse[band]);
        
//        pSc->asTmp[3*band+2] = sExpectedErrEn;
        
        sErleLoss = pSc->asErrEn[band] - sExpectedErrEn;
        sErleLoss -= pDb->Cfg.sDtThr;

        if (sErleLoss < (GAEC_CRIT_MIN>>2)) // >>2 ???
            sErleLoss = (GAEC_CRIT_MIN>>2);

//        pSc->asTmp[3*band+3] = sErleLoss;

        pDb->asErlCrit[band] += (sErleLoss - 1*pDb->Cfg.sDtThr);

        if (pDb->asErlCrit[band] > GAEC_CRIT_MAX)
            pDb->asErlCrit[band] = GAEC_CRIT_MAX;

        if (pDb->asErlCrit[band] < GAEC_CRIT_MIN)
            pDb->asErlCrit[band] = GAEC_CRIT_MIN;

        if (sErleLoss > 2*_DTD_CRIT_THR)
            sErleLoss = 2*_DTD_CRIT_THR;

        pDb->asDtCrit[band] += sErleLoss;

        if (pDb->asDtCrit[band] > GAEC_CRIT_MAX)
            pDb->asDtCrit[band] = GAEC_CRIT_MAX;

        if (pDb->asDtCrit[band] < GAEC_CRIT_MIN)
            pDb->asDtCrit[band] = GAEC_CRIT_MIN;

    } // end-of-for
}
/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_dtd_dec_criteria
#else
void                        gaec_dtd_dec_criteriaC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb, 
GAEC_tSc *pSc 
)
{
    S16     band;

    for (band = 0; band < (GAEC_BANDS+1); band++)
    {
        pDb->asDtCrit[band] -= (_DTD_CRIT_THR/3);
        if (pDb->asDtCrit[band] < GAEC_CRIT_MIN)
            pDb->asDtCrit[band] = GAEC_CRIT_MIN;
    }

    pDb->sDtCrit -= (_DTD_CRIT_THR/3);
    if (pDb->sDtCrit < GAEC_CRIT_MIN)
        pDb->sDtCrit = GAEC_CRIT_MIN;


}
/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_dtd_avrg_erle_sb
#else
void                        gaec_dtd_avrg_erle_sbC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb, 
GAEC_tSc *pSc 
)
{
// asTmp use: GAEC_BANDS+1
    S16 band;
    S16 sCoef = Q15(1./32.);

    S16 *psTmp = pSc->u.asTmp;

    if (pDb->uAdfMode > 2)
        sCoef >>= 1;
    if (pDb->uAdfMode > 3)
        sCoef >>= 1;

    for (band = 0; band < GAEC_BANDS+1; band++)
    {
        // if expected higher than noise & no DT
        if (( pSc->asSinEn[band] - pDb->asErleAv[band] > pDb->asVadErrNse[band]) &&
            ((pSc->asRoutEn[band] - pDb->asErleAv[band] - pDb->asErlAv[band]) > pDb->asVadErrNse[band]) &&
            (pDb->asDtCrit[band] < 0))
        {
            // slower for low bands
            S32 ac0 = (4096 - (GAEC_BANDS-band) * 300) * (S32) sCoef;
            *psTmp++ = _Srnd(ac0, 3);
        }
        else
        {
            *psTmp++ = 0;
        }
    }
    psTmp -= (GAEC_BANDS+1);
    
    for (band = 0; band < GAEC_BANDS+1; band++)
    {
        S32 ac0;
        S16 sErleAv = pDb->asErleAv[band];
        S16 coef = psTmp[band];
        S16 sErleLoss = sErleAv - pSc->asErle[band];

        if (sErleLoss > GAEC_DB(6.0))
            sErleLoss = GAEC_DB(6.0);

        ac0 = sErleLoss * (S32) coef;
        ac0 += (1<<14);
        ac0 >>= 15;
        sErleAv -= (S16)ac0;
        if (sErleAv < 0)
            sErleAv = 0;
        pDb->asErleAv[band] = sErleAv;
	}
    for (band = 0; band < GAEC_BANDS+1; band++)
    {
        S16 coef = psTmp[band];
        gaec_utl_avrg(&pDb->asErlAv [band], pSc->asErl [band], coef);
        if (pDb->asErlAv[band] < pDb->Cfg.sErlMin)
            pDb->asErlAv[band] = pDb->Cfg.sErlMin;
    }

}