cpd.c

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 MICHAEL TSIROULNIKOV, ALL RIGHTS RESERVED"         *
 *                                                                         *
 *-------------------------------------------------------------------------*/


#if !defined _dsp
#include <memory.h>
#include <math.h>
#include <stdlib.h>
#else
#include <string.h> /* memcpy() */
#endif

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


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

/* it should be bigger than any State no */ 
#define CPD_ERR    (CPD_ST_MAX)

#define CPD_MASK_ST_IDLE1   (\
CPD_MASK_MAX_EN   |\
CPD_MASK_RS_EDGE  |\
CPD_MASK_LO_OK    |\
CPD_MASK_HI_OK    |\
CPD_MASK_CLEAN_MB )

#define CPD_MASK_ST_IDLE2   (\
CPD_MASK_MAX_EN   |\
CPD_MASK_RS_EDGE  |\
CPD_MASK_LO_OK    |\
CPD_MASK_SINGLE_MB |\
CPD_MASK_CLEAN_MB )

#define CPD_MASK_ST_MAYBE1   (\
CPD_MASK_MAX_EN   |\
CPD_MASK_TWIST    |\
CPD_MASK_LO_OK    |\
CPD_MASK_HI_OK    |\
CPD_MASK_SAME_DGT |\
CPD_MASK_CLEAN_MB )

#define CPD_MASK_ST_MAYBE2   (\
CPD_MASK_MAX_EN   |\
CPD_MASK_LO_OK    |\
CPD_MASK_SAME_DGT |\
CPD_MASK_SINGLE_MB |\
CPD_MASK_CLEAN_MB )

#define CPD_MASK_ST_TONE1    (\
CPD_MASK_MAX_EN   |\
CPD_MASK_LO_STABLE |\
CPD_MASK_HI_STABLE |\
CPD_MASK_TWIST    |\
CPD_MASK_CLEAN    |\
CPD_MASK_LO_OK    |\
CPD_MASK_HI_OK    |\
CPD_MASK_SAME_DGT )

#define CPD_MASK_ST_TONE2    (\
CPD_MASK_MAX_EN   |\
CPD_MASK_LO_STABLE |\
CPD_MASK_CLEAN    |\
CPD_MASK_LO_OK    |\
CPD_MASK_SINGLE |\
CPD_MASK_SAME_DGT )

#define CPD_MASK_ST_GOOD_TONE1 (\
CPD_MASK_MAX_EN   |\
CPD_MASK_LO_STABLE   |\
CPD_MASK_HI_STABLE   |\
CPD_MASK_TWIST    |\
CPD_MASK_CLEAN    |\
CPD_MASK_LO_OK    |\
CPD_MASK_HI_OK    |\
CPD_MASK_SAME_DGT )

#define CPD_MASK_ST_GOOD_TONE2 (\
CPD_MASK_MAX_EN   |\
CPD_MASK_LO_STABLE  |\
CPD_MASK_CLEAN    |\
CPD_MASK_LO_OK    |\
CPD_MASK_SINGLE |\
CPD_MASK_SAME_DGT )

#define CPD_MASK_ST_ABORT      0x1fff

/*--------------------- public vars ---------------------------------------*/
const char			CPD_MIKET_aVersion[] = __DATE__;
/*--------------------- local vars ----------------------------------------*/


static const S16 _asMask1[CPD_ST_MAX] = 
{
    CPD_MASK_ST_IDLE1,
    CPD_MASK_ST_MAYBE1,
    CPD_MASK_ST_TONE1,
    CPD_MASK_ST_GOOD_TONE1,
    CPD_MASK_ST_ABORT
};

static const S16 _asMask2[CPD_ST_MAX] = 
{
    CPD_MASK_ST_IDLE2,
    CPD_MASK_ST_MAYBE2,
    CPD_MASK_ST_TONE2,
    CPD_MASK_ST_GOOD_TONE2,
    CPD_MASK_ST_ABORT
};

#if defined (_dsp)

#pragma DATA_SECTION (CPD_aBpI,     ".cpd_i")
#pragma DATA_SECTION (CPD_aBpQ,     ".cpd_q")
#pragma DATA_SECTION (CPD_aPg,      ".cpd_d")
#pragma DATA_SECTION (CPD_aPhase,   ".cpd_d")

#include "cpd_tab.c"
#endif

/*--------------------- local functions -----------------------------------*/
/*-------------------------------------------------------------------------*/
void 					_init_statistics
/*-------------------------------------------------------------------------*/
(
CPD_tDb *pDb,
CPD_tSc *pSc
)
{
    pDb->v.sAvrEn      = pSc->sSumEn;
    pDb->Stts.sLoEn  = pSc->Lo.sEn;
    pDb->Stts.sLoErr = pSc->Lo.sErr;
    pDb->Stts.sHiEn  = pSc->Hi.sEn;
    pDb->Stts.sHiErr = pSc->Hi.sErr;
}
/*-------------------------------------------------------------------------*/
void 					cpd_update_statistics
/*-------------------------------------------------------------------------*/
(
CPD_tDb *pDb,
CPD_tSc *pSc
)
{
	cpd_avrg(&pDb->v.sAvrEn,    pSc->sSumEn,  Q15(0.25));
    cpd_avrg(&pDb->Stts.sHiErr, pSc->Hi.sErr, Q15(0.125));
    cpd_avrg(&pDb->Stts.sHiEn,  pSc->Hi.sEn,  Q15(0.125));
    cpd_avrg(&pDb->Stts.sLoErr, pSc->Lo.sErr, Q15(0.125));
    cpd_avrg(&pDb->Stts.sLoEn,  pSc->Lo.sEn,  Q15(0.125));
}

/*-------------------------------------------------------------------------*/
S16                  _state_machine
/*-------------------------------------------------------------------------*/
(
CPD_tDb *pDb,
CPD_tSc *pSc
)
{
    const ICPD_tCfg *pCfg = pDb->pCfg;
    S16 sReport = 0;
    S16 sErr;
    S16 sMask = (pSc->sTestFailed & CPD_MASK_DUAL) ?
        _asMask1[pDb->v.sState] : _asMask2[pDb->v.sState];

    sErr = (pSc->sTestFailed & sMask) ? CPD_ERR : 0;

    switch(pDb->v.sState + sErr)
    {
    /* IDLE: we are looking for an energy jump, 
        and some degree of spectrum cleanness */
    case CPD_ST_IDLE + CPD_ERR:
        pDb->v.sFrCntr = 0;
        break;
    case CPD_ST_IDLE + 0:
        pDb->v.sState = CPD_ST_MAYBE;
        pDb->v.sDigit = pSc->sDigit;
        pDb->v.sFrCntr = 1;
        break;

    /* MAYBE: to confirm that energy is ok, and freq content is fine: 
        several consequtive packets have the same freq,
        and spectrum is clean enough */
    case CPD_ST_MAYBE + CPD_ERR:  
        pDb->v.sFrCntr--;
        if (pDb->v.sFrCntr <= 0)
        {
            pDb->v.sState = CPD_ST_IDLE;
        }
        break;
    case CPD_ST_MAYBE + 0:  
        pDb->v.sFrCntr++;
        if (pDb->v.sFrCntr >= 7)
        {
            _init_statistics(pDb, pSc);
            pDb->v.sState = CPD_ST_TONE;
        }
        break;
    /* TONE: here we expect several good frame after a seemingly good beginning,
       1 bad frame in a row is allowed, 
        due to a short hole (s) / interference */
    case CPD_ST_TONE + CPD_ERR:  
        pDb->v.sFrCntr--;
        if (pDb->v.sFrCntr <= 0)
        {
            pDb->v.sState = CPD_ST_ABORT;
        }
        break;
    case CPD_ST_TONE + 0:
        pDb->v.sFrCntr++;
        cpd_update_statistics(pDb, pSc);

        if (pDb->v.sFrCntr >= pCfg->sMinToneDuration)
        {
            pDb->v.sFrCntr = 0;
            pDb->v.sState = CPD_ST_GOOD_TONE;
            sReport = pDb->v.sDigit | (S16)ICPD_EV_START;
        }
        break;

    /* GOOD_TONE: */
    case CPD_ST_GOOD_TONE + CPD_ERR:   
        if ((pSc->sTestFailed & CPD_MASK_FL_EDGE) == 0)
        {
            pDb->v.sFrCntr++;
            if (pDb->v.sFrCntr >= pCfg->sMinEndDuration) 
            {
                pDb->v.sState = CPD_ST_IDLE;
                sReport = pDb->v.sDigit | (S16)ICPD_EV_END;
            }
        }
        break;
    case CPD_ST_GOOD_TONE + 0: 
        if (pSc->sBpEn > pSc->sSumEn - 6*170)
        {
            cpd_update_statistics(pDb, pSc);
        }
        pDb->v.sFrCntr--;
        if (pDb->v.sFrCntr < 0) 
        {
            pDb->v.sFrCntr = 0;
        }

        break;

    /* ABORT: just wait for some time and back to work, ignoring error mask */
    case CPD_ST_ABORT + 0:
    case CPD_ST_ABORT + CPD_ERR:
        pDb->v.sFrCntr++;
        if (pDb->v.sFrCntr > pCfg->sAbortTimeout)
        {
            pDb->v.sState = CPD_ST_IDLE;
        }
        break;
    }
    return sReport;
}

/*---------------------------------------------------------------------*/
void                        _reset_db
/*---------------------------------------------------------------------*/
(
CPD_tDb *pDb
)
{
        memset(pDb, 0, sizeof(CPD_tDb));
}
        
/*--------------------- public  functions ---------------------------------*/

/*---------------------------------------------------------------------*/
void                        CPD_MIKET_init_db
/*---------------------------------------------------------------------*/
(
void *p2db,
ICPD_tCfg *pCfg
)
{
    CPD_tDb *pDb = (CPD_tDb *)p2db;

    _reset_db(pDb);
    pDb->pCfg = pCfg;
}

/*---------------------------------------------------------------------*/
void                         CPD_MIKET_control
/*---------------------------------------------------------------------*/
(
void *p2db,
Int sCmd,
ICPD_Status *pStatus
)
{
    CPD_tDb *pDb = (CPD_tDb *)p2db;

    if (sCmd & ICPD_CMD_RESET) 
    {
        const ICPD_tCfg *pCfgSav = pDb->pCfg;
        _reset_db(pDb);
        pDb->pCfg = pCfgSav;
    }
  
    if (sCmd & ICPD_CMD_CFG)
    {
    	pDb->pCfg = pStatus->pCfg;
    }

    pDb->sIsOn = 1;
    
    if (sCmd & ICPD_CMD_OFF)
    {
    	pDb->sIsOn =  0;
    }

    if (sCmd & ICPD_CMD_GET_STTS)
    {
    	pStatus->sLoEn = pDb->Stts.sLoEn;
    	pStatus->sHiEn = pDb->Stts.sHiEn;
    	pStatus->sLoFreqDev = pDb->Stts.sLoErr;
    	pStatus->sHiFreqDev = pDb->Stts.sHiErr;
    }
    pStatus->size = sizeof(ICPD_Status);

}

/*-------------------------------------------------------------------------*/
S16                         CPD_MIKET_process
/*-------------------------------------------------------------------------*/
(
void *p2db,
void *p2sc,
S16 *psIn
)
{
    CPD_tDb *pDb = (CPD_tDb *)p2db;
    CPD_tSc *pSc = (CPD_tSc *)p2sc;
    S16 sReport = 0;

    if (pDb->sIsOn)
    {
    	memset(pSc, 0, sizeof(CPD_tSc));
		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);
		
        sReport = _state_machine (pDb, pSc);
    }
    return sReport;
}