xpmr.c

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

/*
 * xpmr.c - Xelatec Private Mobile Radio Processes
 * 
 * All Rights Reserved. Copyright (C)2007, Xelatec, LLC
 * 
 * 20070808 1235 Steven Henke, W9SH, [e-mail]
 *
 * See http://www.asterisk.org for more information about
 * the Asterisk project. Please do not directly contact
 * any of the maintainers of this project for assistance;
 * the project provides a web site, mailing lists and IRC
 * channels for your use.
 *
 * This program is free software, distributed under the terms of
 * the GNU General Public License Version 2. See the LICENSE file
 * at the top of the source tree.
 */

/*! \file
 *
 * \brief Private Land Mobile Radio Channel Voice and Signaling Processor
 *
 * \author Steven Henke, W9SH <[e-mail]> Xelatec, LLC
 */
/*
	FYI 	= For Your Information
	PMR 	= Private Mobile Radio
	RX  	= Receive
	TX  	= Transmit
	CTCSS	= Continuous Tone Coded Squelch System
	TONE	= Same as above.
	LSD 	= Low Speed Data, subaudible signaling. May be tones or codes.
	VOX 	= Voice Operated Transmit
	DSP 	= Digital Signal Processing
	LPF 	= Low Pass Filter
	FIR 	= Finite Impulse Response (Filter)
	IIR 	= Infinite Impulse Response (Filter)
*/
#include <stdio.h>
#include <ctype.h>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <sys/time.h>
#include <stdlib.h>
#include <errno.h>

#include "xpmr.h"
#include "xpmr_coef.h"
#include "sinetabx.h"

static i16 pmrChanIndex=0;	 			// count of created pmr instances

/*
	Convert a Frequency in Hz to a zero based CTCSS Table index
*/
i16 CtcssFreqIndex(float freq)
{
	i16 i,hit=-1;

	for(i=0;i<CTCSS_NUM_CODES;i++){
		if(freq==freq_ctcss[i])hit=i;
	}
	return hit;
}
/*
	pmr_rx_frontend 
	Takes a block of data and low pass filters it.
	Determines the amplitude of high frequency noise for carrier detect.
	Decimates input data to change the rate.
*/
i16 pmr_rx_frontend(t_pmr_sps *mySps) 
{
	#define DCgainBpfNoise 	65536
 
	i16 samples,iOutput, *input, *output, *noutput;
	i16 *x, *coef, *coef2;
    i32 i, naccum, outputGain, calcAdjust;
	i64 y;
	i16 nx, hyst, setpt, compOut;
	i16 amax, amin, apeak, discounteru, discounterl, discfactor;
	i16 decimator, decimate, doNoise;

	TRACEX(("pmr_rx_frontend()\n"));

	if(!mySps->enabled)return(1);

	decimator = mySps->decimator;
	decimate = mySps->decimate;

	input     = mySps->source;
	output    = mySps->sink;
	noutput   = mySps->parentChan->pRxNoise;

	nx        = mySps->nx;
	coef      = mySps->coef;
	coef2     = mySps->coef2;

	calcAdjust = mySps->calcAdjust;
	outputGain = mySps->outputGain;

	amax=mySps->amax;
	amin=mySps->amin;
	apeak=mySps->apeak;
 	discounteru=mySps->discounteru;
	discounterl=mySps->discounterl;
	discfactor=mySps->discfactor;
	setpt=mySps->setpt;
	hyst=mySps->hyst;
	compOut=mySps->compOut;

	samples=mySps->nSamples*decimate;
	x=mySps->x;		
	iOutput=0;

	if(mySps->parentChan->rxCdType!=CD_XPMR_VOX)doNoise=1;
	else doNoise=0;
			
	for(i=0;i<samples;i++)
	{
		i16 n;

		//shift the old samples
	    for(n=nx-1; n>0; n--)
	       x[n] = x[n-1];

	    x[0] = input[i*2];
	
		--decimator;

		if(decimator<=0)
		{
			decimator=decimate;
			
		    y=0; 
		    for(n=0; n<nx; n++)
		        y += coef[n] * x[n];

		    y=((y/calcAdjust)*outputGain)/M_Q8;

			if(y>32767)y=32767;
			else if(y<-32767)y=-32767;

		    output[iOutput]=y;					// Rx Baseband decimated
			noutput[iOutput++] = apeak;		  	// Rx Noise
		}

		if(doNoise)
		{
			// calculate noise output
			naccum=0;
		    for(n=0; n<nx; n++)
		        naccum += coef_fir_bpf_noise_1[n] * x[n];
		    
		    naccum /= DCgainBpfNoise;
	
			if(naccum>amax)
			{
				amax=naccum;
				discounteru=discfactor;
			}
			else if(--discounteru<=0)
			{
				discounteru=discfactor;
				amax=(i32)((amax*32700)/32768);
			}

			if(naccum<amin)
			{
				amin=naccum;
				discounterl=discfactor;
			}
			else if(--discounterl<=0)
			{
				discounterl=discfactor;
				amin=(i32)((amin*32700)/32768);
			}
		
			apeak=(amax-amin)/2;
			
		}  // if doNoise
	}

	if(doNoise)
	{
		((t_pmr_chan *)(mySps->parentChan))->rxRssi=apeak;
		
		if(apeak>setpt || (compOut&&(apeak>(setpt-hyst)))) compOut=1;
		else compOut=0;
		mySps->compOut=compOut;
		mySps->amax=amax;
		mySps->amin=amin; 				
		mySps->apeak=apeak;
	 	mySps->discounteru=discounteru;
		mySps->discounterl=discounterl; 
	}

	return 0;
}
/*             
	pmr general purpose fir
	works on a block of samples
*/
i16 pmr_gp_fir(t_pmr_sps *mySps) 
{
	i32 nsamples,inputGain,outputGain,calcAdjust;
	i16 *input, *output;
	i16 *x, *coef;
    i32 i, ii;
	i16 nx, hyst, setpt, compOut;
	i16 amax, amin, apeak=0, discounteru=0, discounterl=0, discfactor;
	i16 decimator, decimate, interpolate;
	i16 numChanOut, selChanOut, mixOut, monoOut;

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

	if(!mySps->enabled)return(1);

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

	input      = mySps->source;
	output     = mySps->sink;
	x          = mySps->x;
	nx         = mySps->nx;
	coef       = mySps->coef;
	
	decimator   = mySps->decimator;
	decimate 	= mySps->decimate;
	interpolate = mySps->interpolate;

	setpt	   = mySps->setpt;
	compOut    = mySps->compOut;

	inputGain  = mySps->inputGain;
	outputGain = mySps->outputGain;
	numChanOut = mySps->numChanOut;
	selChanOut = mySps->selChanOut;
	mixOut     = mySps->mixOut;
	monoOut    = mySps->monoOut;

	amax=mySps->amax;
	amin=mySps->amin; 				

	discfactor=mySps->discfactor;
	hyst=mySps->hyst; 				
	setpt=mySps->setpt; 	
	nsamples=mySps->nSamples;

	if(mySps->option==3)
	{
		mySps->option=0;
		mySps->enabled=0;
		for(i=0;i<nsamples;i++)
		{
			if(monoOut)
				output[(i*2)]=output[(i*2)+1]=0;
			else
				output[(i*numChanOut)+selChanOut]=0;
		}
		return 0;
	}

	ii=0;
	for(i=0;i<nsamples;i++)
	{
		int ix;

		int64_t y=0;
		
		if(decimate<0)
		{
			decimator=decimate;	
		}

		for(ix=0;ix<interpolate;ix++)
		{
			i16 n; 
			y=0;
	
		    for(n=nx-1; n>0; n--)
		       x[n] = x[n-1];
		    x[0] = (input[i]*inputGain)/M_Q8;
		
			#if 0
			--decimator;
			if(decimator<=0)
			{
				decimator=decimate;
			    for(n=0; n<nx; n++)
			        y += coef[n] * x[n];
				y /= (outputGain*3);  
				output[ii++]=y;
			}
		 	#else
		    for(n=0; n<nx; n++)
		        y += coef[n] * x[n];

			y=((y/calcAdjust)*outputGain)/M_Q8;
			
			if(mixOut){
				if(monoOut){
					output[(ii*2)]=output[(ii*2)+1]+=y;
				}
				else{
					output[(ii*numChanOut)+selChanOut]+=y;	
				}
			}
			else{
				if(monoOut){
					output[(ii*2)]=output[(ii*2)+1]=y;
				}
				else{
					output[(ii*numChanOut)+selChanOut]=y;
				}
			}
			ii++;
		    #endif
		}

		// amplitude detector
		if(setpt)
		{
			i16 accum=y;

			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(apeak>setpt)compOut=1;
			else if(compOut&&(apeak<(setpt-hyst)))compOut=0;
		}
	}

	mySps->decimator = decimator;

	mySps->amax=amax;
	mySps->amin=amin; 				
	mySps->apeak=apeak;  
 	mySps->discounteru=discounteru;
	mySps->discounterl=discounterl; 
	
	mySps->compOut=compOut;
	
	return 0;
}
/*
	general purpose integrator lpf
*/
i16 gp_inte_00(t_pmr_sps *mySps)
{
	i16 npoints;
 	i16 *input, *output;

	i32 inputGain, outputGain,calcAdjust;
	i32	i;
	i32 accum;

	i32 state00;
 	i16 coeff00, coeff01;

	TRACEX(("gp_inte_00() %i\n",mySps->enabled));
	if(!mySps->enabled)return(1);

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

	npoints=mySps->nSamples;

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

	coeff00=((i16*)mySps->coef)[0];
	coeff01=((i16*)mySps->coef)[1];
	state00=((i32*)mySps->x)[0];

	// note fixed gain of 2 to compensate for attenuation
	// in passband

	for(i=0;i<npoints;i++)
	{
		accum=input[i];
		state00 = accum + (state00*coeff01)/M_Q15;
		accum = (state00*coeff00)/(M_Q15/4);
		output[i]=(accum*outputGain)/M_Q8;
	}

	((i32*)(mySps->x))[0]=state00;

	return 0;
}
/*
	general purpose differentiator hpf
*/
i16 gp_diff(t_pmr_sps *mySps)
{
 	i16 *input, *output;
	i16 npoints;
	i32 inputGain, outputGain, calcAdjust;
	i32	i;
	i32 temp0,temp1;
 	i16 x0;
	i32 y0;
	i16 a0,a1;
	i16 b0;
	i16 *coef;
	i16 *x;

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

	npoints=mySps->nSamples;

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

	coef=(i16*)(mySps->coef);
	x=(i16*)(mySps->x);
	a0=coef[0];
	a1=coef[1];
	b0=coef[2];

	x0=x[0];

	TRACEX(("gp_diff()\n"));

  	for (i=0;i<npoints;i++)
    {
		temp0 =	x0 * a1;
		   x0 = input[i];
		temp1 = input[i] * a0;
		   y0 = (temp0 + temp1)/calcAdjust;
		output[i]=(y0*outputGain)/M_Q8;
    }

	x[0]=x0;

	return 0;
}
/* 	----------------------------------------------------------------------
	CenterSlicer
*/
i16 CenterSlicer(t_pmr_sps *mySps)
{
	i16 npoints,lhit,uhit;
 	i16 *input, *output, *buff;

	i32 inputGain, outputGain, inputGainB;
	i32	i;
	i32 accum;

	i32  amax;			// buffer amplitude maximum
	i32  amin;			// buffer amplitude minimum
	i32  apeak;			// buffer amplitude peak
	i32  center;
	i32  setpt;			// amplitude set point for peak tracking

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

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

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

	npoints=mySps->nSamples;

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

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

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

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

		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;	
		
		apeak = (amax-amin)/2;
		center = (amax+amin)/2;
		accum = accum - center;
		output[i]=accum;
	
		// do limiter function
		if(accum>inputGainB)accum=inputGainB;
		else if(accum<-inputGainB)accum=-inputGainB;
		buff[i]=accum;

		#if XPMR_DEBUG0 == 1
		#if 0
		mySps->debugBuff0[i]=center;
		#endif
	    #if 0