cpds.cpp

CP detector (CPD) only reports the onset and removal of a tone. The analysis of timing (if required)

/*-------------------------------------------------------------------------*
*                                                                         *
*   THIS IS AN UNPUBLISHED WORK CONTAINING CONFIDENTIAL AND PROPRIETARY   *
*   INFORMATION.  IF PUBLICATION OCCURS, THE FOLLOWING NOTICE APPLIES:    *
*      "COPYRIGHT 2001 MIKET DSP SOLUTIONS, ALL RIGHTS RESERVED"          *
*                                                                         *
*-------------------------------------------------------------------------*/
#if ! defined (_dsp)
#include <stdlib.h> // abs()
#endif

#include "stddefs.h"
#include "cpdi.h"

/*--------------------- local defs ----------------------------------------*/

/*--------------------- public vars ---------------------------------------*/
#if defined (_dsp)
extern const S16 CPD_aBpI[];
extern const S16 CPD_aBpQ[];
extern const S16 CPD_aPg[];
extern const S16 CPD_aPhase[];
#else
#include "cpd_tab.c"
#endif

/*--------------------- local vars ----------------------------------------*/
/*--------------------- local functions -----------------------------------*/
/*-------------------------------------------------------------------------*/
S16                  cpd_en2logC
/*-------------------------------------------------------------------------*/
(
S32 ac0,    /* but it is positive */
S16 sCorr
)
{
    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 + sCorr); 
}

/*-------------------------------------------------------------------------*/
void 					cpd_move_data_inC
/*-------------------------------------------------------------------------*/
(
CPD_tDb *pDb,
CPD_tSc *pSc,
S16 *psIn
)
{
	S16 k;
    S16 sShift = pDb->pCfg->sNormShift;
	
    /* input data buffer: for bandpass filter */
    for (k = 0; k < CPD_BP_SSZ; k++)
    {
        pSc->asBpData[k] = pDb->asBpDataSav[k];
    }

    for (k = 0; k < ICPD_FR_SZ; k++)
    {
        S16 x = psIn[k];
        if (sShift > 0) 
        {
            x <<= sShift;
        }
        else
        {
            x >>= (-sShift);
        }
        pSc->asBpData[k+CPD_BP_SSZ] = x;
    }

    /* bandpass data buffer - for periodogram filters */

    for (k = 0; k < CPD_PG_SSZ; k++)
    {
        pSc->asPgIData[k] = pDb->asPgIDataSav[k];
        pSc->asPgQData[k] = pDb->asPgQDataSav[k];
    }
}

/*-------------------------------------------------------------------------*/
void 					cpd_filter_bpC
/*-------------------------------------------------------------------------*/
(
CPD_tSc *pSc
)
{
    S16 *psOutI  = &pSc->asPgIData[CPD_PG_SSZ];
    S16 *psOutQ  = &pSc->asPgQData[CPD_PG_SSZ];
    S16 *psCDP  = pSc->asBpData;
    const S16 *psIflt = CPD_aBpI;
    const S16 *psQflt = CPD_aBpQ;
    S32 ac0;
    S32 ac1;
    S32 ac2;
	S16 sSample;
    S16 k;
	
	
    ac0 = 0;

    for (sSample = 0; sSample < ICPD_FR_SZ/CPD_BP_DR; sSample++)
    {
        ac1 = 0;
        ac2 = 0;
        for (k = 0; k < CPD_BP_FSZ; k++)
        {
            ac1 += *psCDP * (S32) *psIflt;
            ac2 += *psCDP * (S32) *psQflt;
            psCDP++;
            psIflt++;
            psQflt++;
        }

        ac1 += 0x8000L;
        ac2 += 0x8000L;

        ac1 >>= 16;
        ac2 >>= 16;
        *psOutI++ = (S16)(ac1);
        *psOutQ++ = (S16)(ac2);
        ac0 += (ac1)*(ac1);
        ac0 += (ac2)*(ac2);
        psCDP  -= CPD_BP_FSZ - CPD_BP_DR; 

        psIflt -= CPD_BP_FSZ; /* circular ? */
        psQflt -= CPD_BP_FSZ; /* circular ? */
    }
    pSc->slBpEn = ac0;
    pSc->sBpEn = cpd_en2logC(pSc->slBpEn, CPD_EN_BP);
}

/*-------------------------------------------------------------------------*/
void 					cpd_sum_subC
/*-------------------------------------------------------------------------*/
(
CPD_tSc *pSc
)
{
    S16 k;
    /* negative increments, from N-1 to N/2 */
    S16 *psFromP = &pSc->asPgIData[0];
    S16 *psFromN = &pSc->asPgIData[CPD_PG_FSZ-1];
    S16 *psToSum = &pSc->asPgISum[0];
    S16 *psToDif = &pSc->asPgIDif[0];

    for (k = 0; k < CPD_PG_HSZ; k++)
    {
        S16 tx = *psFromN--;

        S16 acHi = *psFromP + tx;
        S16 acLo = *psFromP - tx;
        psFromP++;

        *psToSum++ = acHi;
        *psToDif++ = acLo;
    }

    psFromP = &pSc->asPgQData[0];
    psFromN = &pSc->asPgQData[CPD_PG_FSZ-1];
    psToSum = &pSc->asPgQSum[0];
    psToDif = &pSc->asPgQDif[0];

    for (k = 0; k < CPD_PG_HSZ; k++)
    {
        S16 tx = *psFromN--;

        S16 acHi = *psFromP + tx;
        S16 acLo = *psFromP - tx;
        psFromP++;

        *psToSum++ = acHi;
        *psToDif++ = acLo;
    }
}

/*-------------------------------------------------------------------------*/
void 							cpd_filter_pgC
/*-------------------------------------------------------------------------*/
(
CPD_tSc *pSc
)
{
    S32 ac0;
    S32 ac1;
    const S16 *psCDP = CPD_aPg;
    S16 *psI = pSc->asPgISum;
    S16 *psQ = pSc->asPgQSum;
    CPD_tOut *pOut = pSc->aOut;
    S16 sFilterNo;
    S16 k;

    

    for (sFilterNo = 0; sFilterNo < CPD_FREQS; sFilterNo++)
    {
        psI = pSc->asPgISum;
        psQ = pSc->asPgQSum;
        ac0 = 0;
        ac1 = 0;
        /* psCDP points to Cos coeffs */
        for (k = 0; k < CPD_PG_HSZ; k++)
        {
            ac0 += *psCDP * (S32) *psI; /* ac+ * fc */
            ac1 += *psCDP * (S32) *psQ; /* as+ * fc */
            psI ++;
            psQ ++;
            psCDP++;
        }
        /* psCDP now points to Sin coeffs */
        psI = pSc->asPgIDif;
        psQ = pSc->asPgQDif;

        for (k = 0; k < CPD_PG_HSZ; k++)
        {
            ac0 -= *psCDP * (S32) *psQ; /* as- * fs */
            ac1 += *psCDP * (S32) *psI; /* ac- * fs */

            psI ++;
            psQ ++;
            psCDP++;
        }

        /* psCDP points to Cos coeffs of the next filter */
        /* ac0 = I * c - Q * s;
           ac1 = I * s + Q * c;
         */
        ac0 += 0x8000L;
        ac1 += 0x8000L;

        ac0 >>= 16;
        ac1 >>= 16;

        pOut->sC = (S16)(ac0); 
        pOut->sS = (S16)(ac1);
        ac0  = ac0 * ac0;
        ac0 += ac1 * ac1;
        pOut->slEn = ac0;
        pOut++;
    }
}

/*-------------------------------------------------------------------------*/
void 					cpd_sort_freqsC
/*-------------------------------------------------------------------------*/
(
CPD_tSc *pSc
)
/* order frequencies ... we need 2 dominant freqs and the next 
 * after them, to test that the spectrum is clean
 */
{
    S32 *pslEn = &pSc->aslEnSort[0];
    S16 sMaxIdx = 0;
    S32 slMax;
    S16 k;

    for (k = 0; k < CPD_FREQS; k++)
    {
        pslEn[k] = pSc->aOut[k].slEn;
    }

    /* find max */    
    slMax = 0;
    for (k = 0; k < CPD_FREQS; k++)
    {
        if (pslEn[k] > slMax)
        {
            slMax = pslEn[k];
            sMaxIdx = k;
        }
    }
    pSc->sMaxIdx = sMaxIdx;
    pSc->sMaxEn = cpd_en2logC(slMax, CPD_EN_PG);

    /* find next */
    pslEn[sMaxIdx] = 0;
    slMax = 0;
    for (k = 0; k < CPD_FREQS; k++)
    {
        if (pslEn[k] > slMax)
        {
            slMax = pslEn[k];
            sMaxIdx = k;
        }
    }
    pSc->sNextIdx = sMaxIdx;
    pSc->sNextEn = cpd_en2logC(slMax, CPD_EN_PG);
    
    /* find next ... after next = the rest */
    pslEn[sMaxIdx] = 0;
    slMax = 0;
    for (k = 0; k < CPD_FREQS; k++)
    {
        if (pslEn[k] > slMax)
        {
            slMax = pslEn[k];
            sMaxIdx = k;
        }
    }
    pSc->sRestIdx = sMaxIdx;
    pSc->sRestEn = cpd_en2logC(slMax, CPD_EN_PG);
}

/*-------------------------------------------------------------------------*/
S16 					_get_err
/*-------------------------------------------------------------------------*/
(
S32 slEn,
S32 slErr
)
{
    S16 t0;
    S16 sign = 1;
    
    if (slErr < 0)
    {
    	slErr = -slErr;
    	sign = -1;
    }
    
    if (slErr < slEn)
    {
	    slEn += 1;
	    // mant aco :: nexp ac0, t0
	    for (t0 = 0;;)
	    {
	        if (slEn &0x40000000L)
	            break;
	        slEn <<= 1;
	        t0++;
	    }
	    slErr <<= (t0-1);
	//    slErr >>= 1;
	    slErr &= 0xffff8000;
	    slEn >>= 16;
	    slErr = slErr / slEn;
	    
	    slErr *= (-5450);
	    slErr >>= 15;
    }
    else
    {
    	slErr = -5450;
    }
    
    if (sign < 0) 
    {
    	slErr = -slErr;
    }

    return (S16)(slErr);
}

/*-------------------------------------------------------------------------*/
void                        cpd_get_elem_dataC
/*-------------------------------------------------------------------------*/
(
CPD_tDb *pDb, 
CPD_tSc *pSc,
CPD_tElem *pElem,
S16 sIdx
)
{
    S32 ac2; 
    S32 ac3; 
    const S16 *psCDP = &CPD_aPhase[sIdx * 2];
    CPD_tOut *pOut = &pSc->aOut[sIdx];
    CPD_tOutSav *pOutSav = &pDb->aOutSav[sIdx];

    ac2 = pOutSav->sC * (S32) *psCDP;
    ac3 = pOutSav->sS * (S32) *psCDP;
    psCDP++;

    ac2 += pOutSav->sS * (S32) *psCDP;
    ac3 -= pOutSav->sC * (S32) *psCDP;
//    psCDP++;
    ac2 += 0x4000; ac2 >>= 15;
    ac3 += 0x4000; ac3 >>= 15;

    pElem->sCpred = (S16)ac2;
    pElem->sSpred = (S16)ac3;

    /* calculate (ac3=) square distance between prediction 
       and the actual point */
    ac2 = (pElem->sCpred - pOut->sC);
    ac3 = ac2 * ac2;
    ac2 = (pElem->sSpred - pOut->sS);
    ac3 += ac2 * ac2;

    pElem->sDist = cpd_en2logC(ac3, CPD_EN_PG);

    /* calculate projection of error onto positive
        phase advanced vector.
        p=error = [C1 - Cpred, S1 - Spred];
        e=positive phase vector is [-Spred, Cpred];
        p'*e=Cpred*s1-Spred*c1;
     */
    ac3 = pOut->sS * (S32) pElem->sCpred;
    ac3 = ac3 - pOut->sC * (S32) pElem->sSpred;
    
    pElem->sErr = _get_err(pOut->slEn, ac3);
}

/*-------------------------------------------------------------------------*/
void                        cpd_move_data_outC
/*-------------------------------------------------------------------------*/
(
CPD_tDb *pDb, 
CPD_tSc *pSc
)
{
    S16 k;

    for (k = 0; k < CPD_BP_SSZ; k++)
    {
        pDb->asBpDataSav[k] = pSc->asBpData[k+(CPD_BP_ASZ-CPD_BP_SSZ)];
    }
    for (k = 0; k < CPD_PG_SSZ; k++)
    {
        pDb->asPgIDataSav[k] = pSc->asPgIData[k+(CPD_PG_ASZ-CPD_PG_SSZ)];
        pDb->asPgQDataSav[k] = pSc->asPgQData[k+(CPD_PG_ASZ-CPD_PG_SSZ)];
    }

    for (k = 0; k < CPD_FREQS; k++)
    {
        pDb->aOutSav[k].sC = pSc->aOut[k].sC;
        pDb->aOutSav[k].sS = pSc->aOut[k].sS;
    }

    /* required for next frame */
//    pDb->v.sPrevFrEn = pDb->v.sLastFrEn;
//    pDb->v.sLastFrEn = pSc->sBpEn;

    for (k = CPD_EN_SZ-1; k > 0; k--)
    {
        pDb->v.asSumEn[k] = pDb->v.asSumEn[k-1];
    }
    pDb->v.asSumEn[0] = pSc->sSumEn;
}

/*--------------------- public  functions ---------------------------------*/

/*-------------------------------------------------------------------------*/
void                    cpd_filter_testC
/*-------------------------------------------------------------------------*/
(
CPD_tDb *pDb,
CPD_tSc *pSc,
S16 *psIn
)
{

	cpd_move_data_in(pDb, pSc, psIn);

    cpd_filter_bp(pSc);
    cpd_sum_sub(pSc);
	cpd_filter_pg(pSc);

    cpd_sort_freqs(pSc);
    cpd_get_lohi(pDb, pSc);

    cpd_get_elem_data(pDb, pSc, &pSc->Lo, pSc->sLoIdx);
    cpd_get_elem_data(pDb, pSc, &pSc->Hi, pSc->sHiIdx);

	cpd_test(pDb, pSc);
    cpd_move_data_out (pDb, pSc);
}

/* ------------------------------------------------------------------------ */
void		                    cpd_avrgC
/* ------------------------------------------------------------------------ */
(
S16 *psVal, 
S16 sNew,
S16 sCoeff
)
/* Vk+1 = Vk + (y - Vk) * coeff/32768; */
{
    S32 acc;

    acc = (sNew - *psVal);
    acc *= sCoeff;
    acc += ((S32)(*psVal))<<15;
    acc += 0x4000;
    acc >>= 15;
    *psVal = (S16)acc;
}

#if !defined (_dsp)
S16  cpd_en2log(S32 ac0, S16 sCorr)
{
    return cpd_en2logC(ac0, sCorr);
}
void cpd_move_data_in(CPD_tDb *pDb,CPD_tSc *pSc,S16 *psIn)
{
    cpd_move_data_inC(pDb, pSc, psIn);
}
void cpd_filter_bp(CPD_tSc *pSc)
{
    cpd_filter_bpC(pSc);
}
void cpd_sum_sub(CPD_tSc *pSc)
{
    cpd_sum_subC(pSc);
}
void cpd_filter_pg(CPD_tSc *pSc)
{
    cpd_filter_pgC(pSc);
}
void cpd_sort_freqs(CPD_tSc *pSc)
{
    cpd_sort_freqsC(pSc);
}
void cpd_move_data_out(CPD_tDb *pDb, CPD_tSc *pSc)
{
    cpd_move_data_outC(pDb, pSc);
}
void cpd_get_elem_data(CPD_tDb *pDb, CPD_tSc *pSc,CPD_tElem *pElem,S16 sIdx)
{
    cpd_get_elem_dataC(pDb, pSc, pElem, sIdx);
}
void cpd_avrg(S16 *psVal, S16 sNew,S16 sCoeff)
{
    cpd_avrgC(psVal, sNew, sCoeff);
}
#endif