dtmfs.c

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

        /* return data pointers back */
        psI -= DTMF_LO_DFT_SZ*2;
        psQ -= DTMF_LO_DFT_SZ*2;
        for (k = 0; k < DTMF_LO_DFT_SZ; k++)
        {
            ac1 += *psCDP * (S32) *psI;
            ac0 -= *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* return data pointers back */
        psI -= DTMF_LO_DFT_SZ*2;
        psQ -= DTMF_LO_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_en2logC(ac0);;
    }
}
/*-------------------------------------------------------------------------*/
void 							dtmf_lo_sortC
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S16 *psIn = pSc->asLoMainEn;
    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->sLoNextEn = sMaxFreqEn;
    pSc->sLoFreqNo = sFreqNo;
}
/*-------------------------------------------------------------------------*/
void 							dtmf_lo_dft_devC
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    const S16 *psCDP = __apsLoDev[pSc->sLoFreqNo];
    S16 *psI = pSc->asLoData;
    S16 *psQ = psI + 1;
    S16 *psOut = pSc->asLoDevEn;
    S16 sFilterNo;
    S16 k;
    S32 ac0;
    S32 ac1;

    for (sFilterNo = 0; sFilterNo < 4; sFilterNo++)
    {
        ac0 = 0;
        ac1 = 0;
        /* psCDP points to Cos coeffs */
        for (k = 0; k < DTMF_LO_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_LO_DFT_SZ*2;
        psQ -= DTMF_LO_DFT_SZ*2;
        for (k = 0; k < DTMF_LO_DFT_SZ; k++)
        {
            ac1 += *psCDP * (S32) *psI;
            ac0 -= *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* return data pointers back */
        psI -= DTMF_LO_DFT_SZ*2;
        psQ -= DTMF_LO_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_en2logC(ac0);;
    }
}
/*-------------------------------------------------------------------------*/
void 							dtmf_lo_freqC
/*-------------------------------------------------------------------------*/
(
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);
}
/*-------------------------------------------------------------------------*/
void 							dtmf_hi_dft_mainC
/*-------------------------------------------------------------------------*/
(
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_en2logC(ac0);;
    }
}
/*-------------------------------------------------------------------------*/
void 							dtmf_hi_sortC
/*-------------------------------------------------------------------------*/
(
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;
}
/*-------------------------------------------------------------------------*/
void 							dtmf_hi_dft_devC
/*-------------------------------------------------------------------------*/
(
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_en2logC(ac0);;
    }
}
/*-------------------------------------------------------------------------*/
void 							dtmf_hi_freqC
/*-------------------------------------------------------------------------*/
(
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);
}
/*-------------------------------------------------------------------------*/
void 					dtmf_frame_enC
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
#if (IDTMF_FR_SZ == 40)
     S32 ac0 = 0;
     S16 *ps0 = &pSc->as2kData[0];
     S16 Sample; 

     for (Sample = 0; Sample < IDTMF_FR_SZ; Sample++)
     {
          ac0 += (*ps0 * (long)*ps0);
          ps0 += 1;
     }
     pSc->slFrameEn = (ac0);
#else
     S32 ac0 = 0;
     S32 ac1 = 0;
     S16 *ps0 = &pSc->as2kData[0];
     S16 *ps1 = &pSc->as2kData[IDTMF_FR_SZ/2];
     S16 Sample; 

     for (Sample = 0; Sample < IDTMF_FR_SZ/2; Sample++)
     {
          ac0 += (*ps0 * (long)*ps0);
          ac1 += (*ps1 * (long)*ps1);
          ps0 += 1;
          ps1 += 1;
     }
     ac0 >>= 3;
     ac1 >>= 3;
     ac0 += ac1;
     pSc->sFrameEn = dtmf_en2logC(ac0);
#endif
}
/*-------------------------------------------------------------------------*/
void                        dtmf_move_data_outC
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb, 
DTMF_tSc *pSc
)
{
    S16 k;
#if (IDTMF_FR_SZ == 40)
    {
        S32 slTmp = pSc->slFrameEn + pDb->v.slFrameEn;
        S16 sTmp =  dtmf_en2logC(slTmp>>3);
        pDb->v.sZFrameEn = sTmp;
    }

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

    for (k = 0; k < DTMF_2KFLT_SZ; k++)
    {
        pDb->as2kDataSav[k] = pSc->as2kData[k+IDTMF_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];
    }
#if (IDTMF_FR_SZ != 80)
    for (k = 0; k < DTMF_HISD_SZ; k++)
    {
        pDb->asHiDataSav[k] = pSc->asHiData[k+DTMF_DFR_SZ*2];
    }
#endif
}

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