xpmr.c

asterisk 是一个很有知名度开源软件

		if(mySps->parentChan->frameCountRx&0x01) mySps->parentChan->prxDebug1[i]=amax;
		else mySps->parentChan->prxDebug1[i]=amin;
		#endif
		#endif
	}

	mySps->amax=amax;
	mySps->amin=amin;
	mySps->apeak=apeak;
	mySps->discounteru=discounteru;
	mySps->discounterl=discounterl;

	return 0;
}
/* 	----------------------------------------------------------------------
	MeasureBlock
	determine peak amplitude
*/
i16 MeasureBlock(t_pmr_sps *mySps)
{
	i16 npoints;
 	i16 *input, *output;

	i32 inputGain, outputGain;
	i32	i;
	i32 accum;

	i16  amax;			// buffer amplitude maximum
	i16  amin;			// buffer amplitude minimum
	i16  apeak=0;			// buffer amplitude peak (peak to peak)/2
	i16  setpt;			// amplitude set point for amplitude comparator

	i32  discounteru;	// amplitude detector integrator discharge counter upper
	i32  discounterl;	// amplitude detector integrator discharge counter lower
	i32  discfactor;	// amplitude detector integrator discharge factor

	TRACEX(("MeasureBlock() %i\n",mySps->enabled));

	if(!mySps->enabled)return 1;

	if(mySps->option==3)
	{
		mySps->amax = mySps->amin = mySps->apeak = \
		mySps->discounteru = mySps->discounterl = \
		mySps->enabled = 0;
		return 1;
	}

	input   = mySps->source;
	output	= mySps->sink;

	npoints=mySps->nSamples;

	inputGain=mySps->inputGain;
	outputGain=mySps->outputGain;

	amax=mySps->amax;
	amin=mySps->amin;
	setpt=mySps->setpt;
	discounteru=mySps->discounteru;
	discounterl=mySps->discounterl;

	discfactor=mySps->discfactor;
	npoints=mySps->nSamples;

	for(i=0;i<npoints;i++)
	{
		accum=input[i];

		if(accum>amax)
		{
			amax=accum;
			discounteru=discfactor;
		}
		else if(--discounteru<=0)
		{
			discounteru=discfactor;
			amax=(i32)((amax*32700)/32768);
		}

		if(accum<amin)
		{
			amin=accum;
			discounterl=discfactor;
		}
		else if(--discounterl<=0)
		{
			discounterl=discfactor;
			amin=(i32)((amin*32700)/32768);
		}

		apeak = (i32)(amax-amin)/2;
		if(output)output[i]=apeak;
	}
    
	mySps->amax=amax;
	mySps->amin=amin;
	mySps->apeak=apeak;
	mySps->discounteru=discounteru;
	mySps->discounterl=discounterl;
	if(apeak>=setpt) mySps->compOut=1;
	else mySps->compOut=0;
	
	//TRACEX((" -MeasureBlock()=%i\n",mySps->apeak));
	return 0;
}
/*
	SoftLimiter
*/
i16 SoftLimiter(t_pmr_sps *mySps)
{
	i16 npoints;
	//i16 samples, lhit,uhit;
 	i16 *input, *output;

	i32 inputGain, outputGain;
	i32	i;
	i32 accum;
	i32  tmp;

	i32  amax;			// buffer amplitude maximum
	i32  amin;			// buffer amplitude minimum
	//i32  apeak;		// buffer amplitude peak
	i32  setpt;			// amplitude set point for amplitude comparator
	i16  compOut;		// amplitude comparator output

	input   = mySps->source;
	output	= mySps->sink;

	inputGain=mySps->inputGain;
	outputGain=mySps->outputGain;

	npoints=mySps->nSamples;

	setpt=mySps->setpt;
	amax=(setpt*124)/128;
	amin=-amax;

	TRACEX(("SoftLimiter() %i %i %i) \n",amin, amax,setpt));

	for(i=0;i<npoints;i++)
	{
		accum=input[i];
		//accum=input[i]*mySps->inputGain/256;

		if(accum>setpt)
		{
		    tmp=((accum-setpt)*4)/128;
		    accum=setpt+tmp;
			if(accum>amax)accum=amax;
			compOut=1;
			accum=setpt;
		}
		else if(accum<-setpt)
		{
		    tmp=((accum+setpt)*4)/128;
		    accum=(-setpt)-tmp;
			if(accum<amin)accum=amin;
			compOut=1;
			accum=-setpt;
		}

		output[i]=(accum*outputGain)/M_Q8;
	}

	return 0;
}
/*
	SigGen() - sine, square function generator
	sps overloaded values
	discfactor  = phase factor
	discfactoru = phase index
	if source is not NULL then mix it in!

	sign table and output gain are in Q15 format (32767=.999)
*/
i16	SigGen(t_pmr_sps *mySps)
{
	#define PH_FRACT_FACT	128

	i32 ph;
	i16 i,outputgain,waveform,numChanOut,selChanOut;
	i32 accum;				 

	TRACEX(("SigGen(%i) \n",mySps->option));

	if(!mySps->freq ||!mySps->enabled)return 0;

	outputgain=mySps->outputGain;
	waveform=0;
	numChanOut=mySps->numChanOut; 
	selChanOut=mySps->selChanOut;

    if(mySps->option==1)
	{
		mySps->option=0;
		mySps->state=1;
		mySps->discfactor=
			(SAMPLES_PER_SINE*mySps->freq*PH_FRACT_FACT)/mySps->sampleRate/10;
	
		TRACEX((" SigGen() discfactor = %i\n",mySps->discfactor));
		if(mySps->discounterl)mySps->state=2;
	}
	else if(mySps->option==2)
	{
		i16 shiftfactor=CTCSS_TURN_OFF_SHIFT;
		// phase shift request
		mySps->option=0;
		mySps->state=2;
		mySps->discounterl=CTCSS_TURN_OFF_TIME-(2*MS_PER_FRAME);   		// 

		mySps->discounteru = \
			(mySps->discounteru + (((SAMPLES_PER_SINE*shiftfactor)/360)*PH_FRACT_FACT)) % (SAMPLES_PER_SINE*PH_FRACT_FACT);
		//printf("shiftfactor = %i\n",shiftfactor);
		//shiftfactor+=10;
	}
	else if(mySps->option==3)
	{
		// stop it and clear the output buffer
		mySps->option=0;
		mySps->state=0;
		mySps->enabled=0;
		for(i=0;i<mySps->nSamples;i++)
			mySps->sink[(i*numChanOut)+selChanOut]=0;
		return(0);
	}
	else if(mySps->state==2)
	{
		// doing turn off
		mySps->discounterl-=MS_PER_FRAME;
		if(mySps->discounterl<=0)
		{
			mySps->option=3;
			mySps->state=2;
		}
	}
	else if(mySps->state==0)
	{
		return(0);
	}

	ph=mySps->discounteru;

	for(i=0;i<mySps->nSamples;i++)
	{
		if(!waveform)
		{
			// sine
			//tmp=(sinetablex[ph/PH_FRACT_FACT]*amplitude)/M_Q16;
			accum=sinetablex[ph/PH_FRACT_FACT];
			accum=(accum*outputgain)/M_Q8;
	    }
		else
		{
			// square
			if(ph>SAMPLES_PER_SINE/2)
				accum=outputgain/M_Q8;
			else
				accum=-outputgain/M_Q8;
		}

		if(mySps->source)accum+=mySps->source[i];

		mySps->sink[(i*numChanOut)+selChanOut]=accum;

		ph=(ph+mySps->discfactor)%(SAMPLES_PER_SINE*PH_FRACT_FACT);
	}

	mySps->discounteru=ph;

	return 0;
}
/*
	adder/mixer
	takes existing buffer and adds source buffer to destination buffer
	sink buffer = (sink buffer * gain) + source buffer 
*/
i16 pmrMixer(t_pmr_sps *mySps)
{
	i32 accum;
	i16 i, *input, *inputB, *output;
	i16  inputGain, inputGainB;	  	// apply to input data	 in Q7.8 format
	i16  outputGain;	// apply to output data  in Q7.8 format
	i16	 discounteru,discounterl,amax,amin,setpt,discfactor;
	i16	 npoints,uhit,lhit,apeak,measPeak;

	TRACEX(("pmrMixer()\n"));

	input     = mySps->source;
	inputB    = mySps->sourceB;
	output    = mySps->sink;

	inputGain=mySps->inputGain;
	inputGainB=mySps->inputGainB;
	outputGain=mySps->outputGain;
	
	amax=mySps->amax;
	amin=mySps->amin;
	setpt=mySps->setpt;
	discounteru=mySps->discounteru;
	discounterl=mySps->discounteru;

	discfactor=mySps->discfactor;
	npoints=mySps->nSamples;
	measPeak=mySps->measPeak;

	for(i=0;i<npoints;i++)
	{
		accum = ((input[i]*inputGain)/M_Q8) + 
				((inputB[i]*inputGainB)/M_Q8);

		accum=(accum*outputGain)/M_Q8;
		output[i]=accum;

		if(measPeak){
	  		lhit=uhit=0;
	
			if(accum>amax){
				amax=accum;
				uhit=1;
				if(amin<(amax-setpt)){
					amin=(amax-setpt);
					lhit=1;
				}
			}
			else if(accum<amin){
				amin=accum;
				lhit=1;
				if(amax>(amin+setpt)){
					amax=(amin+setpt);
					uhit=1;
				}
			}
		
			if(--discounteru<=0 && amax>0){
				amax--;
				uhit=1;
			}
		
			if(--discounterl<=0 && amin<0){
				amin++;
				lhit=1;
			}
		
			if(uhit)discounteru=discfactor;	
			if(lhit)discounterl=discfactor;
		}	
 	}

	if(measPeak){
		apeak = (amax-amin)/2;
		mySps->apeak=apeak;
		mySps->amax=amax;
		mySps->amin=amin;
		mySps->discounteru=discounteru;
		mySps->discounterl=discounterl;
	}
	
	return 0;
}
/*
	DelayLine 
*/
i16 DelayLine(t_pmr_sps *mySps)
{
	i16 *input, *output, *buff;
	i16	 i, npoints,buffsize,inindex,outindex;

	TRACEX((" DelayLine() %i\n",mySps->enabled));
	
	input    	= mySps->source;
	output    	= mySps->sink;
	buff     	= (i16*)(mySps->buff);
	buffsize  	= mySps->buffSize;
	npoints		= mySps->nSamples;

	outindex	= mySps->buffOutIndex;
	inindex		= outindex + mySps->buffLead;

	for(i=0;i<npoints;i++)
	{
		inindex %= buffsize;
		outindex %= buffsize;
		
		buff[inindex]=input[i];
		output[i]=buff[outindex];
		inindex++;
		outindex++;
 	}
	mySps->buffOutIndex=outindex;

 	return 0;
}
/*
	Continuous Tone Coded Squelch (CTCSS) Detector
*/
i16 ctcss_detect(t_pmr_chan *pmrChan)
{
	i16 i,points2do, points=0, *pInput, hit, thit,relax; 
	i16 tnum, tmp, indexWas=0, indexNow, gain, peakwas=0, diffpeak;
	i16 difftrig;
	i16 lasttv0=0, lasttv1=0, lasttv2=0, tv0, tv1, tv2, indexDebug;

	TRACEX(("ctcss_detect(%p) %i %i %i %i\n",pmrChan, 
		pmrChan->rxCtcss->enabled,
		pmrChan->rxCtcssIndex,
		pmrChan->rxCtcss->testIndex,
		pmrChan->rxCtcss->decode));

	if(!pmrChan->rxCtcss->enabled)return(1);

	relax  = pmrChan->rxCtcss->relax; 
	pInput = pmrChan->rxCtcss->input;
	gain   = pmrChan->rxCtcss->gain;
	
	if(relax) difftrig=(-0.1*M_Q15);
	else difftrig=(-0.05*M_Q15);

	thit=hit=-1;

	//TRACEX((" ctcss_detect() %i  %i  %i  %i\n", CTCSS_NUM_CODES,0,0,0));

	for(tnum=0;tnum<CTCSS_NUM_CODES;tnum++)
	{
		i32 accum, peak;
		t_tdet	*ptdet;
		i16 fudgeFactor;
		i16 binFactor;

		//TRACEX((" ctcss_detect() tnum=%i %i\n",tnum,pmrChan->rxCtcssMap[tnum]));

		if( (pmrChan->rxCtcssMap[tnum] < 0) || 
		    (pmrChan->rxCtcss->decode>=0 && (tnum!= pmrChan->rxCtcss->decode)) ||
			(!pmrChan->rxCtcss->multiFreq && (tnum!= pmrChan->rxCtcssIndex))
		  )
			continue;

		//TRACEX((" ctcss_detect() tnum=%i\n",tnum));

		ptdet=&(pmrChan->rxCtcss->tdet[tnum]);
		indexDebug=0;
		points=points2do=pmrChan->nSamplesRx;
		fudgeFactor=ptdet->fudgeFactor;
		binFactor=ptdet->binFactor;

		while(ptdet->counter < (points2do*CTCSS_SCOUNT_MUL))
		{
			//TRACEX((" ctcss_detect() - inner loop\n"));
			tmp=(ptdet->counter/CTCSS_SCOUNT_MUL)+1;
		    ptdet->counter-=(tmp*CTCSS_SCOUNT_MUL);
			points2do-=tmp;
			indexNow=points-points2do;
			
			ptdet->counter += ptdet->counterFactor;

			accum = pInput[indexNow-1];	 	// dude's major bug fix!

			peakwas=ptdet->peak;

			ptdet->z[ptdet->zIndex]+=
				(((accum - ptdet->z[ptdet->zIndex])*binFactor)/M_Q15);

			peak = abs(ptdet->z[0]-ptdet->z[2]) + abs(ptdet->z[1]-ptdet->z[3]);

			if (ptdet->peak < peak)
				ptdet->peak += ( ((peak-ptdet->peak)*binFactor)/M_Q15);
			else
				ptdet->peak=peak;

			{
				static const i16 a0=13723;
				static const i16 a1=-13723;
				i32 temp0,temp1;
				i16 x0;
	
				//differentiate
				x0=ptdet->zd;
				temp0 =	x0 * a1;
				ptdet->zd = ptdet->peak;
				temp1 = ptdet->peak * a0;
			    diffpeak = (temp0 + temp1)/1024;
			}

			if(diffpeak<(-0.03*M_Q15))ptdet->dvd-=4;
			else if(ptdet->dvd<0)ptdet->dvd++;

			if((ptdet->dvd < -12) && diffpeak > (-0.02*M_Q15))ptdet->dvu+=2;
			else if(ptdet->dvu)ptdet->dvu--;

			tmp=ptdet->setpt;
			if(pmrChan->rxCtcss->decode==tnum)
			{
				if(relax)tmp=(tmp*55)/100;
				else tmp=(tmp*80)/100;
			}

			if(ptdet->peak > tmp)
			{
			    if(ptdet->decode<(fudgeFactor*32))ptdet->decode++;
			}
			else if(pmrChan->rxCtcss->decode==tnum)
			{
				if(ptdet->peak > ptdet->hyst)ptdet->decode--;
				else if(relax) ptdet->decode--;	
				else ptdet->decode-=4; 
			}
			else
			{
				ptdet->decode=0;
			}

			if((pmrChan->rxCtcss->decode==tnum) && !relax && (ptdet->dvu > (0.00075*M_Q15)))
			{
				ptdet->decode=0;
				ptdet->z[0]=ptdet->z[1]=ptdet->z[2]=ptdet->z[3]=ptdet->dvu=0;
				//printf("ctcss_detect() turnoff code!\n");
			}

			if(ptdet->decode<0 || !pmrChan->rxCarrierDetect)ptdet->decode=0;

			if(ptdet->decode>=fudgeFactor)thit=tnum;  

			#if XPMR_DEBUG0 == 1
			//if(thit>=0 && thit==tnum)
			//	printf(" ctcss_detect() %i %i %i %i \n",tnum,ptdet->peak,ptdet->setpt,ptdet->hyst);

			// tv0=accum;
			tv0=ptdet->peak;
			tv1=diffpeak;
			tv2=ptdet->dvu;
			
			//tv1=ptdet->zi*100;
			while(indexDebug<indexNow)
			{
				if(indexDebug==0)lasttv0=ptdet->pDebug0[points-1];
				if(ptdet->pDebug0)ptdet->pDebug0[indexDebug]=lasttv0;

				if(indexDebug==0)lasttv1=ptdet->pDebug1[points-1];
				if(ptdet->pDebug1)ptdet->pDebug1[indexDebug]=lasttv1;

				if(indexDebug==0)lasttv2=ptdet->pDebug2[points-1];
				if(ptdet->pDebug2)ptdet->pDebug2[indexDebug]=lasttv2;

				indexDebug++;
			}
			lasttv0=tv0;
			lasttv1=tv1;
			lasttv2=tv2*100;
			#endif
			indexWas=indexNow;
			ptdet->zIndex=(++ptdet->zIndex)%4;
		}
		ptdet->counter-=(points2do*CTCSS_SCOUNT_MUL);

		#if XPMR_DEBUG0 == 1
		for(i=indexWas;i<points;i++)
		{
			if(ptdet->pDebug0)ptdet->pDebug0[i]=lasttv0;
			if(ptdet->pDebug1)ptdet->pDebug1[i]=lasttv1;
			if(ptdet->pDebug2)ptdet->pDebug2[i]=lasttv2;
		}
		#endif
	}

	//TRACEX((" ctcss_detect() thit %i\n",thit));