dtmf.cpp

DTMF检测程序

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "alaw.h"
#include "dtmf.h"

const short COEF[ENERGY_LEN]={
		27980, 26956, 25701, 24219, 19073, 16325,13085, 9315		//1st
		};

const short KEYS[DTMF_NUMBERS]={
		0x0301,		// '0'  
		0x0000,		// '1'
		0x0001,		// '2'
		0x0002,		// '3'
		0x0100,		// '4'
		0x0101,		// '5'
		0x0102,		// '6'
		0x0200,		// '7'
		0x0201,		// '8'
		0x0202,		// '9'
		0x0003,		// 'A'
		0x0103,		// 'B'
		0x0203,		// 'C'
		0x0303,		// 'D'
		0x0300,		// 'E' = '*'
		0x0302		// 'F' = '#'
		};

DTMFDet_t m_dtmfTab;
FILE *fpSource;

#ifdef DEBUG
int sample = 0;
FILE *fpGC;
char afterGC[] = "afterGC.pcm";
#endif

int main(int argc, char* argv[])
{
#ifdef ALAW
	unsigned char a_val[FRAME_SIZE]={0};
#else
	short a_val=0;
#endif
	unsigned short dtmf_nbr;
	short linearCode[FRAME_SIZE]={0};
	int i=0;
	int totalNum=0;

	if(argc != 2)
	{
		printf("Usage: %s filename\n", argv[0]); 
		return -1;
	}
	
	fpSource = fopen(argv[1], "rb");
	if(fpSource == NULL)
	{
		printf("Open file %s failed!\n", argv[1]);
		return -1;
	}

#ifdef DEBUG
	fpGC = fopen(afterGC, "wb");
	if(fpGC == NULL)
	{
		printf("Open file %s failed!\n", afterGC);
		fclose(fpSource);
		return -1;
	}
#endif

	init(&m_dtmfTab);

	while(1)
	{
		if(fread((void*)&a_val, sizeof(unsigned char), FRAME_SIZE, fpSource) != FRAME_SIZE)
		{
			printf("\nEnd of file %s!\n", argv[1]);
			printf("Total Number detected = %d\n", totalNum);
			fclose(fpSource);
			return 0;
		}

#ifdef ALAW
		for(i=0; i<FRAME_SIZE; i++)
		{
			linearCode[i] = alaw2linear(a_val[i]);
		}
#else
		linearCode[i] = a_val[i];
#endif

		if(dtmfDet(linearCode, &dtmf_nbr))
		{
			printf("DTMF number %x detected!\n", dtmf_nbr);
//			printf("%x", dtmf_nbr);
			totalNum++;
		}
	}
}

bool dtmfDet(short *linearCode, unsigned short* pNum)
{
	int i;
	short rowMax, colMax, rowMax2, colMax2;
	unsigned short row, col, numMap, currentNum;
	const short *pKEYS = KEYS;
	bool ret = false;
#ifdef ALAW
	unsigned char a_val[FRAME_SIZE];
#endif

	gaincrtl(linearCode);
//	linearCode >>= 6;

	for(i=0; i<FRAME_SIZE; i++)
	{
#ifdef ALAW
		a_val[i] = linear2alaw(linearCode[i]);
#else
		a_val[i] = linearCode[i];
#endif
	}

#ifdef DEBUG
	if(fwrite(a_val, sizeof(a_val[0]), FRAME_SIZE, fpGC) != FRAME_SIZE)
	{
		printf("Writing file %s ERROR!\n", afterGC);
		fclose(fpSource);
		fclose(fpGC);
		return 0;
	}
#endif

	for(i=0; i<FRAME_SIZE; i++)
	{
		goertzel(linearCode[i], &m_dtmfTab);
	}

	getEnergy(&m_dtmfTab);

	maxSearch(&rowMax, &colMax, &rowMax2, &colMax2, &row, &col, &m_dtmfTab);

	if(strengthCheck(rowMax, colMax, &m_dtmfTab) == false)
	{
		return false;
	}
	if(twistCheck(rowMax, colMax) == false)
	{
		return false;
	}
	if(relCheck(rowMax, colMax, rowMax2, colMax2) == false)
	{
		return false;
	}
		
	numMap = (row<<8) + col;
	for(i=0; i<DTMF_NUMBERS; i++)
	{
		if(numMap == *pKEYS++)
		{
			currentNum = (unsigned short)i;
			break;
		}
	}

	if(currentNum == m_dtmfTab.digitLast)
	{
#if 0
		printf("E:%5d, %5d, %5d, %5d,      %5d, %5d, %5d, %5d\n", 
		m_dtmfTab.energy[0], m_dtmfTab.energy[1], m_dtmfTab.energy[2], m_dtmfTab.energy[3], 
		m_dtmfTab.energy[4], m_dtmfTab.energy[5],m_dtmfTab.energy[6], m_dtmfTab.energy[7]);
#endif
		ret = true;
		if(pNum)
			*pNum = currentNum;
		m_dtmfTab.digitLast = 16;
		m_dtmfTab.pauseCnt = 0;
	}
	else
		m_dtmfTab.digitLast = currentNum;

	return ret;
}

inline short MUL_S16_S16(short x, short y)
{
	short ret = (short)((x*y)>>15);
	//printf("MUL %#x * %#x = %#x\n", x, y, ret);
	return ret;
}

short DIV_S16_S16(short x, short y)
{
	int iteration;
	short var_out;
	short L_num;
	short L_denom;

	if (y == 0)
	{
		printf("Division by 0, Fatal error!\n");
		return 0;
	}

	if (x == 0)
	{
		var_out = 0;
		return var_out;
	}

	if (x == y) 
	{
		var_out = 0x7fff;
		return var_out;
	}

	var_out = 0;
	L_num = (int)(x);
	L_denom = (int)(y);

	for(iteration=0; iteration<15; iteration++)
	{
		var_out <<=1;
		L_num <<= 1;

		if (L_num >= L_denom)
		{
			L_num -= L_denom;
			var_out += 1;
		}
	}

	return var_out;
}

void init(DTMFDet_t *pTab)
{
	pTab->goertzelCnt = MAX_LOOPS;
	pTab->digitLast = 16;
	pTab->pauseCnt = PAUSE_TIME;
//	pTab->pOldSample = pTab->gainBuf;
//	memset(pTab->gainBuf, 0, GAINBUF_LEN*sizeof(short));
	memset(pTab->taps, 0, TAPS_LEN*sizeof(short));
	memset(pTab->energy, 0, ENERGY_LEN*sizeof(short));
	return;
}

/*
GAIN_AMP(n) = (1/32)*[abs(x(n))+abs(x(n+1))+...+abs(x(n+31))]

if( GAIN_AMP > GAIN_THR )
	y(n) = (GAIN_THR/GAIN_AMP) * x(n)
else
	y(n) = x(n)
*/

void gaincrtl(short* pSample)
{
	int i = 0;
	unsigned int sum = 0;
	short gainAmp = 0;
	short factor = 0;
	
	for(i=0; i<FRAME_SIZE; i++)
	{
		sum += abs(pSample[i]);
	}

	if(sum > GAIN_THR)
	{
		gainAmp = sum>>8;
		factor = DIV_S16_S16(GAIN_THR>>8, gainAmp);
		for(i=0; i<FRAME_SIZE; i++)
			pSample[i] = MUL_S16_S16(factor, pSample[i]);
	}

	return;
}

/* vk(n) = 2*coef*vk(n-1) - vk(n-2) + x(n) */
void goertzel(short x, DTMFDet_t *pTab)
{
	int i;
	short vk=0, *pTaps;
	const short *pCOEF;

	pTaps = pTab->taps;
	pCOEF = COEF;
	for(i=0; i<ENERGY_LEN; i++)
	{
		vk = x - *pTaps++;
		vk >>= 1;
		vk += MUL_S16_S16(*pCOEF++, *pTaps++);
		vk <<= 1;
		*(pTaps-2) = *(pTaps-1);
		*(pTaps-1) = vk;
	}

	return;
}

/* y(N)y*(N) = vk(N)*vk(N) - 2*coef*vk(N)*vk(N-1) + vk(N-1)*vk(N-1) */

void getEnergy(DTMFDet_t *pTab)
{
	int i;
	short *pTaps, *pEnergy;
	const short *pCOEF;

	pTaps = pTab->taps;
	pEnergy = pTab->energy;
	pCOEF = COEF;
	for(i=0; i<ENERGY_LEN; i++)
	{
		*pEnergy = MUL_S16_S16(*pTaps, *pTaps);
		pTaps++;
		*pEnergy += MUL_S16_S16(*pTaps, *pTaps);
		*pEnergy >>= 1;
		*pEnergy -= MUL_S16_S16(MUL_S16_S16(*pTaps, *(pTaps-1)), *pCOEF);
		*pEnergy <<= 1;
		pTaps++; pEnergy++; pCOEF++;
	}

#if 0
	pTaps = pTab->taps;
	printf("\ntaps: %d, %d,  %d, %d,  %d, %d,  %d, %d,\n      %d, %d,  %d, %d,  %d, %d,  %d, %d\n",
		*pTaps, *(pTaps+1), *(pTaps+2), *(pTaps+3), 
		*(pTaps+4), *(pTaps+5),*(pTaps+6), *(pTaps+7),
		*(pTaps+8), *(pTaps+9), *(pTaps+10), *(pTaps+11), 
		*(pTaps+12), *(pTaps+13),*(pTaps+14), *(pTaps+15));
#endif

	memset(pTab->taps, 0, TAPS_LEN*sizeof(short));	//reset taps to 0

#if 0
	pEnergy = pTab->energy;
	printf("E:%5d, %5d, %5d, %5d,      %5d, %5d, %5d, %5d\n", 
		*pEnergy, *(pEnergy+1), *(pEnergy+2), *(pEnergy+3), 
		*(pEnergy+4), *(pEnergy+5),*(pEnergy+6), *(pEnergy+7));
#endif

	return;
}

void maxSearch(short *rowMax, short *colMax, short *rowMax2, short *colMax2,
	unsigned short *row, unsigned short *col, DTMFDet_t *pTab)
{
	int i;
	short *pEnergy = pTab->energy;

	*rowMax = *pEnergy;
	*rowMax2 = *pEnergy++;
	*row = 0;
	for(i=1; i<4; i++)
	{
		if(*rowMax < *pEnergy)
		{
			*row = i;
			*rowMax2 = *rowMax;
			*rowMax = *pEnergy;
		}
		else if(*rowMax2 < *pEnergy)
		{
			*rowMax2 = *pEnergy;
		}
		else if(i == 1)
			*rowMax2 = *pEnergy;
			
		pEnergy++;
	}

	*colMax = *pEnergy;
	*colMax2 = *pEnergy++;
	*col = 0;
	for(i=1; i<4; i++)
	{
		if(*colMax < *pEnergy)
		{
			*col = i;
			*colMax2 = *colMax;
			*colMax = *pEnergy;
		}
		else if(*colMax2 < *pEnergy)
		{
			*colMax2 = *pEnergy;
		}
		else if(i == 1)
			*colMax2 = *pEnergy;

		pEnergy++;
	}

	return;
}

/*
bool pauseCheck(short rowMax, short colMax, DTMFDet_t *pTab)
{
	if(pTab->pauseCnt == PAUSE_TIME)
		return true;

	return false;
}
*/

bool strengthCheck(short rowMax, short colMax, DTMFDet_t *pTab)
{
	if(rowMax + colMax > THR_SIG)
	{
		if(pTab->pauseCnt == PAUSE_TIME)
			return true;
		return false;
	}
	else if(rowMax + colMax > THR_PAU)
		return false;
	else
	{
		if(pTab->pauseCnt != PAUSE_TIME)
			pTab->pauseCnt++;
		return false;
	}
}

bool twistCheck(short rowMax, short colMax)
{
	short tmp;

	if(rowMax > colMax)
	{
		tmp = THR_TWI1;
		if(MUL_S16_S16(tmp, rowMax) < colMax)
			return true;
	}
	else
	{
		tmp = THR_TWI2;
		if(MUL_S16_S16(tmp, colMax) < rowMax)
			return true;
	}
//	printf("twist false\n");
	return false;
}

bool relCheck(short rowMax, short colMax, short rowMax2, short colMax2)
{
	short tmp = THR_ROWREL;

	if(MUL_S16_S16(tmp, rowMax) < rowMax2)
	{
		return false;
	}

	tmp = THR_COLREL;

	if(MUL_S16_S16(tmp, colMax) < colMax2)
	{
		return false;
	}
//	printf("rel false\n");
	return true;
}