demo.cpp

TI公司DSP/tms320c55x/tms320c54x系列的声学回声消除代码

			{
				rewind(_Control.pInTrxFile);
				DEMO_StepMode = TRUE;
				Cntr = 30;
			}

			if (++Tries == 5)
			{
				exit(15);
			}
		}
	}
	else
	{
		Cntr--;
	}
}

/*-------------------------------------------------------------------------*/
void                    _add_rx_trx_txt
/*-------------------------------------------------------------------------*/
(
)
{
	static char aBuffer[81];

    int k;

	if (_Frame > _Control.iAddRxDelay) 
	{
		if (_Control.pAddRxTrxFile)
		{
			for (k = 0; k < IGAEC_FR_SZ; k++)
			{
				int iRx = 0;
				int iTx = 0;
				char *p = fgets(aBuffer, 80, _Control.pAddRxTrxFile);


				if (p != NULL)
				{
					sscanf(aBuffer, "%d %d", &iTx, &iRx);
					iTx = (int)(iTx * _Control.fAddRxCorr);
   					_asRx[k] += iTx;
				}
			}
		}
		else if (_Control.pAddRxTxFile)
		{
			for (k = 0; k < IGAEC_FR_SZ; k++)
			{
				int iTx = 0;
				char *p = fgets(aBuffer, 80, _Control.pAddRxTxFile);
				if (p != NULL)
				{
					sscanf(aBuffer, "%d %d", &iTx);
					iTx = (int)(iTx * _Control.fAddRxCorr);
   					_asRx[k] += iTx;
				}
			}
		}
	}
}
/*-------------------------------------------------------------------------*/
S16                     _gen_rand
/*-------------------------------------------------------------------------*/
(
double fNorma
)
{
    double f = 0;
    S16 n;

    for (n = 0; n < 12; n++)
    {
        f += rand();
    }
    f = ((f - 6.0*RAND_MAX)* fNorma)/RAND_MAX;
    return (S16)f;
}

/*-------------------------------------------------------------------------*/
void                    _generate_input
/*-------------------------------------------------------------------------*/
(
S16 *pRx,
S16 *pTx
)
{
	S16 Rx;
	S16 Tx;

	if (_Gen.liStateCntr-- == 0)
	{
		/* change state*/
		switch(_Gen.iState)
		{
			case 0: /* silence */
			{
				if (rand() < RAND_MAX * _Gen.fDoubleTalk)
				{
					_Gen.iState = 3;
					_Gen.liStateCntr = long(_Gen.fWordLen * (16000.0/RAND_MAX) * rand()+1);
				}
				else
				{
					if (--_Gen.liCurrWord < 0) /* change direction */
					{
						if (_Gen.iDirection == 2)
						{
							_Gen.iState = 1;
							_Gen.iDirection = 1;
						}
						else
						{
							_Gen.iState = 2;
							_Gen.iDirection  = 2;
						}
						_Gen.liCurrWord = _Gen.liSentenceLen;
					}
					else /* go on with the sentence */
					{
						_Gen.iState = _Gen.iDirection;
					}
					_Gen.liStateCntr = long(_Gen.fWordLen * (16000.0/RAND_MAX) * rand());
				}
				break;
			}
			case 1: /* tx */
			case 2: /* rx */
			{
				if (rand() < RAND_MAX * _Gen.fDoubleTalk)
				{
					_Gen.iState = 3;
					_Gen.liStateCntr = long(_Gen.fWordLen * (16000.0/RAND_MAX) * rand() + 1);
				}
				else
				{
					_Gen.iState = 0;
					_Gen.liStateCntr = long(_Gen.fPauseLen * (16000.0/RAND_MAX) * rand() +1);
				}
				break;
			}
			case 3: /* double talk */
			{
				_Gen.iState = 0;
				_Gen.liStateCntr = long(_Gen.fPauseLen * (16000.0/RAND_MAX) * rand() +1);
				break;
			}
		} /* end of switch */
	} /* state chandge */

	/* periodic */
	switch(_Gen.iState)
	{
		case 0: /* silence */
		{
			Rx = _gen_rand(_Gen.fRxNoise);
			Tx = _gen_rand(_Gen.fTxNoise);
			break;
		}
		case 1: /* tx */
		{
			Rx = _gen_rand(_Gen.fRxNoise);
			Tx = _gen_rand(_Gen.fTxLevel);
			break;
		}
		case 2: /* rx */
		{
			Rx = _gen_rand(_Gen.fRxLevel);
			Tx = _gen_rand(_Gen.fTxNoise);
			break;
		}
		case 3: /* double talk */
		{
			Rx = _gen_rand(_Gen.fRxLevel);
			Tx = _gen_rand(_Gen.fTxLevel);
			break;
		}
	}

	*pRx = Rx;
	*pTx = Tx;

}

/*-------------------------------------------------------------------------*/
void                            _init_generator
/*-------------------------------------------------------------------------*/
(
void
)
{
	_Gen.iDirection  = 1;
	_Gen.liCurrWord  = 3;
	switch(_Gen.iState)
	{
		case 0:
		{
			_Gen.liStateCntr = long(_Gen.fPauseLen * 8000);
			break;
		}
		case 1:
		case 2:
		{
			_Gen.liStateCntr = long(_Gen.fWordLen * 8000);
			_Gen.liCurrWord = _Gen.liSentenceLen;
			break;
		}
		default:
		{
			_Gen.iState = 0;
			break;
		}
	}
//	randomize();
	srand(105);
}

static S16 _asMeandr[] =
{
	 1,	 1,	 1,	 1,	 1,
	-1,	-1,	-1,	-1,	-1,
	 1,	 1,	 1,	 1,	 1,
	-1,	-1,	-1,	-1,	-1,
	 1,	 1,	 1,	 1,	 1,
	-1,	-1,	-1,	-1,	-1,
	 1,	 1,	 1,	 1,	 1,
	-1,	-1,	-1,	-1,	-1
};

/*-------------------------------------------------------------------------*/
void                    _make_input2
/*-------------------------------------------------------------------------*/
(
)
{
	int k;
	static double __dTx = 0;
	static double __dRx = 0;

	S16 Rx;
	S16 Tx;

	for (k = 0; k < IGAEC_FR_SZ; k++)
	{
		if (_Frame < 40)
		{
			Tx = _gen_rand(_Gen.fTxNoise);
			Rx = _gen_rand(_Gen.fRxNoise);
		}
		else if (_Frame < 5*200)
		{
			Tx = _gen_rand(_Gen.fTxLevel);
			Rx = _gen_rand(_Gen.fRxNoise);
		}
#if 0
		else if (_Frame < 5*200+1) // some silence to account for delays
		{
			Tx = _gen_rand(_Gen.fTxNoise);
			Rx = _gen_rand(_Gen.fRxNoise);
		}
#endif
		else if (_Frame < 7*200)
		{
			Tx = _gen_rand(_Gen.fTxLevel);
			Rx = _gen_rand(_Gen.fRxLevel);
//			Rx = S16(_Gen.fRxLevel * _asMeandr[k]);
		}
		else
		{
			Tx = _gen_rand(_Gen.fTxLevel);
			Rx = _gen_rand(_Gen.fRxNoise);
		}

		__dTx = Tx + 0.9 * __dTx;
		__dRx = Rx + 0.9 * __dRx;

		_asRx[k] = (S16)(__dRx*0.5);
		_asTx[k] = (S16)(__dTx*0.5);
	}
}
/*-------------------------------------------------------------------------*/
void                _make_input
/*-------------------------------------------------------------------------*/
(
)
{

#define FR1     50
#define FR2     (FR1 + 80)
#define FR3     (FR2 + 40)
#define FR4     (FR3 + 40)
#define FR5     (FR4 + 30)

#define F1 (350/8000.)
#define F2 (440/8000.)
#define F3 (697/8000.)
#define F4 (1633/8000.)
#define F5 (697/8000.)
#define F6 (1633/8000.)
#define ADT (4096*sqrt(2.0)/sqrt(10.0))
#define ADTMF (3000)

    int k;
	if (_Frame < FR1)
	{
		for (k = 0; k < IGAEC_FR_SZ; k++)
		{
			double ph = 2*M_PI*(k+IGAEC_FR_SZ*_Frame);
			_asRx[k] = S16(ADT*(sin(ph*F1)+sin(ph*F2)));
			_asTx[k] = 0;
		}
	}
	else if (_Frame < FR2 && _Frame >= FR1 )
	{
		for (k = 0; k < IGAEC_FR_SZ; k++)
		{
			double ph = 2*M_PI*(k+IGAEC_FR_SZ*_Frame);
			_asRx[k] = S16(ADT*(sin(ph*F1)+sin(ph*F2)));
			_asTx[k] = S16(ADTMF*(sin(ph*F3)+sin(ph*F4)));
//			_asTx[k] = S16(ADTMF*(sin(ph*200/8000.)));
		}
	}
	else if (_Frame < FR3 && _Frame >=FR2 )
	{
		for (k = 0; k < IGAEC_FR_SZ; k++)
		{
			double ph = 2*M_PI*(k+IGAEC_FR_SZ*_Frame);
			_asRx[k] = S16(ADT*(sin(ph*F1)+sin(ph*F2)));
			_asTx[k] = 0;
		}
	}
	else if (_Frame < FR4 && _Frame >= FR3 )
	{
		for (k = 0; k < IGAEC_FR_SZ; k++)
		{
			_asRx[k] = 0;
			_asTx[k] = 0;
		}
	}
#if 1
	else if (_Frame < FR5 && _Frame >= FR4 )
	{
		for (k = 0; k < IGAEC_FR_SZ; k++)
		{
			double ph = 2*M_PI*(k+IGAEC_FR_SZ*_Frame);
			_asRx[k] = 0;
			_asTx[k] = S16(ADTMF*(sin(ph*F5)+sin(ph*F6)));
		}
	}
#endif
	else
    {
		for (k = 0; k < IGAEC_FR_SZ; k++)
		{
            S16 sRx, sTx;
			_generate_input(&sRx, &sTx);
			_asRx[k] = sRx;
			_asTx[k] = sTx;
//			aRx[k] = 0;
//			if(xxx == FILTER_LEN){ aTx[k] = 700; xxx = 1;}
//			else {aTx[k] = (rand()-0x4000)/0x1000;xxx++;}
		}
    }
}

/*-------------------------------------------------------------------------*/
void                        _make_singular
/*-------------------------------------------------------------------------*/
(
)
{
	int Rx = 0;
	double dTx = 0;
	int Tx;
	int k;
	static float f = 247.0;
	static double sph = 0;

	for (k = 0; k < IGAEC_FR_SZ; k++)
	{
#if (IN_MODE == 1)
		dTx = 0.5*5810.0*sin(2*M_PI*sph*375.0/8000.0)+

			  0.1*5810.0*sin(2*M_PI*sph*300.0/8000.0);
		Rx = 0;
#elif (IN_MODE == 2)
		dTx = (long(sph)%20-10)*300.0;
		Rx = 0;
#elif (IN_MODE == 3)
		int xxx = 10.0+sph/1e4;
		dTx = (long(sph)%xxx-xxx/2 < 0)? 1000:-1000;
		Rx = 0;
#elif (IN_MODE == 4)

//		if(k== 0)
		{
			f+= 0.1/IGAEC_FR_SZ;
		}
		dTx = 0.5*5810.0*sin(2*M_PI*sph*f/8000.0);
		Rx = 0;
#elif (IN_MODE == 5)
        dTx = 4096*sqrt(2.0)*sin(2*M_PI*sph*400.0/8000.0);
        Rx = 0;
#elif (IN_MODE == 6)
        Rx = (S16)(4096*sqrt(2.0)*sin(2*M_PI*sph*400.0/8000.0));
        dTx = 0;
#endif
		sph += 1;
		Tx = _limit(dTx);
		_asTx[k] = Tx;
		_asRx[k] = Rx;
	}

}
/*-------------------------------------------------------------------------*/
void                        _invert_echo
/*-------------------------------------------------------------------------*/
(
)
{
	int k;
	for (k = 75*8; k < ECHO_SZ; k++)
	{
		_afEcho[k] = -_afEcho[k];
	}
}
/*-------------------------------------------------------------------------*/
void                        _add_reflections
/*-------------------------------------------------------------------------*/
(
)
{
	int k;

	for (k = 0; k < ECHO_SZ + IGAEC_FR_SZ; k++)
	{
		_asEchoTx[k] = _asEchoTx[k + IGAEC_FR_SZ];
	}
    
    for (k = 0; k < IGAEC_FR_SZ; k++)
    {
//	    _asEchoTx[k + ECHO_SZ + IGAEC_FR_SZ] = _asTx[k];
	    _asEchoTx[k + ECHO_SZ + IGAEC_FR_SZ] = _asTxOut[k];
    }

    if (_Control.fAddTxQNoise > -99.0)
    {
        double f = pow(10., _Control.fAddTxQNoise/20.)/(RAND_MAX/2);
        S16 *psIn = &_asEchoTx[ECHO_SZ + IGAEC_FR_SZ];
        S16 k;

        for (k = 0; k < IGAEC_FR_SZ; k++)
        {
            double fNorm = psIn[k] * f;
            psIn[k] += S16((rand() - RAND_MAX/2)*fNorm);
        }
    }

	for (k = 0; k < IGAEC_FR_SZ; k++)
	{
		double Sum = 0;
		int n;
		int DataIndex = ECHO_SZ + k + IGAEC_FR_SZ + _Control.iTxDelay;

		for (n = 0; n < ECHO_SZ - IGAEC_FR_SZ; n++, DataIndex--)
		{
			Sum += _asEchoTx[DataIndex] * _afEcho[n];
		}
//		_asRx[k] += S16(Sum * 0.4);
//		_asRx[k] += S16(Sum * 0.563);
		_asRx[k] += S16(Sum * 1.0);
	}

    if (_Control.fAddRxQNoise > -99.0)
    {
        double f = pow(10., _Control.fAddRxQNoise/20.)/(RAND_MAX/2);
        S16 *psIn = _asRx;
        S16 k;

        for (k = 0; k < IGAEC_FR_SZ; k++)
        {
            double fNorm = psIn[k] * f;
            psIn[k] += S16((rand() - RAND_MAX/2)*fNorm);
        }
    }
}

/*-------------------------------------------------------------------------*/
void                    _add_pink_noise
/*-------------------------------------------------------------------------*/
(
)
{
    static double f = 0;

    if (_Control.fAddPinkNoise > 1.0)
    {
        S16 *psIn = _asRx;
        int k;

        for (k = 0; k < IGAEC_FR_SZ; k++)
        {
            f +=  (_gen_rand(_Control.fAddPinkNoise*10) - f) * 0.9;
            psIn[k] += S16(f/10);
        }
    }
}

/*-------------------------------------------------------------------------*/
void                            _fill_input
/*-------------------------------------------------------------------------*/
(
)
{
    int k;
    int iReflectionAdded = FALSE;

    for (k = 0; k < IGAEC_FR_SZ; k++)
    {
        _asRx[k] = 0;
        _asTx[k] = 0;
    }

#if (IN_MODE != 0)

    _make_singular();

#else /* IN_MODE == 0*/

    if (_Control.pInTrxFile)
    {
        _read_trx_txt();
    }
    else if (_Control.pInTxFile)
	{
        _read_tx_txt();
        if (_Control.pInTxFile)
        {
            _read_rx_txt();
        }
    }
    else if (_Control.pInTxBinFile)
	{
        _read_tx_bin();
        if (_Control.pInRxBinFile)
        {
            _read_rx_bin();
        }