dtmfs.cpp

来自「Express DSP compliant C55x DTMF detector」· C++ 代码 · 共 890 行 · 第 1/2 页

         */

        ac0 >>= 16; ac0  = ac0 * ac0;
        ac1 >>= 16; ac0 += ac1 * ac1;
        *psOut++ = dtmf_en2logS(ac0);;
    }
}
/*-------------------------------------------------------------------------*/
static void 					freq_lo
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S16 *psDev = pSc->asLoDevEn;
    const S16 *psDevFlt = &__aasLoDevFlt[pSc->sLoFreqNo][0];
    S32 ac0;

    ac0  = psDev[3]-psDev[0];
    ac0 += psDev[2]-psDev[1];
    ac0 *= psDevFlt[0];
    ac0 >>= 16;
	/* limit to be meaningfull, 5% is the max that could be expected */
	if (ac0 > 170*5) ac0 = 170*5;
	if (ac0 < -170*5) ac0 = -170*5;
	
    pSc->sLoFreqDev = (S16)(ac0);

    ac0 = (ac0)*(ac0);
    ac0 = ac0 * psDevFlt[1]; 
    ac0 >>= 16;
    pSc->sLoMaxFreqEn = pSc->asLoMainEn[pSc->sLoFreqNo] + (S16)(ac0);
}
/*-------------------------------------------------------------------------*/
static void 					dft_hi_main
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S32 ac0;
    S32 ac1;
    const S16 *psCDP = dtmf_aHi;
    S16 *psI = pSc->asHiData;
    S16 *psQ = psI + 1;
    S16 *psOut = pSc->asHiMainEn;
    S16 sFilterNo;
    S16 k;

    for (sFilterNo = 0; sFilterNo < 4; sFilterNo++)
    {
        ac0 = 0;
        ac1 = 0;
        /* psCDP points to Cos coeffs */
        for (k = 0; k < DTMF_HI_DFT_SZ; k++)
        {
            ac0 += *psCDP * (S32) *psI;
            ac1 += *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* psCDP points to Sin coeffs */
        /* return data pointers back */
        psI -= DTMF_HI_DFT_SZ*2;
        psQ -= DTMF_HI_DFT_SZ*2;
        for (k = 0; k < DTMF_HI_DFT_SZ; k++)
        {
            ac1 += *psCDP * (S32) *psI;
            ac0 -= *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* return data pointers back */
        psI -= DTMF_HI_DFT_SZ*2;
        psQ -= DTMF_HI_DFT_SZ*2;
        /* psCDP points to Cos coeffs of the next filter */
        /* ac0 = I * c - Q * s;
           ac1 = I * s + Q * c;
         */

        ac0 >>= 16; ac0  = ac0 * ac0;
        ac1 >>= 16; ac0 += ac1 * ac1;
        *psOut++ = dtmf_en2logS(ac0);;
    }
}
/*-------------------------------------------------------------------------*/
static void 					sort_hi
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S16 *psIn = pSc->asHiMainEn;
    S16 sFreqNo = 0;
    S16 sMaxFreqEn = psIn[0];
    S16 k;

    for (k = 1; k < 4; k++)
    {
        if (psIn[k] > sMaxFreqEn)
        {
            sMaxFreqEn = psIn[k];
            sFreqNo = k;
        }
    }

    sMaxFreqEn = -32767;
    for (k = 0; k < 4; k++)
    {
        if (k != sFreqNo)
        {
            if (psIn[k] > sMaxFreqEn)
            {
                sMaxFreqEn = psIn[k];
            }
        }
    }
    pSc->sHiNextEn = sMaxFreqEn;
    pSc->sHiFreqNo = sFreqNo;
}
/*-------------------------------------------------------------------------*/
static void 					dft_hi_dev
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S32 ac0;
    S32 ac1;
    const S16 *psCDP = __apsHiDev[pSc->sHiFreqNo];
    S16 *psI = pSc->asHiData;
    S16 *psQ = psI + 1;
    S16 *psOut = pSc->asHiDevEn;
    S16 sFilterNo;
    S16 k;

    for (sFilterNo = 0; sFilterNo < 4; sFilterNo++)
    {
        ac0 = 0;
        ac1 = 0;
        /* psCDP points to Cos coeffs */
        for (k = 0; k < DTMF_HI_DFT_SZ; k++)
        {
            ac0 += *psCDP * (S32) *psI;
            ac1 += *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* psCDP points to Sin coeffs */
        /* return data pointers back */
        psI -= DTMF_HI_DFT_SZ*2;
        psQ -= DTMF_HI_DFT_SZ*2;
        for (k = 0; k < DTMF_HI_DFT_SZ; k++)
        {
            ac1 += *psCDP * (S32) *psI;
            ac0 -= *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* return data pointers back */
        psI -= DTMF_HI_DFT_SZ*2;
        psQ -= DTMF_HI_DFT_SZ*2;
        /* psCDP points to Cos coeffs of the next filter */
        /* ac0 = I * c - Q * s;
           ac1 = I * s + Q * c;
         */

        ac0 >>= 16; ac0  = ac0 * ac0;
        ac1 >>= 16; ac0 += ac1 * ac1;
        *psOut++ = dtmf_en2logS(ac0);;
    }
}
/*-------------------------------------------------------------------------*/
static void 					freq_hi
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S32 ac0;
    S16 *psDev = pSc->asHiDevEn;
    const S16 *psDevFlt = &__aasHiDevFlt[pSc->sHiFreqNo][0];

    ac0  = psDev[3]-psDev[0];
    ac0 += psDev[2]-psDev[1];
    ac0 *= psDevFlt[0]; 
    ac0 >>= 16;

	/* limit to be meaningfull, 5% is the max that could be expected */
	if (ac0 > 170*5) ac0 = 170*5;
	if (ac0 < -170*5) ac0 = -170*5;
	

    pSc->sHiFreqDev = (S16)(ac0);

    ac0 = (ac0)*(ac0);
    ac0 = ac0 * psDevFlt[1]; 
    ac0 >>= 16;
    pSc->sHiMaxFreqEn = pSc->asHiMainEn[pSc->sHiFreqNo] + (S16)(ac0);
}
/*-------------------------------------------------------------------------*/
static void 					frame_en
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
     S32 ac0 = 0;
     S16 *ps0 = &pSc->as2kData[0];
     S16 Sample; 

     for (Sample = 0; Sample < STD_FR_SZ; Sample++)
     {
          ac0 += (*ps0 * (long)*ps0);
          ps0 += 1;
     }
     pSc->slFrameEn = (ac0);
}
/*-------------------------------------------------------------------------*/
static void 					test
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    DTMF_tCfg *pCfg   = pDb->pCfg;
    S16 sTestFailed = 0;
    
    /* test that frame energy is high enough. */
    if (pSc->sSumEn < pCfg->sMinEnThr)
        sTestFailed |= DTMF_MASK_MIN_EN;

#if ! defined(_dsp)
        DBG_sFrameEn = pDb->v.sZFrameEn;
#endif
    /*  test that {dial + lo + hi} energies constitute 
        a major part of average of 2 frames energy. */
        if ((pDb->v.sZFrameEn - pSc->sSumDialEn) > pCfg->sSumEnThr)
            sTestFailed |= DTMF_MASK_SUM_EN;

    /* test leading edge */
    {
        S16 ac0 = pDb->v.asSumEn[0];
        S16 ac1 = pDb->v.asSumEn[1];
        S16 ac2 = pDb->v.asSumEn[4];
        S16 ac3 = pDb->v.asSumEn[5];
        
        if (ac0 > ac1) ac0 = ac1; /* ac0 =  min of 2 most recent frames */
        if (ac2 < ac3) ac2 = ac3; /* ac2 =  max of 2 oldest frames */
        ac0 -= ac2;               /* energy diff */  
        if (ac0 < pCfg->sNoiseThr)
            sTestFailed |= DTMF_MASK_RS_EDGE;
    }

    /* test falling edge */
    if ((pDb->v.Stat.sEn - pSc->sSumEn) < pCfg->sNoiseThr)
        sTestFailed |= DTMF_MASK_FL_EDGE;

    /* test that lo(idx) and hi(idx) energies are reasonably stable */
    if (abs(pSc->sLoMaxFreqEn - pDb->v.Stat.sLoEn) > pCfg->sVarThr)
        sTestFailed |= DTMF_MASK_STABLE;
    if (abs(pSc->sHiMaxFreqEn - pDb->v.Stat.sHiEn) > pCfg->sVarThr)
        sTestFailed |= DTMF_MASK_STABLE;

    /* test that <2k energy is significantly bigger than >2k energy */
    if (pSc->s2kEnDlt < pCfg->s2kUpThr)
        sTestFailed |= DTMF_MASK_2K_BOUND;
    
    /* test that twists are ok */
/*    if ((pSc->Hi.sMaxFreqEn - pSc->Lo.sMaxFreqEn) > pCfg->sFwdTwistThr) */
    if ((pSc->sHiBpEn-pSc->sLoBpEn) > (pCfg->sFwdTwistThr+(170+85)))
        sTestFailed |= DTMF_MASK_FWD_TWIST;
    if ((pSc->sHiBpEn-pSc->sLoBpEn) < -(pCfg->sRevTwistThr-85) )
        sTestFailed |= DTMF_MASK_REV_TWIST;

    if (pDb->v.sState < DTMF_ST_TONE)
    {
        /*  DFT delay lines are not full. at least one frame less.
            we shall account for wider sinc mainlobe and lower energy.
            Noise does not have much influence on the parameters
            in this case, so the threshold are hard coded.
            tuning criteria was chosen the number of early-on hits 
            on talk-off imunity test.
            varying those thrs shall affect early detection delay
            and talk-off immunity as well.
        */
        if ((pSc->sLoBpEn - pSc->sLoMaxFreqEn) > (6*DTMF_1DB))
            sTestFailed |= DTMF_MASK_LO_PART;

        if ((pSc->sHiBpEn - pSc->sHiMaxFreqEn) > (4*DTMF_1DB))
            sTestFailed |= DTMF_MASK_HI_PART; 

        if ((pSc->sLoMaxFreqEn - pSc->sLoNextEn) < (5*DTMF_1DB))
            sTestFailed |= DTMF_MASK_LO_CLEAN;

        if ((pSc->sHiMaxFreqEn - pSc->sHiNextEn) < (7*DTMF_1DB))
            sTestFailed |= DTMF_MASK_HI_CLEAN;
    }
    else
    {
        if ((pSc->sLoBpEn - pSc->sLoMaxFreqEn) > pCfg->sPartThr)
            sTestFailed |= DTMF_MASK_LO_PART;

        if ((pSc->sHiBpEn - pSc->sHiMaxFreqEn) > pCfg->sPartThr)
            sTestFailed |= DTMF_MASK_HI_PART;

        if ((pSc->sLoMaxFreqEn - pSc->sLoNextEn) < pCfg->sCleanThr)
            sTestFailed |= DTMF_MASK_LO_CLEAN;

        if ((pSc->sHiMaxFreqEn - pSc->sHiNextEn) < pCfg->sCleanThr)
            sTestFailed |= DTMF_MASK_HI_CLEAN;
    }

    if (abs(pSc->sLoFreqDev) > pCfg->sMaxFreqDevThr) 
        sTestFailed |= DTMF_MASK_LO_INRANGE;

    if (abs(pSc->sHiFreqDev) > pCfg->sMaxFreqDevThr) 
        sTestFailed |= DTMF_MASK_HI_INRANGE;


    pSc->sDigit = pSc->sLoFreqNo | (pSc->sHiFreqNo<<2);

    if (pDb->v.sLastDigit != pSc->sDigit)
        sTestFailed |=  DTMF_MASK_SAME_FRQ;
    
	pSc->sTestFailed = sTestFailed;
}	
/*-------------------------------------------------------------------------*/
static void                        dtmf_move_scratch_to_dbC
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb, 
DTMF_tSc *pSc
)
{
    S16 k;

    {
        S32 slTmp = pSc->slFrameEn + pDb->v.slFrameEn;
        S16 sTmp =  dtmf_en2logS(slTmp>>3);
        pDb->v.sZFrameEn = sTmp;
    }

    /* required for next frame */
    pDb->v.slFrameEn = pSc->slFrameEn;

    for (k = 0; k < DTMF_2KFLT_SZ; k++)
    {
        pDb->as2kDataSav[k] = pSc->as2kData[k+STD_FR_SZ];
    }
    for (k = 0; k < DTMF_BPFLT_SZ; k++)
    {
        pDb->asBpDataSav[k] = pSc->asBpData[k+DTMF_BPFR_SZ];
    }
    for (k = 0; k < DTMF_LOSD_SZ; k++)
    {
        pDb->asLoDataSav[k] = pSc->asLoData[k+DTMF_DFR_SZ*2];
    }
    for (k = 0; k < DTMF_HISD_SZ; k++)
    {
        pDb->asHiDataSav[k] = pSc->asHiData[k+DTMF_DFR_SZ*2];
    }
}
/*-------------------------------------------------------------------------*/
void                    dtmf_filter_test
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc,
S16 *psIn
)
{

	move_data(pDb, pSc, psIn);
	filter_2k(pDb, pSc);
	filter_lo(pDb, pSc);
	filter_hi(pDb, pSc);

    pSc->sSumEn   = dtmf_en2logS(pSc->slLoBpEn + pSc->slHiBpEn);
    pDb->v.asSumEn[0] = pSc->sSumEn;

	filter_dial(pDb, pSc);
	

#if ! defined (_dsp)
    DBG_slDialEn = pDb->v.slDialEn + pSc->slDialEn;
#endif
    pSc->sSumDialEn = dtmf_en2logS(pSc->slLoBpEn + 
                                  pSc->slHiBpEn + 
                                  pDb->v.slDialEn +
                                  pSc->slDialEn);
    pDb->v.slDialEn = pSc->slDialEn;
    
	dft_lo_main(pDb, pSc);
	sort_lo(pDb, pSc);
	dft_lo_dev(pDb, pSc);
	freq_lo(pDb, pSc);

	dft_hi_main(pDb, pSc);
	sort_hi(pDb, pSc);
	dft_hi_dev(pDb, pSc);
	freq_hi(pDb, pSc);

	frame_en(pDb, pSc);
	test(pDb, pSc);
    dtmf_move_scratch_to_dbC (pDb, pSc);
}


/*-------------------------------------------------------------------------*/
S16                     dtmf_en2log
/*-------------------------------------------------------------------------*/
(
S32 ac0    /* but it is positive */
)
{
    S16 t0 = 0;

    ac0 += 1; // min val -13952
    // mant aco :: nexp ac0, t0
    for (;;)
    {
        if (ac0 &0x40000000L)
            break;
        ac0 <<= 1;
        t0--;
    }
    ac0 >>= 21;
    ac0 += 0x780;
    ac0 += t0*512;
    return (S16)ac0; 
}   
/*-------------------------------------------------------------------------*/