postfilt.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
          /*LINTLIBRARY*/
          /*PROTOLIB1*/

#include "main.h"
#include "filter.h"
#include <math.h>
#include "bwexp.h"
#include "lsftopc.h"
#include "movarray.h"
#include "pctorc.h"
#include "postfilt.h"

#define TC	0.01

FILTER	PostP, PostZ, PostZ2;

/**************************************************************************
*                                                                         *
* ROUTINE
*		PostFilter
*
* FUNCTION
*		Filter input signal
* SYNOPSIS
*		PostFilter(speech_in, lsf, frame_num, speech_out)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	speech_in	float	 i	input speech
*	lsf		float	 i	input line spectral frequencies
*	frame_num	int	 i	frame number
*	speech		float	 o	output speech
*
***************************************************************************
*
* CALLED BY
*
*	Synthesis
*
* CALLS
*
*	BWExpand do_zfilt_dynamic do_pfilt_dynamic PCtoRC MoveArray
*
**************************************************************************/
void PostFilter(
float	speech_in[],
float	lsf[],
int	frame_num,
float	speech_out[])
{
int		n;
static float 	powerin=0.0;
static float	powerout=0.0;
float 		ast[2];
float 		pcexp1[ORDER + 1], pcexp2[ORDER + 1], rcexp2[ORDER];
float		alpha = 0.8;
float		beta = 0.5;
float		pc[ORDER+1];

#ifdef POSTFIL2
float 		scale;
#else
float 		newpowerin[MAX_CW_VEC_LEN + 1], newpowerout[MAX_CW_VEC_LEN + 1];
#endif

/*  estimate input power */

#ifdef POSTFIL2
	for (n = 0; n < SF_LEN; n++)
	  powerin = powerin * (1.0 - TC) + TC * speech_in[n] * speech_in[n];
#else
	newpowerin[0] = powerin;
	for (n = 0; n < SF_LEN; n++)
	  newpowerin[n + 1] = (1.0 - TC) * newpowerin[n] + 
		TC * speech_in[n] * speech_in[n];
	powerin = newpowerin[SF_LEN];
#endif

/*  Convert input LSFs to PCs */
	LSFtoPC(lsf, pc, frame_num);

/*  BW expansion */
	BWExpand(beta, pc, pcexp1);
	BWExpand(alpha, pc, pcexp2);

/*  pole-zero postfilter */
	MoveArray(SF_LEN, speech_in, speech_out);
	do_zfilt_dynamic(&PostZ, pcexp1, speech_out);
	do_pfilt_dynamic(&PostP, pcexp2, speech_out);

/*  find spectral tilt (1st order fit) of postfilter
      (denominator dominates the tilt)			*/
	PCtoRC(pcexp2, rcexp2);

/*  tilt compensation by a scaled zero
      (don't allow hF roll-off)				*/
	ast[0] = 1.0;
	ast[1] = (rcexp2[0] > 0.) ? -0.5 * rcexp2[0] : 0;
	do_zfilt_dynamic(&PostZ2, ast, speech_out);

/*  estimate output power	*/

#ifdef POSTFIL2
	for (n = 0; n < SF_LEN; n++)
	  powerout = powerout * (1.0 - TC) + TC * speech_out[n] * speech_out[n];

/*  block wise automatic gain control	*/

	if (powerout > 0.0)
	  for (scale = sqrt(powerin / powerout), n = 0; n < SF_LEN; n++)
      	    speech_out[n] *= scale;
#else
	newpowerout[0] = powerout;
	for (n = 0; n < SF_LEN; n++)
	  newpowerout[n + 1] = (1.0 - TC) * newpowerout[n] + 
		TC * speech_out[n] * speech_out[n];
	powerout = newpowerout[SF_LEN];

/*  sample wise automatic gain control	*/

	for (n = 0; n < SF_LEN; n++)
	{
	  if (newpowerout[n + 1] > 0.0)
	    speech_out[n] *= (float)sqrt((double)(newpowerin[n + 1] / newpowerout[n + 1]));
	}
#endif


}